dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: modes display fixes, don't activate failsafe while disarmed

This commit is contained in:
Anton Babushkin 2014-06-17 13:19:50 +02:00
parent e0ed0625f8
commit 55e5f747de
4 changed files with 66 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
src/modules/commander/commander.cpp
View File

@ -624,22 +624,34 @@ int commander_thread_main(int argc, char *argv[])
warnx("starting");
char *main_states_str[MAIN_STATE_MAX];
main_states_str[0] = "MANUAL";
main_states_str[1] = "ALTCTL";
main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO_MISSION";
main_states_str[4] = "AUTO_LOITER";
main_states_str[5] = "AUTO_RTL";
main_states_str[6] = "ACRO";
main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
arming_states_str[1] = "STANDBY";
arming_states_str[2] = "ARMED";
arming_states_str[3] = "ARMED_ERROR";
arming_states_str[4] = "STANDBY_ERROR";
arming_states_str[5] = "REBOOT";
arming_states_str[6] = "IN_AIR_RESTORE";
arming_states_str[ARMING_STATE_INIT] = "INIT";
arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[ARMING_STATE_ARMED] = "ARMED";
arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
char *nav_states_str[NAVIGATION_STATE_MAX];
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@ -844,7 +856,7 @@ int commander_thread_main(int argc, char *argv[])
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_state_changed = false;
bool failsafe_old = false;
while (!thread_should_exit) {
@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[])
was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */
set_nav_state(&status);
bool nav_state_changed = set_nav_state(&status);
// TODO handle mode changes by commands
if (main_state_changed) {
status_changed = true;
warnx("main state: %s", main_states_str[status.main_state]);
mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
main_state_changed = false;
}
if (failsafe_state_changed) {
if (status.failsafe != failsafe_old) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
failsafe_state_changed = false;
failsafe_old = status.failsafe;
}
if (nav_state_changed) {
status_changed = true;
warnx("nav state: %s", nav_states_str[status.nav_state]);
mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */

32
src/modules/commander/state_machine_helper.cpp
View File

@ -366,8 +366,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
void set_nav_state(struct vehicle_status_s *status)
bool set_nav_state(struct vehicle_status_s *status)
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */
@ -377,7 +380,7 @@ void set_nav_state(struct vehicle_status_s *status)
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost) {
if (status->rc_signal_lost && armed) {
status->failsafe = true;
} else {
@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status)
case MAIN_STATE_AUTO_MISSION:
/* require data link and global position */
if (status->data_link_lost || !status->condition_global_position_valid) {
if ((status->data_link_lost || !status->condition_global_position_valid) && armed) {
status->failsafe = true;
} else {
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
if (armed) {
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
}
break;
case MAIN_STATE_AUTO_LOITER:
/* require data link and local position */
if (status->data_link_lost || !status->condition_local_position_valid) {
if ((status->data_link_lost || !status->condition_local_position_valid) && armed) {
status->failsafe = true;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if (!status->condition_global_position_valid || !status->condition_home_position_valid) {
if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) {
status->failsafe = true;
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
if (armed) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
}
break;
@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status)
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
}
return status->nav_state != nav_state_old;
}

2
src/modules/commander/state_machine_helper.h
View File

@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
void set_nav_state(struct vehicle_status_s *status);
bool set_nav_state(struct vehicle_status_s *status);
#endif /* STATE_MACHINE_HELPER_H_ */

3
src/modules/uORB/topics/vehicle_status.h
View File

@ -96,15 +96,16 @@ typedef enum {
*/
typedef enum {
NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_MAX,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {