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

Commander: add functionality for RTL due to GNSS loss

Browse Source 
System switches to RTL if GNSS data is declared invalid but global position is still valid
(e.g. through dead-reckoning). Only in AUTO_MISSION, AUTO_LOITER, MANUAL_POSITION modes.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-07-11 19:25:34 +02:00
parent 466aa4b618
commit 346945b501
3 changed files with 33 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+17 -3
View File
@@ -4257,9 +4257,23 @@ void Commander::estimator_check()
// report GPS failure if flying and the global position estimate is still valid
if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid) {
mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid\t");
events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info},
"GPS no longer valid");
if (_param_com_gnss_ivd_rtl.get() && (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
|| _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION
|| _commander_state.main_state == commander_state_s::MAIN_STATE_POSCTL)) {
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
_status_changed = true;
mavlink_log_critical(&_mavlink_log_pub, "GPS no longer valid, returning\t");
events::send(events::ID("commander_gps_lost_rtl"), {events::Log::Critical, events::LogInternal::Info},
"GPS no longer valid, returning");
} else {
mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid\t");
events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info},
"GPS no longer valid");
}
}
} else if (!condition_gps_position_was_valid && _vehicle_status_flags.gps_position_valid) {
src/modules/commander/Commander.hpp
+3 -1
View File
@@ -260,7 +260,9 @@ private:
(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
(ParamBool<px4::params::COM_GNSS_IVD_RTL>) _param_com_gnss_ivd_rtl
)
// optional parameters
src/modules/commander/commander_params.c
+13
View File
@@ -1090,3 +1090,16 @@ PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);
* @unit m/s
*/
PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
/**
* Trigger RTL when GNSS gets invalid.
*
* Even after the loss of GNSS sensoring capabilities on the vehicle, its position can still be estimated
* by relying on the remaining sensor data. As the quality of the estimate in that case though deteriorates
* with time, it is recommended to start returning to launch as soon as the GNSS failure is detected. By setting
* this parameter to true, this is done automatically for Mission, Loiter and Position flight mode.
*
* @group Commander
* @boolean
*/
PARAM_DEFINE_INT32(COM_GNSS_IVD_RTL, 1);