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

Source/BSP/Inc/imu.h

@@ -8,7 +8,6 @@
 #define REG_BANK_SEL			0x7F
 #define REG_GYRO_CONFIG_1		0x01
 #define REG_ACCEL_CONFIG		0x14
-#define IMU_RATE_HZ             1000                    // 1 ms
 #define IMU_DT					0.002f   // 2 ms
 
 typedef struct
@@ -17,17 +16,21 @@ typedef struct
 	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
+void imu_pow_init();
+
 void i2c_gpio_init();
 
 void i2c1_init();
 
-void icm_init();
+void imu_init();
+
+void imu_tim6_init();
 
 void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
 
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);
 
-void icm_read_raw(imu_raw_t* data);
+void imu_read_raw(imu_raw_t* data);
 
 static void i2c_wait_idle(I2C_TypeDef* i2c);
 

Source/BSP/Inc/lidar.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#ifndef LIDAR_H
+#define LIDAR_H
+
+#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
+#define TF02_I2C_ADDR		0x10
+
+typedef struct
+{
+	uint8_t header1;		// 0x59
+	uint8_t header2;		// 0x59
+	uint8_t distance_l;		// cm
+ 	uint8_t distance_h;		// cm
+ 	uint8_t strength_l;
+ 	uint8_t strength_h;
+ 	uint8_t temp_l;
+ 	uint8_t temp_h;
+ 	uint8_t checksum;
+} lidar_data_buf;
+
+typedef struct
+{
+	uint16_t distance;		// cm
+	uint16_t strength;
+	uint16_t temperature;
+} lidar_data;
+
+void lidar_init();
+void lidar_tim7_init();
+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();
+static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size);
+void tf02_force_uart();
+
+#endif

Source/BSP/Src/imu.c

@@ -1,5 +1,15 @@
 #include "imu.h"
 
+void imu_pow_init()
+{
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
+	GPIOC->MODER &= ~(3 << (13 * 2));
+	GPIOC->MODER |= 1 << (13 * 2);
+	GPIOC->OTYPER &= ~(1 << 13);
+	GPIOC->PUPDR &= ~(3 << (13 * 2));
+	GPIOC->BSRR = 1 << (13 + 16);
+}
+
 void i2c_gpio_init()
 {
 	// enable GPIOB clock
@@ -32,7 +42,7 @@ void i2c1_init()
 	I2C1->CR1 |= I2C_CR1_PE;
 }
 
-void icm_init()
+void imu_init()
 {
 	// select bank 0
 	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
@@ -53,6 +63,19 @@ void icm_init()
 	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
 }
 
+void imu_tim6_init()
+{
+	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
+	
+	TIM6->PSC = 16000 - 1;        // 16 MHz / 16000 = 1000 Hz (1 ms)
+	TIM6->ARR = 2 - 1;            // 2 ms
+	
+	TIM6->DIER |= TIM_DIER_UIE;   // interrupt enable
+	TIM6->CR1 |= TIM_CR1_CEN;     // counter enable
+	
+	NVIC_EnableIRQ(TIM6_DAC_IRQn);
+}
+
 void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
 {
 	i2c_wait_idle(I2C1);
@@ -94,7 +117,7 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
 	I2C1->CR2 |= I2C_CR2_STOP;
 }
 
-void icm_read_raw(imu_raw_t* data)
+void imu_read_raw(imu_raw_t* data)
 {
 	uint8_t buf[12];
 	

Source/BSP/Src/imu_processing.c

@@ -33,7 +33,7 @@ void imu_read_scaled(imu_scaled_t* out)
 {
 	static imu_raw_t raw;
 	
-  icm_read_raw(&raw);
+	imu_read_raw(&raw);
 	
 	out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
 	out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
@@ -68,7 +68,7 @@ void imu_calibrate()
 	
 	for (uint16_t i = 0; i < samples; ++i)
 	{
-    icm_read_raw(&imu);
+		imu_read_raw(&imu);
 		
 		sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
 		sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;

Source/BSP/Src/lidar.c

@@ -0,0 +1,214 @@
+#include "lidar.h"
+
+volatile uint8_t usart3_rx_buf[USART3_BUF_SIZE];
+static uint8_t usart3_rx_head = 0;
+static uint8_t usart3_rx_tail = 0;
+
+void lidar_init()
+{
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
+	
+	// port 11 alt func mode
+	GPIOB->MODER &= ~(3 << (11 * 2));
+	GPIOB->MODER |= 2 << (11 * 2);
+	
+	// set AF7 on AFRegister for GPIOB11
+	GPIOB->AFR[1] &= ~(0xF << 12);
+	GPIOB->AFR[1] |= 7 << 12;
+	
+	// very high speed
+	GPIOB->OSPEEDR |= 3 << (11 * 2);
+	
+	// pull-up
+	GPIOB->PUPDR &= ~(3 << (11 * 2));
+	GPIOB->PUPDR |= 1 << (11 * 2);
+	
+	// SYSCLK selected as USART3 clock
+	RCC->CCIPR &= ~(RCC_CCIPR_USART3SEL);
+	RCC->CCIPR |= 1 << RCC_CCIPR_USART3SEL_Pos;
+	RCC->APB1ENR1 |= RCC_APB1ENR1_USART3EN;
+	
+	USART3->CR1 = 0;
+	USART3->CR2 = 0;
+	USART3->CR3 = 0;
+	
+	USART3->BRR = 16000000UL / 115200UL;
+	
+	// parity control disable
+	USART3->CR1 &= ~USART_CR1_PCE;
+	
+	// word length 8 bit
+	USART3->CR1 &= ~USART_CR1_M1 & ~USART_CR1_M0;
+	
+	// 1 stop bit
+	USART3->CR2 &= ~USART_CR2_STOP;
+	
+	// receiver enable
+	// interrupt generated whenever ORE = 1 or RXNE = 1
+	USART3->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
+	
+	// overrun disable
+	// USART3->CR3 |= USART_CR3_OVRDIS;
+	
+	// USART3 enable
+	USART3->CR1 |= USART_CR1_UE;
+	
+	// Interrupt enable
+	NVIC_EnableIRQ(USART3_IRQn);
+}
+
+void lidar_tim7_init()
+{
+	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
+	
+	TIM7->PSC = 16000 - 1;        // 16 MHz / 16000 = 1000 Hz (1 ms)
+	TIM7->ARR = 10 - 1;           // 10 ms
+	
+	TIM7->DIER |= TIM_DIER_UIE;   // interrupt enable
+	TIM7->CR1 |= TIM_CR1_CEN;     // counter enable
+	
+	NVIC_EnableIRQ(TIM7_DAC_IRQn);
+}
+	
+void TIM7_DAC_IRQHandler()
+{
+	if (TIM7->SR & TIM_SR_UIF)
+	{
+		TIM7->SR &= ~TIM_SR_UIF;
+		//lidar_update_flag = 1;
+	}
+}
+	
+void USART3_IRQHandler()
+{
+	if (USART3->ISR & USART_ISR_RXNE)
+	{
+		usart3_rx_buf[usart3_rx_head] = USART3->RDR;
+		usart3_rx_head = (usart3_rx_head + 1) % USART3_BUF_SIZE;
+	}
+}
+
+uint8_t usart_available()
+{
+	return usart3_rx_head != usart3_rx_tail;
+}
+
+uint8_t usart_read()
+{
+	uint8_t data = usart3_rx_buf[usart3_rx_tail];
+	usart3_rx_tail = (usart3_rx_tail + 1) % USART3_BUF_SIZE;
+	return data;
+}
+
+void lidar_update(lidar_data* lidar)
+{
+	static uint8_t frame[USART3_FRAME_SIZE];
+	static uint8_t index = 0;
+	
+	while(usart_available())
+	{
+		uint8_t c = usart_read();
+		
+		frame[index++] = c;
+		
+		if (index == USART3_FRAME_SIZE)
+		{
+			uint8_t checksum = 0;
+			for (uint8_t i = 0; i < USART3_FRAME_SIZE - 1; ++i) checksum += frame[i];
+			
+			if (checksum == frame[USART3_FRAME_SIZE - 1])
+			{
+				lidar->distance 	= frame[2] | (frame[3] << 8);
+                lidar->strength 	= frame[4] | (frame[5] << 8);
+                lidar->temperature 	= frame[6] | (frame[7] << 8);
+			}
+			
+			index = 0;
+		}
+	}
+}
+
+void lidar_i2c2_init()
+{
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+	
+	GPIOA->MODER &= ~(3 << (8 * 2)) & ~(3 << (9 * 2));
+	GPIOA->MODER |= 2 << (8 * 2) | 2 << (9 * 2);		// alt func mode
+	
+	GPIOA->AFR[1] &= ~(0xF << 0) & ~(0xF << 4);
+	GPIOA->AFR[1] |= 4 << 0 | 4 << 4;					// AF4
+	
+	GPIOA->OTYPER |= 1 << 8 | 1 << 9;					// open-drain
+	
+	GPIOA->PUPDR &= ~(3 << (8 * 2)) & ~(3 << (9 * 2));
+	GPIOA->PUPDR |= 1 << (8 * 2) | 1 << (9 * 2);		// pull-up
+	
+	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C2EN;				// enable I2C2
+	I2C2->TIMINGR = 0x00303D5B;							// 400 kHz @ 16 MHz
+	I2C2->CR1 |= I2C_CR1_PE;
+}
+
+static void i2c2_wait_txis()
+{
+    while (!(I2C2->ISR & I2C_ISR_TXIS));
+}
+
+static void i2c2_wait_stop()
+{
+    while (!(I2C2->ISR & I2C_ISR_STOPF));
+    I2C2->ICR |= I2C_ICR_STOPCF;
+}
+
+static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size)
+{
+    while (I2C2->ISR & I2C_ISR_BUSY);
+
+    I2C2->CR2 = 0;
+    I2C2->CR2 |= (addr << 1);                 // 7-bit addr
+    I2C2->CR2 |= (size << 16);                // bite count
+    I2C2->CR2 |= I2C_CR2_AUTOEND;             // auto stop
+    I2C2->CR2 |= I2C_CR2_START;               // start
+
+    for (uint8_t i = 0; i < size; i++)
+    {
+        i2c2_wait_txis();
+        I2C2->TXDR = data[i];
+    }
+
+    i2c2_wait_stop();
+
+    // check NACK
+    if (I2C2->ISR & I2C_ISR_NACKF)
+    {
+        I2C2->ICR |= I2C_ICR_NACKCF;
+        return 0;
+    }
+
+    return 1;
+}
+
+void tf02_force_uart()
+{
+    uint8_t cmd_uart[] = {0x5A, 0x05, 0x0A, 0x00, 0x69};
+    uint8_t cmd_save[] = {0x5A, 0x04, 0x11, 0x6F};
+
+    // force UART command
+    if (!i2c2_write(TF02_I2C_ADDR, cmd_uart, sizeof(cmd_uart)))
+    {
+        // no ACK — lidar is not on i2c
+        return;
+    }
+
+    for (volatile int i = 0; i < 100000; i++);
+
+	// save command
+    i2c2_write(TF02_I2C_ADDR, cmd_save, sizeof(cmd_save));
+
+    for (volatile int i = 0; i < 200000; i++);
+}
+
+
+
+
+
+

Source/BSP/Src/radio_receiver.c

@@ -21,7 +21,6 @@ void receiver_gpio_init()
 	// pull-up
 	GPIOA->PUPDR &= ~(3 << (3 * 2));
 	GPIOA->PUPDR |= 1 << (3 * 2);
-  
 }
 
 void receiver_lpuart_clock_init()
@@ -167,39 +166,3 @@ int8_t bool_mapping_gt(int16_t x, int16_t boundary)
 {
 	return x >= boundary;
 }
-
-//------------------------------------------------------------------------------
-
-void toggle_led()
-{
-    if (GPIOA->ODR & (1 << 15)) 
-    {
-        GPIOA->BSRR = 1 << (15 + 16);
-    }
-    else
-    {
-        GPIOA->BSRR = 1 << 15;
-    }
-}
-
-void led_init(void)
-{
-    /* Enable GPIOA clock */
-    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
-
-    /* PA15 -> Output mode */
-    GPIOA->MODER &= ~(3U << (15 * 2));
-    GPIOA->MODER |=  (1U << (15 * 2));
-
-    /* Push-pull */
-    GPIOA->OTYPER &= ~(1U << 15);
-
-    /* Low speed (?????????? ??? LED) */
-    GPIOA->OSPEEDR &= ~(3U << (15 * 2));
-
-    /* No pull-up / pull-down */
-    GPIOA->PUPDR &= ~(3U << (15 * 2));
-
-    /* Start with LED OFF */
-    GPIOA->BSRR = (1U << (15 + 16));
-}

Source/Core/Inc/timer.h

@@ -1,8 +0,0 @@
-#ifndef TIMER_H
-#define TIMER_H
-
-#include "stm32g431xx.h"
-
-void tim6_init();
-
-#endif

Source/Core/Src/stm32g4xx_it.c

@@ -1,166 +0,0 @@
-/**
-  ******************************************************************************
-  * @file    Templates/Src/stm32g4xx_it.c
-  * @author  MCD Application Team
-  * @brief   Main Interrupt Service Routines.
-  *          This file provides template for all exceptions handler and
-  *          peripherals interrupt service routine.
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2019 STMicroelectronics.
-  * All rights reserved.
-  *
-  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
-  ******************************************************************************
-  */
-
-/* Includes ------------------------------------------------------------------*/
-#include "stm32g4xx_it.h"
-
-/** @addtogroup STM32G4xx_HAL_Examples
-  * @{
-  */
-
-/** @addtogroup Templates
-  * @{
-  */
-
-/* Private typedef -----------------------------------------------------------*/
-/* Private define ------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
-
-/* Private function prototypes -----------------------------------------------*/
-/* Private functions ---------------------------------------------------------*/
-
-/******************************************************************************/
-/*            Cortex-M4 Processor Exceptions Handlers                         */
-/******************************************************************************/
-
-/**
-  * @brief   This function handles NMI exception.
-  * @param  None
-  * @retval None
-  */
-void NMI_Handler(void)
-{
-}
-
-/**
-  * @brief  This function handles Hard Fault exception.
-  * @param  None
-  * @retval None
-  */
-void HardFault_Handler(void)
-{
-  /* Go to infinite loop when Hard Fault exception occurs */
-  while (1)
-  {
-  }
-}
-
-/**
-  * @brief  This function handles Memory Manage exception.
-  * @param  None
-  * @retval None
-  */
-void MemManage_Handler(void)
-{
-  /* Go to infinite loop when Memory Manage exception occurs */
-  while (1)
-  {
-  }
-}
-
-/**
-  * @brief  This function handles Bus Fault exception.
-  * @param  None
-  * @retval None
-  */
-void BusFault_Handler(void)
-{
-  /* Go to infinite loop when Bus Fault exception occurs */
-  while (1)
-  {
-  }
-}
-
-/**
-  * @brief  This function handles Usage Fault exception.
-  * @param  None
-  * @retval None
-  */
-void UsageFault_Handler(void)
-{
-  /* Go to infinite loop when Usage Fault exception occurs */
-  while (1)
-  {
-  }
-}
-
-/**
-  * @brief  This function handles SVCall exception.
-  * @param  None
-  * @retval None
-  */
-void SVC_Handler(void)
-{
-}
-
-/**
-  * @brief  This function handles Debug Monitor exception.
-  * @param  None
-  * @retval None
-  */
-void DebugMon_Handler(void)
-{
-}
-
-/**
-  * @brief  This function handles PendSVC exception.
-  * @param  None
-  * @retval None
-  */
-void PendSV_Handler(void)
-{
-}
-
-/**
-  * @brief  This function handles SysTick Handler.
-  * @param  None
-  * @retval None
-  */
-void SysTick_Handler(void)
-{
-}
-
-/******************************************************************************/
-/*                 STM32G4xx Peripherals Interrupt Handlers                   */
-/*  Add here the Interrupt Handler for the used peripheral(s) (PPP), for the  */
-/*  available peripheral interrupt handler's name please refer to the startup */
-/*  file (startup_stm32g4xxxx.s).                                             */
-/******************************************************************************/
-
-/**
-  * @brief  This function handles PPP interrupt request.
-  * @param  None
-  * @retval None
-  */
-/*void PPP_IRQHandler(void)
-{
-}*/
-
-
-/**
-  * @}
-  */
-
-/**
-  * @}
-  */
-
-

Source/Core/Src/stm32g4xx_it.h

@@ -1,49 +0,0 @@
-/**
-  ******************************************************************************
-  * @file    Templates/Inc/stm32g4xx_it.h
-  * @author  MCD Application Team
-  * @brief   This file contains the headers of the interrupt handlers.
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2019 STMicroelectronics.
-  * All rights reserved.
-  *
-  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
-  ******************************************************************************
-  */
-
-/* Define to prevent recursive inclusion -------------------------------------*/
-#ifndef STM32G4xx_IT_H
-#define STM32G4xx_IT_H
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-/* Includes ------------------------------------------------------------------*/
-/* Exported types ------------------------------------------------------------*/
-/* Exported constants --------------------------------------------------------*/
-/* Exported macros -----------------------------------------------------------*/
-/* Exported functions ------------------------------------------------------- */
-
-void NMI_Handler(void);
-void HardFault_Handler(void);
-void MemManage_Handler(void);
-void BusFault_Handler(void);
-void UsageFault_Handler(void);
-void SVC_Handler(void);
-void DebugMon_Handler(void);
-void PendSV_Handler(void);
-void SysTick_Handler(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* STM32G4xx_IT_H */
-
-

Source/Core/Src/timer.c

@@ -1,14 +0,0 @@
-#include "timer.h"
-
-void tim6_init()
-{
-  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
-  
-  TIM6->PSC = 16000 - 1; // 16 MHz / 16000 = 1000 Hz (1 ms)
-  TIM6->ARR = 2 - 1;    // 2 ms
-  
-  TIM6->DIER |= TIM_DIER_UIE;   // interrupt enable
-  TIM6->CR1 |= TIM_CR1_CEN;     // counter enable
-  
-  NVIC_EnableIRQ(TIM6_DAC_IRQn);
-}

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

@@ -1,11 +1,11 @@
 #include "stm32g431xx.h"
 #include "imu.h"
 #include "imu_processing.h"
-#include "timer.h"
 #include "attitude.h"
 #include "radio_receiver.h"
 #include "motors.h"
 #include "pid.h"
+#include "lidar.h"
 
 
 imu_scaled_t imu;
@@ -13,6 +13,7 @@ attitude_t attitude;
 rc_channels rx_chs_raw;
 rc_channels rx_chs_normalized;
 control_channels_t ctrl_chs;
+lidar_data lidar;
 
 void delay_ms(uint32_t ms);
 
@@ -20,23 +21,16 @@ void delay_ms(uint32_t ms);
 {
 	__enable_irq();
 	
-  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
-  GPIOC->MODER &= ~(3 << (13 * 2));
-  GPIOC->MODER |= 1 << (13 * 2);
-  GPIOC->OTYPER &= ~(1 << 13);
-  GPIOC->PUPDR &= ~(3 << (13 * 2));
-  GPIOC->BSRR = 1 << (13 + 16);
-  
-  delay_ms(200);
-  
-  NVIC_SetPriority(TIM6_DAC_IRQn, 1);
+	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
+	NVIC_SetPriority(USART3_IRQn, 1);
 	NVIC_SetPriority(LPUART1_IRQn, 0);
 	
+	imu_pow_init();
 	i2c_gpio_init();
 	i2c1_init();
-  icm_init();
+	imu_init();
+	imu_tim6_init();
 	imu_processing_init();
-  tim6_init();
 	
 	imu_calibrate();
 	
@@ -53,6 +47,8 @@ void delay_ms(uint32_t ms);
 		
 		attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
 		
+		lidar_update(&lidar);
+		
 		if (rx_chs_normalized.rc_armed)
 		{
 			motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed);

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>
@@ -2311,24 +2312,12 @@
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Inc\timer.h</name>
-                </file>
             </group>
             <group>
                 <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.h</name>
-                </file>
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\timer.c</name>
-                </file>
             </group>
         </group>
         <group>
@@ -2379,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

@@ -3496,24 +3496,12 @@
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Inc\timer.h</name>
-                </file>
             </group>
             <group>
                 <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.h</name>
-                </file>
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\timer.c</name>
-                </file>
             </group>
         </group>
         <group>
@@ -3564,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>