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

DroneClient/utils/Quaternion.cs

@@ -0,0 +1,178 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace DroneClient.utils
+{
+    internal class Quaternion
+    {
+        private float w;
+        private float x;
+        private float y;
+        private float z;
+
+        public float W
+        {
+            get => w;
+            private set => w = value;
+        }
+        public float X
+        {
+            get => x;
+            private set => x = value;
+        }
+        public float Y
+        {
+            get => y;
+            private set => y = value;
+        }
+        public float Z
+        {
+            get => z;
+            private set => z = value;
+        }
+
+        public Quaternion()
+        {
+            w = 1;
+            x = y = z = 0;
+        }
+
+        public Quaternion(float w, float x, float y, float z)
+        {
+            this.w = w;
+            this.x = x;
+            this.y = y;
+            this.z = z;
+        }
+
+        public bool IsZero()
+        {
+            return w == 0 && x == 0 && y == 0 && z == 0;
+        }
+
+        public float getNorm()
+        {
+            if (IsZero()) return 0;
+            return (float)Math.Sqrt(w * w + x * x + y * y + z * z);
+        }
+
+        public void Normalize()
+        {
+            float norm = getNorm();
+            if (norm != 0)
+            {
+                w /= norm;
+                x /= norm;
+                y /= norm;
+                z /= norm;
+            }
+        }
+
+        static public Quaternion FromRotationVector(Vector Va)
+        {
+            float a = Va.getNorm();
+            if (a == 0)
+            {
+                return new Quaternion();
+            }
+
+            Quaternion w = new Quaternion();
+            float cos = (float)Math.Cos(a / 2);
+            float sin = (float)Math.Sin(a / 2);
+
+            w.w = cos;
+            w.x = Va.X / a * sin;
+            w.y = Va.Y / a * sin;
+            w.z = Va.Z / a * sin;
+            return w;
+        }
+
+        static public Quaternion FromEulerAngles(Vector euler)
+        {
+            float halfphi = euler.X / 2;
+            float halftheta = euler.Y / 2;
+            float halfpsi= euler.Z / 2;
+            float cosx = (float)Math.Cos(halfphi);
+            float sinx = (float)Math.Sin(halfphi);
+            float cosy = (float)Math.Cos(halftheta);
+            float siny = (float)Math.Sin(halftheta);
+            float cosz = (float)Math.Cos(halfpsi);
+            float sinz = (float)Math.Sin(halfpsi);
+
+            return new Quaternion(
+                cosx * cosy * cosz + sinx * siny * sinz,
+                sinx * cosy * cosz - cosx * siny * sinz,
+                cosx * siny * cosz + sinx * cosy * sinz,
+                cosx * cosy * sinz - sinx * siny * cosz);
+        }
+
+        static public Quaternion Copy(Quaternion q)
+        {
+            return new Quaternion(q.w, q.x, q.y, q.z);
+        }
+
+        public Quaternion getInverse()
+        {
+            float n = getNorm();
+            return new Quaternion(w / n, -x / n, -y / n, -z / n);
+        }
+
+        static public Quaternion Mul(Quaternion q1, Quaternion q2)
+        {
+            Quaternion res = new Quaternion();
+            res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
+            res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
+            res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
+            res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
+            return res;
+        }
+
+        public void UpdateByRates(Vector w, float dt)
+        {
+            Quaternion omega = new Quaternion(0, w.X, w.Y, w.Z);
+            Quaternion res = Mul(this, omega);
+            this.w += res.w * 0.5f * dt;
+            this.x += res.x * 0.5f * dt;
+            this.y += res.y * 0.5f * dt;
+            this.z += res.z * 0.5f * dt;
+        }
+
+        static public Vector RotateVector(Vector a, Quaternion q)
+        {
+            Quaternion fromVector = new Quaternion(0, a.X, a.Y, a.Z);
+            Quaternion inverse = q.getInverse();
+            Quaternion r = Mul(q, fromVector);
+            r = Mul(r, inverse);
+            return new Vector(r.x, r.y, r.z);
+        }
+        static public Quaternion QuaternionBetweenVectors(Vector a, Vector b)
+        {
+            Quaternion res = new Quaternion();
+            Vector mul_v = Vector.Mul_Vector(a, b);
+            //Если векторы коллинеарны, то нет поворота
+            if (mul_v.IsZero()) return res;
+
+            float mul_s = Vector.Mul_Scalar(a, b);
+            //Хотя бы один из векторов нулевой => кватернион неопределен, но мы возвращаем (1, 0, 0, 0)
+            if (mul_s == 0) return res;
+
+            float norm_scalar = a.getNorm() * b.getNorm() + mul_s;
+            float normQ = (float)Math.Sqrt(norm_scalar * norm_scalar + mul_v.getNorm() * mul_v.getNorm());
+
+            res.w = norm_scalar / normQ;
+            res.x = mul_v.X / normQ;
+            res.y = mul_v.Y / normQ;
+            res.z = mul_v.Z / normQ;
+            return res;
+        }
+
+        override public string ToString()
+        {
+            return $"({w}, {x}, {y}, {z})";
+        }
+    }
+}