fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FWModeManager: improve fixed-wing landing flare stability

Browse Source 
- Ramp pitch_min and pitch_max from current pitch to flare minimum
- Ramp throttle from current setpoint to idle
This commit is contained in:
mahima-yoga 2026-01-06 12:08:16 +01:00 committed by Mahima Yoga
parent 9169f9cd44
commit 25de111a4a
2 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/fw_mode_manager/FixedWingModeManager.cpp
View File

@ -170,6 +170,8 @@ FixedWingModeManager::airspeed_poll()
_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
}
_time_airspeed_last_valid = airspeed_validated.timestamp;
}
// no airspeed updates for one second --> declare invalid
@ -248,6 +250,7 @@ FixedWingModeManager::vehicle_attitude_poll()
const Eulerf euler_angles(R);
_yaw = euler_angles(2);
_pitch = euler_angles(1);
const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
_body_acceleration_x = body_acceleration(0);
@ -257,6 +260,15 @@ FixedWingModeManager::vehicle_attitude_poll()
}
}
void FixedWingModeManager::vehicle_attitude_setpoint_poll()
{
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
if (_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint)) {
_throttle = vehicle_attitude_setpoint.thrust_body[0];
}
}
float
FixedWingModeManager::get_manual_airspeed_setpoint()
{
@ -1470,6 +1482,8 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
_flare_states.flaring = true;
_flare_states.start_time = now;
_flare_states.initial_height_rate_setpoint = -_local_pos.vz;
_flare_states.initial_pitch = _pitch;
_flare_states.initial_throttle = math::min(_throttle, _param_fw_thr_max.get());
events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info,
"Landing, flaring");
}
@ -1502,9 +1516,9 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint;
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());
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_pitch;
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());
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_pitch;
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());
@ -1521,9 +1535,9 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
// idle throttle may be >0 for internal combustion engines
// normally set to zero for electric motors
// Ramp throttle from current value to idle over flare duration
const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() +
(1.0f - flare_ramp_interpolator_sqrt) *
_param_fw_thr_max.get();
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_throttle;
const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
.timestamp = now,
@ -1654,6 +1668,8 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
_flare_states.flaring = true;
_flare_states.start_time = now;
_flare_states.initial_height_rate_setpoint = -_local_pos.vz;
_flare_states.initial_pitch = _pitch;
_flare_states.initial_throttle = math::min(_throttle, _param_fw_thr_max.get());
events::send(events::ID("fixedwing_position_control_landing_circle_flaring"), events::Log::Info,
"Landing, flaring");
}
@ -1681,9 +1697,9 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint;
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());
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_pitch;
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());
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_pitch;
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());
@ -1699,9 +1715,9 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
// idle throttle may be >0 for internal combustion engines
// normally set to zero for electric motors
// Ramp throttle from current value to idle over flare duration
const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() +
(1.0f - flare_ramp_interpolator_sqrt) *
_param_fw_thr_max.get();
(1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_throttle;
const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
.timestamp = now,
.altitude = NAN,
@ -2129,6 +2145,7 @@ FixedWingModeManager::Run()
airspeed_poll();
manual_control_setpoint_poll();
vehicle_attitude_poll();
vehicle_attitude_setpoint_poll();
vehicle_command_poll();
vehicle_control_mode_poll();
wind_poll(now);

6
src/modules/fw_mode_manager/FixedWingModeManager.hpp
View File

@ -184,6 +184,7 @@ private:
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@ -252,6 +253,8 @@ private:
float _yaw{0.0f};
float _yawrate{0.0f};
float _pitch{0.0f}; // [rad] current pitch angle from attitude
float _throttle{0.0f}; // [0-1] last set throttle
float _body_acceleration_x{0.f};
float _body_velocity_x{0.f};
@ -336,6 +339,8 @@ private:
bool flaring{false};
hrt_abstime start_time{0}; // [us]
float initial_height_rate_setpoint{0.0f}; // [m/s]
float initial_pitch{0.0f}; // [rad]
float initial_throttle{0.0f}; // [0-1] throttle value when flare started
} _flare_states;
// [m] last terrain estimate which was valid
@ -419,6 +424,7 @@ private:
void manual_control_setpoint_poll();
void vehicle_attitude_poll();
void vehicle_attitude_setpoint_poll();
void vehicle_command_poll();
void vehicle_control_mode_poll();