From 067d3c0ac2645af283f0277c78d0d2183b0156bd Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 12 Mar 2017 11:26:30 +0000 Subject: [PATCH] CF/BF - First cut of Current/Voltage/Battery cleanup. many refactorings, bugs squished, concerns separated, single-responsibility violations fixed and performance optimizations. --- Makefile | 2 + src/main/blackbox/blackbox.c | 17 +- src/main/cms/cms_menu_misc.c | 14 +- src/main/config/parameter_group_ids.h | 7 +- src/main/drivers/adc.h | 2 +- src/main/fc/cli.c | 37 +- src/main/fc/config.c | 19 +- src/main/fc/config.h | 4 +- src/main/fc/fc_init.c | 11 +- src/main/fc/fc_msp.c | 119 +++++-- src/main/fc/fc_tasks.c | 54 ++- src/main/io/beeper.c | 7 +- src/main/io/dashboard.c | 14 +- src/main/io/ledstrip.c | 4 +- src/main/io/osd.c | 37 +- src/main/msp/msp_protocol.h | 8 +- src/main/rx/crsf.h | 2 +- src/main/rx/jetiexbus.c | 6 +- src/main/scheduler/scheduler.h | 3 +- src/main/sensors/battery.c | 429 ++++++++++++----------- src/main/sensors/battery.h | 72 ++-- src/main/sensors/current.c | 129 +++++++ src/main/sensors/current.h | 59 ++++ src/main/sensors/esc_sensor.c | 2 - src/main/sensors/esc_sensor.h | 2 + src/main/sensors/voltage.c | 151 ++++++++ src/main/sensors/voltage.h | 91 +++++ src/main/target/AIRHEROF3/target.h | 1 - src/main/target/ALIENFLIGHTF4/config.c | 8 +- src/main/target/ALIENFLIGHTNGF7/config.c | 8 +- src/main/target/BETAFLIGHTF3/config.c | 3 +- src/main/target/BETAFLIGHTF3/target.h | 3 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 111 ++++-- src/main/target/COLIBRI_RACE/target.h | 2 +- src/main/target/ELLE0/target.h | 1 - src/main/target/F4BY/target.h | 2 +- src/main/target/FISHDRONEF4/target.h | 2 +- src/main/target/FURYF4/target.h | 2 +- src/main/target/IMPULSERCF3/target.h | 2 +- src/main/target/KISSFC/target.h | 1 - src/main/target/KIWIF4/target.h | 2 +- src/main/target/KROOZX/config.c | 10 +- src/main/target/KROOZX/target.h | 3 +- src/main/target/MULTIFLITEPICO/config.c | 7 +- src/main/target/MULTIFLITEPICO/target.h | 2 +- src/main/target/OMNIBUS/target.h | 5 +- src/main/target/OMNIBUSF4/target.h | 4 +- src/main/target/PIKOBLX/config.c | 4 +- src/main/target/RACEBASE/target.h | 2 +- src/main/target/RCEXPLORERF3/target.h | 1 - src/main/target/RG_SSD_F3/config.c | 1 - src/main/target/SINGULARITY/target.h | 2 +- src/main/target/SPRACINGF3/target.h | 3 +- src/main/target/SPRACINGF3EVO/target.h | 3 +- src/main/target/SPRACINGF3NEO/target.h | 4 +- src/main/target/TINYFISH/config.c | 19 +- src/main/target/TINYFISH/target.h | 3 +- src/main/target/VRRACE/target.h | 2 +- src/main/telemetry/crsf.c | 6 +- src/main/telemetry/frsky.c | 20 +- src/main/telemetry/hott.c | 12 +- src/main/telemetry/ibus_shared.c | 4 +- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 6 +- src/main/telemetry/smartport.c | 18 +- src/main/telemetry/srxl.c | 8 +- src/test/unit/telemetry_crsf_unittest.cc | 36 +- src/test/unit/telemetry_hott_unittest.cc | 21 +- src/test/unit/telemetry_ibus_unittest.cc | 34 +- 69 files changed, 1178 insertions(+), 514 deletions(-) create mode 100644 src/main/sensors/current.c create mode 100644 src/main/sensors/current.h create mode 100644 src/main/sensors/voltage.c create mode 100644 src/main/sensors/voltage.h diff --git a/Makefile b/Makefile index 0e09a1b3d1..432f6533bf 100644 --- a/Makefile +++ b/Makefile @@ -641,6 +641,8 @@ COMMON_SRC = \ scheduler/scheduler.c \ sensors/acceleration.c \ sensors/battery.c \ + sensors/current.c \ + sensors/voltage.c \ sensors/boardalignment.c \ sensors/compass.c \ sensors/gyro.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 08c4150f0c..0f31ce388e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -425,10 +425,10 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) #endif case FLIGHT_LOG_FIELD_CONDITION_VBAT: - return feature(FEATURE_VBAT); + return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE; case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC: - return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC; + return batteryConfig()->currentMeterSource == CURRENT_METER_ADC; case FLIGHT_LOG_FIELD_CONDITION_SONAR: #ifdef SONAR @@ -841,7 +841,7 @@ void startBlackbox(void) blackboxHistory[1] = &blackboxHistoryRing[1]; blackboxHistory[2] = &blackboxHistoryRing[2]; - vbatReference = vbatLatest; + vbatReference = getBatteryVoltageLatest(); //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it @@ -1037,8 +1037,8 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->motor[i] = motor[i]; } - blackboxCurrent->vbatLatest = vbatLatest; - blackboxCurrent->amperageLatest = amperageLatest; + blackboxCurrent->vbatLatest = getBatteryVoltageLatest(); + blackboxCurrent->amperageLatest = getAmperageLatest(); #ifdef MAG for (i = 0; i < XYZ_AXIS_COUNT; i++) { @@ -1214,7 +1214,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { - blackboxPrintfHeaderLine("vbatscale:%u", batteryConfig()->vbatscale); + blackboxPrintfHeaderLine("vbatscale:%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } @@ -1226,9 +1226,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( - //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too: - if (feature(FEATURE_CURRENT_METER)) { - blackboxPrintfHeaderLine("currentMeter:%d,%d", batteryConfig()->currentMeterOffset, batteryConfig()->currentMeterScale); + if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { + blackboxPrintfHeaderLine("currentSensor:%d,%d",currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC)->offset, currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC)->scale); } ); diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 14453d7957..fe5bb9d3be 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -91,14 +91,14 @@ CMS_Menu cmsx_menuRcPreview = { static uint16_t motorConfig_minthrottle; static uint8_t motorConfig_digitalIdleOffsetPercent; -static uint8_t batteryConfig_vbatscale; +static uint8_t voltageSensorADCConfig_vbatscale; static uint8_t batteryConfig_vbatmaxcellvoltage; static long cmsx_menuMiscOnEnter(void) { motorConfig_minthrottle = motorConfig()->minthrottle; motorConfig_digitalIdleOffsetPercent = 10 * motorConfig()->digitalIdleOffsetPercent; - batteryConfig_vbatscale = batteryConfig()->vbatscale; + voltageSensorADCConfig_vbatscale = voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale; batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage; return 0; } @@ -109,7 +109,7 @@ static long cmsx_menuMiscOnExit(const OSD_Entry *self) motorConfigMutable()->minthrottle = motorConfig_minthrottle; motorConfigMutable()->digitalIdleOffsetPercent = motorConfig_digitalIdleOffsetPercent / 10.0f; - batteryConfigMutable()->vbatscale = batteryConfig_vbatscale; + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = voltageSensorADCConfig_vbatscale; batteryConfigMutable()->vbatmaxcellvoltage = batteryConfig_vbatmaxcellvoltage; return 0; } @@ -118,10 +118,10 @@ static OSD_Entry menuMiscEntries[]= { { "-- MISC --", OME_Label, NULL, NULL, 0 }, - { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 }, 0 }, - { "DIGITAL IDLE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &motorConfig_digitalIdleOffsetPercent, 0, 200, 1, 100 }, 0 }, - { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatscale, 1, 250, 1 }, 0 }, - { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 }, + { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 }, 0 }, + { "DIGITAL IDLE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &motorConfig_digitalIdleOffsetPercent, 0, 200, 1, 100 }, 0 }, + { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &voltageSensorADCConfig_vbatscale, 1, 250, 1 }, 0 }, + { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 }, { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, { "BACK", OME_Back, NULL, NULL, 0}, diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 47eb1b7f2a..7e40448066 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -67,9 +67,12 @@ #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_METER_CONFIG 49 // Cleanflight has voltageMeterConfig_t, betaflight has batteryConfig_t -#define PG_AMPERAGE_METER_CONFIG 50 // Cleanflight has amperageMeterConfig_t, betaflight has batteryConfig_t +#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_DEBUG_CONFIG 51 // does not exist in betaflight #define PG_SERVO_CONFIG 52 +#define PG_IBUS_TELEMETRY_CONFIG 53 // CF 1.x +//#define PG_VTX_CONFIG 54 // CF 1.x // Driver configuration #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index f4270cdb2d..89b0dccd78 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -43,7 +43,7 @@ typedef struct adcChannelConfig_t { typedef struct adcConfig_s { adcChannelConfig_t vbat; adcChannelConfig_t rssi; - adcChannelConfig_t currentMeter; + adcChannelConfig_t currentMeter; // FIXME rename to current/amperage adcChannelConfig_t external1; } adcConfig_t; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a35c6c2a1e..1c64370294 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -249,7 +249,7 @@ static const char * const lookupTableCurrentSensor[] = { }; static const char * const lookupTableBatterySensor[] = { - "ADC", "ESC" + "NONE", "ADC", "ESC" }; #ifdef USE_SERVOS @@ -339,7 +339,7 @@ static const char * const lookupTableSuperExpoYaw[] = { static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", #ifdef USE_DSHOT - "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", + "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200" #endif }; @@ -375,8 +375,8 @@ typedef enum { #ifdef BLACKBOX TABLE_BLACKBOX_DEVICE, #endif - TABLE_CURRENT_SENSOR, - TABLE_BATTERY_SENSOR, + TABLE_CURRENT_METER, + TABLE_VOLTAGE_METER, #ifdef USE_SERVOS TABLE_GIMBAL_MODE, #endif @@ -622,21 +622,26 @@ static const clivalue_t valueTable[] = { // PG_BATTERY_CONFIG { "bat_capacity", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 20000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batteryCapacity) }, - { "vbat_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatscale) }, { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmaxcellvoltage) }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmincellvoltage) }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatwarningcellvoltage) }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 250 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbathysteresis) }, - { "ibat_scale", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterScale) }, - { "ibat_offset", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterOffset) }, { "mwii_ibat_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, multiwiiCurrentMeterOutput) }, - { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CURRENT_SENSOR }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterType) }, - { "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BATTERY_SENSOR }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batteryMeterType) }, - { "bat_detect_thresh", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batterynotpresentlevel) }, + { "current_meter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CURRENT_METER }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterSource) }, + { "battery_meter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_VOLTAGE_METER }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, voltageMeterSource) }, + { "bat_detect_thresh", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batteryNotPresentLevel) }, { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useVBatAlerts) }, { "use_cbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useConsumptionAlerts) }, { "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, consumptionWarningPercentage) }, +// 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_BEEPER_DEV_CONFIG #ifdef BEEPER { "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, isInverted) }, @@ -3739,7 +3744,7 @@ static void cliStatus(char *cmdline) UNUSED(cmdline); cliPrintf("System Uptime: %d seconds\r\n", millis() / 1000); - cliPrintf("Voltage: %d * 0.1V (%dS battery - %s)\r\n", getVbat(), batteryCellCount, getBatteryStateString()); + cliPrintf("Voltage: %d * 0.1V (%dS battery - %s)\r\n", getBatteryVoltage(), getBatteryCellCount(), getBatteryStateString()); cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); @@ -3800,7 +3805,7 @@ static void cliTasks(char *cmdline) #ifndef MINIMAL_CLI if (systemConfig()->task_statistics) { - cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); + cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); } else { cliPrintf("Task list\r\n"); } @@ -3815,14 +3820,14 @@ static void cliTasks(char *cmdline) subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom; if (pidConfig()->pid_process_denom > 1) { - cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName); + cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); } else { taskFrequency = subTaskFrequency; - cliPrintf("%02d - (%9s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); + cliPrintf("%02d - (%11s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); } } else { taskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); - cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName); + cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); } const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000; const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000; @@ -3838,7 +3843,7 @@ static void cliTasks(char *cmdline) cliPrintf("%6d\r\n", taskFrequency); } if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) { - cliPrintf(" - (%13s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency); + cliPrintf(" - (%15s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency); } } } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index fe7592d628..42e7be6d4a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -113,7 +113,7 @@ pidProfile_t *currentPidProfile; PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, - .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_FAILSAFE + .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_FAILSAFE ); PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); @@ -503,12 +503,6 @@ void createDefaultConfig(master_t *config) #endif #endif // USE_PARAMETER_GROUPS -#ifdef BOARD_HAS_VOLTAGE_DIVIDER - // only enable the VBAT feature by default if the board has a voltage divider otherwise - // the user may see incorrect readings and unexpected issues with pin mappings may occur. - intFeatureSet(FEATURE_VBAT, featuresPtr); -#endif - config->version = EEPROM_CONF_VERSION; // global settings @@ -873,8 +867,8 @@ void validateAndFixConfig(void) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); // current meter needs the same ports - if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) { - featureClear(FEATURE_CURRENT_METER); + if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { + batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; } #endif // software serial needs free PWM ports @@ -883,14 +877,15 @@ void validateAndFixConfig(void) #ifdef USE_SOFTSPI if (featureConfigured(FEATURE_SOFTSPI)) { - featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL | FEATURE_VBAT); + featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL); + batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE; #if defined(STM32F10X) featureClear(FEATURE_LED_STRIP); // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); // current meter needs the same ports - if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) { - featureClear(FEATURE_CURRENT_METER); + if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { + batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; } #endif } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 254640b9b3..ea7d3b5815 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -34,7 +34,7 @@ typedef enum { FEATURE_RX_PPM = 1 << 0, - FEATURE_VBAT = 1 << 1, + //FEATURE_VBAT = 1 << 1, FEATURE_INFLIGHT_ACC_CAL = 1 << 2, FEATURE_RX_SERIAL = 1 << 3, FEATURE_MOTOR_STOP = 1 << 4, @@ -44,7 +44,7 @@ typedef enum { FEATURE_FAILSAFE = 1 << 8, FEATURE_SONAR = 1 << 9, FEATURE_TELEMETRY = 1 << 10, - FEATURE_CURRENT_METER = 1 << 11, + //FEATURE_CURRENT_METER = 1 << 11, FEATURE_3D = 1 << 12, FEATURE_RX_PARALLEL_PWM = 1 << 13, FEATURE_RX_MSP = 1 << 14, diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7ae10f1bd8..017744018a 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -377,9 +377,9 @@ void init(void) #endif #ifdef USE_ADC - /* these can be removed from features! */ - adcConfigMutable()->vbat.enabled = feature(FEATURE_VBAT); - adcConfigMutable()->currentMeter.enabled = feature(FEATURE_CURRENT_METER); + adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC); + adcConfigMutable()->currentMeter.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC); + adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC); adcInit(adcConfig()); #endif @@ -556,10 +556,7 @@ void init(void) serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif - // Now that everything has powered up the voltage and cell count be determined. - - if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) - batteryInit(); + batteryInit(); // always needs doing, regardless of features. #ifdef USE_DASHBOARD if (feature(FEATURE_DASHBOARD)) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5f09cf5b9d..4c3a8ac5f6 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -727,16 +727,18 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif break; - case MSP_ANALOG: - sbufWriteU8(dst, (uint8_t)constrain(getVbat(), 0, 255)); - sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery + case MSP_ANALOG: { + sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); + sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery sbufWriteU16(dst, rssi); - if(batteryConfig()->multiwiiCurrentMeterOutput) { - sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero - } else - sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A - break; + if(batteryConfig()->multiwiiCurrentMeterOutput) { + sbufWriteU16(dst, (uint16_t)constrain(getAmperage() * 10, 0, 0xFFFF)); // send current in 0.001 A steps. Negative range is truncated to zero + } else + sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A + + break; + } case MSP_ARMING_CONFIG: sbufWriteU8(dst, armingConfig()->auto_disarm_delay); sbufWriteU8(dst, armingConfig()->disarm_kill_switch); @@ -839,7 +841,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, compassConfig()->mag_declination / 10); - sbufWriteU8(dst, batteryConfig()->vbatscale); + sbufWriteU8(dst, 0); // batteryConfig()->vbatscale sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); @@ -912,18 +914,40 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_VOLTAGE_METER_CONFIG: - sbufWriteU8(dst, batteryConfig()->vbatscale); - sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); - sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); - sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); - sbufWriteU8(dst, batteryConfig()->batteryMeterType); + BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0); + sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload + 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 + 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); + sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval); + sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier); + } + // if we had any other voltage sensors, this is where we would output any needed configuration break; case MSP_CURRENT_METER_CONFIG: - sbufWriteU16(dst, batteryConfig()->currentMeterScale); - sbufWriteU16(dst, batteryConfig()->currentMeterOffset); - sbufWriteU8(dst, batteryConfig()->currentMeterType); + 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); + } + // if we had any other current sensors, this is where we would output any needed configuration + break; + + case MSP_BATTERY_CONFIG: + sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); + sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); sbufWriteU16(dst, batteryConfig()->batteryCapacity); + sbufWriteU8(dst, batteryConfig()->voltageMeterSource); + sbufWriteU8(dst, batteryConfig()->currentMeterSource); break; case MSP_MIXER: @@ -982,8 +1006,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, boardAlignment()->pitchDegrees); sbufWriteU16(dst, boardAlignment()->yawDegrees); - sbufWriteU16(dst, batteryConfig()->currentMeterScale); - sbufWriteU16(dst, batteryConfig()->currentMeterOffset); + sbufWriteU16(dst, 0); // was currentMeterScale, see MSP_CURRENT_METER_CONFIG + sbufWriteU16(dst, 0); //was currentMeterOffset, see MSP_CURRENT_METER_CONFIG break; case MSP_CF_SERIAL_CONFIG: @@ -1417,7 +1441,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; - batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + sbufReadU8(src); // actual vbatscale as intended // was batteryConfigMutable()->vbatscale batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert @@ -1756,21 +1780,48 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) boardAlignmentMutable()->yawDegrees = sbufReadU16(src); break; - case MSP_SET_VOLTAGE_METER_CONFIG: - batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert - if (dataSize > 4) { - batteryConfigMutable()->batteryMeterType = sbufReadU8(src); + case MSP_SET_VOLTAGE_METER_CONFIG: { + int sensor = sbufReadU8(src); + if (sensor != VOLTAGE_METER_ADC) { + return -1; } - break; - case MSP_SET_CURRENT_METER_CONFIG: - batteryConfigMutable()->currentMeterScale = sbufReadU16(src); - batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); - batteryConfigMutable()->currentMeterType = sbufReadU8(src); + int index = sbufReadU8(src); + if (index >= MAX_VOLTAGE_SENSOR_ADC) { + 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 index = sbufReadU8(src); + + if (index >= MAX_ADC_OR_VIRTUAL_CURRENT_METERS) { + return -1; + } + + currentMeterADCOrVirtualConfigMutable(index)->scale = sbufReadU16(src); + currentMeterADCOrVirtualConfigMutable(index)->offset = sbufReadU16(src); + break; + } + + case MSP_SET_BATTERY_CONFIG: + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert batteryConfigMutable()->batteryCapacity = sbufReadU16(src); + batteryConfigMutable()->voltageMeterSource = sbufReadU8(src); + batteryConfigMutable()->currentMeterSource = sbufReadU8(src); break; #ifndef USE_QUAD_MIXER_ONLY @@ -1849,8 +1900,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw - batteryConfigMutable()->currentMeterScale = sbufReadU16(src); - batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); + sbufReadU16(src); // was currentMeterScale, see MSP_SET_CURRENT_METER_CONFIG + sbufReadU16(src); // was currentMeterOffset see MSP_SET_CURRENT_METER_CONFIG break; case MSP_SET_CF_SERIAL_CONFIG: diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 357c76abf4..9180e6c336 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -26,6 +26,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/utils.h" +#include "common/filter.h" #include "config/feature.h" #include "config/config_profile.h" @@ -114,29 +115,44 @@ static void taskHandleSerial(timeUs_t currentTimeUs) mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); } -static void taskUpdateBattery(timeUs_t currentTimeUs) + +void taskBatterySensors(timeUs_t currentTimeUs) { -#if defined(USE_ADC) || defined(USE_ESC_SENSOR) - if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) { - static uint32_t vbatLastServiced = 0; + static uint32_t vbatLastServiced = 0; + + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTimeUs; - updateBattery(); + + batteryUpdateVoltage(); } } -#endif - if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) { + if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { + static uint32_t ibatLastServiced = 0; const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - updateCurrentMeter(ibatTimeSinceLastServiced); + batteryUpdateCurrentMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED)); } } } +void taskBatteryAlerts(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (!ARMING_FLAG(ARMED)) { + // the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup. + batteryUpdatePresence(); + } + batteryUpdateStates(); + + batteryUpdateAlarms(); +} + static void taskUpdateRxMain(timeUs_t currentTimeUs) { processRx(currentTimeUs); @@ -237,7 +253,13 @@ void fcTasksInit(void) setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz)); - setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); + + bool useBatterySensors = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE || batteryConfig()->currentMeterSource != CURRENT_METER_NONE; + setTaskEnabled(TASK_BATTERY_SENSORS, useBatterySensors); + + bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || feature(FEATURE_OSD); + setTaskEnabled(TASK_BATTERY_ALERTS, useBatterySensors && useBatteryAlerts); + setTaskEnabled(TASK_RX, true); setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled()); @@ -366,13 +388,19 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_HIGH, }, - [TASK_BATTERY] = { - .taskName = "BATTERY", - .taskFunc = taskUpdateBattery, - .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz + [TASK_BATTERY_ALERTS] = { + .taskName = "BATTERY_ALERTS", + .taskFunc = taskBatteryAlerts, + .desiredPeriod = TASK_PERIOD_HZ(5), // 5 Hz .staticPriority = TASK_PRIORITY_MEDIUM, }, + [TASK_BATTERY_SENSORS] = { + .taskName = "BATTERY_SENSORS", + .taskFunc = taskBatterySensors, + .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, #ifdef BEEPER [TASK_BEEPER] = { .taskName = "BEEPER", diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 67233aa9b5..7327fafcb0 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -208,7 +208,12 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL; */ void beeper(beeperMode_e mode) { - if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (batteryCellCount == 0)))) { + if ( + mode == BEEPER_SILENCE || ( + (getBeeperOffMask() & (1 << (BEEPER_USB - 1))) + && (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && (getBatteryCellCount() == 0)) + ) + ) { beeperSilence(); return; } diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index f07e3e8f11..2a65a6e63a 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -452,24 +452,26 @@ void showBatteryPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - if (feature(FEATURE_VBAT)) { - tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getVbat() / 10, getVbat() % 10, batteryCellCount); + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { + tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount()); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - uint8_t batteryPercentage = calculateBatteryPercentage(); + uint8_t batteryPercentage = calculateBatteryPercentageRemaining(); i2c_OLED_set_line(rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage); } - if (feature(FEATURE_CURRENT_METER)) { - tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, mAhDrawn); + if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { + + int32_t amperage = getAmperage(); + tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn()); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - uint8_t capacityPercentage = calculateBatteryPercentage(); + uint8_t capacityPercentage = calculateBatteryPercentageRemaining(); i2c_OLED_set_line(rowIndex++); drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage); } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index ee4841e43d..ca84e77b5e 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -472,7 +472,7 @@ static void applyLedFixedLayers() case LED_FUNCTION_BATTERY: color = HSV(RED); - hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120); + hOffset += scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120); break; case LED_FUNCTION_RSSI: @@ -522,7 +522,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) if (warningFlashCounter == 0) { // update when old flags was processed warningFlags = 0; - if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryState() != BATTERY_OK) warningFlags |= 1 << WARNING_LOW_BATTERY; if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) warningFlags |= 1 << WARNING_FAILSAFE; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 00797ab1f6..890c59e040 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -192,12 +192,13 @@ static void osdDrawSingleElement(uint8_t item) case OSD_MAIN_BATT_VOLTAGE: { buff[0] = SYM_BATT_5; - sprintf(buff + 1, "%d.%1dV", getVbat() / 10, getVbat() % 10); + sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10); break; } case OSD_CURRENT_DRAW: { + int32_t amperage = getAmperage(); buff[0] = SYM_AMP; sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100); break; @@ -206,7 +207,7 @@ static void osdDrawSingleElement(uint8_t item) case OSD_MAH_DRAWN: { buff[0] = SYM_MAH; - sprintf(buff + 1, "%d", mAhDrawn); + sprintf(buff + 1, "%d", getMAhDrawn()); break; } @@ -401,7 +402,7 @@ static void osdDrawSingleElement(uint8_t item) case OSD_POWER: { - sprintf(buff, "%dW", amperage * getVbat() / 1000); + sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000); break; } @@ -415,16 +416,24 @@ static void osdDrawSingleElement(uint8_t item) case OSD_MAIN_BATT_WARNING: { - if (getVbat() > (batteryWarningVoltage - 1)) - return; + switch(getBatteryState()) { + case BATTERY_WARNING: + sprintf(buff, "LOW BATTERY"); + break; - sprintf(buff, "LOW VOLTAGE"); + case BATTERY_CRITICAL: + sprintf(buff, " LAND NOW"); + break; + + default: + break; + } break; } case OSD_AVG_CELL_VOLTAGE: { - uint16_t cellV = getVbat() * 10 / batteryCellCount; + uint16_t cellV = getBatteryVoltage() * 10 / getBatteryCellCount(); buff[0] = SYM_BATT_5; sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100); break; @@ -582,7 +591,7 @@ void osdUpdateAlarms(void) else CLR_BLINK(OSD_RSSI_VALUE); - if (getVbat() <= (batteryWarningVoltage - 1)) { + if (getBatteryState() != BATTERY_OK) { SET_BLINK(OSD_MAIN_BATT_VOLTAGE); SET_BLINK(OSD_MAIN_BATT_WARNING); SET_BLINK(OSD_AVG_CELL_VOLTAGE); @@ -602,7 +611,7 @@ void osdUpdateAlarms(void) else CLR_BLINK(OSD_FLYTIME); - if (mAhDrawn >= osdConfig()->cap_alarm) + if (getMAhDrawn() >= osdConfig()->cap_alarm) SET_BLINK(OSD_MAH_DRAWN); else CLR_BLINK(OSD_MAH_DRAWN); @@ -644,10 +653,10 @@ static void osdUpdateStats(void) if (stats.max_speed < value) stats.max_speed = value; - if (stats.min_voltage > getVbat()) - stats.min_voltage = getVbat(); + if (stats.min_voltage > getBatteryVoltage()) + stats.min_voltage = getBatteryVoltage(); - value = amperage / 100; + value = getAmperage() / 100; if (stats.max_current < value) stats.max_current = value; @@ -722,14 +731,14 @@ static void osdShowStats(void) strcat(buff, "%"); displayWrite(osdDisplayPort, 22, top++, buff); - if (feature(FEATURE_CURRENT_METER)) { + if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { displayWrite(osdDisplayPort, 2, top, "MAX CURRENT :"); itoa(stats.max_current, buff, 10); strcat(buff, "A"); displayWrite(osdDisplayPort, 22, top++, buff); displayWrite(osdDisplayPort, 2, top, "USED MAH :"); - itoa(mAhDrawn, buff, 10); + itoa(getMAhDrawn(), buff, 10); strcat(buff, "\x07"); displayWrite(osdDisplayPort, 22, top++, buff); } diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index d2c368f28a..830bf36f4d 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 33 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 @@ -105,6 +105,9 @@ // // MSP commands for Cleanflight original features // +#define MSP_BATTERY_CONFIG 32 +#define MSP_SET_BATTERY_CONFIG 33 + #define MSP_MODE_RANGES 34 //out message Returns all mode ranges #define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range @@ -253,6 +256,9 @@ #define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll #define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag #define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings +#define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter) +#define MSP_CURRENT_METERS 129 //out message Amperage (per meter) +#define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 38b8f65d6b..7b93bf8df8 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -81,4 +81,4 @@ void crsfRxSendTelemetryData(void); struct rxConfig_s; struct rxRuntimeConfig_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); -bool crsfRxIsActive(void); \ No newline at end of file +bool crsfRxIsActive(void); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index c077fb6e22..4654cf7933 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -523,10 +523,10 @@ void handleJetiExBusTelemetry(void) } if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { - jetiExSensors[EX_VOLTAGE].value = getVbat(); - jetiExSensors[EX_CURRENT].value = amperage; + jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage(); + jetiExSensors[EX_CURRENT].value = getAmperage(); jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt; - jetiExSensors[EX_CAPACITY].value = mAhDrawn; + jetiExSensors[EX_CAPACITY].value = getMAhDrawn(); jetiExSensors[EX_FRAMES_LOST].value = framesLost; jetiExSensors[EX_TIME_DIFF].value = timeDiff; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index e84ca3822c..d8044fa22d 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -56,7 +56,8 @@ typedef enum { TASK_RX, TASK_SERIAL, TASK_DISPATCH, - TASK_BATTERY, + TASK_BATTERY_SENSORS, + TASK_BATTERY_ALERTS, #ifdef BEEPER TASK_BEEPER, #endif diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9a08c2a0c1..1e1ede53bc 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -35,107 +35,100 @@ #include "fc/config.h" #include "fc/rc_controls.h" -#include "fc/runtime_config.h" #include "io/beeper.h" #include "sensors/battery.h" -#include "sensors/esc_sensor.h" +/** + * terminology: meter vs sensors + * + * 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 + * - e.g. a meter exposes normalized, and often filtered, values from a sensor. + * meters require different or little configuration. + * + */ -#define VBAT_LPF_FREQ 0.4f -#define IBAT_LPF_FREQ 0.4f +static void batteryUpdateConsumptionState(void); // temporary forward reference #define VBAT_STABLE_MAX_DELTA 2 -#define ADCVREF 3300 // in mV - -#define MAX_ESC_BATTERY_AGE 10 - // Battery monitoring stuff -uint8_t batteryCellCount; +uint8_t batteryCellCount; // Note: this can be 0 when no battery is detected or when the battery voltage sensor is missing or disabled. uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; +// +static currentMeter_t currentMeter; +static voltageMeter_t voltageMeter; -uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) -uint16_t vbatLatest = 0; // most recent unsmoothed value - -int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) -int32_t amperageLatest = 0; // most recent value - -int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start - -static batteryState_e vBatState; +static batteryState_e batteryState; +static batteryState_e voltageState; static batteryState_e consumptionState; -PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); - -#ifndef CURRENT_METER_SCALE_DEFAULT -#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A) +#ifdef BOARD_HAS_CURRENT_SENSOR +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#else +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL #endif +#ifdef BOARD_HAS_VOLTAGE_DIVIDER +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#else +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 1); + PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, - .vbatscale = VBAT_SCALE_DEFAULT, - .vbatresdivval = VBAT_RESDIVVAL_DEFAULT, - .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT, + // voltage .vbatmaxcellvoltage = 43, .vbatmincellvoltage = 33, .vbatwarningcellvoltage = 35, - .vbathysteresis = 1, - .batteryMeterType = BATTERY_SENSOR_ADC, - .currentMeterOffset = 0, - .currentMeterScale = CURRENT_METER_SCALE_DEFAULT, + .batteryNotPresentLevel = 55, // VBAT below 5.5 V will be ignored + .voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE, + + // current .batteryCapacity = 0, - .currentMeterType = CURRENT_SENSOR_ADC, - .batterynotpresentlevel = 55, // VBAT below 5.5 V will be igonored + .currentMeterSource = DEFAULT_VOLTAGE_METER_SOURCE, + + // warnings / alerts .useVBatAlerts = true, .useConsumptionAlerts = false, - .consumptionWarningPercentage = 10 + .consumptionWarningPercentage = 10, + .vbathysteresis = 1 ); -static uint16_t batteryAdcToVoltage(uint16_t src) +void batteryUpdateVoltage(void) { - // calculate battery voltage based on ADC reading - // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier); -} + switch(batteryConfig()->voltageMeterSource) { +#ifdef USE_ESC_SENSOR + case VOLTAGE_METER_ESC: + if (feature(FEATURE_ESC_SENSOR)) { + voltageMeterESCUpdate(&voltageMeter); + } + break; +#endif + case VOLTAGE_METER_ADC: + voltageMeterADCRefresh(); + voltageMeterADCUpdate(&voltageMeter, ADC_BATTERY); + break; -static void updateBatteryVoltage(void) -{ - static biquadFilter_t vBatFilter; - static bool vBatFilterIsInitialised; - - if (!vBatFilterIsInitialised) { - biquadFilterInitLPF(&vBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update - vBatFilterIsInitialised = true; - } - - #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) { - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); - vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0; - if (debugMode == DEBUG_BATTERY) { - debug[0] = -1; - } - vbat = biquadFilterApply(&vBatFilter, vbatLatest); - } - else - #endif - { - uint16_t vBatSample = adcGetChannel(ADC_BATTERY); - if (debugMode == DEBUG_BATTERY) { - debug[0] = vBatSample; - } - vbat = batteryAdcToVoltage(biquadFilterApply(&vBatFilter, vBatSample)); - vbatLatest = batteryAdcToVoltage(vBatSample); + default: + case VOLTAGE_METER_NONE: + voltageMeterReset(&voltageMeter); + break; } if (debugMode == DEBUG_BATTERY) { - debug[1] = vbat; + debug[0] = voltageMeter.unfiltered; + debug[1] = voltageMeter.filtered; } } -static void updateBatteryAlert(void) +static void updateBatteryBeeperAlert(void) { switch(getBatteryState()) { case BATTERY_WARNING: @@ -152,75 +145,94 @@ static void updateBatteryAlert(void) } } -void updateBattery(void) +void batteryUpdatePresence(void) { - uint16_t vBatPrevious = vbatLatest; - updateBatteryVoltage(); - uint16_t vBatMeasured = vbatLatest; + static uint16_t previousVoltage = 0; - /* battery has just been connected*/ - if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { - /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ - vBatState = BATTERY_OK; + bool isVoltageStable = ( + previousVoltage > 0 + && ABS(voltageMeter.filtered - previousVoltage) <= VBAT_STABLE_MAX_DELTA + ); - unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1; + if ( + voltageState == BATTERY_NOT_PRESENT + && voltageMeter.filtered > batteryConfig()->batteryNotPresentLevel + && isVoltageStable + ) { + /* battery has just been connected - calculate cells, warning voltages and reset state */ + + + unsigned cells = (voltageMeter.filtered / batteryConfig()->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } + + consumptionState = voltageState = BATTERY_OK; batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */ - } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { - vBatState = BATTERY_NOT_PRESENT; + } else if ( + voltageState != BATTERY_NOT_PRESENT + && voltageMeter.filtered <= batteryConfig()->batteryNotPresentLevel + && isVoltageStable + ) { + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */ + + consumptionState = voltageState = BATTERY_NOT_PRESENT; + batteryCellCount = 0; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } if (debugMode == DEBUG_BATTERY) { - debug[2] = vBatState; + debug[2] = voltageState; debug[3] = batteryCellCount; } - if (batteryConfig()->useVBatAlerts) { - switch(vBatState) { - case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { - vBatState = BATTERY_WARNING; - } + previousVoltage = voltageMeter.filtered; // record the current value so we can detect voltage stabilisation next the presence needs updating. +} - break; - case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) { - vBatState = BATTERY_CRITICAL; - } else if (vbat > batteryWarningVoltage) { - vBatState = BATTERY_OK; - } +static void batteryUpdateVoltageState(void) +{ + // alerts are currently used by beeper, osd and other subsystems + switch(voltageState) { + case BATTERY_OK: + if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { + voltageState = BATTERY_WARNING; + } + break; - break; - case BATTERY_CRITICAL: - if (vbat > batteryCriticalVoltage) { - vBatState = BATTERY_WARNING; - } + case BATTERY_WARNING: + if (voltageMeter.filtered <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) { + voltageState = BATTERY_CRITICAL; + } else if (voltageMeter.filtered > batteryWarningVoltage) { + voltageState = BATTERY_OK; + } + break; - break; - case BATTERY_NOT_PRESENT: - break; - } + case BATTERY_CRITICAL: + if (voltageMeter.filtered > batteryCriticalVoltage) { + voltageState = BATTERY_WARNING; + } + break; - updateBatteryAlert(); + default: + break; } } +void batteryUpdateStates(void) +{ + batteryUpdateVoltageState(); + batteryUpdateConsumptionState(); + + batteryState = MAX(voltageState, consumptionState); +} + batteryState_e getBatteryState(void) { - batteryState_e batteryState = BATTERY_NOT_PRESENT; - if (vBatState != BATTERY_NOT_PRESENT) { - batteryState = MAX(vBatState, consumptionState); - } - return batteryState; } @@ -233,140 +245,163 @@ const char * getBatteryStateString(void) void batteryInit(void) { - vBatState = BATTERY_NOT_PRESENT; - consumptionState = BATTERY_OK; + // + // presence + // + batteryState = BATTERY_NOT_PRESENT; batteryCellCount = 0; + + // + // voltage + // + voltageState = BATTERY_NOT_PRESENT; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; -} -static int32_t currentSensorToCentiamps(uint16_t src) -{ - int32_t millivolts; + voltageMeterReset(&voltageMeter); + switch(batteryConfig()->voltageMeterSource) { + case VOLTAGE_METER_ESC: +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + voltageMeterESCInit(); + } +#endif + break; - millivolts = ((uint32_t)src * ADCVREF) / 4096; - millivolts -= batteryConfig()->currentMeterOffset; + case VOLTAGE_METER_ADC: + voltageMeterADCInit(); + break; - return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps -} - -static void updateBatteryCurrent(void) -{ - static biquadFilter_t iBatFilter; - static bool iBatFilterIsInitialised; - - if (!iBatFilterIsInitialised) { - biquadFilterInitLPF(&iBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update - iBatFilterIsInitialised = true; + default: + break; + } + + // + // current + // + consumptionState = BATTERY_OK; + resetCurrentMeterState(¤tMeter); + switch(batteryConfig()->currentMeterSource) { + case CURRENT_METER_ADC: + currentMeterADCInit(); + break; + + default: + break; } - uint16_t iBatSample = adcGetChannel(ADC_CURRENT); - amperageLatest = currentSensorToCentiamps(iBatSample); - amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample)); } -static void updateCurrentDrawn(int32_t lastUpdateAt) +static void batteryUpdateConsumptionState(void) { - static float mAhDrawnF = 0.0f; // used to get good enough resolution + if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) { + uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining(); - mAhDrawnF = mAhDrawnF + (amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600)); - mAhDrawn = mAhDrawnF; -} - -void updateConsumptionWarning(void) -{ - if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { - if (calculateBatteryPercentage() == 0) { - vBatState = BATTERY_CRITICAL; - } else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) { + if (batteryPercentageRemaining == 0) { + consumptionState = BATTERY_CRITICAL; + } else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) { consumptionState = BATTERY_WARNING; } else { consumptionState = BATTERY_OK; } - - updateBatteryAlert(); } } -void updateCurrentMeter(int32_t lastUpdateAt) +void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed) { - if (getBatteryState() != BATTERY_NOT_PRESENT) { - switch(batteryConfig()->currentMeterType) { - case CURRENT_SENSOR_ADC: - updateBatteryCurrent(); - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; - case CURRENT_SENSOR_VIRTUAL: - amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; - if (ARMING_FLAG(ARMED)) { - const throttleStatus_e throttleStatus = calculateThrottleStatus(); - int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { - throttleOffset = 0; - } - int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000; - } - amperage = amperageLatest; - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; - case CURRENT_SENSOR_ESC: -#ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); - if (escData->dataAge <= MAX_ESC_BATTERY_AGE) { - amperageLatest = escData->current; - mAhDrawn = escData->consumption; - } else { - amperageLatest = 0; - mAhDrawn = 0; - } - amperage = amperageLatest; + if (batteryCellCount == 0) { + resetCurrentMeterState(¤tMeter); + return; + } - updateConsumptionWarning(); - } + switch(batteryConfig()->currentMeterSource) { + case CURRENT_METER_ADC: + currentUpdateADCMeter(¤tMeter, lastUpdateAt); + break; - break; -#endif - case CURRENT_SENSOR_NONE: - amperage = 0; - amperageLatest = 0; + case CURRENT_METER_VIRTUAL: { + throttleStatus_e throttleStatus = calculateThrottleStatus(); + bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)); + int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; - break; + currentUpdateVirtualMeter(¤tMeter, lastUpdateAt, armed, throttleLowAndMotorStop, throttleOffset); + break; } - } else { - amperage = 0; - amperageLatest = 0; + + case CURRENT_METER_ESC: +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + currentUpdateESCMeter(¤tMeter, lastUpdateAt); + } +#endif + break; + + default: +// case CURRENT_METER_NONE: +// resetCurrentMeterState(¤tMeter); + break; } } float calculateVbatPidCompensation(void) { float batteryScaler = 1.0f; - if (feature(FEATURE_VBAT) && batteryCellCount > 0) { + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) { // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference - batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); + batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.filtered), 1.0f, 1.33f); } return batteryScaler; } -uint8_t calculateBatteryPercentage(void) +uint8_t calculateBatteryPercentageRemaining(void) { uint8_t batteryPercentage = 0; if (batteryCellCount > 0) { uint16_t batteryCapacity = batteryConfig()->batteryCapacity; + if (batteryCapacity > 0) { - batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100); + batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100); } else { - batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100); + batteryPercentage = constrain((((uint32_t)voltageMeter.filtered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100); } } return batteryPercentage; } -uint16_t getVbat(void) +void batteryUpdateAlarms(void) { - return vbat; + // use the state to trigger beeper alerts + if (batteryConfig()->useVBatAlerts) { + updateBatteryBeeperAlert(); + } +} + +uint16_t getBatteryVoltage(void) +{ + return voltageMeter.filtered; +} + +uint16_t getBatteryVoltageLatest(void) +{ + return voltageMeter.unfiltered; +} + +uint8_t getBatteryCellCount(void) +{ + return batteryCellCount; +} + + +int32_t getAmperage(void) { + return currentMeter.amperage; +} + +int32_t getAmperageLatest(void) +{ + return currentMeter.amperageLatest; +} + +int32_t getMAhDrawn(void) +{ + return currentMeter.mAhDrawn; } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 91b59568ce..03f0f4efa7 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -19,48 +19,37 @@ #include "config/parameter_group.h" -#ifndef VBAT_SCALE_DEFAULT -#define VBAT_SCALE_DEFAULT 110 -#endif +#include "common/filter.h" +#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 enum { - CURRENT_SENSOR_NONE = 0, - CURRENT_SENSOR_ADC, - CURRENT_SENSOR_VIRTUAL, - CURRENT_SENSOR_ESC, - CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC -} currentSensor_e; - -typedef enum { - BATTERY_SENSOR_ADC = 0, - BATTERY_SENSOR_ESC -} batterySensor_e; - typedef struct batteryConfig_s { - uint8_t vbatscale; // adjust this to match battery voltage to reported value - uint8_t vbatresdivval; // resistor divider R2 (default NAZE 10(K)) - uint8_t vbatresdivmultiplier; // multiplier for scale (e.g. 2.5:1 ratio with multiplier of 4 can use '100' instead of '25' in ratio) to get better precision + // voltage 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 critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) - uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V - batterySensor_e batteryMeterType; // type of battery meter uses, either ADC or ESC + uint8_t batteryNotPresentLevel; // Below this level battery is considered as not present - int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A - int16_t currentMeterOffset; // offset of the current sensor in millivolt steps - currentSensor_e currentMeterType; // type of current meter used, either ADC, Virtual or ESC + voltageMeterSource_e voltageMeterSource; // source of battery voltage meter used, either ADC or ESC - // 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 + // current uint16_t batteryCapacity; // mAh - uint8_t batterynotpresentlevel; // Below this level battery is considered as not present + currentMeterSource_e currentMeterSource; // source of battery current meter used, either ADC, Virtual or ESC + + // warnings / alerts bool useVBatAlerts; // Issue alerts based on VBat readings bool useConsumptionAlerts; // Issue alerts based on total power consumption uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning + uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V + + // 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; PG_DECLARE(batteryConfig_t, batteryConfig); @@ -72,23 +61,26 @@ typedef enum { BATTERY_NOT_PRESENT } batteryState_e; -extern uint16_t vbatRaw; -extern uint16_t vbatLatest; -extern uint8_t batteryCellCount; -extern uint16_t batteryWarningVoltage; -extern int32_t amperageLatest; -extern int32_t amperage; -extern int32_t mAhDrawn; +void batteryInit(void); +void batteryUpdateVoltage(void); +void batteryUpdatePresence(void); batteryState_e getBatteryState(void); const char * getBatteryStateString(void); -void updateBattery(void); -void batteryInit(void); + +void batteryUpdateStates(void); +void batteryUpdateAlarms(void); struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt); -int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); -uint8_t calculateBatteryPercentage(void); -uint16_t getVbat(void); +uint8_t calculateBatteryPercentageRemaining(void); +uint16_t getBatteryVoltage(void); +uint16_t getBatteryVoltageLatest(void); +uint8_t getBatteryCellCount(void); + +int32_t getAmperage(void); +int32_t getAmperageLatest(void); +int32_t getMAhDrawn(void); + +void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed); diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c new file mode 100644 index 0000000000..10ab77acfb --- /dev/null +++ b/src/main/sensors/current.c @@ -0,0 +1,129 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include "stdbool.h" +#include "stdint.h" +#include "string.h" + +#include +#include "build/build_config.h" + +#include "common/maths.h" +#include "common/utils.h" +#include "common/filter.h" + +#include "drivers/adc.h" +#include "drivers/system.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "config/config_reset.h" + +#include "sensors/current.h" +#include "sensors/esc_sensor.h" + +#define ADCVREF 3300 // in mV + +#define IBAT_LPF_FREQ 0.4f +static biquadFilter_t adciBatFilter; + +#ifndef CURRENT_METER_SCALE_DEFAULT +#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A) +#endif + +PG_REGISTER_ARRAY_WITH_RESET_FN(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_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, + ); + } + } +} + +static int32_t currentMeterADCToCentiamps(const uint16_t src) +{ + int32_t millivolts; + + const currentMeterADCOrVirtualConfig_t *config = currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC); + + millivolts = ((uint32_t)src * ADCVREF) / 4096; + millivolts -= config->offset; + + return (millivolts * 1000) / (int32_t)config->scale; // current in 0.01A steps +} + +void updateCurrentDrawn(currentMeter_t *state, int32_t lastUpdateAt) +{ + state->mAhDrawnF = state->mAhDrawnF + (state->amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600)); + state->mAhDrawn = state->mAhDrawnF; +} + +void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt) +{ + uint16_t iBatSample = adcGetChannel(ADC_CURRENT); + state->amperageLatest = currentMeterADCToCentiamps(iBatSample); + state->amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample)); + + updateCurrentDrawn(state, lastUpdateAt); +} + +void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset) +{ + state->amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset; + if (armed) { + if (throttleLowAndMotorStop) { + throttleOffset = 0; + } + + int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz? + state->amperageLatest = state->amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000; + } + updateCurrentDrawn(state, lastUpdateAt); +} + +void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt) +{ + UNUSED(lastUpdateAt); +#ifndef USE_ESC_SENSOR + UNUSED(state); +#else + escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); + if (escData->dataAge <= ESC_BATTERY_AGE_MAX) { + state->amperageLatest = escData->current; + state->mAhDrawn = escData->consumption; + } else { + state->amperageLatest = 0; + state->mAhDrawn = 0; + } + state->amperage = state->amperageLatest; +#endif +} + +void resetCurrentMeterState(currentMeter_t *state) +{ + state->amperage = 0; + state->amperageLatest = 0; +} + +void currentMeterADCInit(void) +{ + biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update +} diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h new file mode 100644 index 0000000000..be3dd50f1f --- /dev/null +++ b/src/main/sensors/current.h @@ -0,0 +1,59 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + + +typedef enum { + CURRENT_METER_NONE = 0, + CURRENT_METER_ADC, + CURRENT_METER_VIRTUAL, + CURRENT_METER_ESC, + CURRENT_METER_MAX = CURRENT_METER_ESC +} currentMeterSource_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) + int32_t mAhDrawn; // milliampere hours drawn from the battery since start + float mAhDrawnF; +} 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 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); + +void currentMeterADCInit(void); + +void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt); +void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt); +void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset); + +void resetCurrentMeterState(currentMeter_t *state); diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 0e4fce0455..c4af56e62d 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -43,8 +43,6 @@ #include "flight/mixer.h" -#include "sensors/battery.h" - #include "io/serial.h" /* diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 8e6fa2feee..c18def94ff 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -30,6 +30,8 @@ typedef struct { #define ESC_DATA_INVALID 255 +#define ESC_BATTERY_AGE_MAX 10 + bool escSensorInit(void); void escSensorProcess(timeUs_t currentTime); diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c new file mode 100644 index 0000000000..65a15887ca --- /dev/null +++ b/src/main/sensors/voltage.c @@ -0,0 +1,151 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include "stdbool.h" +#include "stdint.h" +#include "string.h" + +#include + +#include "build/build_config.h" + +#include "common/maths.h" +#include "common/filter.h" +#include "common/utils.h" + +#include "drivers/adc.h" +#include "drivers/system.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "config/config_reset.h" + +#include "sensors/voltage.h" +#include "sensors/esc_sensor.h" + +#ifndef VBAT_SCALE_DEFAULT +#define VBAT_SCALE_DEFAULT 110 +#endif + +#ifdef USE_ESC_SENSOR +static biquadFilter_t escvBatFilter; +#endif + +voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC]; + +voltageMeterADCState_t *getVoltageMeterADC(uint8_t index) +{ + return &voltageMeterADCStates[index]; +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig, PG_VOLTAGE_SENSOR_ADC_CONFIG, 0); + +void pgResetFn_voltageSensorADCConfig(voltageSensorADCConfig_t *instance) +{ + for (int i = 0; i < MAX_VOLTAGE_SENSOR_ADC; i++) { + RESET_CONFIG(voltageSensorADCConfig_t, &instance[i], + .vbatscale = VBAT_SCALE_DEFAULT, + .vbatresdivval = VBAT_RESDIVVAL_DEFAULT, + .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT, + ); + } +} + + +static const uint8_t voltageMeterAdcChannelMap[] = { + ADC_BATTERY, +#ifdef ADC_POWER_12V + ADC_POWER_12V, +#endif +#ifdef ADC_POWER_5V + ADC_POWER_5V, +#endif +}; + +STATIC_UNIT_TESTED uint16_t voltageAdcToVoltage(const uint16_t src, const voltageSensorADCConfig_t *config) +{ + // calculate battery voltage based on ADC reading + // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 10:1 voltage divider (10k:1k) * 10 for 0.1V + return ((((uint32_t)src * config->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier); +} + +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 + + voltageMeterADCState_t *state = &voltageMeterADCStates[i]; + const voltageSensorADCConfig_t *config = voltageSensorADCConfig(i); + + uint8_t channel = voltageMeterAdcChannelMap[i]; + uint16_t rawSample = adcGetChannel(channel); + + uint16_t filteredSample = biquadFilterApply(&state->vbatFilterState, rawSample); + + // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. + state->voltageFiltered = voltageAdcToVoltage(filteredSample, config); + state->voltageUnfiltered = voltageAdcToVoltage(rawSample, config); + } +} + +void voltageMeterADCUpdate(voltageMeter_t *voltageMeter, voltageSensorADC_e adcChannel) +{ + voltageMeterADCState_t *state = &voltageMeterADCStates[adcChannel]; + + voltageMeter->filtered = state->voltageFiltered; + voltageMeter->unfiltered = state->voltageUnfiltered; +} + +void voltageMeterADCInit(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 + + voltageMeterADCState_t *state = &voltageMeterADCStates[i]; + memset(state, 0, sizeof(voltageMeterADCState_t)); + + biquadFilterInitLPF(&state->vbatFilterState, VBATT_LPF_FREQ, 50000); + } +} + +void voltageMeterReset(voltageMeter_t *meter) +{ + meter->filtered = 0; + meter->unfiltered = 0; +} + +#define VBAT_LPF_FREQ 0.4f + +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); +#endif +} + diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h new file mode 100644 index 0000000000..393deecaab --- /dev/null +++ b/src/main/sensors/voltage.h @@ -0,0 +1,91 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +// +// meters +// + +typedef enum { + VOLTAGE_METER_NONE = 0, + VOLTAGE_METER_ADC, + VOLTAGE_METER_ESC +} voltageMeterSource_e; + +typedef struct voltageMeter_s { + uint16_t filtered; // voltage in 0.1V steps + uint16_t unfiltered; // voltage in 0.1V steps +} voltageMeter_t; + +// +// sensors +// + +typedef enum { + VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER = 0, + VOLTAGE_SENSOR_TYPE_ESC +} voltageSensorType_e; + + +// +// 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_METER_*, they are separate concerns. + +#define VBAT_RESDIVVAL_DEFAULT 10 +#define VBAT_RESDIVMULTIPLIER_DEFAULT 1 +#define VBAT_SCALE_MIN 0 +#define VBAT_SCALE_MAX 255 + +#define VBATT_LPF_FREQ 1.0f + +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)) + uint8_t vbatresdivmultiplier; // multiplier for scale (e.g. 2.5:1 ratio with multiplier of 4 can use '100' instead of '25' in ratio) to get better precision +} voltageSensorADCConfig_t; + +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 voltageMeterReset(voltageMeter_t *voltageMeter); +void voltageMeterADCUpdate(voltageMeter_t *voltageMeter, voltageSensorADC_e adcChannel); +void voltageMeterESCUpdate(voltageMeter_t *voltageMeter); diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index 14dfe9e959..704065e302 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -83,7 +83,6 @@ #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 -#define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index b70f3623a0..1e4e45a2b4 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -56,8 +56,8 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - batteryConfigMutable()->currentMeterOffset = CURRENTOFFSET; - batteryConfigMutable()->currentMeterScale = CURRENTSCALE; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE; compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { @@ -74,7 +74,9 @@ void targetConfiguration(void) rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; telemetryConfigMutable()->telemetry_inversion = 0; - featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY); + batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; + batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; + featureSet(FEATURE_TELEMETRY); } pidProfilesMutable(0)->P8[FD_ROLL] = 53; diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 02330c6692..02141331ec 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -55,8 +55,8 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - batteryConfigMutable()->currentMeterOffset = CURRENTOFFSET; - batteryConfigMutable()->currentMeterScale = CURRENTSCALE; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE; compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { @@ -73,7 +73,9 @@ void targetConfiguration(void) rxConfigMutable()->sbus_inversion = 0; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; telemetryConfigMutable()->telemetry_inversion = 0; - featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY); + batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; + batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; + featureSet(FEATURE_TELEMETRY); } pidProfilesMutable(0)->P8[FD_ROLL] = 53; diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index 2cc57a0e80..33ca443339 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -25,8 +25,9 @@ #include "fc/config.h" #include "sensors/battery.h" +#define CURRENTSCALE 220 void targetConfiguration(void) { - batteryConfigMutable()->currentMeterScale = 220; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE; } #endif diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 1917890cf0..7d39407f8e 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -112,6 +112,7 @@ #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 #define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_CURRENT_SENSOR #define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 @@ -124,7 +125,7 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 #define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD) #define SPEKTRUM_BIND_PIN UART2_RX_PIN diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 8e163b1bff..85caf8528b 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -78,7 +78,7 @@ #define BST_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -120,6 +120,9 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; // // MSP commands for Cleanflight original features // +#define BST_BATTERY_CONFIG 32 +#define BST_SET_BATTERY_CONFIG 33 + #define BST_MODE_RANGES 34 //out message Returns all mode ranges #define BST_SET_MODE_RANGE 35 //in message Sets a single mode range @@ -674,13 +677,14 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) #endif break; case BST_ANALOG: - bstWrite8((uint8_t)constrain(getVbat(), 0, 255)); - bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery + bstWrite8((uint8_t)constrain(getBatteryVoltage(), 0, 255)); + bstWrite16((uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery bstWrite16(rssi); + // FIXME - what does the TBS OSD actually need? the 'multiwiiCurrentMeterOutput' setting pre-dates the TBS i2c_bst code so likely we can just output exactly what we need. if(batteryConfig()->multiwiiCurrentMeterOutput) { - bstWrite16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero + bstWrite16((uint16_t)constrain(getAmperage() * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero } else - bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A + bstWrite16((int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A break; case BST_ARMING_CONFIG: bstWrite8(armingConfig()->auto_disarm_delay); @@ -773,7 +777,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(compassConfig()->mag_declination / 10); - bstWrite8(batteryConfig()->vbatscale); + bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); bstWrite8(batteryConfig()->vbatmincellvoltage); bstWrite8(batteryConfig()->vbatmaxcellvoltage); bstWrite8(batteryConfig()->vbatwarningcellvoltage); @@ -855,18 +859,40 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(boardAlignment()->yawDegrees); break; + case BST_VOLTAGE_METER_CONFIG: - bstWrite8(batteryConfig()->vbatscale); - bstWrite8(batteryConfig()->vbatmincellvoltage); - bstWrite8(batteryConfig()->vbatmaxcellvoltage); - bstWrite8(batteryConfig()->vbatwarningcellvoltage); + 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: - bstWrite16(batteryConfig()->currentMeterScale); - bstWrite16(batteryConfig()->currentMeterOffset); - bstWrite8(batteryConfig()->currentMeterType); + 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(batteryConfig()->vbatmincellvoltage); + bstWrite8(batteryConfig()->vbatmaxcellvoltage); + bstWrite8(batteryConfig()->vbatwarningcellvoltage); bstWrite16(batteryConfig()->batteryCapacity); + bstWrite8(batteryConfig()->voltageMeterSource); + bstWrite8(batteryConfig()->currentMeterSource); break; case BST_MIXER: @@ -916,8 +942,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(boardAlignment()->pitchDegrees); bstWrite16(boardAlignment()->yawDegrees); - bstWrite16(batteryConfig()->currentMeterScale); - bstWrite16(batteryConfig()->currentMeterOffset); + bstWrite16(0); // was batteryConfig()->currentMeterScale); + bstWrite16(0); // was batteryConfig()->currentMeterOffset); break; case BST_CF_SERIAL_CONFIG: @@ -1134,7 +1160,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) compassConfigMutable()->mag_declination = bstRead16() * 10; - batteryConfigMutable()->vbatscale = bstRead8(); // actual vbatscale as intended + bstRead8(); // was batteryConfigMutable()->vbatscale // 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 @@ -1265,17 +1291,48 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) boardAlignmentMutable()->pitchDegrees = bstRead16(); boardAlignmentMutable()->yawDegrees = bstRead16(); break; - case BST_SET_VOLTAGE_METER_CONFIG: - batteryConfigMutable()->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 + + 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: - batteryConfigMutable()->currentMeterScale = bstRead16(); - batteryConfigMutable()->currentMeterOffset = bstRead16(); - batteryConfigMutable()->currentMeterType = bstRead8(); + } + + 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 + batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert batteryConfigMutable()->batteryCapacity = bstRead16(); + batteryConfigMutable()->currentMeterSource = bstRead8(); break; #ifndef USE_QUAD_MIXER_ONLY @@ -1338,8 +1395,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) boardAlignmentMutable()->pitchDegrees = bstRead16(); // board_align_pitch boardAlignmentMutable()->yawDegrees = bstRead16(); // board_align_yaw - batteryConfigMutable()->currentMeterScale = bstRead16(); - batteryConfigMutable()->currentMeterOffset = bstRead16(); + bstRead16(); // was batteryConfigMutable()->currentMeterScale + bstRead16(); // was batteryConfigMutable()->currentMeterOffset break; case BST_SET_CF_SERIAL_CONFIG: { diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index c4304e7590..d2701c17d7 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -117,7 +117,7 @@ #define RSSI_ADC_PIN PC2 #define EXTERNAL1_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP) +#define DEFAULT_FEATURES (FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/ELLE0/target.h b/src/main/target/ELLE0/target.h index 17c71981e6..ec5cd8aaae 100644 --- a/src/main/target/ELLE0/target.h +++ b/src/main/target/ELLE0/target.h @@ -94,7 +94,6 @@ #undef LED_STRIP -#define DEFAULT_FEATURES (FEATURE_VBAT) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index c888912b67..7887779d77 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -141,7 +141,7 @@ #undef LED_STRIP -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 4580a07b64..c7c6418a29 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -127,7 +127,7 @@ #define RSSI_ADC_PIN PC1 // *************** FEATURES ************************ -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX | FEATURE_VTX) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_BLACKBOX | FEATURE_VTX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index a463b14b24..37934ebb86 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -166,7 +166,7 @@ #define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index f6912f9b36..aeaaa712b3 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -89,7 +89,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 70f3a6b237..a552c0089c 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -95,7 +95,6 @@ #define CURRENT_METER_ADC_PIN PA2 //#define RSSI_ADC_PIN PB2 -#define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 287d5fae8a..445749323f 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -119,7 +119,7 @@ #define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/KROOZX/config.c b/src/main/target/KROOZX/config.c index 1749bd766b..971d624c3d 100755 --- a/src/main/target/KROOZX/config.c +++ b/src/main/target/KROOZX/config.c @@ -29,17 +29,17 @@ #include "io/osd.h" #define VBAT_SCALE 113 -#define CURRENT_SCALE 1000 -#define CURRENT_OFFSET 0 +#define CURRENTSCALE 1000 +#define CURRENTOFFSET 0 #define OSD_POS(x,y) (x | (y << 5)) #ifdef TARGET_CONFIG void targetConfiguration(void) { - batteryConfigMutable()->vbatscale = VBAT_SCALE; - batteryConfigMutable()->currentMeterScale = CURRENT_SCALE; - batteryConfigMutable()->currentMeterOffset = CURRENT_OFFSET; + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE; barometerConfigMutable()->baro_hardware = 0; compassConfigMutable()->mag_hardware = 0; osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG; diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index 8c71b2a12e..d3a1f5b2d2 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -86,6 +86,7 @@ #define OSD_CH_SWITCH PC5 #define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_CURRENT_SENSOR #define USE_ADC #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PC3 @@ -144,7 +145,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define RX_CHANNELS_TAER -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD) #define LED_STRIP diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 2d959ede47..cb66264278 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -39,15 +39,16 @@ #include "sensors/compass.h" #include "sensors/gyro.h" +#define VBAT_SCALE 100 // alternative defaults settings for MULTIFLITEPICO targets void targetConfiguration(void) { compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default - batteryConfigMutable()->vbatscale = 100; - batteryConfigMutable()->vbatresdivval = 15; - batteryConfigMutable()->vbatresdivmultiplier = 4; + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE; + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatresdivval = 15; + voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatresdivmultiplier = 4; batteryConfigMutable()->vbatmaxcellvoltage = 44; batteryConfigMutable()->vbatmincellvoltage = 32; batteryConfigMutable()->vbatwarningcellvoltage = 33; diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index 24fce97dd7..7db666246a 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/src/main/target/MULTIFLITEPICO/target.h @@ -116,8 +116,8 @@ //#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP|FEATURE_VBAT) #define BRUSHED_MOTORS #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 #define SERIALRX_UART SERIAL_PORT_USART3 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 44c043bdf0..d3e61c321d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -145,7 +145,8 @@ // #define AFATFS_USE_INTROSPECTIVE_LOGGING #define USE_ADC -//#define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_CURRENT_SENSOR #define VBAT_ADC_PIN PA0 #define CURRENT_METER_ADC_PIN PA1 #define ADC_INSTANCE ADC1 @@ -159,7 +160,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) #define BUTTONS #define BUTTON_A_PIN PB1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index b5c4e767b1..f083ace06b 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -181,9 +181,9 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define AVOID_UART1_FOR_PWM_PPM #if defined(CL_RACINGF4) -#define DEFAULT_FEATURES (FEATURE_BLACKBOX |FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD ) #else -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) #endif #define SPEKTRUM_BIND_PIN UART1_RX_PIN diff --git a/src/main/target/PIKOBLX/config.c b/src/main/target/PIKOBLX/config.c index b901f5bfb9..2a81a7ffe5 100644 --- a/src/main/target/PIKOBLX/config.c +++ b/src/main/target/PIKOBLX/config.c @@ -25,8 +25,10 @@ #include "sensors/battery.h" +#define CURRENTSCALE 125 + void targetConfiguration(void) { - batteryConfigMutable()->currentMeterScale = 125; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE; } #endif diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 9362626b76..8931a45939 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -105,7 +105,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_RSSI_ADC | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_RSSI_ADC | FEATURE_OSD) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 211c8ec6c5..7ef39d054f 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -102,7 +102,6 @@ #define CURRENT_METER_ADC_PIN PB2 #define RSSI_ADC_PIN PA6 -#define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/RG_SSD_F3/config.c b/src/main/target/RG_SSD_F3/config.c index 93039ea940..88b348d2c7 100644 --- a/src/main/target/RG_SSD_F3/config.c +++ b/src/main/target/RG_SSD_F3/config.c @@ -28,6 +28,5 @@ void targetConfiguration(void) { batteryConfigMutable()->vbatmaxcellvoltage = 45; - batteryConfigMutable()->vbatscale = VBAT_SCALE_DEFAULT; } #endif diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index ef4f77fbb7..8e942e45f4 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -89,7 +89,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 2eb49df919..a564b9c5ef 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -164,6 +164,7 @@ #define M25P16_SPI_INSTANCE SPI2 #define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_CURRENT_SENSOR #define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 @@ -181,7 +182,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index b1e16cf175..15c1ced144 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -132,6 +132,7 @@ #define MPU6500_SPI_INSTANCE SPI1 #define BOARD_HAS_VOLTAGE_DIVIDER + #define USE_ADC #define ADC_INSTANCE ADC2 #define RSSI_ADC_PIN PB2 @@ -148,7 +149,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 393bb10100..2da896bfd7 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -136,6 +136,8 @@ #define MPU6500_SPI_INSTANCE SPI1 #define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_CURRENT_SENSOR + #define USE_ADC #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PC1 @@ -158,7 +160,7 @@ #define OSD #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) #define BUTTONS #define BUTTON_A_PIN PD2 diff --git a/src/main/target/TINYFISH/config.c b/src/main/target/TINYFISH/config.c index fe04790e29..b7ecbc941f 100644 --- a/src/main/target/TINYFISH/config.c +++ b/src/main/target/TINYFISH/config.c @@ -38,24 +38,25 @@ #define TARGET_CPU_VOLTAGE 3.0 +#define CURRENTOFFSET 0 +// board uses an ina139, RL=0.005, Rs=30000 +// V/A = (0.005 * 0.001 * 30000) * I +// rescale to 1/10th mV / A -> * 1000 * 10 +// use 3.0V as cpu and adc voltage -> rescale by 3.0/3.3 +#define CURRENTSCALE (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3) + // set default settings to match our target void targetConfiguration(void) { - batteryConfigMutable()->currentMeterOffset = 0; - // we use an ina139, RL=0.005, Rs=30000 - // V/A = (0.005 * 0.001 * 30000) * I - // rescale to 1/10th mV / A -> * 1000 * 10 - // we use 3.0V as cpu and adc voltage -> rescale by 3.0/3.3 - batteryConfigMutable()->currentMeterScale = (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3); + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET; + currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE; - // we use the same uart for frsky telemetry and SBUS, both non inverted + // use the same uart for frsky telemetry and SBUS, both non inverted const int index = findSerialPortIndexByIdentifier(SBUS_TELEMETRY_UART); serialConfigMutable()->portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->sbus_inversion = 0; telemetryConfigMutable()->telemetry_inversion = 0; - - featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT); } #endif diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h index 1deffa81bc..99f1075029 100644 --- a/src/main/target/TINYFISH/target.h +++ b/src/main/target/TINYFISH/target.h @@ -94,6 +94,7 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER +#define BOARD_HAS_CURRENT_SENSOR #define VBAT_ADC_PIN PB1 #define CURRENT_METER_ADC_PIN PB0 #define ADC_INSTANCE ADC3 @@ -107,7 +108,7 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY) #define TARGET_CONFIG #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index da064f9e64..deef0916e2 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -159,7 +159,7 @@ #undef LED_STRIP -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index b8bbb9f008..f428e73dff 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -174,7 +174,7 @@ void crsfFrameBatterySensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); - crsfSerialize16(dst, getVbat()); // vbat is in units of 0.1V + crsfSerialize16(dst, getBatteryVoltage()); // vbat is in units of 0.1V #ifdef CLEANFLIGHT const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource); const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A @@ -182,9 +182,9 @@ void crsfFrameBatterySensor(sbuf_t *dst) const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); #else - crsfSerialize16(dst, amperage / 10); + crsfSerialize16(dst, getAmperage() / 10); const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; - const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); + const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining(); #endif crsfSerialize8(dst, (batteryCapacity >> 16)); crsfSerialize8(dst, (batteryCapacity >> 8)); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 932f4bdc0e..daac014f63 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -371,6 +371,7 @@ static void sendVoltage(void) uint32_t cellVoltage; uint16_t payload; + uint8_t cellCount = getBatteryCellCount(); /* * Format for Voltage Data for single cells is like this: * @@ -382,7 +383,7 @@ static void sendVoltage(void) * The actual value sent for cell voltage has resolution of 0.002 volts * Since vbat has resolution of 0.1 volts it has to be multiplied by 50 */ - cellVoltage = ((uint32_t)getVbat() * 100 + batteryCellCount) / (batteryCellCount * 2); + cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2); // Cell number is at bit 9-12 payload = (currentCell << 4); @@ -397,7 +398,7 @@ static void sendVoltage(void) serialize16(payload); currentCell++; - currentCell %= batteryCellCount; + currentCell %= cellCount; } /* @@ -405,17 +406,18 @@ static void sendVoltage(void) */ static void sendVoltageAmp(void) { + uint16_t batteryVoltage = getBatteryVoltage(); if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { /* * Use new ID 0x39 to send voltage directly in 0.1 volts resolution */ sendDataHead(ID_VOLTAGE_AMP); - serialize16(getVbat()); + serialize16(batteryVoltage); } else { - uint16_t voltage = (getVbat() * 110) / 21; + uint16_t voltage = (batteryVoltage * 110) / 21; uint16_t vfasVoltage; if (telemetryConfig()->frsky_vfas_cell_voltage) { - vfasVoltage = voltage / batteryCellCount; + vfasVoltage = voltage / getBatteryCellCount(); } else { vfasVoltage = voltage; } @@ -429,7 +431,7 @@ static void sendVoltageAmp(void) static void sendAmperage(void) { sendDataHead(ID_CURRENT); - serialize16((uint16_t)(amperage / 10)); + serialize16((uint16_t)(getAmperage() / 10)); } static void sendFuelLevel(void) @@ -437,9 +439,9 @@ static void sendFuelLevel(void) sendDataHead(ID_FUEL_LEVEL); if (batteryConfig()->batteryCapacity > 0) { - serialize16((uint16_t)calculateBatteryPercentage()); + serialize16((uint16_t)calculateBatteryPercentageRemaining()); } else { - serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); + serialize16((uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); } } @@ -537,7 +539,7 @@ void handleFrSkyTelemetry(void) sendTemperature1(); sendThrottleOrBatterySizeAsRpm(); - if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) { + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) { sendVoltage(); sendVoltageAmp(); sendAmperage(); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index de4318bf75..eb6a4c1ebf 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -247,24 +247,24 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) { - hottEAMMessage->main_voltage_L = getVbat() & 0xFF; - hottEAMMessage->main_voltage_H = getVbat() >> 8; - hottEAMMessage->batt1_voltage_L = getVbat() & 0xFF; - hottEAMMessage->batt1_voltage_H = getVbat() >> 8; + hottEAMMessage->main_voltage_L = getBatteryVoltage() & 0xFF; + hottEAMMessage->main_voltage_H = getBatteryVoltage() >> 8; + hottEAMMessage->batt1_voltage_L = getBatteryVoltage() & 0xFF; + hottEAMMessage->batt1_voltage_H = getBatteryVoltage() >> 8; updateAlarmBatteryStatus(hottEAMMessage); } static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t amp = amperage / 10; + int32_t amp = getAmperage() / 10; hottEAMMessage->current_L = amp & 0xFF; hottEAMMessage->current_H = amp >> 8; } static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t mAh = mAhDrawn / 10; + int32_t mAh = getMAhDrawn() / 10; hottEAMMessage->batt_cap_L = mAh & 0xFF; hottEAMMessage->batt_cap_H = mAh >> 8; } diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 48c853ad41..a60d00f006 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -125,9 +125,9 @@ static uint8_t dispatchMeasurementReply(ibusAddress_t address) switch (sensorAddressTypeLookup[address - ibusBaseAddress]) { case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: - value = getVbat() * 10; + value = getBatteryVoltage() * 10; if (telemetryConfig()->report_cell_voltage) { - value /= batteryCellCount; + value /= getBatteryCellCount(); } return sendIbusMeasurement(address, value); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index fec48d7b75..cb292e8cd5 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -190,7 +190,7 @@ static void ltm_sframe(void) if (failsafeIsActive()) lt_statemode |= 2; ltm_initialise_packet('S'); - ltm_serialise_16(getVbat() * 100); //vbat converted to mv + ltm_serialise_16(getBatteryVoltage() * 100); //vbat converted to mv ltm_serialise_16(0); // current, not implemented ltm_serialise_8((uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar) ltm_serialise_8(0); // no airspeed diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 5423919613..63a9fafb22 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -223,11 +223,11 @@ void mavlinkSendSystemStatus(void) // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 0, // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - feature(FEATURE_VBAT) ? getVbat() * 100 : 0, + (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? getBatteryVoltage() * 100 : 0, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - feature(FEATURE_VBAT) ? amperage : -1, + (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) ? getAmperage() : -1, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, + (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? calculateBatteryPercentageRemaining() : 100, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 476a90632d..1e8aa16611 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -618,20 +618,20 @@ void handleSmartPortTelemetry(void) break; #endif case FSSP_DATAID_VFAS : - if (feature(FEATURE_VBAT) && batteryCellCount > 0) { + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) { uint16_t vfasVoltage; if (telemetryConfig()->frsky_vfas_cell_voltage) { - vfasVoltage = getVbat() / batteryCellCount; + vfasVoltage = getBatteryVoltage() / getBatteryCellCount(); } else { - vfasVoltage = getVbat(); + vfasVoltage = getBatteryVoltage(); } smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; case FSSP_DATAID_CURRENT : - if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) { - smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit + if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { + smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit smartPortHasRequest = 0; } break; @@ -643,8 +643,8 @@ void handleSmartPortTelemetry(void) } break; case FSSP_DATAID_FUEL : - if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) { - smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit + if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { + smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit smartPortHasRequest = 0; } break; @@ -791,8 +791,8 @@ void handleSmartPortTelemetry(void) break; #endif case FSSP_DATAID_A4 : - if (feature(FEATURE_VBAT) && batteryCellCount > 0) { - smartPortSendPackage(id, getVbat() * 10 / batteryCellCount ); // given in 0.1V, convert to volts + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) { + smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index f7ec4e4713..faa3734cc0 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -161,7 +161,7 @@ void srxlFrameRpm(sbuf_t *dst) srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM); srxlSerialize8(dst, SRXL_FRAMETYPE_SID); srxlSerialize16(dst, 0xFFFF); // pulse leading edges - srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V + srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V srxlSerialize16(dst, 0x7FFF); // temperature srxlSerialize8(dst, 0xFF); // dbmA srxlSerialize8(dst, 0xFF); // dbmB @@ -200,9 +200,9 @@ void srxlFramePowerBox(sbuf_t *dst) { srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX); srxlSerialize8(dst, SRXL_FRAMETYPE_SID); - srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V - vbat1 - srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V - vbat2 - srxlSerialize16(dst, amperage / 10); + srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V - vbat1 + srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V - vbat2 + srxlSerialize16(dst, getAmperage() / 10); srxlSerialize16(dst, 0xFFFF); srxlSerialize16(dst, 0xFFFF); // spare diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 4fb0cea52f..0f4afaf44d 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -54,7 +54,10 @@ extern "C" { #include "telemetry/telemetry.h" bool airMode; - uint16_t vbat; + + uint16_t testBatteryVoltage = 0; + int32_t testAmperage = 0; + serialPort_t *telemetrySharedPort; PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); @@ -138,7 +141,7 @@ TEST(TelemetryCrsfTest, TestBattery) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - vbat = 0; // 0.1V units + testBatteryVoltage = 0; // 0.1V units int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address @@ -154,8 +157,8 @@ TEST(TelemetryCrsfTest, TestBattery) EXPECT_EQ(67, remaining); EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]); - vbat = 33; // 3.3V = 3300 mv - amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps + testBatteryVoltage = 33; // 3.3V = 3300 mv + testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps batteryConfigMutable()->batteryCapacity = 1234; frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 @@ -283,9 +286,6 @@ uint16_t GPS_altitude; // altitude in m uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_ground_course = 0; // degrees * 10 -int32_t amperage; -int32_t mAhDrawn; - void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} uint32_t micros(void) {return 0;} @@ -308,11 +308,23 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} -uint8_t batteryCapacityRemainingPercentage(void) {return 67;} -uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;} -uint8_t calculateBatteryPercentage(void) {return 67;} -batteryState_e getBatteryState(void) {return BATTERY_OK;} bool isAirmodeActive(void) {return airMode;} -uint16_t getVbat(void) { return vbat; } + +int32_t getAmperage(void) { + return testAmperage; +} + +uint16_t getBatteryVoltage(void) { + return testBatteryVoltage; +} + +batteryState_e getBatteryState(void) { + return BATTERY_OK; +} + +uint8_t calculateBatteryPercentageRemaining(void) { + return 67; +} + } diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index b732dc61e7..939fd7dc93 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -51,6 +51,11 @@ extern "C" { #include "telemetry/hott.h" PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + + uint16_t testBatteryVoltage = 0; + int32_t testAmperage = 0; + int32_t testMAhDrawn = 0; + } #include "unittest_macros.h" @@ -172,10 +177,7 @@ uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_altitude; // altitude in 0.1m int16_t GPS_directionToHome; // direction to home or hol point in degrees -uint16_t vbat; -int32_t amperage; -int32_t mAhDrawn; uint32_t fixedMillis = 0; @@ -264,8 +266,17 @@ batteryState_e getBatteryState(void) return BATTERY_OK; } -uint16_t getVbat(void) +uint16_t getBatteryVoltage(void) { - return vbat; + return testBatteryVoltage; } + +int32_t getAmperage(void) { + return testAmperage; +} + +int32_t getMAhDrawn(void) { + return testMAhDrawn; +} + } diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index 6a90a3ac87..2dd03a924b 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -37,7 +37,7 @@ extern "C" { extern "C" { - uint8_t batteryCellCount = 3; + uint8_t testBatteryCellCount =3; int16_t rcCommand[4] = {0, 0, 0, 0}; telemetryConfig_t telemetryConfig_System; } @@ -62,6 +62,16 @@ typedef struct serialPortStub_s { } serialPortStub_t; +static uint16_t testBatteryVoltage = 100; +uint16_t getBatteryVoltage(void) +{ + return testBatteryVoltage; +} + +uint8_t getBatteryCellCount(void) { + return testBatteryCellCount; +} + static serialPortStub_t serialWriteStub; static serialPortStub_t serialReadStub; @@ -367,7 +377,7 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattZero) { //Given ibus command: Sensor at address 1, please send your measurement //then we respond with: I'm reading 0 volts - vbat = 0; + testBatteryVoltage = 0; checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x00\x00\x58\xFF", 6); } @@ -377,14 +387,14 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattCellV //Given ibus command: Sensor at address 1, please send your measurement //then we respond with: I'm reading 0.1 volts - batteryCellCount = 3; - vbat = 30; + testBatteryCellCount =3; + testBatteryVoltage = 30; checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6); //Given ibus command: Sensor at address 1, please send your measurement //then we respond with: I'm reading 0.1 volts - batteryCellCount = 1; - vbat = 10; + testBatteryCellCount =1; + testBatteryVoltage = 10; checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6); } @@ -394,14 +404,14 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattPackV //Given ibus command: Sensor at address 1, please send your measurement //then we respond with: I'm reading 0.1 volts - batteryCellCount = 3; - vbat = 10; + testBatteryCellCount =3; + testBatteryVoltage = 10; checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6); //Given ibus command: Sensor at address 1, please send your measurement //then we respond with: I'm reading 0.1 volts - batteryCellCount = 1; - vbat = 10; + testBatteryCellCount =1; + testBatteryVoltage = 10; checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6); } @@ -488,8 +498,8 @@ TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToGetMeasureme { //Given ibus command: Sensor at address 3, please send your measurement //then we respond with: I'm reading 0.1 volts - batteryCellCount = 1; - vbat = 10; + testBatteryCellCount = 1; + testBatteryVoltage = 10; checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xfe", 6); //Given ibus command: Sensor at address 4, please send your measurement