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"); warnx("starting");
char *main_states_str[MAIN_STATE_MAX]; char *main_states_str[MAIN_STATE_MAX];
main_states_str[0] = "MANUAL"; main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[1] = "ALTCTL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[2] = "POSCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[3] = "AUTO_MISSION"; main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[4] = "AUTO_LOITER"; main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[5] = "AUTO_RTL"; main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[6] = "ACRO"; main_states_str[MAIN_STATE_ACRO] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX]; char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT"; arming_states_str[ARMING_STATE_INIT] = "INIT";
arming_states_str[1] = "STANDBY"; arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[2] = "ARMED"; arming_states_str[ARMING_STATE_ARMED] = "ARMED";
arming_states_str[3] = "ARMED_ERROR"; arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[4] = "STANDBY_ERROR"; arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[5] = "REBOOT"; arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[6] = "IN_AIR_RESTORE"; 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 for slow low prio thread */
pthread_t commander_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 */ /* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false; bool arming_state_changed = false;
bool main_state_changed = false; bool main_state_changed = false;
bool failsafe_state_changed = false; bool failsafe_old = false;
while (!thread_should_exit) { while (!thread_should_exit) {
@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[])
was_armed = armed.armed; was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */ /* 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) { if (main_state_changed) {
status_changed = true; 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]); mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
main_state_changed = false; main_state_changed = false;
} }
if (failsafe_state_changed) { if (status.failsafe != failsafe_old) {
status_changed = true; status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); 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 */ /* publish states (armed, control mode, vehicle status) at least with 5 Hz */

28
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 * 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; status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */ /* 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_ALTCTL:
case MAIN_STATE_POSCTL: case MAIN_STATE_POSCTL:
/* require RC for all manual modes */ /* require RC for all manual modes */
if (status->rc_signal_lost) { if (status->rc_signal_lost && armed) {
status->failsafe = true; status->failsafe = true;
} else { } else {
@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status)
case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_MISSION:
/* require data link and global position */ /* 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; status->failsafe = true;
} else { } else {
if (armed) {
status->nav_state = NAVIGATION_STATE_AUTO_MISSION; status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
} }
break; break;
case MAIN_STATE_AUTO_LOITER: case MAIN_STATE_AUTO_LOITER:
/* require data link and local position */ /* 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; status->failsafe = true;
} else { } else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER; status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} }
break; break;
case MAIN_STATE_AUTO_RTL: case MAIN_STATE_AUTO_RTL:
/* require global position and home */ /* 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; status->failsafe = true;
} else { } else {
if (armed) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL; status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
} }
break; break;
@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status)
status->nav_state = NAVIGATION_STATE_TERMINATION; 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); 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_ */ #endif /* STATE_MACHINE_HELPER_H_ */

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

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