fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 13:04:06 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
apps/commander/commander.c
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 (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, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
arming_state_transition(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
arming_state_transition(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
arming_state_transition(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// XXX fix for now
current_status.flag_system_sensors_initialized = true;
arming_state_transition(stat_pub, &current_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, &current_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, &current_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, &current_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, &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;
} 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, &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;
} else {

25
apps/commander/state_machine_helper.c
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 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,12 +572,14 @@ 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;
}

2
apps/sensors/sensors.cpp
View File

@ -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");