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

commander: use control mode flags and cleanup arm_disarm

Browse Source 
- 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
2021-02-15 12:03:21 -05:00
committed by GitHub
co-authored by Matthias Grob
parent 37f78537c3
commit 0618f048f2
2 changed files with 297 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+288 -307
View File
@@ -382,57 +382,113 @@ bool Commander::shutdown_if_allowed()
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN); hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN);
} }
transition_result_t static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
Commander::arm_disarm(bool arm, bool run_preflight_checks, 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 case arm_disarm_reason_t::RC_STICK: return "RC";
// 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);
if (arming_res == TRANSITION_CHANGED) { case arm_disarm_reason_t::RC_SWITCH: return "RC (switch)";
const char *reason = "";
switch (calling_reason) { case arm_disarm_reason_t::COMMAND_INTERNAL: return "internal command";
case arm_disarm_reason_t::TRANSITION_TO_STANDBY: reason = ""; break;
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) { } else if (arming_res == TRANSITION_DENIED) {
tune_negative(true); 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 // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters // 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); mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1);
} else { } 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. if (!forced) {
const bool enforce = (static_cast<int>(roundf(cmd.param2)) == 21196);
if (!enforce) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&_status)) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&_status)) {
if (cmd_arms) { if (cmd_arms) {
if (_armed.armed) { 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 if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
&& cmd_from_io && cmd_arms) { && cmd_from_io && cmd_arms) {
_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; _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, transition_result_t arming_res = TRANSITION_DENIED;
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL));
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) { if (arming_res == TRANSITION_DENIED) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; 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 // switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags,
&_internal_state)) &_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; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1528,10 +1571,8 @@ Commander::run()
while (!should_exit()) { while (!should_exit()) {
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */ /* update parameters */
bool params_updated = _parameter_update_sub.updated(); const bool params_updated = _parameter_update_sub.updated();
if (params_updated || param_init_forced) { if (params_updated || param_init_forced) {
// clear update // clear update
@@ -1754,9 +1795,7 @@ Commander::run()
} }
if (safety_disarm_allowed) { if (safety_disarm_allowed) {
if (TRANSITION_CHANGED == arm_disarm(false, true, arm_disarm_reason_t::SAFETY_BUTTON)) { disarm(arm_disarm_reason_t::SAFETY_BUTTON);
_status_changed = true;
}
} }
} }
@@ -1848,8 +1887,12 @@ Commander::run()
} }
if (_auto_disarm_landed.get_state()) { if (_auto_disarm_landed.get_state()) {
arm_disarm(false, true, if (_have_taken_off_since_arming) {
(_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT)); 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 (_auto_disarm_killed.get_state()) {
if (_armed.manual_lockdown) { if (_armed.manual_lockdown) {
arm_disarm(false, true, arm_disarm_reason_t::KILL_SWITCH); disarm(arm_disarm_reason_t::KILL_SWITCH);
} else { } else {
arm_disarm(false, true, arm_disarm_reason_t::LOCKDOWN); disarm(arm_disarm_reason_t::LOCKDOWN);
} }
} }
} else { } else {
@@ -1895,15 +1937,10 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */ /* 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) { 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, arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags, true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp), _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY); 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;
}
} }
/* start mission result check */ /* start mission result check */
@@ -2053,35 +2090,29 @@ Commander::run()
_geofence_violated_prev = false; _geofence_violated_prev = false;
} }
// abort auto mode or geofence reaction if sticks are moved significantly // abort autonomous mode and switch to position mode if sticks are moved significantly
// but only if not in a low battery handling action if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
const bool is_rotary_wing = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
const bool override_auto_mode = const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT)
(_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) && && _vehicle_control_mode.flag_control_auto_enabled;
(_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_offboard_mode = const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT)
(_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) && && _vehicle_control_mode.flag_control_offboard_enabled;
_internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD;
if ((override_auto_mode || override_offboard_mode) && is_rotary_wing if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) {
&& !in_low_battery_failsafe && !_geofence_warning_action_on) { const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get();
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 &&
if (!_status.rc_signal_lost && ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) ||
((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
(fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
// revert to position control in any case if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); &_internal_state) == TRANSITION_CHANGED) {
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); 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) && (_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)) { (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 const bool manual_thrust_mode = _vehicle_control_mode.flag_control_manual_enabled
|| _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO && !_vehicle_control_mode.flag_control_climb_rate_enabled;
|| _internal_state.main_state == commander_state_s::MAIN_STATE_STAB
|| _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE;
const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst) const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst)
|| arm_switch_to_disarm_transition; || arm_switch_to_disarm_transition;
if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) { if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed, if (arm_switch_to_disarm_transition) {
true /* fRunPreArmChecks */, disarm(arm_disarm_reason_t::RC_SWITCH);
&_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)); } else {
disarm(arm_disarm_reason_t::RC_STICK);
}
} }
_stick_off_counter++; _stick_off_counter++;
@@ -2197,29 +2229,16 @@ Commander::run()
* for being in manual mode only applies to manual arming actions. * for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS. * the system can be armed in auto if armed via the GCS.
*/ */
if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) if (!_vehicle_control_mode.flag_control_manual_enabled) {
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Switch to a manual mode first");
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB) tune_negative(true);
&& (_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");
} else if (!_status_flags.condition_home_position_valid && } else {
(_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) { if (arm_switch_to_arm_transition) {
arm(arm_disarm_reason_t::RC_SWITCH);
print_reject_arm("Not arming: Geofence RTL requires valid home"); } else {
arm(arm_disarm_reason_t::RC_STICK);
} 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");
} }
} }
} }
@@ -2234,54 +2253,34 @@ Commander::run()
_last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch; _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) { if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
// handle landing gear switch if configured and in a manual mode // 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) && (_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
(_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) { (_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) {
// TODO: replace with vehicle_control_mode manual // Only switch the landing gear up if the user switched from gear down to gear up.
if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || int8_t gear = landing_gear_s::GEAR_KEEP;
_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. if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
int8_t gear = landing_gear_s::GEAR_KEEP; gear = landing_gear_s::GEAR_DOWN;
if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
gear = landing_gear_s::GEAR_DOWN; // 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) { } else {
// gear up ignored unless flying mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear")
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")
}
} }
}
if (gear != landing_gear_s::GEAR_KEEP) { if (gear != landing_gear_s::GEAR_KEEP) {
landing_gear_s landing_gear{}; landing_gear_s landing_gear{};
landing_gear.landing_gear = gear; landing_gear.landing_gear = gear;
landing_gear.timestamp = hrt_absolute_time(); landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(landing_gear); _landing_gear_pub.publish(landing_gear);
}
} }
} }
@@ -2440,7 +2439,7 @@ Commander::run()
if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { 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 // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { 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"); 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; _have_taken_off_since_arming = false;
} }
_was_armed = _armed.armed;
/* now set navigation state according to failsafe and main state */ /* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&_status, bool nav_state_changed = set_nav_state(&_status,
&_armed, &_armed,
@@ -2611,9 +2608,6 @@ Commander::run()
_internal_state.timestamp = hrt_absolute_time(); _internal_state.timestamp = hrt_absolute_time();
_commander_state_pub.publish(_internal_state); _commander_state_pub.publish(_internal_state);
/* publish vehicle_status_flags */
_status_flags.timestamp = hrt_absolute_time();
// Evaluate current prearm status // Evaluate current prearm status
if (!_armed.armed && !_status_flags.condition_calibration_enabled) { if (!_armed.armed && !_status_flags.condition_calibration_enabled) {
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, 30_s); bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, 30_s);
@@ -2627,6 +2621,8 @@ Commander::run()
&& prearm_check_res), _status); && prearm_check_res), _status);
} }
/* publish vehicle_status_flags */
_status_flags.timestamp = hrt_absolute_time();
_vehicle_status_flags_pub.publish(_status_flags); _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_local_position_valid = _status_flags.condition_local_position_valid;
_last_condition_global_position_valid = _status_flags.condition_global_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); arm_auth_update(now, params_updated || param_init_forced);
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed); 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 void
Commander::update_control_mode() Commander::update_control_mode()
{ {
vehicle_control_mode_s control_mode{}; _vehicle_control_mode = {};
control_mode.timestamp = hrt_absolute_time();
/* set vehicle_control_mode according to set_navigation_state */ /* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = _armed.armed; _vehicle_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_external_manual_override_ok =
(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol);
switch (_status.nav_state) { switch (_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = stabilization_required(); _vehicle_control_mode.flag_control_rates_enabled = stabilization_required();
control_mode.flag_control_attitude_enabled = stabilization_required(); _vehicle_control_mode.flag_control_attitude_enabled = stabilization_required();
break; break;
case vehicle_status_s::NAVIGATION_STATE_STAB: case vehicle_status_s::NAVIGATION_STATE_STAB:
control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = true; _vehicle_control_mode.flag_control_rattitude_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !_status.in_transition_mode; _vehicle_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_velocity_enabled = !_status.in_transition_mode;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
/* override is not ok for the RTL and recovery mode */ /* 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 */ /* fallthrough */
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: 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_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
control_mode.flag_control_auto_enabled = true; _vehicle_control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !_status.in_transition_mode; _vehicle_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_velocity_enabled = !_status.in_transition_mode;
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND: case vehicle_status_s::NAVIGATION_STATE_DESCEND:
control_mode.flag_control_auto_enabled = false; _vehicle_control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */ /* disable all controllers on termination */
control_mode.flag_control_termination_enabled = true; _vehicle_control_mode.flag_control_termination_enabled = true;
break; break;
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: { 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;
/* } else if (!offboard.ignore_position) {
* The control flags depend on what is ignored according to the offboard control mode topic // OFFBOARD position
* Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) _vehicle_control_mode.flag_control_position_enabled = true;
*/ _vehicle_control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_rates_enabled = _vehicle_control_mode.flag_control_altitude_enabled = true;
!offboard_control_mode.ignore_bodyrate_x || _vehicle_control_mode.flag_control_climb_rate_enabled = true;
!offboard_control_mode.ignore_bodyrate_y || _vehicle_control_mode.flag_control_attitude_enabled = true;
!offboard_control_mode.ignore_bodyrate_z || _vehicle_control_mode.flag_control_rates_enabled = true;
!offboard_control_mode.ignore_attitude ||
!offboard_control_mode.ignore_position ||
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || } else if (!offboard.ignore_velocity) {
!offboard_control_mode.ignore_position || // OFFBOARD velocity
!offboard_control_mode.ignore_velocity || _vehicle_control_mode.flag_control_velocity_enabled = true;
!offboard_control_mode.ignore_acceleration_force; _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 // TO-DO: Add support for other modes than yawrate control
control_mode.flag_control_yawrate_override_enabled = _vehicle_control_mode.flag_control_yawrate_override_enabled = offboard.ignore_bodyrate_x && offboard.ignore_bodyrate_y
offboard_control_mode.ignore_bodyrate_x && && !offboard.ignore_bodyrate_z && !offboard.ignore_attitude;
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;
// 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; break;
case vehicle_status_s::NAVIGATION_STATE_ORBIT: case vehicle_status_s::NAVIGATION_STATE_ORBIT:
control_mode.flag_control_manual_enabled = false; _vehicle_control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false; _vehicle_control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = false; _vehicle_control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !_status.in_transition_mode; _vehicle_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_velocity_enabled = !_status.in_transition_mode;
control_mode.flag_control_acceleration_enabled = false; _vehicle_control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false; _vehicle_control_mode.flag_control_termination_enabled = false;
break; break;
default: default:
break; break;
} }
_control_mode_pub.publish(control_mode); _vehicle_control_mode.timestamp = hrt_absolute_time();
_control_mode_pub.publish(_vehicle_control_mode);
} }
bool 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) void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
{ {
switch (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 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 const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL && _vehicle_control_mode.flag_control_position_enabled);
|| _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 sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_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)); const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy));
src/modules/commander/Commander.hpp
+9 -6
View File
@@ -124,7 +124,8 @@ public:
private: private:
void answer_command(const vehicle_command_s &cmd, uint8_t result); 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(); void battery_status_check();
@@ -153,7 +154,6 @@ private:
void offboard_control_update(); void offboard_control_update();
void print_reject_arm(const char *msg);
void print_reject_mode(const char *msg); void print_reject_mode(const char *msg);
void reset_posvel_validity(); void reset_posvel_validity();
@@ -391,16 +391,19 @@ private:
main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL}; 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{}; cpuload_s _cpuload{};
geofence_result_s _geofence_result{}; geofence_result_s _geofence_result{};
vehicle_land_detected_s _land_detector{}; vehicle_land_detected_s _land_detector{};
safety_s _safety{}; safety_s _safety{};
vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{};
vtol_vehicle_status_s _vtol_status{}; 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; WorkerThread _worker_thread;
// Subscriptions // Subscriptions