mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
commander: use control mode flags and cleanup arm_disarm
- keep `vehicle_control_mode` last state in commander and use appropriate flags in place of various main_state and nav_state checks
- consolidate scattered arming requirements in `arm_disarm()`
- there were a number of different requirements from arming via RC vs Mavlink that don't make any sense
- if geofence enabled require valid home before arming
- throttle requirements for manual modes
- remove unnecessary mavlink feedback that differs between arming interfaces (mavlink vs RC)
- let the preflight/prearm checks respond directly in most cases
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
GitHub
parent
37f78537c3
commit
0618f048f2
@ -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<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 1) {
|
||||
const int param1_arm = static_cast<int>(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<int>(cmd.param1 + 0.5f) == 1);
|
||||
// Arm is forced (checks skipped) when param2 is set to a magic number.
|
||||
const bool forced = (static_cast<int>(roundf(cmd.param2)) == 21196);
|
||||
|
||||
// Arm/disarm is enforced when param2 is set to a magic number.
|
||||
const bool enforce = (static_cast<int>(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));
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user