diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e6dcea7254..a8a702f4e0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3738,15 +3738,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); } -#if defined(USE_FAKE_BATT_SENSOR) if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { - fakeBattSensorSetVbat(sbufReadU8(src) * 10); + simulatorData.vbat = sbufReadU8(src); } else { -#endif - fakeBattSensorSetVbat((uint16_t)(SIMULATOR_FULL_BATTERY * 10.0f)); -#if defined(USE_FAKE_BATT_SENSOR) + simulatorData.vbat = SIMULATOR_FULL_BATTERY; } -#endif if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 15dadd1c59..175de20c48 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -179,7 +179,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) #ifdef USE_SIMULATOR simulatorData_t simulatorData = { - .flags = 0, - .debugIndex = 0 + .flags = 0, + .debugIndex = 0, + .vbat = 0 }; #endif diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 241e5f7e25..8f7bcaa678 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -171,7 +171,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); #define SIMULATOR_MSP_VERSION 2 // Simulator MSP version #define SIMULATOR_BARO_TEMP 25 // °C -#define SIMULATOR_FULL_BATTERY 12.6f // Volts +#define SIMULATOR_FULL_BATTERY 126 // Volts*10 #define SIMULATOR_HAS_OPTION(flag) ((simulatorData.flags & flag) != 0) typedef enum { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 78e52f75c2..3eaeecc5f0 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -84,7 +84,7 @@ static bool batteryUseCapacityThresholds = false; static bool batteryFullWhenPluggedIn = false; static bool profileAutoswitchDisable = false; -static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) +static uint16_t vbat = 0; // battery voltage in 0.01V steps (filtered) static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm static uint16_t sagCompensatedVBat = 0; // calculated no load vbat static bool powerSupplyImpedanceIsValid = false; @@ -297,6 +297,14 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) vbat = 0; break; } + +#ifdef USE_SIMULATOR + if (ARMING_FLAG(SIMULATOR_MODE_HITL) && SIMULATOR_HAS_OPTION(HITL_SIMULATE_BATTERY)) { + vbat = ((uint16_t)simulatorData.vbat)*10; + return; + } +#endif + if (justConnected) { pt1FilterReset(&vbatFilterState, vbat); } else {