Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 18:06:39 +08:00
Code Issues Actions 5 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_mode_manager/FixedWingModeManager.cpp
+25 -8
View File
@@ -170,6 +170,8 @@ FixedWingModeManager::airspeed_poll()
_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s; _airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
} }
_time_airspeed_last_valid = airspeed_validated.timestamp;
} }
// no airspeed updates for one second --> declare invalid // no airspeed updates for one second --> declare invalid
@@ -248,6 +250,7 @@ FixedWingModeManager::vehicle_attitude_poll()
const Eulerf euler_angles(R); const Eulerf euler_angles(R);
_yaw = euler_angles(2); _yaw = euler_angles(2);
_pitch = euler_angles(1);
const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
_body_acceleration_x = body_acceleration(0); _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 float
FixedWingModeManager::get_manual_airspeed_setpoint() 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.flaring = true;
_flare_states.start_time = now; _flare_states.start_time = now;
_flare_states.initial_height_rate_setpoint = -_local_pos.vz; _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, events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info,
"Landing, flaring"); "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; (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()) + 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()) + 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) { 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_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 // idle throttle may be >0 for internal combustion engines
// normally set to zero for electric motors // 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() + const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() +
(1.0f - flare_ramp_interpolator_sqrt) * (1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_throttle;
_param_fw_thr_max.get();
const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
.timestamp = now, .timestamp = now,
@@ -1654,6 +1668,8 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
_flare_states.flaring = true; _flare_states.flaring = true;
_flare_states.start_time = now; _flare_states.start_time = now;
_flare_states.initial_height_rate_setpoint = -_local_pos.vz; _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, events::send(events::ID("fixedwing_position_control_landing_circle_flaring"), events::Log::Info,
"Landing, flaring"); "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; (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()) + 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()) + 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) { 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_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 // idle throttle may be >0 for internal combustion engines
// normally set to zero for electric motors // 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() + const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() +
(1.0f - flare_ramp_interpolator_sqrt) * (1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_throttle;
_param_fw_thr_max.get();
const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
.timestamp = now, .timestamp = now,
.altitude = NAN, .altitude = NAN,
@@ -2129,6 +2145,7 @@ FixedWingModeManager::Run()
airspeed_poll(); airspeed_poll();
manual_control_setpoint_poll(); manual_control_setpoint_poll();
vehicle_attitude_poll(); vehicle_attitude_poll();
vehicle_attitude_setpoint_poll();
vehicle_command_poll(); vehicle_command_poll();
vehicle_control_mode_poll(); vehicle_control_mode_poll();
wind_poll(now); wind_poll(now);
src/modules/fw_mode_manager/FixedWingModeManager.hpp
+6
View File
@@ -184,6 +184,7 @@ private:
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; 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_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -252,6 +253,8 @@ private:
float _yaw{0.0f}; float _yaw{0.0f};
float _yawrate{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_acceleration_x{0.f};
float _body_velocity_x{0.f}; float _body_velocity_x{0.f};
@@ -336,6 +339,8 @@ private:
bool flaring{false}; bool flaring{false};
hrt_abstime start_time{0}; // [us] hrt_abstime start_time{0}; // [us]
float initial_height_rate_setpoint{0.0f}; // [m/s] 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; } _flare_states;
// [m] last terrain estimate which was valid // [m] last terrain estimate which was valid
@@ -419,6 +424,7 @@ private:
void manual_control_setpoint_poll(); void manual_control_setpoint_poll();
void vehicle_attitude_poll(); void vehicle_attitude_poll();
void vehicle_attitude_setpoint_poll();
void vehicle_command_poll(); void vehicle_command_poll();
void vehicle_control_mode_poll(); void vehicle_control_mode_poll();