From cbfa64b59eef8362494d0753ce3567e804f2d682 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 11:54:41 -0800 Subject: Checkpoint: Added switch handling --- apps/commander/commander.c | 162 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 160 insertions(+), 2 deletions(-) (limited to 'apps/commander/commander.c') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 953ba7a1e..38d2ffc96 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1725,7 +1725,7 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + if (orb_check(gps_sub, &new_data)) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); @@ -1858,7 +1858,165 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - #warning do that + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ -- cgit v1.2.3