From fee79afb8e3c3e423e24f5401f7ae28f7889650b Mon Sep 17 00:00:00 2001
From: Florent Martel <flomartel@gmail.com>
Date: Tue, 10 Oct 2017 08:42:47 -0500
Subject: [PATCH] AP_Frsky_Telem: add second battery info

---
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 17 ++++++++++++-----
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.h   |  3 ++-
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
index 9734056091..3152f5aced 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
@@ -123,10 +123,17 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
                 return;
             }
             if ((now - _passthrough.batt_timer) >= 1000) {
-                send_uint32(DIY_FIRST_ID+3, calc_batt());
+                send_uint32(DIY_FIRST_ID+3, calc_batt(0));
                 _passthrough.batt_timer = AP_HAL::millis();
                 return;
             }
+            if (_battery.num_instances() > 1) {
+                if ((now - _passthrough.batt_timer2) >= 1000) {
+                    send_uint32(DIY_FIRST_ID+8, calc_batt(1));
+                    _passthrough.batt_timer2 = AP_HAL::millis();
+                    return;
+                }
+            }
             if ((now - _passthrough.gps_status_timer) >= 1000) {
                 send_uint32(DIY_FIRST_ID+2, calc_gps_status());
                 _passthrough.gps_status_timer = AP_HAL::millis();
@@ -652,16 +659,16 @@ uint32_t AP_Frsky_Telem::calc_gps_status(void)
  * prepare battery data
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
-uint32_t AP_Frsky_Telem::calc_batt(void)
+uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance)
 {
     uint32_t batt;
     
     // battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
-    batt = (((uint16_t)roundf(_battery.voltage() * 10.0f)) & BATT_VOLTAGE_LIMIT);
+    batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT);
     // battery current draw in deciamps
-    batt |= prep_number(roundf(_battery.current_amps() * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET; 
+    batt |= prep_number(roundf(_battery.current_amps(instance) * 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.current_total_mah() < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.current_total_mah()) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
+    batt |= ((_battery.current_total_mah(instance) < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.current_total_mah(instance)) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
     return batt;
 }
 
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
index 8c51ad074f..3c291bc225 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
@@ -198,6 +198,7 @@ private:
         uint32_t params_timer;
         uint32_t ap_status_timer;
         uint32_t batt_timer;
+        uint32_t batt_timer2;
         uint32_t gps_status_timer;
         uint32_t home_timer;
         uint32_t velandyaw_timer;
@@ -249,7 +250,7 @@ private:
     uint32_t calc_param(void);
     uint32_t calc_gps_latlng(bool *send_latitude);
     uint32_t calc_gps_status(void);
-    uint32_t calc_batt(void);
+    uint32_t calc_batt(uint8_t instance);
     uint32_t calc_ap_status(void);
     uint32_t calc_home(void);
     uint32_t calc_velandyaw(void);