Apply Google Style to Commander Private methods, rename geofence message geofence_violation to primary_geofence_breached.

msg/geofence_result.msg

@@ -1,12 +1,13 @@
-uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp                        # time since system start (microseconds)
-uint8 GF_ACTION_NONE = 0        # no action on geofence violation
+uint8 GF_ACTION_NONE = 0                # no action on geofence violation
-uint8 GF_ACTION_WARN = 1        # critical mavlink message
+uint8 GF_ACTION_WARN = 1                # critical mavlink message
-uint8 GF_ACTION_LOITER = 2      # switch to AUTO|LOITER
+uint8 GF_ACTION_LOITER = 2              # switch to AUTO|LOITER
-uint8 GF_ACTION_RTL = 3         # switch to AUTO|RTL
+uint8 GF_ACTION_RTL = 3                 # switch to AUTO|RTL
-uint8 GF_ACTION_TERMINATE = 4   # flight termination
+uint8 GF_ACTION_TERMINATE = 4           # flight termination
-uint8 GF_ACTION_LAND = 5        # switch to AUTO|LAND
+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

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)
 

src/modules/commander/Commander.cpp

@@ -73,15 +73,15 @@
 #include <uORB/topics/tune_control.h>
 
 typedef enum VEHICLE_MODE_FLAG {
-	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
+	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_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_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_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_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_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_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_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_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
+unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
-Commander::handle_command_actuator_test(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
+void Commander::run()
-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
+void Commander::updateControlMode()
-Commander::update_control_mode()
 {
 	_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
+void Commander::printRejectMode(uint8_t nav_state)
-Commander::print_reject_mode(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);

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();
+	void checkAndInformReadyForTakeoff();
-
-	DEFINE_PARAMETERS(
-
-		(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
-
-		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
-
-		(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
-		(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
-
-		(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en,
-		(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
-
-		(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
-		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
-
-		(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
-
-		(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
-
-		(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
-
-		(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
-		(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
-		(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
-
-		(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
-
-		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
-		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _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};
 
 	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};
+	ArmStateMachine		_arm_state_machine{};
-	bool		_flight_termination_triggered{false};
+	Failsafe		_failsafe_instance{this};
-	bool		_lockdown_triggered{false};
+	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};
+	Hysteresis _auto_disarm_landed{false};
-	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};
+	Hysteresis _auto_disarm_killed{false};
-	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};
 
-	hrt_abstime	_high_latency_datalink_heartbeat{0};
+	hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
-	hrt_abstime	_high_latency_datalink_lost{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};
+	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent
-	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 _high_latency_datalink_heartbeat{0};
-	bool            _mode_switch_mapped{false};
+	hrt_abstime _high_latency_datalink_lost{0};
 
-	bool		_last_overload{false};
+	hrt_abstime _boot_timestamp{0};
-
+	hrt_abstime _last_disarmed_timestamp{0};
-	bool		_is_throttle_above_center{false};
+	hrt_abstime _overload_start{0};		///< time when CPU overload started
-	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 _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};
+	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
-	bool		_arm_tune_played{false};
+
-	bool		_was_armed{false};
+	bool _failsafe_user_override_request{false}; ///< override request due to stick movements
-	bool		_have_taken_off_since_arming{false};
+
+	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{};
+	vtol_vehicle_status_s	_vtol_vehicle_status{};
-
-	Safety _safety;
-
-	WorkerThread _worker_thread;
 
 	// 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_s>        _telemetry_status_subs{ORB_ID::telemetry_status};
+	uORB::SubscriptionMultiArray<telemetry_status_s>	_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_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
-	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
+	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
+	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
 	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
 
-	uORB::Publication<vehicle_command_ack_s>		_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};
+	// optional parameters
-	hrt_abstime _last_health_and_arming_check{0};
+	param_t _param_mav_comp_id{PARAM_INVALID};
-	const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()};
+	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};
+	DEFINE_PARAMETERS(
-	Failsafe _failsafe_instance{this};
+
-	FailsafeBase &_failsafe{_failsafe_instance};
+		(ParamFloat<px4::params::COM_DISARM_LAND>)  _param_com_disarm_land,
-	bool _failsafe_user_override_request{false}; ///< override request due to stick movements
+		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
-	UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
+		(ParamInt<px4::params::COM_DL_LOSS_T>)      _param_com_dl_loss_t,
+		(ParamInt<px4::params::COM_HLDL_LOSS_T>)    _param_com_hldl_loss_t,
+		(ParamInt<px4::params::COM_HLDL_REG_T>)     _param_com_hldl_reg_t,
+		(ParamBool<px4::params::COM_HOME_EN>)       _param_com_home_en,
+		(ParamBool<px4::params::COM_HOME_IN_AIR>)   _param_com_home_in_air,
+		(ParamInt<px4::params::COM_FLT_PROFILE>)    _param_com_flt_profile,
+		(ParamBool<px4::params::COM_FORCE_SAFETY>)  _param_com_force_safety,
+		(ParamFloat<px4::params::COM_KILL_DISARM>)  _param_com_kill_disarm,
+		(ParamBool<px4::params::COM_MOT_TEST_EN>)   _param_com_mot_test_en,
+		(ParamBool<px4::params::COM_OBS_AVOID>)     _param_com_obs_avoid,
+		(ParamFloat<px4::params::COM_OBC_LOSS_T>)   _param_com_obc_loss_t,
+		(ParamInt<px4::params::COM_PREARM_MODE>)    _param_com_prearm_mode,
+		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
+		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
+		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action
+	)
 };

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
 		 * <profile name="dev">
@@ -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

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));

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;
 			}
 

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;