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

VTOL Backtransition lateral position control improvements (#25704)

Browse Source 
During backtransition in gusty conditions the current "pitch-up to decelerate" strategy had deficiencies as the motion is not always purely in body-x direction. Thus we replace it here with a "tilt-up to decelerate" strategy.
Secondly, in GNSS-denied environments where the position error increases, tracking a precise landing point through position feedback is not reasonable, and we now instead discard the position feedback in cases where the position error exceeds 10m.

* add eph limit check for dist-to-target VT BT deceleration

* add alphafilter for acceleration estimate in VT BT, rename constants

* explicitly set yawspeed_setpoint to 0

* replace backtransition pitch-setpoint with tilt-setpoint

* blend vtol-backtransition roll vehicle_attitude_sp based on mc_weight

* remove memcpy for v_att_sp
This commit is contained in:
Marco Hauswirth 2025-11-06 13:36:22 +01:00 committed by GitHub
parent 517f04259c
commit 561a83cf6c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 49 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View File

@ -57,6 +57,9 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
_vel_z_filter.reset(_velocity(2));
}
const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay};
_accel_filter.reset(acceleration_xy);
if (_sub_vehicle_status.get().in_transition_to_fw) {
_gear.landing_gear = landing_gear_s::GEAR_UP;
@ -86,21 +89,27 @@ bool FlightTaskTransition::update()
// calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_fw_psp_off
// and zero roll angle
float pitch_setpoint = math::radians(_param_fw_psp_off);
float tilt_setpoint = math::radians(_param_fw_psp_off);
Vector2f horizontal_acceleration_direction;
if (!_sub_vehicle_status.get().in_transition_to_fw) {
pitch_setpoint = computeBackTranstionPitchSetpoint();
tilt_setpoint = computeBackTransitionTiltSetpoint();
const Vector2f velocity_xy{_velocity};
horizontal_acceleration_direction = -velocity_xy.unit_or_zero();
} else {
// Forward transition: use heading direction
horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f);
}
// Calculate horizontal acceleration components to follow a pitch setpoint with the current vehicle heading
const Vector2f horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f);
_acceleration_setpoint.xy() = tanf(pitch_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction;
_acceleration_setpoint.xy() = tanf(tilt_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction;
_yaw_setpoint = NAN;
_yawspeed_setpoint = 0.f;
return ret;
}
float FlightTaskTransition::computeBackTranstionPitchSetpoint()
float FlightTaskTransition::computeBackTransitionTiltSetpoint()
{
const Vector2f position_xy{_position};
const Vector2f velocity_xy{_velocity};
@ -109,8 +118,11 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint()
float deceleration_setpoint = _param_vt_b_dec_mss;
if (_sub_position_sp_triplet.get().current.valid && _sub_vehicle_local_position.get().xy_global
&& position_xy.isAllFinite() && velocity_xy.isAllFinite()) {
const float max_hor_pos_uncertainty_limit = 10.f;
if (_sub_vehicle_local_position.get().eph < max_hor_pos_uncertainty_limit
&& _sub_position_sp_triplet.get().current.valid
&& _sub_vehicle_local_position.get().xy_global && position_xy.isAllFinite() && velocity_xy.isAllFinite()) {
Vector2f position_setpoint_local;
_geo_projection.project(_sub_position_sp_triplet.get().current.lat, _sub_position_sp_triplet.get().current.lon,
position_setpoint_local(0), position_setpoint_local(1));
@ -129,13 +141,13 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint()
deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss);
}
// Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow
const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay};
const Vector2f acceleration_xy_raw{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay};
const Vector2f acceleration_xy = _accel_filter.update(acceleration_xy_raw, _deltatime);
const float deceleration = -acceleration_xy.dot(velocity_xy_direction); // Zero when velocity invalid
const float deceleration_error = deceleration_setpoint - deceleration;
// Update back-transition deceleration error integrator
_decel_error_bt_int += (_param_vt_b_dec_i * deceleration_error) * _deltatime;
_decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, DECELERATION_INTEGRATOR_LIMIT);
_decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, kDecelerationIntegratorLimit);
return _decel_error_bt_int;
}

10
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View File

@ -61,8 +61,9 @@ public:
bool update() override;
private:
static constexpr float VERTICAL_VELOCITY_TIME_CONSTANT = 2.0f;
static constexpr float DECELERATION_INTEGRATOR_LIMIT = .3f;
static constexpr float kVerticalVelocityTimeConstant = 2.0f;
static constexpr float kDecelerationIntegratorLimit = 0.3f;
static constexpr float kAccelerationFilterTimeConstant = 0.05f;
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)};
@ -71,8 +72,9 @@ private:
float _param_vt_b_dec_i{0.f};
float _param_vt_b_dec_mss{0.f};
AlphaFilter<float> _vel_z_filter{VERTICAL_VELOCITY_TIME_CONSTANT};
AlphaFilter<float> _vel_z_filter{kVerticalVelocityTimeConstant};
AlphaFilter<matrix::Vector2f> _accel_filter{kAccelerationFilterTimeConstant};
float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value
float computeBackTranstionPitchSetpoint();
float computeBackTransitionTiltSetpoint();
};

32
src/modules/vtol_att_control/standard.cpp
View File

@ -180,7 +180,7 @@ void Standard::update_transition_state()
return;
}
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
*_v_att_sp = *_mc_virtual_att_sp;
} else {
// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
@ -188,7 +188,7 @@ void Standard::update_transition_state()
return;
}
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
*_v_att_sp = *_fw_virtual_att_sp;
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
@ -198,11 +198,12 @@ void Standard::update_transition_state()
float pitch_body = attitude_setpoint_euler.theta();
float yaw_body = attitude_setpoint_euler.psi();
if (_v_control_mode->flag_control_climb_rate_enabled) {
roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
}
if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
if (_v_control_mode->flag_control_climb_rate_enabled) {
roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
}
if (_param_vt_psher_slew.get() <= FLT_EPSILON) {
// just set the final target throttle value
_pusher_throttle = _param_vt_f_trans_thr.get();
@ -239,12 +240,24 @@ void Standard::update_transition_state()
_v_att_sp->thrust_body[0] = _pusher_throttle;
const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get();
}
mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
if (_v_control_mode->flag_control_climb_rate_enabled) {
// control backtransition deceleration using pitch.
pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta();
// blend roll setpoint between FW and MC
const float roll_body_fw = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
roll_body = mc_weight * roll_body + (1.0f - mc_weight) * roll_body_fw;
}
const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
@ -252,15 +265,8 @@ void Standard::update_transition_state()
q_sp.copyTo(_v_att_sp->q_d);
_pusher_throttle = 0.0f;
// continually increase mc attitude control as we transition back to mc mode
if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get();
}
}
mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
_mc_roll_weight = mc_weight;
_mc_pitch_weight = mc_weight;
_mc_yaw_weight = mc_weight;

2
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -341,7 +341,7 @@ PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f);
PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f);
/**
* Backtransition deceleration setpoint to pitch I gain.
* Backtransition deceleration setpoint to tilt I gain.
*
* @unit rad s/m
* @min 0