/// <summary> /// Vertical Stabilization algorithm /// </summary> /// <param name="targetAltitude">Altitude that we want to reach. It'll be compared with the actual to extract the error</param> void yStabilization(float targetAltitude) { //calculates the error and extracts the measurements from the sensors float distanceToPoint = (targetAltitude - bar.getHeight()); // adding the value to the test class //tHeight.addValue(distanceToPoint); float acc = bar.getverticalAcc(); float vel = bar.getverticalSpeed(); //calculates the idealVelocity, we'll use this to extract an error that will be given to the PID float idealVel = distanceToPoint * (testing ? constVertVel : droneSettings.constVerticalIdealVelocity); idealVel = droneSettings.keepOnRange(idealVel, droneSettings.saturationValues.minVerticalVel, droneSettings.saturationValues.maxVerticalVel); //calculates the idealAcc, we'll use this to extract an error that will be given to the PID float idealAcc = (idealVel - vel) * (testing ? constVertAcc : droneSettings.constVerticalIdealAcceler); idealAcc = droneSettings.keepOnRange(idealAcc, droneSettings.saturationValues.minVerticalAcc, droneSettings.saturationValues.maxVerticalAcc); //Error used by the PID float Err = (idealAcc - acc); //If this is TRUE we are near the point and with a low velocity. It is not necessary to modify the Power if (Mathf.Abs(vel) + Mathf.Abs(distanceToPoint) > 0.005f) { //modifying the rotors rotation, using the output of the PID modifyAllRotorsRotation(yPID.getU(Err, Time.deltaTime)); } }