fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 05:37:36 +08:00
Code Issues 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.
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 arming_state # current arming 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;
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 */
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
/* indicate main control state */
if (vehicle_control_mode.flag_control_auto_enabled) {
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;
} 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;
} 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 home_position_s _home;
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 manual_control_setpoint_s _last_sp_man = {};
@@ -579,7 +580,7 @@ void print_status()
const char *armed_str;
switch (state.arming_state) {
switch (status.arming_state) {
case vehicle_status_s::ARMING_STATE_INIT:
armed_str = "INIT";
break;
@@ -698,34 +699,34 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_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) {
/* 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) {
/* 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) {
/* AUTO */
if (custom_sub_mode > 0) {
switch(custom_sub_mode) {
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;
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;
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;
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;
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;
case PX4_CUSTOM_SUB_MODE_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 {
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) {
/* 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) {
/* 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) {
/* 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) {
/* 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 {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* 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) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* 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) {
/* 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 {
/* 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
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 {
@@ -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: {
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) {
main_state_pre_offboard = status_local->main_state;
if (internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = internal_state.main_state;
}
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) {
print_reject_mode(status_local, "OFFBOARD");
@@ -1000,7 +1001,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else {
/* If the mavlink command is used to enable or disable offboard control:
* 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;
}
}
@@ -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: {
/* 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!");
} else {
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: {
/* 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!");
} else {
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
// until we are sure we really don't need them.
// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
// main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL";
// main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL";
// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
// main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO";
// main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB";
// main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
// const char *main_states_str[commander_state_s::MAIN_STATE_MAX];
// main_states_str[commander_state_s::MAIN_STATE_MANUAL] = "MANUAL";
// main_states_str[commander_state_s::MAIN_STATE_ALTCTL] = "ALTCTL";
// main_states_str[commander_state_s::MAIN_STATE_POSCTL] = "POSCTL";
// main_states_str[commander_state_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
// main_states_str[commander_state_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
// main_states_str[commander_state_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
// main_states_str[commander_state_s::MAIN_STATE_ACRO] = "ACRO";
// main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB";
// main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
// const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
// 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
status_flags.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL;
main_state_prev = commander_state_s::MAIN_STATE_MAX;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
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_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
if (geofence_result.geofence_violated) {
@@ -2164,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[])
break;
}
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;
}
break;
}
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;
}
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
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);
// reset if no longer in RTL or if manually switched to RTL
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);
if (!geofence_loiter_on && !geofence_rtl_on) {
// 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
if ((geofence_loiter_on || geofence_rtl_on) &&
(geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL ||
geofence_main_state_before_violation == vehicle_status_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_MANUAL ||
geofence_main_state_before_violation == commander_state_s::MAIN_STATE_ALTCTL ||
geofence_main_state_before_violation == commander_state_s::MAIN_STATE_POSCTL ||
geofence_main_state_before_violation == commander_state_s::MAIN_STATE_ACRO ||
geofence_main_state_before_violation == commander_state_s::MAIN_STATE_STAB)) {
// transition to previous state if sticks are increased
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.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 */
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.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE ||
(internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
land_detector.landed) &&
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.
*/
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL)
&& (status.main_state != vehicle_status_s::MAIN_STATE_ACRO)
&& (status.main_state != vehicle_status_s::MAIN_STATE_STAB)
&& (status.main_state != vehicle_status_s::MAIN_STATE_ALTCTL)) {
if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)
&& (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)
&& (internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
&& (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)) {
print_reject_arm("NOT ARMING: Switch to a manual mode first.");
} 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 */
/* only switch back to at least altitude controlled modes */
if (main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL ||
main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) {
if (main_state_prev == commander_state_s::MAIN_STATE_POSCTL ||
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) ||
(status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND
&& 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
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE &&
status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status_flags.gps_failure) ||
(status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) {
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
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state ==vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status_flags.gps_failure) ||
(status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) {
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 */
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.stay_in_failsafe,
&status_flags,
@@ -2833,7 +2836,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (set_normal_color) {
/* set color */
if (status_local->failsafe) {
if (status.failsafe) {
rgbled_set_color(RGBLED_COLOR_PURPLE);
} else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
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
/* if offboard is set already by a mavlink command, abort */
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 */
@@ -2924,7 +2927,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switch overrides main switch */
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) {
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 */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_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) {
print_reject_mode(status_local, "AUTO RTL");
/* 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) {
@@ -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
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) {
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 {
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
* rattitude mode.*/
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 {
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 {
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
@@ -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
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) {
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
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) {
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
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
break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
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) {
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");
} 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) {
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");
// 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) {
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
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) {
break; // changed successfully or already in this state
}
// 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) {
break; // changed successfully or already in this state
}
// 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
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
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 may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_ACRO:
case commander_state_s::MAIN_STATE_RATTITUDE:
case commander_state_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
case commander_state_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
@@ -371,7 +371,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
case commander_state_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status_flags->condition_local_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;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status_flags->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
case vehicle_status_s::MAIN_STATE_AUTO_LAND:
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
case commander_state_s::MAIN_STATE_AUTO_MISSION:
case commander_state_s::MAIN_STATE_AUTO_RTL:
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
case commander_state_s::MAIN_STATE_AUTO_LAND:
/* need global position and home position */
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
case commander_state_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
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;
case vehicle_status_s::MAIN_STATE_MAX:
case commander_state_s::MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
main_state_prev = status->main_state;
status->main_state = new_main_state;
if (internal_state->main_state != new_main_state) {
main_state_prev = internal_state->main_state;
internal_state->main_state = new_main_state;
} else {
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
*/
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)
{
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);
status->failsafe = false;
//status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
switch (internal_state->main_state) {
case commander_state_s::MAIN_STATE_ACRO:
case commander_state_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_RATTITUDE:
case commander_state_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_ALTCTL:
case commander_state_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
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) {
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 {
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
switch (internal_state->main_state) {
case commander_state_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case vehicle_status_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case commander_state_s::MAIN_STATE_RATTITUDE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
break;
case vehicle_status_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
case commander_state_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
case commander_state_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
@@ -660,7 +661,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case commander_state_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - 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:
* check for datalink lost: this should always trigger RTGS */
} 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) {
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 */
} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
(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) {
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;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} 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) {
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 */
} 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) {
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;
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 */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
//status->failsafe = true;
if (status_flags->condition_local_position_valid) {
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;
case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
/* require global position and home */
if (status->engine_failure) {
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;
if (status->condition_local_position_valid) {
if (status_flags->condition_local_position_valid) {
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;
} else {
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;
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
//status->failsafe = true;
if (status_flags->condition_local_position_valid) {
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;
case vehicle_status_s::MAIN_STATE_AUTO_LAND:
case commander_state_s::MAIN_STATE_AUTO_LAND:
/* require global position and home */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
//status->failsafe = true;
if (status_flags->condition_local_altitude_valid) {
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;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
case commander_state_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
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;
} 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) {
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/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/commander_state.h>
typedef enum {
TRANSITION_DENIED = -1,
@@ -103,11 +104,12 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
transition_result_t
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);
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);
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/ekf2_replay.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/commander_state.h>
#include <systemlib/systemlib.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 ekf2_replay_s replay;
struct vehicle_land_detected_s land_detected;
struct commander_state_s commander_state;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1265,6 +1267,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int cam_trig_sub;
int replay_sub;
int land_detected_sub;
int commander_state_sub;
} subs;
subs.cmd_sub = -1;
@@ -1305,6 +1308,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.cam_trig_sub = -1;
subs.replay_sub = -1;
subs.land_detected_sub = -1;
subs.commander_state_sub = -1;
/* add new topics HERE */
@@ -1424,6 +1428,10 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
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 (log_when_armed) {
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();
LOGBUFFER_WRITE_AND_COUNT(TIME);
/* --- VEHICLE STATUS --- */
if (status_updated) {
/* --- VEHICLE STATUS / COMMANDER DEBUGGING --- */
if (status_updated || commander_state_updated) {
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.failsafe = (uint8_t) buf_status.failsafe;
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
* modification, are permitted provided that the following conditions
@@ -178,6 +178,7 @@ struct log_ATTC_s {
#define LOG_STAT_MSG 10
struct log_STAT_s {
uint8_t main_state;
uint8_t nav_state;
uint8_t arming_state;
uint8_t failsafe;
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_S(ATTC, 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(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"),
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"
ORB_DEFINE(follow_target, struct follow_target_s);
#include "topics/commander_state.h"
ORB_DEFINE(commander_state, struct commander_state_s);