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;
}
public void updateRollPitchYawValue(double roll, double pitch, double yaw)
{
this.roll.Value = roll;
this.pitch.Value = pitch;
this.yaw.Value = yaw;
}
public void updateQuaternionValue(Utils.Quaternion Value)
{
this.Value = Value;
}
public void updateRollPitchYawSetPoint(double rollSetPoint, double pitchSetPoint, double yawSetPoint)
{
this.roll.SetPoint = rollSetPoint;
this.pitch.SetPoint = pitchSetPoint;
this.yaw.SetPoint = yawSetPoint;
}
public void updateQuaternionSetPoint(Utils.Quaternion SetPoint)
{
this.SetPoint = SetPoint;
}
