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

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

Browse Source
This commit is contained in:
Julian Oes
2014-11-12 09:48:32 +10:00
parent cf2baac516
commit d63c054bb0
1 changed files with 9 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+9 -20
View File
@@ -501,8 +501,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* go into failsafe /* go into failsafe
* - if commanded to do so * - if commanded to do so
* - if we have an engine failure * - if we have an engine failure
* - if the datalink is enabled and lost as well as RC is lost * - depending on datalink, RC and if the mission is finished */
* or if the datalink is disabled and lost as well as RC is lost and the mission is finished */
/* first look at the commands */ /* first look at the commands */
if (status->engine_failure_cmd) { 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) { } else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* check for RC signal and datalink lost (with datalink enabled) */ /* datalink loss enabled:
} else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { * check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { 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; status->nav_state = NAVIGATION_STATE_TERMINATION;
} }
/* check if RC signal is lost (with datalink disabled) after mission is finished */ /* datalink loss disabled:
} else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { * 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; status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) { 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; 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 */ /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){ } else if (!stay_in_failsafe){
status->nav_state = NAVIGATION_STATE_AUTO_MISSION; status->nav_state = NAVIGATION_STATE_AUTO_MISSION;