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

Checkpoint: Navigation states and arming seem to work

Browse Source
This commit is contained in:
Julian Oes
2013-02-22 15:52:13 -08:00
parent cbfa64b59e
commit 793b482e00
3 changed files with 43 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+23 -20
View File
@@ -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 ((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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
} }
} else { } 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; 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 */ /* request to arm */
if ((int)cmd->param1 == 1) { 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
} }
/* request to disarm */ /* request to disarm */
} else if ((int)cmd->param1 == 0) { } 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; 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 */ /* request for an autopilot reboot */
if ((int)cmd->param1 == 1) { 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 */ /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
result = VEHICLE_CMD_RESULT_ACCEPTED; result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* gyro calibration */ /* gyro calibration */
if ((int)(cmd->param1) == 1) { 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; 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"); mavlink_log_info(mavlink_fd, "finished gyro cal");
tune_confirm(); tune_confirm();
// XXX if this fails, go to ERROR // XXX if this fails, go to ERROR
arming_state_transition(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
@@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* magnetometer calibration */ /* magnetometer calibration */
if ((int)(cmd->param2) == 1) { 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; 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"); mavlink_log_info(mavlink_fd, "finished mag cal");
tune_confirm(); tune_confirm();
// XXX if this fails, go to ERROR // XXX if this fails, go to ERROR
arming_state_transition(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
@@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* accel calibration */ /* accel calibration */
if ((int)(cmd->param5) == 1) { 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; 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"); mavlink_log_info(mavlink_fd, "finished acc cal");
tune_confirm(); tune_confirm();
// XXX what if this fails // XXX what if this fails
arming_state_transition(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
} else { } else {
result = VEHICLE_CMD_RESULT_DENIED; 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 in INIT state, try to proceed to STANDBY state */
if (current_status.arming_state == ARMING_STATE_INIT) { if (current_status.arming_state == ARMING_STATE_INIT) {
//XXX what if this fails // XXX fix for now
arming_state_transition(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); current_status.flag_system_sensors_initialized = true;
arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
} else { } else {
// XXX: Add emergency stuff if sensors are lost // 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 * Check if manual control modes have to be switched
*/ */
if (!isfinite(sp_man.mode_switch)) { if (!isfinite(sp_man.mode_switch)) {
warnx("mode sw not finite");
warnx("mode sw not finite");
/* no valid stick position, go to default */ /* no valid stick position, go to default */
} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, go to manual mode */ /* 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; current_status.mode_switch = MODE_SWITCH_AUTO;
} else { } else {
/* center stick position, set seatbelt/simple control */ /* center stick position, set seatbelt/simple control */
current_status.mode_switch = MODE_SWITCH_SEATBELT; 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, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
// These is not supposed to happen // 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 */ /* Try seatbelt or fallback to manual */
@@ -1879,7 +1880,7 @@ int commander_thread_main(int argc, char *argv[])
// fallback to MANUAL_STANDBY // fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
// These is not supposed to happen // 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 // or fallback to MANUAL_STANDBY
if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
// These is not supposed to happen // 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 ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
arming_state_transition(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY); printf("Try disarm\n");
arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
stick_off_counter = 0; stick_off_counter = 0;
} else { } else {
@@ -2052,7 +2054,8 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position --> arm */ /* 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 (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
arming_state_transition(stat_pub, &current_status, mavlink_fd, ARMING_STATE_ARMED); printf("Try arm\n");
arming_state_transition(stat_pub, &current_status, ARMING_STATE_ARMED, mavlink_fd);
stick_on_counter = 0; stick_on_counter = 0;
} else { } else {
apps/commander/state_machine_helper.c
+19 -6
View File
@@ -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 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; int ret = ERROR;
/* only check transition if the new state is actually different from the current one */ /* 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) { switch (new_arming_state) {
case ARMING_STATE_INIT: case ARMING_STATE_INIT:
@@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
default: default:
break; break;
} }
if (ret == OK) {
current_state->arming_state = new_arming_state;
state_machine_publish(status_pub, current_state, mavlink_fd);
}
} }
return ret; return ret;
} }
@@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
int ret = ERROR; int ret = ERROR;
/* only check transition if the new state is actually different from the current one */ /* 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) { switch (new_navigation_state) {
case NAVIGATION_STATE_INIT: case NAVIGATION_STATE_INIT:
@@ -561,12 +572,14 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
default: default:
break; 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; return ret;
} }
apps/sensors/sensors.cpp
+1 -1
View File
@@ -432,7 +432,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */ /* 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_return_sw = param_find("RC_MAP_RETURN_SW");
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");