/// <summary>
/// Yaw Stabilization algorithm
/// </summary>
/// <param name="idealYaw">Yaw value that we want to reach. It'll be compared with the actual to extract the error</param>
/// <returns>The absolute value of the error, used to decrease the effect of the others stabilization algorithms</returns>
float yawStabilization(float idealYaw)
{
//calculates the error and extracts the measurements from the sensors
float yawDistance = mag.getYaw() - idealYaw;
yawDistance = (Mathf.Abs(yawDistance) < 1 ? yawDistance : (yawDistance > 0 ? yawDistance - 2 : yawDistance + 2));
//calculates idealVelocity, we'll use this to extract an error that will be given to the PID
float vel = mag.getYawVel();
float idealVel = -yawDistance * (testing ? constYawVel : droneSettings.constYawIdealVelocity);
//Error used by the PID
float Err = (idealVel - vel);
Err *= Mathf.Abs(yawDistance) * (Mathf.Abs(yawDistance) > 0.3f ? -10 : -50);
//modifying the rotors rotation, using the output of the PID
float res = yawPID.getU(Err, Time.deltaTime);
modifyPairsRotorsRotation(res);
return(Math.Abs(idealYaw - mag.getYaw()));
}