From 25de111a4a01038e6c3db72044afc9e4338a0495 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Tue, 6 Jan 2026 12:08:16 +0100 Subject: [PATCH] FWModeManager: improve fixed-wing landing flare stability - Ramp pitch_min and pitch_max from current pitch to flare minimum - Ramp throttle from current setpoint to idle --- .../fw_mode_manager/FixedWingModeManager.cpp | 33 ++++++++++++++----- .../fw_mode_manager/FixedWingModeManager.hpp | 6 ++++ 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 31d610473e..0ac0d36264 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -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); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index bf207b6d68..e026f8ce26 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -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();