AP_BattMonitor: add torqeedo battery monitor

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -16,6 +16,7 @@
 #include "AP_BattMonitor_MPPT_PacketDigital.h"
 #include "AP_BattMonitor_INA231.h"
 #include "AP_BattMonitor_LTC2946.h"
+#include "AP_BattMonitor_Torqeedo.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -306,6 +307,11 @@ AP_BattMonitor::init()
             case Type::LTC2946:
                 drivers[instance] = new AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]);
                 break;
+#endif
+#if HAL_TORQEEDO_ENABLED
+            case Type::Torqeedo:
+                drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]);
+                break;
 #endif
             case Type::NONE:
             default:

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -48,6 +48,7 @@ class AP_BattMonitor_Generator;
 class AP_BattMonitor_MPPT_PacketDigital;
 class AP_BattMonitor_INA231;
 class AP_BattMonitor_LTC2946;
+class AP_BattMonitor_Torqeedo;
 
 class AP_BattMonitor
 {
@@ -67,6 +68,8 @@ class AP_BattMonitor
     friend class AP_BattMonitor_INA231;
     friend class AP_BattMonitor_LTC2946;
 
+    friend class AP_BattMonitor_Torqeedo;
+
 public:
 
     // battery failsafes must be defined in levels of severity so that vehicles wont fall backwards
@@ -99,6 +102,7 @@ public:
         MPPT_PacketDigital         = 20,
         INA231                     = 21,
         LTC2946                    = 22,
+        Torqeedo                   = 23,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

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: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,20:MPPT,21:INA231,22:LTC2946
+    // @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,20:MPPT,21:INA231,22:LTC2946,23:Torqeedo
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),

libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.cpp

@@ -0,0 +1,77 @@
+/*
+   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_Torqeedo.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+#define AP_BATTMON_TORQEEDO_TIMEOUT_US 5000000
+
+extern const AP_HAL::HAL &hal;
+
+void AP_BattMonitor_Torqeedo::read(void)
+{
+    AP_Torqeedo *torqeedo = AP_Torqeedo::get_singleton();
+    if (torqeedo == nullptr) {
+        _state.healthy = false;
+        return;
+    }
+
+    // get voltage, current, temp and remaining capacity percentage
+    float volts;
+    float current_amps;
+    float temp_C;
+    if (torqeedo->get_batt_info(volts, current_amps, temp_C, remaining_pct)) {
+        have_info = true;
+        _state.voltage = volts;
+        _state.current_amps = current_amps;
+        _state.temperature = temp_C;
+        _state.temperature_time = AP_HAL::millis();
+
+        // update total current draw
+        const uint32_t tnow_us = AP_HAL::micros();
+        const uint32_t diff_us = tnow_us - _state.last_time_micros;
+        if (diff_us < AP_BATTMON_TORQEEDO_TIMEOUT_US) {
+            _state.consumed_mah += _state.current_amps * diff_us / 1000000.0 / 3600.0 * 1000.0;
+        }
+        _state.last_time_micros = tnow_us;
+        _state.healthy = true;
+    }
+
+    // read battery pack capacity
+    if (!have_capacity) {
+        uint16_t batt_capacity_ah;
+        if (torqeedo->get_batt_capacity_Ah(batt_capacity_ah)) {
+            have_capacity = true;
+            if (batt_capacity_ah * 1000 != _params._pack_capacity) {
+                _params._pack_capacity.set_and_notify(batt_capacity_ah * 1000);
+            }
+        }
+    }
+}
+
+// capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument
+// returns false if the battery is unhealthy, does not have current monitoring, or the pack_capacity is too small
+bool AP_BattMonitor_Torqeedo::capacity_remaining_pct(uint8_t &percentage) const
+{
+    if (have_info) {
+        percentage = remaining_pct;
+    }
+    return have_info;
+}
+
+#endif // HAL_TORQEEDO_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Torqeedo.h

@@ -0,0 +1,52 @@
+/*
+   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_Torqeedo/AP_Torqeedo.h>
+#include "AP_BattMonitor_Backend.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+class AP_BattMonitor_Torqeedo: public AP_BattMonitor_Backend
+{
+public:
+    // constructor. This incorporates initialisation as well.
+    AP_BattMonitor_Torqeedo(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params):
+        AP_BattMonitor_Backend(mon, mon_state, params)
+    {};
+
+    // read the latest battery voltage
+    void read() override;
+
+    /// returns true if battery monitor instance provides current info
+    bool has_current() const override { return have_info; };
+
+    // returns true if battery monitor provides temperature
+    bool has_temperature() const override { return have_info; };
+
+    // capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument
+    // returns false if the battery is unhealthy, does not have current monitoring, or the pack_capacity is too small
+    bool capacity_remaining_pct(uint8_t &percentage) const override WARN_IF_UNUSED;
+
+private:
+
+    bool have_info;             // true if torqeedo has provided battery info at least once
+    bool have_capacity;         // true once torqeedo has provided battery capacity
+    uint8_t remaining_pct;      // battery remaining capacity as a percentage
+};
+
+#endif