Определён базовый функционал IRS

Source/INS/IRS.c

@@ -0,0 +1 @@
+#include "IRS.h"

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

Source/INS/geometry/quaternion.c

@@ -1,4 +1,26 @@
 #include "quaternion.h"
+#include <math.h>
+
+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;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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);