Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: factor out a Log_Write_Current_instance

Browse Source
This commit is contained in:
Peter Barker 2017-09-29 13:09:59 +10:00 committed by Francisco Ferreira
parent 8d3267a731
commit 79ba2b357f
2 changed files with 45 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/DataFlash/DataFlash.h
View File

@ -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);

99
libraries/DataFlash/LogFile.cpp
View File

@ -1526,71 +1526,54 @@ void DataFlash_Class::Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f
WriteBlock(&pkt, sizeof(pkt));
}
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)
{
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)
};
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");
}
}
// 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) {
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)
};
WriteBlock(&pkt, sizeof(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");
}
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);
}
}