Merge branch 'main' into pr-MPC_param_readability

ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow

@@ -47,5 +47,5 @@ param set-default MPC_ALT_MODE 2
 
 param set-default SENS_FLOW_ROT 6
 param set-default SENS_FLOW_MINHGT 0.7
-param set-default SENS_FLOW_MAXHGT 3
+param set-default SENS_FLOW_MAXHGT 15
 param set-default SENS_FLOW_MAXR 2.5

Tools/upload.sh

@@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
 fi
 
 if [ $SYSTYPE = "Linux" ]; then
-SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/ARK*,"
+SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/usb-ARK*,"
 fi
 
 if [[ $SYSTYPE = *"CYGWIN"* ]]; then

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -727,7 +727,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 				     _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
 
 	// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
-	const float eph_critical = 2.5f * _param_com_pos_fs_eph.get(); // threshold used to trigger the navigation failsafe
+	const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe
 	const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
 
 	estimator_status_flags_s estimator_status_flags;

src/modules/commander/failsafe/framework.cpp

@@ -224,14 +224,24 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action
 				{events::Log::Warning, events::LogInternal::Warning},
 				"Failsafe warning:", mavlink_mode);
 
+			} else if (action == Action::Descend || action == Action::FallbackAltCtrl || action == Action::FallbackStab) {
+				/* EVENT
+				* @description Failsafe actions that disengage the autopilot (remove position control)
+				* @type append_health_and_arming_messages
+				*/
+				events::send<uint32_t, events::px4::enums::failsafe_action_t>(
+					events::ID("commander_failsafe_enter_autopilot_disengaged"),
+				{events::Log::Critical, events::LogInternal::Warning},
+				"Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action);
+
 			} else {
 				/* EVENT
 				* @type append_health_and_arming_messages
 				*/
 				events::send<uint32_t, events::px4::enums::failsafe_action_t>(
 					events::ID("commander_failsafe_enter_generic"),
 				{events::Log::Critical, events::LogInternal::Warning},
-				"Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action);
+				"Failsafe activated: switching to {2}", mavlink_mode, failsafe_action);
 			}
 
 		} else {
@@ -467,7 +477,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 
 	// Check if we should enter delayed Hold
 	if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly
-	    && selected_action != Action::Disarm && selected_action != Action::Terminate) {
+	    && selected_action != Action::Disarm && selected_action != Action::Terminate && selected_action != Action::Hold) {
 		returned_state.delayed_action = selected_action;
 		selected_action = Action::Hold;
 		allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -452,7 +452,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
 
 		} else {
-			_triplet_prev_wp = _position;
+			_triplet_prev_wp = _triplet_target;
 		}
 
 		_prev_was_valid = _sub_triplet_setpoint.get().previous.valid;

src/modules/navigator/land.cpp

@@ -57,8 +57,13 @@ Land::on_activation()
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->previous.valid = false;
+
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		_navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon);
+	}
+
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/mission_block.cpp

@@ -1042,3 +1042,29 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i
 		_mission_item.altitude_is_relative = false;
 	}
 }
+
+void MissionBlock::updateFailsafeChecks()
+{
+	updateMaxHaglFailsafe();
+}
+
+void MissionBlock::updateMaxHaglFailsafe()
+{
+	const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
+
+	if (_navigator->get_global_position()->terrain_alt_valid
+	    && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) {
+		// Handle case where the altitude setpoint is above the maximum HAGL (height above ground level)
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t");
+		events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");
+
+		_navigator->trigger_hagl_failsafe(getNavigatorStateId());
+
+		// While waiting for a failsafe action from commander, keep the curren position
+		setLoiterItemFromCurrentPosition(&_mission_item);
+
+		mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
+
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}

src/modules/navigator/mission_block.h

@@ -145,6 +145,8 @@ class MissionBlock : public NavigatorMode
 	void set_align_mission_item(struct mission_item_s *const mission_item,
 				    const struct mission_item_s *const mission_item_next) const;
 
+	void updateFailsafeChecks() override;
+
 protected:
 	/**
 	 * @brief heading mode for setting navigation items
@@ -249,4 +251,8 @@ class MissionBlock : public NavigatorMode
 	bool _payload_deploy_ack_successful{false};	// Flag to keep track of whether we received an acknowledgement for a successful payload deployment
 	hrt_abstime _payload_deployed_time{0};		// Last payload deployment start time to handle timeouts
 	float _payload_deploy_timeout_s{0.0f};		// Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received
+
+private:
+	void updateMaxHaglFailsafe();
+
 };

src/modules/navigator/navigator_mode.cpp

@@ -61,6 +61,7 @@ NavigatorMode::run(bool active)
 		} else {
 			/* periodic updates when active */
 			on_active();
+			updateFailsafeChecks();
 		}
 
 	} else {

src/modules/navigator/navigator_mode.h

@@ -80,6 +80,8 @@ class NavigatorMode
 	 */
 	virtual void on_active();
 
+	virtual void updateFailsafeChecks() {};
+
 protected:
 	Navigator *_navigator{nullptr};
 

src/modules/navigator/rtl.cpp

@@ -283,10 +283,12 @@ void RTL::on_active()
 	case RtlType::RTL_MISSION_FAST_REVERSE:
 	case RtlType::RTL_DIRECT_MISSION_LAND:
 		_rtl_mission_type_handle->on_active();
+		_rtl_mission_type_handle->updateFailsafeChecks();
 		break;
 
 	case RtlType::RTL_DIRECT:
 		_rtl_direct.on_active();
+		_rtl_direct.updateFailsafeChecks();
 		break;
 
 	default:
@@ -528,13 +530,14 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &
 	// avoid the vehicle touching the ground while still moving horizontally.
 	const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get();
 
-	float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get();
+	const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get();
+
+	float return_altitude_amsl = max_return_altitude;
 
 	if (destination_dist <= _param_nav_acc_rad.get()) {
 		return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist;
 
 	} else {
-
 		if (destination_dist <= _param_rtl_min_dist.get()) {
 
 			// constrain cone half angle to meaningful values. All other cases are already handled above.
@@ -549,7 +552,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &
 		return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl);
 	}
 
-	return max(return_altitude_amsl, _global_pos_sub.get().alt);
+	return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude);
 }
 
 void RTL::init_rtl_mission_type()

src/modules/navigator/rtl_direct.cpp

@@ -158,16 +158,6 @@ void RtlDirect::set_rtl_item()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	if (_global_pos_sub.get().terrain_alt_valid
-	    && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) {
-		// Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL
-		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t");
-		events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL");
-
-		_navigator->trigger_hagl_failsafe(getNavigatorStateId());
-		_rtl_state = RTLState::IDLE;
-	}
-
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon,
 				       _global_pos_sub.get().lat, _global_pos_sub.get().lon);
 	const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);