public void Init()
{
roll.Init();
roll.useExternDerivative = true;
roll.useExternError = true;
pitch.Init();
pitch.useExternDerivative = true;
pitch.useExternError = true;
yaw.Init();
yaw.useExternDerivative = true;
yaw.useExternError = true;
roll.HighSatValue = 0.5;
roll.LowSatValue = -0.5;
pitch.HighSatValue = 0.5;
pitch.LowSatValue = -0.5;
yaw.HighSatValue = 0.5;
yaw.LowSatValue = -0.5;
}
public void startAltitudControl()
{
//altitud.SetPoint = 0;
altitud.Init();
}