Vector, Quaternion - математика для ориентации. AdaptivePID - ПИД-регулятор. DroneOrientation.CalcAttitude() - цикл вычисления положения.

DroneClient/utils/AdaptivePID.cs

@@ -0,0 +1,88 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace DroneClient.utils
+{
+    internal class AdaptivePID
+    {
+        //Initial
+        private float P = 1;
+        private float I = 0.2f;
+        private float D = 0.005f;
+
+        //Adaptive
+        private float aP = 1;
+        private float aI = 0.2f;
+        private float aD = 0.005f;
+
+        private float P_min = 0; private float P_max = 10;
+        private float I_min = 0; private float I_max = 1;
+        private float D_min = 0; private float D_max = 0.1f;
+
+        private float alpha = 0.001f;
+        private float beta = 0.001f;
+        private float gamma = 0.0005f;
+
+        private float prev_error = float.NaN;
+        private float integral = 0;
+
+        public AdaptivePID()
+        {
+            alpha = 0; beta = 0; gamma = 0;
+        }
+
+        public AdaptivePID(float p, float i, float d, float alpha = 0.001f, float beta = 0.0005f, float gamma = 0.00005f)
+        {
+            P = p; aP = p;
+            I = i; aI = i;
+            D = d; aD = d;
+            this.alpha = alpha;
+            this.beta = beta;
+            this.gamma = gamma;
+        }
+
+        public float process(float error, float T)
+        {
+            if (T <= 0) return 0;
+            integral += error * T;
+            float derivative = 0;
+            if (float.IsFinite(prev_error))
+            {
+                derivative = (error - prev_error) / T;
+            }
+            prev_error = error;
+
+            float dP = -alpha * error * error;
+            float dI = -beta * error * integral;
+            float dD = -gamma * error * derivative;
+
+            aP += dP; aI += dI; aD += dD;
+
+            aP = constrain(aP, P_min, P_max);
+            aI = constrain(aI, I_min, I_max);
+            aD = constrain(aD, D_min, D_max);
+            integral = constrain(integral, -10, 10);
+            float output = aP * error + aI * integral + aD * derivative;
+            
+            return output;
+        }
+
+        public void reset()
+        {
+            integral = 0;
+            prev_error = 0;
+            aP = P; aI = I; aD = D;
+        }
+
+        static float constrain(float val, float min, float max)
+        {
+            if (min >= max) return val;
+            if(val < min) return min;
+            if(val > max) return max;
+            return val;
+        }
+    }
+}