/// <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())); }