From d05d422bc098f44c6612ad2a4248899c9e6c8951 Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Sat, 2 Jan 2021 01:53:54 +0100
Subject: [PATCH] Made fuel reporting for FrSky telemetry consistent.

---
 src/main/cli/settings.c        |  1 +
 src/main/telemetry/smartport.c | 19 ++++++++++++++++++-
 src/main/telemetry/telemetry.c |  4 ++--
 src/main/telemetry/telemetry.h |  3 ++-
 4 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 1eb0c73ad0..5a7b548204 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1215,6 +1215,7 @@ const clivalue_t valueTable[] = {
     { "telemetry_disabled_esc_rpm",         VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(ESC_SENSOR_RPM),         PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)},
     { "telemetry_disabled_esc_temperature", VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(ESC_SENSOR_TEMPERATURE), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)},
     { "telemetry_disabled_temperature",     VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_TEMPERATURE),     PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)},
+    { "telemetry_disabled_cap_used",        VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_CAP_USED),        PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)},
 #else
     { "telemetry_disabled_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)},
 #endif
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 3cecbf8cfd..d3fddbf943 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -121,11 +121,12 @@ enum
     FSSP_DATAID_ADC1       = 0xF102 ,
     FSSP_DATAID_ADC2       = 0xF103 ,
     FSSP_DATAID_LATLONG    = 0x0800 ,
-    FSSP_DATAID_CAP_USED   = 0x0600 ,
     FSSP_DATAID_VARIO      = 0x0110 ,
     FSSP_DATAID_CELLS      = 0x0300 ,
     FSSP_DATAID_CELLS_LAST = 0x030F ,
     FSSP_DATAID_HEADING    = 0x0840 ,
+// DIY range 0x5100 to 0x52FF
+    FSSP_DATAID_CAP_USED   = 0x5250 ,
 #if defined(USE_ACC)
     FSSP_DATAID_PITCH      = 0x5230 , // custom
     FSSP_DATAID_ROLL       = 0x5240 , // custom
@@ -360,6 +361,10 @@ static void initSmartPortSensors(void)
         if (telemetryIsSensorEnabled(SENSOR_FUEL)) {
             ADD_SENSOR(FSSP_DATAID_FUEL);
         }
+
+        if (telemetryIsSensorEnabled(SENSOR_CAP_USED)) {
+            ADD_SENSOR(FSSP_DATAID_CAP_USED);
+        }
     }
 
     if (telemetryIsSensorEnabled(SENSOR_HEADING)) {
@@ -696,6 +701,18 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 *clearToSend = false;
                 break;
             case FSSP_DATAID_FUEL       :
+                {
+                    uint32_t data;
+                    if (batteryConfig()->batteryCapacity > 0) {
+                        data = calculateBatteryPercentageRemaining();
+                    } else {
+                        data = getMAhDrawn();
+                    }
+                    smartPortSendPackage(id, data);
+                    *clearToSend = false;
+                }
+                break;
+            case FSSP_DATAID_CAP_USED   :
                 smartPortSendPackage(id, getMAhDrawn()); // given in mAh, should be in percent according to SmartPort spec
                 *clearToSend = false;
                 break;
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 7a713996fe..2e907bfaff 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -60,7 +60,7 @@
 #include "telemetry/ibus.h"
 #include "telemetry/msp_shared.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 5);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_inverted = false,
@@ -78,7 +78,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
             IBUS_SENSOR_TYPE_RPM_FLYSKY,
             IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE
     },
-    .disabledSensors = ESC_SENSOR_ALL,
+    .disabledSensors = ESC_SENSOR_ALL | SENSOR_CAP_USED,
     .mavlink_mah_as_heading_divisor = 0,
 );
 
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index 6ba8d0f205..e11f1f1c32 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -67,7 +67,8 @@ typedef enum {
                             | ESC_SENSOR_RPM \
                             | ESC_SENSOR_TEMPERATURE,
     SENSOR_TEMPERATURE     = 1 << 19,
-    SENSOR_ALL             = (1 << 20) - 1,
+    SENSOR_CAP_USED        = 1 << 20,
+    SENSOR_ALL             = (1 << 21) - 1,
 } sensor_e;
 
 typedef struct telemetryConfig_s {