diff --git a/src/main/mw.c b/src/main/mw.c index 7085a8a7a0..5645c34d9b 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -246,7 +246,7 @@ void annexCode(void) } if (feature(FEATURE_CURRENT_METER)) { - updateCurrentMeter(vbatCycleTime); + updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } vbatCycleTime = 0; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 36bd34de06..e9c8d574ee 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -20,16 +20,18 @@ #include "common/maths.h" -#include "config/runtime_config.h" - #include "drivers/adc.h" #include "drivers/system.h" -#include "rx/rx.h" -#include "io/rc_controls.h" +#include "config/runtime_config.h" +#include "config/config.h" #include "sensors/battery.h" +#include "rx/rx.h" + +#include "io/rc_controls.h" + // Battery monitoring stuff uint8_t batteryCellCount = 3; // cell count uint16_t batteryWarningVoltage; @@ -111,7 +113,7 @@ int32_t currentSensorToCentiamps(uint16_t src) return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps } -void updateCurrentMeter(int32_t lastUpdateAt) +void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; @@ -127,6 +129,9 @@ void updateCurrentMeter(int32_t lastUpdateAt) case CURRENT_SENSOR_VIRTUAL: amperage = (int32_t)batteryConfig->currentMeterOffset; if(ARMING_FLAG(ARMED)) { + throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) + throttleOffset = 0; throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 60404b0f53..fdbc881626 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + #define VBAT_SCALE_DEFAULT 110 #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 @@ -62,7 +64,7 @@ batteryState_e calculateBatteryState(void); void updateBatteryVoltage(void); void batteryInit(batteryConfig_t *initialBatteryConfig); -void updateCurrentMeter(int32_t lastUpdateAt); +void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle); int32_t currentMeterToCentiamps(uint16_t src); uint8_t calculateBatteryPercentage(void); diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 0f51dffad4..b63af366c1 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -22,6 +22,8 @@ extern "C" { #include "sensors/battery.h" + + #include "io/rc_controls.h" } #include "unittest_macros.h" @@ -88,6 +90,19 @@ extern "C" { uint8_t armingFlags = 0; int16_t rcCommand[4] = {0,0,0,0}; +bool feature(uint32_t mask) +{ + UNUSED(mask); + return false; +} + +throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +{ + UNUSED(*rxConfig); + UNUSED(deadband3d_throttle); + return THROTTLE_HIGH; +} + uint16_t adcGetChannel(uint8_t channel) { UNUSED(channel);