current sensor support added

also optimized the vbat code a bit

Conflicts:
	src/config.c
	src/drv_adc.c
	src/drv_adc.h
	src/mw.c
	src/mw.h
	src/sensors.c
	src/serial_cli.c

src/battery.c

@@ -10,7 +10,10 @@
 uint8_t batteryCellCount = 3;       // cell count
 uint16_t batteryWarningVoltage;     // annoying buzzer after this one, battery ready to be dead
 
-uint8_t vbat;                   // battery voltage in 0.1V steps
+uint8_t vbat = 0;                   // battery voltage in 0.1V steps
+
+int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
+uint32_t mAhdrawn = 0;              // milliampere hours drawn from the battery since start
 
 static batteryConfig_t *batteryConfig;
 
@@ -68,3 +71,26 @@ void batteryInit(batteryConfig_t *initialBatteryConfig)
     batteryWarningVoltage = i * batteryConfig->vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
 }
 
+#define ADCVREF 33L
+int32_t currentSensorToCentiamps(uint16_t src)
+{
+    int32_t millivolts;
+
+    millivolts = ((uint32_t)src * ADCVREF * 100) / 4095;
+    millivolts -= batteryConfig->currentMeterOffset;
+
+    return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
+}
+
+void updateCurrentMeter(uint32_t lastUpdateAt)
+{
+    static int32_t amperageRaw = 0;
+    static uint32_t mAhdrawnRaw = 0;
+
+	amperageRaw -= amperageRaw / 8;
+	amperageRaw += adcGetChannel(ADC_CURRENT);
+	amperage = currentSensorToCentiamps(amperageRaw / 8);
+
+	mAhdrawnRaw += (amperage * lastUpdateAt) / 1000; // will overflow at ~11000mAh
+	mAhdrawn = mAhdrawnRaw / (3600 * 100);
+}

src/battery.h

@@ -4,14 +4,24 @@ typedef struct batteryConfig_s {
     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)
+
+    uint16_t currentMeterScale;             // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
+    uint16_t currentMeterOffset;            // offset of the current sensor in millivolt steps
+
+    // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
+    uint8_t multiwiiCurrentMeterOutput;     // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
 } batteryConfig_t;
 
 extern uint8_t vbat;
 extern uint8_t batteryCellCount;
 extern uint16_t batteryWarningVoltage;
+extern int32_t amperage;
+extern uint32_t mAhdrawn;
 
 uint16_t batteryAdcToVoltage(uint16_t src);
 bool shouldSoundBatteryAlarm(void);
 void updateBatteryVoltage(void);
 void batteryInit(batteryConfig_t *initialBatteryConfig);
 
+void updateCurrentMeter(uint32_t lastUpdateAt);
+int32_t currentMeterToCentiamps(uint16_t src);

src/config.c

@@ -206,9 +206,12 @@ static void resetConf(void)
     masterConfig.max_angle_inclination = 500;    // 50 degrees
     masterConfig.yaw_control_direction = 1;
     masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
+
     masterConfig.batteryConfig.vbatscale = 110;
     masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
     masterConfig.batteryConfig.vbatmincellvoltage = 33;
+    masterConfig.batteryConfig.currentMeterOffset = 0;
+    masterConfig.batteryConfig.currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
 
     resetTelemetryConfig(&masterConfig.telemetryConfig);
 
@@ -373,6 +376,9 @@ void validateAndFixConfig(void)
         if (feature(FEATURE_RSSI_ADC)) {
             featureClear(FEATURE_RSSI_ADC);
         }
+        if (feature(FEATURE_CURRENT_METER)) {
+            featureClear(FEATURE_CURRENT_METER);
+        }
     }
 
     if (feature(FEATURE_RX_MSP)) {

src/config.h

@@ -13,12 +13,12 @@ typedef enum {
     FEATURE_FAILSAFE = 1 << 9,
     FEATURE_SONAR = 1 << 10,
     FEATURE_TELEMETRY = 1 << 11,
-    FEATURE_POWERMETER = 1 << 12,
+    FEATURE_CURRENT_METER = 1 << 12,
     FEATURE_VARIO = 1 << 13,
     FEATURE_3D = 1 << 14,
     FEATURE_RX_PARALLEL_PWM = 1 << 15,
     FEATURE_RX_MSP = 1 << 16,
-    FEATURE_RSSI_ADC = 1 << 17,
+    FEATURE_RSSI_ADC = 1 << 17
 } AvailableFeatures;
 
 bool feature(uint32_t mask);

src/drivers/adc_common.h

@@ -3,7 +3,8 @@
 typedef enum {
     ADC_BATTERY = 0,
     ADC_RSSI = 1,
-    ADC_EXTERNAL1 = 2,
+    ADC_CURRENT = 2,
+    ADC_EXTERNAL1 = 3,
     ADC_CHANNEL_MAX = ADC_EXTERNAL1
 } AdcChannel;
 
@@ -18,6 +19,7 @@ typedef struct adc_config_t {
 
 typedef struct drv_adc_config_t {
     bool enableRSSI;
+    bool enableCurrentMeter;
 } drv_adc_config_t;
 
 void adcInit(drv_adc_config_t *init);

src/drivers/adc_stm32f10x.c

@@ -12,11 +12,12 @@
 #include "adc_common.h"
 
 // Driver for STM32F103CB onboard ADC
-// VBAT is connected to PA4 (ADC1_IN4) with 10k:1k divider
-// rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
 //
-// RSSI ADC uses PA1
+// Battery Voltage (VBAT) is connected to PA4 (ADC1_IN4) with 10k:1k divider
-// An additional ADC source is available on CH8 (PB1, ADC1_IN9)
+// RSSI ADC uses CH2 (PA1, ADC1_IN1)
+// Current ADC uses CH8 (PB1, ADC1_IN9)
+//
+// NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
 
 extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
 extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
@@ -30,6 +31,13 @@ void adcInit(drv_adc_config_t *init)
     uint8_t configuredAdcChannels = 0;
     memset(&adcConfig, 0, sizeof(adcConfig));
 
+    if (init->enableRSSI) {
+        adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1;
+        adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
+        adcConfig[ADC_RSSI].enabled = true;
+        adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5;
+    }
+
     // configure always-present battery index (ADC4)
     adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4;
     adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
@@ -44,8 +52,8 @@ void adcInit(drv_adc_config_t *init)
         adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5;
     }
 
-    if (init->enableRSSI > 0) {
+    if (init->enableCurrentMeter) {
-        adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1;
+        adcConfig[ADC_RSSI].adcChannel = ADC_Channel_9;
         adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
         adcConfig[ADC_RSSI].enabled = true;
         adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5;

src/drivers/adc_stm32f30x.c

@@ -27,17 +27,24 @@ void adcInit(drv_adc_config_t *init)
     adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_6;
     adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
     adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5;
+    adcConfig[ADC_BATTERY].enabled = true;
     adcChannelCount++;
 
-    adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_7;
+    if (init->enableCurrentMeter) {
-    adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
+		adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_7;
-    adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5;
+		adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount;
+		adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5;
+		adcConfig[ADC_CURRENT].enabled = true;
 		adcChannelCount++;
+    }
 
+    if (init->enableRSSI) {
 		adcConfig[ADC_RSSI].adcChannel = ADC_Channel_8;
 		adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
 		adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5;
+		adcConfig[ADC_RSSI].enabled = true;
 		adcChannelCount++;
+    }
 
     DMA_DeInit(DMA1_Channel1);
 
@@ -98,8 +105,13 @@ void adcInit(drv_adc_config_t *init)
 
     ADC_Init(ADC1, &ADC_InitStructure);
 
-    for (i = 0; i < adcChannelCount; i++)
+    uint8_t rank = 1;
-        ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, i + 1, adcConfig[i].sampleTime);
+    for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
+        if (!adcConfig[i].enabled) {
+            continue;
+        }
+        ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime);
+    }
 
     ADC_Cmd(ADC1, ENABLE);
 

src/main.c

@@ -105,6 +105,7 @@ void init(void)
     systemInit(masterConfig.emf_avoidance);
 
     adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
+    adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
 
     adcInit(&adc_params);
 

src/mw.c

@@ -159,8 +159,8 @@ void annexCode(void)
     int32_t axis, prop1, prop2;
 
     static uint8_t batteryWarningEnabled = false;
-
     static uint8_t vbatTimer = 0;
+    static uint32_t vbatCycleTime = 0;
 
     // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
     if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) {
@@ -222,9 +222,17 @@ void annexCode(void)
         rcCommand[PITCH] = rcCommand_PITCH;
     }
 
-    if (feature(FEATURE_VBAT)) {
+    if (feature(FEATURE_VBAT || feature(FEATURE_CURRENT_METER))) {
+        vbatCycleTime += cycleTime;
         if (!(++vbatTimer % VBATFREQ)) {
+
+        	if (feature(FEATURE_VBAT)) {
         		updateBatteryVoltage();
+        	}
+
+        	if (feature(FEATURE_CURRENT_METER)) {
+        		updateCurrentMeter(vbatCycleTime);
+        	}
 
         	batteryWarningEnabled = shouldSoundBatteryAlarm();
         }

src/serial_cli.c

@@ -85,7 +85,7 @@ static const char * const mixerNames[] = {
 static const char * const featureNames[] = {
     "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
     "SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", "FAILSAFE",
-    "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", "RX_PARALLEL_PWM",
+    "SONAR", "TELEMETRY", "CURRENT_METER", "VARIO", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", NULL
 };
 
@@ -188,9 +188,13 @@ const clivalue_t valueTable[] = {
     { "telemetry_switch", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 },
     { "frsky_inversion", VAR_UINT8, &masterConfig.telemetryConfig.frsky_inversion, 0, 1 },
 
-    { "vbatscale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 },
+    { "vbat_scale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 },
-    { "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
+    { "vbat_max_cell_voltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
-    { "vbatmincellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 },
+    { "vbat_min_cell_voltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 },
+    { "current_meter_scale", VAR_UINT16, &masterConfig.batteryConfig.currentMeterScale, 1, 10000 },
+    { "current_meter_offset", VAR_UINT16, &masterConfig.batteryConfig.currentMeterOffset, 0, 1650 },
+    { "multiwii_current_meter_output", VAR_UINT8, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 },
+
 
     { "align_gyro", VAR_UINT8, &masterConfig.sensorAlignmentConfig.gyro_align, 0, 8 },
     { "align_acc", VAR_UINT8, &masterConfig.sensorAlignmentConfig.acc_align, 0, 8 },

src/serial_msp.c

@@ -618,10 +618,13 @@ static void evaluateCommand(void)
         break;
     case MSP_ANALOG:
         headSerialReply(7);
-        serialize8(vbat);
+        serialize8((uint8_t)constrain(vbat, 0, 255));
-        serialize16(0); // power meter trash
+        serialize16(mAhdrawn); // milliamphours drawn from battery
         serialize16(rssi);
-        serialize16(0); // amperage
+        if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
+            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);