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

Fix spurious RTL triggers

Browse Source 
Two sources:
1. global to local conversion was sometimes giving issues, so do everything in global
2. on startup the RTL didn't check if the home position was valid before processing it
This commit is contained in:
Julian Kent
2020-10-13 15:46:41 +02:00
committed by Lorenz Meier
parent b5f0a7ea03
commit af8d178ae5
4 changed files with 62 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+13 -14
View File
@@ -3881,25 +3881,24 @@ void Commander::battery_status_check()
battery_level /= num_connected_batteries;
_rtl_flight_time_sub.update();
float battery_usage_to_home = _rtl_flight_time_sub.valid() ?
_rtl_flight_time_sub.get().rtl_limit_fraction : 0;
float battery_usage_to_home = 0;
if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) {
battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction;
}
auto warning_level = [this](float battery_level_fraction, float battery_to_home) {
float battery_at_home = battery_level_fraction - battery_to_home;
uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE;
if (PX4_ISFINITE(battery_usage_to_home)) {
float battery_at_home = battery_level - battery_usage_to_home;
if (battery_at_home < _param_bat_crit_thr.get()) {
return battery_status_s::BATTERY_WARNING_CRITICAL;
battery_range_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (battery_at_home < _param_bat_low_thr.get()) {
battery_range_warning = battery_status_s::BATTERY_WARNING_LOW;
}
if (battery_at_home < _param_bat_low_thr.get()) {
return battery_status_s::BATTERY_WARNING_LOW;
}
return battery_status_s::BATTERY_WARNING_NONE;
};
uint8_t battery_range_warning = warning_level(battery_level, battery_usage_to_home);
}
if (battery_range_warning > worst_warning) {
worst_warning = battery_range_warning;