diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ed8d4bda23..2d1f255bac 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1782,10 +1782,21 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const float height_rate_setpoint = flare_ramp_interpolator_sqrt * (-_param_fw_lnd_fl_sink.get()) + (1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint; - const float pitch_min_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmin.get()) + - (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_min.get()); - const float pitch_max_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmax.get()) + - (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_max.get()); + float pitch_min_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmin.get()) + + (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_min.get()); + float pitch_max_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmax.get()) + + (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_max.get()); + + if (_param_fw_lnd_td_time.get() > FLT_EPSILON) { + const float touchdown_time = math::max(_param_fw_lnd_td_time.get(), _param_fw_lnd_fl_time.get()); + + const float touchdown_interpolator = math::constrain((seconds_since_flare_start - touchdown_time) / + POST_TOUCHDOWN_CLAMP_TIME, 0.0f, + 1.0f); + + pitch_max_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_max_rad; + pitch_min_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_min_rad; + } // idle throttle may be >0 for internal combustion engines // normally set to zero for electric motors diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 9d21d6f54b..7ec8e806e1 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -166,6 +166,9 @@ static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f; // [.] normalized deadzone threshold for manual nudging input static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f; +// [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare) +static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f; + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -867,13 +870,16 @@ private: (ParamFloat) _param_fw_lnd_fl_time, (ParamFloat) _param_fw_lnd_fl_sink, + (ParamFloat) _param_fw_lnd_td_time, (ParamFloat) _param_fw_lnd_td_off, (ParamInt) _param_fw_lnd_nudge, (ParamInt) _param_fw_lnd_abort, (ParamFloat) _param_fw_wind_arsp_sc, - (ParamFloat) _param_fw_tko_airspd + (ParamFloat) _param_fw_tko_airspd, + + (ParamFloat) _param_rwto_psp ) }; 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 ea85acd11f..1b27edaf43 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 @@ -1034,6 +1034,26 @@ PARAM_DEFINE_FLOAT(FW_WING_HEIGHT, 0.5); */ PARAM_DEFINE_FLOAT(FW_LND_FL_TIME, 1.0f); +/** + * Landing touchdown time (since flare start) + * + * This is the time after the start of flaring that we expect the vehicle to touch the runway. + * At this time, a 0.5s clamp down ramp will engage, constraining the pitch setpoint to RWTO_PSP. + * If enabled, ensure that RWTO_PSP is configured appropriately for full gear contact on ground roll. + * + * Set to -1.0 to disable touchdown clamping. E.g. it may not be desirable to clamp on belly landings. + * + * The touchdown time will be constrained to be greater than or equal to the flare time (FW_LND_FL_TIME). + * + * @unit s + * @min -1.0 + * @max 5.0 + * @decimal 1 + * @increment 0.1 + * @group FW Auto Landing + */ +PARAM_DEFINE_FLOAT(FW_LND_TD_TIME, -1.0f); + /** * Landing flare sink rate *