diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-11-12 13:19:07 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-11-12 13:19:07 +0100 |
commit | 5afc64e42427b875ad926862367ebddc9bc0baff (patch) | |
tree | 227a17fb782cae10d5a36d23eebf0788aa498846 /src/modules/commander | |
parent | d7ebea6ec2f3783bfdd4becd7cda5fd24c340a85 (diff) | |
parent | d63c054bb00a0df6666f498950ab797dc07817b4 (diff) | |
download | px4-firmware-5afc64e42427b875ad926862367ebddc9bc0baff.tar.gz px4-firmware-5afc64e42427b875ad926862367ebddc9bc0baff.tar.bz2 px4-firmware-5afc64e42427b875ad926862367ebddc9bc0baff.zip |
Merge pull request #1443 from PX4/fix_strangertl
Fix strange RTL behavior
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 9 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 35 |
2 files changed, 27 insertions, 17 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 149c0f60e..7917bc9f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -758,7 +758,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -1846,7 +1849,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { + mavlink_log_critical(mavlink_fd, "failsafe mode off"); + } failsafe_old = status.failsafe; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9568752ae..e37019d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; @@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_MISSION: + /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if either the datalink is enabled and lost as well as RC is lost - * - if there is no datalink 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) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { @@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->gps_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX - /* Finished handling commands which have priority , now handle failures */ + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || - (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + + /* 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) { @@ -529,12 +534,15 @@ 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 */ - } else if (status->data_link_lost && data_link_loss_enabled) { + /* 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) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost && !stay_in_failsafe) { - - /* this mode is ok, we don't need RC for missions */ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; |