diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 638f4c6dcaf..304421f0abf 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -206,13 +206,16 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { - // transition to MC mode if transition time has passed - // XXX: base this on XY hold velocity of MC - float vel = sqrtf(_local_pos->vx * _local_pos->vx + _local_pos->vy * _local_pos->vy); + // transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed + + const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed()); + const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); + + float x_vel = vel(0); if (hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.back_trans_dur * 1000000.0f) || - (_local_pos->v_xy_valid && vel <= _params_standard.mpc_xy_cruise)) { + (_local_pos->v_xy_valid && x_vel <= _params_standard.mpc_xy_cruise)) { _vtol_schedule.flight_mode = MC_MODE; }