diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 41625d5ae6..e39ec6d6ce 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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 ¤t_waypoint) { if (_global_local_proj_ref.isInitialized()) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index a10f1d9444..eb13efac2f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/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) _param_fw_airspd_max, @@ -736,8 +746,10 @@ private: (ParamFloat) _param_nav_fw_alt_rad, (ParamFloat) _param_weight_base, - (ParamFloat) _param_weight_gross + (ParamFloat) _param_weight_gross, + (ParamFloat) _param_fw_wing_span, + (ParamFloat) _param_fw_wing_height ) }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index a19a0111b9..e01577895f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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);