Пример #1
0
        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);
        }
Пример #2
0
        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;
        }