fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 18:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: switch battery bitflied to dedicated datatype

Browse Source
This commit is contained in:
Matthias Grob
2022-06-08 11:32:26 +02:00
parent 9b2166de72
commit f5e7b1e6a8
2 changed files with 6 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+4 -10
View File
@@ -3806,7 +3806,7 @@ void Commander::avoidance_check()
void Commander::battery_status_check() void Commander::battery_status_check()
{ {
int battery_required_count{0}; size_t battery_required_count = 0;
bool battery_has_fault = false; bool battery_has_fault = false;
// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
// option is to check if ANY of them have a warning, and specifically find which one has the most // option is to check if ANY of them have a warning, and specifically find which one has the most
@@ -3831,7 +3831,7 @@ void Commander::battery_status_check()
if (_arm_state_machine.isArmed()) { if (_arm_state_machine.isArmed()) {
if ((_last_connected_batteries & (1 << index)) && !battery.connected) { if (_last_connected_batteries[index] && !battery.connected) {
mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1); mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning}, events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning},
"Battery {1} disconnected. Land now!", index + 1); "Battery {1} disconnected. Land now!", index + 1);
@@ -3848,13 +3848,7 @@ void Commander::battery_status_check()
} }
} }
if (battery.connected) { _last_connected_batteries.set(index, battery.connected);
_last_connected_batteries |= 1 << index;
} else {
_last_connected_batteries &= ~(1 << index);
}
_last_battery_mode[index] = battery.mode; _last_battery_mode[index] = battery.mode;
if (battery.connected) { if (battery.connected) {
@@ -3959,7 +3953,7 @@ void Commander::battery_status_check()
// All connected batteries are regularly being published // All connected batteries are regularly being published
(hrt_elapsed_time(&oldest_update) < 5_s) (hrt_elapsed_time(&oldest_update) < 5_s)
// There is at least one connected battery (in any slot) // There is at least one connected battery (in any slot)
&& (math::countSetBits(_last_connected_batteries) >= battery_required_count) && (_last_connected_batteries.count() >= battery_required_count)
// No currently-connected batteries have any warning // No currently-connected batteries have any warning
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE) && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)
// No currently-connected batteries have any fault // No currently-connected batteries have any fault
src/modules/commander/Commander.hpp
+2 -1
View File
@@ -41,6 +41,7 @@
#include "state_machine_helper.h" #include "state_machine_helper.h"
#include "worker_thread.hpp" #include "worker_thread.hpp"
#include <containers/Bitset.hpp>
#include <lib/controllib/blocks.hpp> #include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h> #include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
@@ -347,7 +348,7 @@ private:
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _battery_failsafe_timestamp{0}; hrt_abstime _battery_failsafe_timestamp{0};
uint8_t _last_connected_batteries{0}; px4::Bitset<battery_status_s::MAX_INSTANCES> _last_connected_batteries;
uint32_t _last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {}; uint32_t _last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
uint16_t _last_battery_fault[battery_status_s::MAX_INSTANCES] {}; uint16_t _last_battery_fault[battery_status_s::MAX_INSTANCES] {};
uint8_t _last_battery_mode[battery_status_s::MAX_INSTANCES] {}; uint8_t _last_battery_mode[battery_status_s::MAX_INSTANCES] {};