fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Orbit failsafe: switch internal_state to Posctrl such that Manual-Positionctrl is activated after failsafe recovery. This is needed because Orbit can only be invoked via vehicle_command msg.

Browse Source
This commit is contained in:
Dennis Mannhart 2019-08-15 08:22:25 +02:00 committed by Mathieu Bresciani
parent 82ecf4d942
commit bf25c462b2
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/commander/state_machine_helper.cpp
View File

@ -593,8 +593,17 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
// failsafe: on engine failure
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
// Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// failsafe: necessary position estimate lost (nothing to do - everything done in check_invalid_pos_nav_state)
// failsafe: necessary position estimate lost; witching is done in check_invalid_pos_nav_state
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// failsafe: just datalink is lost and we're in air
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act,
@ -602,6 +611,10 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (rc_lost && !data_link_loss_act_configured && is_armed) {
// failsafe: RC is lost, datalink loss is not set up and rc loss is not disabled
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
@ -609,6 +622,10 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act,
vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
} else {
// no failsafe, RC is not mandatory for orbit
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;