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

Navigator/Commander: make GPS failsafe consitent: switch to Descend also for FW and VTOL

Browse Source 
- remove GPS failsafe mode
- for VTOL: transition to hover in Descend (unless NAV_FORCE_VT is not set)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-05 12:09:39 +03:00
committed by Roman Bapst
parent b77487d69c
commit f02786d112
15 changed files with 59 additions and 459 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+18 -11
View File
@@ -83,7 +83,6 @@ Navigator::Navigator() :
_precland(this),
_rtl(this),
_engineFailure(this),
_gpsFailure(this),
_follow_target(this)
{
/* Create a list of our possible navigation types */
@@ -91,11 +90,10 @@ Navigator::Navigator() :
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_engineFailure;
_navigation_mode_array[4] = &_gpsFailure;
_navigation_mode_array[5] = &_takeoff;
_navigation_mode_array[6] = &_land;
_navigation_mode_array[7] = &_precland;
_navigation_mode_array[8] = &_follow_target;
_navigation_mode_array[4] = &_takeoff;
_navigation_mode_array[5] = &_land;
_navigation_mode_array[6] = &_precland;
_navigation_mode_array[7] = &_follow_target;
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -706,11 +704,6 @@ Navigator::run()
navigation_mode_new = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_follow_target;
@@ -750,6 +743,20 @@ Navigator::run()
navigation_mode_new == &_loiter)) {
reset_triplets();
}
// transition to hover in Descend mode
if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND &&
_vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
force_vtol()) {
vehicle_command_s vcmd = {};
vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
publish_vehicle_cmd(&vcmd);
mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t");
events::send(events::ID("navigator_transition_descend"), events::Log::Critical,
"Transition to hover mode and descend");
}
}
_navigation_mode = navigation_mode_new;