From ea82d6ea4c4faf57c14ed28706e430b1bad66b20 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Sun, 7 Jul 2019 14:37:28 +0000
Subject: [PATCH] AP_FrSkyTelem: Refactor battery current interface

---
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 27 +++++++++++++++++----
 1 file changed, 22 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
index 5cd5559776..e8a9901090 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
@@ -214,8 +214,14 @@ void AP_Frsky_Telem::send_SPort(void)
                             send_uint32(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
                             break;
                         case 2:
-                            send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
-                            break;
+                            {
+                                float current;
+                                if (!_battery.current_amps(current)) {
+                                    current = 0;
+                                }
+                                send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
+                                break;
+                            }
                     }
                     if (_SPort.fas_call++ > 2) _SPort.fas_call = 0;
                     break;
@@ -306,7 +312,11 @@ void AP_Frsky_Telem::send_D(void)
         send_uint16(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
         send_uint16(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
         send_uint16(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
-        send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
+        float current;
+        if (!_battery.current_amps(current)) {
+            current = 0;
+        }
+        send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
         calc_nav_alt();
         send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part
         send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part
@@ -684,13 +694,20 @@ uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance)
     const AP_BattMonitor &_battery = AP::battery();
 
     uint32_t batt;
+    float current, consumed_mah;
+    if (!_battery.current_amps(current, instance)) {
+        current = 0;
+    }
+    if (!_battery.consumed_mah(consumed_mah, instance)) {
+        consumed_mah = 0;
+    }
     
     // battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
     batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT);
     // battery current draw in deciamps
-    batt |= prep_number(roundf(_battery.current_amps(instance) * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET; 
+    batt |= prep_number(roundf(current * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET;
     // battery current drawn since power on in mAh (limit to 32767 (0x7FFF) since value is stored on 15 bits)
-    batt |= ((_battery.consumed_mah(instance) < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.consumed_mah(instance)) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
+    batt |= ((consumed_mah < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(consumed_mah) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
     return batt;
 }