Определены и реализованы основные функции кватернионов

Source/BSP/Inc/lidar.h

@@ -6,6 +6,7 @@
 #include "stm32g431xx.h"
 
 #define USART3_START_BYTE	0x59
+#define USART3_BUF_SIZE		64
 #define USART3_FRAME_SIZE	9
 #define LIDAR_MIN_DIST		0.01f
 #define LIDAR_MAX_DIST		40.0f
@@ -26,7 +27,7 @@ typedef struct
 
 typedef struct
 {
-	uint16_t distance;		// meters
+	uint16_t distance;		// cm
 	uint16_t strength;
 	uint16_t temperature;
 } lidar_data;
@@ -37,6 +38,9 @@ void TIM7_DAC_IRQHandler();
 void USART3_IRQHandler();
 void lidar_update(lidar_data* lidar);
 
+uint8_t usart_available();
+uint8_t usart_read();
+
 void lidar_i2c2_init();
 static void i2c2_wait_txis();
 static void i2c2_wait_stop();

Source/BSP/Src/lidar.c

@@ -1,11 +1,8 @@
 #include "lidar.h"
 
-volatile uint8_t usart3_index = 0;
+volatile uint8_t usart3_rx_buf[USART3_BUF_SIZE];
-volatile uint8_t usart3_checksum = 0;
+static uint8_t usart3_rx_head = 0;
-volatile uint8_t usart3_frame_ready = 0;
+static uint8_t usart3_rx_tail = 0;
-volatile uint8_t lidar_update_flag = 0;
-static lidar_data_buf buffer;
-static uint8_t* buff_data = (uint8_t*)&buffer;
 
 void lidar_init()
 {
@@ -78,7 +75,7 @@ void TIM7_DAC_IRQHandler()
 	if (TIM7->SR & TIM_SR_UIF)
 	{
 		TIM7->SR &= ~TIM_SR_UIF;
-		lidar_update_flag = 1;
+		//lidar_update_flag = 1;
 	}
 }
 	
@@ -86,42 +83,49 @@ void USART3_IRQHandler()
 {
 	if (USART3->ISR & USART_ISR_RXNE)
 	{
-		uint8_t b = USART3->RDR;
+		usart3_rx_buf[usart3_rx_head] = USART3->RDR;
+		usart3_rx_head = (usart3_rx_head + 1) % USART3_BUF_SIZE;
+	}
+}
 
-		if (usart3_index < 2)
+uint8_t usart_available()
-		{
+{
-			if (b == USART3_START_BYTE)
+	return usart3_rx_head != usart3_rx_tail;
-				buff_data[usart3_index++] = b;
+}
-		}
-		else if (usart3_index < USART3_FRAME_SIZE)
-			buff_data[usart3_index++] = b;
 
-		if (usart3_index == USART3_FRAME_SIZE)
+uint8_t usart_read()
-		{
+{
-			usart3_index = 0;
+	uint8_t data = usart3_rx_buf[usart3_rx_tail];
-			usart3_frame_ready = 1;
+	usart3_rx_tail = (usart3_rx_tail + 1) % USART3_BUF_SIZE;
-		}
+	return data;
-	}
 }
 
 void lidar_update(lidar_data* lidar)
 {
-	if (!lidar_update_flag)
+	static uint8_t frame[USART3_FRAME_SIZE];
-		return;
+	static uint8_t index = 0;
 	
-	if (!usart3_frame_ready)
+	while(usart_available())
-		return;
+	{
+		uint8_t c = usart_read();
 		
-	usart3_frame_ready = 0;
+		frame[index++] = c;
 		
-	for (uint8_t i = 0; i < USART3_FRAME_SIZE; ++i) usart3_checksum += buff_data[i];
+		if (index == USART3_FRAME_SIZE)
+		{
+			uint8_t checksum = 0;
+			for (uint8_t i = 0; i < USART3_FRAME_SIZE - 1; ++i) checksum += frame[i];
 			
-	if (buffer.checksum != usart3_checksum)
+			if (checksum == frame[USART3_FRAME_SIZE - 1])
-	  return;
+			{
+				lidar->distance 	= frame[2] | (frame[3] << 8);
+                lidar->strength 	= frame[4] | (frame[5] << 8);
+                lidar->temperature 	= frame[6] | (frame[7] << 8);
+			}
 			
-	lidar->distance		= 	buffer.distance_l 	| (buffer.distance_h << 8);
+			index = 0;
-	lidar->strength		= 	buffer.strength_l 	| (buffer.strength_h << 8);
+		}
-	lidar->temperature	= 	buffer.temp_l 		| (buffer.temp_h << 8);
+	}
 }
 
 void lidar_i2c2_init()

Source/INS/geometry/quaternion.c

@@ -0,0 +1,64 @@
+#include "quaternion.h"
+
+Quaternion QuatConjugate(const Quaternion* q)
+{
+  Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = q->w};
+  return res;
+}
+
+Quaternion QuatInvert(const Quaternion* q)
+{
+	Quaternion res;
+	float nsq = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
+	
+	if (nsq > 1e-6f)
+	{
+		nsq = 1.0f / nsq;
+		
+		res.x = q->x * nsq;
+		res.y = q->y * nsq;
+		res.z = q->z * nsq;
+		res.w = q->w * nsq;
+		
+		return res;
+	}
+	
+	return *q;
+}
+
+Quaternion QuatNegate(const Quaternion* q)
+{
+	Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = -q->w};
+	return res;
+}
+
+Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2)
+{
+	Quaternion res = {.x = q1->x + q2->x, .y = q1->y + q2->y, .z = q1->z + q2->z, .w = q1->w + q2->w};
+	return res;
+}
+
+Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
+{
+	Quaternion res = {.x = q1->x - q2->x, .y = q1->y - q2->y, .z = q1->z - q2->z, .w = q1->w - q2->w};
+	return res;
+}
+
+Quaternion QuatConstProd(const Quaternion* q, const float value)
+{
+	Quaternion res = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value};
+	return res;
+}
+
+Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
+{
+	Quaternion res = {
+	.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
+	.y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
+	.z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
+	.w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
+	};
+	
+	
+	return res;
+}

Source/INS/geometry/quaternion.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include "vector.h"
+#include <stdbool.h>
+
+typedef struct
+{
+	float x, y, z, w;
+} Quaternion;
+
+Quaternion QuatNormalize(const Quaternion* q, const float gain);
+Quaternion QuatConjugate(const Quaternion* q);
+Quaternion QuatInvert(const Quaternion* q);
+Quaternion QuatNegate(const Quaternion* q);
+
+Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2);
+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);
+Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad);
+Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
+
+
+#endif
+

Source/INS/geometry/vector.c

@@ -0,0 +1,138 @@
+#include "vector.h"
+#include <math.h>
+
+Vector2 normalizeV2(const Vector2* v, float gain)
+{
+	float len = lengthV2(v);
+	Vector2 res = {.x = v->x / len, .y = v->y / len};
+	return res;
+}
+
+Vector3 normalizeV3(const Vector3* v, float gain)
+{
+	float len = lengthV3(v);
+	Vector3 res = {.x = v->x / len, .y = v->y / len, .z = v->z};
+	return res;
+}
+
+Vector2 absV2(const Vector2* v)
+{
+	Vector2 res = {.x = fabsf(v->x), .y = fabsf(v->y)};
+	return res;
+}
+
+Vector3 absV3(const Vector3* v)
+{
+	Vector3 res = {.x = fabsf(v->x), .y = fabsf(v->y), .z = fabsf(v->z)};
+	return res;
+}
+
+float lengthV2(const Vector2* v)
+{
+	return sqrtf(v->x * v->x + v->y * v->y);
+}
+
+float lengthV3(const Vector3* v)
+{
+	return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
+}
+
+float lengthSquaredV2(const Vector2* v)
+{
+	return v->x * v->x + v->y * v->y;
+}
+
+float lengthSquaredV3(const Vector3* v)
+{
+	return v->x * v->x + v->y * v->y + v->z * v->z;
+}
+
+Vector2 limitV2(const Vector2* v, float min, float max)
+{
+	Vector2 lim;
+	
+	if (v->x < min) lim.x = min; else if (v->x > max) lim.x = max; else lim.x = v->x;
+	if (v->y < min) lim.y = min; else if (v->y > max) lim.y = max; else lim.y = v->y;
+	
+	return lim;
+}
+
+Vector3 limitV3(const Vector3* v, float min, float max)
+{
+	Vector3 lim;
+	
+	if (v->x < min) lim.x = min; else if (v->x > max) lim.x = max; else lim.x = v->x;
+	if (v->y < min) lim.y = min; else if (v->y > max) lim.y = max; else lim.y = v->y;
+	if (v->z < min) lim.z = min; else if (v->z > max) lim.z = max; else lim.z = v->z;
+	
+	return lim;
+}
+
+Vector2 powerV2(const Vector2* v, float pow)
+{
+	Vector2 res = {.x = powf(v->x, pow), .y = powf(v->y, pow)};
+	return res;
+}
+
+Vector3 powerV3(const Vector3* v, float pow)
+{
+	Vector3 res = {.x = powf(v->x, pow), .y = powf(v->y, pow), .z = powf(v->z, pow)};
+	return res;
+}
+
+Vector2 sumV2(const Vector2* v1, const Vector2* v2)
+{
+	Vector2 res = {.x = v1->x + v2->x, .y = v1->y + v2->y};
+	return res;
+}
+
+Vector3 sumV3(const Vector3* v1, const Vector3* v2)
+{
+	Vector3 res = {.x = v1->x + v2->x, .y = v1->y + v2->y, .z = v1->z + v2->z};
+	return res;
+}
+
+Vector2 diffV2(const Vector2* v1, const Vector2* v2)
+{
+	Vector2 res = {.x = v1->x - v2->x, .y = v1->y - v2->y};
+	return res;
+}
+
+Vector3 diffV3(const Vector3* v1, const Vector3* v2)
+{
+	Vector3 res = {.x = v1->x - v2->x, .y = v1->y - v2->y, .z = v1->z - v2->z};
+	return res;
+}
+
+Vector2 constProdV2(const Vector2* v, float value)
+{
+	Vector2 res = {.x = v->x * value, .y = v->y * value};
+	return res;
+}
+
+Vector3 constProdV3(const Vector3* v, float value)
+{
+	Vector3 res = {.x = v->x * value, .y = v->y * value, .z = v->z * value};
+	return res;
+}
+
+float scalarProdV2(const Vector2* v1, const Vector2* v2)
+{
+	float res = v1->x * v2->x + v1->y * v2->y;
+	return res;
+}
+
+float scalarProdV3(const Vector3* v1, const Vector3* v2)
+{
+	float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
+	return res;
+}
+
+
+
+
+
+
+
+
+

Source/INS/geometry/vector.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#ifndef VECTOR_H
+#define VECTOR_H
+
+
+typedef struct
+{
+	float x, y;
+} Vector2;
+
+typedef struct
+{
+	float x, y, z;
+} Vector3;
+
+Vector2 normalizeV2(const Vector2* v, float gain);
+Vector3 normalizeV3(const Vector3* v, float gain);
+
+Vector2 absV2(const Vector2* v);
+Vector3 absV3(const Vector3* v);
+
+float lengthV2(const Vector2* v);
+float lengthV3(const Vector3* v);
+
+float lengthSquaredV2(const Vector2* v);
+float lengthSquaredV3(const Vector3* v);
+
+Vector2 limitV2(const Vector2* v, float min, float max);
+Vector3 limitV3(const Vector3* v, float min, float max);
+
+Vector2 powerV2(const Vector2* v, float pow);
+Vector3 powerV3(const Vector3* v, float pow);
+
+Vector2 sumV2(const Vector2* v1, const Vector2* v2);
+Vector3 sumV3(const Vector3* v1, const Vector3* v2);
+
+Vector2 diffV2(const Vector2* v1, const Vector2* v2);
+Vector3 diffV3(const Vector3* v1, const Vector3* v2);
+
+Vector2 constProdV2(const Vector2* v, float value);
+Vector3 constProdV3(const Vector3* v, float value);
+
+float scalarProdV2(const Vector2* v1, const Vector2* v2);
+float scalarProdV3(const Vector3* v1, const Vector3* v2);
+
+Vector2 vectorProdV2(const Vector2* v1, const Vector2* v2);
+Vector3 vectorProdV3(const Vector3* v1, const Vector3* v2);
+
+#endif

Source/main.c

@@ -19,7 +19,7 @@ void delay_ms(uint32_t ms);
 
 int main(void)
 {
-	/*__enable_irq();
+	__enable_irq();
 	
 	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
 	NVIC_SetPriority(USART3_IRQn, 1);
@@ -36,17 +36,9 @@ int main(void)
 	
 	receiver_init();
 	
-	motors_init();*/
+	motors_init();
-	
-	lidar_init();
-	lidar_tim7_init();
 	
 	while (1)
-	{
-		lidar_update(&lidar);
-	}
-	
-	/*while (1)
 	{
 		attitude_update(&attitude, &imu);
 		
@@ -65,7 +57,7 @@ int main(void)
 		{
 			motors_turn_off();
 		}
-	}*/
+	}
 }
 
 void delay_ms(uint32_t ms)

drone.ewp

@@ -362,6 +362,7 @@
                     <state>$PROJ_DIR$\Source\Core\Inc</state>
                     <state>$PROJ_DIR$\Source\BSP\Inc</state>
                     <state>$PROJ_DIR$\Source\Control\Inc</state>
+                    <state>$PROJ_DIR$\Source\INS\geometry</state>
                 </option>
                 <option>
                     <name>CCStdIncCheck</name>
@@ -2367,6 +2368,24 @@
                 </group>
             </group>
         </group>
+        <group>
+            <name>INS</name>
+            <group>
+                <name>geometry</name>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.h</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.h</name>
+                </file>
+            </group>
+        </group>
         <file>
             <name>$PROJ_DIR$\Source\main.c</name>
         </file>

drone.ewt

@@ -3552,6 +3552,24 @@
                 </group>
             </group>
         </group>
+        <group>
+            <name>INS</name>
+            <group>
+                <name>geometry</name>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.h</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.h</name>
+                </file>
+            </group>
+        </group>
         <file>
             <name>$PROJ_DIR$\Source\main.c</name>
         </file>