From a09f0261955dc0c41260f2cab95ad4f575c17020 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Tue, 10 Mar 2026 21:16:02 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=BF=D1=80=D0=B5=D0=B4=D0=B5=D0=BB?= =?UTF-8?q?=D1=91=D0=BD=20=D0=B1=D0=B0=D0=B7=D0=BE=D0=B2=D1=8B=D0=B9=20?= =?UTF-8?q?=D1=84=D1=83=D0=BD=D0=BA=D1=86=D0=B8=D0=BE=D0=BD=D0=B0=D0=BB=20?= =?UTF-8?q?IRS?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/INS/IRS.c | 1 + Source/INS/IRS.h | 38 ++++++++++++++ Source/INS/geometry/quaternion.c | 85 ++++++++++++++++++++++++++++++++ Source/INS/geometry/quaternion.h | 2 +- drone.ewp | 6 +++ drone.ewt | 6 +++ 6 files changed, 137 insertions(+), 1 deletion(-) create mode 100644 Source/INS/IRS.c create mode 100644 Source/INS/IRS.h diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c new file mode 100644 index 0000000..c449f88 --- /dev/null +++ b/Source/INS/IRS.c @@ -0,0 +1 @@ +#include "IRS.h" diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h new file mode 100644 index 0000000..1645a77 --- /dev/null +++ b/Source/INS/IRS.h @@ -0,0 +1,38 @@ +#pragma once + +#ifndef IRS_H +#define IRS_H + +#include "quaternion.h" +#include "vector.h" + +Quaternion orientationQuat; +Vector3 tilts; // local oriented sinX sinY cosZ + +typedef struct +{ + Vector3 Gyro, Accel; +} IMU; // последние значения датчиков + +typedef struct +{ + Vector3 Accel, Spd, Pos; +} Inertial; // инерциальные значения движения + +void updateGyro(Vector3* gyro, const Vector3* newGyro); +void updateAccel(Vector3* accel, const Vector3* newAccel); + +void setShiftAlpha(const Vector3* pos, const Vector3* spd, const float* qua); // коэффициент восстановления позиции и скорости в секунду + +void updatePosSpeed(); +void updateQuat(); + +void restoreAllShift(); + +void setAccelShift(); + +void getSinXYCosZ(); +void getRollPitchYaw(); +void getInertial(); + +#endif \ No newline at end of file diff --git a/Source/INS/geometry/quaternion.c b/Source/INS/geometry/quaternion.c index c74570c..bbca733 100644 --- a/Source/INS/geometry/quaternion.c +++ b/Source/INS/geometry/quaternion.c @@ -1,4 +1,26 @@ #include "quaternion.h" +#include + +Quaternion QuatNormalize(const Quaternion* q, const float gain) +{ + Quaternion res; + + float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w); + + if (norm > 1e-6f) + { + norm = gain / norm; + + res.x = q->x * norm; + res.y = q->y * norm; + res.z = q->z * norm; + res.w = q->w * norm; + + return res; + } + + return res; +} Quaternion QuatConjugate(const Quaternion* q) { @@ -62,3 +84,66 @@ Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2) return res; } + +Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW) +{ + Quaternion v = {vec->x, vec->y, 0, 0}; + Quaternion h = {0, 0, CCW ? q->z : -q->z, q->w}; + h = QuatNormalize(&h, 1.0f); + + Quaternion vhprod = QuatProd(&v, &h); + h = QuatProd(&vhprod, &h); + + Vector3 res = {h.x, h.y, vec->z}; + + return res; +} + +Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad) +{ + Quaternion res; + + float h_r = 0.5f * RollPitchYawRad->y; + float h_p = 0.5f * RollPitchYawRad->x; + float h_y = 0.5f * RollPitchYawRad->z; + + float c_r = cosf(h_r), s_r = sinf(h_r); + float c_p = cosf(h_p), s_p = sinf(h_p); + float c_y = cosf(h_y), s_y = sinf(h_y); + + res.x = c_r * s_p * c_y - s_r * c_p * s_y; // Был +, стал - + res.y = s_r * c_p * c_y + c_r * s_p * s_y; // Был -, стал + + res.z = -c_r * c_p * s_y - s_r * s_p * c_y; // Первое слагаемое стало отрицательным + res.w = c_r * c_p * c_y - s_r * s_p * s_y; // Был +, стал - + + return res; +} + +Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay) +{ + Quaternion error = { + current->w * target->x + current->x * target->w + current->y * target->z - current->z * target->y, + current->w * target->y + current->x * target->z + current->y * target->w - current->z * target->x, + current->w * target->z + current->x * target->y + current->y * target->x - current->z * target->w, + current->w * target->w + current->x * target->x + current->y * target->y - current->z * target->z + }; + + if (fastWay && error.w < 0.0f) return QuatNegate(&error); + + return error; +} + + + + + + + + + + + + + + + diff --git a/Source/INS/geometry/quaternion.h b/Source/INS/geometry/quaternion.h index 2ee2739..e32d96d 100644 --- a/Source/INS/geometry/quaternion.h +++ b/Source/INS/geometry/quaternion.h @@ -21,7 +21,7 @@ Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2); Quaternion QuatConstProd(const Quaternion* q, const float value); Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2); -Vector3 QuatRotateAroundZ(const Vector3* vec, bool CCW); +Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW); Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad); Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay); diff --git a/drone.ewp b/drone.ewp index 89ad8f4..7a3938d 100644 --- a/drone.ewp +++ b/drone.ewp @@ -2385,6 +2385,12 @@ $PROJ_DIR$\Source\INS\geometry\vector.h + + $PROJ_DIR$\Source\INS\IRS.c + + + $PROJ_DIR$\Source\INS\IRS.h + $PROJ_DIR$\Source\main.c diff --git a/drone.ewt b/drone.ewt index 5e98ea8..eaa093e 100644 --- a/drone.ewt +++ b/drone.ewt @@ -3569,6 +3569,12 @@ $PROJ_DIR$\Source\INS\geometry\vector.h + + $PROJ_DIR$\Source\INS\IRS.c + + + $PROJ_DIR$\Source\INS\IRS.h + $PROJ_DIR$\Source\main.c