AP_BattMonitor: UAVCAN battery information added

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -2,6 +2,9 @@
 #include "AP_BattMonitor_Analog.h"
 #include "AP_BattMonitor_SMBus.h"
 #include "AP_BattMonitor_Bebop.h"
+#if HAL_WITH_UAVCAN
+#include "AP_BattMonitor_UAVCAN.h"
+#endif
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <DataFlash/DataFlash.h>
 
@@ -20,7 +23,6 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
     // @Path: AP_BattMonitor_Params.cpp
     AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params),
 
-
     AP_GROUPEND
 };
 
@@ -83,6 +85,12 @@ AP_BattMonitor::init()
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
                 drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]);
                 _num_instances++;
+#endif
+                break;
+            case AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo:
+#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_NONE:

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -25,6 +25,7 @@ class AP_BattMonitor_Analog;
 class AP_BattMonitor_SMBus;
 class AP_BattMonitor_SMBus_Solo;
 class AP_BattMonitor_SMBus_Maxell;
+class AP_BattMonitor_UAVCAN;
 
 class AP_BattMonitor
 {
@@ -33,6 +34,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_SMBus;
     friend class AP_BattMonitor_SMBus_Solo;
     friend class AP_BattMonitor_SMBus_Maxell;
+    friend class AP_BattMonitor_UAVCAN;
 
 public:
     AP_BattMonitor(uint32_t log_battery_bit);

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -49,6 +49,10 @@ public:
     // update battery resistance estimate and voltage_resting_estimate
     void update_resistance_estimate();
 
+    // callback for UAVCAN messages
+    virtual void handle_bi_msg(float voltage, float current,
+            float temperature) {}
+
 protected:
     AP_BattMonitor                      &_mon;      // reference to front-end
     AP_BattMonitor::BattMonitor_State   &_state;    // reference to this instances state (held in the front-end)

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -7,7 +7,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-Maxell
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo
     // @User: Standard
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),
 
@@ -63,7 +63,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
 
     // @Param: SERIAL_NUM
     // @DisplayName: Battery serial number
-    // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1
+    // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1. With UAVCAN it is the battery_id.
     // @User: Advanced
     AP_GROUPINFO("SERIAL_NUM", 9, AP_BattMonitor_Params, _serial_number, AP_BATT_SERIAL_NUMBER_DEFAULT),
 

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

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

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp

@@ -0,0 +1,80 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_UAVCAN.h"
+
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define debug_bm_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
+
+/// Constructor
+AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params) :
+    AP_BattMonitor_Backend(mon, mon_state, params),
+    _type(type)
+{
+    // starts with not healthy
+    _state.healthy = false;
+}
+
+void AP_BattMonitor_UAVCAN::init()
+{
+    if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) {
+        for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
+            if (hal.can_mgr[i] != nullptr) {
+                AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
+                if (uavcan != nullptr) {
+                    switch (_type) {
+                        case UAVCAN_BATTERY_INFO:
+                            if (uavcan->register_BM_bi_listener_to_id(this, _params._serial_number)) {
+                                debug_bm_uavcan(2, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number);
+                            }
+                            break;
+                    }
+                }
+            }
+        }
+    }
+}
+
+// read - read the voltage and current
+void AP_BattMonitor_UAVCAN::read()
+{
+    uint32_t tnow = AP_HAL::micros();
+
+    // timeout after 5 seconds
+    if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) {
+        _state.healthy = false;
+    }
+}
+
+void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float temperature)
+{
+    _state.temperature = temperature;
+    _state.voltage = voltage;
+    _state.current_amps = current;
+
+    uint32_t tnow = AP_HAL::micros();
+    uint32_t dt = tnow - _state.last_time_micros;
+
+    // update total current drawn since startup
+    if (_state.last_time_micros != 0 && dt < 2000000) {
+        // .0002778 is 1/3600 (conversion to hours)
+        float mah = (float) ((double) _state.current_amps * (double) dt * (double) 0.0000002778f);
+        _state.current_total_mah += mah;
+        _state.consumed_wh  += 0.001f * mah * _state.voltage;
+    }
+
+    // record time
+    _state.last_time_micros = tnow;
+
+    _state.healthy = true;
+}
+
+#endif

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_Backend.h"
+
+#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS         5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
+
+class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
+{
+public:
+
+    enum BattMonitor_UAVCAN_Type {
+        UAVCAN_BATTERY_INFO = 0
+    };
+
+    /// Constructor
+    AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params);
+
+    /// Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+
+    void init() override;
+
+    bool has_current() const override {
+        return true;
+    }
+
+    void handle_bi_msg(float voltage, float current, float temperature) override;
+
+protected:
+    BattMonitor_UAVCAN_Type _type;
+};