fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 19:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Checkpoint, arming/disarming still has a bug

Browse Source
This commit is contained in:
Julian Oes 2013-02-17 23:07:07 -08:00
parent 3bc385c789
commit 47b05eeb87
5 changed files with 303 additions and 243 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
apps/commander/commander.c
View File

@ -817,22 +817,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
/* request to arm */
if ((int)cmd->param1 == 1) {
if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
// result = VEHICLE_CMD_RESULT_ACCEPTED;
//
// } else {
// result = VEHICLE_CMD_RESULT_DENIED;
// }
/* request to disarm */
// XXX this arms it instad of disarming
} else if ((int)cmd->param1 == 0) {
if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
// result = VEHICLE_CMD_RESULT_ACCEPTED;
//
// } else {
// result = VEHICLE_CMD_RESULT_DENIED;
// }
}
}
break;
@ -840,7 +842,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* request for an autopilot reboot */
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
if ((int)cmd->param1 == 1) {
if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) {
if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) {
/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
result = VEHICLE_CMD_RESULT_ACCEPTED;
@ -883,25 +885,26 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if ((int)(cmd->param1) == 1) {
/* transition to init state */
if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
&& OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
mavlink_log_info(mavlink_fd, "starting gyro cal");
tune_confirm();
do_gyro_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished gyro cal");
tune_confirm();
/* back to standby state */
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
//
// mavlink_log_info(mavlink_fd, "starting gyro cal");
// tune_confirm();
// do_gyro_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished gyro cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
//
// result = VEHICLE_CMD_RESULT_ACCEPTED;
//
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
// result = VEHICLE_CMD_RESULT_DENIED;
// }
handled = true;
}
@ -910,24 +913,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if ((int)(cmd->param2) == 1) {
/* transition to init state */
if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
&& OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
mavlink_log_info(mavlink_fd, "starting mag cal");
tune_confirm();
do_mag_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished mag cal");
tune_confirm();
/* back to standby state */
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
// mavlink_log_info(mavlink_fd, "starting mag cal");
// tune_confirm();
// do_mag_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished mag cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
//
// result = VEHICLE_CMD_RESULT_ACCEPTED;
//
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
// result = VEHICLE_CMD_RESULT_DENIED;
// }
handled = true;
}
@ -935,21 +939,22 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* zero-altitude pressure calibration */
if ((int)(cmd->param3) == 1) {
/* transition to calibration state */
if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
&& OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
// XXX implement this
mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
tune_confirm();
/* back to standby state */
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
} else {
mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
//
// // XXX implement this
// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
//
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
// result = VEHICLE_CMD_RESULT_DENIED;
// }
handled = true;
}
@ -957,49 +962,51 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* trim calibration */
if ((int)(cmd->param4) == 1) {
/* transition to calibration state */
if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
&& OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
mavlink_log_info(mavlink_fd, "starting trim cal");
tune_confirm();
do_rc_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished trim cal");
tune_confirm();
/* back to standby state */
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
// mavlink_log_info(mavlink_fd, "starting trim cal");
// tune_confirm();
// do_rc_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished trim cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
//
// result = VEHICLE_CMD_RESULT_ACCEPTED;
//
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
// result = VEHICLE_CMD_RESULT_DENIED;
// }
handled = true;
}
/* accel calibration */
if ((int)(cmd->param5) == 1) {
if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
&& OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
mavlink_log_info(mavlink_fd, "CMD starting accel cal");
tune_confirm();
do_accel_calibration(status_pub, &current_status);
tune_confirm();
mavlink_log_info(mavlink_fd, "CMD finished accel cal");
/* back to standby state */
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
//
// mavlink_log_info(mavlink_fd, "CMD starting accel cal");
// tune_confirm();
// do_accel_calibration(status_pub, &current_status);
// tune_confirm();
// mavlink_log_info(mavlink_fd, "CMD finished accel cal");
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
//
// result = VEHICLE_CMD_RESULT_ACCEPTED;
//
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
// result = VEHICLE_CMD_RESULT_DENIED;
// }
handled = true;
}
@ -1315,7 +1322,7 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.navigation_state = NAVIGATION_STATE_INIT;
current_status.navigation_state = NAVIGATION_STATE_STANDBY;
current_status.arming_state = ARMING_STATE_INIT;
current_status.flag_system_armed = false;
@ -1336,6 +1343,9 @@ int commander_thread_main(int argc, char *argv[])
/* set battery warning flag */
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
// XXX for now just set sensors as initialized
current_status.flag_system_sensors_initialized = true;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
/* publish current state machine */
@ -1886,8 +1896,8 @@ int commander_thread_main(int argc, char *argv[])
((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
current_status.flag_system_armed = false;
stick_on_counter = 0;
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
stick_off_counter = 0;
} else {
stick_off_counter++;
@ -1898,7 +1908,7 @@ 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) {
current_status.flag_system_armed = true;
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_ARMED);
stick_on_counter = 0;
} else {
@ -2054,9 +2064,6 @@ int commander_thread_main(int argc, char *argv[])
state_changed = false;
}
/* make changes in state machine if needed */
update_state_machine(stat_pub, &current_status, mavlink_fd);
/* Store old modes to detect and act on state transitions */

270
apps/commander/state_machine_helper.c
View File

@ -53,33 +53,12 @@
#include "state_machine_helper.h"
void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
/* check arming first */
if (current_status->flag_system_armed && current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
} else if(current_status->flag_system_armed && !current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED);
} else if(!current_status->flag_system_armed && current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
} else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else if (current_status->flag_system_sensors_ok) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
} else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
}
/* now determine the navigation state */
}
/**
* Transition from one navigation state to another
*/
int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state)
{
bool valid_path = false;
bool valid_transition = false;
int ret = ERROR;
@ -89,41 +68,43 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
} else {
switch (new_state) {
case NAVIGATION_STATE_INIT:
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE");
valid_transition = true;
}
break;
case NAVIGATION_STATE_STANDBY:
if (current_status->navigation_state == NAVIGATION_STATE_INIT
|| current_status->navigation_state == NAVIGATION_STATE_MANUAL
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE");
valid_transition = true;
if (!current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed");
}
valid_path = true;
}
break;
case NAVIGATION_STATE_MANUAL:
if (
( current_status->navigation_state == NAVIGATION_STATE_STANDBY
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION
|| current_status->navigation_state == NAVIGATION_STATE_RTL
|| current_status->navigation_state == NAVIGATION_STATE_LAND
|| current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY)
&& current_status->arming_state == ARMING_STATE_ARMED) {
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE");
/* only check for armed flag when coming from STANDBY XXX does that make sense? */
if (current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed");
}
valid_path = true;
} else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION
|| current_status->navigation_state == NAVIGATION_STATE_RTL
|| current_status->navigation_state == NAVIGATION_STATE_LAND
|| current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
valid_transition = true;
valid_path = true;
}
break;
@ -133,67 +114,132 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE");
mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_LOITER:
if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT)
&& current_status->flag_global_position_valid)
|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE");
/* Check for position lock when coming from MANUAL or SEATBELT */
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
valid_transition = true;
}
if (current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock");
}
valid_path = true;
} else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_AUTO_READY:
if (
(current_status->navigation_state == NAVIGATION_STATE_STANDBY
&& current_status->flag_global_position_valid
&& current_status->flag_valid_launch_position)
|| current_status->navigation_state == NAVIGATION_STATE_LAND) {
/* coming from STANDBY pos and home lock are needed */
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE");
valid_transition = true;
if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
valid_transition = true;
} else if (!current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock");
} else if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
} else {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock");
}
valid_path = true;
/* coming from LAND home lock is needed */
} else if (current_status->navigation_state == NAVIGATION_STATE_LAND) {
if (current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
}
valid_path = true;
}
break;
case NAVIGATION_STATE_MISSION:
if (
current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
|| current_status->navigation_state == NAVIGATION_STATE_RTL
|| (
(current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT
|| current_status->navigation_state == NAVIGATION_STATE_LOITER)
&& current_status->flag_global_position_valid
&& current_status->flag_valid_launch_position)
) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE");
/* coming from TAKEOFF or RTL is always possible */
if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
|| current_status->navigation_state == NAVIGATION_STATE_RTL) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
valid_transition = true;
valid_path = true;
/* coming from MANUAL or SEATBELT requires home and pos lock */
} else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
valid_transition = true;
} else if (!current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock");
} else if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
} else {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock");
}
valid_path = true;
/* coming from loiter a home lock is needed */
} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
if (current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
}
valid_path = true;
}
break;
case NAVIGATION_STATE_RTL:
if (
current_status->navigation_state == NAVIGATION_STATE_MISSION
|| (
(current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT
|| current_status->navigation_state == NAVIGATION_STATE_LOITER)
&& current_status->flag_global_position_valid
&& current_status->flag_valid_launch_position)
) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE");
/* coming from MISSION is always possible */
if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "Switched to RTL state");
valid_transition = true;
valid_path = true;
/* coming from MANUAL or SEATBELT requires home and pos lock */
} else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to RTL state");
valid_transition = true;
} else if (!current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock");
} else if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
} else {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock");
}
valid_path = true;
/* coming from loiter a home lock is needed */
} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
if (current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to RTL state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
}
valid_path = true;
}
break;
@ -201,8 +247,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE");
/* TAKEOFF is straight forward from AUTO READY */
mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state");
valid_transition = true;
valid_path = true;
}
break;
@ -210,29 +258,12 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
if (current_status->navigation_state == NAVIGATION_STATE_RTL
|| current_status->navigation_state == NAVIGATION_STATE_LOITER) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE");
mavlink_log_critical(mavlink_fd, "Switched to LAND state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_REBOOT:
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY
|| current_status->navigation_state == NAVIGATION_STATE_INIT
|| current_status->flag_hil_enabled) {
valid_transition = true;
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
}
break;
default:
warnx("Unknown navigation state");
break;
@ -244,7 +275,9 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_
state_machine_publish(status_pub, current_status, mavlink_fd);
// publish_armed_status(current_status);
ret = OK;
} else {
}
if (!valid_path){
mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition");
}
@ -269,7 +302,8 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
case ARMING_STATE_INIT:
if (current_status->arming_state == ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE");
mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE");
valid_transition = true;
}
break;
@ -280,8 +314,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
// XXX check if coming from reboot?
if (current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE");
valid_transition = true;
if (current_status->flag_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
}
}
break;
@ -290,16 +328,16 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE");
mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state");
valid_transition = true;
}
break;
case ARMING_STATE_MISSION_ABORT:
case ARMING_STATE_ABORT:
if (current_status->arming_state == ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE");
mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state");
valid_transition = true;
}
break;
@ -307,10 +345,10 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
case ARMING_STATE_ERROR:
if (current_status->arming_state == ARMING_STATE_ARMED
|| current_status->arming_state == ARMING_STATE_MISSION_ABORT
|| current_status->arming_state == ARMING_STATE_ABORT
|| current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE");
mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state");
valid_transition = true;
}
break;
@ -320,16 +358,18 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_ERROR) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE");
valid_transition = true;
// XXX reboot here?
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
if (current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE");
mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state");
valid_transition = true;
}
break;

2
apps/commander/state_machine_helper.h
View File

@ -47,8 +47,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);

15
apps/mavlink/mavlink.c
View File

@ -248,26 +248,19 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.flag_system_emergency) {
} else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) {
*mavlink_state = MAV_STATE_EMERGENCY;
} else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_RTL
|| v_status.navigation_state == NAVIGATION_STATE_LAND
|| v_status.navigation_state == NAVIGATION_STATE_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
} else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) {
} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
} else if (v_status.arming_state == ARMING_STATE_INIT) {
*mavlink_state = MAV_STATE_UNINIT;
} else {

34
apps/uORB/topics/vehicle_status.h
View File

@ -60,8 +60,7 @@
/* State Machine */
typedef enum {
NAVIGATION_STATE_INIT = 0,
NAVIGATION_STATE_STANDBY,
NAVIGATION_STATE_STANDBY=0,
NAVIGATION_STATE_MANUAL,
NAVIGATION_STATE_SEATBELT,
NAVIGATION_STATE_LOITER,
@ -70,15 +69,13 @@ typedef enum {
NAVIGATION_STATE_RTL,
NAVIGATION_STATE_TAKEOFF,
NAVIGATION_STATE_LAND,
NAVIGATION_STATE_GROUND_ERROR,
NAVIGATION_STATE_REBOOT
} navigation_state_t;
typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
ARMING_STATE_ARMED,
ARMING_STATE_MISSION_ABORT,
ARMING_STATE_ABORT,
ARMING_STATE_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE
@ -95,6 +92,22 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
typedef enum {
MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_ASSISTED,
MODE_SWITCH_AUTO
} mode_switch_pos_t;
typedef enum {
RETURN_SWITCH_NONE = 0,
RETURN_SWITCH_RETURN
} return_switch_pos_t;
typedef enum {
MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_MISSION
} mission_switch_pos_t;
//enum VEHICLE_FLIGHT_MODE {
// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */
@ -168,9 +181,18 @@ struct vehicle_status_s
/* system flags - these represent the state predicates */
mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch;
mission_switch_pos_t mission_switch;
bool flag_system_armed; /**< true is motors / actuators are armed */
bool flag_system_emergency;
bool flag_system_sensors_ok;
bool flag_system_in_air_restore; /**< true if we can restore in mid air */
bool flag_system_sensors_initialized;
bool flag_system_arming_requested;
bool flag_system_disarming_requested;
bool flag_system_reboot_requested;
bool flag_system_on_ground;
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */