diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 8ad5e81dbd..883ba8ebae 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -59,7 +59,6 @@ set(msg_files cellular_status.msg collision_constraints.msg collision_report.msg - commander_state.msg control_allocator_status.msg cpuload.msg differential_pressure.msg diff --git a/msg/action_request.msg b/msg/action_request.msg index a9c2f7258f..888814e0cc 100644 --- a/msg/action_request.msg +++ b/msg/action_request.msg @@ -16,4 +16,4 @@ uint8 SOURCE_RC_SWITCH = 1 uint8 SOURCE_RC_BUTTON = 2 uint8 SOURCE_RC_MODE_SLOT = 3 -uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_ +uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to vehicle_status_s::NAVIGATION_STATE_* diff --git a/msg/commander_state.msg b/msg/commander_state.msg deleted file mode 100644 index ca51625819..0000000000 --- a/msg/commander_state.msg +++ /dev/null @@ -1,24 +0,0 @@ -# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. -uint64 timestamp # time since system start (microseconds) - -uint8 MAIN_STATE_MANUAL = 0 -uint8 MAIN_STATE_ALTCTL = 1 -uint8 MAIN_STATE_POSCTL = 2 -uint8 MAIN_STATE_AUTO_MISSION = 3 -uint8 MAIN_STATE_AUTO_LOITER = 4 -uint8 MAIN_STATE_AUTO_RTL = 5 -uint8 MAIN_STATE_ACRO = 6 -uint8 MAIN_STATE_OFFBOARD = 7 -uint8 MAIN_STATE_STAB = 8 -# LEGACY RATTITUDE = 9 -uint8 MAIN_STATE_AUTO_TAKEOFF = 10 -uint8 MAIN_STATE_AUTO_LAND = 11 -uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 -uint8 MAIN_STATE_AUTO_PRECLAND = 13 -uint8 MAIN_STATE_ORBIT = 14 -uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF = 15 -uint8 MAIN_STATE_MAX = 16 - -uint8 main_state - -uint16 main_state_changes diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index d5f11afcb5..599dca6c1b 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -33,8 +33,9 @@ uint8 ARM_DISARM_REASON_UNIT_TEST = 13 uint64 nav_state_timestamp # time when current nav_state activated -# Navigation state "what should vehicle do" -uint8 nav_state +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode @@ -83,6 +84,7 @@ uint8 VEHICLE_TYPE_ROVER = 3 uint8 VEHICLE_TYPE_AIRSHIP = 4 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control uint64 failsafe_timestamp # time when failsafe was activated # Link loss @@ -97,8 +99,6 @@ bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotatio bool in_transition_mode # True if VTOL is doing a transition bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW -bool mission_failure # Set to true if mission could not continue/finish -bool geofence_violated # MAVLink identification uint8 system_type # system type, contains mavlink MAV_TYPE diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 88fb851f29..5d9e32807a 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -36,7 +36,6 @@ bool home_position_valid # Home position valid bool offboard_control_signal_lost # Offboard signal lost bool geofence_violated # Geofence violated -bool rc_signal_found_once bool rc_signal_lost # RC signal lost bool data_link_lost # Data link lost bool mission_failure # Mission failure @@ -46,5 +45,14 @@ bool vtol_transition_failure # VTOL transition failed bool battery_low_remaining_time bool battery_unhealthy -uint8 battery_warning +bool wind_limit_exceeded +bool flight_time_limit_exceeded + +# failure detector +bool fd_esc_arming_failure +bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) +bool fd_imbalanced_prop # Imbalanced propeller detected +bool fd_motor_failure + +uint8 battery_warning # Battery warning level diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 1a26b1f597..9a1c060f8e 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -276,41 +276,83 @@ } } }, - "failsafe_reason_t": { + "failsafe_cause_t": { "type": "uint8_t", - "description": "Reason for entering failsafe", + "description": "Cause for entering failsafe", "entries": { "0": { - "name": "no_rc", - "description": "No manual control stick input" + "name": "generic", + "description": "" }, "1": { - "name": "no_offboard", - "description": "No offboard control inputs" + "name": "rc_loss", + "description": "manual control loss" }, "2": { - "name": "no_rc_and_no_offboard", - "description": "No manual control stick and no offboard control inputs" + "name": "datalink_loss", + "description": "data link loss" }, "3": { - "name": "no_local_position", - "description": "No local position estimate" + "name": "low_battery_level", + "description": "low battery level" }, "4": { - "name": "no_global_position", - "description": "No global position estimate" + "name": "critical_battery_level", + "description": "critical battery level" }, "5": { - "name": "no_datalink", - "description": "No datalink" + "name": "emergency_battery_level", + "description": "emergency battery level" + } + } + }, + "failsafe_action_t": { + "type": "uint8_t", + "description": "failsafe action", + "entries": { + "0": { + "name": "none", + "description": "" + }, + "1": { + "name": "warn", + "description": "warning" + }, + "2": { + "name": "fallback_posctrl", + "description": "fallback to position control" + }, + "3": { + "name": "fallback_altctrl", + "description": "fallback to altitude control" + }, + "4": { + "name": "fallback_stabilized", + "description": "fallback to stabilized" + }, + "5": { + "name": "hold", + "description": "hold" }, "6": { - "name": "no_rc_and_no_datalink", - "description": "No RC and no datalink" + "name": "rtl", + "description": "RTL" }, "7": { - "name": "no_gps", - "description": "No valid GPS" + "name": "land", + "description": "land" + }, + "8": { + "name": "descend", + "description": "descend" + }, + "9": { + "name": "disarm", + "description": "disarm" + }, + "10": { + "name": "terminate", + "description": "terminate" } } }, @@ -373,6 +415,10 @@ "13": { "name": "rc_button", "description": "RC (button)" + }, + "14": { + "name": "failsafe", + "description": "failsafe" } } }, diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 53eda6991a..5fc8f2792e 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -41,7 +41,11 @@ #include #include -#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED +} transition_result_t; using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index df8168f06e..fdc5e949b1 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -52,12 +52,12 @@ px4_add_module( factory_calibration_storage.cpp gyro_calibration.cpp HomePosition.cpp + UserModeIntention.cpp level_calibration.cpp lm_fit.cpp mag_calibration.cpp rc_calibration.cpp Safety.cpp - state_machine_helper.cpp worker_thread.cpp DEPENDS circuit_breaker diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6ee7c0509f..50490454c6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -36,10 +36,6 @@ * * Main state machine / business logic * - * @TODO This application is currently in a rewrite process. Main changes: - * - Calibration routines are moved into the event system - * - Commander is rewritten as class - * - State machines will be model driven */ #include "Commander.hpp" @@ -50,8 +46,8 @@ #include "esc_calibration.h" #define DEFINE_GET_PX4_CUSTOM_MODE #include "px4_custom_mode.h" -#include "state_machine_helper.h" #include "ModeUtil/control_mode.hpp" +#include "ModeUtil/conversions.hpp" /* PX4 headers */ #include @@ -66,7 +62,6 @@ #include #include #include -#include #include #include @@ -480,7 +475,9 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); - PX4_INFO("navigation: %s", nav_state_names[_vehicle_status.nav_state]); + 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"); perf_print_counter(_loop_perf); perf_print_counter(_preflight_check_perf); return 0; @@ -528,89 +525,13 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r case arm_disarm_reason_t::unit_test: return "unit tests"; case arm_disarm_reason_t::rc_button: return "RC (button)"; + + case arm_disarm_reason_t::failsafe: return "failsafe"; } return ""; }; - -using navigation_mode_t = events::px4::enums::navigation_mode_t; - -static inline navigation_mode_t navigation_mode(uint8_t main_state) -{ - switch (main_state) { - case commander_state_s::MAIN_STATE_MANUAL: return navigation_mode_t::manual; - - case commander_state_s::MAIN_STATE_ALTCTL: return navigation_mode_t::altctl; - - case commander_state_s::MAIN_STATE_POSCTL: return navigation_mode_t::posctl; - - case commander_state_s::MAIN_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission; - - case commander_state_s::MAIN_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter; - - case commander_state_s::MAIN_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl; - - case commander_state_s::MAIN_STATE_ACRO: return navigation_mode_t::acro; - - case commander_state_s::MAIN_STATE_OFFBOARD: return navigation_mode_t::offboard; - - case commander_state_s::MAIN_STATE_STAB: return navigation_mode_t::stab; - - case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff; - - case commander_state_s::MAIN_STATE_AUTO_LAND: return navigation_mode_t::auto_land; - - case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target; - - case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland; - - case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit; - - case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff; - } - - static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff, - "enum definition mismatch"); - - return navigation_mode_t::unknown; -} - -static constexpr const char *main_state_str(uint8_t main_state) -{ - switch (main_state) { - case commander_state_s::MAIN_STATE_MANUAL: return "Manual"; - - case commander_state_s::MAIN_STATE_ALTCTL: return "Altitude"; - - case commander_state_s::MAIN_STATE_POSCTL: return "Position"; - - case commander_state_s::MAIN_STATE_AUTO_MISSION: return "Mission"; - - case commander_state_s::MAIN_STATE_AUTO_LOITER: return "Hold"; - - case commander_state_s::MAIN_STATE_AUTO_RTL: return "RTL"; - - case commander_state_s::MAIN_STATE_ACRO: return "Acro"; - - case commander_state_s::MAIN_STATE_OFFBOARD: return "Offboard"; - - case commander_state_s::MAIN_STATE_STAB: return "Stabilized"; - - case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return "Takeoff"; - - case commander_state_s::MAIN_STATE_AUTO_LAND: return "Land"; - - case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "Follow target"; - - case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "Precision land"; - - case commander_state_s::MAIN_STATE_ORBIT: return "Orbit"; - - default: return "Unknown"; - } -} - 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 @@ -622,7 +543,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (run_preflight_checks && !_arm_state_machine.isArmed()) { if (_vehicle_control_mode.flag_control_manual_enabled) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && - !_vehicle_status.rc_signal_lost && _is_throttle_above_center) { + !_vehicle_status_flags.rc_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}, @@ -632,7 +553,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } if (!_vehicle_control_mode.flag_control_climb_rate_enabled && - !_vehicle_status.rc_signal_lost && !_is_throttle_low + !_vehicle_status_flags.rc_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"), @@ -652,16 +573,6 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ tune_negative(true); return TRANSITION_DENIED; } - - if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL) - && !_vehicle_status_flags.home_position_valid) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t"); - events::send(events::ID("commander_arm_denied_geofence_rtl"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: Geofence RTL requires valid home"); - tune_negative(true); - return TRANSITION_DENIED; - } } transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, @@ -729,9 +640,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f } Commander::Commander() : - ModuleParams(nullptr), - _failure_detector(this), - _health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status) + ModuleParams(nullptr) { _vehicle_land_detected.landed = true; @@ -741,12 +650,11 @@ Commander::Commander() : _vehicle_status.system_type = 0; _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; - // We want to accept RC inputs as default - _vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + _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.rc_signal_lost = true; _vehicle_status.data_link_lost = true; _vehicle_status_flags.offboard_control_signal_lost = true; @@ -793,10 +701,7 @@ Commander::handle_command(const vehicle_command_s &cmd) // Check if a mode switch had been requested if ((((uint32_t)cmd.param2) & 1) > 0) { - transition_result_t main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, - _vehicle_status_flags, _commander_state); - - if ((main_ret != TRANSITION_DENIED)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -821,50 +726,50 @@ Commander::handle_command(const vehicle_command_s &cmd) uint8_t custom_main_mode = (uint8_t)cmd.param2; uint8_t custom_sub_mode = (uint8_t)cmd.param3; - uint8_t desired_main_state = commander_state_s::MAIN_STATE_MAX; + uint8_t desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX; transition_result_t main_ret = TRANSITION_NOT_CHANGED; if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - desired_main_state = commander_state_s::MAIN_STATE_MANUAL; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { - desired_main_state = commander_state_s::MAIN_STATE_ALTCTL; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { - desired_main_state = commander_state_s::MAIN_STATE_POSCTL; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { if (custom_sub_mode > 0) { switch (custom_sub_mode) { case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; break; case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; break; case PX4_CUSTOM_SUB_MODE_AUTO_RTL: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_RTL; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; break; case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; break; case PX4_CUSTOM_SUB_MODE_AUTO_LAND: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; break; case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET; break; case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: - desired_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; default: @@ -876,39 +781,44 @@ Commander::handle_command(const vehicle_command_s &cmd) } } else { - desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { - desired_main_state = commander_state_s::MAIN_STATE_ACRO; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { - desired_main_state = commander_state_s::MAIN_STATE_STAB; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { - desired_main_state = commander_state_s::MAIN_STATE_OFFBOARD; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } } else { /* use base mode */ if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - desired_main_state = commander_state_s::MAIN_STATE_POSCTL; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { - desired_main_state = commander_state_s::MAIN_STATE_STAB; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; } else { - desired_main_state = commander_state_s::MAIN_STATE_MANUAL; + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; } } } - if (desired_main_state != commander_state_s::MAIN_STATE_MAX) { - main_ret = main_state_transition(_vehicle_status, desired_main_state, _vehicle_status_flags, _commander_state); + if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) { + if (_user_mode_intention.change(desired_nav_state)) { + main_ret = TRANSITION_CHANGED; + + } else { + main_ret = TRANSITION_DENIED; + } } if (main_ret != TRANSITION_DENIED) { @@ -1051,9 +961,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { /* switch to RTL which ends the mission */ - if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, - _vehicle_status_flags, - _commander_state)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) { mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t"); events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch"); cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1072,12 +980,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { /* ok, home set, use it to take off */ - if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, - _vehicle_status_flags, - _commander_state)) { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1095,12 +998,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF: /* ok, home set, use it to take off */ - if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF, - _vehicle_status_flags, - _commander_state)) { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1111,9 +1009,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { - if (TRANSITION_DENIED != main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND, - _vehicle_status_flags, - _commander_state)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) { mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t"); events::send(events::ID("commander_landing_current_pos"), events::Log::Info, "Landing at current position"); @@ -1132,9 +1028,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { - if (TRANSITION_DENIED != main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, - _vehicle_status_flags, - _commander_state)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) { mavlink_log_info(&_mavlink_log_pub, "Precision landing\t"); events::send(events::ID("commander_landing_prec_land"), events::Log::Info, "Landing using precision landing"); @@ -1163,9 +1057,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { // switch to AUTO_MISSION and ARM - if ((TRANSITION_DENIED != main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, - _vehicle_status_flags, - _commander_state)) + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1209,16 +1101,24 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { // for fixed wings the behavior of orbit is the same as loiter - main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, - _vehicle_status_flags, _commander_state); + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + main_ret = TRANSITION_CHANGED; + + } else { + main_ret = TRANSITION_DENIED; + } } else { // Switch to orbit state and let the orbit task handle the command further - main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_ORBIT, _vehicle_status_flags, - _commander_state); + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) { + main_ret = TRANSITION_CHANGED; + + } else { + main_ret = TRANSITION_DENIED; + } } - if ((main_ret != TRANSITION_DENIED)) { + if (main_ret != TRANSITION_DENIED) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1660,18 +1560,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_SWITCH_MODE: - // if there's never been a mode change force RC switch as initial state - if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT - && !_arm_state_machine.isArmed() && (_commander_state.main_state_changes == 0) - && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL - || action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) { - _commander_state.main_state = action_request.mode; - _commander_state.main_state_changes++; - } - - int ret = main_state_transition(_vehicle_status, action_request.mode, _vehicle_status_flags, _commander_state); - - if (ret == transition_result_t::TRANSITION_DENIED) { + if (!_user_mode_intention.change(action_request.mode, true)) { print_reject_mode(action_request.mode); } @@ -1685,8 +1574,6 @@ void Commander::updateParameters() // update parameters from storage updateParams(); - get_circuit_breaker_params(); - int32_t value_int32 = 0; // MAV_SYS_ID => vehicle_status.system_id @@ -1808,19 +1695,9 @@ Commander::run() vtolStatusUpdate(); _home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed); - _vehicle_status_flags.home_position_valid = _home_position.valid(); handleAutoDisarm(); - if (_geofence_warning_action_on - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { - - // reset flag again when we switched out of it - _geofence_warning_action_on = false; - } - battery_status_check(); /* If in INIT state, try to proceed to STANDBY state */ @@ -1833,135 +1710,11 @@ Commander::run() checkForMissionUpdate(); - /* start geofence result check */ - if (_geofence_result_sub.update(&_geofence_result)) { - _vehicle_status.geofence_violated = _geofence_result.geofence_violated; - } - - const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0; - - // Geofence actions - if (_arm_state_machine.isArmed() - && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE) - && !in_low_battery_failsafe_delay) { - - // check for geofence violation transition - if (_geofence_result.geofence_violated && !_geofence_violated_prev) { - - switch (_geofence_result.geofence_action) { - case (geofence_result_s::GF_ACTION_NONE) : { - // do nothing - break; - } - - case (geofence_result_s::GF_ACTION_WARN) : { - // do nothing, mavlink critical messages are sent by navigator - break; - } - - case (geofence_result_s::GF_ACTION_LOITER) : { - if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, - _vehicle_status_flags, - _commander_state)) { - _geofence_loiter_on = true; - } - - break; - } - - case (geofence_result_s::GF_ACTION_RTL) : { - if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, - _vehicle_status_flags, - _commander_state)) { - _geofence_rtl_on = true; - } - - break; - } - - case (geofence_result_s::GF_ACTION_LAND) : { - if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND, - _vehicle_status_flags, - _commander_state)) { - _geofence_land_on = true; - } - - break; - } - - case (geofence_result_s::GF_ACTION_TERMINATE) : { - PX4_WARN("Flight termination because of geofence"); - - if (!_flight_termination_triggered && !_lockdown_triggered) { - _flight_termination_triggered = true; - mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated\t"); - events::send(events::ID("commander_geofence_termination"), {events::Log::Alert, events::LogInternal::Warning}, - "Geofence violation! Flight terminated"); - _actuator_armed.force_failsafe = true; - _status_changed = true; - send_parachute_command(); - } - - break; - } - } - } - - _geofence_violated_prev = _geofence_result.geofence_violated; - - // reset if no longer in LOITER or if manually switched to LOITER - const bool in_loiter_mode = _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; - - if (!in_loiter_mode) { - _geofence_loiter_on = false; - } - - - // reset if no longer in RTL or if manually switched to RTL - const bool in_rtl_mode = _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; - - if (!in_rtl_mode) { - _geofence_rtl_on = false; - } - - // reset if no longer in LAND or if manually switched to LAND - const bool in_land_mode = _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND; - - if (!in_land_mode) { - _geofence_land_on = false; - } - - _geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on - || _geofence_land_on); - - } else { - // No geofence checks, reset flags - _geofence_loiter_on = false; - _geofence_rtl_on = false; - _geofence_land_on = false; - _geofence_warning_action_on = false; - _geofence_violated_prev = false; - } - manual_control_check(); // data link checks which update the status data_link_check(); - /* check if we are disarmed and there is a better mode to wait in */ - if (!_arm_state_machine.isArmed()) { - /* if there is no radio control but GPS lock the user might want to fly using - * just a tablet. Since the RC will force its mode switch setting on connecting - * we can as well just wait in a hold mode which enables tablet control. - */ - if (_vehicle_status.rc_signal_lost && (_commander_state.main_state_changes == 0) - && _vehicle_status_flags.global_position_valid) { - - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags, - _commander_state); - } - } - /* handle commands last, as the system needs to be updated to handle them */ if (_vehicle_command_sub.updated()) { /* got command */ @@ -1994,158 +1747,8 @@ Commander::run() /* Check for failure detector status */ if (_failure_detector.update(_vehicle_status, _vehicle_control_mode)) { - const bool motor_failure_changed = ((_vehicle_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) != - _failure_detector.getStatus().flags.motor; _vehicle_status.failure_detector_status = _failure_detector.getStatus().value; - auto fd_status_flags = _failure_detector.getStatusFlags(); _status_changed = true; - - if (_arm_state_machine.isArmed()) { - if (fd_status_flags.arm_escs) { - // Checks have to pass within the spool up time - if (hrt_elapsed_time(&_vehicle_status.armed_time) < _param_com_spoolup_time.get() * 1_s) { - disarm(arm_disarm_reason_t::failure_detector); - mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t"); - events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request"); - } - } - - if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) { - const bool is_right_after_takeoff = hrt_elapsed_time(&_vehicle_status.takeoff_time) < - (1_s * _param_com_lkdown_tko.get()); - - if (is_right_after_takeoff && !_lockdown_triggered) { - // This handles the case where something fails during the early takeoff phase - _actuator_armed.lockdown = true; - _lockdown_triggered = true; - mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown\t"); - /* EVENT - * @description - * When a critical failure is detected right after takeoff, the system turns off the motors. - * Failures include an exceeding tilt angle, altitude failure or an external failure trigger. - * - * - * This can be configured with the parameter COM_LKDOWN_TKO. - * - */ - events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning}, - "Critical failure detected: lockdown"); - - } else if (!_circuit_breaker_flight_termination_disabled && - !_flight_termination_triggered && !_lockdown_triggered) { - - _actuator_armed.force_failsafe = true; - _flight_termination_triggered = true; - mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight\t"); - /* EVENT - * @description - * Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger. - * - * - * Flight termination can be disabled with the parameter CBRK_FLIGHTTERM. - * - */ - events::send(events::ID("commander_fd_terminate"), {events::Log::Emergency, events::LogInternal::Warning}, - "Critical failure detected: terminate flight"); - send_parachute_command(); - } - } - - if (fd_status_flags.imbalanced_prop - && !_imbalanced_propeller_check_triggered) { - _status_changed = true; - _imbalanced_propeller_check_triggered = true; - imbalanced_prop_failsafe(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, &_commander_state, - (imbalanced_propeller_action_t)_param_com_imb_prop_act.get()); - } - } - - // One-time actions based on motor failure - if (motor_failure_changed) { - if (fd_status_flags.motor) { - mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t"); - events::send(events::ID("commander_motor_failure"), events::Log::Emergency, - "Motor failure! Land immediately"); - - } else { - mavlink_log_critical(&_mavlink_log_pub, "Motor recovered\t"); - events::send(events::ID("commander_motor_recovered"), events::Log::Warning, - "Motor recovered, landing still advised"); - } - } - - if (fd_status_flags.motor) { - switch ((ActuatorFailureActions)_param_com_actuator_failure_act.get()) { - case ActuatorFailureActions::AUTO_LOITER: - mavlink_log_critical(&_mavlink_log_pub, "Loitering due to actuator failure\t"); - events::send(events::ID("commander_act_failure_loiter"), events::Log::Warning, - "Loitering due to actuator failure"); - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_POSCTL, _vehicle_status_flags, _commander_state); - _status_changed = true; - break; - - case ActuatorFailureActions::AUTO_LAND: - mavlink_log_critical(&_mavlink_log_pub, "Landing due to actuator failure\t"); - events::send(events::ID("commander_act_failure_land"), events::Log::Warning, - "Landing due to actuator failure"); - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND, _vehicle_status_flags, - _commander_state); - _status_changed = true; - break; - - case ActuatorFailureActions::AUTO_RTL: - mavlink_log_critical(&_mavlink_log_pub, "Returning home due to actuator failure\t"); - events::send(events::ID("commander_act_failure_rtl"), events::Log::Warning, - "Returning home due to actuator failure"); - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state); - _status_changed = true; - break; - - case ActuatorFailureActions::TERMINATE: - if (!_actuator_armed.manual_lockdown) { - mavlink_log_critical(&_mavlink_log_pub, "Flight termination due to actuator failure\t"); - events::send(events::ID("commander_act_failure_term"), events::Log::Warning, - "Flight termination due to actuator failure"); - _status_changed = true; - _actuator_armed.manual_lockdown = true; - send_parachute_command(); - } - - break; - - default: - // nothing to do here - break; - } - - } - } - - // Check wind speed actions if either high wind warning or high wind RTL is enabled - if ((_param_com_wind_warn.get() > FLT_EPSILON || _param_com_wind_max.get() > FLT_EPSILON) - && !_vehicle_land_detected.landed) { - checkWindSpeedThresholds(); - } - - /* Get current timestamp */ - const hrt_abstime now = hrt_absolute_time(); - - // Trigger RTL if flight time is larger than max flight time specified in COM_FLT_TIME_MAX. - // The user is not able to override once above threshold, except for triggering Land. - if (!_vehicle_land_detected.landed - && _param_com_flt_time_max.get() > FLT_EPSILON - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND - && (now - _vehicle_status.takeoff_time) > (1_s * _param_com_flt_time_max.get())) { - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state); - _status_changed = true; - mavlink_log_critical(&_mavlink_log_pub, "Maximum flight time reached, abort operation and RTL"); - /* EVENT - * @description - * Maximal flight time reached, return to launch. - */ - events::send(events::ID("commander_max_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Warning}, - "Maximum flight time reached, abort operation and RTL"); } // check for arming state changes @@ -2164,60 +1767,20 @@ Commander::run() _last_disarmed_timestamp = hrt_absolute_time(); - // Switch back to Hold mode after autonomous landing - if (_vehicle_control_mode.flag_control_auto_enabled) { - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, - _vehicle_status_flags, _commander_state); - } + _user_mode_intention.onDisarm(); } if (!_arm_state_machine.isArmed()) { /* Reset the flag if disarmed. */ _have_taken_off_since_arming = false; - _imbalanced_propeller_check_triggered = false; } - /* now set navigation state according to failsafe and main state */ - bool nav_state_changed = set_nav_state(_vehicle_status, - _actuator_armed, - _commander_state, - &_mavlink_log_pub, - static_cast(_param_nav_dll_act.get()), - _mission_result_sub.get().finished, - _mission_result_sub.get().stay_in_failsafe, - _vehicle_status_flags, - _vehicle_land_detected.landed, - static_cast(_param_nav_rcl_act.get()), - static_cast(_param_com_obl_act.get()), - static_cast(_param_com_qc_act.get()), - static_cast(_param_com_obl_rc_act.get()), - static_cast(_param_com_posctl_navl.get()), - _param_com_rcl_act_t.get(), - _param_com_rcl_except.get()); - - if (nav_state_changed) { - _vehicle_status.nav_state_timestamp = hrt_absolute_time(); - } - - if (_vehicle_status.failsafe != _failsafe_old) { - _status_changed = true; - - if (_vehicle_status.failsafe) { - mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated\t"); - events::send(events::ID("commander_failsafe_activated"), events::Log::Info, "Failsafe mode activated"); - - } else { - mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated\t"); - events::send(events::ID("commander_failsafe_deactivated"), events::Log::Info, "Failsafe mode deactivated"); - } - - _failsafe_old = _vehicle_status.failsafe; - } + const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe(); _actuator_armed.prearmed = getPrearmState(); - // publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed - if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_changed + // publish states (armed, control_mode, vehicle_status, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed + if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_or_failsafe_changed || !(_actuator_armed == actuator_armed_prev)) { // Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already) @@ -2249,10 +1812,6 @@ Commander::run() _vehicle_status_flags.timestamp = hrt_absolute_time(); _vehicle_status_flags_pub.publish(_vehicle_status_flags); - // commander_state publish internal state for logging purposes - _commander_state.timestamp = hrt_absolute_time(); - _commander_state_pub.publish(_commander_state); - // failure_detector_status publish failure_detector_status_s fd_status{}; fd_status.fd_roll = _failure_detector.getStatusFlags().roll; @@ -2278,7 +1837,7 @@ Commander::run() _was_armed = _arm_state_machine.isArmed(); - arm_auth_update(now, params_updated); + arm_auth_update(hrt_absolute_time(), params_updated); px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed()); @@ -2312,18 +1871,6 @@ void Commander::checkForMissionUpdate() _vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid; if (mission_result_ok) { - if (_vehicle_status.mission_failure != mission_result.failure) { - _vehicle_status.mission_failure = mission_result.failure; - _status_changed = true; - - if (_vehicle_status.mission_failure) { - // navigator sends out the exact reason - mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed\t"); - events::send(events::ID("commander_mission_cannot_be_completed"), {events::Log::Critical, events::LogInternal::Info}, - "Mission cannot be completed"); - } - } - /* Only evaluate mission state if home is set */ if (_vehicle_status_flags.home_position_valid && (prev_mission_instance_count != mission_result.instance_count)) { @@ -2343,7 +1890,7 @@ void Commander::checkForMissionUpdate() } } - // Transition main state to loiter or auto-mission after takeoff is completed. + // Transition mode to loiter or auto-mission after takeoff is completed. if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed && (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) @@ -2351,12 +1898,10 @@ void Commander::checkForMissionUpdate() && mission_result.finished) { if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) { - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags, - _commander_state); + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); } else { - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags, - _commander_state); + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); } } @@ -2541,10 +2086,6 @@ void Commander::vtolStatusUpdate() _status_changed = true; } - if (_vehicle_status_flags.vtol_transition_failure != _vtol_vehicle_status.vtol_transition_failsafe) { - _vehicle_status_flags.vtol_transition_failure = _vtol_vehicle_status.vtol_transition_failsafe; - _status_changed = true; - } } } @@ -2663,12 +2204,65 @@ void Commander::handleAutoDisarm() } } -void -Commander::get_circuit_breaker_params() +bool Commander::handleModeIntentionAndFailsafe() { - _circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val( - _param_cbrk_flightterm.get(), - CBRK_FLIGHTTERM_KEY); + const uint8_t prev_nav_state = _vehicle_status.nav_state; + const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction(); + + FailsafeBase::State state{}; + state.armed = _arm_state_machine.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(); + state.vehicle_type = _vehicle_status.vehicle_type; + + // There might have been a mode change request without changing the user intended mode. + // If a failsafe is active we must pass the request along as it might lead to a user-takeover. + bool mode_change_requested = _user_mode_intention.getHadModeChangeAndClear(); + + uint8_t updated_user_intented_mode = _failsafe.update(hrt_absolute_time(), state, mode_change_requested, + _failsafe_user_override_request, + _vehicle_status_flags); + _failsafe_user_override_request = false; + + // Force intended mode if changed by the failsafe state machine + if (state.user_intended_mode != updated_user_intented_mode) { + _user_mode_intention.change(updated_user_intented_mode, false, true); + _user_mode_intention.getHadModeChangeAndClear(); + } + + // Handle failsafe action + _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); + _vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get()); + + switch (_failsafe.selectedAction()) { + case FailsafeBase::Action::Disarm: + disarm(arm_disarm_reason_t::failsafe, true); + break; + + 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: + break; + } + + _vehicle_status.failsafe = _failsafe.inFailsafe(); + _vehicle_status.failsafe_and_user_took_over = _failsafe.userTakeoverActive(); + + if (prev_nav_state != _vehicle_status.nav_state) { + _vehicle_status.nav_state_timestamp = hrt_absolute_time(); + } + + return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction(); } void Commander::check_and_inform_ready_for_takeoff() @@ -2849,16 +2443,17 @@ Commander::update_control_mode() } void -Commander::print_reject_mode(uint8_t main_state) +Commander::print_reject_mode(uint8_t nav_state) { if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) { - mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available\t", main_state_str(main_state)); + mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available\t", + mode_util::nav_state_names[nav_state]); /* EVENT * @description Check for a valid position estimate */ events::send(events::ID("commander_modeswitch_not_avail"), {events::Log::Critical, events::LogInternal::Info}, - "Switching to mode '{1}' is currently not possible", navigation_mode(main_state)); + "Switching to mode '{1}' is currently not possible", mode_util::navigation_mode(nav_state)); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -3128,85 +2723,10 @@ void Commander::data_link_check() void Commander::battery_status_check() { - // Compare estimate of RTL time to estimate of remaining flight time - if (_vehicle_status_flags.battery_low_remaining_time - && _arm_state_machine.isArmed() - && !_vehicle_land_detected.ground_contact // not in any landing stage - && !_rtl_time_actions_done - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { - // Try to trigger RTL - if (main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, - _commander_state) == TRANSITION_CHANGED) { - mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, returning to land\t"); - events::send(events::ID("commander_remaining_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Info}, - "Remaining flight time low, returning to land"); - - } else { - mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, land now!\t"); - events::send(events::ID("commander_remaining_flight_time_land"), {events::Log::Critical, events::LogInternal::Info}, - "Remaining flight time low, land now!"); - } - - _rtl_time_actions_done = true; - } - - bool battery_warning_level_increased_while_armed = false; - bool update_internal_battery_state = false; - - if (_arm_state_machine.isArmed()) { - if (_vehicle_status_flags.battery_warning > _battery_warning) { - battery_warning_level_increased_while_armed = true; - update_internal_battery_state = true; - } - - } else { - if (_battery_warning != _vehicle_status_flags.battery_warning) { - update_internal_battery_state = true; - } - } - - if (update_internal_battery_state) { - _battery_warning = _vehicle_status_flags.battery_warning; - } - - // execute battery failsafe if the state has gotten worse while we are armed - if (battery_warning_level_increased_while_armed) { - uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning, - (low_battery_action_t)_param_com_low_bat_act.get()); - - warn_user_about_battery(&_mavlink_log_pub, _battery_warning, - failsafe_action, _param_com_bat_act_t.get(), - main_state_str(failsafe_action), navigation_mode(failsafe_action)); - _battery_failsafe_timestamp = hrt_absolute_time(); - - // Switch to loiter to wait for the reaction delay - if (_param_com_bat_act_t.get() > 0.f - && failsafe_action != commander_state_s::MAIN_STATE_MAX) { - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags, - _commander_state); - } - } - - if (_battery_failsafe_timestamp != 0 - && hrt_elapsed_time(&_battery_failsafe_timestamp) > _param_com_bat_act_t.get() * 1_s - && (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER - || _vehicle_control_mode.flag_control_auto_enabled)) { - _battery_failsafe_timestamp = 0; - uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning, - (low_battery_action_t)_param_com_low_bat_act.get()); - - if (failsafe_action != commander_state_s::MAIN_STATE_MAX) { - _commander_state.main_state = failsafe_action; - _commander_state.main_state_changes++; - _commander_state.timestamp = hrt_absolute_time(); - } - } - // Handle shutdown request from emergency battery action - if (update_internal_battery_state) { + if (_battery_warning != _vehicle_status_flags.battery_warning) { - if (_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + if (_vehicle_status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) if (shutdown_if_allowed() && (px4_shutdown_request(60_s) == 0)) { @@ -3228,6 +2748,8 @@ void Commander::battery_status_check() #endif // BOARD_HAS_POWER_CONTROL } } + + _battery_warning = _vehicle_status_flags.battery_warning; } void Commander::manual_control_check() @@ -3237,24 +2759,6 @@ void Commander::manual_control_check() if (manual_control_updated && manual_control_setpoint.valid) { - if (!_vehicle_status_flags.rc_signal_found_once) { - _vehicle_status_flags.rc_signal_found_once = true; - - } else if (_vehicle_status.rc_signal_lost) { - if (_last_valid_manual_control_setpoint > 0) { - float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f; - mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed); - events::send(events::ID("commander_rc_regained"), events::Log::Info, - "Manual control regained after {1:.1} s", elapsed); - } - } - - if (_vehicle_status.rc_signal_lost) { - _vehicle_status.rc_signal_lost = false; - _status_changed = true; - } - - _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; _is_throttle_above_center = (manual_control_setpoint.z > 0.6f); _is_throttle_low = (manual_control_setpoint.z < 0.1f); @@ -3279,32 +2783,17 @@ void Commander::manual_control_check() } } - const bool in_low_battery_failsafe_delay = (_battery_failsafe_timestamp != 0); - - if (override_enabled && !in_low_battery_failsafe_delay && !_geofence_warning_action_on) { - - const transition_result_t posctl_result = - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_POSCTL, _vehicle_status_flags, _commander_state); - - if (posctl_result == TRANSITION_CHANGED) { - tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t"); - events::send(events::ID("commander_rc_override_pos"), events::Log::Info, - "Pilot took over position control using sticks"); - _status_changed = true; - - } else if (posctl_result == TRANSITION_DENIED) { - // If transition to POSCTL was denied, then we can try again with ALTCTL. - const transition_result_t altctl_result = - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_ALTCTL, _vehicle_status_flags, _commander_state); - - if (altctl_result == TRANSITION_CHANGED) { + if (override_enabled) { + // If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine + if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, true)) { tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t"); - events::send(events::ID("commander_rc_override_alt"), events::Log::Info, - "Pilot took over altitude control using sticks"); - _status_changed = true; + mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t"); + events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks"); } + + } else { + _failsafe_user_override_request = true; } } } @@ -3312,28 +2801,11 @@ void Commander::manual_control_check() } else { const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC); - // disarmed // if there's never been a mode change force position control as initial state - if (_commander_state.main_state_changes == 0) { - if (is_mavlink || !_mode_switch_mapped) { - _commander_state.main_state = commander_state_s::MAIN_STATE_POSCTL; - _commander_state.main_state_changes++; - } + if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) { + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true); } } - - } else if ((manual_control_updated && !manual_control_setpoint.valid) - || hrt_elapsed_time(&_last_valid_manual_control_setpoint) > _param_com_rc_loss_t.get() * 1_s) { - - // prohibit stick use in case of reported invalidity or data timeout - if (!_vehicle_status.rc_signal_lost) { - _vehicle_status.rc_signal_lost = true; - _status_changed = true; - - mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t"); - events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, - "Manual control lost"); - } } } @@ -3404,46 +2876,6 @@ void Commander::send_parachute_command() set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } -void Commander::checkWindSpeedThresholds() -{ - wind_s wind_estimate; - - if (_wind_sub.update(&wind_estimate)) { - const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east); - - // publish a warning if it's the first since in air or 60s have passed since the last warning - const bool warning_timeout_passed = _last_wind_warning == 0 || hrt_elapsed_time(&_last_wind_warning) > 60_s; - - if (_param_com_wind_max.get() > FLT_EPSILON - && wind.longerThan(_param_com_wind_max.get()) - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { - - main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state); - _status_changed = true; - mavlink_log_critical(&_mavlink_log_pub, "Wind speeds above limit, abort operation and RTL (%.1f m/s)\t", - (double)wind.norm()); - - events::send(events::ID("commander_high_wind_rtl"), - {events::Log::Warning, events::LogInternal::Info}, - "Wind speeds above limit, abort operation and RTL ({1:.1m/s})", wind.norm()); - - } else if (_param_com_wind_warn.get() > FLT_EPSILON - && wind.longerThan(_param_com_wind_warn.get()) - && warning_timeout_passed - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { - - mavlink_log_critical(&_mavlink_log_pub, "High wind speed detected (%.1f m/s), landing advised\t", (double)wind.norm()); - - events::send(events::ID("commander_high_wind_warning"), - {events::Log::Warning, events::LogInternal::Info}, - "High wind speed detected ({1:.1m/s}), landing advised", wind.norm()); - _last_wind_warning = hrt_absolute_time(); - } - } -} - int Commander::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e3c7e0bacb..b271ae12c7 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -36,11 +36,12 @@ /* Helper classes */ #include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "failure_detector/FailureDetector.hpp" +#include "failsafe/failsafe.h" #include "Safety.hpp" -#include "state_machine_helper.h" #include "worker_thread.hpp" #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include "HomePosition.hpp" +#include "UserModeIntention.hpp" #include #include @@ -68,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -84,7 +84,6 @@ #include #include #include -#include using math::constrain; using systemlib::Hysteresis; @@ -117,8 +116,6 @@ public: void enable_hil(); - void get_circuit_breaker_params(); - private: void answer_command(const vehicle_command_s &cmd, uint8_t result); @@ -152,7 +149,7 @@ private: void offboard_control_update(); - void print_reject_mode(uint8_t main_state); + void print_reject_mode(uint8_t nav_state); void update_control_mode(); @@ -160,7 +157,6 @@ private: void send_parachute_command(); - void checkWindSpeedThresholds(); void checkForMissionUpdate(); void handlePowerButtonState(); void systemPowerUpdate(); @@ -172,6 +168,7 @@ private: bool getPrearmState() const; void handleAutoDisarm(); + bool handleModeIntentionAndFailsafe(); void updateParameters(); @@ -179,7 +176,6 @@ private: DEFINE_PARAMETERS( - (ParamInt) _param_nav_dll_act, (ParamInt) _param_com_dl_loss_t, (ParamInt) _param_com_rc_override, @@ -187,19 +183,9 @@ private: (ParamInt) _param_com_hldl_loss_t, (ParamInt) _param_com_hldl_reg_t, - (ParamFloat) _param_com_rc_loss_t, - (ParamInt) _param_nav_rcl_act, - (ParamFloat) _param_com_rcl_act_t, - (ParamInt) _param_com_rcl_except, - (ParamBool) _param_com_home_en, (ParamBool) _param_com_home_in_air, - (ParamInt) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */ - - (ParamInt) _param_com_low_bat_act, - (ParamFloat) _param_com_bat_act_t, - (ParamInt) _param_com_imb_prop_act, (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_preflight, @@ -209,40 +195,17 @@ private: (ParamFloat) _param_com_obc_loss_t, - (ParamFloat) _param_com_wind_warn, - - // Quadchute - (ParamInt) _param_com_qc_act, - // Offboard (ParamFloat) _param_com_of_loss_t, - (ParamInt) _param_com_obl_act, - (ParamInt) _param_com_obl_rc_act, (ParamInt) _param_com_prearm_mode, (ParamBool) _param_com_force_safety, (ParamBool) _param_com_mot_test_en, (ParamFloat) _param_com_kill_disarm, - (ParamFloat) _param_com_lkdown_tko, - - // Engine failure - (ParamInt) _param_com_actuator_failure_act, (ParamInt) _param_flight_uuid, - (ParamInt) _param_takeoff_finished_action, - - // Circuit breakers - (ParamInt) _param_cbrk_supply_chk, - (ParamInt) _param_cbrk_usb_chk, - (ParamInt) _param_cbrk_airspd_chk, - (ParamInt) _param_cbrk_flightterm, - (ParamInt) _param_cbrk_vtolarming, - - (ParamInt) _param_com_flt_time_max, - (ParamFloat) _param_com_wind_max, - - (ParamFloat) _param_com_spoolup_time + (ParamInt) _param_takeoff_finished_action ) // optional parameters @@ -262,34 +225,16 @@ private: OFFBOARD_MODE_BIT = (1 << 1), }; - enum class ActuatorFailureActions { - DISABLED = 0, - AUTO_LOITER = 1, - AUTO_LAND = 2, - AUTO_RTL = 3, - TERMINATE = 4, - }; - /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; ArmStateMachine _arm_state_machine{}; - bool _geofence_loiter_on{false}; - bool _geofence_rtl_on{false}; - bool _geofence_land_on{false}; - bool _geofence_warning_action_on{false}; - bool _geofence_violated_prev{false}; - bool _circuit_breaker_flight_termination_disabled{false}; - - bool _rtl_time_actions_done{false}; - - FailureDetector _failure_detector; + FailureDetector _failure_detector{this}; bool _flight_termination_triggered{false}; bool _lockdown_triggered{false}; - bool _imbalanced_propeller_check_triggered{false}; hrt_abstime _datalink_last_heartbeat_gcs{0}; @@ -306,7 +251,6 @@ private: hrt_abstime _high_latency_datalink_lost{0}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; - hrt_abstime _battery_failsafe_timestamp{0}; Hysteresis _auto_disarm_landed{false}; Hysteresis _auto_disarm_killed{false}; Hysteresis _offboard_available{false}; @@ -316,8 +260,6 @@ private: bool _last_overload{false}; - hrt_abstime _last_valid_manual_control_setpoint{0}; - bool _is_throttle_above_center{false}; bool _is_throttle_low{false}; @@ -333,18 +275,13 @@ private: bool _status_changed{true}; bool _arm_tune_played{false}; bool _was_armed{false}; - bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag bool _have_taken_off_since_arming{false}; - geofence_result_s _geofence_result{}; vehicle_land_detected_s _vehicle_land_detected{}; vtol_vehicle_status_s _vtol_vehicle_status{}; - hrt_abstime _last_wind_warning{0}; - // commander publications actuator_armed_s _actuator_armed{}; - commander_state_s _commander_state{}; vehicle_control_mode_s _vehicle_control_mode{}; vehicle_status_s _vehicle_status{}; vehicle_status_flags_s _vehicle_status_flags{}; @@ -356,14 +293,12 @@ private: // Subscriptions uORB::Subscription _action_request_sub {ORB_ID(action_request)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; - uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -378,7 +313,6 @@ private: // Publications uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; - uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; uORB::Publication _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)}; @@ -391,6 +325,11 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")}; - HealthAndArmingChecks _health_and_arming_checks; + + HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status_flags, _vehicle_status}; HomePosition _home_position{_vehicle_status_flags}; + Failsafe _failsafe_instance{this}; + FailsafeBase &_failsafe{_failsafe_instance}; + bool _failsafe_user_override_request{false}; ///< override request due to stick movements + UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; }; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index c631e19f06..6daf570504 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -53,6 +53,13 @@ px4_add_library(health_and_arming_checks checks/sdcardCheck.cpp checks/parachuteCheck.cpp checks/batteryCheck.cpp + checks/windCheck.cpp + checks/geofenceCheck.cpp + checks/homePositionCheck.cpp + checks/flightTimeCheck.cpp + checks/missionCheck.cpp + checks/rcAndDataLinkCheck.cpp + checks/vtolCheck.cpp ) add_dependencies(health_and_arming_checks mode_util) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 48b2f31cf8..b976cf5cd3 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -254,6 +254,8 @@ public: const HealthResults &healthResults() const { return _results[_current_result].health; } const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; } + + bool modePreventsArming(uint8_t nav_state) const { return _status_flags.mode_req_prevent_arming & (1u << nav_state); } private: /** diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 64247a7ce4..fcefe8d5bd 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -51,6 +51,7 @@ #include "checks/imuConsistencyCheck.hpp" #include "checks/magnetometerCheck.hpp" #include "checks/manualControlCheck.hpp" +#include "checks/homePositionCheck.hpp" #include "checks/modeCheck.hpp" #include "checks/parachuteCheck.hpp" #include "checks/powerCheck.hpp" @@ -58,6 +59,12 @@ #include "checks/sdcardCheck.hpp" #include "checks/systemCheck.hpp" #include "checks/batteryCheck.hpp" +#include "checks/windCheck.hpp" +#include "checks/geofenceCheck.hpp" +#include "checks/flightTimeCheck.hpp" +#include "checks/missionCheck.hpp" +#include "checks/rcAndDataLinkCheck.hpp" +#include "checks/vtolCheck.hpp" class HealthAndArmingChecks : public ModuleParams @@ -84,6 +91,11 @@ public: */ bool canRun(uint8_t nav_state) const { return _reporter.canRun(nav_state); } + /** + * Query the mode requirements: check if a mode prevents arming + */ + bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); } + protected: void updateParams() override; private: @@ -106,6 +118,7 @@ private: ImuConsistencyChecks _imu_consistency_checks; MagnetometerChecks _magnetometer_checks; ManualControlChecks _manual_control_checks; + HomePositionChecks _home_position_checks; ModeChecks _mode_checks; ParachuteChecks _parachute_checks; PowerChecks _power_checks; @@ -113,6 +126,12 @@ private: SdCardChecks _sd_card_checks; SystemChecks _system_checks; BatteryChecks _battery_checks; + WindChecks _wind_checks; + GeofenceChecks _geofence_checks; + FlightTimeChecks _flight_time_checks; + MissionChecks _mission_checks; + RcAndDataLinkChecks _rc_and_data_link_checks; + VtolChecks _vtol_checks; HealthAndArmingCheckBase *_checks[30] = { &_accelerometer_checks, @@ -127,13 +146,20 @@ private: &_imu_consistency_checks, &_magnetometer_checks, &_manual_control_checks, - &_mode_checks, // must be after _estimator_checks + &_home_position_checks, + &_mode_checks, // must be after _estimator_checks & _home_position_checks &_parachute_checks, &_power_checks, &_rc_calibration_checks, &_sd_card_checks, - &_system_checks, + &_system_checks, // must be after _estimator_checks & _home_position_checks &_battery_checks, + &_wind_checks, + &_geofence_checks, // must be after _home_position_checks + &_flight_time_checks, + &_mission_checks, + &_rc_and_data_link_checks, + &_vtol_checks, }; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index cbdab2191c..2e9678fd21 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -199,7 +199,8 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_warning = worst_warning; } - if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) { + if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE + && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) { bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning; @@ -222,12 +223,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // There is at least one connected battery (in any slot) || num_connected_batteries < battery_required_count // No currently-connected batteries have any fault - || battery_has_fault; + || battery_has_fault + || reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED; if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already /* EVENT * @description - * Make sure all batteries are connected. + * Make sure all batteries are connected and operational. */ reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"), events::Log::Error, "Battery unhealthy"); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp index 26acd0e65b..6dbfcdffee 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp @@ -35,81 +35,125 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter) { - if (context.status().failure_detector_status != vehicle_status_s::FAILURE_NONE) { - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) { - /* EVENT - * @description - * The vehicle exceeded the maximum configured roll angle. - * - * - * This check can be configured via FD_FAIL_R parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"), - events::Log::Critical, "Attitude failure detected"); + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) { + /* EVENT + * @description + * The vehicle exceeded the maximum configured roll angle. + * + * + * This check can be configured via FD_FAIL_R parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"), + events::Log::Critical, "Attitude failure detected"); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)"); - } - - } else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) { - /* EVENT - * @description - * The vehicle exceeded the maximum configured pitch angle. - * - * - * This check can be configured via FD_FAIL_P parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"), - events::Log::Critical, "Attitude failure detected"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)"); - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)"); } - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) { - /* EVENT - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"), - events::Log::Critical, "Altitude failure detected"); + } else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) { + /* EVENT + * @description + * The vehicle exceeded the maximum configured pitch angle. + * + * + * This check can be configured via FD_FAIL_P parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"), + events::Log::Critical, "Attitude failure detected"); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure"); - } - } - - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) { - /* EVENT - * @description - * - * This check can be configured via FD_EXT_ATS_EN parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"), - events::Log::Critical, "Failure triggered by external system"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system"); - } - } - - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { - /* EVENT - * @description - * One or more ESCs failed to arm. - * - * - * This check can be configured via FD_ESCS_EN parameter. - * - */ - reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"), - events::Log::Critical, "ESC failure"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected"); - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)"); } } + + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"), + events::Log::Critical, "Altitude failure detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure"); + } + } + + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) { + /* EVENT + * @description + * + * This check can be configured via FD_EXT_ATS_EN parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"), + events::Log::Critical, "Failure triggered by external system"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system"); + } + } + + reporter.failsafeFlags().fd_critical_failure = context.status().failure_detector_status & + (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT | + vehicle_status_s::FAILURE_EXT); + + + reporter.failsafeFlags().fd_esc_arming_failure = context.status().failure_detector_status & + vehicle_status_s::FAILURE_ARM_ESC; + + if (reporter.failsafeFlags().fd_esc_arming_failure) { + /* EVENT + * @description + * One or more ESCs failed to arm. + * + * + * This check can be configured via FD_ESCS_EN parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"), + events::Log::Critical, "ESC failure"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected"); + } + } + + reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status & + vehicle_status_s::FAILURE_IMBALANCED_PROP; + + if (reporter.failsafeFlags().fd_imbalanced_prop) { + /* EVENT + * @description + * Check that all propellers are mounted correctly and are not damaged. + * + * + * This check can be configured via FD_IMB_PROP_THR and COM_IMB_PROP_ACT parameters. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_imbalanced_prop"), + events::Log::Critical, "Imbalanced propeller detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Imbalanced propeller detected"); + } + } + + reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR; + + if (reporter.failsafeFlags().fd_motor_failure) { + /* EVENT + * @description + * + * This check can be configured via FD_ACT_EN parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_motor"), + events::Log::Critical, "Motor failure detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor failure detected"); + } + } + } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp new file mode 100644 index 0000000000..85b68a8485 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "flightTimeCheck.hpp" + +void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (_param_com_flt_time_max.get() > FLT_EPSILON && context.status().takeoff_time != 0 && + (hrt_absolute_time() - context.status().takeoff_time) > (1_s * _param_com_flt_time_max.get())) { + reporter.failsafeFlags().flight_time_limit_exceeded = true; + /* EVENT + * @description + * + * This check can be configured via COM_FLT_TIME_MAX parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_flight_time_limit"), + events::Log::Error, "Maximum flight time reached"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Maximum flight time reached\t"); + } + + } else { + reporter.failsafeFlags().flight_time_limit_exceeded = false; + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.hpp new file mode 100644 index 0000000000..a4350886fb --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +class FlightTimeChecks : public HealthAndArmingCheckBase +{ +public: + FlightTimeChecks() = default; + ~FlightTimeChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_flt_time_max + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp new file mode 100644 index 0000000000..b1a36541a2 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "geofenceCheck.hpp" + +void GeofenceChecks::checkAndReport(const Context &context, Report &reporter) +{ + geofence_result_s geofence_result; + + if (!_geofence_result_sub.copy(&geofence_result)) { + geofence_result = {}; + } + + reporter.failsafeFlags().geofence_violated = geofence_result.geofence_violated; + + if (geofence_result.geofence_action != 0 && reporter.failsafeFlags().geofence_violated) { + /* EVENT + * @description + * + * This check can be configured via GF_ACTION parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_gf_violation"), + events::Log::Error, "Vehicle outside geofence"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence"); + } + } + + if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL + && !reporter.failsafeFlags().home_position_valid) { + /* EVENT + * @description + * + * This check can be configured via GF_ACTION parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_gf_no_home"), + events::Log::Error, "Geofence RTL requires valid home"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence RTL requires valid home"); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.hpp new file mode 100644 index 0000000000..d2163ffa88 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class GeofenceChecks : public HealthAndArmingCheckBase +{ +public: + GeofenceChecks() = default; + ~GeofenceChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp new file mode 100644 index 0000000000..ac86fa0b44 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "homePositionCheck.hpp" + +void HomePositionChecks::checkAndReport(const Context &context, Report &reporter) +{ + home_position_s home_position; + + if (_home_position_sub.copy(&home_position)) { + reporter.failsafeFlags().home_position_valid = home_position.valid_alt && home_position.valid_hpos; + + } else { + reporter.failsafeFlags().home_position_valid = false; + } + + // No need to report, as it's a mode requirement +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.hpp new file mode 100644 index 0000000000..813c0161c5 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class HomePositionChecks : public HealthAndArmingCheckBase +{ +public: + HomePositionChecks() = default; + ~HomePositionChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp new file mode 100644 index 0000000000..39abe81401 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "missionCheck.hpp" + +void MissionChecks::checkAndReport(const Context &context, Report &reporter) +{ + reporter.failsafeFlags().mission_failure = false; + mission_result_s mission_result; + + if (_mission_result_sub.copy(&mission_result) && mission_result.valid) { + reporter.failsafeFlags().mission_failure = mission_result.failure; + + if (reporter.failsafeFlags().mission_failure) { + // navigator sends out the exact reason + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_mission_failure"), + events::Log::Error, "Mission cannot be completed"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t"); + } + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.hpp new file mode 100644 index 0000000000..0f6d843a07 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class MissionChecks : public HealthAndArmingCheckBase +{ +public: + MissionChecks() = default; + ~MissionChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index 63619118f0..0f1fdfc1df 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -109,7 +109,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system, events::ID("check_modes_mission"), - events::Log::Info, "No mission available"); + events::Log::Info, "No valid mission available"); reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission); } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp new file mode 100644 index 0000000000..cdcc190132 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "rcAndDataLinkCheck.hpp" + +using namespace time_literals; + +void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter) +{ + // RC + bool rc_is_optional = true; + + if (_param_com_rc_in_mode.get() == 4) { // RC disabled + reporter.failsafeFlags().rc_signal_lost = false; + + } else { + + manual_control_setpoint_s manual_control_setpoint; + + if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + manual_control_setpoint = {}; + reporter.failsafeFlags().rc_signal_lost = true; + } + + // Check if RC is valid + if (!manual_control_setpoint.valid + || hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) { + + if (!reporter.failsafeFlags().rc_signal_lost && _last_valid_manual_control_setpoint > 0) { + + events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, + "Manual control lost"); + } + + reporter.failsafeFlags().rc_signal_lost = true; + + } else { + reporter.setIsPresent(health_component_t::remote_control); + + if (reporter.failsafeFlags().rc_signal_lost && _last_valid_manual_control_setpoint > 0) { + float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f; + events::send(events::ID("commander_rc_regained"), events::Log::Info, + "Manual control regained after {1:.1} s", elapsed); + } + + reporter.failsafeFlags().rc_signal_lost = false; + _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; + } + + + if (reporter.failsafeFlags().rc_signal_lost) { + + NavModes affected_modes = rc_is_optional ? NavModes::None : NavModes::All; + events::LogLevel log_level = rc_is_optional ? events::Log::Info : events::Log::Error; + /* EVENT + * @description + * + * This check can be configured via COM_RC_IN_MODE parameter. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::remote_control, events::ID("check_rc_dl_no_rc"), + log_level, "No manual control input"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: No manual control input\t"); + } + } + } + + // Data Link + reporter.failsafeFlags().data_link_lost = context.status().data_link_lost; + + if (reporter.failsafeFlags().data_link_lost) { + + // Prevent arming if we neither have RC nor a Data Link. TODO: disabled for now due to MAVROS tests + bool data_link_required = _param_nav_dll_act.get() > 0 + /*|| (rc_is_optional && reporter.failsafeFlags().rc_signal_lost) */; + NavModes affected_modes = data_link_required ? NavModes::All : NavModes::None; + events::LogLevel log_level = data_link_required ? events::Log::Error : events::Log::Info; + /* EVENT + * @description + * To arm, at least a data link or manual control (RC) must be present. + * + * + * This check can be configured via NAV_DLL_ACT parameter. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::communication_links, + events::ID("check_rc_dl_no_dllink"), + log_level, "No connection to the ground control station"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t"); + } + + } else { + reporter.setIsPresent(health_component_t::communication_links); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.hpp new file mode 100644 index 0000000000..56bfc0263c --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class RcAndDataLinkChecks : public HealthAndArmingCheckBase +{ +public: + RcAndDataLinkChecks() = default; + ~RcAndDataLinkChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + + hrt_abstime _last_valid_manual_control_setpoint{0}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_rc_in_mode, + (ParamFloat) _param_com_rc_loss_t, + (ParamInt) _param_nav_dll_act + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index ec7aaa2905..f827d0b229 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -191,21 +191,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - if (_param_gf_action.get() != 0 && context.status().geofence_violated) { - /* EVENT - * @description - * - * This check can be configured via GF_ACTION parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_gf_violation"), - events::Log::Error, "Vehicle outside geofence"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence"); - } - } - // Arm Requirements: authorization if (_param_com_arm_auth_req.get() != 0 && !context.isArmed()) { if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { @@ -215,8 +200,4 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) events::Log::Error, "Arm authorization denied"); } } - - if (reporter.failsafeFlags().rc_signal_found_once) { - reporter.setIsPresent(health_component_t::remote_control); - } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.cpp new file mode 100644 index 0000000000..c953c4281e --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "vtolCheck.hpp" + +using namespace time_literals; + +void VtolChecks::checkAndReport(const Context &context, Report &reporter) +{ + vtol_vehicle_status_s vtol_vehicle_status; + + if (_vtol_vehicle_status_sub.copy(&vtol_vehicle_status)) { + reporter.failsafeFlags().vtol_transition_failure = vtol_vehicle_status.vtol_transition_failsafe; + + if (reporter.failsafeFlags().vtol_transition_failure) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_vtol_transition_failure"), + events::Log::Error, "VTOL transition failure"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: VTOL transition failure\t"); + } + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.hpp new file mode 100644 index 0000000000..eec44f1524 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class VtolChecks : public HealthAndArmingCheckBase +{ +public: + VtolChecks() = default; + ~VtolChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp new file mode 100644 index 0000000000..fd77a75267 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "windCheck.hpp" + +void WindChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (_param_com_wind_warn.get() < FLT_EPSILON && _param_com_wind_max.get() < FLT_EPSILON) { + reporter.failsafeFlags().wind_limit_exceeded = false; + return; + } + + wind_s wind_estimate; + const hrt_abstime now = hrt_absolute_time(); + + if (_wind_sub.copy(&wind_estimate)) { + const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east); + + // publish a warning if it's the first since in air or 60s have passed since the last warning + const bool warning_timeout_passed = _last_wind_warning == 0 || now - _last_wind_warning > 60_s; + + reporter.failsafeFlags().wind_limit_exceeded = _param_com_wind_max.get() > FLT_EPSILON + && wind.longerThan(_param_com_wind_max.get()); + + if (reporter.failsafeFlags().wind_limit_exceeded) { + + /* EVENT + * @description + * + * This check can be configured via COM_WIND_MAX parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_wind_too_high"), + events::Log::Warning, "Wind speed is above limit ({1:.1m/s})", wind.norm()); + + } else if (_param_com_wind_warn.get() > FLT_EPSILON + && wind.longerThan(_param_com_wind_warn.get()) + && warning_timeout_passed + && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + + events::send(events::ID("check_high_wind_warning"), + {events::Log::Warning, events::LogInternal::Info}, + "High wind speed detected ({1:.1m/s}), landing advised", wind.norm()); + _last_wind_warning = now; + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp new file mode 100644 index 0000000000..e59c0c309f --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class WindChecks : public HealthAndArmingCheckBase +{ +public: + WindChecks() = default; + ~WindChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _wind_sub{ORB_ID(wind)}; + hrt_abstime _last_wind_warning{0}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamFloat) _param_com_wind_max, + (ParamFloat) _param_com_wind_warn + + ) +}; diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp new file mode 100644 index 0000000000..5f9928c856 --- /dev/null +++ b/src/modules/commander/ModeUtil/conversions.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +using navigation_mode_t = events::px4::enums::navigation_mode_t; + +namespace mode_util +{ + +static inline navigation_mode_t navigation_mode(uint8_t nav_state) +{ + switch (nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: return navigation_mode_t::manual; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return navigation_mode_t::altctl; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: return navigation_mode_t::posctl; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: return navigation_mode_t::acro; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: return navigation_mode_t::offboard; + + case vehicle_status_s::NAVIGATION_STATE_STAB: return navigation_mode_t::stab; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return navigation_mode_t::auto_land; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland; + + case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff; + } + + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "code requires update"); + + return navigation_mode_t::unknown; +} + +const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { + "MANUAL", + "ALTCTL", + "POSCTL", + "AUTO_MISSION", + "AUTO_LOITER", + "AUTO_RTL", + "6: unallocated", + "7: unallocated", + "AUTO_LANDENGFAIL", + "9: unallocated", + "ACRO", + "11: UNUSED", + "DESCEND", + "TERMINATION", + "OFFBOARD", + "STAB", + "16: UNUSED2", + "AUTO_TAKEOFF", + "AUTO_LAND", + "AUTO_FOLLOW_TARGET", + "AUTO_PRECLAND", + "ORBIT" +}; + +} // namespace mode_util diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp new file mode 100644 index 0000000000..851f1f772e --- /dev/null +++ b/src/modules/commander/UserModeIntention.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include "UserModeIntention.hpp" + +UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status, + const HealthAndArmingChecks &health_and_arming_checks) + : ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks) +{ +} + +bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force) +{ + _ever_had_mode_change = true; + _had_mode_change = true; + + if (_user_intented_nav_state == user_intended_nav_state) { + return true; + } + + // Always allow mode change while disarmed + bool always_allow = force || !isArmed(); + bool allow_change = true; + + if (!always_allow) { + allow_change = _health_and_arming_checks.canRun(user_intended_nav_state); + + // Check fallback + if (!allow_change && allow_fallback && _param_com_posctl_navl.get() == 0) { + if (user_intended_nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) { + allow_change = _health_and_arming_checks.canRun(vehicle_status_s::NAVIGATION_STATE_ALTCTL); + // We still use the original user intended mode. The failsafe state machine will then set the + // fallback and once can_run becomes true, the actual user intended mode will be selected. + } + } + } + + if (allow_change) { + _user_intented_nav_state = user_intended_nav_state; + + if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) { + _nav_state_after_disarming = user_intended_nav_state; + } + } + + return allow_change; +} + +void UserModeIntention::onDisarm() +{ + _user_intented_nav_state = _nav_state_after_disarming; +} diff --git a/src/modules/commander/UserModeIntention.hpp b/src/modules/commander/UserModeIntention.hpp new file mode 100644 index 0000000000..3c18a6ff73 --- /dev/null +++ b/src/modules/commander/UserModeIntention.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" +#include + +class UserModeIntention : ModuleParams +{ +public: + UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status, + const HealthAndArmingChecks &health_and_arming_checks); + ~UserModeIntention() = default; + + /** + * Change the user intended mode + * @param user_intended_nav_state new mode + * @param allow_fallback allow to fallback to a lower mode if current mode cannot run + * @param force always set if true + * @return true if successfully set (also if unchanged) + */ + bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false); + + uint8_t get() const { return _user_intented_nav_state; } + + /** + * Change the user intention to the last user intended mode where arming is possible + */ + void onDisarm(); + + /** + * Returns false if there has not been any mode change yet + */ + bool everHadModeChange() const { return _ever_had_mode_change; } + + bool getHadModeChangeAndClear() { bool ret = _had_mode_change; _had_mode_change = false; return ret; } + +private: + bool isArmed() const { return _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; } + + const vehicle_status_s &_vehicle_status; + const HealthAndArmingChecks &_health_and_arming_checks; + + uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode + uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming + + bool _ever_had_mode_change{false}; ///< true if there was ever a mode change call (also if the same mode as already set) + bool _had_mode_change{false}; ///< true if there was a mode change call since the last getHadModeChangeAndClear() + + DEFINE_PARAMETERS( + (ParamInt) _param_com_posctl_navl + ); +}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 13c9c21637..a0c652389a 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -283,13 +283,14 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0); PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); /** - * Delay between battery state change and failsafe reaction + * Delay between failsafe condition triggered and failsafe reaction * - * Battery state requires action -> wait COM_BAT_ACT_T seconds in Hold mode - * for the user to realize and take a custom action - * -> React with failsafe action COM_LOW_BAT_ACT + * Before entering failsafe (RTL, Land, Hold), wait COM_FAIL_ACT_T seconds in Hold mode + * for the user to realize. + * During that time the user cannot take over control. + * Afterwards the configured failsafe action is triggered and the user may take over. * - * A zero value disables the delay. + * A zero value disables the delay and the user cannot take over via stick movements (switching modes is still allowed). * * @group Commander * @unit s @@ -297,7 +298,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); * @max 25.0 * @decimal 3 */ -PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f); +PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f); /** * Imbalanced propeller failsafe mode @@ -319,7 +320,7 @@ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0); /** * Time-out to wait when offboard connection is lost before triggering offboard lost action. * - * See COM_OBL_ACT and COM_OBL_RC_ACT to configure action. + * See COM_OBL_RC_ACT to configure action. * * @group Commander * @unit s @@ -329,27 +330,10 @@ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0); */ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f); -/** - * Set offboard loss failsafe mode - * - * The offboard loss failsafe will only be entered after a timeout, - * set by COM_OF_LOSS_T in seconds. - * - * @value -1 Disabled - * @value 0 Land mode - * @value 1 Hold mode - * @value 2 Return mode - * @value 3 Terminate - * @value 4 Lockdown - * - * @group Commander - */ -PARAM_DEFINE_INT32(COM_OBL_ACT, 0); - /** * Set command after a quadchute * - * @value -1 No action: stay in current flight mode + * @value -1 Warning only * @value 0 Return mode * @value 1 Land mode * @value 2 Hold mode @@ -358,12 +342,11 @@ PARAM_DEFINE_INT32(COM_OBL_ACT, 0); PARAM_DEFINE_INT32(COM_QC_ACT, 0); /** - * Set offboard loss failsafe mode when RC is available + * Set offboard loss failsafe mode * * The offboard loss failsafe will only be entered after a timeout, * set by COM_OF_LOSS_T in seconds. * - * @value -1 Disabled * @value 0 Position mode * @value 1 Altitude mode * @value 2 Manual @@ -371,7 +354,7 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0); * @value 4 Land mode * @value 5 Hold mode * @value 6 Terminate - * @value 7 Lockdown + * @value 7 Disarm * @group Commander */ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); @@ -676,10 +659,10 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * * If Altitude/Manual is selected: assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. * - * If Land/Terminate is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. + * If Land/Descend is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to Descend. * * @value 0 Altitude/Manual - * @value 1 Land/Terminate + * @value 1 Land/Descend * * @group Commander */ @@ -813,7 +796,7 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0); * @value 2 Return mode * @value 3 Land mode * @value 5 Terminate - * @value 6 Lockdown + * @value 6 Disarm * @min 0 * @max 6 * @@ -832,7 +815,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0); * @value 2 Return mode * @value 3 Land mode * @value 5 Terminate - * @value 6 Lockdown + * @value 6 Disarm * @min 1 * @max 6 * @@ -862,7 +845,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); * * @min 0 * @max 3 - * @value 0 Disabled + * @value 0 Warning only * @value 1 Hold mode * @value 2 Land mode * @value 3 Return mode @@ -988,8 +971,8 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1); /** * Timeout for detecting a failure after takeoff * - * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into - * a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R. + * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to disarm the vehicle + * if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R. * The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW) and Manual (FW). * A zero or negative value means that the check is disabled. * diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 5d80dfa2fa..c80153a848 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -35,8 +35,12 @@ #include #include +#include +#include -FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) const +using namespace time_literals; + +FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) { ActionOptions options{}; @@ -77,7 +81,7 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) c return options; } -FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) const +FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) { ActionOptions options{}; @@ -119,9 +123,211 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) const return options; } +FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value) +{ + ActionOptions options{}; + + switch (param_value) { + case -1: + default: + options.action = Action::None; + break; + + case 0: + options.action = Action::Warn; + break; + + case 1: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case 2: + options.action = Action::Land; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + } + + return options; +} + +FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_value) +{ + ActionOptions options{}; + + switch (param_value) { + case 0: + default: + options.action = Action::Warn; + break; + + case 1: + options.action = Action::Hold; + break; + + case 2: + options.action = Action::Land; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case 3: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case 4: + options.action = Action::Terminate; + options.clear_condition = ClearCondition::Never; + break; + } + + return options; +} + +FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value, uint8_t battery_warning) +{ + ActionOptions options{}; + + switch (battery_warning) { + default: + case battery_status_s::BATTERY_WARNING_NONE: + options.action = Action::None; + break; + + case battery_status_s::BATTERY_WARNING_LOW: + options.action = Action::Warn; + options.cause = Cause::BatteryLow; + break; + + case battery_status_s::BATTERY_WARNING_CRITICAL: + options.action = Action::Warn; + options.cause = Cause::BatteryCritical; + + switch ((LowBatteryAction)param_value) { + case LowBatteryAction::Return: + case LowBatteryAction::ReturnOrLand: + options.action = Action::RTL; + break; + + case LowBatteryAction::Land: + options.action = Action::Land; + break; + + case LowBatteryAction::Warning: + options.action = Action::Warn; + break; + } + + break; + + case battery_status_s::BATTERY_WARNING_EMERGENCY: + options.action = Action::Warn; + options.cause = Cause::BatteryEmergency; + + switch ((LowBatteryAction)param_value) { + case LowBatteryAction::Return: + options.action = Action::RTL; + break; + + case LowBatteryAction::ReturnOrLand: + case LowBatteryAction::Land: + options.action = Action::Land; + break; + + case LowBatteryAction::Warning: + options.action = Action::Warn; + break; + } + + break; + } + + return options; +} + +FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value) +{ + ActionOptions options{}; + + switch (param_value) { + case -1: + default: + options.action = Action::Warn; + break; + + case 0: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case 1: + options.action = Action::Land; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case 2: + options.action = Action::Hold; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + } + + return options; +} + +FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode) +{ + Action action{Action::None}; + + switch (param_value) { + case 0: + default: + action = Action::FallbackPosCtrl; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + break; + + case 1: + action = Action::FallbackAltCtrl; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + break; + + case 2: + action = Action::FallbackStab; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB; + break; + + case 3: + action = Action::RTL; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + break; + + case 4: + action = Action::Land; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + break; + + case 5: + action = Action::Hold; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + break; + + case 6: + action = Action::Terminate; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + break; + + case 7: + action = Action::Disarm; + break; + } + + return action; +} + void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const vehicle_status_flags_s &status_flags) { + updateArmingState(time_us, state.armed, status_flags); + const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || state.vtol_in_transition_mode; @@ -131,6 +337,11 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, && in_forward_flight && !state.mission_finished; // RC loss + if (!status_flags.rc_signal_lost) { + // If RC was lost and arming was allowed, consider it optional until we regain RC + _rc_lost_at_arming = false; + } + const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION && (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Mission); const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER @@ -140,11 +351,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) && (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Hold); - const bool rc_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || - rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff || - ignore_link_failsafe; + const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || + rc_loss_ignored_takeoff || ignore_link_failsafe || _rc_lost_at_arming; if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) { CHECK_FAILSAFE(status_flags, rc_signal_lost, @@ -160,12 +368,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::DatalinkLoss)); } - // VTOL transition failure + // VTOL transition failure (quadchute) if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) { - CHECK_FAILSAFE(status_flags, vtol_transition_failure, Action::RTL); + CHECK_FAILSAFE(status_flags, vtol_transition_failure, fromQuadchuteActParam(_param_com_qc_act.get())); } // Mission @@ -182,10 +390,53 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } - + CHECK_FAILSAFE(status_flags, wind_limit_exceeded, + ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm)); + CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, + ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never)); CHECK_FAILSAFE(status_flags, geofence_violated, fromGfActParam(_param_gf_action.get())); + // Battery + CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow)); + CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn); + + if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_LOW) { + _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, + true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_LOW)); + + } else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + _last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical, + _last_state_battery_warning_critical, + true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_CRITICAL)); + + } else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + _last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency, + _last_state_battery_warning_emergency, + true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_EMERGENCY)); + } + + + // Failure detector + if (_armed_time != 0 && time_us - _armed_time < _param_com_spoolup_time.get() * 1_s) { + CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, Action::Disarm); + } + + if (_armed_time != 0 && time_us - _armed_time < (_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s) { + // This handles the case where something fails during the early takeoff phase + CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Disarm); + + } else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) { + CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Terminate); + + } else { + CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn); + } + + CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get())); + CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get())); + + // Mode fallback (last) Action mode_fallback_action = checkModeFallback(status_flags, state.user_intended_mode); @@ -194,6 +445,20 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always)); } +void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags) +{ + if (!_was_armed && armed) { + _armed_time = time_us; + _rc_lost_at_arming = status_flags.rc_signal_lost; + + } else if (!armed) { + _rc_lost_at_arming = status_flags.rc_signal_lost; // ensure action isn't added while disarmed + _armed_time = 0; + } + + _was_armed = armed; +} + FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const { @@ -201,8 +466,7 @@ FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &s // offboard signal if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) { - action = Action::FallbackPosCtrl; // TODO: use param - user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode); } // posctrl diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 240755f687..f6cc593f9d 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -51,28 +51,62 @@ protected: uint8_t user_intended_mode) const override; private: + void updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags); + enum class RCLossExceptionBits : int32_t { Mission = (1 << 0), Hold = (1 << 1), Offboard = (1 << 2) }; - ActionOptions fromNavDllOrRclActParam(int param_value) const; + // COM_LOW_BAT_ACT parameter values + enum class LowBatteryAction : int32_t { + Warning = 0, // Warning + Return = 1, // Return mode (deprecated) + Land = 2, // Land mode + ReturnOrLand = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels + }; - ActionOptions fromGfActParam(int param_value) const; + static ActionOptions fromNavDllOrRclActParam(int param_value); + + static ActionOptions fromGfActParam(int param_value); + static ActionOptions fromImbalancedPropActParam(int param_value); + static ActionOptions fromActuatorFailureActParam(int param_value); + static ActionOptions fromBatteryWarningActParam(int param_value, uint8_t battery_warning); + static ActionOptions fromQuadchuteActParam(int param_value); + static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode); const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; const int _caller_id_mission_control_lost{genCallerId()}; bool _last_state_mission_control_lost{false}; + const int _caller_id_battery_warning_low{genCallerId()}; ///< battery warning caller ID's: use different ID's as they have different actions + bool _last_state_battery_warning_low{false}; + const int _caller_id_battery_warning_critical{genCallerId()}; + bool _last_state_battery_warning_critical{false}; + const int _caller_id_battery_warning_emergency{genCallerId()}; + bool _last_state_battery_warning_emergency{false}; + + hrt_abstime _armed_time{0}; + bool _was_armed{false}; + bool _rc_lost_at_arming{false}; ///< true if RC was lost at arming time + DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase, (ParamInt) _param_nav_dll_act, (ParamInt) _param_nav_rcl_act, (ParamInt) _param_com_rcl_except, (ParamInt) _param_com_rc_in_mode, (ParamInt) _param_com_posctl_navl, - (ParamInt) _param_gf_action + (ParamInt) _param_gf_action, + (ParamFloat) _param_com_spoolup_time, + (ParamInt) _param_com_imb_prop_act, + (ParamFloat) _param_com_lkdown_tko, + (ParamInt) _param_cbrk_flightterm, + (ParamInt) _param_com_actuator_failure_act, + (ParamInt) _param_com_low_bat_act, + (ParamInt) _param_com_obl_rc_act, + (ParamInt) _param_com_qc_act ); }; diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 06d45c9398..5a1b6e0e7b 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -458,6 +458,9 @@ void FailsafeBase::getSelectedAction(const State &state, const vehicle_status_fl if (!_user_takeover_active) { PX4_DEBUG("Activating user takeover"); +#ifndef EMSCRIPTEN_BUILD + events::send(events::ID("failsafe_rc_override"), events::Log::Info, "Pilot took over using sticks"); +#endif // EMSCRIPTEN_BUILD } // We must check for mode fallback again here diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp deleted file mode 100644 index 345b732310..0000000000 --- a/src/modules/commander/state_machine_helper.cpp +++ /dev/null @@ -1,1120 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.cpp - * State machine helper functions implementations - * - * @author Thomas Gubler - * @author Julian Oes - * @author Sander Smeets - */ -#include -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" -#include "commander_helper.h" - -using namespace time_literals; - -static constexpr const char reason_no_rc[] = "No manual control stick input"; -static constexpr const char reason_no_offboard[] = "no offboard"; -static constexpr const char reason_no_rc_and_no_offboard[] = "no RC and no offboard"; -static constexpr const char reason_no_local_position[] = "no local position"; -static constexpr const char reason_no_global_position[] = "no global position"; -static constexpr const char reason_no_datalink[] = "no datalink"; -static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink"; -static constexpr const char reason_no_gps[] = "no GPS"; - -// You can index into the array with an navigation_state_t in order to get its textual representation -const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { - "MANUAL", - "ALTCTL", - "POSCTL", - "AUTO_MISSION", - "AUTO_LOITER", - "AUTO_RTL", - "6: unallocated", - "7: unallocated", - "AUTO_LANDENGFAIL", - "9: unallocated", - "ACRO", - "11: UNUSED", - "DESCEND", - "TERMINATION", - "OFFBOARD", - "STAB", - "16: UNUSED2", - "AUTO_TAKEOFF", - "AUTO_LAND", - "AUTO_FOLLOW_TARGET", - "AUTO_PRECLAND", - "ORBIT" -}; - -void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act, - const float ll_delay); - -void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, const link_loss_actions_t link_loss_act); - -void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, - const offboard_loss_actions_t offboard_loss_act); - -void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, - const quadchute_actions_t quadchute_act); - -void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, - const offboard_loss_rc_actions_t offboard_loss_rc_act); - -void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsafe, - const offboard_loss_actions_t offboard_loss_act, - const offboard_loss_rc_actions_t offboard_loss_rc_act); - -transition_result_t -main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, - const vehicle_status_flags_s &status_flags, commander_state_s &internal_state) -{ - // IMPORTANT: The assumption of callers of this function is that the execution of - // this check is essentially "free". Therefore any runtime checking in here has to be - // kept super lightweight. No complex logic or calls on external function should be - // implemented here. - - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if the same state is requested because conditions may have changed */ - switch (new_main_state) { - case commander_state_s::MAIN_STATE_MANUAL: - case commander_state_s::MAIN_STATE_STAB: - case commander_state_s::MAIN_STATE_ACRO: - ret = TRANSITION_CHANGED; - break; - - case commander_state_s::MAIN_STATE_ALTCTL: - - /* need at minimum altitude estimate */ - if (status_flags.local_altitude_valid || - status_flags.global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_POSCTL: - - /* need at minimum local position estimate */ - if (status_flags.local_position_valid || - status_flags.global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_LOITER: - - /* need global position estimate */ - if (status_flags.global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: - case commander_state_s::MAIN_STATE_ORBIT: - - /* Follow and orbit only implemented for multicopter */ - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_MISSION: - - /* need global position, home position, and a valid mission */ - if (status_flags.global_position_valid && - status_flags.auto_mission_available) { - - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_RTL: - - /* need global position and home position */ - if (status_flags.global_position_valid && status_flags.home_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: - case commander_state_s::MAIN_STATE_AUTO_LAND: - - /* need local position */ - if (status_flags.local_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_PRECLAND: - - /* need local and global position, and precision land only implemented for multicopters */ - if (status_flags.local_position_valid - && status_flags.global_position_valid - && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_OFFBOARD: - - /* need offboard signal */ - if (!status_flags.offboard_control_signal_lost) { - - ret = TRANSITION_CHANGED; - } - - break; - - case commander_state_s::MAIN_STATE_MAX: - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - if (internal_state.main_state != new_main_state) { - internal_state.main_state = new_main_state; - internal_state.main_state_changes++; - internal_state.timestamp = hrt_absolute_time(); - - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - - return ret; -} - -using event_failsafe_reason_t = events::px4::enums::failsafe_reason_t; -/** - * Enable failsafe and report to user - */ -static void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, - event_failsafe_reason_t event_failsafe_reason) -{ - if (!old_failsafe && status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - // make sure intermittent failsafes don't lead to infinite delay by not constantly reseting the timestamp - if (status.failsafe_timestamp == 0 || - hrt_elapsed_time(&status.failsafe_timestamp) > 30_s) { - status.failsafe_timestamp = hrt_absolute_time(); - } - - const char *reason = ""; - - switch (event_failsafe_reason) { - case event_failsafe_reason_t::no_rc: reason = reason_no_rc; break; - - case event_failsafe_reason_t::no_offboard: reason = reason_no_offboard; break; - - case event_failsafe_reason_t::no_rc_and_no_offboard: reason = reason_no_rc_and_no_offboard; break; - - case event_failsafe_reason_t::no_local_position: reason = reason_no_local_position; break; - - case event_failsafe_reason_t::no_global_position: reason = reason_no_global_position; break; - - case event_failsafe_reason_t::no_datalink: reason = reason_no_datalink; break; - - case event_failsafe_reason_t::no_rc_and_no_datalink: reason = reason_no_rc_and_no_datalink; break; - - case event_failsafe_reason_t::no_gps: reason = reason_no_gps; break; - } - - mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s\t", reason); - events::send( - events::ID("commander_enable_failsafe"), {events::Log::Critical, events::LogInternal::Info}, - "Failsafe enabled: {1}", event_failsafe_reason); - } - - status.failsafe = true; -} - -/** - * Check failsafe and main status and set navigation status for navigator accordingly - */ -bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state, - orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, - const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, - const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, - const quadchute_actions_t quadchute_act, - const offboard_loss_rc_actions_t offb_loss_rc_act, - const position_nav_loss_actions_t posctl_nav_loss_act, - const float param_com_rcl_act_t, const int param_com_rcl_except) -{ - const navigation_state_t nav_state_old = status.nav_state; - - const bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool data_link_loss_act_configured = (data_link_loss_act > link_loss_actions_t::DISABLED); - - bool old_failsafe = status.failsafe; - status.failsafe = false; - - // Safe to do reset flags here, as if loss state persists flags will be restored in the code below - reset_link_loss_globals(armed, old_failsafe, rc_loss_act); - reset_link_loss_globals(armed, old_failsafe, data_link_loss_act); - reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act); - - // Failsafe decision logic for every normal non-failsafe mode - switch (internal_state.main_state) { - case commander_state_s::MAIN_STATE_ACRO: - case commander_state_s::MAIN_STATE_MANUAL: - case commander_state_s::MAIN_STATE_STAB: - case commander_state_s::MAIN_STATE_ALTCTL: - - // Require RC for all manual modes - if (status.rc_signal_lost && is_armed) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); - - } else { - switch (internal_state.main_state) { - case commander_state_s::MAIN_STATE_ACRO: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; - break; - - case commander_state_s::MAIN_STATE_MANUAL: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - - case commander_state_s::MAIN_STATE_STAB: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; - break; - - case commander_state_s::MAIN_STATE_ALTCTL: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - break; - - default: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - } - } - - break; - - case commander_state_s::MAIN_STATE_POSCTL: { - - const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed; - - if (status.rc_signal_lost && is_armed) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); - - /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ - /* A local position estimate is enough for POSCTL for multirotors, - * this enables POSCTL using e.g. flow. - * For fixedwing, a global position is needed. */ - - } else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, - rc_fallback_allowed, status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { - // nothing to do - everything done in check_invalid_pos_nav_state - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - } - } - break; - - case commander_state_s::MAIN_STATE_AUTO_MISSION: - - /* go into failsafe - * - if we have an engine failure - * - if we have vtol transition failure - * - on data and RC link loss */ - - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { - // nothing to do - everything done in check_invalid_pos_nav_state - } else if (status_flags.vtol_transition_failure) { - - set_quadchute_nav_state(status, armed, status_flags, quadchute_act); - - } else if (status.mission_failure) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - - } else if (status.data_link_lost && data_link_loss_act_configured - && is_armed && !landed) { - // Data link lost, data link loss reaction configured -> do configured reaction - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) - && status_flags.rc_signal_found_once && is_armed && !landed) { - // RC link lost, rc loss not disabled in mission, RC was used before -> RC loss reaction after delay - // Safety pilot expects to be able to take over by RC in case anything unexpected happens - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) - && status.data_link_lost && !data_link_loss_act_configured - && is_armed && !landed) { - // All links lost, no data link loss reaction configured -> immediately do RC loss reaction - // Lost all communication, by default it's considered unsafe to continue the mission - // This is only reached when flying mission completely without RC (it was not present since boot) - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); - - } else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) - && status.data_link_lost && !data_link_loss_act_configured - && is_armed && !landed - && mission_finished) { - // All links lost, all link loss reactions disabled -> return after mission finished - // Pilot disabled all reactions, finish mission but then return to avoid lost vehicle - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0); - - } else if (!stay_in_failsafe) { - // normal mission operation if there's no need to stay in failsafe - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_LOITER: - - /* go into failsafe on a engine failure */ - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { - // nothing to do - everything done in check_invalid_pos_nav_state - - } else if (status_flags.vtol_transition_failure) { - - set_quadchute_nav_state(status, armed, status_flags, quadchute_act); - - } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { - // Data link lost, data link loss reaction configured -> do configured reaction - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) - && status_flags.rc_signal_found_once && is_armed && !landed) { - // RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay - // Safety pilot expects to be able to take over by RC in case anything unexpected happens - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) - && status.data_link_lost && !data_link_loss_act_configured - && is_armed && !landed) { - // All links lost, no data link loss reaction configured -> immediately do RC loss reaction - // Lost all communication, by default it's considered unsafe to continue the mission - // This is only reached when flying mission completely without RC (it was not present since boot) - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_RTL: - - /* require global position and home, also go into failsafe on an engine failure */ - - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { - // nothing to do - everything done in check_invalid_pos_nav_state - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: - - // require global position and home - - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { - // nothing to do - everything done in check_invalid_pos_nav_state - - } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { - // failsafe: datalink is lost - // Trigger RTL - set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); - - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); - - } else if (status.rc_signal_lost && status_flags.rc_signal_found_once && !data_link_loss_act_configured && is_armed) { - // Trigger failsafe on RC loss only if RC was present once before - // Otherwise fly without RC, as follow-target only depends on the datalink - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); - - } else { - // no failsafe, RC is not mandatory for follow_target - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET; - } - - break; - - case commander_state_s::MAIN_STATE_ORBIT: - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { - // failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state - - // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit - // is not possible and therefore the internal_state needs to be adjusted. - internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; - - } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { - // failsafe: just datalink is lost and we're in air - set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); - - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); - - // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit - // is not possible and therefore the internal_state needs to be adjusted. - internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; - - } else if (status.rc_signal_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) { - // Orbit does not depend on RC but while armed & all links lost & when datalink loss is not set up, we failsafe - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); - - // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit - // is not possible and therefore the internal_state needs to be adjusted. - internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; - - } else { - // no failsafe, RC is not mandatory for orbit - status.nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: - case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: - - // require local position - - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { - // nothing to do - everything done in check_invalid_pos_nav_state - - } else if (status_flags.vtol_transition_failure) { - - set_quadchute_nav_state(status, armed, status_flags, quadchute_act); - - } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { - // Data link lost, data link loss reaction configured -> do configured reaction - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) - && status_flags.rc_signal_found_once && is_armed && !landed) { - // RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay - // Safety pilot expects to be able to take over by RC in case anything unexpected happens - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) - && status.data_link_lost && !data_link_loss_act_configured - && is_armed && !landed) { - // All links lost, no data link loss reaction configured -> immediately do RC loss reaction - // Lost all communication, by default it's considered unsafe to continue the mission - // This is only reached when flying mission completely without RC (it was not present since boot) - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); - - } else { - status.nav_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ? - vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF : vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_LAND: - - // require local position - - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { - - // nothing to do - everything done in check_invalid_pos_nav_state - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } - - break; - - case commander_state_s::MAIN_STATE_AUTO_PRECLAND: - - // must be rotary wing plus same requirements as normal landing - - if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { - // nothing to do - everything done in check_invalid_pos_nav_state - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; - } - - break; - - case commander_state_s::MAIN_STATE_OFFBOARD: - - if (status_flags.offboard_control_signal_lost) { - if (status.rc_signal_lost) { - // Offboard and RC are lost - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_offboard); - set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); - - } else { - // Offboard is lost, RC is ok - if (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_offboard); - set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); - - } else { - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_offboard); - set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act); - - } - - } - - } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) { - // Only RC is lost - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; - } - - break; - - default: - break; - } - - return status.nav_state != nav_state_old; -} - -bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, - const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos) -{ - bool fallback_required = false; - - if (using_global_pos && !status_flags.global_position_valid) { - fallback_required = true; - - } else if (!using_global_pos - && (!status_flags.local_position_valid || !status_flags.local_velocity_valid)) { - - fallback_required = true; - } - - if (fallback_required) { - if (use_rc) { - // fallback to a mode that gives the operator stick control - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && status_flags.local_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - - } else if (status_flags.local_altitude_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; - } - - } else { - // go into a descent that does not require stick control - if (status_flags.local_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - - } else if (status_flags.local_altitude_valid) { - - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } - - if (using_global_pos) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_global_position); - - } else { - enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_local_position); - } - - } - - return fallback_required; - -} - -void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act, - const float ll_delay) -{ - if (hrt_elapsed_time(&status.failsafe_timestamp) < (ll_delay * 1_s) - && link_loss_act != link_loss_actions_t::DISABLED) { - // delay failsafe reaction by trying to hold position if configured - link_loss_act = link_loss_actions_t::AUTO_LOITER; - } - - // do the best you can according to the action set - switch (link_loss_act) { - case link_loss_actions_t::DISABLED: - // If datalink loss failsafe is disabled then no action must be taken. - break; - - case link_loss_actions_t::AUTO_RTL: - if (status_flags.global_position_valid && status_flags.home_position_valid) { - main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state); - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - return; - } - - // FALLTHROUGH - case link_loss_actions_t::AUTO_LOITER: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - return; - } - - // FALLTHROUGH - case link_loss_actions_t::AUTO_LAND: - if (status_flags.global_position_valid) { - main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state); - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - return; - - } else { - if (status_flags.local_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - return; - - } else if (status_flags.local_altitude_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - return; - } - - } - - // FALLTHROUGH - case link_loss_actions_t::TERMINATE: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - armed.force_failsafe = true; - break; - - case link_loss_actions_t::LOCKDOWN: - armed.lockdown = true; - break; - } -} - -void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, const link_loss_actions_t link_loss_act) -{ - if (old_failsafe) { - if (link_loss_act == link_loss_actions_t::TERMINATE) { - armed.force_failsafe = false; - - } else if (link_loss_act == link_loss_actions_t::LOCKDOWN) { - armed.lockdown = false; - } - } -} - -void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, - const quadchute_actions_t quadchute_act) -{ - switch (quadchute_act) { - case quadchute_actions_t::NO_ACTION: - // If quadchute action is disabled then no action must be taken. - break; - - default: - case quadchute_actions_t::AUTO_RTL: - if (status_flags.global_position_valid && status_flags.home_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } - - break; - - // FALLTHROUGH - case quadchute_actions_t::AUTO_LAND: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } - - break; - - // FALLTHROUGH - case quadchute_actions_t::AUTO_LOITER: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } - - break; - } -} - -void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, - const offboard_loss_actions_t offboard_loss_act) -{ - switch (offboard_loss_act) { - case offboard_loss_actions_t::DISABLED: - // If offboard loss failsafe is disabled then no action must be taken. - return; - - case offboard_loss_actions_t::TERMINATE: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - armed.force_failsafe = true; - return; - - case offboard_loss_actions_t::LOCKDOWN: - armed.lockdown = true; - return; - - case offboard_loss_actions_t::AUTO_RTL: - if (status_flags.global_position_valid && status_flags.home_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - return; - } - - // FALLTHROUGH - case offboard_loss_actions_t::AUTO_LOITER: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - return; - } - - // FALLTHROUGH - case offboard_loss_actions_t::AUTO_LAND: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - return; - } - } - - // If none of the above worked, try to mitigate - if (status_flags.local_altitude_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } -} - -void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed, - const vehicle_status_flags_s &status_flags, - const offboard_loss_rc_actions_t offboard_loss_rc_act) -{ - switch (offboard_loss_rc_act) { - case offboard_loss_rc_actions_t::DISABLED: - // If offboard loss failsafe is disabled then no action must be taken. - return; - - case offboard_loss_rc_actions_t::TERMINATE: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - armed.force_failsafe = true; - return; - - case offboard_loss_rc_actions_t::LOCKDOWN: - armed.lockdown = true; - return; - - case offboard_loss_rc_actions_t::MANUAL_POSITION: - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && status_flags.local_position_valid) { - - status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - return; - - } else if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - return; - } - - // FALLTHROUGH - case offboard_loss_rc_actions_t::MANUAL_ALTITUDE: - if (status_flags.local_altitude_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - return; - } - - // FALLTHROUGH - case offboard_loss_rc_actions_t::MANUAL_ATTITUDE: - status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - return; - - case offboard_loss_rc_actions_t::AUTO_RTL: - if (status_flags.global_position_valid && status_flags.home_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - return; - } - - // FALLTHROUGH - case offboard_loss_rc_actions_t::AUTO_LOITER: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - return; - } - - // FALLTHROUGH - case offboard_loss_rc_actions_t::AUTO_LAND: - if (status_flags.global_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - return; - } - } - - // If none of the above worked, try to mitigate - if (status_flags.local_altitude_valid) { - //TODO: Add case for rover - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } -} - -void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsafe, - const offboard_loss_actions_t offboard_loss_act, - const offboard_loss_rc_actions_t offboard_loss_rc_act) -{ - if (old_failsafe) { - if (offboard_loss_act == offboard_loss_actions_t::TERMINATE - || offboard_loss_rc_act == offboard_loss_rc_actions_t::TERMINATE) { - armed.force_failsafe = false; - - } - - if (offboard_loss_act == offboard_loss_actions_t::LOCKDOWN - || offboard_loss_rc_act == offboard_loss_rc_actions_t::LOCKDOWN) { - armed.lockdown = false; - } - } -} - -void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning, - const uint8_t failsafe_action, const float param_com_bat_act_t, - const char *failsafe_action_string, const events::px4::enums::navigation_mode_t failsafe_action_navigation_mode) -{ - static constexpr char battery_level[] = "battery level"; - - // User warning - switch (battery_warning) { - case battery_status_s::BATTERY_WARNING_LOW: - mavlink_log_critical(mavlink_log_pub, "Low %s, return advised\t", battery_level); - events::send(events::ID("commander_bat_low"), {events::Log::Warning, events::LogInternal::Info}, - "Low battery level, return advised"); - - break; - - case battery_status_s::BATTERY_WARNING_CRITICAL: - if (failsafe_action == commander_state_s::MAIN_STATE_MAX) { - mavlink_log_critical(mavlink_log_pub, "Critical %s, return now\t", battery_level); - events::send(events::ID("commander_bat_crit"), {events::Log::Critical, events::LogInternal::Info}, - "Critical battery level, return now"); - - } else { - mavlink_log_critical(mavlink_log_pub, "Critical %s, executing %s in %d seconds\t", battery_level, - failsafe_action_string, static_cast(param_com_bat_act_t)); - events::send(events::ID("commander_bat_crit_act"), {events::Log::Critical, events::LogInternal::Info}, - "Critical battery level, executing {1} in {2} seconds", - failsafe_action_navigation_mode, static_cast(param_com_bat_act_t)); - } - - break; - - case battery_status_s::BATTERY_WARNING_EMERGENCY: - if (failsafe_action == commander_state_s::MAIN_STATE_MAX) { - mavlink_log_emergency(mavlink_log_pub, "Dangerous %s, land now\t", battery_level); - events::send(events::ID("commander_bat_emerg"), {events::Log::Emergency, events::LogInternal::Info}, - "Dangerous battery level, land now"); - - } else { - mavlink_log_emergency(mavlink_log_pub, "Dangerous %s, executing %s in %d seconds\t", battery_level, - failsafe_action_string, static_cast(param_com_bat_act_t)); - events::send(events::ID("commander_bat_emerg_act"), {events::Log::Emergency, events::LogInternal::Info}, - "Dangerous battery level, executing {1} in {2} seconds", - failsafe_action_navigation_mode, static_cast(param_com_bat_act_t)); - } - - break; - - case battery_status_s::BATTERY_WARNING_FAILED: - mavlink_log_emergency(mavlink_log_pub, "Battery failure detected, land and check battery\t"); - events::send(events::ID("commander_bat_failure"), events::Log::Emergency, - "Battery failure detected, land and check battery"); - break; - - case battery_status_s::BATTERY_WARNING_NONE: break; // no warning - } -} - -uint8_t get_battery_failsafe_action(const commander_state_s &internal_state, const uint8_t battery_warning, - const low_battery_action_t param_com_low_bat_act) -{ - uint8_t ret = commander_state_s::MAIN_STATE_MAX; - - const bool already_landing = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND - || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND; - const bool already_landing_or_rtl = already_landing - || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; - - // The main state is directly changed for the action because we need the fallbacks by the navigation state. - switch (battery_warning) { - case battery_status_s::BATTERY_WARNING_NONE: - case battery_status_s::BATTERY_WARNING_LOW: - break; - - case battery_status_s::BATTERY_WARNING_CRITICAL: - switch (param_com_low_bat_act) { - case LOW_BAT_ACTION::RETURN: - case LOW_BAT_ACTION::RETURN_OR_LAND: - if (!already_landing_or_rtl) { - ret = commander_state_s::MAIN_STATE_AUTO_RTL; - } - - break; - - case LOW_BAT_ACTION::LAND: - if (!already_landing) { - ret = commander_state_s::MAIN_STATE_AUTO_LAND; - } - - break; - - case LOW_BAT_ACTION::WARNING: break; // no action - } - - break; - - case battery_status_s::BATTERY_WARNING_EMERGENCY: - switch (param_com_low_bat_act) { - case LOW_BAT_ACTION::RETURN: - if (!already_landing_or_rtl) { - ret = commander_state_s::MAIN_STATE_AUTO_RTL; - } - - break; - - case LOW_BAT_ACTION::RETURN_OR_LAND: - case LOW_BAT_ACTION::LAND: - if (!already_landing) { - ret = commander_state_s::MAIN_STATE_AUTO_LAND; - } - - break; - - case LOW_BAT_ACTION::WARNING: break; // no action - } - - break; - } - - return ret; -} - -void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, - const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, - const imbalanced_propeller_action_t failsafe_action) -{ - static constexpr char failure_msg[] = "Imbalanced propeller detected"; - - switch (failsafe_action) { - case imbalanced_propeller_action_t::DISABLED: - break; - - case imbalanced_propeller_action_t::WARNING: - mavlink_log_warning(mavlink_log_pub, "%s, landing advised", failure_msg); - break; - - case imbalanced_propeller_action_t::RETURN: - - if (status_flags.global_position_valid && status_flags.home_position_valid) { - if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL || - internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { - - internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL; - internal_state->timestamp = hrt_absolute_time(); - mavlink_log_warning(mavlink_log_pub, "%s, executing RTL", failure_msg); - } - - } else { - if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { - internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND; - internal_state->timestamp = hrt_absolute_time(); - mavlink_log_warning(mavlink_log_pub, "%s, can't execute RTL, landing instead", failure_msg); - } - } - - break; - - case imbalanced_propeller_action_t::LAND: - if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { - internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND; - internal_state->timestamp = hrt_absolute_time(); - mavlink_log_warning(mavlink_log_pub, "%s, landing", failure_msg); - } - - break; - } -} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h deleted file mode 100644 index 4f93b7f685..0000000000 --- a/src/modules/commander/state_machine_helper.h +++ /dev/null @@ -1,158 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.h - * State machine helper functions definitions - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef STATE_MACHINE_HELPER_H_ -#define STATE_MACHINE_HELPER_H_ - -#include - -#include -#include -#include -#include -#include -#include -#include - -typedef enum { - TRANSITION_DENIED = -1, - TRANSITION_NOT_CHANGED = 0, - TRANSITION_CHANGED -} transition_result_t; - -enum class link_loss_actions_t { - DISABLED = 0, - AUTO_LOITER = 1, // Hold mode - AUTO_RTL = 2, // Return mode - AUTO_LAND = 3, // Land mode - TERMINATE = 5, // Terminate flight (set actuator outputs to failsafe values, and stop controllers) - LOCKDOWN = 6, // Lock actuators (set actuator outputs to disarmed values) -}; - -enum class quadchute_actions_t { - NO_ACTION = -1, - AUTO_RTL = 0, // Return mode - AUTO_LAND = 1, // Land mode - AUTO_LOITER = 2, // Hold mode -}; - -enum class offboard_loss_actions_t { - DISABLED = -1, - AUTO_LAND = 0, // Land mode - AUTO_LOITER = 1, // Hold mode - AUTO_RTL = 2, // Return mode - TERMINATE = 3, // Terminate flight (set actuator outputs to failsafe values, and stop controllers) - LOCKDOWN = 4, // Lock actuators (set actuator outputs to disarmed values) -}; - -enum class offboard_loss_rc_actions_t { - DISABLED = -1, // Disabled - MANUAL_POSITION = 0, // Position mode - MANUAL_ALTITUDE = 1, // Altitude mode - MANUAL_ATTITUDE = 2, // Manual - AUTO_RTL = 3, // Return mode - AUTO_LAND = 4, // Land mode - AUTO_LOITER = 5, // Hold mode - TERMINATE = 6, // Terminate flight (set actuator outputs to failsafe values, and stop controllers) - LOCKDOWN = 7, // Lock actuators (set actuator outputs to disarmed values) -}; - -enum class position_nav_loss_actions_t { - ALTITUDE_MANUAL = 0, // Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. - LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. -}; - -extern const char *const nav_state_names[]; - -enum RCLossExceptionBits { - RCL_EXCEPT_MISSION = (1 << 0), - RCL_EXCEPT_HOLD = (1 << 1), - RCL_EXCEPT_OFFBOARD = (1 << 2) -}; - -transition_result_t -main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, - const vehicle_status_flags_s &status_flags, commander_state_s &internal_state); - -bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state, - orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, - const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, - const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, - const quadchute_actions_t quadchute_act, - const offboard_loss_rc_actions_t offb_loss_rc_act, - const position_nav_loss_actions_t posctl_nav_loss_act, - const float param_com_rcl_act_t, const int param_com_rcl_except); - -/* - * Checks the validty of position data against the requirements of the current navigation - * mode and switches mode if position data required is not available. - */ -bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, - const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos); - - -// COM_LOW_BAT_ACT parameter values -typedef enum LOW_BAT_ACTION { - WARNING = 0, // Warning - RETURN = 1, // Return mode - LAND = 2, // Land mode - RETURN_OR_LAND = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels -} low_battery_action_t; - -void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning, - const uint8_t failsafe_action, const float param_com_bat_act_t, - const char *failsafe_action_string, const events::px4::enums::navigation_mode_t failsafe_action_navigation_mode); -uint8_t get_battery_failsafe_action(const commander_state_s &internal_state, const uint8_t battery_warning, - const low_battery_action_t param_com_low_bat_act); - -// COM_IMB_PROP_ACT parameter values -enum class imbalanced_propeller_action_t { - DISABLED = -1, - WARNING = 0, - RETURN = 1, - LAND = 2 -}; - -void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, - const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, - const imbalanced_propeller_action_t failsafe_action); - -#endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 056f6cbe5e..b0601e2749 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -35,7 +35,6 @@ #include #include -#include #include ManualControl::ManualControl() : @@ -205,7 +204,7 @@ void ManualControl::Run() if (switches.return_switch != _previous_switches.return_switch) { if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, - commander_state_s::MAIN_STATE_AUTO_RTL); + vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); } else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) { evaluateModeSlot(switches.mode_slot); @@ -215,7 +214,7 @@ void ManualControl::Run() if (switches.loiter_switch != _previous_switches.loiter_switch) { if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, - commander_state_s::MAIN_STATE_AUTO_LOITER); + vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); } else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) { evaluateModeSlot(switches.mode_slot); @@ -225,7 +224,7 @@ void ManualControl::Run() if (switches.offboard_switch != _previous_switches.offboard_switch) { if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, - commander_state_s::MAIN_STATE_OFFBOARD); + vehicle_status_s::NAVIGATION_STATE_OFFBOARD); } else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) { evaluateModeSlot(switches.mode_slot); @@ -364,27 +363,33 @@ void ManualControl::evaluateModeSlot(uint8_t mode_slot) break; case manual_control_switches_s::MODE_SLOT_1: - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_1.get()); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + navStateFromParam(_param_fltmode_1.get())); break; case manual_control_switches_s::MODE_SLOT_2: - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_2.get()); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + navStateFromParam(_param_fltmode_2.get())); break; case manual_control_switches_s::MODE_SLOT_3: - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_3.get()); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + navStateFromParam(_param_fltmode_3.get())); break; case manual_control_switches_s::MODE_SLOT_4: - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_4.get()); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + navStateFromParam(_param_fltmode_4.get())); break; case manual_control_switches_s::MODE_SLOT_5: - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_5.get()); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + navStateFromParam(_param_fltmode_5.get())); break; case manual_control_switches_s::MODE_SLOT_6: - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_6.get()); + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, + navStateFromParam(_param_fltmode_6.get())); break; default: @@ -520,6 +525,28 @@ Module consuming manual_control_inputs publishing one manual_control_setpoint. return 0; } +int8_t ManualControl::navStateFromParam(int32_t param_value) +{ + switch(param_value) { + case 0: return vehicle_status_s::NAVIGATION_STATE_MANUAL; + case 1: return vehicle_status_s::NAVIGATION_STATE_ALTCTL; + case 2: return vehicle_status_s::NAVIGATION_STATE_POSCTL; + case 3: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + case 4: return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + case 5: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + case 6: return vehicle_status_s::NAVIGATION_STATE_ACRO; + case 7: return vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + case 8: return vehicle_status_s::NAVIGATION_STATE_STAB; + case 10: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + case 11: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + case 12: return vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET; + case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; + case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT; + case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF; + } + return -1; +} + extern "C" __EXPORT int manual_control_main(int argc, char *argv[]) { return ManualControl::main(argc, argv); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 1ddf25c46e..738fd92ea5 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -80,6 +80,8 @@ private: void Run() override; void processStickArming(const manual_control_setpoint_s &input); + static int8_t navStateFromParam(int32_t param_value); + void evaluateModeSlot(uint8_t mode_slot); void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0); void publishLandingGear(int8_t action); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index a92073d1ea..77f7b96902 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -437,10 +437,6 @@ private: msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; } - if (status.mission_failure) { - msg->failure_flags |= HL_FAILURE_FLAG_MISSION; - } - // flight mode union px4_custom_mode custom_mode {get_px4_custom_mode(status.nav_state)}; msg->custom_mode = custom_mode.custom_mode_hl; @@ -464,6 +460,10 @@ private: msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK; } + if (status_flags.mission_failure) { + msg->failure_flags |= HL_FAILURE_FLAG_MISSION; + } + return true; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0396350c26..6cbbfa0b7a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -808,7 +808,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) float test_point_bearing; float test_point_distance; float vertical_test_point_distance; - char geofence_violation_warning[50]; if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); @@ -843,12 +842,10 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) if (_geofence.getPredict()) { fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); - snprintf(geofence_violation_warning, sizeof(geofence_violation_warning), "Approaching on geofence"); } else { fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon); vertical_test_point_distance = 0; - snprintf(geofence_violation_warning, sizeof(geofence_violation_warning), "Geofence exceeded"); } gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0), @@ -875,9 +872,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) /* Issue a warning about the geofence violation once and only if we are armed */ if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - mavlink_log_critical(&_mavlink_log_pub, "%s", geofence_violation_warning); - events::send(events::ID("navigator_geofence_violation"), {events::Log::Warning, events::LogInternal::Info}, - geofence_violation_warning); // we have predicted a geofence violation and if the action is to loiter then // demand a reposition to a location which is inside the geofence