fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 13:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: switch all state_machine_helper functions to pass by reference

Browse Source
This commit is contained in:
Matthias Grob
2021-03-30 20:48:33 +02:00
committed by Daniel Agar
parent d97fba67e5
commit 531de5c588
4 changed files with 274 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+70 -70
View File
@@ -395,8 +395,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
bool Commander::shutdown_if_allowed()
{
return TRANSITION_DENIED != arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN,
&_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags, _arm_requirements,
return TRANSITION_DENIED != arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN,
_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN);
}
@@ -501,13 +501,13 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
}
}
transition_result_t arming_res = arming_state_transition(&_status,
transition_result_t arming_res = arming_state_transition(_status,
_safety,
vehicle_status_s::ARMING_STATE_ARMED,
&_armed,
_armed,
run_preflight_checks,
&_mavlink_log_pub,
&_status_flags,
_status_flags,
_arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason);
@@ -525,13 +525,13 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason)
{
transition_result_t arming_res = arming_state_transition(&_status,
transition_result_t arming_res = arming_state_transition(_status,
_safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&_armed,
_armed,
false,
&_mavlink_log_pub,
&_status_flags,
_status_flags,
_arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason);
@@ -550,7 +550,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason)
transition_result_t
Commander::try_mode_change(main_state_t desired_mode)
{
transition_result_t res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
transition_result_t res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
if (res == TRANSITION_DENIED) {
@@ -563,51 +563,51 @@ Commander::try_mode_change(main_state_t desired_mode)
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_RTL && (res == TRANSITION_DENIED)) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LAND && (res == TRANSITION_DENIED)) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF && (res == TRANSITION_DENIED)) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET && (res == TRANSITION_DENIED)) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LOITER && (res == TRANSITION_DENIED)) {
/* fall back to position control */
desired_mode = commander_state_s::MAIN_STATE_POSCTL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_POSCTL && (res == TRANSITION_DENIED)) {
/* fall back to altitude control */
desired_mode = commander_state_s::MAIN_STATE_ALTCTL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_ALTCTL && (res == TRANSITION_DENIED)) {
/* fall back to stabilized */
desired_mode = commander_state_s::MAIN_STATE_STAB;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_STAB && (res == TRANSITION_DENIED)) {
/* fall back to manual */
desired_mode = commander_state_s::MAIN_STATE_MANUAL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
res = main_state_transition(_status, desired_mode, _status_flags, _internal_state);
}
}
@@ -671,7 +671,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Check if a mode switch had been requested
if ((((uint32_t)cmd.param2) & 1) > 0) {
transition_result_t main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
_status_flags, &_internal_state);
_status_flags, _internal_state);
if ((main_ret != TRANSITION_DENIED)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -698,16 +698,16 @@ Commander::handle_command(const vehicle_command_s &cmd)
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
reset_posvel_validity();
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
@@ -716,12 +716,12 @@ Commander::handle_command(const vehicle_command_s &cmd)
switch (custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
if (_status_flags.condition_auto_mission_available) {
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state);
} else {
main_ret = TRANSITION_DENIED;
@@ -730,24 +730,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags, _internal_state);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, _status_flags,
&_internal_state);
_internal_state);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags, _internal_state);
break;
default:
@@ -757,42 +757,42 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
} else {
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state);
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
reset_posvel_validity();
/* OFFBOARD */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state);
}
} else {
/* use base mode */
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state);
} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else {
/* MANUAL */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
}
}
}
@@ -976,7 +976,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
/* switch to RTL which ends the mission */
if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
&_internal_state)) {
_internal_state)) {
mavlink_log_info(&_mavlink_log_pub, "Returning to launch");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -990,7 +990,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags,
&_internal_state)) {
_internal_state)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
@@ -1005,7 +1005,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags,
&_internal_state)) {
_internal_state)) {
mavlink_log_info(&_mavlink_log_pub, "Landing at current position");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1018,7 +1018,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags,
&_internal_state)) {
_internal_state)) {
mavlink_log_info(&_mavlink_log_pub, "Precision landing");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1041,7 +1041,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
// switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags,
&_internal_state))
_internal_state))
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::MISSION_START))) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1071,7 +1071,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Switch to orbit state and let the orbit task handle the command further
if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags,
&_internal_state)) {
_internal_state)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@@ -1140,8 +1140,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&_status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &_armed,
false /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags,
if (TRANSITION_DENIED == arming_state_transition(_status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, _armed,
false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL))
@@ -2006,8 +2006,8 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */
if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags,
arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY);
}
@@ -2061,10 +2061,10 @@ Commander::run()
&& mission_result.finished) {
if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state);
} else {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
}
}
}
@@ -2097,7 +2097,7 @@ Commander::run()
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags,
&_internal_state)) {
_internal_state)) {
_geofence_loiter_on = true;
}
@@ -2106,7 +2106,7 @@ Commander::run()
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
&_internal_state)) {
_internal_state)) {
_geofence_rtl_on = true;
}
@@ -2115,7 +2115,7 @@ Commander::run()
case (geofence_result_s::GF_ACTION_LAND) : {
if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags,
&_internal_state)) {
_internal_state)) {
_geofence_land_on = true;
}
@@ -2235,7 +2235,7 @@ Commander::run()
&& !in_low_battery_failsafe && !_geofence_warning_action_on
&& _manual_control.wantsOverride(_vehicle_control_mode)) {
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
&_internal_state) == TRANSITION_CHANGED) {
_internal_state) == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
_status_changed = true;
@@ -2385,7 +2385,7 @@ Commander::run()
if (_status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
&& _status_flags.condition_global_position_valid) {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
}
}
@@ -2506,9 +2506,9 @@ Commander::run()
}
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&_status,
&_armed,
&_internal_state,
bool nav_state_changed = set_nav_state(_status,
_armed,
_internal_state,
&_mavlink_log_pub,
(link_loss_actions_t)_param_nav_dll_act.get(),
_mission_result_sub.get().finished,
@@ -2869,7 +2869,7 @@ transition_result_t
Commander::set_main_state_override_on(bool *changed)
{
const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags,
&_internal_state);
_internal_state);
*changed = (res == TRANSITION_CHANGED);
return res;
@@ -2939,7 +2939,7 @@ Commander::set_main_state_rc()
/* offboard switch overrides main switch */
if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state);
if (res == TRANSITION_DENIED) {
print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD);
@@ -2965,7 +2965,7 @@ Commander::set_main_state_rc()
/* Loiter switch overrides main switch */
if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
if (res == TRANSITION_DENIED) {
print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER);
@@ -3016,17 +3016,17 @@ Commander::set_main_state_rc()
* for any non-manual mode
*/
if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_status.is_vtol) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
}
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
}
} else {
@@ -3035,21 +3035,21 @@ Commander::set_main_state_rc()
* - Manual is not default anymore when the manual switch is assigned
*/
if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
} else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
// default to MANUAL when no manual switch is set
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} else {
// default to STAB when the manual switch is assigned (but off)
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
}
}
@@ -3682,7 +3682,7 @@ void Commander::battery_status_check()
// execute battery failsafe if the state has gotten worse while we are armed
if (battery_warning_level_increased_while_armed) {
battery_failsafe(&_mavlink_log_pub, _status, _status_flags, &_internal_state, _battery_warning,
battery_failsafe(&_mavlink_log_pub, _status, _status_flags, _internal_state, _battery_warning,
(low_battery_action_t)_param_com_low_bat_act.get());
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+3 -3
View File
@@ -300,10 +300,10 @@ bool StateMachineHelperTest::armingStateTransitionTest()
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed,
transition_result_t result = arming_state_transition(status, safety, test->requested_state, armed,
true /* enable pre-arm checks */,
nullptr /* no mavlink_log_pub */,
&status_flags,
status_flags,
arm_req,
2e6, /* 2 seconds after boot, everything should be checked */
arm_disarm_reason_t::UNIT_TEST);
@@ -517,7 +517,7 @@ bool StateMachineHelperTest::mainStateTransitionTest()
// Attempt transition
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,
&current_commander_state);
current_commander_state);
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);
src/modules/commander/state_machine_helper.cpp
+193 -193
View File
@@ -113,27 +113,27 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, link_loss_actions_t link_loss_act,
void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act,
const float ll_delay);
void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act);
void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, const link_loss_actions_t link_loss_act);
void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const offboard_loss_actions_t offboard_loss_act);
void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const offboard_loss_rc_actions_t offboard_loss_rc_act);
void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe,
void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsafe,
const offboard_loss_actions_t offboard_loss_act,
const offboard_loss_rc_actions_t offboard_loss_rc_act);
transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety,
const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags,
transition_result_t arming_state_transition(vehicle_status_s &status, const safety_s &safety,
const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason)
{
@@ -143,10 +143,10 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
"ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
arming_state_t current_arming_state = status.arming_state;
bool feedback_provided = false;
const bool hil_enabled = (status->hil_state == vehicle_status_s::HIL_STATE_ON);
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
@@ -163,18 +163,18 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !hil_enabled) {
preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, true, true,
preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, true, true,
time_since_boot);
if (preflight_check_ret) {
status_flags->condition_system_sensors_initialized = true;
status_flags.condition_system_sensors_initialized = true;
}
feedback_provided = true;
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status_flags->condition_system_sensors_initialized
if (!status_flags.condition_system_sensors_initialized
&& fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
@@ -182,8 +182,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
status_flags->condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, *status,
*status_flags, false, status->arming_state != vehicle_status_s::ARMING_STATE_ARMED,
status_flags.condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status,
status_flags, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
time_since_boot);
last_preflight_check = hrt_absolute_time();
@@ -191,7 +191,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
@@ -199,13 +199,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
// Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
bool prearm_check_ret = true;
if (fRunPreArmChecks && preflight_check_ret) {
// only bother running prearm if preflight was successful
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, *status_flags, safety, arm_requirements, *status);
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, safety, arm_requirements, status);
}
if (!preflight_check_ret || !prearm_check_ret) {
@@ -219,12 +219,12 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
if (hil_enabled) {
/* enforce lockdown in HIL */
armed->lockdown = true;
status_flags->condition_system_sensors_initialized = true;
armed.lockdown = true;
status_flags.condition_system_sensors_initialized = true;
/* recover from a prearm fail */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
// HIL can always go to standby
@@ -235,10 +235,10 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
if (!hil_enabled &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
(status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
// Sensors need to be initialized for STANDBY state, except for HIL
if (!status_flags->condition_system_sensors_initialized) {
if (!status_flags.condition_system_sensors_initialized) {
feedback_provided = true;
valid_transition = false;
}
@@ -246,25 +246,25 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
// Finish up the state transition
if (valid_transition) {
bool was_armed = armed->armed;
armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
bool was_armed = armed.armed;
armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
status.arming_state = new_arming_state;
if (was_armed && !armed->armed) { // disarm transition
status->latest_disarming_reason = (uint8_t)calling_reason;
if (was_armed && !armed.armed) { // disarm transition
status.latest_disarming_reason = (uint8_t)calling_reason;
} else if (!was_armed && armed->armed) { // arm transition
status->latest_arming_reason = (uint8_t)calling_reason;
} else if (!was_armed && armed.armed) { // arm transition
status.latest_arming_reason = (uint8_t)calling_reason;
}
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
status->armed_time = hrt_absolute_time();
status.armed_time = hrt_absolute_time();
} else {
status->armed_time = 0;
status.armed_time = 0;
}
}
}
@@ -273,7 +273,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
/* print to MAVLink and console if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s",
arming_state_names[status->arming_state], arming_state_names[new_arming_state]);
arming_state_names[status.arming_state], arming_state_names[new_arming_state]);
}
}
@@ -282,7 +282,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
transition_result_t
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state)
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state)
{
// IMPORTANT: The assumption of callers of this function is that the execution of
// this check is essentially "free". Therefore any runtime checking in here has to be
@@ -396,10 +396,10 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
}
if (ret == TRANSITION_CHANGED) {
if (internal_state->main_state != new_main_state) {
internal_state->main_state = new_main_state;
internal_state->main_state_changes++;
internal_state->timestamp = hrt_absolute_time();
if (internal_state.main_state != new_main_state) {
internal_state.main_state = new_main_state;
internal_state.main_state_changes++;
internal_state.timestamp = hrt_absolute_time();
} else {
ret = TRANSITION_NOT_CHANGED;
@@ -412,25 +412,25 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
/**
* Enable failsafe and report to user
*/
void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason)
void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason)
{
if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (!old_failsafe && status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// make sure intermittent failsafes don't lead to infinite delay by not constantly reseting the timestamp
if (status->failsafe_timestamp == 0 ||
hrt_elapsed_time(&status->failsafe_timestamp) > 30_s) {
status->failsafe_timestamp = hrt_absolute_time();
if (status.failsafe_timestamp == 0 ||
hrt_elapsed_time(&status.failsafe_timestamp) > 30_s) {
status.failsafe_timestamp = hrt_absolute_time();
}
mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason);
}
status->failsafe = true;
status.failsafe = true;
}
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state,
bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state,
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
@@ -438,15 +438,15 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
const position_nav_loss_actions_t posctl_nav_loss_act,
const float param_com_rcl_act_t)
{
const navigation_state_t nav_state_old = status->nav_state;
const navigation_state_t nav_state_old = status.nav_state;
const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED;
const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;
const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost);
const bool rc_lost = rc_loss_act_configured && (status.rc_signal_lost);
bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED);
bool old_failsafe = status->failsafe;
status->failsafe = false;
bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
bool old_failsafe = status.failsafe;
status.failsafe = false;
// Safe to do reset flags here, as if loss state persists flags will be restored in the code below
reset_link_loss_globals(armed, old_failsafe, rc_loss_act);
@@ -454,7 +454,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act);
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (internal_state->main_state) {
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_STAB:
@@ -467,25 +467,25 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
} else {
switch (internal_state->main_state) {
switch (internal_state.main_state) {
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;
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;
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;
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;
default:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
@@ -506,11 +506,11 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
* For fixedwing, a global position is needed. */
} else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
rc_fallback_allowed, status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
rc_fallback_allowed, status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
}
}
break;
@@ -524,30 +524,30 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags.vtol_transition_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (status->mission_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (status.mission_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (status->data_link_lost && data_link_loss_act_configured
} else if (status.data_link_lost && data_link_loss_act_configured
&& is_armed && !landed) {
// Data link lost, data link loss reaction configured -> do configured reaction
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
} else if (status->rc_signal_lost && rc_loss_act_configured && status_flags.rc_signal_found_once
} else if (status.rc_signal_lost && rc_loss_act_configured && status_flags.rc_signal_found_once
&& is_armed && !landed) {
// RC link lost, rc loss reaction configured, RC was used before -> RC loss reaction after delay
// Safety pilot expects to be able to take over by RC in case anything unexpected happens
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
} else if (status->rc_signal_lost && rc_loss_act_configured
&& status->data_link_lost && !data_link_loss_act_configured
} else if (status.rc_signal_lost && rc_loss_act_configured
&& status.data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed) {
// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
// Lost all communication, by default it's considered unsafe to continue the mission
@@ -555,8 +555,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else if (status->rc_signal_lost && !rc_loss_act_configured
&& status->data_link_lost && !data_link_loss_act_configured
} else if (status.rc_signal_lost && !rc_loss_act_configured
&& status.data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed
&& mission_finished) {
// All links lost, all link loss reactions disabled -> return after mission
@@ -566,7 +566,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
} else if (!stay_in_failsafe) {
// normal mission operation if there's no need to stay in failsafe
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
break;
@@ -574,32 +574,32 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
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;
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
/* also go into failsafe if just datalink is lost, and we're actually in air */
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
} else if (rc_lost && !data_link_loss_act_configured && status->data_link_lost && is_armed) {
} else if (rc_lost && !data_link_loss_act_configured && status.data_link_lost && is_armed) {
/* go into failsafe if RC is lost and datalink is lost and datalink loss is not set up */
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else if (status->rc_signal_lost) {
} else if (status.rc_signal_lost) {
/* don't bother if RC is lost if datalink is connected */
/* this mode is ok, we don't need RC for LOITERing */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
@@ -608,13 +608,13 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
/* 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;
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
@@ -623,35 +623,35 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
/* require global position and home */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
}
break;
case commander_state_s::MAIN_STATE_ORBIT:
if (status->engine_failure) {
if (status.engine_failure) {
// failsafe: on engine failure
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
// Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// failsafe: just datalink is lost and we're in air
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
@@ -659,9 +659,9 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (rc_lost && status->data_link_lost && !data_link_loss_act_configured && is_armed) {
} else if (rc_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) {
// Orbit does not depend on RC but while armed & all links lost & when datalink loss is not set up, we failsafe
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
@@ -669,11 +669,11 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
// Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit
// is not possible and therefore the internal_state needs to be adjusted.
internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL;
internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
} else {
// no failsafe, RC is not mandatory for orbit
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
}
break;
@@ -682,14 +682,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
/* require local position */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
}
break;
@@ -698,14 +698,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
/* require local position */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
break;
@@ -714,14 +714,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
/* must be rotary wing plus same requirements as normal landing */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
}
break;
@@ -729,7 +729,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
case commander_state_s::MAIN_STATE_OFFBOARD:
if (status_flags.offboard_control_signal_lost) {
if (status->rc_signal_lost) {
if (status.rc_signal_lost) {
// Offboard and RC are lost
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard);
set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act);
@@ -741,17 +741,17 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}
default:
break;
}
return status->nav_state != nav_state_old;
return status.nav_state != nav_state_old;
}
bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos)
{
bool fallback_required = false;
@@ -768,33 +768,33 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or
if (fallback_required) {
if (use_rc) {
// fallback to a mode that gives the operator stick control
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& status_flags.condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status_flags.condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
}
} else {
// go into a descent that does not require stick control
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_flags.condition_local_altitude_valid) {
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}
@@ -811,11 +811,11 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or
}
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, link_loss_actions_t link_loss_act,
void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act,
const float ll_delay)
{
if (hrt_elapsed_time(&status->failsafe_timestamp) < (ll_delay * 1_s)
if (hrt_elapsed_time(&status.failsafe_timestamp) < (ll_delay * 1_s)
&& link_loss_act != link_loss_actions_t::DISABLED) {
// delay failsafe reaction by trying to hold position if configured
link_loss_act = link_loss_actions_t::AUTO_LOITER;
@@ -829,67 +829,67 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
case link_loss_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return;
}
// FALLTHROUGH
case link_loss_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
return;
}
// FALLTHROUGH
case link_loss_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return;
} else {
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
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;
return;
} 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;
return;
}
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
return;
}
}
// FALLTHROUGH
case link_loss_actions_t::TERMINATE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed->force_failsafe = true;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed.force_failsafe = true;
break;
case link_loss_actions_t::LOCKDOWN:
armed->lockdown = true;
armed.lockdown = true;
break;
}
}
void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act)
void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, const link_loss_actions_t link_loss_act)
{
if (old_failsafe) {
if (link_loss_act == link_loss_actions_t::TERMINATE) {
armed->force_failsafe = false;
armed.force_failsafe = false;
} else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
armed->lockdown = false;
armed.lockdown = false;
}
}
}
void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const offboard_loss_actions_t offboard_loss_act)
{
@@ -899,51 +899,51 @@ void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *arm
return;
case offboard_loss_actions_t::TERMINATE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed->force_failsafe = true;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed.force_failsafe = true;
return;
case offboard_loss_actions_t::LOCKDOWN:
armed->lockdown = true;
armed.lockdown = true;
return;
case offboard_loss_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return;
}
// FALLTHROUGH
case offboard_loss_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
return;
}
// FALLTHROUGH
case offboard_loss_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return;
}
}
// If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) {
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}
void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const offboard_loss_rc_actions_t offboard_loss_rc_act)
{
@@ -953,55 +953,55 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *
return;
case offboard_loss_rc_actions_t::TERMINATE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed->force_failsafe = true;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed.force_failsafe = true;
return;
case offboard_loss_rc_actions_t::LOCKDOWN:
armed->lockdown = true;
armed.lockdown = true;
return;
case offboard_loss_rc_actions_t::MANUAL_POSITION:
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& status_flags.condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
return;
} else if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
return;
}
// FALLTHROUGH
case offboard_loss_rc_actions_t::MANUAL_ALTITUDE:
if (status_flags.condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
return;
}
// FALLTHROUGH
case offboard_loss_rc_actions_t::MANUAL_ATTITUDE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
return;
case offboard_loss_rc_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return;
}
// FALLTHROUGH
case offboard_loss_rc_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
return;
}
// FALLTHROUGH
case offboard_loss_rc_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return;
}
}
@@ -1009,39 +1009,39 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *
// If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) {
//TODO: Add case for rover
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}
void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe,
void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsafe,
const offboard_loss_actions_t offboard_loss_act,
const offboard_loss_rc_actions_t offboard_loss_rc_act)
{
if (old_failsafe) {
if (offboard_loss_act == offboard_loss_actions_t::TERMINATE
|| offboard_loss_rc_act == offboard_loss_rc_actions_t::TERMINATE) {
armed->force_failsafe = false;
armed.force_failsafe = false;
}
if (offboard_loss_act == offboard_loss_actions_t::LOCKDOWN
|| offboard_loss_rc_act == offboard_loss_rc_actions_t::LOCKDOWN) {
armed->lockdown = false;
armed.lockdown = false;
}
}
}
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning,
const low_battery_action_t low_battery_action)
{
switch (battery_warning) {
@@ -1067,20 +1067,20 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case LOW_BAT_ACTION::RETURN_OR_LAND:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state->timestamp = hrt_absolute_time();
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical);
}
} else {
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical);
}
}
@@ -1088,10 +1088,10 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::LAND:
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical);
}
@@ -1111,19 +1111,19 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
case LOW_BAT_ACTION::RETURN:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state->timestamp = hrt_absolute_time();
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
internal_state.timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_dangerous);
}
} else {
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_dangerous);
}
}
@@ -1134,10 +1134,10 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
// FALLTHROUGH
case LOW_BAT_ACTION::LAND:
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state->timestamp = hrt_absolute_time();
if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
internal_state.timestamp = hrt_absolute_time();
mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_dangerous);
}
src/modules/commander/state_machine_helper.h
+8 -8
View File
@@ -116,18 +116,18 @@ enum class arm_disarm_reason_t {
};
transition_result_t
arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state,
actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s *status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
transition_result_t
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state);
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);
void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason);
void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason);
bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state,
bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state,
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
@@ -139,7 +139,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
* Checks the validty of position data against the requirements of the current navigation
* mode and switches mode if position data required is not available.
*/
bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
@@ -152,7 +152,7 @@ typedef enum LOW_BAT_ACTION {
} low_battery_action_t;
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning,
const low_battery_action_t low_bat_action);
#endif /* STATE_MACHINE_HELPER_H_ */