fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 12:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed all swapping of uORB instances

Browse Source
This commit is contained in:
Timothy Scott
2020-02-07 11:48:56 +01:00
committed by Lorenz Meier
parent 6452b7e014
commit 4c1adc088f
5 changed files with 77 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+70 -71
View File
@@ -3717,7 +3717,7 @@ void Commander::battery_status_check()
size_t num_connected_batteries = 0;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
for (size_t i = 0; i < sizeof(_battery_subs) / sizeof(_battery_subs[0]); i++) {
if (_battery_subs[i].updated() && _battery_subs[i].copy(&batteries[num_connected_batteries])) {
// We need to update the status flag if ANY battery is updated, because the system source might have
// changed, or might be nothing (if there is no battery connected)
@@ -3729,84 +3729,83 @@ void Commander::battery_status_check()
}
}
/* update battery status */
if (battery_sub_updated) {
if (!battery_sub_updated) {
// Nothing has changed since the last time this function was called, so nothing needs to be done now.
return;
}
// 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
// urgent warning.
uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
// Sum the total current of all connected batteries. This is used to detect engine failure.
float total_current = 0;
// To make sure that all connected batteries are being regularly reported, we check which one has the
// oldest timestamp.
hrt_abstime oldest_update = hrt_absolute_time();
// 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
// urgent warning.
uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
// To make sure that all connected batteries are being regularly reported, we check which one has the
// oldest timestamp.
hrt_abstime oldest_update = hrt_absolute_time();
// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
for (size_t i = 0; i < num_connected_batteries; i++) {
if (batteries[i].warning > worst_warning) {
worst_warning = batteries[i].warning;
}
if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) {
oldest_update = batteries[i].timestamp;
}
total_current += batteries[i].current_filtered_a;
// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
for (size_t i = 0; i < num_connected_batteries; i++) {
if (batteries[i].warning > worst_warning) {
worst_warning = batteries[i].warning;
}
bool battery_warning_level_increased_while_armed = false;
bool update_internal_battery_state = false;
if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) {
oldest_update = batteries[i].timestamp;
}
if (armed.armed) {
if (worst_warning > _battery_warning) {
battery_warning_level_increased_while_armed = true;
update_internal_battery_state = true;
}
if (batteries[i].system_source) {
_battery_current = batteries[i].current_filtered_a;
}
}
} else {
if (_battery_warning != worst_warning) {
update_internal_battery_state = true;
bool battery_warning_level_increased_while_armed = false;
bool update_internal_battery_state = false;
if (armed.armed) {
if (worst_warning > _battery_warning) {
battery_warning_level_increased_while_armed = true;
update_internal_battery_state = true;
}
} else {
if (_battery_warning != worst_warning) {
update_internal_battery_state = true;
}
}
if (update_internal_battery_state) {
_battery_warning = worst_warning;
}
status_flags.condition_battery_healthy =
// All connected batteries are regularly being published
(hrt_elapsed_time(&oldest_update) < 5_s)
// There is at least one connected battery (in any slot)
&& num_connected_batteries > 0
// No currently-connected batteries have any warning
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
// execute battery failsafe if the state has gotten worse while we are armed
if (battery_warning_level_increased_while_armed) {
battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,
(low_battery_action_t)_param_com_low_bat_act.get());
}
// Handle shutdown request from emergency battery action
if (update_internal_battery_state) {
if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) {
mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
px4_usleep(200000);
int ret_val = px4_shutdown_request(false, false);
if (ret_val) {
mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
} else {
while (1) { px4_usleep(1); }
}
}
if (update_internal_battery_state) {
_battery_warning = worst_warning;
}
status_flags.condition_battery_healthy =
// All connected batteries are regularly being published
(hrt_elapsed_time(&oldest_update) < 5_s)
// There is at least one connected battery (in any slot)
&& num_connected_batteries > 0
// No currently-connected batteries have any warning
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
// execute battery failsafe if the state has gotten worse while we are armed
if (battery_warning_level_increased_while_armed) {
battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,
(low_battery_action_t)_param_com_low_bat_act.get());
}
// Handle shutdown request from emergency battery action
if (update_internal_battery_state) {
if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) {
mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
px4_usleep(200000);
int ret_val = px4_shutdown_request(false, false);
if (ret_val) {
mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
} else {
while (1) { px4_usleep(1); }
}
}
}
_battery_current = total_current;
}
}