AP_BattMonitor: refector to use AP_ESC_Telem

rename AP_BattMonitor_BLHeliESC -> AP_BattMonitor_ESC
record volts, amps and consumption as floats
Correct ESC-telemetry-based voltage and temperature (<amilcar.lucas@iav.de>)
Correct ESC-telemetry-based voltage and temperature when less than 12 ESCs are used (<amilcar.lucas@iav.de>)
fix jumps in consumed current (<amilcar.lucas@iav.de>)
Implement temperature readings (<amilcar.lucas@iav.de>)
Fix temperature scaling (<amilcar.lucas@iav.de>)

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -6,7 +6,7 @@
 #include "AP_BattMonitor_SMBus_Maxell.h"
 #include "AP_BattMonitor_SMBus_Rotoye.h"
 #include "AP_BattMonitor_Bebop.h"
-#include "AP_BattMonitor_BLHeliESC.h"
+#include "AP_BattMonitor_ESC.h"
 #include "AP_BattMonitor_SMBus_SUI.h"
 #include "AP_BattMonitor_SMBus_NeoDesign.h"
 #include "AP_BattMonitor_Sum.h"
@@ -167,8 +167,8 @@ AP_BattMonitor::init()
 #endif
                 break;
             case Type::BLHeliESC:
-#ifdef HAVE_AP_BLHELI_SUPPORT
+#if HAL_WITH_ESC_TELEM && !defined(HAL_BUILD_AP_PERIPH)
-                drivers[instance] = new AP_BattMonitor_BLHeliESC(*this, state[instance], _params[instance]);
+                drivers[instance] = new AP_BattMonitor_ESC(*this, state[instance], _params[instance]);
 #endif
                 break;
             case Type::Sum:

libraries/AP_BattMonitor/AP_BattMonitor_BLHeliESC.cpp

@@ -1,90 +0,0 @@
-/*
-   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_singleton();
-    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<AP_BLHELI_MAX_ESCS; i++) {
-        AP_BLHeli::telem_data td;
-        if (!blheli->get_telem_data(i, td)) {
-            continue;
-        }
-
-        // accumulate consumed_sum regardless of age, to cope with ESC
-        // dropping out
-        consumed_sum += td.consumption;
-
-        if (now - td.timestamp_ms > 1000) {
-            // don't use old data
-            continue;
-        }
-
-        num_escs++;
-        voltage_sum += td.voltage;
-        current_sum += td.current;
-        temperature_sum += td.temperature;
-        if (td.timestamp_ms > highest_ms) {
-            highest_ms = td.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;
-
-    if (current_sum > 0) {
-        // if we have ever got a current value then we know we have a
-        // current sensor
-        have_current = true;
-    }
-}
-
-#endif // HAVE_AP_BLHELI_SUPPORT

libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp

@@ -0,0 +1,95 @@
+/*
+   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_BattMonitor_ESC.h"
+
+#if HAL_WITH_ESC_TELEM
+
+extern const AP_HAL::HAL &hal;
+
+void AP_BattMonitor_ESC::init(void)
+{
+}
+
+void AP_BattMonitor_ESC::read(void)
+{
+    AP_ESC_Telem& telem = AP::esc_telem();
+
+    uint8_t voltage_escs = 0;     // number of ESCs with valid voltage
+    uint8_t temperature_escs = 0; // number of ESCs with valid temperature
+    float voltage_sum = 0;
+    float current_sum = 0;
+    float temperature_sum = 0;
+    uint32_t highest_ms = 0;
+
+    for (uint8_t i=0; i<ESC_TELEM_MAX_ESCS; i++) {
+        int16_t  temperature_cdeg;
+        float voltage;
+        float current;
+        float consumption_mah;
+
+        if (telem.get_consumption_mah(i, consumption_mah)) {
+            // accumulate consumed_sum regardless of age, to cope with ESC
+            // dropping out
+            _state.consumed_mah += consumption_mah;
+        }
+
+        if (telem.get_voltage(i, voltage)) {
+            voltage_sum += voltage;
+            voltage_escs++;
+        }
+
+        if (telem.get_current(i, current)) {
+            current_sum += current;
+        }
+
+        if (telem.get_temperature(i, temperature_cdeg)) {
+            temperature_sum += float(temperature_cdeg) * 0.01f;
+            temperature_escs++;
+        }
+
+        if (telem.get_last_telem_data_ms(i) > highest_ms) {
+            highest_ms = telem.get_last_telem_data_ms(i);
+        }
+    }
+
+    if (voltage_escs > 0) {
+        _state.voltage = voltage_sum / voltage_escs;
+        _state.healthy = true;
+    } else {
+        _state.voltage = 0;
+        _state.healthy = false;
+    }
+    if (temperature_escs > 0) {
+        _state.temperature = temperature_sum / temperature_escs;
+        have_temperature = true;
+    } else {
+        _state.temperature = 0;
+    }
+
+    _state.current_amps = current_sum;
+    _state.last_time_micros = highest_ms * 1000;
+    _state.temperature_time = highest_ms;
+
+    if (current_sum > 0) {
+        // if we have ever got a current value then we know we have a
+        // current sensor
+        have_current = true;
+    }
+}
+
+#endif // HAL_WITH_ESC_TELEM

libraries/AP_BattMonitor/AP_BattMonitor_ESC.h

@@ -16,17 +16,20 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
+#include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include "AP_BattMonitor_Backend.h"
 
-class AP_BattMonitor_BLHeliESC :public AP_BattMonitor_Backend
+#if HAL_WITH_ESC_TELEM
+
+class AP_BattMonitor_ESC :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_ESC(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) {};
+    virtual ~AP_BattMonitor_ESC(void) {};
 
     // initialise
     void init() override;
@@ -34,9 +37,15 @@ public:
     // read the latest battery voltage
     void read() override;
 
-    // BLHeliESC provides current info
+    // ESC_Telem provides current info
     bool has_current() const override { return have_current; };
 
+    // ESC_Telem provides temperature info
+    bool has_temperature() const override { return have_temperature; };
+
 private:
     bool have_current;
+    bool have_temperature;
 };
+
+#endif

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -13,7 +13,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Param: MONITOR
     // @DisplayName: Battery monitoring
     // @Description: Controls enabling monitoring of the battery's voltage and current
-    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),