diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0f31ce388..92e54368b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1227,7 +1227,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { - blackboxPrintfHeaderLine("currentSensor:%d,%d",currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC)->offset, currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC)->scale); + blackboxPrintfHeaderLine("currentSensor:%d,%d",currentSensorADCConfig()->offset, currentSensorADCConfig()->scale); } ); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 7e4044806..f4b4ad457 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -// FC configuration +// FC configuration (defined by cleanflight v1) #define PG_FAILSAFE_CONFIG 1 // struct OK #define PG_BOARD_ALIGNMENT 2 // struct OK #define PG_GIMBAL_CONFIG 3 // struct OK @@ -67,8 +67,8 @@ #define PG_SPECIAL_COLOR_CONFIG 46 // part of led strip, structs OK #define PG_PILOT_CONFIG 47 // does not exist in betaflight #define PG_MSP_SERVER_CONFIG 48 // does not exist in betaflight -#define PG_VOLTAGE_SENSOR_ADC_CONFIG 49 // renamed from PG_VOLTAGE_METER_CONFIG -#define PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG 50 // renamed from PG_AMPERAGE_METER_CONFIG +#define PG_VOLTAGE_METER_CONFIG 49 // renamed from PG_VOLTAGE_METER_CONFIG // deprecated +#define PG_AMPERAGE_METER_CONFIG 50 // renamed from PG_AMPERAGE_METER_CONFIG // deprecated #define PG_DEBUG_CONFIG 51 // does not exist in betaflight #define PG_SERVO_CONFIG 52 #define PG_IBUS_TELEMETRY_CONFIG 53 // CF 1.x @@ -78,6 +78,13 @@ #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight #define PG_DRIVER_FLASHCHIP_CONFIG 101 // does not exist in betaflight + +// cleanflight v2 specific parameter group ids start at 256 +#define PG_CURRENT_SENSOR_ADC_CONFIG 256 +#define PG_CURRENT_SENSOR_VIRTUAL_CONFIG 257 +#define PG_VOLTAGE_SENSOR_ADC_CONFIG 258 + + // betaflight specific parameter group ids start at 500 #define PG_BETAFLIGHT_START 500 #define PG_MODE_ACTIVATION_OPERATOR_CONFIG 500 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9a8cafb0d..35d235f53 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -636,10 +636,12 @@ static const clivalue_t valueTable[] = { // PG_VOLTAGE_SENSOR_ADC_CONFIG { "vbat_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_VOLTAGE_SENSOR_ADC_CONFIG, offsetof(voltageSensorADCConfig_t, vbatscale) }, -// PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG - // FIXME this will only allow configuration of the FIRST current meter. there are currently two - { "ibat_scale", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, offsetof(currentMeterADCOrVirtualConfig_t, scale) }, - { "ibat_offset", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, offsetof(currentMeterADCOrVirtualConfig_t, offset) }, +// PG_CURRENT_SENSOR_ADC_CONFIG + { "ibata_scale", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_ADC_CONFIG, offsetof(currentSensorADCConfig_t, scale) }, + { "ibata_offset", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_ADC_CONFIG, offsetof(currentSensorADCConfig_t, offset) }, +// PG_CURRENT_SENSOR_ADC_CONFIG + { "ibatv_scale", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_VIRTUAL_CONFIG, offsetof(currentMeterVirtualConfig_t, scale) }, + { "ibatv_offset", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_VIRTUAL_CONFIG, offsetof(currentMeterVirtualConfig_t, offset) }, // PG_BEEPER_DEV_CONFIG #ifdef BEEPER diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index feb5a7ab0..54d5b96d4 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -957,6 +957,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); // VOLTAGE_SENSOR_ADC_VBAT should be the first index, sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) { + sbufWriteU8(dst, voltageMeterADCtoIDMap[i]); // id of the sensor sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for sbufWriteU8(dst, 3); // ADC sensor sub-frame length sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale); @@ -970,16 +971,18 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn // the ADC and VIRTUAL sensors have the same configuration requirements, however this API reflects // that this situation may change and allows us to support configuration of any current sensor with // specialist configuration requirements. - BUILD_BUG_ON(CURRENT_SENSOR_VIRTUAL != 0); - BUILD_BUG_ON(CURRENT_SENSOR_ADC != 1); - sbufWriteU8(dst, MAX_ADC_OR_VIRTUAL_CURRENT_METERS); // current meters in payload - for (int i = CURRENT_SENSOR_VIRTUAL; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) { - sbufWriteU8(dst, i); // indicate the type of sensor that the next part of the payload is for - sbufWriteU8(dst, 4); // ADC or Virtual sensor sub-frame length - sbufWriteU16(dst, currentMeterADCOrVirtualConfig(i)->scale); - sbufWriteU16(dst, currentMeterADCOrVirtualConfig(i)->offset); - } + sbufWriteU8(dst, 2); // current meters in payload + sbufWriteU8(dst, 6); // ADC sensor sub-frame length + sbufWriteU8(dst, CURRENT_METER_ID_BATTERY_1); // the id of the sensor + sbufWriteU8(dst, CURRENT_SENSOR_ADC); // indicate the type of sensor that the next part of the payload is for + sbufWriteU16(dst, currentSensorADCConfig()->scale); + sbufWriteU16(dst, currentSensorADCConfig()->offset); + sbufWriteU8(dst, 6); // Virtual sensor sub-frame length + sbufWriteU8(dst, CURRENT_METER_ID_VIRTUAL_1); // the id of the sensor + sbufWriteU8(dst, CURRENT_SENSOR_VIRTUAL); // indicate the type of sensor that the next part of the payload is for + sbufWriteU16(dst, currentMeterVirtualConfig()->scale); + sbufWriteU16(dst, currentMeterVirtualConfig()->offset); // if we had any other current sensors, this is where we would output any needed configuration break; @@ -1823,37 +1826,46 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_VOLTAGE_METER_CONFIG: { - int sensor = sbufReadU8(src); - if (sensor != VOLTAGE_METER_ADC) { - return -1; + int id = sbufReadU8(src); + + // + // find and configure an ADC voltage sensor + // + int voltageSensorADCIndex; + for (voltageSensorADCIndex = 0; voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC; voltageSensorADCIndex++) { + if (id == voltageMeterADCtoIDMap[voltageSensorADCIndex]) { + break; + } } - int index = sbufReadU8(src); - if (index >= MAX_VOLTAGE_SENSOR_ADC) { + if (voltageSensorADCIndex < MAX_VOLTAGE_SENSOR_ADC) { + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatscale = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivval = sbufReadU8(src); + voltageSensorADCConfigMutable(voltageSensorADCIndex)->vbatresdivmultiplier = sbufReadU8(src); + } else { + // if we had any other types of voltage sensor to configure, this is where we'd do it. return -1; } - - - voltageSensorADCConfigMutable(index)->vbatscale = sbufReadU8(src); - voltageSensorADCConfigMutable(index)->vbatresdivval = sbufReadU8(src); - voltageSensorADCConfigMutable(index)->vbatresdivmultiplier = sbufReadU8(src); break; } case MSP_SET_CURRENT_METER_CONFIG: { - int sensor = sbufReadU8(src); - if (sensor != CURRENT_SENSOR_VIRTUAL || sensor != CURRENT_SENSOR_ADC) { - return -1; + int id = sbufReadU8(src); + + switch (id) { + case CURRENT_METER_ID_BATTERY_1: + currentSensorADCConfigMutable()->scale = sbufReadU16(src); + currentSensorADCConfigMutable()->offset = sbufReadU16(src); + break; + case CURRENT_METER_ID_VIRTUAL_1: + currentMeterVirtualConfigMutable()->scale = sbufReadU16(src); + currentMeterVirtualConfigMutable()->offset = sbufReadU16(src); + break; + + default: + return -1; } - int index = sbufReadU8(src); - - if (index >= MAX_ADC_OR_VIRTUAL_CURRENT_METERS) { - return -1; - } - - currentMeterADCOrVirtualConfigMutable(index)->scale = sbufReadU16(src); - currentMeterADCOrVirtualConfigMutable(index)->offset = sbufReadU16(src); break; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 6b81591f9..f56c2e4e0 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -46,9 +46,10 @@ * sensors are used to collect data. * - e.g. voltage at an MCU ADC input pin, value from an ESC sensor. * sensors require very specific configuration, such as resistor values. - * meters are used to process and expose data collected from sensors to the rest of the system + * meters are used to process and expose data collected from sensors to the rest of the system. * - e.g. a meter exposes normalized, and often filtered, values from a sensor. * meters require different or little configuration. + * meters also have different precision concerns, and may use different units to the sensors. * */ diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index 09e36c99b..712ccd3c6 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -37,7 +37,7 @@ #include "sensors/esc_sensor.h" const uint8_t currentMeterIds[] = { - CURRENT_METER_ID_VBAT_1, + CURRENT_METER_ID_BATTERY_1, CURRENT_METER_ID_VIRTUAL_1, #ifdef USE_ESC_SENSOR CURRENT_METER_ID_ESC_COMBINED_1, @@ -86,25 +86,20 @@ static biquadFilter_t adciBatFilter; #define CURRENT_METER_OFFSET_DEFAULT 0 #endif -PG_REGISTER_ARRAY_WITH_RESET_FN(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(currentSensorADCConfig_t, currentSensorADCConfig, PG_CURRENT_SENSOR_ADC_CONFIG, 0); -void pgResetFn_currentMeterADCOrVirtualConfig(currentMeterADCOrVirtualConfig_t *instance) -{ - for (int i = 0; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) { - if (i == CURRENT_METER_ADC) { - RESET_CONFIG(currentMeterADCOrVirtualConfig_t, &instance[i], - .scale = CURRENT_METER_SCALE_DEFAULT, - .offset = CURRENT_METER_OFFSET_DEFAULT, - ); - } - } -} +PG_RESET_TEMPLATE(currentSensorADCConfig_t, currentSensorADCConfig, + .scale = CURRENT_METER_SCALE_DEFAULT, + .offset = CURRENT_METER_OFFSET_DEFAULT, +); + +PG_REGISTER(currentMeterVirtualConfig_t, currentMeterVirtualConfig, PG_CURRENT_SENSOR_VIRTUAL_CONFIG, 0); static int32_t currentMeterADCToCentiamps(const uint16_t src) { int32_t millivolts; - const currentMeterADCOrVirtualConfig_t *config = currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC); + const currentSensorADCConfig_t *config = currentSensorADCConfig(); millivolts = ((uint32_t)src * ADCVREF) / 4096; millivolts -= config->offset; @@ -159,14 +154,14 @@ void currentMeterVirtualInit(void) void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset) { - currentMeterVirtualState.amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset; + currentMeterVirtualState.amperage = (int32_t)currentMeterVirtualConfig()->offset; if (armed) { if (throttleLowAndMotorStop) { throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz? - currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000; + currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentMeterVirtualConfig()->scale / 1000; } updateCurrentmAhDrawnState(¤tMeterVirtualState.mahDrawnState, currentMeterVirtualState.amperage, lastUpdateAt); } @@ -238,10 +233,12 @@ void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter) // // API for current meters using IDs // +// This API is used by MSP, for configuration/status. +// void currentMeterRead(currentMeterId_e id, currentMeter_t *meter) { - if (id == CURRENT_METER_ID_VBAT_1) { + if (id == CURRENT_METER_ID_BATTERY_1) { currentMeterADCRead(meter); } else if (id == CURRENT_METER_ID_VIRTUAL_1) { currentMeterVirtualRead(meter); diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h index 50b887523..937656f63 100644 --- a/src/main/sensors/current.h +++ b/src/main/sensors/current.h @@ -33,14 +33,6 @@ typedef struct currentMeter_s { int32_t mAhDrawn; // milliampere hours drawn from the battery since start } currentMeter_t; -// NOTE: currentMeterConfig is only used by physical and virtual current meters, not ESC based current meters. -#define MAX_ADC_OR_VIRTUAL_CURRENT_METERS 2 // 1x ADC, 1x Virtual - -typedef enum { - CURRENT_SENSOR_VIRTUAL = 0, // virtual is FIRST because it should be possible to build without ADC current sensors. - CURRENT_SENSOR_ADC, -} currentSensor_e; - // WARNING - do not mix usage of CURRENT_SENSOR_* and CURRENT_METER_*, they are separate concerns. typedef struct currentMeterMAhDrawnState_s { @@ -48,33 +40,59 @@ typedef struct currentMeterMAhDrawnState_s { float mAhDrawnF; } currentMeterMAhDrawnState_t; +// +// Sensors +// + +typedef enum { + CURRENT_SENSOR_VIRTUAL = 0, + CURRENT_SENSOR_ADC, + CURRENT_SENSOR_ESC, +} currentSensor_e; + + +// +// ADC +// + typedef struct currentMeterADCState_s { currentMeterMAhDrawnState_t mahDrawnState; 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) } currentMeterADCState_t; +typedef struct currentSensorADCConfig_s { + int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + uint16_t offset; // offset of the current sensor in millivolt steps +} currentSensorADCConfig_t; + +PG_DECLARE(currentSensorADCConfig_t, currentSensorADCConfig); + +// +// Virtual +// + typedef struct currentMeterVirtualState_s { currentMeterMAhDrawnState_t mahDrawnState; int32_t amperage; // current read by current sensor in centiampere (1/100th A) } currentMeterVirtualState_t; +typedef struct currentMeterVirtualConfig_s { + int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + uint16_t offset; // offset of the current sensor in millivolt steps +} currentMeterVirtualConfig_t; + +PG_DECLARE(currentMeterVirtualConfig_t, currentMeterVirtualConfig); + +// +// ESC +// + typedef struct currentMeterESCState_s { int32_t mAhDrawn; // milliampere hours drawn from the battery since start int32_t amperage; // current read by current sensor in centiampere (1/100th A) } currentMeterESCState_t; -typedef struct currentMeterADCOrVirtualConfig_s { - int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A - uint16_t offset; // offset of the current sensor in millivolt steps -} currentMeterADCOrVirtualConfig_t; - -PG_DECLARE_ARRAY(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig); - -// -// Main API -// - void currentMeterReset(currentMeter_t *meter); void currentMeterADCInit(void); diff --git a/src/main/sensors/current_ids.h b/src/main/sensors/current_ids.h index f61028e6c..2f366c6f1 100644 --- a/src/main/sensors/current_ids.h +++ b/src/main/sensors/current_ids.h @@ -24,10 +24,10 @@ 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_BATTERY_1 = 10, // 10-19 for battery meters + CURRENT_METER_ID_BATTERY_2, //.. - CURRENT_METER_ID_VBAT_10 = 19, + CURRENT_METER_ID_BATTERY_10 = 19, CURRENT_METER_ID_5V_1 = 20, // 20-29 for 5V meters CURRENT_METER_ID_5V_2, diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index 88a3fe1fa..22cc1105b 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -38,7 +38,7 @@ #include "sensors/esc_sensor.h" const uint8_t voltageMeterIds[] = { - VOLTAGE_METER_ID_VBAT_1, + VOLTAGE_METER_ID_BATTERY_1, #ifdef ADC_POWER_12V VOLTAGE_METER_ID_12V_1, #endif @@ -67,6 +67,7 @@ const uint8_t voltageMeterIds[] = { const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds); + // // ADC/ESC shared // @@ -237,12 +238,29 @@ void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter) } // -// API for voltage meters using IDs +// API for using voltage meters using IDs // +// This API is used by MSP, for configuration/status. +// + + +// the order of these much match the indexes in voltageSensorADC_e +const uint8_t voltageMeterADCtoIDMap[MAX_VOLTAGE_SENSOR_ADC] = { + VOLTAGE_METER_ID_BATTERY_1, +#ifdef ADC_POWER_12V + VOLTAGE_METER_ID_12V_1, +#endif +#ifdef ADC_POWER_9V + VOLTAGE_METER_ID_9V_1, +#endif +#ifdef ADC_POWER_5V + VOLTAGE_METER_ID_5V_1, +#endif +}; void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *meter) { - if (id == VOLTAGE_METER_ID_VBAT_1) { + if (id == VOLTAGE_METER_ID_BATTERY_1) { voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, meter); } else #ifdef ADC_POWER_12V diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index c8e6aff3d..4011e6714 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -29,11 +29,14 @@ typedef enum { VOLTAGE_METER_ESC } voltageMeterSource_e; +// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns. + typedef struct voltageMeter_s { uint16_t filtered; // voltage in 0.1V steps uint16_t unfiltered; // voltage in 0.1V steps } voltageMeter_t; + // // sensors // @@ -48,23 +51,23 @@ typedef enum { // adc sensors // -#ifndef MAX_VOLTAGE_SENSOR_ADC -#define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V and 5V meters. -#endif - -typedef enum { - VOLTAGE_SENSOR_ADC_VBAT = 0, - VOLTAGE_SENSOR_ADC_5V = 1, - VOLTAGE_SENSOR_ADC_12V = 2 -} voltageSensorADC_e; - -// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns. - #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 #define VBATT_LPF_FREQ 1.0f +#ifndef MAX_VOLTAGE_SENSOR_ADC +#define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V, 9V and 5V meters. +#endif + +typedef enum { + VOLTAGE_SENSOR_ADC_VBAT = 0, + VOLTAGE_SENSOR_ADC_12V = 1, + VOLTAGE_SENSOR_ADC_9V = 2, + VOLTAGE_SENSOR_ADC_5V = 3 +} voltageSensorADC_e; // see also voltageMeterADCtoIDMap + + typedef struct voltageSensorADCConfig_s { uint8_t vbatscale; // adjust this to match battery voltage to reported value uint8_t vbatresdivval; // resistor divider R2 (default NAZE 10(K)) @@ -89,8 +92,10 @@ void voltageMeterESCReadMotor(uint8_t motor, voltageMeter_t *voltageMeter); // -// API for reading current meters by id. +// API for reading/configuring current meters by id. // +extern const uint8_t voltageMeterADCtoIDMap[MAX_VOLTAGE_SENSOR_ADC]; + extern const uint8_t supportedVoltageMeterCount; extern const uint8_t voltageMeterIds[]; void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *voltageMeter); diff --git a/src/main/sensors/voltage_ids.h b/src/main/sensors/voltage_ids.h index ba3344fc2..e5c954aaa 100644 --- a/src/main/sensors/voltage_ids.h +++ b/src/main/sensors/voltage_ids.h @@ -24,10 +24,10 @@ 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_BATTERY_1 = 10, // 10-19 for battery meters + VOLTAGE_METER_ID_BATTERY_2, //.. - VOLTAGE_METER_ID_VBAT_10 = 19, + VOLTAGE_METER_ID_BATTERY_10 = 19, VOLTAGE_METER_ID_5V_1 = 20, // 20-29 for 5V meters VOLTAGE_METER_ID_5V_2, diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index e2e6ccb4a..eab7e6747 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -857,38 +857,17 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) case BST_VOLTAGE_METER_CONFIG: - BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); - for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) { - // note, by indicating a sensor type and a sub-frame length it's possible to configure any type of voltage meter, i.e. all sensors not built directly into the FC such as ESC/RX/SPort/SBus - bstWrite8(VOLTAGE_METER_ADC); // indicate the type of sensor that the next part of the payload is for - bstWrite8(3); // ADC sensor sub-frame length - bstWrite8(voltageSensorADCConfig(i)->vbatscale); - bstWrite8(voltageSensorADCConfig(i)->vbatresdivval); - bstWrite8(voltageSensorADCConfig(i)->vbatresdivmultiplier); - } - // if we had any other voltage sensors, this is where we would output any needed configuration - break; - - case BST_CURRENT_METER_CONFIG: - BUILD_BUG_ON(CURRENT_SENSOR_VIRTUAL != 0); - BUILD_BUG_ON(CURRENT_SENSOR_ADC != 1); - - for (int i = CURRENT_SENSOR_VIRTUAL; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) { - bstWrite8(i); // indicate the type of sensor that the next part of the payload is for - bstWrite8(4); // ADC or Virtual sensor sub-frame length - bstWrite16(currentMeterADCOrVirtualConfig(i)->scale); - bstWrite16(currentMeterADCOrVirtualConfig(i)->offset); - } - // if we had any other voltage sensors, this is where we would output any needed configuration - break; - - case BST_BATTERY_CONFIG: + bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); bstWrite8(batteryConfig()->vbatmincellvoltage); bstWrite8(batteryConfig()->vbatmaxcellvoltage); bstWrite8(batteryConfig()->vbatwarningcellvoltage); - bstWrite16(batteryConfig()->batteryCapacity); - bstWrite8(batteryConfig()->voltageMeterSource); + break; + + case BST_CURRENT_METER_CONFIG: + bstWrite16(currentSensorADCConfig()->scale); + bstWrite16(currentSensorADCConfig()->offset); bstWrite8(batteryConfig()->currentMeterSource); + bstWrite16(batteryConfig()->batteryCapacity); break; case BST_MIXER: @@ -938,8 +917,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(boardAlignment()->pitchDegrees); bstWrite16(boardAlignment()->yawDegrees); - bstWrite16(0); // was batteryConfig()->currentMeterScale); - bstWrite16(0); // was batteryConfig()->currentMeterOffset); + bstWrite16(currentSensorADCConfig()->scale); + bstWrite16(currentSensorADCConfig()->offset); break; case BST_CF_SERIAL_CONFIG: @@ -1156,7 +1135,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) compassConfigMutable()->mag_declination = bstRead16() * 10; - bstRead8(); // was batteryConfigMutable()->vbatscale // actual vbatscale as intended + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = bstRead8(); // actual vbatscale as intended batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert @@ -1287,48 +1266,17 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) boardAlignmentMutable()->pitchDegrees = bstRead16(); boardAlignmentMutable()->yawDegrees = bstRead16(); break; - - case BST_SET_VOLTAGE_METER_CONFIG: { - int sensor = bstRead8(); - if (sensor != VOLTAGE_METER_ADC) { - return -1; - } - - int index = bstRead8(); - if (index >= MAX_VOLTAGE_SENSOR_ADC) { - return -1; - } - - - voltageSensorADCConfigMutable(index)->vbatscale = bstRead8(); - voltageSensorADCConfigMutable(index)->vbatresdivval = bstRead8(); - voltageSensorADCConfigMutable(index)->vbatresdivmultiplier = bstRead8(); - break; - } - - case BST_SET_CURRENT_METER_CONFIG: { - int sensor = bstRead8(); - if (sensor != CURRENT_SENSOR_VIRTUAL || sensor != CURRENT_SENSOR_ADC) { - return -1; - } - - int index = bstRead8(); - - if (index >= MAX_ADC_OR_VIRTUAL_CURRENT_METERS) { - return -1; - } - - currentMeterADCOrVirtualConfigMutable(index)->scale = bstRead16(); - currentMeterADCOrVirtualConfigMutable(index)->offset = bstRead16(); - break; - } - - case BST_SET_BATTERY_CONFIG: - batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + case BST_SET_VOLTAGE_METER_CONFIG: + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = bstRead8(); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert - batteryConfigMutable()->batteryCapacity = bstRead16(); + break; + case BST_SET_CURRENT_METER_CONFIG: + currentSensorADCConfigMutable()->scale = bstRead16(); + currentSensorADCConfigMutable()->offset = bstRead16(); batteryConfigMutable()->currentMeterSource = bstRead8(); + batteryConfigMutable()->batteryCapacity = bstRead16(); break; #ifndef USE_QUAD_MIXER_ONLY @@ -1391,8 +1339,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) boardAlignmentMutable()->pitchDegrees = bstRead16(); // board_align_pitch boardAlignmentMutable()->yawDegrees = bstRead16(); // board_align_yaw - bstRead16(); // was batteryConfigMutable()->currentMeterScale - bstRead16(); // was batteryConfigMutable()->currentMeterOffset + currentSensorADCConfigMutable()->scale = bstRead16(); + currentSensorADCConfigMutable()->offset = bstRead16(); break; case BST_SET_CF_SERIAL_CONFIG: {