From 79ba2b357f24f9df6eb7ea661d3e668088bde3d4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 29 Sep 2017 13:09:59 +1000 Subject: [PATCH] DataFlash: factor out a Log_Write_Current_instance --- libraries/DataFlash/DataFlash.h | 4 ++ libraries/DataFlash/LogFile.cpp | 99 ++++++++++++++------------------- 2 files changed, 45 insertions(+), 58 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index f3fe82242a..08be49b70f 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -282,6 +282,10 @@ private: uint64_t time_us, uint8_t mag_instance, enum LogMessages type); + void Log_Write_Current_instance(const AP_BattMonitor &battery, + uint64_t time_us, + uint8_t battery_instance, + enum LogMessages type); void backend_starting_new_log(const DataFlash_Backend *backend); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index de39f54511..5a4a7dd526 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1526,71 +1526,54 @@ void DataFlash_Class::Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f WriteBlock(&pkt, sizeof(pkt)); } -// Write an Current data packet -void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery) +void DataFlash_Class::Log_Write_Current_instance(const AP_BattMonitor &battery, + const uint64_t time_us, + const uint8_t battery_instance, + const enum LogMessages type) { - if (battery.num_instances() >= 1) { - float temp; - bool has_temp = battery.get_temperature(temp, 0); - struct log_Current pkt = { - LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), - time_us : AP_HAL::micros64(), - voltage : battery.voltage(0), - voltage_resting : battery.voltage_resting_estimate(0), - current_amps : battery.current_amps(0), - current_total : battery.current_total_mah(0), - temperature : (int16_t)(has_temp ? (temp * 100) : 0), - resistance : battery.get_resistance(0) + float temp; + bool has_temp = battery.get_temperature(temp, battery_instance); + struct log_Current pkt = { + LOG_PACKET_HEADER_INIT(type), + time_us : time_us, + voltage : battery.voltage(battery_instance), + voltage_resting : battery.voltage_resting_estimate(battery_instance), + current_amps : battery.current_amps(battery_instance), + current_total : battery.current_total_mah(battery_instance), + temperature : (int16_t)(has_temp ? (temp * 100) : 0), + resistance : battery.get_resistance(battery_instance) + }; + WriteBlock(&pkt, sizeof(pkt)); + + // individual cell voltages + if (battery.has_cell_voltages(battery_instance)) { + const AP_BattMonitor::cells &cells = battery.get_cell_voltages(battery_instance); + struct log_Current_Cells cell_pkt = { + LOG_PACKET_HEADER_INIT(LOG_CURRENT_CELLS_MSG), + time_us : time_us, + voltage : battery.voltage(battery_instance) }; - WriteBlock(&pkt, sizeof(pkt)); + for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) { + cell_pkt.cell_voltages[i] = cells.cells[i] + 1; + } + WriteBlock(&cell_pkt, sizeof(cell_pkt)); - // individual cell voltages - if (battery.has_cell_voltages(0)) { - const AP_BattMonitor::cells &cells = battery.get_cell_voltages(0); - struct log_Current_Cells cell_pkt = { - LOG_PACKET_HEADER_INIT(LOG_CURRENT_CELLS_MSG), - time_us : AP_HAL::micros64(), - voltage : battery.voltage(0) - }; - for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) { - cell_pkt.cell_voltages[i] = cells.cells[i] + 1; - } - WriteBlock(&cell_pkt, sizeof(cell_pkt)); + // check battery structure can hold all cells + static_assert(ARRAY_SIZE(cells.cells) == (sizeof(cell_pkt.cell_voltages) / sizeof(cell_pkt.cell_voltages[0])), + "Battery cell number doesn't match in library and log structure"); + } +} - // check battery structure can hold all cells - static_assert(ARRAY_SIZE(cells.cells) == (sizeof(cell_pkt.cell_voltages) / sizeof(cell_pkt.cell_voltages[0])), - "Battery cell number doesn't match in library and log structure"); - } +// Write an Current data packet +void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery) +{ + const uint64_t time_us = AP_HAL::micros64(); + if (battery.num_instances() >= 1) { + Log_Write_Current_instance(battery, time_us, 0, LOG_CURRENT_MSG); } if (battery.num_instances() >= 2) { - float temp; - bool has_temp = battery.get_temperature(temp, 1); - struct log_Current pkt = { - LOG_PACKET_HEADER_INIT(LOG_CURRENT2_MSG), - time_us : AP_HAL::micros64(), - voltage : battery.voltage(1), - voltage_resting : battery.voltage_resting_estimate(1), - current_amps : battery.current_amps(1), - current_total : battery.current_total_mah(1), - temperature : (int16_t)(has_temp ? (temp * 100) : 0), - resistance : battery.get_resistance(1) - }; - WriteBlock(&pkt, sizeof(pkt)); - - // individual cell voltages - if (battery.has_cell_voltages(1)) { - const AP_BattMonitor::cells &cells = battery.get_cell_voltages(1); - struct log_Current_Cells cell_pkt = { - LOG_PACKET_HEADER_INIT(LOG_CURRENT_CELLS_MSG), - time_us : AP_HAL::micros64(), - voltage : battery.voltage(1) - }; - for (uint8_t i = 0; i < ARRAY_SIZE(cells.cells); i++) { - cell_pkt.cell_voltages[i] = cells.cells[i] + 1; - } - WriteBlock(&cell_pkt, sizeof(cell_pkt)); - } + Log_Write_Current_instance(battery, time_us, 1, LOG_CURRENT2_MSG); } }