Added logging of BMU battery ID

ArduCopter/Log.cpp

@@ -550,6 +550,7 @@ void Copter::Log_Write_Vehicle_Startup_Messages()
 #endif
     ahrs.Log_Write_Home_And_Origin();
     gps.Write_DataFlash_Log_Startup_messages();
+    battery.Write_DataFlash_Log_Startup_messages();
 }
 
 

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -520,6 +520,15 @@ bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage)
     return ret;
 }
 
+void AP_BattMonitor::Write_DataFlash_Log_Startup_messages()
+{
+    for (uint8_t i = 0; i < _num_instances; i++) {
+        if (drivers[i]) {
+            drivers[i]->Write_DataFlash_Log_Startup_messages();
+        }
+    }
+}
+
 namespace AP {
 
 AP_BattMonitor &battery()

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -169,6 +169,8 @@ class AP_BattMonitor
     // reset battery remaining percentage
     bool reset_remaining(uint16_t battery_mask, float percentage);
 
+    void Write_DataFlash_Log_Startup_messages();
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:

libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h

@@ -15,11 +15,11 @@ class AP_BattMonitor_Analog_Table : public AP_BattMonitor_Analog
 
     /// returns true if battery monitor provides current info
     bool has_current() const override { return true; }
+
+    // lookup SoC in table, returning SoC as a percentage
+    static float lookup_SoC_table(float voltage);
 
 private:
     uint32_t last_armed_ms;
     bool using_table = true;
-
-    // lookup SoC in table, returning SoC as a percentage
-    float lookup_SoC_table(float voltage);
 };

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -51,11 +51,14 @@ class AP_BattMonitor_Backend
 
     // callback for UAVCAN messages
     virtual void handle_bi_msg(float voltage, float current,
-            float temperature) {}
+                               float temperature,
+                               uint32_t model_instance_id, uint8_t model_name[33]) {}
 
     // reset remaining percentage to given value
     virtual bool reset_remaining(float percentage);
 
+    virtual void Write_DataFlash_Log_Startup_messages() {};
+
 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_UAVCAN.cpp

@@ -6,9 +6,11 @@
 #include <AP_Math/AP_Math.h>
 #include "AP_BattMonitor.h"
 #include "AP_BattMonitor_UAVCAN.h"
+#include "AP_BattMonitor_Analog_Table.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -50,18 +52,30 @@ void AP_BattMonitor_UAVCAN::read()
 {
     uint32_t tnow = AP_HAL::micros();
 
+    if (batt_changed) {
+        batt_changed = false;
+        float soc_pct = AP_BattMonitor_Analog_Table::lookup_SoC_table(_state.voltage);
+        reset_remaining(soc_pct);
+        Write_DataFlash_Log_Startup_messages();
+    }
+
     // 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)
+void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float temperature, uint32_t _model_instance_id, uint8_t _model_name[33])
 {
     _state.temperature = temperature;
     _state.voltage = voltage;
     _state.current_amps = current;
 
+    batt_changed = strncmp((const char *)model_name, (const char *)_model_name, sizeof(model_name)) != 0;
+
+    model_instance_id = _model_instance_id;
+    memcpy(model_name, _model_name, sizeof(model_name));
+
     uint32_t tnow = AP_HAL::micros();
     uint32_t dt = tnow - _state.last_time_micros;
 
@@ -79,4 +93,9 @@ void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float te
     _state.healthy = true;
 }
 
+void AP_BattMonitor_UAVCAN::Write_DataFlash_Log_Startup_messages()
+{
+    gcs().send_text(MAV_SEVERITY_INFO, "BMU Inst=%u Model=%s", unsigned(model_instance_id), (const char *)model_name);
+}
+
 #endif

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h

@@ -26,8 +26,13 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
         return true;
     }
 
-    void handle_bi_msg(float voltage, float current, float temperature) override;
+    void handle_bi_msg(float voltage, float current, float temperature, uint32_t model_instance_id, uint8_t model_name[33]) override;
+
+    void Write_DataFlash_Log_Startup_messages() override;
 
 protected:
     BattMonitor_UAVCAN_Type _type;
+    uint32_t model_instance_id;
+    char model_name[33];
+    bool batt_changed;
 };

libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm

@@ -30,6 +30,8 @@ ATC_THR_MIX_MAX	0.5
 ATC_THR_MIX_MIN	0.1
 BATT_CAPACITY	13000
 BATT_MONITOR	20
+BATT2_MONITOR   8
+BATT2_CAPACITY	13000
 BATT_VOLT_MULT	22.7
 BRD_SAFETYENABLE	0
 BRD_SAFETY_MASK	0

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -366,6 +366,12 @@ static void battery_info_st_cb(const uavcan::ReceivedDataStructure<uavcan::equip
     state->full_charge_capacity_wh = msg.full_charge_capacity_wh;
     state->remaining_capacity_wh = msg.remaining_capacity_wh;
     state->status_flags = msg.status_flags;
+    state->model_instance_id = msg.model_instance_id;
+    memset(state->model_name, 0, sizeof(state->model_name));
+    const uint8_t len = msg.model_name.size();
+    for (uint8_t i=0; i<len; i++) {
+        state->model_name[i] = msg.model_name[i];
+    }
 
     // after all is filled, update all listeners with new data
     ap_uavcan->update_bi_state((uint16_t) msg.battery_id);
@@ -1316,7 +1322,7 @@ void AP_UAVCAN::update_mag_state(uint8_t node, uint8_t sensor_id)
     }
 }
 
-uint8_t AP_UAVCAN::register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id)
+uint8_t AP_UAVCAN::register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, int32_t id)
 {
     uint8_t sel_place = UINT8_MAX, ret = 0;
 
@@ -1332,7 +1338,7 @@ uint8_t AP_UAVCAN::register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_lis
     }
 
     for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
-        if (_bi_id[i] != id) {
+        if (id != -1 && _bi_id[i] != id) {
             continue;
         }
         _bi_BM_listeners[sel_place] = new_listener;
@@ -1410,7 +1416,8 @@ void AP_UAVCAN::update_bi_state(uint8_t id)
             if (_bi_BM_listener_to_id[j] != i) {
                 continue;
             }
-            _bi_BM_listeners[j]->handle_bi_msg(_bi_id_state[i].voltage, _bi_id_state[i].current, _bi_id_state[i].temperature);
+            _bi_BM_listeners[j]->handle_bi_msg(_bi_id_state[i].voltage, _bi_id_state[i].current, _bi_id_state[i].temperature,
+                                               _bi_id_state[i].model_instance_id, _bi_id_state[i].model_name);
         }
     }
 }

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -118,9 +118,11 @@ class AP_UAVCAN {
         float remaining_capacity_wh;
         float full_charge_capacity_wh;
         uint8_t status_flags;
+        uint32_t model_instance_id;
+        uint8_t model_name[32+1];
     };
 
-    uint8_t register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id);
+    uint8_t register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, int32_t id);
     void remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener);
     BatteryInfo_Info *find_bi_id(uint8_t id);
     uint8_t find_smallest_free_bi_id();