Custom character movement flight controller having issues with not returning upright

I’m trying to remake a flight controller in C++. originally I had it in Blueprint but it had too many bugs, so it’s being swapped to C++. However, although I’ve solved most of the issues with the Blueprint version, I’m getting errors with returning upright when flying and often end up on a permanent lean.

All my movement for flying is done through a custom movement mode and custom physics. I’ve attached images of the old code and pasted in the new code at the bottom.

I tried doing it without quaternions but couldn’t seem to get the rotation working like how I did it below so had to swap to them. In the original Blueprint version when I didn’t provide X/Y input it would slowly go back to flying straight, but now - especially if I hit a wall or bank too fast - it gets stuck on a permanent tilt and I can’t work it out.

I’ve got a video of the example of what normally triggers it (hpoefully it works, never put a video on here before) that one of my friends took while playtesting, and I think that sometimes if the player bank too fast it also happens.

Old Blueprint code (had no issues with permanent tilts):

C++ remake (it’s a bit long):

void UFlightMovementComponent::PhysicsFlight(float DeltaTime, int32 Iterations)
{
if (DeltaTime < MIN_TICK_TIME) { return; }
if ((!CharacterOwner) || (!UpdatedComponent)) { return; }

// Initialize control orientation
if (!bFlightQuatInitialized) {
FRotator MeshRot = UpdatedComponent->GetComponentRotation();

// Prevent startup banking contaminating control orientation
MeshRot.Roll = 0.f;

FlightControlQuat = MeshRot.Quaternion();
bFlightQuatInitialized = true;
}

// Smooth steering input
SmoothedTurnInput = FMath::FInterpTo(SmoothedTurnInput, FlightInput.X, DeltaTime, 3.0f);

const float TurnInput = SmoothedTurnInput;
const float PitchInput = FlightInput.Y;

// Flight orientation
// World stabilized yaw prevents roll accumulation
const FVector YawAxis = FVector::UpVector;

// Local pitch axis preserves loops / inversion
const FVector PitchAxis = FlightControlQuat.GetRightVector();

const float YawRadians = FMath::DegreesToRadians(TurnInput * TurnSpeed * DeltaTime);
const float PitchRadians = FMath::DegreesToRadians(PitchInput * PitchSpeed * DeltaTime);

// Apply yaw + pitch
const FQuat YawQuat(YawAxis, YawRadians);
const FQuat PitchQuat(PitchAxis, PitchRadians);

FlightControlQuat = YawQuat * PitchQuat * FlightControlQuat;
FlightControlQuat.Normalize();

// Visual banking only
TargetRoll = FMath::Clamp(-TurnInput * MaxBank, -MaxBank, MaxBank);
CurrentRoll = FMath::FInterpTo(CurrentRoll, TargetRoll, DeltaTime, 5.0f);

// Snap tiny residual banking back to level
if (FMath::Abs(CurrentRoll) < 0.05f) { CurrentRoll = 0.0f; }

// Apply cosmetic banking only
const FQuat LocalBankQuat(FVector::ForwardVector, FMath::DegreesToRadians(CurrentRoll));

// Final rendered orientation
const FQuat FinalQuat = FlightControlQuat * LocalBankQuat;

// Speed / velocity
// Physics movement ignores cosmetic banking
const FVector FlightForward = FlightControlQuat.GetForwardVector();
const float NormalizedPitch = FMath::RadiansToDegrees(FMath::Asin(FMath::Clamp(FlightForward.Z, -1.0f, 1.0f)));

// Diving acceleration
if ((bCanDive) && (NormalizedPitch < -45.f)) {
const float DiveAlpha = FMath::GetMappedRangeValueClamped(FVector2D(-45.f, -90.f), FVector2D(0.f, 1.f), NormalizedPitch);
CurrentFlightSpeed += DiveAlpha * DiveAcceleration * DeltaTime;
}
// Climbing slowdown
else if (NormalizedPitch > 45.f) {
const float ClimbAlpha = FMath::GetMappedRangeValueClamped(FVector2D(45.f, 90.f), FVector2D(0.f, 1.f), NormalizedPitch);
CurrentFlightSpeed -= ClimbAlpha * ClimbDeceleration * DeltaTime;
}
// Cruise recovery
else { CurrentFlightSpeed = FMath::FInterpTo(CurrentFlightSpeed, FlightSpeed, DeltaTime, 0.5f); }

CurrentFlightSpeed = FMath::Clamp(CurrentFlightSpeed, MinFlightSpeed, MaxFlightSpeed);

// Transition to hover if too slow
if (CurrentFlightSpeed <= MinFlightSpeed + 50.f) {
bFlightQuatInitialized = false;
SetFlightMode(EFlightCustomMode::HOVER);
PhysicsHover(DeltaTime, Iterations);
return;
}

// Apply velocity
const FVector DesiredVelocity = FlightForward * CurrentFlightSpeed;

Velocity = FMath::VInterpTo(Velocity, DesiredVelocity, DeltaTime, 2.0f);

// Apply gravity
Velocity.Z -= FlyGravity * 980.f * DeltaTime;

// Move
FHitResult Hit;

SafeMoveUpdatedComponent(Velocity * DeltaTime, FinalQuat, true, Hit);

if (Hit.IsValidBlockingHit()) {
HandleFlightCollision(Hit);
SlideAlongSurface(Velocity * DeltaTime, 1.f - Hit.Time, Hit.Normal, Hit, true);
}

}

If you need anything else please ask.