From f3e740c5989a4ad2741c30abd607bafc5f0634b7 Mon Sep 17 00:00:00 2001 From: Hydra Date: Thu, 16 Mar 2017 22:11:17 +0000 Subject: [PATCH] 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 | 30 ++++++++++ src/main/sensors/battery.c | 9 ++- src/main/sensors/battery.h | 5 -- src/main/sensors/current.h | 43 ++++++++++++++ src/main/sensors/voltage.c | 116 +++++++++++++++++++++++++++++++------ src/main/sensors/voltage.h | 74 ++++++++++++++++++----- 6 files changed, 234 insertions(+), 43 deletions(-) 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);