Skip to content

VTOL Backtransition Improvements #25704

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
+49 −28
Draft

VTOL Backtransition Improvements #25704

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2897dbe
* replace backtransition pitch-setpoint with tilt-setpoint
haumarco Oct 3, 2025
84a3f85
blend vtol-backtransition roll vehicle_attitude_sp based on mc_weight
haumarco Oct 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
if (_sub_vehicle_local_position.get().xy_valid) {
_position(0) = _sub_vehicle_local_position.get().x;
_position(1) = _sub_vehicle_local_position.get().y;
_eph = _sub_vehicle_local_position.get().eph;
}

if (_sub_vehicle_local_position.get().z_valid) {
Expand Down
1 change: 1 addition & 0 deletions src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,7 @@ class FlightTask : public ModuleParams
/* Current vehicle state */
matrix::Vector3f _position; /**< current vehicle position */
matrix::Vector3f _velocity; /**< current vehicle velocity */
float _eph;

float _yaw{}; /**< current vehicle yaw heading */
float _unaided_yaw{};
Expand Down
33 changes: 22 additions & 11 deletions src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand 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;
Copy link

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Corrected spelling of 'yawspeed' to 'yaw_speed' for consistency with naming conventions.

Suggested change
_yawspeed_setpoint = 0.f;
_yaw_speed_setpoint = 0.f;

Copilot uses AI. Check for mistakes.
return ret;
}

float FlightTaskTransition::computeBackTranstionPitchSetpoint()
float FlightTaskTransition::computeBackTransitionTiltSetpoint()
{
const Vector2f position_xy{_position};
const Vector2f velocity_xy{_velocity};
Expand All @@ -109,8 +118,10 @@ 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 (_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));
Expand All @@ -129,13 +140,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 changes: 6 additions & 4 deletions src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,9 @@ class FlightTaskTransition : public FlightTask
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)};
Expand All @@ -71,8 +72,9 @@ class FlightTaskTransition : public FlightTask
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 changes: 19 additions & 13 deletions src/modules/vtol_att_control/standard.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,15 +180,15 @@ 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)
if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
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];
}

Expand All @@ -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();
Expand Down Expand Up @@ -239,28 +240,33 @@ 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));

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;
Expand Down
Loading