mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 15:57:35 +08:00
commander: collapse ArmStateMachine and simplify
- simplify vehicle_status.arming_state down to just armed and disarmed
- ARMING_STATE_INIT doesn't matter
- ARMING_STATE_STANDBY is effectively pre_flight_checks_pass
- ARMING_STATE_STANDBY_ERROR not needed
- ARMING_STATE_SHUTDOWN effectively not used (all the poweroff/shutdown calls loop forever in place)
- ARMING_STATE_IN_AIR_RESTORE doesn't exist anymore
- collapse ArmStateMachine into commander
- all requests already go through Commander::arm() and Commander::dismarm()
- other minor changes
- VEHICLE_CMD_DO_FLIGHTTERMINATION undocumented (unused?) test command (param1 > 1.5f) removed
- switching to NAVIGATION_STATE_TERMINATION triggers parachute command centrally (only if armed)
---------
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
+142
-197
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -473,7 +473,7 @@ int Commander::custom_command(int argc, char *argv[])
|
||||
|
||||
int Commander::print_status()
|
||||
{
|
||||
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
|
||||
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
|
||||
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
|
||||
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
|
||||
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
|
||||
@@ -487,13 +487,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
|
||||
return Commander::main(argc, argv);
|
||||
}
|
||||
|
||||
bool Commander::shutdownIfAllowed()
|
||||
{
|
||||
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
|
||||
false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
|
||||
}
|
||||
|
||||
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
|
||||
{
|
||||
switch (calling_reason) {
|
||||
@@ -533,20 +526,37 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
|
||||
|
||||
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 (isArmed()) {
|
||||
return TRANSITION_NOT_CHANGED;
|
||||
}
|
||||
|
||||
if (_vehicle_status.calibration_enabled
|
||||
|| _vehicle_status.rc_calibration_in_progress
|
||||
|| _actuator_armed.in_esc_calibration_mode) {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: calibrating\t");
|
||||
events::send(events::ID("commander_arm_denied_calibrating"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: calibrating");
|
||||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidental in-air disarming
|
||||
if (calling_reason == arm_disarm_reason_t::rc_switch
|
||||
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
|
||||
&& ((_last_disarmed_timestamp != 0) && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) {
|
||||
|
||||
run_preflight_checks = false;
|
||||
}
|
||||
|
||||
if (run_preflight_checks && !_arm_state_machine.isArmed()) {
|
||||
if (run_preflight_checks) {
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
||||
|
||||
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
||||
!_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
|
||||
events::send(events::ID("commander_arm_denied_throttle_center"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: throttle above center");
|
||||
events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: throttle above center");
|
||||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
@@ -554,10 +564,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
||||
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
||||
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
|
||||
&& _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
|
||||
events::send(events::ID("commander_arm_denied_throttle_high"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: high throttle");
|
||||
events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: high throttle");
|
||||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
@@ -565,35 +575,45 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
||||
} else if (calling_reason == arm_disarm_reason_t::rc_stick
|
||||
|| calling_reason == arm_disarm_reason_t::rc_switch
|
||||
|| calling_reason == arm_disarm_reason_t::rc_button) {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
|
||||
events::send(events::ID("commander_arm_denied_not_manual"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: switch to manual mode first");
|
||||
events::send(events::ID("commander_arm_denied_not_manual"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: switch to manual mode first");
|
||||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
_health_and_arming_checks.update();
|
||||
|
||||
if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) {
|
||||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks,
|
||||
&_mavlink_log_pub, calling_reason);
|
||||
_vehicle_status.armed_time = hrt_absolute_time();
|
||||
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_ARMED;
|
||||
_vehicle_status.latest_arming_reason = (uint8_t)calling_reason;
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
|
||||
"Armed by {1}", calling_reason);
|
||||
mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
|
||||
"Armed by {1}", calling_reason);
|
||||
|
||||
_status_changed = true;
|
||||
|
||||
} else if (arming_res == TRANSITION_DENIED) {
|
||||
tune_negative(true);
|
||||
if (_param_com_home_en.get()) {
|
||||
_home_position.setHomePosition();
|
||||
}
|
||||
|
||||
return arming_res;
|
||||
_status_changed = true;
|
||||
|
||||
return TRANSITION_CHANGED;
|
||||
}
|
||||
|
||||
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
|
||||
{
|
||||
if (!isArmed()) {
|
||||
return TRANSITION_NOT_CHANGED;
|
||||
}
|
||||
|
||||
if (!forced) {
|
||||
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
|
||||
|| is_ground_vehicle(_vehicle_status));
|
||||
@@ -606,36 +626,43 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
||||
|
||||
if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
|
||||
if (calling_reason != arm_disarm_reason_t::rc_stick) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
|
||||
events::send(events::ID("commander_disarming_denied_not_landed"),
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t");
|
||||
events::send(events::ID("commander_disarm_denied_not_landed"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Disarming denied, not landed");
|
||||
"Disarming denied: not landed");
|
||||
}
|
||||
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false,
|
||||
&_mavlink_log_pub, calling_reason);
|
||||
_vehicle_status.armed_time = 0;
|
||||
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
|
||||
_vehicle_status.latest_disarming_reason = (uint8_t)calling_reason;
|
||||
_vehicle_status.takeoff_time = 0;
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
|
||||
"Disarmed by {1}", calling_reason);
|
||||
_have_taken_off_since_arming = false;
|
||||
|
||||
if (_param_com_force_safety.get()) {
|
||||
_safety.activateSafety();
|
||||
}
|
||||
_last_disarmed_timestamp = hrt_absolute_time();
|
||||
|
||||
_status_changed = true;
|
||||
_user_mode_intention.onDisarm();
|
||||
|
||||
} else if (arming_res == TRANSITION_DENIED) {
|
||||
tune_negative(true);
|
||||
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
|
||||
"Disarmed by {1}", calling_reason);
|
||||
|
||||
if (_param_com_force_safety.get()) {
|
||||
_safety.activateSafety();
|
||||
}
|
||||
|
||||
return arming_res;
|
||||
// update flight uuid
|
||||
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
|
||||
_param_flight_uuid.set(flight_uuid);
|
||||
_param_flight_uuid.commit_no_notification();
|
||||
|
||||
_status_changed = true;
|
||||
|
||||
return TRANSITION_CHANGED;
|
||||
}
|
||||
|
||||
Commander::Commander() :
|
||||
@@ -643,19 +670,15 @@ Commander::Commander() :
|
||||
{
|
||||
_vehicle_land_detected.landed = true;
|
||||
|
||||
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
|
||||
_vehicle_status.system_id = 1;
|
||||
_vehicle_status.component_id = 1;
|
||||
|
||||
_vehicle_status.system_type = 0;
|
||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
|
||||
|
||||
_vehicle_status.nav_state = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
/* mark all signals lost as long as they haven't been found */
|
||||
_vehicle_status.gcs_connection_lost = true;
|
||||
|
||||
_vehicle_status.power_input_valid = true;
|
||||
|
||||
// default for vtol is rotary wing
|
||||
@@ -858,16 +881,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
} else {
|
||||
// Arm is forced (checks skipped) when param2 is set to a magic number.
|
||||
const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
|
||||
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
|
||||
|
||||
// Flick to in-air restore first if this comes from an onboard system and from IO
|
||||
if (!forced && cmd_from_io
|
||||
&& (cmd.source_system == _vehicle_status.system_id)
|
||||
&& (cmd.source_component == _vehicle_status.component_id)
|
||||
&& (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
|
||||
// TODO: replace with a proper allowed transition
|
||||
_arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE);
|
||||
}
|
||||
|
||||
transition_result_t arming_res = TRANSITION_DENIED;
|
||||
arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external :
|
||||
@@ -886,47 +899,26 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
} else {
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
|
||||
if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED)
|
||||
&& (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))
|
||||
&& (_param_com_home_en.get())) {
|
||||
_home_position.setHomePosition();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
|
||||
if (cmd.param1 > 1.5f) {
|
||||
// Test termination command triggers lockdown but not actual termination.
|
||||
if (!_lockdown_triggered) {
|
||||
_actuator_armed.lockdown = true;
|
||||
_lockdown_triggered = true;
|
||||
PX4_WARN("forcing lockdown (motors off)");
|
||||
}
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
|
||||
} else if (cmd.param1 > 0.5f) {
|
||||
if (cmd.param1 > 0.5f) {
|
||||
// Trigger real termination.
|
||||
if (!_flight_termination_triggered) {
|
||||
_actuator_armed.force_failsafe = true;
|
||||
_flight_termination_triggered = true;
|
||||
PX4_WARN("forcing failsafe (termination)");
|
||||
send_parachute_command();
|
||||
if (!isArmed()) {
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
|
||||
} else if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION)) {
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
|
||||
} else {
|
||||
_actuator_armed.force_failsafe = false;
|
||||
_actuator_armed.lockdown = false;
|
||||
|
||||
_lockdown_triggered = false;
|
||||
_flight_termination_triggered = false;
|
||||
|
||||
PX4_WARN("disabling failsafe and lockdown");
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -1130,7 +1122,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
#if defined(CONFIG_BOARDCTL_RESET)
|
||||
|
||||
} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
|
||||
} else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) {
|
||||
// 1: Reboot autopilot
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
@@ -1140,7 +1132,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
|
||||
} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
|
||||
} else if ((param1 == 2) && !isArmed() && (px4_shutdown_request(400_ms) == 0)) {
|
||||
// 2: Shutdown autopilot
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
@@ -1150,7 +1142,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
#if defined(CONFIG_BOARDCTL_RESET)
|
||||
|
||||
} else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) {
|
||||
} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) {
|
||||
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
@@ -1167,25 +1159,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
|
||||
|
||||
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
|
||||
if (isArmed() || _worker_thread.isBusy()) {
|
||||
|
||||
// reject if armed or shutting down
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||
|
||||
} else {
|
||||
|
||||
/* try to go to INIT/PREFLIGHT arming state */
|
||||
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks,
|
||||
false /* fRunPreArmChecks */, &_mavlink_log_pub,
|
||||
(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
|
||||
) {
|
||||
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
if ((int)(cmd.param1) == 1) {
|
||||
/* gyro calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
@@ -1292,7 +1272,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
|
||||
// Magnetometer quick calibration using world magnetic model and known heading
|
||||
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
|
||||
if (isArmed() || _worker_thread.isBusy()) {
|
||||
|
||||
// reject if armed or shutting down
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||
@@ -1327,7 +1307,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
|
||||
|
||||
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
|
||||
if (isArmed() || _worker_thread.isBusy()) {
|
||||
|
||||
// reject if armed or shutting down
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||
@@ -1424,7 +1404,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
|
||||
{
|
||||
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
|
||||
if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
|
||||
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
}
|
||||
|
||||
@@ -1504,7 +1484,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
||||
case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
|
||||
|
||||
case action_request_s::ACTION_TOGGLE_ARMING:
|
||||
if (_arm_state_machine.isArmed()) {
|
||||
if (isArmed()) {
|
||||
disarm(arm_disarm_reason);
|
||||
|
||||
} else {
|
||||
@@ -1540,7 +1520,6 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
||||
|
||||
_status_changed = true;
|
||||
_actuator_armed.manual_lockdown = true;
|
||||
send_parachute_command();
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -1674,20 +1653,12 @@ void Commander::run()
|
||||
|
||||
vtolStatusUpdate();
|
||||
|
||||
_home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed);
|
||||
_home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed);
|
||||
|
||||
handleAutoDisarm();
|
||||
|
||||
battery_status_check();
|
||||
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) {
|
||||
|
||||
_arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks,
|
||||
true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby);
|
||||
}
|
||||
|
||||
checkForMissionUpdate();
|
||||
|
||||
manualControlCheck();
|
||||
@@ -1754,40 +1725,30 @@ void Commander::run()
|
||||
}
|
||||
}
|
||||
|
||||
// check for arming state changes
|
||||
if (_was_armed != _arm_state_machine.isArmed()) {
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
if (!_was_armed && _arm_state_machine.isArmed() && !_vehicle_land_detected.landed) {
|
||||
_have_taken_off_since_arming = true;
|
||||
}
|
||||
|
||||
if (_was_armed && !_arm_state_machine.isArmed()) {
|
||||
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
|
||||
_param_flight_uuid.set(flight_uuid);
|
||||
_param_flight_uuid.commit_no_notification();
|
||||
|
||||
_last_disarmed_timestamp = hrt_absolute_time();
|
||||
|
||||
_user_mode_intention.onDisarm();
|
||||
_vehicle_status.takeoff_time = 0;
|
||||
}
|
||||
|
||||
if (!_arm_state_machine.isArmed()) {
|
||||
/* Reset the flag if disarmed. */
|
||||
_have_taken_off_since_arming = false;
|
||||
}
|
||||
|
||||
// update actuator_armed
|
||||
_actuator_armed.armed = isArmed();
|
||||
_actuator_armed.prearmed = getPrearmState();
|
||||
_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
|
||||
_actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION)
|
||||
|| (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON));
|
||||
// _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL
|
||||
_actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
|
||||
// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
|
||||
|
||||
// if force_failsafe or manual_lockdown activated send parachute command
|
||||
if ((!actuator_armed_prev.force_failsafe && _actuator_armed.force_failsafe)
|
||||
|| (!actuator_armed_prev.manual_lockdown && _actuator_armed.manual_lockdown)
|
||||
) {
|
||||
if (isArmed()) {
|
||||
send_parachute_command();
|
||||
}
|
||||
}
|
||||
|
||||
// publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed
|
||||
if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed
|
||||
|| !(_actuator_armed == actuator_armed_prev)) {
|
||||
|
||||
// publish actuator_armed first (used by output modules)
|
||||
_actuator_armed.armed = _arm_state_machine.isArmed();
|
||||
_actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
|
||||
_actuator_armed.timestamp = hrt_absolute_time();
|
||||
_actuator_armed_pub.publish(_actuator_armed);
|
||||
|
||||
@@ -1795,7 +1756,6 @@ void Commander::run()
|
||||
updateControlMode();
|
||||
|
||||
// vehicle_status publish (after prearm/preflight updates above)
|
||||
_vehicle_status.arming_state = _arm_state_machine.getArmState();
|
||||
_vehicle_status.timestamp = hrt_absolute_time();
|
||||
_vehicle_status_pub.publish(_vehicle_status);
|
||||
|
||||
@@ -1822,11 +1782,9 @@ void Commander::run()
|
||||
|
||||
_status_changed = false;
|
||||
|
||||
_was_armed = _arm_state_machine.isArmed();
|
||||
|
||||
arm_auth_update(hrt_absolute_time(), params_updated);
|
||||
|
||||
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed());
|
||||
px4_indicate_external_reset_lockout(LockoutComponent::Commander, isArmed());
|
||||
|
||||
perf_end(_loop_perf);
|
||||
|
||||
@@ -1877,7 +1835,7 @@ void Commander::checkForMissionUpdate()
|
||||
}
|
||||
}
|
||||
|
||||
if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
|
||||
if (isArmed() && !_vehicle_land_detected.landed
|
||||
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
|
||||
&& mission_result.finished) {
|
||||
|
||||
@@ -1901,6 +1859,10 @@ void Commander::checkForMissionUpdate()
|
||||
|
||||
bool Commander::getPrearmState() const
|
||||
{
|
||||
if (_vehicle_status.calibration_enabled) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
|
||||
case PrearmedMode::DISABLED:
|
||||
/* skip prearmed state */
|
||||
@@ -1936,7 +1898,7 @@ void Commander::handlePowerButtonState()
|
||||
|
||||
if (_power_button_state_sub.copy(&button_state)) {
|
||||
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
|
||||
if (shutdownIfAllowed() && (px4_shutdown_request() == 0)) {
|
||||
if (!isArmed() && (px4_shutdown_request() == 0)) {
|
||||
while (1) { px4_usleep(1); }
|
||||
}
|
||||
}
|
||||
@@ -1973,7 +1935,7 @@ void Commander::landDetectorUpdate()
|
||||
_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
|
||||
|
||||
// Only take actions if armed
|
||||
if (_arm_state_machine.isArmed()) {
|
||||
if (isArmed()) {
|
||||
if (!was_landed && _vehicle_land_detected.landed) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
|
||||
events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
|
||||
@@ -1987,9 +1949,8 @@ void Commander::landDetectorUpdate()
|
||||
|
||||
// automatically set or update home position
|
||||
if (_param_com_home_en.get()) {
|
||||
// set the home position when taking off, but only if we were previously disarmed
|
||||
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
|
||||
if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
|
||||
// set the home position when taking off
|
||||
if (!_vehicle_land_detected.landed) {
|
||||
if (was_landed) {
|
||||
_home_position.setHomePosition();
|
||||
|
||||
@@ -2060,7 +2021,7 @@ void Commander::vtolStatusUpdate()
|
||||
void Commander::updateTunes()
|
||||
{
|
||||
// play arming and battery warning tunes
|
||||
if (!_arm_tune_played && _arm_state_machine.isArmed()) {
|
||||
if (!_arm_tune_played && isArmed()) {
|
||||
|
||||
/* play tune when armed */
|
||||
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
|
||||
@@ -2069,6 +2030,7 @@ void Commander::updateTunes()
|
||||
} else if (!_vehicle_status.usb_connected &&
|
||||
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
||||
|
||||
/* play tune on battery critical */
|
||||
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
|
||||
|
||||
@@ -2077,7 +2039,7 @@ void Commander::updateTunes()
|
||||
/* play tune on battery warning */
|
||||
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
|
||||
|
||||
} else if (_vehicle_status.failsafe && _arm_state_machine.isArmed()) {
|
||||
} else if (_vehicle_status.failsafe && isArmed()) {
|
||||
tune_failsafe(true);
|
||||
|
||||
} else {
|
||||
@@ -2085,7 +2047,7 @@ void Commander::updateTunes()
|
||||
}
|
||||
|
||||
/* reset arm_tune_played when disarmed */
|
||||
if (!_arm_state_machine.isArmed()) {
|
||||
if (!isArmed()) {
|
||||
|
||||
// Notify the user that it is safe to approach the vehicle
|
||||
if (_arm_tune_played) {
|
||||
@@ -2119,7 +2081,7 @@ void Commander::checkWorkerThread()
|
||||
void Commander::handleAutoDisarm()
|
||||
{
|
||||
// Auto disarm when landed or kill switch engaged
|
||||
if (_arm_state_machine.isArmed()) {
|
||||
if (isArmed()) {
|
||||
|
||||
// Check for auto-disarm on landing or pre-flight
|
||||
if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
|
||||
@@ -2178,7 +2140,7 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||
const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
|
||||
|
||||
FailsafeBase::State state{};
|
||||
state.armed = _arm_state_machine.isArmed();
|
||||
state.armed = isArmed();
|
||||
state.vtol_in_transition_mode = _vehicle_status.in_transition_mode;
|
||||
state.mission_finished = _mission_result_sub.get().finished;
|
||||
state.user_intended_mode = _user_mode_intention.get();
|
||||
@@ -2210,13 +2172,6 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||
|
||||
case FailsafeBase::Action::Terminate:
|
||||
_vehicle_status.nav_state = _vehicle_status.NAVIGATION_STATE_TERMINATION;
|
||||
_actuator_armed.force_failsafe = true;
|
||||
|
||||
if (!_flight_termination_triggered) {
|
||||
_flight_termination_triggered = true;
|
||||
send_parachute_command();
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -2293,14 +2248,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
uint8_t led_color = led_control_s::COLOR_WHITE;
|
||||
bool set_normal_color = false;
|
||||
|
||||
uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms;
|
||||
uint64_t overload_warn_delay = isArmed() ? 1_ms : 250_ms;
|
||||
|
||||
// set mode
|
||||
if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
|
||||
led_mode = led_control_s::MODE_BLINK_FAST;
|
||||
led_color = led_control_s::COLOR_PURPLE;
|
||||
|
||||
} else if (_arm_state_machine.isArmed()) {
|
||||
} else if (isArmed()) {
|
||||
led_mode = led_control_s::MODE_ON;
|
||||
set_normal_color = true;
|
||||
|
||||
@@ -2308,18 +2263,9 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
led_mode = led_control_s::MODE_BLINK_FAST;
|
||||
led_color = led_control_s::COLOR_RED;
|
||||
|
||||
} else if (_arm_state_machine.isStandby()) {
|
||||
} else {
|
||||
led_mode = led_control_s::MODE_BREATHE;
|
||||
set_normal_color = true;
|
||||
|
||||
} else if (_arm_state_machine.isInit()) {
|
||||
// if in init status it should not be in the error state
|
||||
led_mode = led_control_s::MODE_OFF;
|
||||
|
||||
} else {
|
||||
// STANDBY_ERROR and other states
|
||||
led_mode = led_control_s::MODE_BLINK_NORMAL;
|
||||
led_color = led_control_s::COLOR_RED;
|
||||
}
|
||||
|
||||
if (set_normal_color) {
|
||||
@@ -2352,7 +2298,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
|
||||
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
|
||||
|
||||
if (_arm_state_machine.isArmed()) {
|
||||
if (isArmed()) {
|
||||
if (_vehicle_status.failsafe) {
|
||||
BOARD_ARMED_LED_OFF();
|
||||
|
||||
@@ -2368,7 +2314,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
BOARD_ARMED_LED_ON();
|
||||
}
|
||||
|
||||
} else if (_arm_state_machine.isStandby()) {
|
||||
} else if (_vehicle_status.pre_flight_checks_pass) {
|
||||
BOARD_ARMED_LED_OFF();
|
||||
|
||||
// ready to arm, blink at 1Hz
|
||||
@@ -2404,7 +2350,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
void Commander::updateControlMode()
|
||||
{
|
||||
_vehicle_control_mode = {};
|
||||
mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state,
|
||||
mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state,
|
||||
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
|
||||
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
||||
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
|
||||
@@ -2426,7 +2372,7 @@ void Commander::printRejectMode(uint8_t nav_state)
|
||||
|
||||
/* only buzz if armed, because else we're driving people nuts indoors
|
||||
they really need to look at the leds as well. */
|
||||
tune_negative(_arm_state_machine.isArmed());
|
||||
tune_negative(isArmed());
|
||||
|
||||
_last_print_mode_reject_time = hrt_absolute_time();
|
||||
}
|
||||
@@ -2698,7 +2644,7 @@ void Commander::battery_status_check()
|
||||
if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
|
||||
if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) {
|
||||
if (!isArmed() && (px4_shutdown_request(60_s) == 0)) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t");
|
||||
events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
|
||||
"Dangerously low battery! Shutting system down");
|
||||
@@ -2731,7 +2677,7 @@ void Commander::manualControlCheck()
|
||||
_is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f);
|
||||
_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
|
||||
|
||||
if (_arm_state_machine.isArmed()) {
|
||||
if (isArmed()) {
|
||||
// Abort autonomous mode and switch to position mode if sticks are moved significantly
|
||||
// but only if actually in air.
|
||||
if (manual_control_setpoint.sticks_moving
|
||||
@@ -2794,10 +2740,9 @@ void Commander::send_parachute_command()
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
|
||||
vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
|
||||
|
||||
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
vcmd.source_system = vehicle_status_sub.get().system_id;
|
||||
vcmd.target_system = vehicle_status_sub.get().system_id;
|
||||
vcmd.source_component = vehicle_status_sub.get().component_id;
|
||||
vcmd.source_system = _vehicle_status.system_id;
|
||||
vcmd.target_system = _vehicle_status.system_id;
|
||||
vcmd.source_component = _vehicle_status.component_id;
|
||||
vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
|
||||
|
||||
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
||||
|
||||
Reference in New Issue
Block a user