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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 190
      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

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

@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[]) @@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[])
/* 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;
// XXX check for controller status and home position as well
} else {
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;
// XXX check for controller status and home position as well
} else {
current_status.condition_local_position_valid = false;
@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* 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);
@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[]) @@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[])
/* this switch is not properly mapped, set default */
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) {
/* top stick position */
current_status.return_switch = RETURN_SWITCH_RETURN;
} else {
/* center stick position, set default */
/* center or bottom stick position, set default */
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 */
if (!isfinite(sp_man.mission_switch)) {
@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[]) @@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[])
warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
}
/* Try seatbelt or fallback to manual */
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
/* Try assisted or fallback to manual */
} 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
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
@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -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) {
// 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
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
@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[]) @@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[])
/* evaluate the navigation state when armed */
case ARMING_STATE_ARMED:
/* Always accept manual mode */
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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
/* SEATBELT_STANDBY (fallback: MANUAL) */
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
&& current_status.return_switch == RETURN_SWITCH_NONE) {
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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
/* SEATBELT_DESCENT (fallback: MANUAL) */
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
&& current_status.return_switch == RETURN_SWITCH_RETURN) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
/* AUTO_LOITER (fallback: SEATBELT, MANUAL) */
} else if (current_status.mode_switch == MODE_SWITCH_AUTO
&& current_status.return_switch == RETURN_SWITCH_NONE
&& current_status.mission_switch == MISSION_SWITCH_NONE) {
/* 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
} else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) {
/* ASSISTED */
if (current_status.return_switch == RETURN_SWITCH_RETURN) {
/* 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
// These is not supposed to happen
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
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 {
if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) {
/* ASSISTED_SIMPLE */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SIMPLE, 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
}
} else {
/* 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
}
}
/* AUTO_MISSION (fallback: SEATBELT, MANUAL) */
} else if (current_status.mode_switch == MODE_SWITCH_AUTO
&& 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_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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
} 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
}
}
/* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */
} else if (current_status.mode_switch == MODE_SWITCH_AUTO
&& current_status.return_switch == RETURN_SWITCH_RETURN
&& (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) {
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, 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 MANUAL_STANDBY
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 {
if (current_status.mission_switch == MISSION_SWITCH_MISSION) {
/* AUTO_MISSION */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
}
} else {
// TODO check this
if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
/* AUTO_READY */
if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
/* AUTO_READY */
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) {
// 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) {
// These is not supposed to happen
warnx("ERROR: Navigation state MANUAL rejected");
}
}
}
}
}
}

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 @@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
/* transitions back to INIT are possible for calibration */
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) {
ret = OK;
@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -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 */
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_MANUAL) {
@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
}
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 */
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_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) {
|| 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) {
@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
}
break;
case NAVIGATION_STATE_SEATBELT:
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_SEATBELT_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
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
@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
}
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 */
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_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -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 */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| 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) {
/* 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 @@ -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
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| 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) {
/* need to have a position and home lock */
@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -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
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| 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) {
/* need to have a mission ready */
@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -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
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| 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) {
/* 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) @@ -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;
}
if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) {
if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) @@ -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_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
|| 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;
} 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_SEATBELT_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_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 { @@ -58,6 +58,7 @@ struct manual_control_setpoint_s {
float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
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 */
/**

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

@ -60,12 +60,13 @@ @@ -60,12 +60,13 @@
/* State Machine */
typedef enum {
NAVIGATION_STATE_INIT=0,
NAVIGATION_STATE_INIT = 0,
NAVIGATION_STATE_MANUAL_STANDBY,
NAVIGATION_STATE_MANUAL,
NAVIGATION_STATE_SEATBELT_STANDBY,
NAVIGATION_STATE_SEATBELT,
NAVIGATION_STATE_SEATBELT_DESCENT,
NAVIGATION_STATE_ASSISTED_STANDBY,
NAVIGATION_STATE_ASSISTED_SEATBELT,
NAVIGATION_STATE_ASSISTED_SIMPLE,
NAVIGATION_STATE_ASSISTED_DESCENT,
NAVIGATION_STATE_AUTO_STANDBY,
NAVIGATION_STATE_AUTO_READY,
NAVIGATION_STATE_AUTO_TAKEOFF,
@ -98,7 +99,7 @@ typedef enum { @@ -98,7 +99,7 @@ typedef enum {
typedef enum {
MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_SEATBELT,
MODE_SWITCH_ASSISTED,
MODE_SWITCH_AUTO
} mode_switch_pos_t;
@ -107,6 +108,11 @@ typedef enum { @@ -107,6 +108,11 @@ typedef enum {
RETURN_SWITCH_RETURN
} return_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_SIMPLE
} assisted_switch_pos_t;
typedef enum {
MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_MISSION
@ -179,6 +185,7 @@ struct vehicle_status_s @@ -179,6 +185,7 @@ struct vehicle_status_s
mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch;
assisted_switch_pos_t assisted_switch;
mission_switch_pos_t mission_switch;
bool condition_battery_voltage_valid;

Write Preview
Loading…
Cancel
Save