Vector, Quaternion - математика для ориентации. AdaptivePID - ПИД-регулятор. DroneOrientation.CalcAttitude() - цикл вычисления положения.
This commit is contained in:
198
DroneClient/utils/DroneOrientation.cs
Normal file
198
DroneClient/utils/DroneOrientation.cs
Normal file
@ -0,0 +1,198 @@
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user