Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 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;
}
template<typename _Tp>
constexpr _Tp min(_Tp a, _Tp b, _Tp c)
{
return min(min(a, b), c);
}
template<typename _Tp>
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
static constexpr uint8_t 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 = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
static constexpr int 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];
@@ -124,28 +124,32 @@ private:
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.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) {
cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f;
if (battery_status.connected) {
// 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]));
} else {
static constexpr int 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)) {
const int cell_slots = math::min(static_cast<int>(battery_status.cell_count),
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;
}
}
}
// 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];
}
// 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];
}