diff options
author | Julian Oes <julian@oes.ch> | 2014-11-12 09:48:32 +1000 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-11-12 09:48:32 +1000 |
commit | d63c054bb00a0df6666f498950ab797dc07817b4 (patch) | |
tree | c08c12b6f55cd76d0f6110681ae78cee28d643cc /src/modules | |
parent | cf2baac516b6f86daf30342be47264b2dcc8abc4 (diff) | |
download | px4-firmware-d63c054bb00a0df6666f498950ab797dc07817b4.tar.gz px4-firmware-d63c054bb00a0df6666f498950ab797dc07817b4.tar.bz2 px4-firmware-d63c054bb00a0df6666f498950ab797dc07817b4.zip |
state_machine_helper: changed failsafe behaviour: always require at least one link for default, do RTGS as soon as datalink is lost if datalink loss mode is enabled
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 29 |
1 files changed, 9 insertions, 20 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a9a5c4051..e37019d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -501,8 +501,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if the datalink is enabled and lost as well as RC is lost - * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + * - depending on datalink, RC and if the mission is finished */ /* first look at the commands */ if (status->engine_failure_cmd) { @@ -520,8 +519,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) */ - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -534,8 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with datalink disabled) after mission is finished */ - } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -548,20 +551,6 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost (with datalink enabled) */ - } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; |