From bb64e5c0b95262b1e04237b11aff4ece2e99a4ad Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Sat, 30 Jul 2022 22:48:59 +0300
Subject: [PATCH] enhanced battery emulation

---
 src/main/fc/fc_msp.c         |  8 ++++++++
 src/main/fc/runtime_config.h | 13 ++++++++-----
 src/main/sensors/battery.c   |  4 ++--
 3 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 22760b7ca..43396be75 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -3402,6 +3402,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		if (tmp_u8 != 2) break;
 
 		simulatorData.flags = sbufReadU8(src);
+
         if ((simulatorData.flags & SIMU_ENABLE) == 0) {
 
 			if (ARMING_FLAG(SIMULATOR_MODE)) { // just once
@@ -3518,6 +3519,13 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				else {
 					sbufAdvance(src, 2*3);
 				}
+
+                if (simulatorData.flags & SIMU_EXT_BATTERY_VOLTAGE) {
+                     simulatorData.vbat = sbufReadU8(src);
+                }
+                else {
+                     simulatorData.vbat = 126;
+                }
 			}
 			else {
 				DISABLE_STATE(GPS_FIX);
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index f586d84ef..21cef9f0e 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -167,16 +167,19 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);
 
 #ifdef USE_SIMULATOR
 typedef enum {
-	SIMU_ENABLE				= (1 << 0),
-	SIMU_SIMULATE_BATTERY	= (1 << 1),
-	SIMU_MUTE_BEEPER		= (1 << 2),
-	SIMU_USE_SENSORS		= (1 << 3),
-	SIMU_HAS_NEW_GPS_DATA	= (1 << 4)
+	SIMU_ENABLE					= (1 << 0),
+	SIMU_SIMULATE_BATTERY		= (1 << 1),
+	SIMU_MUTE_BEEPER			= (1 << 2),
+	SIMU_USE_SENSORS			= (1 << 3),
+	SIMU_HAS_NEW_GPS_DATA		= (1 << 4),
+	SIMU_EXT_BATTERY_VOLTAGE	= (1 << 5) //extend MSP_SIMULATOR format 2
 } simulatorFlags_t;
 
 typedef struct {
 	simulatorFlags_t flags;
 	uint8_t debugIndex;
+    uint8_t vbat;  //126 ->12.6V
+
 	int16_t INPUT_STABILIZED_ROLL;
 	int16_t INPUT_STABILIZED_PITCH;
 	int16_t INPUT_STABILIZED_YAW;
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 8312045c6..34cc5cb72 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -293,8 +293,8 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
 #ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE)) {
 		if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
-			vbat = 1260;
-			batteryFullVoltage = 1210;
+            vbat = ((uint16_t)simulatorData.vbat)*10;
+            batteryFullVoltage = 1260;
 			batteryWarningVoltage = 1020;
 			batteryCriticalVoltage = 960;
 		}
-- 
2.11.4.GIT