dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

battery: move MAVLink specific handling out of battery class

This commit is contained in:
Matthias Grob 2021-11-24 17:45:37 +01:00
parent da2fdf923e
commit ddc6b6bc9c
3 changed files with 30 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/battery_status.msg
View File

@ -10,7 +10,7 @@ float32 remaining # From 1 to 0, -1 if unknown
float32 scale # Power scaling factor, >= 1, or -1 if unknown
float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
float32 temperature # temperature of the battery. NaN if unknown
int32 cell_count # Number of cells
uint8 cell_count # Number of cells, 0 if unknown
uint8 BATTERY_SOURCE_POWER_MODULE = 0
uint8 BATTERY_SOURCE_EXTERNAL = 1
@ -27,7 +27,7 @@ uint16 max_error # max error, expected margin of error in % in the state-of-ch
uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
uint16 interface_error # interface error counter
float32[14] voltage_cell_v # Battery individual cell voltages
float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown
float32 max_cell_voltage_delta # Max difference between individual cell voltages
bool is_powering_off # Power off event imminent indication, false if unknown

13
src/lib/battery/battery.cpp
View File

@ -134,8 +134,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
battery_status.scale = _scale;
battery_status.time_remaining_s = computeRemainingTime(current_a);
battery_status.temperature = NAN;
// Publish at least one cell such that the total voltage gets into MAVLink BATTERY_STATUS
battery_status.cell_count = math::max(_params.n_cells, static_cast<int32_t>(1));
battery_status.cell_count = _params.n_cells;
battery_status.connected = connected;
battery_status.source = source;
battery_status.priority = priority;
@ -143,16 +142,6 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
battery_status.id = static_cast<uint8_t>(_index);
battery_status.warning = _warning;
static constexpr int32_t uorb_max_cells = sizeof(battery_status.voltage_cell_v) / sizeof(
battery_status.voltage_cell_v[0]);
int max_cells = math::min(battery_status.cell_count, uorb_max_cells);
// Fill cell voltages with average values to work around MAVLink BATTERY_STATUS not allowing to report just total voltage
for (int i = 0; i < max_cells; i++) {
battery_status.voltage_cell_v[i] = battery_status.voltage_filtered_v / max_cells;
}
if (source == _params.source) {
battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(battery_status);

43
src/modules/mavlink/streams/BATTERY_STATUS.hpp
View File

@ -114,28 +114,39 @@ private:
bat_msg.temperature = INT16_MAX;
}
static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
static constexpr int uorb_cells_max =
(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0]));
// fill cell voltages
static constexpr uint8_t mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
static constexpr uint8_t mavlink_cell_slots_extension
= (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0]));
uint16_t cell_voltages[mavlink_cell_slots + mavlink_cell_slots_extension];
for (int cell = 0; cell < mavlink_cells_max; cell++) {
if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) {
bat_msg.voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f;
for (auto &voltage : cell_voltages) {
voltage = UINT16_MAX;
}
} else {
bat_msg.voltages[cell] = UINT16_MAX;
// We don't know the cell count or we don't know the indpendent cell voltages so we report the total voltage in the first cell
if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) {
cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f;
} else {
static constexpr uint8_t uorb_cell_slots =
(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0]));
for (uint8_t cell = 0; cell < mavlink_cell_slots + mavlink_cell_slots_extension; cell++) {
if (battery_status.connected && cell < math::min(battery_status.cell_count, uorb_cell_slots)) {
cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f;
}
}
}
static constexpr int mavlink_cells_ext_max = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0]));
// voltage fields 1-10
for (uint8_t cell = 0; cell < mavlink_cell_slots; cell++) {
bat_msg.voltages[cell] = cell_voltages[cell];
}
for (int cell = mavlink_cells_max; cell < mavlink_cells_max + mavlink_cells_ext_max; cell++) {
if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) {
bat_msg.voltages_ext[cell - mavlink_cells_max] = battery_status.voltage_cell_v[cell] * 1000.0f;
} else {
bat_msg.voltages_ext[cell - mavlink_cells_max] = UINT16_MAX;
}
// voltage fields 11-14 into the extension
for (uint8_t cell = 0; cell < mavlink_cell_slots_extension; cell++) {
bat_msg.voltages_ext[cell] = cell_voltages[mavlink_cell_slots + cell];
}
mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);