diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index 8a366febca96..60227e53d527 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -203,6 +203,8 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() // Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped deceleration_sp = vel_body_forward * vel_body_forward / (2.f * dist_body_forward); + PX4_INFO("Computed deceleration_sp: %f", (double)(deceleration_sp)); + // Check if the deceleration setpoint is finite and within limits if (!PX4_ISFINITE(deceleration_sp)) { deceleration_sp = default_deceleration_sp; @@ -215,10 +217,13 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint() } } + PX4_INFO("Applied deceleration_sp: %f", (double)(deceleration_sp)); + // get accel error, positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number) float deceleration_error = deceleration_sp - (-accel_body_forward); float pitch_sp_new = _decel_error_bt_int; + PX4_INFO("Pitch setpoint: %f", (double)(math::degrees(pitch_sp_new))); updateBackTransitioDecelerationErrorIntegrator(deceleration_error); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 0fb6ed9c9e21..98f6c4be6690 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -239,7 +239,12 @@ void Standard::update_transition_state() } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { - const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); + if (_v_control_mode->flag_control_climb_rate_enabled) { + // control backtransition deceleration using pitch. + _v_att_sp->pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta(); + } + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f;