dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixed-wing landing: add a touchdown time and subsequent ramp down to rwto pitch setpoint after the flare to keep wheels on ground

This commit is contained in:
Thomas Stastny 2022-10-27 18:01:27 +02:00 committed by Daniel Agar
parent 1de1416773
commit a8c2eaf3e0
3 changed files with 42 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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

8
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
{
@ -867,13 +870,16 @@ private:
(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
(ParamFloat<px4::params::FW_LND_TD_TIME>) _param_fw_lnd_td_time,
(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd
(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd,
(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp
)
};

20
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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
*