diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6c65d9113c..69cf6e66f1 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -382,57 +382,113 @@ bool Commander::shutdown_if_allowed() hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN); } -transition_result_t -Commander::arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason) +static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + switch (calling_reason) { + case arm_disarm_reason_t::TRANSITION_TO_STANDBY: return ""; - // Transition the armed state. By passing _mavlink_log_pub to arming_state_transition it will - // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&_status, - _safety, - arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, - &_armed, - run_preflight_checks, - &_mavlink_log_pub, - &_status_flags, - _arm_requirements, - hrt_elapsed_time(&_boot_timestamp), calling_reason); + case arm_disarm_reason_t::RC_STICK: return "RC"; - if (arming_res == TRANSITION_CHANGED) { - const char *reason = ""; + case arm_disarm_reason_t::RC_SWITCH: return "RC (switch)"; - switch (calling_reason) { - case arm_disarm_reason_t::TRANSITION_TO_STANDBY: reason = ""; break; + case arm_disarm_reason_t::COMMAND_INTERNAL: return "internal command"; - case arm_disarm_reason_t::RC_STICK: reason = "RC"; break; + case arm_disarm_reason_t::COMMAND_EXTERNAL: return "external command"; - case arm_disarm_reason_t::RC_SWITCH: reason = "RC (switch)"; break; + case arm_disarm_reason_t::MISSION_START: return "mission start"; - case arm_disarm_reason_t::COMMAND_INTERNAL: reason = "internal command"; break; + case arm_disarm_reason_t::SAFETY_BUTTON: return "safety button"; - case arm_disarm_reason_t::COMMAND_EXTERNAL: reason = "external command"; break; + case arm_disarm_reason_t::AUTO_DISARM_LAND: return "landing"; - case arm_disarm_reason_t::MISSION_START: reason = "mission start"; break; + case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: return "auto preflight disarming"; - case arm_disarm_reason_t::SAFETY_BUTTON: reason = "safety button"; break; + case arm_disarm_reason_t::KILL_SWITCH: return "kill-switch"; - case arm_disarm_reason_t::AUTO_DISARM_LAND: reason = "landing"; break; + case arm_disarm_reason_t::LOCKDOWN: return "lockdown"; - case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: reason = "auto preflight disarming"; break; + case arm_disarm_reason_t::FAILURE_DETECTOR: return "failure detector"; - case arm_disarm_reason_t::KILL_SWITCH: reason = "kill-switch"; break; + case arm_disarm_reason_t::SHUTDOWN: return "shutdown request"; - case arm_disarm_reason_t::LOCKDOWN: reason = "lockdown"; break; + case arm_disarm_reason_t::UNIT_TEST: return "unit tests"; + } - case arm_disarm_reason_t::FAILURE_DETECTOR: reason = "failure detector"; break; + return ""; +}; - case arm_disarm_reason_t::SHUTDOWN: reason = "shutdown request"; break; +transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) +{ + // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming + if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + run_preflight_checks = false; + } - case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break; + if (run_preflight_checks) { + if (_vehicle_control_mode.flag_control_manual_enabled) { + const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f); + const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f); + + if (_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_center) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered"); + tune_negative(true); + return TRANSITION_DENIED; + + } + + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_low) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero"); + tune_negative(true); + return TRANSITION_DENIED; + } } - mavlink_log_info(&_mavlink_log_pub, "%s by %s", arm ? "Armed" : "Disarmed", reason); + if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL) + && !_status_flags.condition_home_position_valid) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home"); + tune_negative(true); + return TRANSITION_DENIED; + } + } + + transition_result_t arming_res = arming_state_transition(&_status, + _safety, + vehicle_status_s::ARMING_STATE_ARMED, + &_armed, + run_preflight_checks, + &_mavlink_log_pub, + &_status_flags, + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), calling_reason); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(&_mavlink_log_pub, "Armed by %s", arm_disarm_reason_str(calling_reason)); + + _status_changed = true; + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; +} + +transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason) +{ + transition_result_t arming_res = arming_state_transition(&_status, + _safety, + vehicle_status_s::ARMING_STATE_STANDBY, + &_armed, + false, + &_mavlink_log_pub, + &_status_flags, + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), calling_reason); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s", arm_disarm_reason_str(calling_reason)); + + _status_changed = true; } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -651,17 +707,18 @@ Commander::handle_command(const vehicle_command_s &cmd) // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters - if (static_cast(cmd.param1 + 0.5f) != 0 && static_cast(cmd.param1 + 0.5f) != 1) { + const int param1_arm = static_cast(roundf(cmd.param1)); + + if (param1_arm != 0 && param1_arm != 1) { mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1); } else { + const bool cmd_arms = (param1_arm == 1); - bool cmd_arms = (static_cast(cmd.param1 + 0.5f) == 1); + // Arm is forced (checks skipped) when param2 is set to a magic number. + const bool forced = (static_cast(roundf(cmd.param2)) == 21196); - // Arm/disarm is enforced when param2 is set to a magic number. - const bool enforce = (static_cast(roundf(cmd.param2)) == 21196); - - if (!enforce) { + if (!forced) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&_status)) { if (cmd_arms) { if (_armed.armed) { @@ -685,41 +742,27 @@ Commander::handle_command(const vehicle_command_s &cmd) if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id && cmd_from_io && cmd_arms) { _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; - - } else { - // Refuse to arm if preflight checks have failed - if (_status.hil_state != vehicle_status_s::HIL_STATE_ON - && !_status_flags.condition_system_sensors_initialized) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Preflight checks have failed"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } - - const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f); - const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f); - - if (cmd_arms && throttle_above_center && - (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } - - if (cmd_arms && throttle_above_low && - (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } } } - transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, - (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)); + transition_result_t arming_res = TRANSITION_DENIED; + + if (cmd_arms) { + if (cmd.from_external) { + arming_res = arm(arm_disarm_reason_t::COMMAND_EXTERNAL); + + } else { + arming_res = arm(arm_disarm_reason_t::COMMAND_INTERNAL, !forced); + } + + } else { + if (cmd.from_external) { + arming_res = disarm(arm_disarm_reason_t::COMMAND_EXTERNAL); + + } else { + arming_res = disarm(arm_disarm_reason_t::COMMAND_INTERNAL); + } + } if (arming_res == TRANSITION_DENIED) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -931,7 +974,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)) - && (TRANSITION_DENIED != arm_disarm(true, true, arm_disarm_reason_t::MISSION_START))) { + && (TRANSITION_DENIED != arm(arm_disarm_reason_t::MISSION_START))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1528,10 +1571,8 @@ Commander::run() while (!should_exit()) { - transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - /* update parameters */ - bool params_updated = _parameter_update_sub.updated(); + const bool params_updated = _parameter_update_sub.updated(); if (params_updated || param_init_forced) { // clear update @@ -1754,9 +1795,7 @@ Commander::run() } if (safety_disarm_allowed) { - if (TRANSITION_CHANGED == arm_disarm(false, true, arm_disarm_reason_t::SAFETY_BUTTON)) { - _status_changed = true; - } + disarm(arm_disarm_reason_t::SAFETY_BUTTON); } } @@ -1848,8 +1887,12 @@ Commander::run() } if (_auto_disarm_landed.get_state()) { - arm_disarm(false, true, - (_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT)); + if (_have_taken_off_since_arming) { + disarm(arm_disarm_reason_t::AUTO_DISARM_LAND); + + } else { + disarm(arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT); + } } } @@ -1866,12 +1909,11 @@ Commander::run() if (_auto_disarm_killed.get_state()) { if (_armed.manual_lockdown) { - arm_disarm(false, true, arm_disarm_reason_t::KILL_SWITCH); + disarm(arm_disarm_reason_t::KILL_SWITCH); } else { - arm_disarm(false, true, arm_disarm_reason_t::LOCKDOWN); + disarm(arm_disarm_reason_t::LOCKDOWN); } - } } else { @@ -1895,15 +1937,10 @@ 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_ret = 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); - - if (arming_ret == TRANSITION_DENIED) { - /* do not complain if not allowed into standby */ - arming_ret = TRANSITION_NOT_CHANGED; - } + 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); } /* start mission result check */ @@ -2053,35 +2090,29 @@ Commander::run() _geofence_violated_prev = false; } - // abort auto mode or geofence reaction if sticks are moved significantly - // but only if not in a low battery handling action - const bool is_rotary_wing = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + // abort autonomous mode and switch to position mode if sticks are moved significantly + if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { - const bool override_auto_mode = - (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) && - (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND); + const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) + && _vehicle_control_mode.flag_control_auto_enabled; - const bool override_offboard_mode = - (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) && - _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD; + const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) + && _vehicle_control_mode.flag_control_offboard_enabled; - if ((override_auto_mode || override_offboard_mode) && is_rotary_wing - && !in_low_battery_failsafe && !_geofence_warning_action_on) { - const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get(); + if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) { + const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get(); - // transition to previous state if sticks are touched - if (!_status.rc_signal_lost && - ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || - (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) { - // revert to position control in any case - main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); + if (!_status.rc_signal_lost && + ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || + (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) { + + if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, + &_internal_state) == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); + _status_changed = true; + } + } } } @@ -2149,18 +2180,19 @@ Commander::run() (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) { - const bool manual_thrust_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL - || _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO - || _internal_state.main_state == commander_state_s::MAIN_STATE_STAB - || _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE; + const bool manual_thrust_mode = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_climb_rate_enabled; + const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition; if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) { - arming_ret = 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_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); + if (arm_switch_to_disarm_transition) { + disarm(arm_disarm_reason_t::RC_SWITCH); + + } else { + disarm(arm_disarm_reason_t::RC_STICK); + } } _stick_off_counter++; @@ -2197,29 +2229,16 @@ Commander::run() * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) - ) { - print_reject_arm("Not arming: Switch to a manual mode first"); + if (!_vehicle_control_mode.flag_control_manual_enabled) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Switch to a manual mode first"); + tune_negative(true); - } else if (!_status_flags.condition_home_position_valid && - (_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) { + } else { + if (arm_switch_to_arm_transition) { + arm(arm_disarm_reason_t::RC_SWITCH); - print_reject_arm("Not arming: Geofence RTL requires valid home"); - - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &_armed, - !in_rearming_grace_period /* fRunPreArmChecks */, - &_mavlink_log_pub, &_status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), - (arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); - - if (arming_ret != TRANSITION_CHANGED) { - px4_usleep(100000); - print_reject_arm("Not arming: Preflight checks failed"); + } else { + arm(arm_disarm_reason_t::RC_STICK); } } } @@ -2234,54 +2253,34 @@ Commander::run() _last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch; - if (arming_ret == TRANSITION_DENIED) { - /* - * the arming transition can be denied to a number of reasons: - * - pre-flight check failed (sensors not ok or not calibrated) - * - safety not disabled - * - system not in manual mode - */ - tune_negative(true); - } - if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) { // handle landing gear switch if configured and in a manual mode - if ((_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && + if ((_vehicle_control_mode.flag_control_manual_enabled) && + (_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && (_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && (_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) { - // TODO: replace with vehicle_control_mode manual - if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) { + // Only switch the landing gear up if the user switched from gear down to gear up. + int8_t gear = landing_gear_s::GEAR_KEEP; - // Only switch the landing gear up if the user switched from gear down to gear up. - int8_t gear = landing_gear_s::GEAR_KEEP; + if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { + gear = landing_gear_s::GEAR_DOWN; - if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { - gear = landing_gear_s::GEAR_DOWN; + } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { + // gear up ignored unless flying + if (!_land_detector.landed && !_land_detector.maybe_landed) { + gear = landing_gear_s::GEAR_UP; - } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { - // gear up ignored unless flying - if (!_land_detector.landed && !_land_detector.maybe_landed) { - gear = landing_gear_s::GEAR_UP; - - } else { - mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") - } + } else { + mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") } + } - if (gear != landing_gear_s::GEAR_KEEP) { - landing_gear_s landing_gear{}; - landing_gear.landing_gear = gear; - landing_gear.timestamp = hrt_absolute_time(); - _landing_gear_pub.publish(landing_gear); - } + if (gear != landing_gear_s::GEAR_KEEP) { + landing_gear_s landing_gear{}; + landing_gear.landing_gear = gear; + landing_gear.timestamp = hrt_absolute_time(); + _landing_gear_pub.publish(landing_gear); } } @@ -2440,7 +2439,7 @@ Commander::run() if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { - arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR); + disarm(arm_disarm_reason_t::FAILURE_DETECTOR); mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request"); } } @@ -2530,8 +2529,6 @@ Commander::run() _have_taken_off_since_arming = false; } - _was_armed = _armed.armed; - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&_status, &_armed, @@ -2611,9 +2608,6 @@ Commander::run() _internal_state.timestamp = hrt_absolute_time(); _commander_state_pub.publish(_internal_state); - /* publish vehicle_status_flags */ - _status_flags.timestamp = hrt_absolute_time(); - // Evaluate current prearm status if (!_armed.armed && !_status_flags.condition_calibration_enabled) { bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, 30_s); @@ -2627,6 +2621,8 @@ Commander::run() && prearm_check_res), _status); } + /* publish vehicle_status_flags */ + _status_flags.timestamp = hrt_absolute_time(); _vehicle_status_flags_pub.publish(_status_flags); } @@ -2718,6 +2714,8 @@ Commander::run() _last_condition_local_position_valid = _status_flags.condition_local_position_valid; _last_condition_global_position_valid = _status_flags.condition_global_position_valid; + _was_armed = _armed.armed; + arm_auth_update(now, params_updated || param_init_forced); px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed); @@ -3360,56 +3358,55 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac void Commander::update_control_mode() { - vehicle_control_mode_s control_mode{}; - - control_mode.timestamp = hrt_absolute_time(); + _vehicle_control_mode = {}; /* set vehicle_control_mode according to set_navigation_state */ - control_mode.flag_armed = _armed.armed; - control_mode.flag_external_manual_override_ok = (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_status.is_vtol); + _vehicle_control_mode.flag_armed = _armed.armed; + + _vehicle_control_mode.flag_external_manual_override_ok = + (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol); switch (_status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = stabilization_required(); - control_mode.flag_control_attitude_enabled = stabilization_required(); + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = stabilization_required(); + _vehicle_control_mode.flag_control_attitude_enabled = stabilization_required(); break; case vehicle_status_s::NAVIGATION_STATE_STAB: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rattitude_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !_status.in_transition_mode; - control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* override is not ok for the RTL and recovery mode */ - control_mode.flag_external_manual_override_ok = false; + _vehicle_control_mode.flag_external_manual_override_ok = false; /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: @@ -3419,109 +3416,109 @@ Commander::update_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !_status.in_transition_mode; - control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_auto_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ - control_mode.flag_control_termination_enabled = true; + _vehicle_control_mode.flag_control_termination_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: { + _vehicle_control_mode.flag_control_offboard_enabled = true; - const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); + const offboard_control_mode_s &offboard = _offboard_control_mode_sub.get(); - control_mode.flag_control_offboard_enabled = true; + if (!offboard.ignore_acceleration_force) { + // OFFBOARD acceleration + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; - /* - * The control flags depend on what is ignored according to the offboard control mode topic - * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) - */ - control_mode.flag_control_rates_enabled = - !offboard_control_mode.ignore_bodyrate_x || - !offboard_control_mode.ignore_bodyrate_y || - !offboard_control_mode.ignore_bodyrate_z || - !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + } else if (!offboard.ignore_position) { + // OFFBOARD position + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + } else if (!offboard.ignore_velocity) { + // OFFBOARD velocity + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + + } else if (!offboard.ignore_attitude) { + // OFFBOARD attitude + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + + } else if (!offboard.ignore_bodyrate_x || !offboard.ignore_bodyrate_y || !offboard.ignore_bodyrate_z) { + // OFFBOARD rate + _vehicle_control_mode.flag_control_rates_enabled = true; + } // TO-DO: Add support for other modes than yawrate control - control_mode.flag_control_yawrate_override_enabled = - offboard_control_mode.ignore_bodyrate_x && - offboard_control_mode.ignore_bodyrate_y && - !offboard_control_mode.ignore_bodyrate_z && - !offboard_control_mode.ignore_attitude; - - control_mode.flag_control_rattitude_enabled = false; - - control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && - !_status.in_transition_mode; - - control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !_status.in_transition_mode && - !control_mode.flag_control_acceleration_enabled; - - control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position); - - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !_status.in_transition_mode && - !control_mode.flag_control_acceleration_enabled; - - control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; + _vehicle_control_mode.flag_control_yawrate_override_enabled = offboard.ignore_bodyrate_x && offboard.ignore_bodyrate_y + && !offboard.ignore_bodyrate_z && !offboard.ignore_attitude; + // VTOL transition override + if (_status.in_transition_mode) { + _vehicle_control_mode.flag_control_acceleration_enabled = false; + _vehicle_control_mode.flag_control_velocity_enabled = false; + _vehicle_control_mode.flag_control_position_enabled = false; + } } break; case vehicle_status_s::NAVIGATION_STATE_ORBIT: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !_status.in_transition_mode; - control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; + _vehicle_control_mode.flag_control_manual_enabled = false; + _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rattitude_enabled = false; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_acceleration_enabled = false; + _vehicle_control_mode.flag_control_termination_enabled = false; break; default: break; } - _control_mode_pub.publish(control_mode); + _vehicle_control_mode.timestamp = hrt_absolute_time(); + _control_mode_pub.publish(_vehicle_control_mode); } bool @@ -3549,18 +3546,6 @@ Commander::print_reject_mode(const char *msg) } } -void -Commander::print_reject_arm(const char *msg) -{ - const hrt_abstime t = hrt_absolute_time(); - - if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { - _last_print_mode_reject_time = t; - mavlink_log_critical(&_mavlink_log_pub, "%s", msg); - tune_negative(true); - } -} - void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result) { switch (result) { @@ -3815,13 +3800,9 @@ void Commander::avoidance_check() const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled; - const bool auto_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL - || _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND; - const bool pos_ctl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL; + const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled; + const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled); const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled)); const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy)); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 9c08bf5485..4574f1c741 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -124,7 +124,8 @@ public: private: void answer_command(const vehicle_command_s &cmd, uint8_t result); - transition_result_t arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason); + transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); + transition_result_t disarm(arm_disarm_reason_t calling_reason); void battery_status_check(); @@ -153,7 +154,6 @@ private: void offboard_control_update(); - void print_reject_arm(const char *msg); void print_reject_mode(const char *msg); void reset_posvel_validity(); @@ -391,16 +391,19 @@ private: main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL}; - actuator_armed_s _armed{}; - commander_state_s _internal_state{}; cpuload_s _cpuload{}; geofence_result_s _geofence_result{}; vehicle_land_detected_s _land_detector{}; safety_s _safety{}; - vehicle_status_s _status{}; - vehicle_status_flags_s _status_flags{}; vtol_vehicle_status_s _vtol_status{}; + // commander publications + actuator_armed_s _armed{}; + commander_state_s _internal_state{}; + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _status{}; + vehicle_status_flags_s _status_flags{}; + WorkerThread _worker_thread; // Subscriptions