diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8aa92b2bdb..d1079d2b8c 100755 --- a/src/main/fc/fc_msp.c +++ b/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 diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 1e1ede53bc..8ba3ad9707 100644 --- a/src/main/sensors/battery.c +++ b/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; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index ea0912b181..d19e0ab7a8 100644 --- a/src/main/sensors/battery.h +++ b/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) diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h index be3dd50f1f..7ed293087f 100644 --- a/src/main/sensors/current.h +++ b/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) diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index 65a15887ca..035b025183 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -41,10 +41,33 @@ #define VBAT_SCALE_DEFAULT 110 #endif -#ifdef USE_ESC_SENSOR -static biquadFilter_t escvBatFilter; +#ifndef VBAT_RESDIVVAL_DEFAULT +#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 -#endif -} -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); + memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t)); + biquadFilterInitLPF(&voltageMeterESCState.filter, VBAT_LPF_FREQ, 50000); //50HZ Update #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); + } +} diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index 393deecaab..be5451af8a 100644 --- a/src/main/sensors/voltage.h +++ b/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);