CF/BF - refactor voltage refresh/read api. update voltage/current msp code

msp code is still unfinished, but the plan is clearer now.

move some defaults from battery.h and allow targets to override them.

define ID's for all voltage and current sensors, the idea being a list
of ids wil lbe created for each build which will allow all the voltage
and current meters to be exposed via MSP.

src/main/fc/fc_msp.c

@@ -909,6 +909,36 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU16(dst, boardAlignment()->yawDegrees);
         break;
 
+    case MSP_BATTERY_STATE: {
+        sbufWriteU8(dst, (uint8_t)constrain(getBatteryCellCount(), 0, 255)); // 0 indicates battery not detected.
+        sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); // in 0.1V steps
+        sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
+        // should we add batteryAmperage?
+        // should we add batteryPercentageRemaining?
+        // should we add batteryState (which is really a combined voltage AND consumption alert state)?
+        // should we add batteryVoltageState and batteryConsumptionState?
+        break;
+    }
+/*
+    case MSP_VOLTAGE_METERS:
+        // TODO write out voltage, once for each meter id we support
+        for (int i = 0; i < MAX_VOLTAGE_METERS; i++) {
+
+            uint16_t voltage = getVoltageMeter(i)->vbat;
+            sbufWriteU8(dst, (uint8_t)constrain(voltage, 0, 255));
+        }
+        break;
+
+    case MSP_CURRENT_METERS:
+        // TODO write out amperage meter for each meter id we support
+        for (int i = 0; i < MAX_AMPERAGE_METERS; i++) {
+            amperageMeter_t *meter = getAmperageMeter(i);
+            // write out amperage, once for each current meter.
+            sbufWriteU16(dst, (uint16_t)constrain(meter->amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
+            sbufWriteU32(dst, meter->mAhDrawn);
+        }
+        break;
+    */
     case MSP_VOLTAGE_METER_CONFIG:
         // by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
         // e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has

src/main/sensors/battery.c

@@ -107,13 +107,14 @@ void batteryUpdateVoltage(void)
 #ifdef USE_ESC_SENSOR
         case VOLTAGE_METER_ESC:
             if (feature(FEATURE_ESC_SENSOR)) {
-                voltageMeterESCUpdate(&voltageMeter);
+                voltageMeterESCRefresh();
+                voltageMeterESCReadCombined(&voltageMeter);
             }
             break;
 #endif
         case VOLTAGE_METER_ADC:
             voltageMeterADCRefresh();
-            voltageMeterADCUpdate(&voltageMeter, ADC_BATTERY);
+            voltageMeterADCRead(ADC_BATTERY, &voltageMeter);
             break;
 
         default:
@@ -262,9 +263,7 @@ void batteryInit(void)
     switch(batteryConfig()->voltageMeterSource) {
         case VOLTAGE_METER_ESC:
 #ifdef USE_ESC_SENSOR
-            if (feature(FEATURE_ESC_SENSOR)) {
+            voltageMeterESCInit();
-                voltageMeterESCInit();
-            }
 #endif
             break;
 

src/main/sensors/battery.h

@@ -23,11 +23,6 @@
 #include "sensors/current.h"
 #include "sensors/voltage.h"
 
-#define VBAT_RESDIVVAL_DEFAULT 10
-#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
-#define VBAT_SCALE_MIN 0
-#define VBAT_SCALE_MAX 255
-
 typedef struct batteryConfig_s {
     // voltage
     uint8_t vbatmaxcellvoltage;             // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)

src/main/sensors/current.h

@@ -26,6 +26,49 @@ typedef enum {
     CURRENT_METER_MAX = CURRENT_METER_ESC
 } currentMeterSource_e;
 
+//
+// fixed ids, current can be measured at many different places, these identifiers are the ones we support or would consider supporting.
+//
+
+typedef enum {
+    CURRENT_METER_ID_NONE = 0,
+
+    CURRENT_METER_ID_VBAT_1 = 10,       // 10-19 for battery meters
+    CURRENT_METER_ID_VBAT_2,
+    //..
+    CURRENT_METER_ID_VBAT_10 = 19,
+
+    CURRENT_METER_ID_5V_1 = 20,         // 20-29 for 5V meters
+    CURRENT_METER_ID_5V_2,
+    //..
+    CURRENT_METER_ID_5V_10 = 29,
+
+    CURRENT_METER_ID_9V_1 = 30,         // 30-39 for 9V meters
+    CURRENT_METER_ID_9V_2,
+    //..
+    CURRENT_METER_ID_9V_10 = 39,
+
+    CURRENT_METER_ID_12V_1 = 40,        // 40-49 for 12V meters
+    CURRENT_METER_ID_12V_2,
+    //..
+    CURRENT_METER_ID_12V_10 = 49,
+
+    CURRENT_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however)
+    // ...
+    CURRENT_METER_ID_ESC_COMBINED_10 = 59,
+
+    CURRENT_METER_ID_ESC_MOTOR_1 = 60,  // 60-79 for ESC motors (20 motors)
+    CURRENT_METER_ID_ESC_MOTOR_2,
+    CURRENT_METER_ID_ESC_MOTOR_3,
+    CURRENT_METER_ID_ESC_MOTOR_4,
+    CURRENT_METER_ID_ESC_MOTOR_5,
+    CURRENT_METER_ID_ESC_MOTOR_6,
+    CURRENT_METER_ID_ESC_MOTOR_7,
+    CURRENT_METER_ID_ESC_MOTOR_8,
+    //...
+    CURRENT_METER_ID_ESC_MOTOR_20 = 79,
+} currentMeterId_e;
+
 typedef struct currentMeter_s {
     int32_t amperage;           // current read by current sensor in centiampere (1/100th A)
     int32_t amperageLatest;     // current read by current sensor in centiampere (1/100th A) (unfiltered)

src/main/sensors/voltage.c

@@ -41,10 +41,33 @@
 #define VBAT_SCALE_DEFAULT 110
 #endif
 
-#ifdef USE_ESC_SENSOR
+#ifndef VBAT_RESDIVVAL_DEFAULT
-static biquadFilter_t escvBatFilter;
+#define VBAT_RESDIVVAL_DEFAULT 10
 #endif
 
+#ifndef VBAT_RESDIVMULTIPLIER_DEFAULT
+#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
+#endif
+
+#ifdef USE_ESC_SENSOR
+typedef struct voltageMeterESCState_s {
+    uint16_t voltageFiltered;         // battery voltage in 0.1V steps (filtered)
+    uint16_t voltageUnfiltered;       // battery voltage in 0.1V steps (unfiltered)
+    biquadFilter_t filter;
+} voltageMeterESCState_t;
+
+static voltageMeterESCState_t voltageMeterESCState;
+#endif
+
+typedef struct voltageMeterADCState_s {
+    uint16_t voltageFiltered;         // battery voltage in 0.1V steps (filtered)
+    uint16_t voltageUnfiltered;       // battery voltage in 0.1V steps (unfiltered)
+    biquadFilter_t filter;
+} voltageMeterADCState_t;
+
+extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
+
+
 voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
 
 voltageMeterADCState_t *getVoltageMeterADC(uint8_t index)
@@ -71,6 +94,9 @@ static const uint8_t voltageMeterAdcChannelMap[] = {
 #ifdef ADC_POWER_12V
     ADC_POWER_12V,
 #endif
+#ifdef ADC_POWER_9V
+    ADC_POWER_9V,
+#endif
 #ifdef ADC_POWER_5V
     ADC_POWER_5V,
 #endif
@@ -85,8 +111,6 @@ STATIC_UNIT_TESTED uint16_t voltageAdcToVoltage(const uint16_t src, const voltag
 
 void voltageMeterADCRefresh(void)
 {
-
-
     for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
         // store the battery voltage with some other recent battery voltage readings
 
@@ -96,7 +120,7 @@ void voltageMeterADCRefresh(void)
         uint8_t channel = voltageMeterAdcChannelMap[i];
         uint16_t rawSample = adcGetChannel(channel);
 
-        uint16_t filteredSample = biquadFilterApply(&state->vbatFilterState, rawSample);
+        uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample);
 
         // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
         state->voltageFiltered = voltageAdcToVoltage(filteredSample, config);
@@ -104,7 +128,7 @@ void voltageMeterADCRefresh(void)
     }
 }
 
-void voltageMeterADCUpdate(voltageMeter_t *voltageMeter, voltageSensorADC_e adcChannel)
+void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageMeter)
 {
     voltageMeterADCState_t *state = &voltageMeterADCStates[adcChannel];
 
@@ -120,7 +144,7 @@ void voltageMeterADCInit(void)
         voltageMeterADCState_t *state = &voltageMeterADCStates[i];
         memset(state, 0, sizeof(voltageMeterADCState_t));
 
-        biquadFilterInitLPF(&state->vbatFilterState, VBATT_LPF_FREQ, 50000);
+        biquadFilterInitLPF(&state->filter, VBATT_LPF_FREQ, 50000);
     }
 }
 
@@ -135,17 +159,73 @@ void voltageMeterReset(voltageMeter_t *meter)
 void voltageMeterESCInit(void)
 {
 #ifdef USE_ESC_SENSOR
-    biquadFilterInitLPF(&escvBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update
+    memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t));
-#endif
+    biquadFilterInitLPF(&voltageMeterESCState.filter, VBAT_LPF_FREQ, 50000); //50HZ Update
-}
-void voltageMeterESCUpdate(voltageMeter_t *voltageMeter)
-{
-#ifndef USE_ESC_SENSOR
-    UNUSED(voltageMeter);
-#else
-    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
-    voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
-    voltageMeter->filtered = biquadFilterApply(&escvBatFilter, voltageMeter->unfiltered);
 #endif
 }
 
+void voltageMeterESCRefresh(void)
+{
+#ifdef USE_ESC_SENSOR
+    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
+    voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
+    voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
+#endif
+}
+
+void voltageMeterESCReadMotor(uint8_t motorNumber, voltageMeter_t *voltageMeter)
+{
+#ifndef USE_ESC_SENSOR
+    UNUSED(motorNumber);
+    voltageMeterReset(voltageMeter);
+#else
+    escSensorData_t *escData = getEscSensorData(motorNumber);
+
+    voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
+    voltageMeter->filtered = voltageMeter->unfiltered; // no filtering for ESC motors currently.
+#endif
+}
+
+void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter)
+{
+#ifndef USE_ESC_SENSOR
+    voltageMeterReset(voltageMeter);
+#else
+    voltageMeter->filtered = voltageMeterESCState.voltageFiltered;
+    voltageMeter->unfiltered = voltageMeterESCState.voltageUnfiltered;
+#endif
+}
+
+void voltageReadMeter(voltageMeterId_e id, voltageMeter_t *voltageMeter)
+{
+    if (id == VOLTAGE_METER_ID_VBAT_1) {
+        voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, voltageMeter);
+    } else
+#ifdef ADC_POWER_12V
+    if (id == VOLTAGE_METER_ID_12V_1) {
+        voltageMeterADCRead(VOLTAGE_SENSOR_ADC_12V, voltageMeter);
+    } else
+#endif
+#ifdef ADC_POWER_9V
+    if (id == VOLTAGE_METER_ID_9V_1) {
+        voltageMeterADCRead(VOLTAGE_SENSOR_ADC_9V, voltageMeter);
+    } else
+#endif
+#ifdef ADC_POWER_5V
+    if (id == VOLTAGE_METER_ID_5V_1) {
+        voltageMeterADCRead(VOLTAGE_SENSOR_ADC_5V, voltageMeter);
+    } else
+#endif
+#ifdef USE_ESC_SENSOR
+    if (id == VOLTAGE_METER_ID_ESC_COMBINED_1) {
+        voltageMeterESCReadCombined(voltageMeter);
+    } else
+    if (id >= VOLTAGE_METER_ID_ESC_MOTOR_1 && id <= VOLTAGE_METER_ID_ESC_MOTOR_20 ) {
+        int motor = id - VOLTAGE_METER_ID_ESC_MOTOR_1;
+        voltageMeterESCReadMotor(motor, voltageMeter);
+    } else
+#endif
+    {
+        voltageMeterReset(voltageMeter);
+    }
+}

src/main/sensors/voltage.h

@@ -32,6 +32,56 @@ typedef struct voltageMeter_s {
     uint16_t unfiltered;                    // voltage in 0.1V steps
 } voltageMeter_t;
 
+//
+// fixed ids, voltage can be measured at many different places, these identifiers are the ones we support or would consider supporting.
+//
+
+typedef enum {
+    VOLTAGE_METER_ID_NONE = 0,
+
+    VOLTAGE_METER_ID_VBAT_1 = 10,       // 10-19 for battery meters
+    VOLTAGE_METER_ID_VBAT_2,
+    //..
+    VOLTAGE_METER_ID_VBAT_10 = 19,
+
+    VOLTAGE_METER_ID_5V_1 = 20,         // 20-29 for 5V meters
+    VOLTAGE_METER_ID_5V_2,
+    //..
+    VOLTAGE_METER_ID_5V_10 = 29,
+
+    VOLTAGE_METER_ID_9V_1 = 30,         // 30-39 for 9V meters
+    VOLTAGE_METER_ID_9V_2,
+    //..
+    VOLTAGE_METER_ID_9V_10 = 39,
+
+    VOLTAGE_METER_ID_12V_1 = 40,        // 40-49 for 12V meters
+    VOLTAGE_METER_ID_12V_2,
+    //..
+    VOLTAGE_METER_ID_12V_10 = 49,
+
+    VOLTAGE_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however)
+    // ...
+    VOLTAGE_METER_ID_ESC_COMBINED_10 = 59,
+
+    VOLTAGE_METER_ID_ESC_MOTOR_1 = 60,  // 60-79 for ESC motors (20 motors)
+    VOLTAGE_METER_ID_ESC_MOTOR_2,
+    VOLTAGE_METER_ID_ESC_MOTOR_3,
+    VOLTAGE_METER_ID_ESC_MOTOR_4,
+    VOLTAGE_METER_ID_ESC_MOTOR_5,
+    VOLTAGE_METER_ID_ESC_MOTOR_6,
+    VOLTAGE_METER_ID_ESC_MOTOR_7,
+    VOLTAGE_METER_ID_ESC_MOTOR_8,
+    //...
+    VOLTAGE_METER_ID_ESC_MOTOR_20 = 79,
+
+    VOLTAGE_METER_ID_CELL_1 = 80,       // 80-119 for cell meters (40 cells)
+    VOLTAGE_METER_ID_CELL_2,
+    //...
+    VOLTAGE_METER_ID_CELL_40 = 119,
+
+} voltageMeterId_e;
+
+
 //
 // sensors
 //
@@ -56,10 +106,8 @@ typedef enum {
     VOLTAGE_SENSOR_ADC_12V = 2
 } voltageSensorADC_e;
 
-// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_METER_*, they are separate concerns.
+// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns.
 
-#define VBAT_RESDIVVAL_DEFAULT 10
-#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
 #define VBAT_SCALE_MIN 0
 #define VBAT_SCALE_MAX 255
 
@@ -73,19 +121,15 @@ typedef struct voltageSensorADCConfig_s {
 
 PG_DECLARE_ARRAY(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig);
 
-typedef struct voltageMeterADCState_s {
-    uint16_t voltageFiltered;                // battery voltage in 0.1V steps (filtered)
-    uint16_t voltageUnfiltered;       // battery voltage in 0.1V steps (unfiltered)
-    biquadFilter_t vbatFilterState;
-} voltageMeterADCState_t; // TODO rename to voltageMeter_t
-
-extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
-
 void voltageMeterADCInit(void);
-void voltageMeterESCInit(void);
-
 void voltageMeterADCRefresh(void);
+void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageMeter);
+
+void voltageMeterESCInit(void);
+void voltageMeterESCRefresh(void);
+void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter);
+void voltageMeterESCReadMotor(uint8_t motor, voltageMeter_t *voltageMeter);
 
 void voltageMeterReset(voltageMeter_t *voltageMeter);
-void voltageMeterADCUpdate(voltageMeter_t *voltageMeter, voltageSensorADC_e adcChannel);
+
-void voltageMeterESCUpdate(voltageMeter_t *voltageMeter);
+void voltageReadMeter(voltageMeterId_e id, voltageMeter_t *voltageMeter);