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