public void ControlStep(double Dt)
{
if (AltitudControl)
{
altitud.ControlStep(Dt);
pitchSetPoint = altitud.u;// *Math.PI / 2;
speed.ControlStep(Dt);
}
if (HeadingControl)
{
pidYawQuaternion.updateQuaternionValue(mathTools.Euler2Quaternion(0, 0, yaw));
pidYawQuaternion.updateRollPitchYawValue(0, 0, yaw);
pidYawQuaternion.updateQuaternionSetPoint(mathTools.Euler2Quaternion(0, 0, yawSetPoint));
pidYawQuaternion.updateRollPitchYawSetPoint(0, 0, yawSetPoint);
pidYawQuaternion.ControlStep(Dt);
//rollSetPoint = pidYawQuaternion.YawControl;
speed.ControlStep(Dt);
}
pidQuaternion.updateQuaternionValue(mathTools.Euler2Quaternion(roll, pitch, 0));
pidQuaternion.updateRollPitchYawValue(roll, pitch, 0);
pidQuaternion.updateQuaternionSetPoint(mathTools.Euler2Quaternion(rollSetPoint, pitchSetPoint, 0));
pidQuaternion.updateRollPitchYawSetPoint(rollSetPoint, pitchSetPoint, 0);
pidQuaternion.ControlStep(Dt);
}
public void ControlStep(double Dt)
{
Utils.Quaternion error = SetPoint.Multiply(Value.Conjugate());
double[] euler = mathTools.Quaternion2Euler(error);
roll.Error = euler[2];
pitch.Error = euler[1];
yaw.Error = euler[0];
roll.Derivative = pqr[0];
pitch.Derivative = pqr[1];
yaw.Derivative = pqr[2];
roll.ControlStep(Dt);
pitch.ControlStep(Dt);
yaw.ControlStep(Dt);
RollControl = roll.u;
PitchControl = pitch.u;
YawControl = yaw.u;
}