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

commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing

sbg
Anton Babushkin 12 years ago
parent
eb52eae153
commit
963abd66ba
5 changed files with 178 additions and 108 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. 144
      src/modules/commander/commander.cpp
  2. 66
      src/modules/commander/state_machine_helper.cpp
  3. 12
      src/modules/mavlink/mavlink.c
  4. 1
      src/modules/uORB/topics/manual_control_setpoint.h
  5. 17
      src/modules/uORB/topics/vehicle_status.h

144
src/modules/commander/commander.cpp
Unescape View File

@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[])
/* check for global or local position updates, set a timeout of 2s */ /* check for global or local position updates, set a timeout of 2s */
if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) {
current_status.condition_global_position_valid = true; current_status.condition_global_position_valid = true;
// XXX check for controller status and home position as well
} else { } else {
current_status.condition_global_position_valid = false; current_status.condition_global_position_valid = false;
} }
if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) {
current_status.condition_local_position_valid = true; current_status.condition_local_position_valid = true;
// XXX check for controller status and home position as well
} else { } else {
current_status.condition_local_position_valid = false; current_status.condition_local_position_valid = false;
@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
/* center stick position, set seatbelt/simple control */ /* center stick position, set seatbelt/simple control */
current_status.mode_switch = MODE_SWITCH_SEATBELT; current_status.mode_switch = MODE_SWITCH_ASSISTED;
} }
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[])
/* this switch is not properly mapped, set default */ /* this switch is not properly mapped, set default */
current_status.return_switch = RETURN_SWITCH_NONE; current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */ /* top stick position */
current_status.return_switch = RETURN_SWITCH_RETURN; current_status.return_switch = RETURN_SWITCH_RETURN;
} else { } else {
/* center stick position, set default */ /* center or bottom stick position, set default */
current_status.return_switch = RETURN_SWITCH_NONE; current_status.return_switch = RETURN_SWITCH_NONE;
} }
/* check assisted switch */
if (!isfinite(sp_man.assisted_switch)) {
/* this switch is not properly mapped, set default */
current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT;
} else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */
current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE;
} else {
/* center or bottom stick position, set default */
current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT;
}
/* check mission switch */ /* check mission switch */
if (!isfinite(sp_man.mission_switch)) { if (!isfinite(sp_man.mission_switch)) {
@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[])
warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
} }
/* Try seatbelt or fallback to manual */ /* Try assisted or fallback to manual */
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY // fallback to MANUAL_STANDBY
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[])
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// first fallback to SEATBELT_STANDY // first fallback to SEATBELT_STANDY
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// or fallback to MANUAL_STANDBY // or fallback to MANUAL_STANDBY
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
@ -1250,86 +1259,98 @@ int commander_thread_main(int argc, char *argv[])
/* evaluate the navigation state when armed */ /* evaluate the navigation state when armed */
case ARMING_STATE_ARMED: case ARMING_STATE_ARMED:
/* Always accept manual mode */
if (current_status.mode_switch == MODE_SWITCH_MANUAL) { if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
/* MANUAL */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
} }
/* SEATBELT_STANDBY (fallback: MANUAL) */ } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) {
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT /* ASSISTED */
&& current_status.return_switch == RETURN_SWITCH_NONE) { if (current_status.return_switch == RETURN_SWITCH_RETURN) {
/* ASSISTED_DESCENT */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY // fallback to MANUAL
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
} }
} }
/* SEATBELT_DESCENT (fallback: MANUAL) */ } else {
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) {
&& current_status.return_switch == RETURN_SWITCH_RETURN) { /* ASSISTED_SIMPLE */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to ASSISTED_SEATBELT
// fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
} }
} }
}
/* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ } else {
} else if (current_status.mode_switch == MODE_SWITCH_AUTO /* ASSISTED_SEATBELT */
&& current_status.return_switch == RETURN_SWITCH_NONE if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
&& current_status.mission_switch == MISSION_SWITCH_NONE) { // fallback to MANUAL
/* we might come from the disarmed state AUTO_STANDBY */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
} }
} }
/* or from some other armed state like SEATBELT or MANUAL */ }
} else if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { }
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY } else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
/* AUTO */
if (current_status.return_switch == RETURN_SWITCH_RETURN) {
/* AUTO_RTL */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to ASSISTED_DESCENT
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
} }
} }
} }
} else {
/* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ if (current_status.mission_switch == MISSION_SWITCH_MISSION) {
} else if (current_status.mode_switch == MODE_SWITCH_AUTO /* AUTO_MISSION */
&& current_status.return_switch == RETURN_SWITCH_NONE
&& current_status.mission_switch == MISSION_SWITCH_MISSION) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to ASSISTED_SEATBELT
// fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
} }
} }
} }
} else {
/* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ // TODO check this
} else if (current_status.mode_switch == MODE_SWITCH_AUTO if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
&& current_status.return_switch == RETURN_SWITCH_RETURN || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
&& (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { /* AUTO_READY */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { /* AUTO_READY */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL_STANDBY // fallback to MANUAL
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
}
} else {
/* AUTO_LOITER */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to ASSISTED_SEATBELT
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// fallback to MANUAL
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
// These is not supposed to happen // These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected"); warnx("ERROR: Navigation state MANUAL rejected");
@ -1337,6 +1358,9 @@ int commander_thread_main(int argc, char *argv[])
} }
} }
} }
}
}
}
break; break;
// XXX we might be missing something that triggers a transition from RTL to LAND // XXX we might be missing something that triggers a transition from RTL to LAND

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

@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* transitions back to INIT are possible for calibration */ /* transitions back to INIT are possible for calibration */
if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
ret = OK; ret = OK;
@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* transitions from INIT and other STANDBY states as well as MANUAL are possible */ /* transitions from INIT and other STANDBY states as well as MANUAL are possible */
if (current_state->navigation_state == NAVIGATION_STATE_INIT if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) { || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
} }
break; break;
case NAVIGATION_STATE_SEATBELT_STANDBY: case NAVIGATION_STATE_ASSISTED_STANDBY:
/* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
if (current_state->navigation_state == NAVIGATION_STATE_INIT if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { || 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 */ /* need to be disarmed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_STANDBY) { if (current_state->arming_state != ARMING_STATE_STANDBY) {
@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
} }
break; break;
case NAVIGATION_STATE_SEATBELT: case NAVIGATION_STATE_ASSISTED_SEATBELT:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT || 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_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
} }
break; break;
case NAVIGATION_STATE_SEATBELT_DESCENT: 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 = false;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_ASSISTED_DESCENT:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT 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_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* transitions from INIT or from other STANDBY modes or from AUTO READY */ /* transitions from INIT or from other STANDBY modes or from AUTO READY */
if (current_state->navigation_state == NAVIGATION_STATE_INIT if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
/* need to be disarmed and have a position and home lock */ /* need to be disarmed and have a position and home lock */
@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT || 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_MANUAL) {
/* need to have a position and home lock */ /* need to have a position and home lock */
@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT || 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_MANUAL) {
/* need to have a mission ready */ /* need to have a mission ready */
@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT || 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_MANUAL) {
/* need to have a position and home lock */ /* need to have a position and home lock */

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

@ -221,9 +221,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
} }
if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
} }
@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) { || v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
*mavlink_state = MAV_STATE_ACTIVE; *mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY; *mavlink_state = MAV_STATE_STANDBY;

1
src/modules/uORB/topics/manual_control_setpoint.h
Unescape View File

@ -58,6 +58,7 @@ struct manual_control_setpoint_s {
float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
float return_switch; /**< land 2 position switch (mandatory): land, no effect */ float return_switch; /**< land 2 position switch (mandatory): land, no effect */
float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
/** /**

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

@ -60,12 +60,13 @@
/* State Machine */ /* State Machine */
typedef enum { typedef enum {
NAVIGATION_STATE_INIT=0, NAVIGATION_STATE_INIT = 0,
NAVIGATION_STATE_MANUAL_STANDBY, NAVIGATION_STATE_MANUAL_STANDBY,
NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_MANUAL,
NAVIGATION_STATE_SEATBELT_STANDBY, NAVIGATION_STATE_ASSISTED_STANDBY,
NAVIGATION_STATE_SEATBELT, NAVIGATION_STATE_ASSISTED_SEATBELT,
NAVIGATION_STATE_SEATBELT_DESCENT, NAVIGATION_STATE_ASSISTED_SIMPLE,
NAVIGATION_STATE_ASSISTED_DESCENT,
NAVIGATION_STATE_AUTO_STANDBY, NAVIGATION_STATE_AUTO_STANDBY,
NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_AUTO_READY,
NAVIGATION_STATE_AUTO_TAKEOFF, NAVIGATION_STATE_AUTO_TAKEOFF,
@ -98,7 +99,7 @@ typedef enum {
typedef enum { typedef enum {
MODE_SWITCH_MANUAL = 0, MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_SEATBELT, MODE_SWITCH_ASSISTED,
MODE_SWITCH_AUTO MODE_SWITCH_AUTO
} mode_switch_pos_t; } mode_switch_pos_t;
@ -107,6 +108,11 @@ typedef enum {
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
@ -179,6 +185,7 @@ struct vehicle_status_s
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;
mission_switch_pos_t mission_switch; mission_switch_pos_t mission_switch;
bool condition_battery_voltage_valid; bool condition_battery_voltage_valid;

Write Preview
Loading…
Cancel
Save