Simulator/DroneClient/utils/DroneOrientation.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
using TelemetryIO.Models;

namespace DroneClient.utils
{
    internal static class DroneOrientation
    {
        public static float AccX, AccY, AccZ;
        public static float GyrX, GyrY, GyrZ;
        public static uint TimeAcc, TimeGyr;
        private static bool isFirstCycle = true;
        public static uint prevTimeAcc, prevTimeGyr;
        private static Telemetry telemetry;

        public static float PosX, PosY;
        public static float LaserRange;
        public static uint TimeRange;
        public static uint prevTimeRange;

        public static Vector2 OF = Vector2.Zero;

        public static float MotorUL, MotorUR, MotorDL, MotorDR;

        public const float K_ACC = 0.1f;

        public static Quaternion attitude = new();

        /***************************************************************/
        private static Quaternion attitudeTarget = new();
        private static Vector ratesTarget = new();
        private static Vector torqueTarget = new();
        private static float thrustTarget = 0;
        /***************************************************************/
        private static AdaptivePID RollPid = new();
        private static AdaptivePID PitchPid = new();

        //private static AdaptivePID YawPid = new();

        private static AdaptivePID RatesRollPid = new(telemetry.pid0[0], telemetry.pid0[1], telemetry.pid0[2], 0.0f, 0, 0);
        private static AdaptivePID RatesPitchPid = new(telemetry.pid0[3], telemetry.pid0[4], telemetry.pid0[5], 0.01f, 0.05f, 0.001f);
        private static AdaptivePID RatesYawPid = new(telemetry.pid0[6], telemetry.pid0[7], telemetry.pid0[8], 0.01f, 0.05f, 0.001f);
        /***************************************************************/
        private static float tim1s = 0;


        /// <summary>
        /// Вычисление текущей ориентации дрона
        /// </summary>
        public static void CalcAttitude(Drone droneData, RC rc)
        {
            readData(droneData);
            
            prevTimeAcc = TimeAcc;
            prevTimeGyr = TimeGyr;


            prevTimeRange = TimeRange;
            while (true)
            {
                readData(droneData);
                if(prevTimeGyr == 0)
                {
                    prevTimeGyr = TimeGyr;
                    continue;
                }
                float gyro_dt = TimeGyr - prevTimeGyr;
                tim1s += gyro_dt;
                if ((prevTimeGyr > 0) && (gyro_dt > 0))
                {
                    gyro_dt /= 1000;
                    thrustTarget = rc.GetThrottle();
                    attitudeTarget = Quaternion.FromEulerAngles(rc.GetControl());
                    Vector rates = new Vector(-GyrY, -GyrX, GyrZ);
                    //Vector rates = new Vector(0, 0, 0);
                    rates *= Vector.TO_RAD;
                    //Quaternion q_rates = Quaternion.FromRotationVector(rates * (gyro_dt));
                    //attitude = Quaternion.Mul(attitude, q_rates);
                    attitude.UpdateByRates(rates, gyro_dt);
                    attitude.Normalize();
                    Vector errorPos = CalculateErrorAxisAngle();

                    if (tim1s > 1000)
                    {
                        //Debug.WriteLine($"Flix = {CalculateErrorFlix().ToString()}");
                        Debug.WriteLine($"Axis = {CalculateErrorAxisAngle().ToString()}");
                        Debug.WriteLine($"Current = {attitude.ToString()}");
                        Debug.WriteLine($"Target = {attitudeTarget.ToString()}");
                        tim1s = 0;
                    }
                    //ratesTarget.X = RollPid.process(errorPos.X, gyro_dt);
                    //ratesTarget.Y = PitchPid.process(errorPos.Y, gyro_dt);

                    //Vector errorRates = ratesTarget - rates;

                    torqueTarget.X = RatesRollPid.process(errorPos.X, gyro_dt);
                    torqueTarget.Y = RatesPitchPid.process(errorPos.Y, gyro_dt);
                    torqueTarget.Z = RatesYawPid.process(errorPos.Z, gyro_dt);
                    //torqueTarget.X = RatesRollPid.process(errorRates.X, gyro_dt);
                    //torqueTarget.Y = RatesPitchPid.process(errorRates.Y, gyro_dt);
                    //torqueTarget.Z = RatesYawPid.process(errorRates.Z, gyro_dt);

                    /*MotorUL = thrustTarget + torqueTarget.X - torqueTarget.Y + torqueTarget.Z;
                    MotorUR = thrustTarget - torqueTarget.X - torqueTarget.Y - torqueTarget.Z;
                    MotorDL = thrustTarget + torqueTarget.X + torqueTarget.Y + torqueTarget.Z;
                    MotorDR = thrustTarget - torqueTarget.X + torqueTarget.Y + torqueTarget.Z;*/
                    MotorUL = thrustTarget - torqueTarget.X - torqueTarget.Y + torqueTarget.Z;
                    MotorUR = thrustTarget + torqueTarget.X - torqueTarget.Y - torqueTarget.Z;
                    MotorDL = thrustTarget - torqueTarget.X + torqueTarget.Y - torqueTarget.Z;
                    MotorDR = thrustTarget + torqueTarget.X + torqueTarget.Y + torqueTarget.Z;

                    MotorUL = (float)Vector.constraint(MotorUL, 0, 1);
                    MotorUR = (float)Vector.constraint(MotorUR, 0, 1);
                    MotorDL = (float)Vector.constraint(MotorDL, 0, 1);
                    MotorDR = (float)Vector.constraint(MotorDR, 0, 1);

                    droneData.writeDataMotor4(MotorUL, MotorUR, MotorDL, MotorDR);

                    prevTimeGyr = TimeGyr;
                }

                
                
                /*
                if (TimeAcc - prevTimeAcc > 0)
                {
                    float AccNorm = (float)Math.Sqrt(AccX * AccX + AccY * AccY + AccZ * AccZ);
                    if (Math.Abs(AccNorm - 1) < 0.05)
                    {
                        //Получение кватерниона, описывающего вращение вектора Z(0, 0, 1) в положение вектора ускорений
                        Quaternion accRotation = Quaternion.QuaternionBetweenVectors(new Vector(AccX, AccY, AccZ), new Vector(0, 0, 1));

                        //Коррекция по акселерометру
                        //accRotation *= K_ACC;
                        attitude = accRotation;
                        attitude.Normalize();
                    }
                    prevTimeAcc = TimeAcc;
                }*/
            }
        }

        public static Quaternion GetAttitude()
        {
            return Quaternion.Copy(attitude);
        }

        private static void CalculateControls(float throttle, float phi, float theta, float psi)
        {
            thrustTarget = throttle;
            attitudeTarget = Quaternion.FromEulerAngles(new Vector(phi, theta, psi));
        }

        /// <summary>
        /// Вычисление ошибки как в flix
        /// </summary>
        private static Vector CalculateErrorFlix()
        {
            Vector up = new Vector(0, 0, 1);
            Vector current = Quaternion.RotateVector(up, attitude);
            Vector target = Quaternion.RotateVector(up, attitudeTarget);
            return Vector.getRotationVector(target, current);
        }

        /// <summary>
        /// Вычисление ошибки через форму ось-угол
        /// </summary>
        private static Vector CalculateErrorAxisAngle()
        {
            Quaternion error = Quaternion.Mul(attitudeTarget, attitude.getInverse());
            error.Normalize();
            float angle = 2 * (float)Math.Acos(error.W);
            Vector n = new Vector(error.X, error.Y, error.Z);
            n.Normalize();
            return n*angle;
        }

        private static void readData(Drone droneData)
        {
            float[]acc = droneData.readDataAcc();
            AccX = acc[0]; AccY = acc[1]; AccZ = acc[2]; TimeAcc = (uint)acc[3];

            float[] gyr = droneData.readDataGyr();
            GyrX = gyr[0]; GyrY = gyr[1]; GyrZ = gyr[2]; TimeGyr = (uint)gyr[3];

            float[] pos = droneData.readDataLocal();
            PosX = pos[0]; PosY = pos[1];

            float[] range = droneData.readDataRange();
            LaserRange = range[0]; TimeRange = (uint)range[1];
        }
    }
}