Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

BATTERY_STATUS: improve filling cell voltage

Browse Source 
according to review comment
This commit is contained in:
Matthias Grob
2021-12-02 15:53:39 +01:00
parent b41e345a5b
commit e8676fe87a
2 changed files with 22 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Limits.hpp
+6
View File
@@ -56,6 +56,12 @@ constexpr _Tp min(_Tp a, _Tp b)
return (a < b) ? a : b; return (a < b) ? a : b;
} }
template<typename _Tp>
constexpr _Tp min(_Tp a, _Tp b, _Tp c)
{
return min(min(a, b), c);
}
template<typename _Tp> template<typename _Tp>
constexpr _Tp max(_Tp a, _Tp b) constexpr _Tp max(_Tp a, _Tp b)
{ {
src/modules/mavlink/streams/BATTERY_STATUS.hpp
+16 -12
View File
@@ -115,8 +115,8 @@ private:
} }
// fill cell voltages // fill cell voltages
static constexpr uint8_t mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); static constexpr int mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
static constexpr uint8_t mavlink_cell_slots_extension static constexpr int mavlink_cell_slots_extension
= (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0])); = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0]));
uint16_t cell_voltages[mavlink_cell_slots + mavlink_cell_slots_extension]; uint16_t cell_voltages[mavlink_cell_slots + mavlink_cell_slots_extension];
@@ -124,28 +124,32 @@ private:
voltage = UINT16_MAX; voltage = 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.connected) {
if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) { // 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
cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f; 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 { } else {
static constexpr uint8_t uorb_cell_slots = static constexpr int uorb_cell_slots =
(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0])); (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++) { const int cell_slots = math::min(static_cast<int>(battery_status.cell_count),
if (battery_status.connected && cell < math::min(battery_status.cell_count, uorb_cell_slots)) { uorb_cell_slots,
mavlink_cell_slots + mavlink_cell_slots_extension);
for (int cell = 0; cell < cell_slots; cell++) {
cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f; cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f;
} }
} }
} }
// voltage fields 1-10 // voltage fields 1-10
for (uint8_t cell = 0; cell < mavlink_cell_slots; cell++) { for (int cell = 0; cell < mavlink_cell_slots; cell++) {
bat_msg.voltages[cell] = cell_voltages[cell]; bat_msg.voltages[cell] = cell_voltages[cell];
} }
// voltage fields 11-14 into the extension // voltage fields 11-14 into the extension
for (uint8_t cell = 0; cell < mavlink_cell_slots_extension; cell++) { for (int cell = 0; cell < mavlink_cell_slots_extension; cell++) {
bat_msg.voltages_ext[cell] = cell_voltages[mavlink_cell_slots + cell]; bat_msg.voltages_ext[cell] = cell_voltages[mavlink_cell_slots + cell];
} }