zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

State machine rewritten, compiles, WIP

sbg
Anton Babushkin 12 years ago
parent
7c1693a877
commit
8c1067a017
5 changed files with 928 additions and 1167 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1178
      src/modules/commander/commander.cpp
  2. 797
      src/modules/commander/state_machine_helper.cpp
  3. 12
      src/modules/commander/state_machine_helper.h
  4. 53
      src/modules/mavlink/mavlink.c
  5. 55
      src/modules/uORB/topics/vehicle_status.h

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

File diff suppressed because it is too large Load Diff

797
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -62,102 +62,110 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { transition_result_t
arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd)
{
int ret = ERROR; transition_result_t ret = TRANSITION_DENIED;
/* 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 == status->arming_state) {
ret = OK; ret = TRANSITION_NOT_CHANGED;
} else { } else {
switch (new_arming_state) { switch (new_arming_state) {
case ARMING_STATE_INIT: case ARMING_STATE_INIT:
/* allow going back from INIT for calibration */ /* allow going back from INIT for calibration */
if (current_state->arming_state == ARMING_STATE_STANDBY) { if (status->arming_state == ARMING_STATE_STANDBY) {
ret = OK; ret = TRANSITION_CHANGED;
armed->armed = false; armed->armed = false;
armed->ready_to_arm = false; armed->ready_to_arm = false;
} }
break;
case ARMING_STATE_STANDBY:
/* allow coming from INIT and disarming from ARMED */
if (current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED) {
/* sensors need to be initialized for STANDBY state */
if (current_state->condition_system_sensors_initialized) {
ret = OK;
armed->armed = false;
armed->ready_to_arm = true;
} else {
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
}
}
break;
case ARMING_STATE_ARMED:
/* allow arming from STANDBY and IN-AIR-RESTORE */ break;
if (current_state->arming_state == ARMING_STATE_STANDBY
|| current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
/* XXX conditions for arming? */ case ARMING_STATE_STANDBY:
ret = OK;
armed->armed = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
/* an armed error happens when ARMED obviously */
if (current_state->arming_state == ARMING_STATE_ARMED) {
/* XXX conditions for an error state? */
ret = OK;
armed->armed = true;
}
break;
case ARMING_STATE_STANDBY_ERROR:
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (current_state->arming_state == ARMING_STATE_STANDBY
|| current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = OK;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
/* an armed error happens when ARMED obviously */ /* allow coming from INIT and disarming from ARMED */
if (current_state->arming_state == ARMING_STATE_INIT if (status->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_ARMED) {
|| current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = OK; /* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
ret = TRANSITION_CHANGED;
armed->armed = false; armed->armed = false;
armed->ready_to_arm = false; armed->ready_to_arm = true;
} else {
mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized");
} }
break; }
case ARMING_STATE_IN_AIR_RESTORE:
/* XXX implement */ break;
break;
default: case ARMING_STATE_ARMED:
break;
} /* allow arming from STANDBY and IN-AIR-RESTORE */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_ARMED_ERROR:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_ARMED) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
if (ret == OK) { break;
current_state->arming_state = new_arming_state;
current_state->counter++;
current_state->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
armed->timestamp = hrt_absolute_time(); case ARMING_STATE_STANDBY_ERROR:
orb_publish(ORB_ID(actuator_armed), armed_pub, armed);
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
/* XXX implement */
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
hrt_abstime t = hrt_absolute_time();
status->arming_state = new_arming_state;
armed->timestamp = t;
} }
} }
@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
} }
transition_result_t
/* main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd)
* This functions does not evaluate any input flags but only checks if the transitions {
* are valid. transition_result_t ret = TRANSITION_DENIED;
*/
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) {
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_main_state == current_state->main_state) {
ret = OK; ret = TRANSITION_NOT_CHANGED;
} else { } else {
switch (new_navigation_state) { switch (new_main_state) {
case NAVIGATION_STATE_INIT: case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
/* transitions back to INIT are possible for calibration */ break;
if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
ret = OK;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_MANUAL_STANDBY:
/* transitions from INIT and other STANDBY states as well as MANUAL are possible */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to be disarmed first */
if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_MANUAL: case MAIN_STATE_SEATBELT:
/* need to be armed first */ /* need position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) { // TODO only need altitude estimate really
mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); if (!current_state->condition_local_position_valid) {
tune_negative(); mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate");
} else { tune_negative();
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
}
break;
case NAVIGATION_STATE_ASSISTED_STANDBY:
/* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) {
/* need to be disarmed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_ASSISTED_SEATBELT:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_ASSISTED_SIMPLE:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_ASSISTED_DESCENT:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_AUTO_STANDBY:
/* transitions from INIT or from other STANDBY modes or from AUTO READY */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
/* need to be disarmed and have a position and home lock */
if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed");
tune_negative();
} else if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_READY:
/* transitions from AUTO_STANDBY or AUTO_LAND */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
// XXX flag test needed?
/* need to be armed and have a position and home lock */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_TAKEOFF: } else {
ret = TRANSITION_CHANGED;
}
/* only transitions from AUTO_READY */ break;
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = OK; case MAIN_STATE_EASY:
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to have a position and home lock */
if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to have a mission ready */
if (!current_state-> condition_auto_mission_available) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_RTL:
/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to have a position and home lock */
if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_LAND:
/* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
/* need to have a position and home lock */
if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
default: /* need position estimate */
break; if (!current_state->condition_local_position_valid) {
} mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate");
tune_negative();
} else {
ret = TRANSITION_CHANGED;
}
if (ret == OK) { break;
current_state->navigation_state = new_navigation_state;
current_state->counter++; case MAIN_STATE_AUTO:
current_state->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_state); /* need position estimate */
if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate");
tune_negative();
} else {
ret = TRANSITION_CHANGED;
}
break;
}
control_mode->timestamp = hrt_absolute_time(); if (ret == TRANSITION_CHANGED) {
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); current_state->main_state = new_main_state;
} }
} }
return ret;
}
transition_result_t
navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == current_status->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_STANDBY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_STABILIZE:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_ALTHOLD:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_VECTOR:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_AUTO_READY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
current_status->navigation_state = new_navigation_state;
}
}
return ret; return ret;
} }
@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
switch (new_state) { switch (new_state) {
case HIL_STATE_OFF: case HIL_STATE_OFF:
if (current_status->arming_state == ARMING_STATE_INIT if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) { || current_status->arming_state == ARMING_STATE_STANDBY) {
current_control_mode->flag_system_hil_enabled = false; current_control_mode->flag_system_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true; valid_transition = true;
} }
break;
case HIL_STATE_ON: break;
if (current_status->arming_state == ARMING_STATE_INIT case HIL_STATE_ON:
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_control_mode->flag_system_hil_enabled = true; if (current_status->arming_state == ARMING_STATE_INIT
mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); || current_status->arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
} current_control_mode->flag_system_hil_enabled = true;
break; mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
default: break;
warnx("Unknown hil state");
break; default:
warnx("Unknown hil state");
break;
} }
} }
@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
ret = OK; ret = OK;
} else { } else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
} }

12
src/modules/commander/state_machine_helper.h
Unescape View File

@ -49,10 +49,18 @@
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); } transition_result_t;
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd);
transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd);
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);

53
src/modules/mavlink/mavlink.c
Unescape View File

@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
} }
/* autonomous mode */ /* autonomous mode */
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND if (v_status.main_state == MAIN_STATE_AUTO) {
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
} }
@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
} }
if (v_status.navigation_state == NAVIGATION_STATE_MANUAL if (v_status.main_state == MAIN_STATE_MANUAL
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { || v_status.main_state == MAIN_STATE_SEATBELT
|| v_status.main_state == MAIN_STATE_EASY) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
} }
if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT if (v_status.navigation_state == MAIN_STATE_EASY) {
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
} }
@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* set calibration state */ /* set calibration state */
if (v_status.preflight_calibration) { if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING; *mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.system_emergency) { } else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY; *mavlink_state = MAV_STATE_EMERGENCY;
} else if (v_status.arming_state == ARMING_STATE_INIT
// XXX difference between active and armed? is AUTO_READY active? || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER *mavlink_state = MAV_STATE_UNINIT;
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION } else if (v_status.arming_state == ARMING_STATE_ARMED) {
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
*mavlink_state = MAV_STATE_ACTIVE; *mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY *mavlink_state = MAV_STATE_CRITICAL;
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? } else if (v_status.arming_state == ARMING_STATE_STANDBY) {
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY; *mavlink_state = MAV_STATE_STANDBY;
} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { *mavlink_state = MAV_STATE_POWEROFF;
*mavlink_state = MAV_STATE_UNINIT;
} else { } else {
warnx("Unknown mavlink state"); warnx("Unknown mavlink state");
*mavlink_state = MAV_STATE_CRITICAL; *mavlink_state = MAV_STATE_CRITICAL;
} }
} }

55
src/modules/uORB/topics/vehicle_status.h
Unescape View File

@ -58,29 +58,28 @@
* @addtogroup topics @{ * @addtogroup topics @{
*/ */
/* State Machine */ /* main state machine */
typedef enum { typedef enum {
NAVIGATION_STATE_INIT = 0, MAIN_STATE_MANUAL = 0,
NAVIGATION_STATE_MANUAL_STANDBY, MAIN_STATE_SEATBELT,
NAVIGATION_STATE_MANUAL, MAIN_STATE_EASY,
NAVIGATION_STATE_ASSISTED_STANDBY, MAIN_STATE_AUTO,
NAVIGATION_STATE_ASSISTED_SEATBELT, } main_state_t;
NAVIGATION_STATE_ASSISTED_SIMPLE,
NAVIGATION_STATE_ASSISTED_DESCENT,
NAVIGATION_STATE_AUTO_STANDBY,
NAVIGATION_STATE_AUTO_READY,
NAVIGATION_STATE_AUTO_TAKEOFF,
NAVIGATION_STATE_AUTO_LOITER,
NAVIGATION_STATE_AUTO_MISSION,
NAVIGATION_STATE_AUTO_RTL,
NAVIGATION_STATE_AUTO_LAND
} navigation_state_t;
/* navigation state machine */
typedef enum { typedef enum {
MANUAL_STANDBY = 0, NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed
MANUAL_READY, NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization
MANUAL_IN_AIR NAVIGATION_STATE_STABILIZE, // attitude stabilization
} manual_state_t; NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff
NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode
NAVIGATION_STATE_AUTO_LOITER, // pause mission
NAVIGATION_STATE_AUTO_MISSION, // fly mission
NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND
NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector)
} navigation_state_t;
typedef enum { typedef enum {
ARMING_STATE_INIT = 0, ARMING_STATE_INIT = 0,
@ -103,16 +102,16 @@ typedef enum {
MODE_SWITCH_AUTO MODE_SWITCH_AUTO
} mode_switch_pos_t; } mode_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_EASY
} assisted_switch_pos_t;
typedef enum { typedef enum {
RETURN_SWITCH_NONE = 0, RETURN_SWITCH_NONE = 0,
RETURN_SWITCH_RETURN RETURN_SWITCH_RETURN
} return_switch_pos_t; } return_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_SIMPLE
} assisted_switch_pos_t;
typedef enum { typedef enum {
MISSION_SWITCH_NONE = 0, MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_MISSION MISSION_SWITCH_MISSION
@ -175,7 +174,8 @@ struct vehicle_status_s
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
// uint64_t failsave_highlevel_begin; TO BE COMPLETED // uint64_t failsave_highlevel_begin; TO BE COMPLETED
navigation_state_t navigation_state; /**< current system state */ main_state_t main_state; /**< main state machine */
navigation_state_t navigation_state; /**< navigation state machine */
arming_state_t arming_state; /**< current arming state */ arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */ hil_state_t hil_state; /**< current hil state */
@ -183,6 +183,8 @@ struct vehicle_status_s
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
bool is_rotary_wing;
mode_switch_pos_t mode_switch; mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch; return_switch_pos_t return_switch;
assisted_switch_pos_t assisted_switch; assisted_switch_pos_t assisted_switch;
@ -198,6 +200,7 @@ struct vehicle_status_s
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid; bool condition_local_position_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool rc_signal_found_once; bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_lost; /**< true if RC reception is terminally lost */

Write Preview
Loading…
Cancel
Save