From 4c2ab8d2dd3ca3ac5abbc8482d9dc49507e92ee0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Oct 2019 14:01:05 +1000 Subject: [PATCH 1/6] AP_BattMonitor: log battery model --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 9 +++++++++ libraries/AP_BattMonitor/AP_BattMonitor.h | 2 ++ .../AP_BattMonitor/AP_BattMonitor_Backend.h | 5 ++++- .../AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 16 +++++++++++++++- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 6 +++++- 5 files changed, 35 insertions(+), 3 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index d04362eac9..2dc2e93689 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 702fc935d4..5f75e5edff 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/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: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 1ad65cd1fa..35af588013 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/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) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 3706da099d..ff394d391a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -9,6 +9,7 @@ #include #include +#include extern const AP_HAL::HAL& hal; @@ -56,12 +57,17 @@ void AP_BattMonitor_UAVCAN::read() } } -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; + bool need_log = 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; @@ -77,6 +83,14 @@ void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float te _state.last_time_micros = tnow; _state.healthy = true; + if (need_log) { + Write_DataFlash_Log_Startup_messages(); + } +} + +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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 7b63ee7a1c..c2128cd8ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -26,8 +26,12 @@ 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]; }; From 05a7ecd5f37d521355082ca760960bc508ee8554 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Oct 2019 14:01:18 +1000 Subject: [PATCH 2/6] AP_UAVCAN: pass batt model to AP_BattMonitor --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 9 ++++++++- libraries/AP_UAVCAN/AP_UAVCAN.h | 2 ++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 3a6c5760e7..1721a7f683 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -366,6 +366,12 @@ static void battery_info_st_cb(const uavcan::ReceivedDataStructurefull_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; imodel_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); @@ -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); } } } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index d747a44c4c..7a99bee388 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -118,6 +118,8 @@ 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); From 594c71fc43478acbc19579633fd4f9b200a9236a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Oct 2019 14:12:43 +1000 Subject: [PATCH 3/6] Copter: log battery IDs --- ArduCopter/Log.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index c03a3b97f2..9c5c8561f4 100644 --- a/ArduCopter/Log.cpp +++ b/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(); } From 0e418b255e7dc300398c54f2e9ae0675a63a4505 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Oct 2019 14:23:46 +1000 Subject: [PATCH 4/6] AP_BattMonitor: use table for initial SoC for uavcan --- libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h | 6 +++--- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 7 +++++-- libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm | 2 ++ 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h b/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h index 9a344b2c25..da949c2b3a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h +++ b/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); }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index ff394d391a..80cca9b04c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -6,6 +6,7 @@ #include #include "AP_BattMonitor.h" #include "AP_BattMonitor_UAVCAN.h" +#include "AP_BattMonitor_Analog_Table.h" #include #include @@ -63,7 +64,7 @@ void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float te _state.voltage = voltage; _state.current_amps = current; - bool need_log = strncmp((const char *)model_name, (const char *)_model_name, sizeof(model_name)) != 0; + bool batt_change = 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)); @@ -83,7 +84,9 @@ void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float te _state.last_time_micros = tnow; _state.healthy = true; - if (need_log) { + if (batt_change) { + float soc_pct = AP_BattMonitor_Analog_Table::lookup_SoC_table(voltage); + reset_remaining(soc_pct); Write_DataFlash_Log_Startup_messages(); } } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm index 6c97992dbd..3281680a06 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm +++ b/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 From 501c2531f568bac20d4a6a6bdb0b06f73771adb5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Oct 2019 15:34:58 +1000 Subject: [PATCH 5/6] AP_UAVCAN: always accept battery serial number -1 --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 4 ++-- libraries/AP_UAVCAN/AP_UAVCAN.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 1721a7f683..f7ce8d6067 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -1322,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; @@ -1338,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; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 7a99bee388..e4a048bd01 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -122,7 +122,7 @@ class AP_UAVCAN { 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(); From 77bf21e55cc7bffcfb3a23b626c4c6f27a3f93b2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Oct 2019 15:51:34 +1000 Subject: [PATCH 6/6] AP_BattMonitor: write log data from main thread --- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 14 ++++++++------ libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 1 + 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 80cca9b04c..2af5f80936 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -52,6 +52,13 @@ 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; @@ -64,7 +71,7 @@ void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float te _state.voltage = voltage; _state.current_amps = current; - bool batt_change = strncmp((const char *)model_name, (const char *)_model_name, sizeof(model_name)) != 0; + 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)); @@ -84,11 +91,6 @@ void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float te _state.last_time_micros = tnow; _state.healthy = true; - if (batt_change) { - float soc_pct = AP_BattMonitor_Analog_Table::lookup_SoC_table(voltage); - reset_remaining(soc_pct); - Write_DataFlash_Log_Startup_messages(); - } } void AP_BattMonitor_UAVCAN::Write_DataFlash_Log_Startup_messages() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index c2128cd8ac..63e7e1167a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -34,4 +34,5 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend BattMonitor_UAVCAN_Type _type; uint32_t model_instance_id; char model_name[33]; + bool batt_changed; };