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