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