From e39ae6d48c4feec24ce1a4589b25809c9b64918f Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Fri, 7 Apr 2017 20:27:31 -0700
Subject: [PATCH] AP_BattMonitor: Add cell monitoring

---
 libraries/AP_BattMonitor/AP_BattMonitor.cpp | 13 +++++++++++++
 libraries/AP_BattMonitor/AP_BattMonitor.h   | 10 ++++++++++
 2 files changed, 23 insertions(+)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
index cfd391d7eb..db783036ef 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
@@ -162,6 +162,9 @@ AP_BattMonitor::init()
 
     // create each instance
     for (uint8_t instance=0; instance<AP_BATT_MONITOR_MAX_INSTANCES; instance++) {
+        // clear out the cell voltages
+        memset(&state[instance].cell_voltages, 0xFF, sizeof(cells));
+
         uint8_t monitor_type = _monitoring[instance];
         switch (monitor_type) {
             case BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY:
@@ -333,3 +336,13 @@ bool AP_BattMonitor::overpower_detected(uint8_t instance) const
     return false;
 #endif
 }
+
+// return the current cell voltages, returns the first monitor instances cells if the instance is out of range
+const AP_BattMonitor::cells & AP_BattMonitor::get_cell_voltages(const uint8_t instance) const
+{
+    if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
+        return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages;
+    } else {
+        return state[instance].cell_voltages;
+    }
+}
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h
index 0c7c0ed3db..22ad4b72b2 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.h
@@ -3,6 +3,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
 
 // maximum number of battery monitors
 #define AP_BATT_MONITOR_MAX_INSTANCES       2
@@ -44,6 +45,10 @@ public:
         BattMonitor_TYPE_MAXELL                     = 7
     };
 
+    struct cells {
+        uint16_t cells[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN];
+    };
+
     // The BattMonitor_State structure is filled in by the backend driver
     struct BattMonitor_State {
         uint8_t     instance;           // the instance number of this monitor
@@ -54,6 +59,7 @@ public:
         float       current_total_mah;  // total current draw since start-up
         uint32_t    last_time_micros;   // time when voltage and current was last read
         uint32_t    low_voltage_start_ms;  // time when voltage dropped below the minimum
+        cells       cell_voltages;      // battery cell voltages in millivolts, 10 cells matches the MAVLink spec
     };
 
     // Return the number of battery monitor instances
@@ -116,6 +122,10 @@ public:
     bool overpower_detected() const;
     bool overpower_detected(uint8_t instance) const;
 
+    // cell voltages
+    const cells & get_cell_voltages() { return get_cell_voltages(AP_BATT_PRIMARY_INSTANCE); };
+    const cells & get_cell_voltages(const uint8_t instance) const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected: