diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 23fde837b7..8b67532c80 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -861,7 +861,7 @@ float pidHeadingHold(float dT) * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more * and keeping ROLL and PITCH attitude though the turn. */ -static void NOINLINE pidTurnAssistant(pidState_t *pidState) +static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -889,8 +889,9 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; + bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); + float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -1003,8 +1004,10 @@ void FAST_CODE pidController(float dT) levelingEnabled = false; } - if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { + float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); + float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH])); + pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT }