From ebc88afe464fd9aaec47cabb75330a4d3ab4e171 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Mon, 5 Sep 2022 18:17:24 -0600 Subject: [PATCH] Apply Google Style to Commander Private methods, rename geofence message geofence_violation to primary_geofence_breached. --- msg/geofence_result.msg | 21 +- msg/vehicle_status_flags.msg | 2 +- src/modules/commander/Commander.cpp | 62 +++--- src/modules/commander/Commander.hpp | 193 +++++++++--------- .../checks/geofenceCheck.cpp | 6 +- src/modules/commander/failsafe/failsafe.cpp | 2 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 2 +- src/modules/navigator/navigator_main.cpp | 6 +- 8 files changed, 145 insertions(+), 149 deletions(-) diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 746eadfd7b..86639d2e7c 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -1,12 +1,13 @@ -uint64 timestamp # time since system start (microseconds) -uint8 GF_ACTION_NONE = 0 # no action on geofence violation -uint8 GF_ACTION_WARN = 1 # critical mavlink message -uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER -uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL -uint8 GF_ACTION_TERMINATE = 4 # flight termination -uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND +uint64 timestamp # time since system start (microseconds) +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination +uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND -bool geofence_violated # true if the geofence is violated -uint8 geofence_action # action to take when geofence is violated -bool home_required # true if the geofence requires a valid home position +bool primary_geofence_breached # true if the primary geofence is breached +uint8 primary_geofence_action # action to take when the primary geofence is breached + +bool home_required # true if the geofence requires a valid home position diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 80f6897cea..ca2274aee0 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -41,7 +41,7 @@ uint8 battery_warning # Battery warning level bool battery_low_remaining_time # Low battery based on remaining flight time bool battery_unhealthy # Battery unhealthy -bool geofence_violated # Geofence violated +bool primary_geofence_breached # Primary Geofence breached bool mission_failure # Mission failure bool vtol_transition_failure # VTOL transition failed (quadchute) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c0c942e314..4ef6e2dae2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -73,15 +73,15 @@ #include typedef enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ - VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ + VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ } VEHICLE_MODE_FLAG; // TODO: generate @@ -487,7 +487,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) return Commander::main(argc, argv); } -bool Commander::shutdown_if_allowed() +bool Commander::shutdownIfAllowed() { return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks, @@ -815,7 +815,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { if (cmd.from_external && cmd.source_component == 190) { // MAV_COMP_ID_MISSIONPLANNER - print_reject_mode(desired_nav_state); + printRejectMode(desired_nav_state); } main_ret = TRANSITION_DENIED; @@ -1130,7 +1130,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST: - cmd_result = handle_command_actuator_test(cmd); + cmd_result = handleCommandActuatorTest(cmd); break; case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { @@ -1143,7 +1143,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { + } else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) { // 1: Reboot autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1153,7 +1153,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(BOARD_HAS_POWER_CONTROL) - } else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { + } else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) { // 2: Shutdown autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1163,7 +1163,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) { + } else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1435,8 +1435,7 @@ Commander::handle_command(const vehicle_command_s &cmd) return true; } -unsigned -Commander::handle_command_actuator_test(const vehicle_command_s &cmd) +unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd) { if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; @@ -1562,7 +1561,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_SWITCH_MODE: if (!_user_mode_intention.change(action_request.mode, true)) { - print_reject_mode(action_request.mode); + printRejectMode(action_request.mode); } break; @@ -1624,8 +1623,7 @@ void Commander::updateParameters() } -void -Commander::run() +void Commander::run() { /* initialize */ led_init(); @@ -1708,10 +1706,10 @@ Commander::run() checkForMissionUpdate(); - manual_control_check(); + manualControlCheck(); // data link checks which update the status - data_link_check(); + dataLinkCheck(); /* handle commands last, as the system needs to be updated to handle them */ if (_vehicle_command_sub.updated()) { @@ -1788,7 +1786,7 @@ Commander::run() _vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state); perf_end(_preflight_check_perf); - check_and_inform_ready_for_takeoff(); + checkAndInformReadyForTakeoff(); } // publish states (armed, control_mode, vehicle_status, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed @@ -1802,7 +1800,7 @@ Commander::run() _actuator_armed_pub.publish(_actuator_armed); // update and publish vehicle_control_mode - update_control_mode(); + updateControlMode(); // vehicle_status publish (after prearm/preflight updates above) _vehicle_status.arming_state = _arm_state_machine.getArmState(); @@ -1941,7 +1939,7 @@ void Commander::handlePowerButtonState() if (_power_button_state_sub.copy(&button_state)) { if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { - if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) { + if (shutdownIfAllowed() && (px4_shutdown_request() == 0)) { while (1) { px4_usleep(1); } } } @@ -2239,7 +2237,7 @@ bool Commander::handleModeIntentionAndFailsafe() return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction(); } -void Commander::check_and_inform_ready_for_takeoff() +void Commander::checkAndInformReadyForTakeoff() { #ifdef CONFIG_ARCH_BOARD_PX4_SITL static bool ready_for_takeoff_printed = false; @@ -2406,8 +2404,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) } } -void -Commander::update_control_mode() +void Commander::updateControlMode() { _vehicle_control_mode = {}; _offboard_control_mode_sub.update(); @@ -2417,8 +2414,7 @@ Commander::update_control_mode() _vehicle_control_mode_pub.publish(_vehicle_control_mode); } -void -Commander::print_reject_mode(uint8_t nav_state) +void Commander::printRejectMode(uint8_t nav_state) { if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) { @@ -2517,7 +2513,7 @@ void Commander::enable_hil() _vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON; } -void Commander::data_link_check() +void Commander::dataLinkCheck() { for (auto &telemetry_status : _telemetry_status_subs) { telemetry_status_s telemetry; @@ -2706,7 +2702,7 @@ void Commander::battery_status_check() 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)) { + if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) { mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t"); events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning}, "Dangerously low battery! Shutting system down"); @@ -2729,7 +2725,7 @@ void Commander::battery_status_check() _battery_warning = _vehicle_status_flags.battery_warning; } -void Commander::manual_control_check() +void Commander::manualControlCheck() { manual_control_setpoint_s manual_control_setpoint; const bool manual_control_updated = _manual_control_setpoint_sub.update(&manual_control_setpoint); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 02c481732c..1ffd6832c7 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -119,6 +119,7 @@ private: void answer_command(const vehicle_command_s &cmd, uint8_t result); transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); + transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false); void battery_status_check(); @@ -128,9 +129,9 @@ private: /** * Checks the status of all available data links and handles switching between different system telemetry states. */ - void data_link_check(); + void dataLinkCheck(); - void manual_control_check(); + void manualControlCheck(); /** * @brief Handle incoming vehicle command relavant to Commander @@ -142,71 +143,43 @@ private: */ bool handle_command(const vehicle_command_s &cmd); - unsigned handle_command_actuator_test(const vehicle_command_s &cmd); + unsigned handleCommandActuatorTest(const vehicle_command_s &cmd); void executeActionRequest(const action_request_s &action_request); - void print_reject_mode(uint8_t nav_state); + void printRejectMode(uint8_t nav_state); - void update_control_mode(); + void updateControlMode(); - bool shutdown_if_allowed(); + bool shutdownIfAllowed(); void send_parachute_command(); void checkForMissionUpdate(); + void handlePowerButtonState(); + void systemPowerUpdate(); + void landDetectorUpdate(); + void safetyButtonUpdate(); + void vtolStatusUpdate(); + void updateTunes(); + void checkWorkerThread(); + bool getPrearmState() const; void handleAutoDisarm(); + bool handleModeIntentionAndFailsafe(); void updateParameters(); - void check_and_inform_ready_for_takeoff(); - - DEFINE_PARAMETERS( - - (ParamInt) _param_com_dl_loss_t, - - (ParamInt) _param_com_rc_override, - - (ParamInt) _param_com_hldl_loss_t, - (ParamInt) _param_com_hldl_reg_t, - - (ParamBool) _param_com_home_en, - (ParamBool) _param_com_home_in_air, - - (ParamFloat) _param_com_disarm_land, - (ParamFloat) _param_com_disarm_preflight, - - (ParamBool) _param_com_obs_avoid, - - (ParamInt) _param_com_flt_profile, - - (ParamFloat) _param_com_obc_loss_t, - - (ParamInt) _param_com_prearm_mode, - (ParamBool) _param_com_force_safety, - (ParamBool) _param_com_mot_test_en, - - (ParamFloat) _param_com_kill_disarm, - - (ParamInt) _param_flight_uuid, - (ParamInt) _param_takeoff_finished_action - ) - - // optional parameters - param_t _param_mav_comp_id{PARAM_INVALID}; - param_t _param_mav_sys_id{PARAM_INVALID}; - param_t _param_mav_type{PARAM_INVALID}; - param_t _param_rc_map_fltmode{PARAM_INVALID}; + void checkAndInformReadyForTakeoff(); enum class PrearmedMode { DISABLED = 0, @@ -223,77 +196,87 @@ private: static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; - ArmStateMachine _arm_state_machine{}; + vehicle_status_s _vehicle_status{}; - FailureDetector _failure_detector{this}; - bool _flight_termination_triggered{false}; - bool _lockdown_triggered{false}; + ArmStateMachine _arm_state_machine{}; + Failsafe _failsafe_instance{this}; + FailsafeBase &_failsafe{_failsafe_instance}; + FailureDetector _failure_detector{this}; + HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; + Safety _safety{}; + UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; + WorkerThread _worker_thread{}; + + const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()}; + HomePosition _home_position{_vehicle_status_flags}; - hrt_abstime _datalink_last_heartbeat_gcs{0}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; - hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; - hrt_abstime _datalink_last_heartbeat_parachute_system{0}; - hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; - bool _onboard_controller_lost{false}; - bool _avoidance_system_lost{false}; - bool _parachute_system_lost{true}; - bool _open_drone_id_system_lost{true}; + Hysteresis _auto_disarm_landed{false}; + Hysteresis _auto_disarm_killed{false}; - hrt_abstime _high_latency_datalink_heartbeat{0}; - hrt_abstime _high_latency_datalink_lost{0}; + hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; + hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; + hrt_abstime _datalink_last_heartbeat_gcs{0}; + hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; + hrt_abstime _datalink_last_heartbeat_parachute_system{0}; - uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; - Hysteresis _auto_disarm_landed{false}; - Hysteresis _auto_disarm_killed{false}; + hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent - hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent - bool _mode_switch_mapped{false}; + hrt_abstime _high_latency_datalink_heartbeat{0}; + hrt_abstime _high_latency_datalink_lost{0}; - bool _last_overload{false}; - - bool _is_throttle_above_center{false}; - bool _is_throttle_low{false}; - - hrt_abstime _boot_timestamp{0}; - hrt_abstime _last_disarmed_timestamp{0}; - hrt_abstime _overload_start{0}; ///< time when CPU overload started + hrt_abstime _boot_timestamp{0}; + hrt_abstime _last_disarmed_timestamp{0}; + hrt_abstime _overload_start{0}; ///< time when CPU overload started hrt_abstime _led_armed_state_toggle{0}; hrt_abstime _led_overload_toggle{0}; - hrt_abstime _last_termination_message_sent{0}; + hrt_abstime _last_health_and_arming_check{0}; - bool _status_changed{true}; - bool _arm_tune_played{false}; - bool _was_armed{false}; - bool _have_taken_off_since_arming{false}; + uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; + + bool _failsafe_user_override_request{false}; ///< override request due to stick movements + + bool _flight_termination_triggered{false}; + bool _lockdown_triggered{false}; + + bool _open_drone_id_system_lost{true}; + bool _avoidance_system_lost{false}; + bool _onboard_controller_lost{false}; + bool _parachute_system_lost{true}; + + bool _last_overload{false}; + bool _mode_switch_mapped{false}; + + bool _is_throttle_above_center{false}; + bool _is_throttle_low{false}; + + bool _arm_tune_played{false}; + bool _was_armed{false}; + bool _have_taken_off_since_arming{false}; + bool _status_changed{true}; vehicle_land_detected_s _vehicle_land_detected{}; - vtol_vehicle_status_s _vtol_vehicle_status{}; // commander publications actuator_armed_s _actuator_armed{}; vehicle_control_mode_s _vehicle_control_mode{}; - vehicle_status_s _vehicle_status{}; - - Safety _safety; - - WorkerThread _worker_thread; + vtol_vehicle_status_s _vtol_vehicle_status{}; // Subscriptions - uORB::Subscription _action_request_sub {ORB_ID(action_request)}; + uORB::Subscription _action_request_sub{ORB_ID(action_request)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; 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 _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; + uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; #if defined(BOARD_HAS_POWER_CONTROL) uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)}; @@ -304,25 +287,41 @@ private: // Publications uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; - uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; + uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)}; uORB::Publication _vehicle_status_pub{ORB_ID(vehicle_status)}; - uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; - orb_advert_t _mavlink_log_pub{nullptr}; 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{this, _vehicle_status}; - hrt_abstime _last_health_and_arming_check{0}; - const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()}; + // optional parameters + param_t _param_mav_comp_id{PARAM_INVALID}; + param_t _param_mav_sys_id{PARAM_INVALID}; + param_t _param_mav_type{PARAM_INVALID}; + param_t _param_rc_map_fltmode{PARAM_INVALID}; - 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}; + DEFINE_PARAMETERS( + + (ParamFloat) _param_com_disarm_land, + (ParamFloat) _param_com_disarm_preflight, + (ParamInt) _param_com_dl_loss_t, + (ParamInt) _param_com_hldl_loss_t, + (ParamInt) _param_com_hldl_reg_t, + (ParamBool) _param_com_home_en, + (ParamBool) _param_com_home_in_air, + (ParamInt) _param_com_flt_profile, + (ParamBool) _param_com_force_safety, + (ParamFloat) _param_com_kill_disarm, + (ParamBool) _param_com_mot_test_en, + (ParamBool) _param_com_obs_avoid, + (ParamFloat) _param_com_obc_loss_t, + (ParamInt) _param_com_prearm_mode, + (ParamInt) _param_com_rc_override, + (ParamInt) _param_flight_uuid, + (ParamInt) _param_takeoff_finished_action + ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp index 3fcd2c6644..570df97233 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp @@ -41,9 +41,9 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter) geofence_result = {}; } - reporter.failsafeFlags().geofence_violated = geofence_result.geofence_violated; + reporter.failsafeFlags().primary_geofence_breached = geofence_result.primary_geofence_breached; - if (geofence_result.geofence_action != 0 && reporter.failsafeFlags().geofence_violated) { + if (geofence_result.primary_geofence_action != 0 && reporter.failsafeFlags().primary_geofence_breached) { /* EVENT * @description * @@ -58,7 +58,7 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter) } } - if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL + if (geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_RTL && reporter.failsafeFlags().home_position_invalid) { /* EVENT * @description diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index c80153a848..7b225803c7 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -395,7 +395,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, 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())); + CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get())); // Battery CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow)); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index e5863f585d..cc60dd023c 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -335,7 +335,7 @@ private: geofence_result_s geofence; if (_geofence_sub.update(&geofence)) { - if (geofence.geofence_violated) { + if (geofence.primary_geofence_breached) { msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6cbbfa0b7a..070fd90dd9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -863,12 +863,12 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) have_geofence_position_data = false; _geofence_result.timestamp = hrt_absolute_time(); - _geofence_result.geofence_action = _geofence.getGeofenceAction(); + _geofence_result.primary_geofence_action = _geofence.getGeofenceAction(); _geofence_result.home_required = _geofence.isHomeRequired(); if (gf_violation_type.value) { /* inform other apps via the mission result */ - _geofence_result.geofence_violated = true; + _geofence_result.primary_geofence_breached = true; /* 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) { @@ -918,7 +918,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) } else { /* inform other apps via the mission result */ - _geofence_result.geofence_violated = false; + _geofence_result.primary_geofence_breached = false; /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false;