From e91953fa3b400f7ff9a00b180c482615cc62dfd0 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Wed, 6 Jan 2021 19:51:50 -0500 Subject: [PATCH] AP_BattMonitor: move log BAT & BCL into AP_BattMonitor --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 28 +++++---- .../AP_BattMonitor/AP_BattMonitor_Backend.h | 4 ++ .../AP_BattMonitor/AP_BattMonitor_Logging.cpp | 47 ++++++++++++++ libraries/AP_BattMonitor/LogStructure.h | 62 +++++++++++++++++++ 4 files changed, 130 insertions(+), 11 deletions(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp create mode 100644 libraries/AP_BattMonitor/LogStructure.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 7f6e40e3fd..10a5db3434 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -285,25 +285,31 @@ void AP_BattMonitor::convert_params(void) { _params[0]._type.save(true); } -// read - read the voltage and current for all instances -void -AP_BattMonitor::read() +// read - For all active instances read voltage & current; log BAT, BCL, POWR +void AP_BattMonitor::read() { +#ifndef HAL_BUILD_AP_PERIPH + AP_Logger *logger = AP_Logger::get_singleton(); + if (logger != nullptr && logger->should_log(_log_battery_bit)) { + logger->Write_Power(); + } +#endif + for (uint8_t i=0; i<_num_instances; i++) { if (drivers[i] != nullptr && get_type(i) != Type::NONE) { drivers[i]->read(); drivers[i]->update_resistance_estimate(); + +#ifndef HAL_BUILD_AP_PERIPH + if (logger != nullptr && logger->should_log(_log_battery_bit)) { + const uint64_t time_us = AP_HAL::micros64(); + drivers[i]->Log_Write_BAT(i, time_us); + drivers[i]->Log_Write_BCL(i, time_us); + } +#endif } } -#ifndef HAL_BUILD_AP_PERIPH - AP_Logger *logger = AP_Logger::get_singleton(); - if (logger != nullptr && logger->should_log(_log_battery_bit)) { - logger->Write_Current(); - logger->Write_Power(); - } -#endif - check_failsafes(); checkPoweringOff(); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 0b21a5a39a..4cf644580b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -68,6 +68,10 @@ public: // reset remaining percentage to given value virtual bool reset_remaining(float percentage); + // logging functions + void Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const; + void Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const; + 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_Logging.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp new file mode 100644 index 0000000000..593f9f949f --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp @@ -0,0 +1,47 @@ +#include "AP_BattMonitor_Backend.h" +#include + +extern const AP_HAL::HAL& hal; + +// Write BAT data packet(s) +void AP_BattMonitor_Backend::Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const +{ + bool has_curr = has_current(); + + const struct log_BAT pkt{ + LOG_PACKET_HEADER_INIT(LOG_BAT_MSG), + time_us : time_us, + instance : instance, + voltage : _state.voltage, + voltage_resting : _state.voltage_resting_estimate, + current_amps : has_curr ? _state.current_amps : AP::logger().quiet_nanf(), + current_total : has_curr ? _state.consumed_mah : AP::logger().quiet_nanf(), + consumed_wh : has_curr ? _state.consumed_wh : AP::logger().quiet_nanf(), + temperature : (int16_t) ( has_temperature() ? _state.temperature * 100 : 0), + resistance : _state.resistance + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + +// Write BCL data packet if has_cell_voltages +void AP_BattMonitor_Backend::Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const +{ + if (!has_cell_voltages()) { + return; + } + + struct log_BCL cell_pkt{ + LOG_PACKET_HEADER_INIT(LOG_BCL_MSG), + time_us : time_us, + instance : instance, + voltage : _state.voltage + }; + for (uint8_t i = 0; i < ARRAY_SIZE(_state.cell_voltages.cells); i++) { + cell_pkt.cell_voltages[i] = _state.cell_voltages.cells[i] + 1; + } + AP::logger().WriteBlock(&cell_pkt, sizeof(cell_pkt)); + + // check battery structure can hold all cells + static_assert(ARRAY_SIZE(_state.cell_voltages.cells) == ARRAY_SIZE(cell_pkt.cell_voltages), + "Battery cell number doesn't match in library and log structure"); +} diff --git a/libraries/AP_BattMonitor/LogStructure.h b/libraries/AP_BattMonitor/LogStructure.h new file mode 100644 index 0000000000..3c3a3ad28f --- /dev/null +++ b/libraries/AP_BattMonitor/LogStructure.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_BATTMONITOR \ + LOG_BAT_MSG, \ + LOG_BCL_MSG + +// @LoggerMessage: BAT +// @Description: Gathered battery data +// @Field: TimeUS: Time since system startup +// @Field: Instance: battery instance number +// @Field: Volt: measured voltage +// @Field: VoltR: estimated resting voltage +// @Field: Curr: measured current +// @Field: CurrTot: current * time +// @Field: EnrgTot: energy this battery has produced +// @Field: Temp: measured temperature +// @Field: Res: estimated battery resistance +struct PACKED log_BAT { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + float voltage; + float voltage_resting; + float current_amps; + float current_total; + float consumed_wh; + int16_t temperature; // degrees C * 100 + float resistance; +}; + +// @LoggerMessage: BCL +// @Description: Battery cell voltage information +// @Field: TimeUS: Time since system startup +// @Field: Instance: battery instance number +// @Field: Volt: battery voltage +// @Field: V1: first cell voltage +// @Field: V2: second cell voltage +// @Field: V3: third cell voltage +// @Field: V4: fourth cell voltage +// @Field: V5: fifth cell voltage +// @Field: V6: sixth cell voltage +// @Field: V7: seventh cell voltage +// @Field: V8: eighth cell voltage +// @Field: V9: ninth cell voltage +// @Field: V10: tenth cell voltage +// @Field: V11: eleventh cell voltage +// @Field: V12: twelfth cell voltage +struct PACKED log_BCL { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + float voltage; + uint16_t cell_voltages[12]; +}; + +#define LOG_STRUCTURE_FROM_BATTMONITOR \ + { LOG_BAT_MSG, sizeof(log_BAT), \ + "BAT", "QBfffffcf", "TimeUS,Instance,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res", "s#vvAiJOw", "F-000!/?0" }, \ + { LOG_BCL_MSG, sizeof(log_BCL), \ + "BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" },