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

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

This commit is contained in:
Anton Babushkin 2013-07-22 21:48:21 +04:00
parent eb52eae153
commit 963abd66ba
5 changed files with 178 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

190
src/modules/commander/commander.cpp
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 */
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[])
} 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[])
/* 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[])
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[])
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[])
/* 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
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 */
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
/* 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
}
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
}
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
}
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
/* 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
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
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
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
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;
}
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)
|| 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
View File

@ -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
View File

@ -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 {
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 {
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
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;