AP_BattMonitor: added ESC telemetry virtual battery

this aggregates all BLHeli ESCs into a single virtual battery

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -2,6 +2,7 @@
 #include "AP_BattMonitor_Analog.h"
 #include "AP_BattMonitor_SMBus.h"
 #include "AP_BattMonitor_Bebop.h"
+#include "AP_BattMonitor_BLHeliESC.h"
 #if HAL_WITH_UAVCAN
 #include "AP_BattMonitor_UAVCAN.h"
 #endif
@@ -96,6 +97,12 @@ AP_BattMonitor::init()
 #if HAL_WITH_UAVCAN
                 drivers[instance] = new AP_BattMonitor_UAVCAN(*this, state[instance], AP_BattMonitor_UAVCAN::UAVCAN_BATTERY_INFO, _params[instance]);
                 _num_instances++;
+#endif
+                break;
+            case AP_BattMonitor_Params::BattMonitor_TYPE_BLHeliESC:
+#ifdef HAVE_AP_BLHELI_SUPPORT
+                drivers[instance] = new AP_BattMonitor_BLHeliESC(*this, state[instance], _params[instance]);
+                _num_instances++;
 #endif
                 break;
             case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:

libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.cpp

@@ -0,0 +1,78 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_BLHeli/AP_BLHeli.h>
+
+#ifdef HAVE_AP_BLHELI_SUPPORT
+
+#include "AP_BattMonitor_BLHeliESC.h"
+
+extern const AP_HAL::HAL &hal;
+
+void AP_BattMonitor_BLHeliESC::init(void)
+{
+}
+
+void AP_BattMonitor_BLHeliESC::read(void)
+{
+    AP_BLHeli *blheli = AP_BLHeli::get_instance();
+    if (!blheli) {
+        return;
+    }
+
+    uint8_t num_escs = 0;
+    float voltage_sum=0;
+    float current_sum=0;
+    float consumed_sum=0;
+    float temperature_sum=0;
+    uint32_t now = AP_HAL::millis();
+    uint32_t highest_ms=0;
+    
+    for (uint8_t i=0; i<8; i++) {
+        AP_BLHeli::telem_data td;
+        uint32_t timestamp_ms;
+        blheli->get_telem_data(i, td, timestamp_ms);
+        if (now - timestamp_ms > 1000) {
+            // don't use old data
+            continue;
+        }
+        num_escs++;
+        voltage_sum += td.voltage;
+        current_sum += td.current;
+        consumed_sum += td.consumption;
+        temperature_sum += td.temperature;
+        if (timestamp_ms > highest_ms) {
+            highest_ms = timestamp_ms;
+        }
+    }
+
+    if (num_escs > 0) {
+        _state.voltage = (voltage_sum / num_escs) * 0.01;
+        _state.temperature = temperature_sum / num_escs;
+        _state.healthy = true;
+    } else {
+        _state.voltage = 0;
+        _state.temperature = 0;
+        _state.healthy = false;
+    }
+    _state.current_amps = current_sum * 0.01;
+    _state.consumed_mah = consumed_sum;
+    _state.last_time_micros = highest_ms * 1000;
+    _state.temperature_time = highest_ms;
+}
+
+#endif // HAVE_AP_BLHELI_SUPPORT

libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.h

@@ -0,0 +1,39 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include "AP_BattMonitor_Backend.h"
+
+class AP_BattMonitor_BLHeliESC :public AP_BattMonitor_Backend
+{
+public:
+    // constructor. This incorporates initialisation as well.
+    AP_BattMonitor_BLHeliESC(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params):
+        AP_BattMonitor_Backend(mon, mon_state, params)
+    {};
+
+    virtual ~AP_BattMonitor_BLHeliESC(void) {};
+
+    // initialise
+    void init() override;
+
+    // read the latest battery voltage
+    void read() override;
+
+    // BLHeliESC provides current info
+    bool has_current() const override { return true; };
+};

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -20,7 +20,8 @@ public:
         BattMonitor_TYPE_SOLO                       = 5,
         BattMonitor_TYPE_BEBOP                      = 6,
         BattMonitor_TYPE_MAXELL                     = 7,
-        BattMonitor_TYPE_UAVCAN_BatteryInfo         = 8
+        BattMonitor_TYPE_UAVCAN_BatteryInfo         = 8,
+        BattMonitor_TYPE_BLHeliESC                  = 9
     };
 
     // low voltage sources (used for BATT_LOW_TYPE parameter)