From 793b482e00013ea66bb1b0cdc0366bb720648938 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 15:52:13 -0800 Subject: Checkpoint: Navigation states and arming seem to work --- apps/commander/commander.c | 43 +++++++++++++++++++---------------- apps/commander/state_machine_helper.c | 25 +++++++++++++++----- apps/sensors/sensors.cpp | 2 +- 3 files changed, 43 insertions(+), 27 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 38d2ffc96..2784e77db 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -903,7 +903,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -925,7 +925,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -996,7 +996,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,8 +1647,9 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - //XXX what if this fails - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX fix for now + current_status.flag_system_sensors_initialized = true; + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1790,11 +1791,10 @@ int commander_thread_main(int argc, char *argv[]) * Check if manual control modes have to be switched */ if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + warnx("mode sw not finite"); /* no valid stick position, go to default */ - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ @@ -1806,6 +1806,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.mode_switch = MODE_SWITCH_AUTO; } else { + /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; } @@ -1869,7 +1870,7 @@ int commander_thread_main(int argc, char *argv[]) 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"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); } /* Try seatbelt or fallback to manual */ @@ -1879,7 +1880,7 @@ int commander_thread_main(int argc, char *argv[]) // 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"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); } } @@ -1892,7 +1893,7 @@ int commander_thread_main(int argc, char *argv[]) // 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"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); } } } @@ -2040,7 +2041,8 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + printf("Try disarm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; } else { @@ -2052,7 +2054,8 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + printf("Try arm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 6c15bd725..ae7f2a1c1 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -255,10 +255,13 @@ //} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_state->arming_state) { + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { switch (new_arming_state) { case ARMING_STATE_INIT: @@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta default: break; } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + return ret; } @@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_state->navigation_state) { + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: @@ -561,13 +572,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current default: break; } - } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + + return ret; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ce404ee7e..b53de8c46 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -432,7 +432,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); -- cgit v1.2.3