From 531de5c588ed2ba9d06b630147a59ba1f3668e3c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 30 Mar 2021 20:48:33 +0200 Subject: [PATCH] Commander: switch all state_machine_helper functions to pass by reference --- src/modules/commander/Commander.cpp | 140 +++---- .../state_machine_helper_test.cpp | 6 +- .../commander/state_machine_helper.cpp | 386 +++++++++--------- src/modules/commander/state_machine_helper.h | 16 +- 4 files changed, 274 insertions(+), 274 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 52ce69e1d1..ae14d5b819 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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()); } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 44a3f63a48..177c65463e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -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, - ¤t_commander_state); + current_commander_state); // Validate result of transition ut_compare(test->assertMsg, test->expected_transition_result, result); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6451ff6d94..0582679980 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b415a7a9dc..657bfed2ab 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -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_ */