Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

commander: take main_state out of vehicle_status

Browse Source 
This state is only commander internal. Therefore it doesn't need to be
in vehicle_status. Instead it is now in the commander_state message.
This commit is contained in:
Julian Oes
2016-02-26 15:41:03 +00:00
parent 1ad0ee0fae
commit 5ca5af5fcd
9 changed files with 214 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/commander_state.msg
+31
View File
@@ -0,0 +1,31 @@
# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
uint8 MAIN_STATE_MANUAL = 0
uint8 MAIN_STATE_ALTCTL = 1
uint8 MAIN_STATE_POSCTL = 2
uint8 MAIN_STATE_AUTO_MISSION = 3
uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_STAB = 8
uint8 MAIN_STATE_RATTITUDE = 9
uint8 MAIN_STATE_AUTO_TAKEOFF = 10
uint8 MAIN_STATE_AUTO_LAND = 11
uint8 MAIN_STATE_MAX = 12
# If you change the order, add or remove arming_state_t states make sure to update the arrays
# in state_machine_helper.cpp as well.
uint8 ARMING_STATE_INIT = 0
uint8 ARMING_STATE_STANDBY = 1
uint8 ARMING_STATE_ARMED = 2
uint8 ARMING_STATE_ARMED_ERROR = 3
uint8 ARMING_STATE_STANDBY_ERROR = 4
uint8 ARMING_STATE_REBOOT = 5
uint8 ARMING_STATE_IN_AIR_RESTORE = 6
uint8 ARMING_STATE_MAX = 7
# state machine / state of vehicle.
# Encodes the complete system state and is set by the commander app.
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine
msg/vehicle_status.msg
-1
View File
@@ -59,7 +59,6 @@ uint8 RC_IN_MODE_GENERATED = 2
# Encodes the complete system state and is set by the commander app. # Encodes the complete system state and is set by the commander app.
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine
uint8 nav_state # set navigation state machine to specified value uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state uint8 arming_state # current arming state
uint8 hil_state # current hil state uint8 hil_state # current hil state
src/drivers/blinkm/blinkm.cpp
+7 -8
View File
@@ -701,19 +701,18 @@ BlinkM::led()
led_blink = LED_BLINK; led_blink = LED_BLINK;
if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) {
led_color_4 = LED_GREEN;
}
/* TODO: add other Auto modes */ /* indicate main control state */
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { if (vehicle_control_mode.flag_control_auto_enabled) {
led_color_4 = LED_BLUE; led_color_4 = LED_BLUE;
} else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) { } else if (vehicle_control_mode.flag_control_position_enabled) {
led_color_4 = LED_GREEN;
} else if (vehicle_control_mode.flag_control_altitude_enabled) {
led_color_4 = LED_YELLOW; led_color_4 = LED_YELLOW;
} else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) { } else if (vehicle_control_mode.flag_control_manual_enabled) {
led_color_4 = LED_WHITE; led_color_4 = LED_WHITE;
} else { } else {
src/modules/commander/commander.cpp
+98 -95
View File
@@ -213,6 +213,7 @@ static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode; static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home; static struct home_position_s _home;
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX]; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
static struct commander_state_s internal_state;
static unsigned _last_mission_instance = 0; static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man = {}; static manual_control_setpoint_s _last_sp_man = {};
@@ -579,7 +580,7 @@ void print_status()
const char *armed_str; const char *armed_str;
switch (state.arming_state) { switch (status.arming_state) {
case vehicle_status_s::ARMING_STATE_INIT: case vehicle_status_s::ARMING_STATE_INIT:
armed_str = "INIT"; armed_str = "INIT";
break; break;
@@ -698,34 +699,34 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */ /* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */ /* ALTCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */ /* AUTO */
if (custom_sub_mode > 0) { if (custom_sub_mode > 0) {
switch(custom_sub_mode) { switch(custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL: case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND: case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET: case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET);
@@ -738,12 +739,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} }
} else { } else {
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);
} }
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */ /* ACRO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) {
/* RATTITUDE */ /* RATTITUDE */
@@ -751,30 +752,30 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */ /* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */ /* OFFBOARD */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
} }
} else { } else {
/* use base mode */ /* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */ /* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
} else { } else {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} }
} }
} }
@@ -809,7 +810,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Flick to inair restore first if this comes from an onboard system // Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
} }
else { else {
@@ -979,14 +980,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: { case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED; transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard = vehicle_status_s::MAIN_STATE_MANUAL; static main_state_t main_state_pre_offboard = commander_state_s::MAIN_STATE_MANUAL;
if (status_local->main_state != vehicle_status_s::MAIN_STATE_OFFBOARD) { if (internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state; main_state_pre_offboard = internal_state.main_state;
} }
if (cmd->param1 > 0.5f) { if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags); res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD"); print_reject_mode(status_local, "OFFBOARD");
@@ -1000,7 +1001,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else { } else {
/* If the mavlink command is used to enable or disable offboard control: /* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */ * switch back to previous mode when disabling */
res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags); res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags, &internal_state);
status_flags.offboard_control_set_by_command = false; status_flags.offboard_control_set_by_command = false;
} }
} }
@@ -1008,7 +1009,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */ /* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags)) { if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) {
warnx("taking off!"); warnx("taking off!");
} else { } else {
warnx("takeoff denied"); warnx("takeoff denied");
@@ -1019,7 +1020,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
/* ok, home set, use it to take off */ /* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags)) { if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) {
warnx("landing!"); warnx("landing!");
} else { } else {
warnx("landing denied"); warnx("landing denied");
@@ -1172,16 +1173,16 @@ int commander_thread_main(int argc, char *argv[])
// These are too verbose, but we will retain them a little longer // These are too verbose, but we will retain them a little longer
// until we are sure we really don't need them. // until we are sure we really don't need them.
// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // const char *main_states_str[commander_state_s::MAIN_STATE_MAX];
// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; // main_states_str[commander_state_s::MAIN_STATE_MANUAL] = "MANUAL";
// main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; // main_states_str[commander_state_s::MAIN_STATE_ALTCTL] = "ALTCTL";
// main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; // main_states_str[commander_state_s::MAIN_STATE_POSCTL] = "POSCTL";
// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; // main_states_str[commander_state_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; // main_states_str[commander_state_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; // main_states_str[commander_state_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
// main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; // main_states_str[commander_state_s::MAIN_STATE_ACRO] = "ACRO";
// main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; // main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB";
// main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; // main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
// const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; // const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
// arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; // arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT";
@@ -1230,8 +1231,8 @@ int commander_thread_main(int argc, char *argv[])
// We want to accept RC inputs as default // We want to accept RC inputs as default
status_flags.rc_input_blocked = false; status_flags.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL;
main_state_prev = vehicle_status_s::MAIN_STATE_MAX; main_state_prev = commander_state_s::MAIN_STATE_MAX;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT; status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
@@ -2144,7 +2145,7 @@ int commander_thread_main(int argc, char *argv[])
static bool geofence_loiter_on = false; static bool geofence_loiter_on = false;
static bool geofence_rtl_on = false; static bool geofence_rtl_on = false;
static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX; static uint8_t geofence_main_state_before_violation = commander_state_s::MAIN_STATE_MAX;
// check for geofence violation // check for geofence violation
if (geofence_result.geofence_violated) { if (geofence_result.geofence_violated) {
@@ -2164,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[])
break; break;
} }
case (geofence_result_s::GF_ACTION_LOITER) : { case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags)) { if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state)) {
geofence_loiter_on = true; geofence_loiter_on = true;
} }
break; break;
} }
case (geofence_result_s::GF_ACTION_RTL) : { case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags)) { if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) {
geofence_rtl_on = true; geofence_rtl_on = true;
} }
break; break;
@@ -2188,26 +2189,26 @@ int commander_thread_main(int argc, char *argv[])
// reset if no longer in LOITER or if manually switched to LOITER // reset if no longer in LOITER or if manually switched to LOITER
geofence_loiter_on = geofence_loiter_on geofence_loiter_on = geofence_loiter_on
&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER) && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)
&& (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF); && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
// reset if no longer in RTL or if manually switched to RTL // reset if no longer in RTL or if manually switched to RTL
geofence_rtl_on = geofence_rtl_on geofence_rtl_on = geofence_rtl_on
&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL) && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL)
&& (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF); && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
if (!geofence_loiter_on && !geofence_rtl_on) { if (!geofence_loiter_on && !geofence_rtl_on) {
// store the last good main_state when not in a geofence triggered action (LOITER or RTL) // store the last good main_state when not in a geofence triggered action (LOITER or RTL)
geofence_main_state_before_violation = status.main_state; geofence_main_state_before_violation = internal_state.main_state;
} }
// revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST // revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST
if ((geofence_loiter_on || geofence_rtl_on) && if ((geofence_loiter_on || geofence_rtl_on) &&
(geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL || (geofence_main_state_before_violation == commander_state_s::MAIN_STATE_MANUAL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL || geofence_main_state_before_violation == commander_state_s::MAIN_STATE_ALTCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL || geofence_main_state_before_violation == commander_state_s::MAIN_STATE_POSCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO || geofence_main_state_before_violation == commander_state_s::MAIN_STATE_ACRO ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) { geofence_main_state_before_violation == commander_state_s::MAIN_STATE_STAB)) {
// transition to previous state if sticks are increased // transition to previous state if sticks are increased
const float min_stick_change = 0.2; const float min_stick_change = 0.2;
@@ -2217,7 +2218,7 @@ int commander_thread_main(int argc, char *argv[])
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, geofence_main_state_before_violation, main_state_prev, &status_flags); main_state_transition(&status, geofence_main_state_before_violation, main_state_prev, &status_flags, &internal_state);
} }
} }
} }
@@ -2286,10 +2287,10 @@ int commander_thread_main(int argc, char *argv[])
* do it only for rotary wings */ * do it only for rotary wings */
if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO || internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB || internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE || internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
land_detector.landed) && land_detector.landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
@@ -2328,10 +2329,10 @@ int commander_thread_main(int argc, char *argv[])
* the system can be armed in auto if armed via the GCS. * the system can be armed in auto if armed via the GCS.
*/ */
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)
&& (status.main_state != vehicle_status_s::MAIN_STATE_ACRO) && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)
&& (status.main_state != vehicle_status_s::MAIN_STATE_STAB) && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
&& (status.main_state != vehicle_status_s::MAIN_STATE_ALTCTL)) { && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)) {
print_reject_arm("NOT ARMING: Switch to a manual mode first."); print_reject_arm("NOT ARMING: Switch to a manual mode first.");
} else if (!status_flags.condition_home_position_valid && } else if (!status_flags.condition_home_position_valid &&
@@ -2525,15 +2526,15 @@ int commander_thread_main(int argc, char *argv[])
/* reset main state after takeoff or land has been completed */ /* reset main state after takeoff or land has been completed */
/* only switch back to at least altitude controlled modes */ /* only switch back to at least altitude controlled modes */
if (main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || if (main_state_prev == commander_state_s::MAIN_STATE_POSCTL ||
main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { main_state_prev == commander_state_s::MAIN_STATE_ALTCTL) {
if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF if ((internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) || && mission_result.finished) ||
(status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND
&& land_detector.landed)) { && land_detector.landed)) {
main_state_transition(&status, main_state_prev, main_state_prev, &status_flags); main_state_transition(&status, main_state_prev, main_state_prev, &status_flags, &internal_state);
} }
} }
@@ -2556,12 +2557,12 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the data link and the gps system have been checked /* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode) * If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */ * and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE && internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
status.main_state !=vehicle_status_s::MAIN_STATE_STAB && internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status_flags.gps_failure) || ((status.data_link_lost && status_flags.gps_failure) ||
(status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) { (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) {
armed.force_failsafe = true; armed.force_failsafe = true;
@@ -2582,12 +2583,12 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the rc signal and the gps system have been checked /* At this point the rc signal and the gps system have been checked
* If we are in manual (controlled with RC): * If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */ * if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
status.main_state ==vehicle_status_s::MAIN_STATE_STAB || internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status_flags.gps_failure) || ((status.rc_signal_lost && status_flags.gps_failure) ||
(status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) {
armed.force_failsafe = true; armed.force_failsafe = true;
@@ -2629,7 +2630,9 @@ int commander_thread_main(int argc, char *argv[])
} }
/* now set navigation state according to failsafe and main state */ /* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, bool nav_state_changed = set_nav_state(&status,
&internal_state,
(bool)datalink_loss_enabled,
mission_result.finished, mission_result.finished,
mission_result.stay_in_failsafe, mission_result.stay_in_failsafe,
&status_flags, &status_flags,
@@ -2833,7 +2836,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (set_normal_color) { if (set_normal_color) {
/* set color */ /* set color */
if (status_local->failsafe) { if (status.failsafe) {
rgbled_set_color(RGBLED_COLOR_PURPLE); rgbled_set_color(RGBLED_COLOR_PURPLE);
} else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) { } else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER); rgbled_set_color(RGBLED_COLOR_AMBER);
@@ -2895,7 +2898,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
// just delete this and respond to mode switches // just delete this and respond to mode switches
/* if offboard is set already by a mavlink command, abort */ /* if offboard is set already by a mavlink command, abort */
if (status_flags.offboard_control_set_by_command) { if (status_flags.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags); return main_state_transition(status_local,commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
} }
/* manual setpoint has not updated, do not re-evaluate it */ /* manual setpoint has not updated, do not re-evaluate it */
@@ -2924,7 +2927,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switch overrides main switch */ /* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD"); print_reject_mode(status_local, "OFFBOARD");
@@ -2939,13 +2942,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* RTL switch overrides main switch */ /* RTL switch overrides main switch */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
warnx("RTL switch changed and ON!"); warnx("RTL switch changed and ON!");
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO RTL"); print_reject_mode(status_local, "AUTO RTL");
/* fallback to LOITER if home position not set */ /* fallback to LOITER if home position not set */
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
} }
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
@@ -3058,11 +3061,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
*/ */
// XXX: put ACRO and STAB on separate switches // XXX: put ACRO and STAB on separate switches
if (status.is_rotary_wing && !status.is_vtol) { if (status.is_rotary_wing && !status.is_vtol) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev, &status_flags); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);
} else if (!status.is_rotary_wing) { } else if (!status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
} else { } else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} }
} }
@@ -3070,12 +3073,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* Similar to acro transitions for multirotors. FW aircraft don't need a /* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/ * rattitude mode.*/
if (status.is_rotary_wing) { if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state);
} else { } else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
} }
}else { }else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} }
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
@@ -3083,7 +3086,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3093,7 +3096,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} }
// fallback to ALTCTL // fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode break; // changed successfully or already in this mode
@@ -3104,13 +3107,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} }
// fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3119,7 +3122,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO PAUSE"); print_reject_mode(status_local, "AUTO PAUSE");
} else { } else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3128,7 +3131,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO MISSION"); print_reject_mode(status_local, "AUTO MISSION");
// fallback to LOITER if home position not set // fallback to LOITER if home position not set
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3136,21 +3139,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} }
// fallback to POSCTL // fallback to POSCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to ALTCTL // fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); res = main_state_transition(status_local,commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
src/modules/commander/state_machine_helper.cpp
+54 -53
View File
@@ -348,20 +348,20 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
transition_result_t transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
status_flags_s *status_flags) status_flags_s *status_flags, struct commander_state_s *internal_state)
{ {
transition_result_t ret = TRANSITION_DENIED; transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if the same state is requested because conditions may have changed */ /* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) { switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL: case commander_state_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO: case commander_state_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_RATTITUDE: case commander_state_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
break; break;
case vehicle_status_s::MAIN_STATE_ALTCTL: case commander_state_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */ /* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */ /* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing || if (!status->is_rotary_wing ||
@@ -371,7 +371,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
} }
break; break;
case vehicle_status_s::MAIN_STATE_POSCTL: case commander_state_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */ /* need at minimum local position estimate */
if (status_flags->condition_local_position_valid || if (status_flags->condition_local_position_valid ||
status_flags->condition_global_position_valid) { status_flags->condition_global_position_valid) {
@@ -379,25 +379,25 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER: case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */ /* need global position estimate */
if (status_flags->condition_global_position_valid) { if (status_flags->condition_global_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET: case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case commander_state_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL: case commander_state_s::MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
case vehicle_status_s::MAIN_STATE_AUTO_LAND: case commander_state_s::MAIN_STATE_AUTO_LAND:
/* need global position and home position */ /* need global position and home position */
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
break; break;
case vehicle_status_s::MAIN_STATE_OFFBOARD: case commander_state_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */ /* need offboard signal */
if (!status_flags->offboard_control_signal_lost) { if (!status_flags->offboard_control_signal_lost) {
@@ -406,14 +406,14 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break; break;
case vehicle_status_s::MAIN_STATE_MAX: case commander_state_s::MAIN_STATE_MAX:
default: default:
break; break;
} }
if (ret == TRANSITION_CHANGED) { if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) { if (internal_state->main_state != new_main_state) {
main_state_prev = status->main_state; main_state_prev = internal_state->main_state;
status->main_state = new_main_state; internal_state->main_state = new_main_state;
} else { } else {
ret = TRANSITION_NOT_CHANGED; ret = TRANSITION_NOT_CHANGED;
} }
@@ -597,25 +597,26 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
/** /**
* Check failsafe and main status and set navigation status for navigator accordingly * Check failsafe and main status and set navigation status for navigator accordingly
*/ */
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed) const bool stay_in_failsafe, status_flags_s *status_flags, bool landed)
{ {
navigation_state_t nav_state_old = status->nav_state; navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
status->failsafe = false; //status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */ /* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) { switch (internal_state->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO: case commander_state_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL: case commander_state_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_RATTITUDE: case commander_state_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL: case commander_state_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL: case commander_state_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */ /* require RC for all manual modes */
if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) { if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
@@ -628,28 +629,28 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
} else { } else {
switch (status->main_state) { switch (internal_state->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO: case commander_state_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break; break;
case vehicle_status_s::MAIN_STATE_MANUAL: case commander_state_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break; break;
case vehicle_status_s::MAIN_STATE_RATTITUDE: case commander_state_s::MAIN_STATE_RATTITUDE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
break; break;
case vehicle_status_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break; break;
case vehicle_status_s::MAIN_STATE_ALTCTL: case commander_state_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break; break;
case vehicle_status_s::MAIN_STATE_POSCTL: case commander_state_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break; break;
@@ -660,7 +661,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case commander_state_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe /* go into failsafe
* - if commanded to do so * - if commanded to do so
@@ -693,7 +694,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* datalink loss enabled: /* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */ * check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) { } else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
@@ -710,7 +711,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
* or RC is lost after the mission is finished: this should always trigger RCRECOVER */ * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
(status->rc_signal_lost && mission_finished))) { (status->rc_signal_lost && mission_finished))) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
@@ -728,13 +729,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER: case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */ /* go into failsafe on a engine failure */
if (status->engine_failure) { if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */ /* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) { } else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
@@ -748,7 +749,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* go into failsafe if RC is lost and datalink loss is not set up */ /* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) { } else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
@@ -771,14 +772,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_RTL: case commander_state_s::MAIN_STATE_AUTO_RTL:
/* require global position and home, also go into failsafe on an engine failure */ /* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) { if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status_flags->condition_global_position_valid || } else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) { !status_flags->condition_home_position_valid)) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_local_position_valid) { if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
@@ -792,17 +793,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET: case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
/* require global position and home */ /* require global position and home */
if (status->engine_failure) { if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (!status->condition_global_position_valid) { } else if (!status_flags->condition_global_position_valid) {
status->failsafe = true; status->failsafe = true;
if (status->condition_local_position_valid) { if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) { } else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else { } else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -819,7 +820,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status_flags->condition_global_position_valid || } else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) { !status_flags->condition_home_position_valid)) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_local_position_valid) { if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
@@ -833,14 +834,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case vehicle_status_s::MAIN_STATE_AUTO_LAND: case commander_state_s::MAIN_STATE_AUTO_LAND:
/* require global position and home */ /* require global position and home */
if (status->engine_failure) { if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status_flags->condition_global_position_valid || } else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) { !status_flags->condition_home_position_valid)) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_local_altitude_valid) { if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -852,14 +853,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case vehicle_status_s::MAIN_STATE_OFFBOARD: case commander_state_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */ /* require offboard control, otherwise stay where you are */
if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) { if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true; //status->failsafe = true;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) { } else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true; //status->failsafe = true;
if (status_flags->condition_local_position_valid) { if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
src/modules/commander/state_machine_helper.h
+4 -2
View File
@@ -46,6 +46,7 @@
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h> #include <uORB/topics/safety.h>
#include <uORB/topics/commander_state.h>
typedef enum { typedef enum {
TRANSITION_DENIED = -1, TRANSITION_DENIED = -1,
@@ -103,11 +104,12 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
transition_result_t transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
status_flags_s *status_flags); status_flags_s *status_flags, struct commander_state_s *internal_state);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub); transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed); const bool stay_in_failsafe, status_flags_s *status_flags, bool landed);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
src/modules/sdlog2/sdlog2.c
+14 -3
View File
@@ -114,6 +114,7 @@
#include <uORB/topics/camera_trigger.h> #include <uORB/topics/camera_trigger.h>
#include <uORB/topics/ekf2_replay.h> #include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/commander_state.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@@ -1159,6 +1160,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct camera_trigger_s camera_trigger; struct camera_trigger_s camera_trigger;
struct ekf2_replay_s replay; struct ekf2_replay_s replay;
struct vehicle_land_detected_s land_detected; struct vehicle_land_detected_s land_detected;
struct commander_state_s commander_state;
} buf; } buf;
memset(&buf, 0, sizeof(buf)); memset(&buf, 0, sizeof(buf));
@@ -1265,6 +1267,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int cam_trig_sub; int cam_trig_sub;
int replay_sub; int replay_sub;
int land_detected_sub; int land_detected_sub;
int commander_state_sub;
} subs; } subs;
subs.cmd_sub = -1; subs.cmd_sub = -1;
@@ -1305,6 +1308,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.cam_trig_sub = -1; subs.cam_trig_sub = -1;
subs.replay_sub = -1; subs.replay_sub = -1;
subs.land_detected_sub = -1; subs.land_detected_sub = -1;
subs.commander_state_sub = -1;
/* add new topics HERE */ /* add new topics HERE */
@@ -1424,6 +1428,10 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */ /* --- VEHICLE STATUS - LOG MANAGEMENT --- */
bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status); bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status);
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
bool commander_state_updated = copy_if_updated(ORB_ID(commander_state), &subs.commander_state_sub,
&buf.commander_state);
if (status_updated) { if (status_updated) {
if (log_when_armed) { if (log_when_armed) {
handle_status(&buf_status); handle_status(&buf_status);
@@ -1447,10 +1455,13 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TIME.t = hrt_absolute_time(); log_msg.body.log_TIME.t = hrt_absolute_time();
LOGBUFFER_WRITE_AND_COUNT(TIME); LOGBUFFER_WRITE_AND_COUNT(TIME);
/* --- VEHICLE STATUS --- */ /* --- VEHICLE STATUS / COMMANDER DEBUGGING --- */
if (status_updated) { if (status_updated || commander_state_updated) {
log_msg.msg_type = LOG_STAT_MSG; log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = buf_status.main_state; // TODO: This field should get DEPRECATED in favor of nav_state. main_state is only for
// commander debugging.
log_msg.body.log_STAT.main_state = buf.commander_state.main_state;
log_msg.body.log_STAT.nav_state = buf_status.nav_state;
log_msg.body.log_STAT.arming_state = buf_status.arming_state; log_msg.body.log_STAT.arming_state = buf_status.arming_state;
log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
log_msg.body.log_STAT.load = buf_status.load; log_msg.body.log_STAT.load = buf_status.load;
src/modules/sdlog2/sdlog2_messages.h
+3 -2
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -178,6 +178,7 @@ struct log_ATTC_s {
#define LOG_STAT_MSG 10 #define LOG_STAT_MSG 10
struct log_STAT_s { struct log_STAT_s {
uint8_t main_state; uint8_t main_state;
uint8_t nav_state;
uint8_t arming_state; uint8_t arming_state;
uint8_t failsafe; uint8_t failsafe;
float load; float load;
@@ -627,7 +628,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBf", "MainState,ArmS,Failsafe,Load"), LOG_FORMAT(STAT, "BBBBf", "MainState,NavState,ArmS,Failsafe,Load"),
LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"), LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
src/modules/uORB/objects_common.cpp
+3
View File
@@ -288,3 +288,6 @@ ORB_DEFINE(mavlink_log, struct mavlink_log_s);
#include "topics/follow_target.h" #include "topics/follow_target.h"
ORB_DEFINE(follow_target, struct follow_target_s); ORB_DEFINE(follow_target, struct follow_target_s);
#include "topics/commander_state.h"
ORB_DEFINE(commander_state, struct commander_state_s);