fw pos ctrl: encapsulate wing tip strike constraint for roll angle

- apply constraint only for takeoff and landing modes
- add two params, wing span and wing height, to calculate a reasonable height at which roll limits can be opened

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1412,6 +1412,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_launch_detection_notify = 0;
 	}
 
+	float terrain_alt = _takeoff_ground_alt;
+
 	if (_runway_takeoff.runwayTakeoffEnabled()) {
 		if (!_runway_takeoff.isInitialized()) {
 			_runway_takeoff.init(now, _yaw, _current_latitude, _current_longitude);
@@ -1428,7 +1430,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_runway_takeoff.forceSetFlyState();
 		}
 
-		float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
+		terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
 
 		// update runway takeoff helper
 		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
@@ -1561,9 +1563,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 							   true,
 							   radians(_takeoff_pitch_min.get()));
 
-				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
-
 			} else {
 				tecs_update_pitch_throttle(control_interval,
 							   pos_sp_curr.alt,
@@ -1614,6 +1613,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.pitch_body = get_tecs_pitch();
 	}
 
+	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
+
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(pos_sp_curr);
 	}
@@ -1821,11 +1822,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 		}
 
-		if (_land_noreturn_horizontal) {
-			/* limit roll motion to prevent wings from touching the ground first */
-			_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
-		}
-
 		/* enable direct yaw control using rudder/wheel */
 		if (_land_noreturn_horizontal) {
 			_att_sp.yaw_body = _target_bearing;
@@ -1954,6 +1950,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 	// when we are landed state we want the motor to spin at idle speed
 	_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
 
+	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
+
 	// Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
 	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
@@ -2609,6 +2607,23 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	tecs_status_publish();
 }
 
+float
+FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, const float altitude,
+		const float terrain_altitude) const
+{
+	// we want the wings level when at the wing height above ground
+	const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f);
+
+	// this is a conservative (linear) approximation of the roll angle that would cause wing tip strike
+	// roll strike = arcsin( 2 * height / span )
+	// d(roll strike)/d(height) = 2 / span / cos(2 * height / span)
+	// d(roll strike)/d(height) (@height=0) = 2 / span
+	// roll strike ~= 2 * height / span
+	const float roll_wingtip_strike = 2.0f * height_above_ground / _param_fw_wing_span.get();
+
+	return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike);
+}
+
 void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
 {
 	if (_global_local_proj_ref.isInitialized()) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -646,6 +646,16 @@ private:
 					bool climbout_mode, float climbout_pitch_min_rad,
 					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
 
+	/**
+	 * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
+	 *
+	 * @param roll_setpoint Unconstrained roll angle setpoint [rad]
+	 * @param altitude Vehicle altitude (AMSL) [m]
+	 * @param terrain_altitude Terrain altitude (AMSL) [m]
+	 * @return Constrained roll angle setpoint [rad]
+	 */
+	float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
+
 	DEFINE_PARAMETERS(
 
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
@@ -736,8 +746,10 @@ private:
 		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
 
 		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
-		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross
+		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
 
+		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
+		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height
 	)
 
 };

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -1007,3 +1007,32 @@ PARAM_DEFINE_FLOAT(WEIGHT_BASE, -1.0f);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);
+
+/**
+ * The aircraft's wing span (length from tip to tip).
+ *
+ * This is used for limiting the roll setpoint near the ground. (if multiple wings, take the longest span)
+ *
+ * @unit m
+ * @min 1.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW Geometry
+ */
+PARAM_DEFINE_FLOAT(FW_WING_SPAN, 6.0);
+
+/**
+ * Height (AGL) of the wings when the aircraft is on the ground.
+ *
+ * This is used to constrain a minimum altitude below which we keep wings level to avoid wing tip strike. It's prudent
+ * to give a slight margin here (> 0m)
+ *
+ * @unit m
+ * @min 0.0
+ * @max 5.0
+ * @decimal 1
+ * @increment 1
+ * @group FW Geometry
+ */
+PARAM_DEFINE_FLOAT(FW_WING_HEIGHT, 0.5);