current sensor support added

also optimized the vbat code a bit

src/cli.c

@@ -141,6 +141,9 @@ const clivalue_t valueTable[] = {
     { "telemetry_port", VAR_UINT8, &mcfg.telemetry_port, 0, TELEMETRY_PORT_MAX },
     { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 },
     { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
+    { "currentscale", VAR_UINT16, &mcfg.currentscale, 1, 10000 },
+    { "currentoffset", VAR_UINT16, &mcfg.currentoffset, 0, 1650 },
+    { "multiwiicurrentoutput", VAR_UINT8, &mcfg.multiwiicurrentoutput, 0, 1 },
     { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
     { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
     { "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 },

src/config.c

@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static const uint8_t EEPROM_CONF_VERSION = 63;
+static const uint8_t EEPROM_CONF_VERSION = 64;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -187,6 +187,7 @@ static void resetConf(void)
     mcfg.max_angle_inclination = 500;    // 50 degrees
     mcfg.yaw_control_direction = 1;
     mcfg.moron_threshold = 32;
+    mcfg.currentscale = 400; // for Allegro ACS758LCB-100U (40mV/A)
     mcfg.vbatscale = 110;
     mcfg.vbatmaxcellvoltage = 43;
     mcfg.vbatmincellvoltage = 33;

src/drv_adc.c

@@ -33,8 +33,8 @@ void adcInit(drv_adc_config_t *init)
     // another channel can be stolen from PWM for current measurement or other things
     if (init->powerAdcChannel > 0) {
         numChannels++;
-        adcConfig[ADC_EXTERNAL2].adcChannel = init->powerAdcChannel;
-        adcConfig[ADC_EXTERNAL2].dmaIndex = numChannels - 1;
+        adcConfig[ADC_EXTERNAL_CURRENT].adcChannel = init->powerAdcChannel;
+        adcConfig[ADC_EXTERNAL_CURRENT].dmaIndex = numChannels - 1;
     }
 
     // ADC driver assumes all the GPIO was already placed in 'AIN' mode

src/drv_adc.h

@@ -3,7 +3,7 @@
 typedef enum {
     ADC_BATTERY = 0,
     ADC_EXTERNAL1 = 1,
-    ADC_EXTERNAL2 = 2,
+    ADC_EXTERNAL_CURRENT = 2,
     ADC_CHANNEL_MAX = 3
 } AdcChannel;
 

src/mw.c

@@ -12,7 +12,9 @@ uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 int16_t headFreeModeHold;
 
-uint8_t vbat;                   // battery voltage in 0.1V steps
+uint16_t vbat;                  // battery voltage in 0.1V steps
+int32_t amperage;               // amperage read by current sensor in centiampere (1/100th A)
+uint32_t mAhdrawn;              // milliampere hours drawn from the battery since start
 int16_t telemTemperature1;      // gyro sensor temperature
 
 int16_t failsafeCnt = 0;
@@ -90,9 +92,10 @@ void annexCode(void)
 
     // vbat shit
     static uint8_t vbatTimer = 0;
-    static uint8_t ind = 0;
-    uint16_t vbatRaw = 0;
-    static uint16_t vbatRawArray[8];
+    static int32_t vbatRaw = 0;
+    static int32_t amperageRaw = 0;
+    static uint32_t mAhdrawnRaw = 0;
+    static uint32_t vbatCycleTime = 0;
 
     int i;
 
@@ -155,11 +158,21 @@ void annexCode(void)
     }
 
     if (feature(FEATURE_VBAT)) {
+        vbatCycleTime += cycleTime;
         if (!(++vbatTimer % VBATFREQ)) {
-            vbatRawArray[(ind++) % 8] = adcGetChannel(ADC_BATTERY);
-            for (i = 0; i < 8; i++)
-                vbatRaw += vbatRawArray[i];
+            vbatRaw -= vbatRaw / 8;
+            vbatRaw += adcGetChannel(ADC_BATTERY);
             vbat = batteryAdcToVoltage(vbatRaw / 8);
+            
+            if (mcfg.power_adc_channel > 0) {
+                amperageRaw -= amperageRaw / 8;
+                amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT);
+                amperage = currentSensorToCentiamps(amperageRaw / 8);
+                mAhdrawnRaw += (amperage * vbatCycleTime) / 1000; // will overflow at ~11000mAh
+                mAhdrawn = mAhdrawnRaw / (3600 * 100);
+                vbatCycleTime = 0;
+            }
+            
         }
         if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
             buzzerFreq = 0;

src/mw.h

@@ -256,6 +256,9 @@ typedef struct master_t {
     int16_t magZero[3];
 
     // Battery/ADC stuff
+    uint16_t currentscale;                  // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
+    uint16_t currentoffset;                 // offset of the current sensor in millivolt steps
+    uint8_t multiwiicurrentoutput;          // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
     uint8_t vbatscale;                      // adjust this to match battery voltage to reported value
     uint8_t vbatmaxcellvoltage;             // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)
     uint8_t vbatmincellvoltage;             // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V)
@@ -363,8 +366,10 @@ extern int16_t motor[MAX_MOTORS];
 extern int16_t servo[MAX_SERVOS];
 extern int16_t rcData[RC_CHANS];
 extern uint16_t rssi;                  // range: [0;1023]
-extern uint8_t vbat;
+extern uint16_t vbat;                  // battery voltage in 0.1V steps
 extern int16_t telemTemperature1;      // gyro sensor temperature
+extern int32_t amperage;               // amperage read by current sensor in 0.01A steps
+extern uint32_t mAhdrawn;              // milli ampere hours drawn from battery since start
 extern uint8_t toggleBeep;
 
 #define PITCH_LOOKUP_LENGTH 7
@@ -415,6 +420,7 @@ int getEstimatedAltitude(void);
 bool sensorsAutodetect(void);
 void batteryInit(void);
 uint16_t batteryAdcToVoltage(uint16_t src);
+int32_t currentSensorToCentiamps(uint16_t src);
 void ACC_getADC(void);
 int Baro_update(void);
 void Gyro_getADC(void);

src/sensors.c

@@ -132,6 +132,17 @@ uint16_t batteryAdcToVoltage(uint16_t src)
     return (((src) * 3.3f) / 4095) * mcfg.vbatscale;
 }
 
+#define ADCVREF 33L
+int32_t currentSensorToCentiamps(uint16_t src)
+{
+    int32_t millivolts;
+    
+    millivolts = ((uint32_t)src * ADCVREF * 100) / 4095;
+    millivolts -= mcfg.currentoffset;
+    
+    return (millivolts * 1000) / (int32_t)mcfg.currentscale; // current in 0.01A steps 
+}
+
 void batteryInit(void)
 {
     uint32_t i;

src/serial.c

@@ -471,10 +471,13 @@ static void evaluateCommand(void)
         break;
     case MSP_ANALOG:
         headSerialReply(7);
-        serialize8(vbat);
-        serialize16(0); // power meter trash
+        serialize8((uint8_t)constrain(vbat, 0, 255));
+        serialize16(mAhdrawn); // milliamphours drawn from battery
         serialize16(rssi);
-        serialize16(0); // amperage
+        if(mcfg.multiwiicurrentoutput) {
+            serialize16((uint16_t)constrain((abs(amperage)*10), 0, 0xFFFF)); // send amperage in 0.001 A steps
+        } else
+            serialize16((uint16_t)abs(amperage)); // send amperage in 0.01 A steps
         break;
     case MSP_RC_TUNING:
         headSerialReply(7);