Починен приём данных с пульта

*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке.
*Определение вращения работает корректно.

Source/BSP/Inc/imu.h

@@ -1,49 +0,0 @@
-#pragma once
-
-#ifndef IMU_H
-#define IMU_H
-
-#include "stm32g431xx.h"
-
-#define ICM_ADDR				0x68
-#define REG_PWR_MGMT_1			0x06
-#define REG_BANK_SEL			0x7F
-#define REG_GYRO_CONFIG_1		0x01
-#define REG_ACCEL_CONFIG		0x14
-#define IMU_DT					0.002f   // 2 ms
-
-#define GYRO_FS_SEL_2000		3 << 1
-#define GYRO_DLPFCFG_73			3 << 3
-#define GYRO_FCHOICE_ON			1
-#define GYRO_FCHOICE_OFF		0
-
-#define ACCEL_FS_SEL_8			2 << 1
-#define ACCEL_DLPFCFG_69		3 << 3
-#define ACCEL_FCHOICE_ON		1
-#define ACCEL_FCHOICE_OFF		0
-
-typedef struct
-{
-	int16_t ax, ay, az; // lsb
-	int16_t gx, gy, gz; // lsb
-} imu_raw_t;
-
-void imu_pow_init();
-
-void i2c_gpio_init();
-
-void i2c1_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 imu_read_raw(imu_raw_t* data);
-
-static void i2c_wait_idle(I2C_TypeDef* i2c);
-
-#endif

Source/BSP/Inc/imu_processing.h

@@ -1,24 +0,0 @@
-#ifndef IMU_PROCESSING_H
-#define IMU_PROCESSING_H
-
-#include "imu.h"
-
-
-#define ACCEL_SENS_SCALE_FACTOR		8192.0f
-#define GYRO_SENS_SCALE_FACTOR		16.4f
-#define PI							3.14159265359f
-#define DEG2RAD						PI / 180.0f
-
-typedef struct
-{
-	float ax, ay, az; // g
-	float gx, gy, gz; // dps
-} imu_scaled_t;
-
-void imu_processing_init();
-
-void imu_read_scaled(imu_scaled_t* out);
-
-void imu_calibrate();
-
-#endif

Source/BSP/Inc/lidar.h

@@ -1,50 +0,0 @@
-#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/Inc/motors.h

@@ -1,24 +0,0 @@
-#ifndef MOTORS_H
-#define MOTORS_H
-
-#include <stdint.h>
-#include "pid.h"
-
-void motors_init();
-void motor_gpio_tim1_ch3_init();
-void motor_gpio_tim1_ch4_init();
-void motors_tim1_ch3_4_init();
-
-void motor_gpio_tim2_ch1_init();
-void motor_gpio_tim2_ch2_init();
-void motors_tim2_ch1_2_init();
-
-void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed);
-void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed);
-void motors_turn_off();
-void motor1_set_throttle(int16_t us);
-void motor2_set_throttle(int16_t us);
-void motor3_set_throttle(int16_t us);
-void motor4_set_throttle(int16_t us);
-
-#endif

Source/BSP/Src/imu.c

@@ -1,143 +0,0 @@
-#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
-	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
-	
-	// Alt function mode PB8, PB9
-	GPIOB->MODER &=  ~((3 << 8 * 2) | (3 << 9 * 2));
-	GPIOB->MODER |= (2 << (8 * 2)) | (2 << (9 * 2));
-	
-	// select AF4 for I2C1 at PB8 and PB9
-	GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
-	GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
-	
-	// enable open-drain
-	GPIOB->OTYPER |= (1 << 8) | (1 << 9);
-	
-	// set pull-up
-	GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
-	GPIOB->PUPDR |= (1 << 8 * 2) | (1 << 9 * 2);
-}
-
-void i2c1_init()
-{
-	// enable I2C1
-	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
-	
-	// 400 kHz @ 16 MHz
-	I2C1->TIMINGR = 0x00303D5B;
-	
-	I2C1->CR1 |= I2C_CR1_PE;
-}
-
-void imu_init()
-{
-	// select bank 0
-	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
-	
-	// wake up, auto clock
-	i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
-	
-	// select bank 2
-	i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
-	
-	// gyro ~2000 dps, FS_SEL = 3
-	i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
-	
-	// accel ~4g, FS_SEL = 1
-	i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
-	
-	// back to bank 0
-	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);
-	
-	// write register address
-	I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
-	
-	while (!(I2C1->ISR & I2C_ISR_TXIS));
-	I2C1->TXDR = reg;
-	
-	while (!(I2C1->ISR & I2C_ISR_TC));
-	
-	I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
-	
-	for (uint8_t i = 0; i < len; ++i)
-	{
-		while (!(I2C1->ISR & I2C_ISR_RXNE));
-		buf[i] = I2C1->RXDR;
-	}
-	
-	while (!(I2C1->ISR & I2C_ISR_STOPF));
-	I2C1->ICR |= I2C_ICR_STOPCF;
-}
-
-void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
-{
-	i2c_wait_idle(I2C1);
-	
-	// write register address
-	I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
-	
-	while (!(I2C1->ISR & I2C_ISR_TXIS));
-	I2C1->TXDR = reg;
-	
-	while (!(I2C1->ISR & I2C_ISR_TXIS));
-	I2C1->TXDR = data;
-	
-	while (!(I2C1->ISR & I2C_ISR_TC));
-	I2C1->CR2 |= I2C_CR2_STOP;
-}
-
-void imu_read_raw(imu_raw_t* data)
-{
-	uint8_t buf[12];
-	
-	i2c_read(ICM_ADDR, 0x2D, buf, 12);
-	
-	data->ax = (buf[0] << 8) | buf[1];
-	data->ay = (buf[2] << 8) | buf[3];
-	data->az = (buf[4] << 8) | buf[5];
-	
-	data->gx = (buf[6] << 8) | buf[7];
-	data->gy = (buf[8] << 8) | buf[9];
-	data->gz = (buf[10] << 8) | buf[11];
-}
-
-static void i2c_wait_idle(I2C_TypeDef* i2c)
-{
-	while (i2c->ISR & I2C_ISR_BUSY);
-	
-	i2c->ICR = I2C_ICR_STOPCF |
-           I2C_ICR_NACKCF |
-           I2C_ICR_BERRCF |
-           I2C_ICR_ARLOCF;
-}

Source/BSP/Src/imu_processing.c

@@ -1,91 +0,0 @@
-#include "imu_processing.h"
-#include "biquad_filter.h"
-#include "math.h"
-
-static float gyro_bias_x = 0.0f;
-static float gyro_bias_y = 0.0f;
-static float gyro_bias_z = 0.0f;
-
-static float accel_bias_x = 0.0f;
-static float accel_bias_y = 0.0f;
-static float accel_bias_z = 0.0f;
-
-static biquad_t accel_x_lpf;
-static biquad_t accel_y_lpf;
-static biquad_t accel_z_lpf;
-
-static biquad_t gyro_x_lpf;
-static biquad_t gyro_y_lpf;
-static biquad_t gyro_z_lpf;
-
-void imu_processing_init()
-{
-	biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
-	
-	biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
-}
-
-void imu_read_scaled(imu_scaled_t* out)
-{
-	static imu_raw_t 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;
-	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z + 1;
-	
-	out->ax = biquad_apply(&accel_x_lpf, out->ax);
-	out->ay = biquad_apply(&accel_y_lpf, out->ay);
-	out->az = biquad_apply(&accel_z_lpf, out->az);
-	
-	out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
-	out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
-	out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
-	
-	out->gx = biquad_apply(&gyro_x_lpf, out->gx);
-	out->gy = biquad_apply(&gyro_y_lpf, out->gy);
-	out->gz = biquad_apply(&gyro_z_lpf, out->gz);
-}
-
-void imu_calibrate()
-{
-	const int samples = 1000;
-	
-	float sum_ax = 0;
-	float sum_ay = 0;
-	float sum_az = 0;
-	
-	float sum_gx = 0;
-	float sum_gy = 0;
-	float sum_gz = 0;
-	
-	imu_raw_t imu;
-	
-	for (uint16_t i = 0; i < samples; ++i)
-	{
-		imu_read_raw(&imu);
-		
-		sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
-		sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
-		sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
-		
-		sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
-		sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
-		sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
-		
-		for (volatile uint16_t d = 0; d < 5000; ++d);
-	}
-	
-	accel_bias_x = sum_ax / samples;
-	accel_bias_y = sum_ay / samples;
-	accel_bias_z = (sum_az / samples) + 1.0f;
-	
-	gyro_bias_x = sum_gx / samples;
-	gyro_bias_y = sum_gy / samples;
-	gyro_bias_z = sum_gz / samples;
-}

Source/BSP/Src/lidar.c

@@ -1,214 +0,0 @@
-#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/Control/Attitude.cpp

@@ -0,0 +1,73 @@
+#include "Attitude.h"
+#include "PID.h"
+#include <math.h>
+
+#define FREQUENCY     100.0f
+#define PERIOD        1.0f / FREQUENCY
+
+#define PI            3.14159265359f
+#define DEG2RAD       PI / 180.0f
+#define RATE_LIM      300.0f
+
+constexpr float angle_kp_pitch = 2.5f;
+constexpr float angle_kp_roll  = 2.5f;
+constexpr float angle_kp_yaw   = 2.0f;
+
+pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f};
+pid_t pid_roll  = {.kp = 0.6f, .kd = 0.025f};
+pid_t pid_yaw   = {.kp = 0.6f};
+
+void attitude_init(attitude_t* att)
+{
+  att->gyro = (Vector3){};
+}
+
+void attitude_controller_update(control_channels_t* control,
+                                const rc_channels* rx,
+                                const Quaternion* current_q,
+                                const Vector3* gyro)
+{
+  Quaternion q_target = rx_to_quaternion(rx);
+  
+  Quaternion q_error = current_q->GetError(q_target, true);
+
+  Vector3 angle_error =
+  {
+    2.0f * q_error.X,
+    2.0f * q_error.Y,
+    2.0f * q_error.Z
+  };
+
+  Vector3 desired_rate =
+  {
+    angle_error.X * angle_kp_pitch,
+    angle_error.Y * angle_kp_roll,
+    angle_error.Z * angle_kp_yaw
+  };
+
+  desired_rate.X = constrain(desired_rate.X, -RATE_LIM, RATE_LIM);
+  desired_rate.Y = constrain(desired_rate.Y, -RATE_LIM, RATE_LIM);
+  desired_rate.Z = constrain(desired_rate.Z, -RATE_LIM, RATE_LIM);
+
+  control->pitch = pid_update(&pid_pitch, desired_rate.X - gyro->X, gyro->X, PERIOD);
+  control->roll  = pid_update(&pid_roll,  desired_rate.Y - gyro->Y, gyro->Y, PERIOD);
+  control->yaw   = pid_update(&pid_yaw,   desired_rate.Z - gyro->Z, gyro->Z, PERIOD);
+}
+
+Quaternion rx_to_quaternion(const rc_channels* rx)
+{
+  Quaternion q;
+	float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
+	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
+	float yaw 	= 0;
+	
+	Vector3 pry = {pitch, roll, yaw};
+	q.CreatePitchRollYaw(pry);
+	return q;
+}
+
+float constrain(float x, float min, float max)
+{
+	if (x < min) x = min; else if (x > max) x = max;
+	return x;
+}

Source/Control/Attitude.h

@@ -1,17 +1,18 @@
+#pragma once
+
 #ifndef ATTITUDE_H
 #define ATTITUDE_H
 
-#include "quaternion.h"
+#include "Quaternion.h"
-#include "vector.h"
+#include "Vector.h"
-#include "pid.h"
+#include "PID.h"
-#include "radio_receiver.h"
+#include "RadioReceiver.h"
-#include "imu_processing.h"
 #include "IRS.h"
 
-typedef struct
+struct attitude_t
 {
   Vector3 gyro;
-} attitude_t;
+};
 
 void attitude_init(attitude_t* att);
 

Source/Control/Autopilot.cpp

@@ -0,0 +1,3 @@
+#include "Autopilot.h"
+
+DroneStatus MainDrone;

Source/Control/Autopilot.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include "Protocol.h"
+
+struct DroneStatus
+{
+  bool MainActive = false; // Индикатор снятия с охраны
+  bool AutoPilotActive = false; // Индикатор актив. автопилота
+  bool ManualInputAllowed = false; // Индикатор возможности ручного управления (безопасного)
+  bool AltChangeAllowed = false; // Индикатор возможности смены целевой высоты в режиме удержания позиции
+  bool PointBeginNeed = true; // Индикатор необходимости указания начальной точки
+  bool MissionExecute = false; // Инидикатор выполняется ли миссия
+  
+  bool MotorMoutionTestEnable = false;
+  unsigned char MotorTestId = 0;
+  unsigned long TimeMotorTestEnable = 0;
+  
+  bool NeedSendRawCalibData = false;
+  SENSOR_ID Sensor = SENSOR_ID::ACC;
+  unsigned long TimeLastSend = 0;
+  unsigned long TimeStartSend = 0;
+  bool NeedSaveNewEEPROMData = false;
+  bool EEPROMSaved = false;
+  bool NeedSaveNewFLASHData = false;
+  bool FLASHSaved = false;
+  
+  bool EEPROMInit = false;
+  bool BarInit = false;
+  bool IMUInit = false;
+  bool GPSInit = false;
+  bool TOFInit = false;
+  bool OFInit = false;
+  bool ExternMagInit = false;
+  
+  float AvgEngineThrust = 0.0f;
+  float MaxHeightGroundMode = 5.0f;
+  
+  ENGINE_STATUS engine = ENGINE_STATUS::disable;
+  SYS_MODE Mode = SYS_MODE::manual;
+  STATUS_MODE StatusMode = STATUS_MODE::on_ground;
+  NAV_SYS_MODE Nav = NAV_SYS_MODE::inertial;
+};
+
+extern DroneStatus MainDrone;
+
+#endif

Source/Control/Inc/biquad_filter.h

@@ -1,20 +0,0 @@
-#ifndef BIQUAD_FILTER_H
-#define BIQUAD_FILTER_H
-
-#include <math.h>
-
-#define PI			3.14159265359f
-
-typedef struct
-{
-	float b0, b1, b2;
-	float a1, a2;
-	
-	float x1, x2;
-	float y1, y2;
-} biquad_t;
-
-void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate);
-float biquad_apply(biquad_t* f, float input);
-
-#endif

Source/Control/PID.cpp

@@ -1,4 +1,4 @@
-#include "pid.h"
+#include "PID.h"
 
 float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
 {

Source/Control/PID.h

@@ -1,9 +1,11 @@
+#pragma once
+
 #ifndef PID_H
 #define PID_H
 
-#include "stm32g431xx.h"
+#include "stm32g4xx.h"
 
-typedef struct
+struct pid_t
 {
 	float kp;
 	float ki;
@@ -11,14 +13,14 @@ typedef struct
 	
 	float integral;
 	float prev_error;
-} pid_t;
+};
 
-typedef struct
+struct control_channels_t
 {
 	float roll;
 	float pitch;
 	float yaw;
-} control_channels_t;
+};
 
 float pid_update(pid_t* pid, float error, float gyro_rate, float dt);
 

Source/Control/Protocol.cpp

@@ -0,0 +1,2 @@
+#include "Protocol.h"
+

Source/Control/Protocol.h

@@ -0,0 +1,296 @@
+#pragma once
+
+#ifndef PROTOCOL_H
+#define PROTOCOL_H
+
+#define MAX_LEN_MISSION_NAME 64
+#define MAX_COUNT_MISSION_POINT 100
+
+enum class MESSAGES_ID : unsigned char
+{
+  SysInfo = 1,
+  GyroInfo = 2,
+  AccelInfo = 3,
+  GpsInfo = 4,
+  InertialInfo = 5,
+  BatteryInfo = 6,
+  Ack = 7,
+  StatusCommand = 8,
+  StatusAllCommands = 9,
+  Command = 10,
+  RequestReg = 11,
+  ResponseReg = 12,
+  Auth = 13,
+  OpenKey = 14,
+  MissionCount = 15,
+  MissionItem = 16,
+  MissionItemAck = 17,
+  MissionRequestItem = 18,
+  RequestLastMessage = 19,
+  ConnectionTest = 20,
+  MissionProgress = 21,
+  CountMenuCategories = 32,
+  CategoriesMenu = 33,
+  CategoriesParameters = 34,
+  RawCalibData = 35
+};
+enum class COMMANDS_ID : unsigned char
+{
+  ChangeNav = 1,
+  ChangeSpeed = 2,
+  Land = 3,
+  GoHome = 4,
+  StopEngine = 5,
+  StartEngine = 6,
+  Pause = 7,
+  Continue = 8,
+  GoToGlobal = 9,
+  GoToLocal = 10,
+  MissionStart = 11,
+  SetParameter = 15,
+  ProcessingCalibration = 16,
+  SetCalibrationData = 17,
+  MotorMoutionTest = 18
+};
+enum class ERROR_CODE_COMMAND : unsigned char
+{
+  No_error = 0,
+  Speed_has_already_been_set = 1,
+  Engines_are_already_running = 2,
+  Engines_have_already_been_stopped = 3,
+  UAV_is_already_on_the_ground = 4,
+  UAV_is_already_landing = 5,
+  UAV_is_already_moving = 6,
+  Mission_has_not_been_launched = 7,
+  Mission_is_already_on_pause = 8,
+  Mission_was_not_on_pause = 9,
+  Mission_was_not_launched = 10,
+  This_NavSys_has_already_been_applied = 11,
+  Lost_connection_with_controller = 12,
+  The_command_is_canceled_when_a_new_one_is_received = 13,
+  Error_reading_the_mission_file = 14,
+  Error_validating_the_mission_file = 15,
+  This_attack_event_already_exists = 16,
+  Error_creating_the_configuration_file_moa = 17,
+  Error_saving_the_configuration_file_moa = 18,
+  Error_deleting_the_attack_event = 19,
+  This_attack_event_cannot_be_deleted = 20,
+  This_attack_event_does_not_exist = 21,
+  Gps_not_init = 22,
+};
+enum class SENSOR_ID : unsigned char
+{
+  ACC = 0,
+  IMU_MAG = 1,
+  EXTERNAL_MAG = 2,
+  LEVEL_HOR = 3
+};
+enum class ENGINE_STATUS : unsigned char
+{
+  disable = 0,
+  enable = 1
+};
+enum class STATUS_MODE : unsigned char
+{
+  on_ground = 1,
+  taking_off = 2,
+  fly = 3,
+  landing = 4
+};
+enum class SYS_MODE : unsigned char
+{
+  take_off = 1,
+  land = 2,
+  go_home = 3,
+  auto_mode = 4,
+  hold = 5,
+  manual = 6
+};
+enum class NAV_SYS_MODE : unsigned char
+{
+  inertial = 1,
+  gps = 2,
+  Optical_flow = 3
+};
+enum class EXEC_CODE_COMMAND : unsigned char
+{
+  error = 0,
+  in_progress = 1,
+  completed = 2
+};
+enum class FEASIBILITY_CODE_COMMAND : unsigned char
+{
+  cannot_be_performed = 0,
+  accepted = 1
+};
+enum class DATA_TYPES : unsigned char
+{
+  dt_char = 0,
+  dt_unsigned_char = 1,
+  dt_string = 2,
+  dt_short = 3,
+  dt_bool = 4,
+  dt_unsigned_short = 5,
+  dt_int = 6,
+  dt_unsigned_int = 7,
+  dt_long = 8,
+  dt_unsigned_long = 9,
+  dt_long_long = 10,
+  dt_unsigned_long_long = 11,
+  dt_float_32 = 12,
+  dt_float_64 = 13,
+  dt_unknown = 255
+};
+
+//extern CRITICAL_SECTION ProtoSection;
+
+#pragma pack(push,1)
+
+struct MissionItemData
+{
+  unsigned long long item_number;
+  COMMANDS_ID item_command; // ID команды, может быть: движение к точке, возврат домой, посадка, смена скорости и др.
+  char param1; // резерв
+  float param2; // значение скорости, если в миссии есть команда смены скорости, для других команд - резерв
+  char param3; // резерв
+  float param4; // yaw
+  double param5; // широта
+  double param6; // долгота
+  float param7; // абс. высота
+};
+
+struct MissionMetaData
+{
+  unsigned long long current_item;
+  unsigned long long total_items;
+  bool launch_confidentiality;
+  bool mission_priority;
+  bool fault_tolerence;
+  bool f_start;
+  bool f_pause;
+  unsigned char mission_name_len;
+  char mission_name[MAX_LEN_MISSION_NAME];
+};
+struct MissionItemsData
+{
+  MissionItemData items[MAX_COUNT_MISSION_POINT];
+};
+
+struct EnginePowerInfo
+{
+  unsigned char power; //Текущая нагрузка на мотор от 0 до 100%
+  unsigned int engineSpeed;//Текущие оборты в секунду мотора
+  float current; // Ток потребляемый мотором
+  float voltage; // Напряжение на моторе
+  float temp; // Температура мотора
+
+  static const unsigned char MaxCount = 8;
+};
+
+struct ProtoDataBattery
+{
+  unsigned char perCharge;
+  float voltage;
+  float amperage;
+  float temp;
+  float totalPower;
+  unsigned long long timeRemaining;
+};
+struct ProtoDataSysInfo
+{
+  SYS_MODE mode;
+  STATUS_MODE statusMode;
+  NAV_SYS_MODE navSys;
+  ENGINE_STATUS engineStatus;
+  unsigned char engineCount;
+  float pressureBaro;
+  float tempBaro;
+
+  EnginePowerInfo enginePower[EnginePowerInfo::MaxCount];
+};
+struct ProtoDataGyroInfo
+{
+  float yawGyroVel;
+  float pitchGyroVel;
+  float rollGyroVel;
+};
+struct ProtoDataAccelInfo
+{
+  float yawAccelVel; 
+  float pitchAccelVel; 
+  float rollAccelVel; 
+  float aX; 
+  float aY; 
+  float aZ; 
+  float tempAccel; 
+};
+struct ProtoDataGpsInfo
+{
+  double lat;
+  double lon;
+  float absAlt;
+  float realAlt;
+  float hdop;
+  float vdop;
+  float pdop;
+  float noise;
+  float jamming;
+  unsigned char satVisible;
+  unsigned char satUsed;
+  float speed;
+  unsigned char fixType;
+  unsigned long long timeUTC;
+};
+struct ProtoDataInertialInfo
+{
+  float x;
+  float y;
+  float z;
+  float headingDeg;
+  float speed;
+  float roll;
+  float pitch;
+  float yaw;
+  float rollVel;
+  float pitchVel;
+  float yawVel;
+  float baroAlt;
+};
+
+struct ProtoDataRawCalib
+{
+  SENSOR_ID sensor;
+  float X;
+  float Y;
+  float Z;
+};
+
+struct ParameterData
+{
+  const char* name;
+  void* value;
+  DATA_TYPES type;
+};
+
+#pragma pack(pop)
+
+typedef void (*SendCallback)(void* data, unsigned long size);
+
+void MainInit(SendCallback send);
+bool MainUpdate(unsigned char byte);
+
+void ProtoDataWriteReadBattery(ProtoDataBattery& data, bool read=true);
+void ProtoDataWriteReadSysInfo(ProtoDataSysInfo& data, bool read = true);
+//void ProtoDataWriteReadEngineInfo(EnginePowerInfo data[], unsigned char count, bool read = true);
+void ProtoDataWriteReadGyroInfo(ProtoDataGyroInfo& data, bool read = true);
+void ProtoDataWriteReadAccelInfo(ProtoDataAccelInfo& data, bool read = true);
+void ProtoDataWriteReadGpsInfo(ProtoDataGpsInfo& data, bool read = true);
+void ProtoDataWriteReadInertialInfo(ProtoDataInertialInfo& data, bool read = true);
+void ProtoDataWriteReadRawCalib(ProtoDataRawCalib& data, bool read = true);
+
+void RawCalibData();
+
+extern MissionMetaData ProtoMissionInfo;
+extern MissionItemsData ProtoMissionData;
+
+#endif

Source/Control/Src/attitude.c

@@ -1,69 +0,0 @@
-#include "attitude.h"
-#include "pid.h"
-#include "imu.h"
-#include "imu_processing.h"
-#include <math.h>
-
-#define RATE_LIM 300.0f
-
-float angle_kp_pitch = 2.5f;
-float angle_kp_roll  = 2.5f;
-float angle_kp_yaw   = 2.0f;
-
-pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f};
-pid_t pid_roll  = {.kp = 0.6f, .kd = 0.025f};
-pid_t pid_yaw   = {.kp = 0.6f};
-
-void attitude_init(attitude_t* att)
-{
-    att->gyro = (Vector3){0};
-}
-
-void attitude_controller_update(control_channels_t* control,
-                                const rc_channels* rx,
-                                const Quaternion* current_q,
-                                const Vector3* gyro)
-{
-    Quaternion q_target = rx_to_quaternion(rx);
-
-    Quaternion q_error = QuatGetError(current_q, &q_target, true);
-
-    Vector3 angle_error =
-    {
-        2.0f * q_error.x,
-        2.0f * q_error.y,
-        2.0f * q_error.z
-    };
-
-    Vector3 desired_rate =
-    {
-        angle_error.x * angle_kp_pitch,
-        angle_error.y * angle_kp_roll,
-        angle_error.z * angle_kp_yaw
-    };
-
-    desired_rate.x = constrain(desired_rate.x, -RATE_LIM, RATE_LIM);
-    desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM);
-    desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM);
-
-    control->pitch = pid_update(&pid_pitch, desired_rate.x - gyro->x, gyro->x, IMU_DT);
-    control->roll  = pid_update(&pid_roll,  desired_rate.y - gyro->y, gyro->y, IMU_DT);
-    control->yaw   = pid_update(&pid_yaw,   desired_rate.z - gyro->z, gyro->z, IMU_DT);
-}
-
-Quaternion rx_to_quaternion(const rc_channels* rx)
-{
-	float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
-	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
-	float yaw 	= 0;
-	
-	Vector3 pry = {pitch, roll, yaw};
-	
-	return QuatCreateFromEuler(&pry);
-}
-
-float constrain(float x, float min, float max)
-{
-	if (x < min) x = min; else if (x > max) x = max;
-	return x;
-}

Source/Control/Src/biquad_filter.c

@@ -1,43 +0,0 @@
-#include "biquad_filter.h"
-
-void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate)
-{
-	float omega = 2.0f * PI * cutoff / sample_rate;
-	float sin_omega = sinf(omega);
-	float cos_omega = cosf(omega);
-	
-	float alpha = sin_omega / sqrtf(2.0f);
-	
-	float b0 = (1 - cos_omega) / 2;
-	float b1 = 1 - cos_omega;
-	float b2 = (1 - cos_omega) / 2;
-	float a0 = 1 + alpha;
-	float a1 = -2 * cos_omega;
-	float a2 = 1 - alpha;
-	
-	f->b0 = b0 / a0;
-	f->b1 = b1 / a0;
-	f->b2 = b2 / a0;
-	f->a1 = a1 / a0;
-	f->a2 = a2 / a0;
-	
-	f->x1 = f->x2 = 0;
-	f->y1 = f->y2 = 0;
-}
-
-float biquad_apply(biquad_t* f, float input)
-{
-	float output = f->b0 * input
-	               + f->b1 * f->x1
-	               + f->b2 * f->x2
-	               - f->a1 * f->y1
-	               - f->a2 * f->y2;
-	
-	f->x2 = f->x1;
-	f->x1 = input;
-	
-	f->y2 = f->y1;
-	f->y1 = output;
-	
-	return output;
-}

Source/Core/system_stm32g4xx.c

@@ -282,4 +282,64 @@ void SystemCoreClockUpdate(void)
   * @}
   */
 
+void SystemClock_Config() // STM32G431CBT6
+{
+  // 1. Включить тактирование для интерфейса управления питанием (PWR)
+  // Это действие необходимо совершать одним из первых.
+  RCC->APB1ENR1 |= RCC_APB1ENR1_PWREN;
+  
+  // 2. Установить задержку Flash ПЕРЕД любым увеличением частоты.
+  // При переключении на PLL 170 МГц и Vcore Range 1 требуется 4 цикла ожидания.
+  // Безопаснее установить это значение заранее, пока система работает на низкой частоте HSI.
+  MODIFY_REG(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_ACR_LATENCY_4WS);
+  
+  // 3. Включить prefetch buffer, instruction cache и data cache для максимальной производительности.
+  FLASH->ACR |= FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN;
+  
+  // 4. Включить и дождаться готовности HSI (16 МГц).
+  // Это важно, даже если он уже включен по умолчанию, для явного контроля.
+  RCC->CR |= RCC_CR_HSION;
+  while(!(RCC->CR & RCC_CR_HSIRDY));
+  
+  // 5. Настроить масштабирование напряжения на Range 1 (High-performance).
+  // Это необходимо для работы на высоких частотах.
+  // ВАЖНО: Делать это ДО включения PLL и переключения на него.
+  MODIFY_REG(PWR->CR1, PWR_CR1_VOS, PWR_CR1_VOS_0);
+  // Ожидаем готовности регулятора напряжения (ухода флага VOSF).
+  while ((PWR->SR2 & PWR_SR2_VOSF) != 0);
+  
+  // 6. Включить режим Range 1 Boost для частот > 150 МГц.
+  // Согласно документации (Reference Manual), это нужно делать, когда система
+  // тактируется от HSI/HSE, ДО включения PLL.
+  PWR->CR5 |= PWR_CR5_R1MODE;
+  
+  // 7. Убедиться, что PLL выключен, перед его настройкой.
+  RCC->CR &= ~RCC_CR_PLLON;
+  while(RCC->CR & RCC_CR_PLLRDY);
+  
+  // 8. Настроить PLL для получения 170 МГц от HSI.
+  // SYSCLK = (HSI / M) * N / R = (16МГц / 4) * 85 / 2 = 170 МГц
+  // VCO = (HSI / M) * N = 4МГц * 85 = 340 МГц (в допустимом диапазоне 64..344 МГц)
+  RCC->PLLCFGR = (RCC_PLLCFGR_PLLSRC_HSI |        // Источник: HSI (16 МГц)
+                  (3 << RCC_PLLCFGR_PLLM_Pos) |   // Предделитель M = 4 (записывается 3)
+                  (85 << RCC_PLLCFGR_PLLN_Pos) |  // Множитель N = 85
+                  RCC_PLLCFGR_PLLREN);            // Включить главный выход PLL 'R'
+                                                  // PLLR divider = 2 (по умолчанию, запись 0)
+  
+  // 9. Включить PLL и дождаться его готовности.
+  RCC->CR |= RCC_CR_PLLON;
+  while(!(RCC->CR & RCC_CR_PLLRDY));
+  
+  // 10. Переключить системные часы (SYSCLK) на PLL.
+  MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_PLL);
+  // Ожидаем подтверждения, что система действительно переключилась на PLL.
+  while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
+  
+  // 11. Обновить глобальную переменную с частотой ядра.
+  // Это необходимо для корректной работы функций HAL/CMSIS (например, для настройки SysTick).
+  SystemCoreClock = 170000000UL;
+}
+
+
+
 

Source/Core/system_stm32g4xx.h

@@ -85,6 +85,7 @@ extern const uint8_t  APBPrescTable[8];     /*!< APB prescalers table values */
 
 extern void SystemInit(void);
 extern void SystemCoreClockUpdate(void);
+extern void SystemClock_Config();
 /**
   * @}
   */

Source/Devices/ICM20948.cpp

@@ -0,0 +1,165 @@
+#include "ICM20948.h"
+#include "i2C.h"
+#include "MathFunc.h"
+#include "Biquad.h"
+#include <cmath>
+
+static BiquadFilter FiltAcc[3];
+static BiquadFilter FiltGyr[3];
+
+static IMU_Proc IMU_DoneProc;
+
+static long GyroShift[3]{0, 0, 0};
+
+struct IMU_DataBuffer
+{
+  short ax, ay, az, gx, gy, gz, temp, mx, my, mz;
+};
+
+static inline void IMU_Calibrate(IMU_DataBuffer& imu, long GyrLim, long AccZero, long AccMax);
+static void IMU_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size);
+
+static unsigned char IMU_Buffer[32];
+static I2C_Request IMU_Device = {&IMU_CallbackProc, IMU_Buffer, sizeof(IMU_Buffer), 0 };
+
+void ICM20948::Init()
+{
+  lpf2p_init(&FiltAcc[0], 500.0f, 25.0f);
+  lpf2p_init(&FiltAcc[1], 500.0f, 25.0f);
+  lpf2p_init(&FiltAcc[2], 500.0f, 25.0f);
+  
+  lpf2p_init(&FiltGyr[0], 500.0f, 25.0f);
+  lpf2p_init(&FiltGyr[1], 500.0f, 25.0f);
+  lpf2p_init(&FiltGyr[2], 500.0f, 25.0f);
+  
+  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
+  
+  IMU_SetReg(IMU_Addr, 0x06, 0x80);
+  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
+  
+  IMU_SetReg(IMU_Addr, 0x06, 0x01);
+  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
+  
+  IMU_SetBank(2);
+  
+  IMU_SetReg(IMU_Addr, 0x01, GYRO_DLPFCFG_73 | GYRO_FS_SEL_2000 | GYRO_FCHOICE_ON); // GYRO_CONFIG_1
+  IMU_SetReg(IMU_Addr, 0x14, ACCEL_DLPFCFG_69 | ACCEL_FS_SEL_8 | ACCEL_FCHOICE_ON); // ACCEL_CONFIG
+  
+  IMU_SetBank(0);
+  
+  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
+}
+
+void ICM20948::IMU_SetBank(unsigned char Bank)
+{
+  unsigned char reg[2];
+  reg[0] = 0x7F; reg[1] = Bank << 4;
+  I2C1_Write(IMU_Addr, reg, 2);
+  I2C1_Stop();
+}
+
+void ICM20948::Get(IMU_Data& Data)
+{
+  struct { short ax; short ay; short az; short gx; short gy; short gz; short temp; short mx; short my; short mz; } data;
+  
+  I2C1_Write(IMU_Addr, IMU_Read_Reg);
+  I2C1_Read(IMU_Addr, &data, sizeof(data));
+  I2C1_Stop();
+  
+  Data.Temp = 21 + (((long)Rev16(data.temp)) * 128) / 42735;
+  
+  Data.Acc.X = Rev16(data.ax);
+  Data.Acc.Y = Rev16(data.ay);
+  Data.Acc.Z = Rev16(data.az);
+  
+  Data.Gyr.X = Rev16(data.gx);
+  Data.Gyr.Y = Rev16(data.gy);
+  Data.Gyr.Z = Rev16(data.gz);
+
+  Data.Mag.X = data.mx;
+  Data.Mag.Y = data.my;
+  Data.Mag.Z = data.mz;
+}
+
+void ICM20948::GetAsync(const IMU_Proc& DoneProc)
+{
+  IMU_DoneProc = DoneProc;
+  I2C1_Trans(&IMU_Device, IMU_Addr, &IMU_Read_Reg, 1, sizeof(IMU_DataBuffer));
+}
+
+
+static void IMU_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size)
+{
+  IMU_DataBuffer& data = *(IMU_DataBuffer*)Data;
+  
+  data.ax = Rev16(data.ax);
+  data.ay = Rev16(data.ay);
+  data.az = Rev16(data.az);
+
+  data.gx = Rev16(data.gx);
+  data.gy = Rev16(data.gy);
+  data.gz = Rev16(data.gz);
+  
+  data.ax = biquad_apply(&FiltAcc[0], data.ax);
+  data.ay = biquad_apply(&FiltAcc[1], data.ay);
+  data.az = biquad_apply(&FiltAcc[2], data.az);
+
+  data.gx = biquad_apply(&FiltGyr[0], data.gx);
+  data.gy = biquad_apply(&FiltGyr[1], data.gy);
+  data.gz = biquad_apply(&FiltGyr[2], data.gz);
+  
+  static IMU_DataBuffer test = *(IMU_DataBuffer*)Data;
+  
+  if (CalibDataIMU.AllowedCalib) IMU_Calibrate(data, 50, 4096, 500);
+  
+  static IMU_Data result;
+  
+  static float alpha = 0.01f;
+  
+  result.RawAcc.X = (result.RawAcc.X * (1.0f - alpha)) + test.ax * alpha;
+  result.RawAcc.Y = (result.RawAcc.Y * (1.0f - alpha)) + test.ay * alpha;
+  result.RawAcc.Z = (result.RawAcc.Z * (1.0f - alpha)) + test.az * alpha;
+
+  result.RawGyr.X = (result.RawGyr.X * (1.0f - alpha)) + test.gx * alpha;
+  result.RawGyr.Y = (result.RawGyr.Y * (1.0f - alpha)) + test.gy * alpha;
+  result.RawGyr.Z = (result.RawGyr.Z * (1.0f - alpha)) + test.gz * alpha;
+  
+  result.Temp = 21 + (((long)Rev16(data.temp)) * 128) / 42735; // 21+(temperature / 333.87)
+  
+  result.Acc.X = Normalize(data.ax, 1.0f, CalibDataIMU.Data.Acc.X[0], CalibDataIMU.Data.Acc.X[1]);
+  result.Acc.Y = Normalize(data.ay, 1.0f, CalibDataIMU.Data.Acc.Y[0], CalibDataIMU.Data.Acc.Y[1]);
+  result.Acc.Z = Normalize(data.az, 1.0f, CalibDataIMU.Data.Acc.Z[0], CalibDataIMU.Data.Acc.Z[1]);
+
+  result.Gyr.X = ((float)(data.gx - GyroShift[0])) / 16.384f;
+  result.Gyr.Y = ((float)(data.gy - GyroShift[1])) / 16.384f;
+  result.Gyr.Z = ((float)(data.gz - GyroShift[2])) / 16.384f;
+  
+  IMU_DoneProc(result);
+}
+
+static inline void IMU_Calibrate(IMU_DataBuffer& imu, long GyrLim, long AccZero, long AccMax)
+{
+  long len = imu.gx * imu.gx + imu.gy * imu.gy + imu.gz * imu.gz;
+  if (len > GyrLim * GyrLim) return;
+  
+  static float x = imu.gx, y = imu.gy, z = imu.gz;
+  
+  const float alpha = 0.001f;
+  
+  x = x * (1.0f - alpha) + alpha * imu.gx;
+  y = y * (1.0f - alpha) + alpha * imu.gy;
+  z = z * (1.0f - alpha) + alpha * imu.gz;
+  
+  GyroShift[0] = x;
+  GyroShift[1] = y;
+  GyroShift[2] = z;
+  
+  len = imu.ax * imu.ax + imu.ay * imu.ay + imu.az * imu.az;
+  if (abs(len - AccZero * AccZero) > (AccMax * AccMax)) return;
+  
+  len = sqrtf(len);
+  
+  static float acc = len;
+  
+  acc = acc * (1.0f - alpha) + alpha * len;
+}

Source/Devices/ICM20948.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#ifndef ICM20948_H
+#define ICM20948_H
+
+#include "IIMU.h"
+
+#define ACCEL_FCHOICE_OFF 0
+#define ACCEL_FCHOICE_ON  1
+
+#define ACCEL_FS_SEL_2  0<<1
+#define ACCEL_FS_SEL_4  1<<1
+#define ACCEL_FS_SEL_8  2<<1
+#define ACCEL_FS_SEL_16 3<<1
+
+#define ACCEL_DLPFCFG_265 0<<3
+#define ACCEL_DLPFCFG_136 2<<3
+#define ACCEL_DLPFCFG_69  3<<3
+#define ACCEL_DLPFCFG_34  4<<3
+#define ACCEL_DLPFCFG_17  5<<3
+#define ACCEL_DLPFCFG_8   6<<3
+#define ACCEL_DLPFCFG_500 7<<3
+
+#define GYRO_FCHOICE_OFF 0
+#define GYRO_FCHOICE_ON  1
+
+#define GYRO_FS_SEL_250  0<<1
+#define GYRO_FS_SEL_500  1<<1
+#define GYRO_FS_SEL_1000 2<<1
+#define GYRO_FS_SEL_2000 3<<1
+
+#define GYRO_DLPFCFG_230 0<<3
+#define GYRO_DLPFCFG_188 1<<3
+#define GYRO_DLPFCFG_154 2<<3
+#define GYRO_DLPFCFG_73  3<<3
+#define GYRO_DLPFCFG_36  4<<3
+#define GYRO_DLPFCFG_18  5<<3
+#define GYRO_DLPFCFG_9   6<<3
+#define GYRO_DLPFCFG_377 7<<3
+
+#define MAG_DATARATE_10_HZ 0x02
+#define MAG_DATARATE_20_HZ 0x04
+#define MAG_DATARATE_50_HZ 0x06
+#define MAG_DATARATE_100_HZ 0x08
+
+class ICM20948 : IIMU
+{
+public:
+  ICM20948(){};
+  ~ICM20948(){};
+  
+  unsigned char IMU_Addr = 0x68;
+  unsigned char IMU_Read_Reg = 0x2D;
+  
+  void Init();  
+  void IMU_SetBank(unsigned char Bank);
+  void Get(IMU_Data& Data);
+  void GetAsync(const IMU_Proc& DoneProc);
+};
+
+#endif

Source/Devices/IIMU.cpp

@@ -0,0 +1,54 @@
+#include "IIMU.h"
+#include "stm32g4xx.h"
+#include "I2C.h"
+#include "ICM20948.h"
+#include "GPIO.h"
+
+
+IMU_Info IMUInfo;
+
+IMU_Calib_Data CalibDataIMU;
+
+void IMU_InitPower()
+{
+  GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_C | GPIO_OUTPUT | GPIO_SET); // POWER ON 3V3 (nPC13)
+  for (volatile int i = 0; i < 1000000; i ++) {};
+  GPIOC->BSRR = GPIO_BSRR_BR13;
+  for (volatile int i = 0; i < 1000000; i ++) {};
+}
+
+void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value)
+{
+  unsigned char reg[2];
+  reg[0] = Reg; reg[1] = Value;
+  I2C1_Write(Addr, reg, 2);
+  I2C1_Stop();
+}
+
+unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg)
+{
+  I2C1_Write(Addr, Reg);
+  I2C1_Read(Addr, &Reg, 1);
+  I2C1_Stop();
+  return Reg;
+}
+
+IIMU* TryFindIMU(bool& find)
+{
+  I2C1_Init();
+  IIMU* Imu = nullptr;
+  for (int i = 0; i < sizeof(IMUInfo.Addrs); ++i)
+  {
+    find = I2C1_CheckDeviceWhoAmI(IMUInfo.Addrs[i], IMUInfo.WhoIAMReg[i], IMUInfo.ExpectedIDs[i]);
+    if (find && i == 0)
+    {
+      static ICM20948 imuICM20948;
+      Imu = (IIMU*)&imuICM20948;
+      break;
+    }
+    for (int a = 0; a < 10000000; ++a) { asm volatile("NOP"); }
+  }
+  if (find) Imu->Init();
+  return Imu;
+}
+

Source/Devices/IIMU.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#ifndef IIMU_H
+#define IIMU_H
+
+#pragma pack(push, 1)
+struct IMU_Data
+{
+  long Temp;
+  struct { float X, Y, Z; } Acc;
+  struct { float X, Y, Z; } RawAcc;
+  struct { float X, Y, Z; } Gyr;
+  struct { float X, Y, Z; } RawGyr;
+  struct { float X, Y, Z; } Mag;
+  struct { float X, Y, Z; } RawMag;
+};
+
+struct XYZ_Calib
+{
+  short X[2] = {-4096, 4096};
+  short Y[2] = {-4096, 4096};
+  short Z[2] = {-4096, 4096};
+};
+
+struct XYZ_IMU_DATA
+{
+  char Writed = sizeof(XYZ_IMU_DATA);
+  XYZ_Calib Acc;
+};
+
+struct IMU_Calib_Data
+{
+  bool AllowedCalib = false;
+  bool AccCalibNeed = true;
+  XYZ_IMU_DATA Data;
+};
+
+struct IMU_Info
+{
+  unsigned char Addrs[1]        = {0x68};
+  unsigned char WhoIAMReg[1]    = {0x00};
+  unsigned char ExpectedIDs[1]  = {0xEA};
+};
+#pragma pack(pop)
+
+typedef void (*IMU_Proc)(IMU_Data&);
+
+extern IMU_Calib_Data CalibDataIMU;
+
+void IMU_InitPower();
+
+void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value);
+
+unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg);
+
+class IIMU
+{
+protected:
+  virtual ~IIMU() = default;
+public:
+  virtual void Init() = 0;
+  virtual void Get(IMU_Data& Data) = 0;
+  virtual void GetAsync(const IMU_Proc& DoneProc) = 0;
+};
+
+IIMU* TryFindIMU(bool& find);
+
+#endif

Source/Devices/Motors.cpp

@@ -1,6 +1,7 @@
-#include "motors.h"
+#include "Motors.h"
-#include "stm32g431xx.h"
 
+
+//======================Init======================
 void motors_init()
 {
 	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
@@ -141,18 +142,20 @@ void motors_tim2_ch1_2_init()
 	// set counter enable
 	TIM2->CR1 |= TIM_CR1_CEN;
 }
+//======================/Init======================
 
-int16_t T;
-int16_t P;
-int16_t R;
-int16_t Y;
 
-int16_t m1;
+short T;
-int16_t m2;
+short P;
-int16_t m3;
+short R;
-int16_t m4;
+short Y;
 
-void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed)
+short m1;
+short m2;
+short m3;
+short m4;
+
+void motors_set_throttle_mix(const short throttle, const control_channels_t* chs, const bool armed)
 {
 	T = throttle;
 	P = (int16_t) chs->pitch;
@@ -170,7 +173,7 @@ void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* c
 	motor_set_throttle(4, m4, armed);
 }
 
-void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed)
+void motor_set_throttle(unsigned char motor_number, unsigned short us, bool armed)
 {
 	if (armed && us < 1050) us = 1050;
 	if (us > 2000) us = 2000;
@@ -200,31 +203,17 @@ void motors_turn_off()
 	motor_set_throttle(4, 900, 0);
 }
 
-void motor1_set_throttle(int16_t throttle)
-{
-	if (throttle < 1000) throttle = 1000;
-	if (throttle > 2000) throttle = 2000;
-	TIM1->CCR3 = throttle;
-}
 
-void motor2_set_throttle(int16_t throttle)
-{
-	if (throttle < 1000) throttle = 1000;
-	if (throttle > 2000) throttle = 2000;
-	TIM1->CCR4 = throttle;
-}
 
-void motor3_set_throttle(int16_t throttle)
-{
-	if (throttle < 1000) throttle = 1000;
-	if (throttle > 2000) throttle = 2000;
-	TIM2->CCR1 = throttle;
-}
 
-void motor4_set_throttle(int16_t throttle)
+
-{
+
-	if (throttle < 1000) throttle = 1000;
+
-	if (throttle > 2000) throttle = 2000;
+
-	TIM2->CCR2 = throttle;
+
-}
+
+
+
+
+
 

Source/Devices/Motors.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include "pid.h"
+
+void motors_init();
+void motor_gpio_tim1_ch3_init();
+void motor_gpio_tim1_ch4_init();
+void motors_tim1_ch3_4_init();
+
+void motor_gpio_tim2_ch1_init();
+void motor_gpio_tim2_ch2_init();
+void motors_tim2_ch1_2_init();
+
+void motors_set_throttle_mix(const short throttle, const control_channels_t* chs, const bool armed);
+void motor_set_throttle(unsigned char motor_number, unsigned short us, bool armed);
+void motors_turn_off();
+
+#endif

Source/Devices/RadioReceiver.cpp

@@ -1,12 +1,11 @@
-#include "radio_receiver.h"
+#include "RadioReceiver.h"
 
-
+volatile unsigned short sbus_channels[16] = {0};
-volatile uint16_t sbus_channels[16] = {0};
+volatile unsigned char  sbus_buffer[SBUS_FRAME_SIZE] = {0};
-volatile uint8_t sbus_buffer[SBUS_FRAME_SIZE] = {0};
+volatile unsigned char  sbus_index = 0;
-volatile uint8_t sbus_index = 0;
+volatile unsigned char  sbus_failsafe = 0;
-volatile uint8_t sbus_failsafe = 0;
+volatile unsigned char  sbus_frame_lost = 0;
-volatile uint8_t sbus_frame_lost = 0;
+volatile bool           sbus_frame_ready = 0;
-volatile uint8_t sbus_frame_ready = 0;
 
 void receiver_gpio_init()
 {
@@ -18,6 +17,9 @@ void receiver_gpio_init()
 	GPIOA->AFR[0] &= ~(0xF << (3 * 4));
 	GPIOA->AFR[0] |= 12 << (3 * 4);
 	
+  GPIOA->OSPEEDR &= ~(3 << (3 * 2));
+  GPIOA->OSPEEDR |= 2 << (3 * 2);
+  
 	// pull-up
 	GPIOA->PUPDR &= ~(3 << (3 * 2));
 	GPIOA->PUPDR |= 1 << (3 * 2);
@@ -38,7 +40,7 @@ void receiver_uart_init()
 	LPUART1->CR2 = 0;
 	LPUART1->CR3 = 0;
 	
-	LPUART1->BRR = (256 * 16000000UL) / 100000UL;
+	LPUART1->BRR = SystemCoreClock / (100000UL / 256);
 	
 	// parity control enable
 	LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
@@ -61,9 +63,12 @@ void receiver_uart_init()
 	// interrupt generated whenever ORE = 1 or RXNE = 1
 	LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
   
+  LPUART1->CR3 |= USART_CR3_OVRDIS;
+	
 	// uart enable
 	LPUART1->CR1 |= USART_CR1_UE;
 	
+  NVIC_SetPriority(LPUART1_IRQn, 0);
 	NVIC_EnableIRQ(LPUART1_IRQn);
 }
 
@@ -73,11 +78,11 @@ void receiver_init()
 	receiver_uart_init();
 }
 
-void LPUART1_IRQHandler()
+extern "C" void LPUART1_IRQHandler()
 {  
 	if (LPUART1->ISR & USART_ISR_RXNE)
 	{
-		uint8_t b = LPUART1->RDR;
+		char b = LPUART1->RDR;
 		
 		if (b == SBUS_START_BYTE)
 			sbus_index = 0;
@@ -157,12 +162,12 @@ rc_channels normalize_channels(rc_channels chs)
 	return chs;
 }
 
-int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
+short int_mapping(short x, short in_min, short in_max, short out_min, short out_max)
 {
 	return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
 }
 
-int8_t bool_mapping_gt(int16_t x, int16_t boundary)
+bool bool_mapping_gt(short x, short boundary)
 {
 	return x >= boundary;
 }

Source/Devices/RadioReceiver.h

@@ -1,31 +1,31 @@
+#pragma once
+
 #ifndef RADIO_RECEIVER_H
 #define RADIO_RECEIVER_H
 
 #include "stm32g431xx.h"
-#include <stdint.h>
 
 #define SBUS_FRAME_SIZE 25
 #define SBUS_START_BYTE 0X0F
 
-typedef struct
+struct rc_channels
 {
-	int16_t rc_roll;     // -500 - 500
+	short rc_roll;      // -500 - 500
-	int16_t rc_pitch;    // -500 - 500
+	short rc_pitch;     // -500 - 500
-	int16_t rc_throttle; // 1000 - 2000
+	short rc_throttle;  // 1000 - 2000
-	int16_t rc_yaw;      // -500 - 500
+	short rc_yaw;       // -500 - 500
-	int16_t rc_armed;    // 0/1
+	bool rc_armed;      // 0/1
-} rc_channels;
+};
 
 void receiver_gpio_init();
 void receiver_lpuart_clock_init();
 void receiver_uart_init();
 void receiver_init();
-void LPUART1_IRQHandler();
 void receiver_update(rc_channels* chs);
 void receiver_parse_frame();
 rc_channels normalize_channels(rc_channels chs);
-int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max);
+short int_mapping(short x, short in_min, short in_max, short out_min, short out_max);
-int8_t bool_mapping_gt(int16_t x, int16_t boundary);
+bool bool_mapping_gt(short x, short boundary);
 
 
 void led_init();

Source/Drivers/GPIO.cpp

@@ -0,0 +1,35 @@
+#include "GPIO.H"
+#include "stm32g4xx.h"
+
+void GPIO_InitPin(unsigned long Pin)
+{
+  unsigned long port = (Pin & 0x000000F0UL) >> 4;
+  GPIO_TypeDef* gpio = (GPIO_TypeDef*)(((unsigned char*)GPIOA) + (port * 0x0400));
+  unsigned long rcc = 1UL << port;
+  unsigned long pin = Pin & 0x0000000FUL;
+  unsigned long af = (Pin & 0x0F000000UL) >> 24;
+  unsigned long pupd = (Pin & 0x00F00000UL) >> 20;
+  unsigned long ospeed = (Pin & 0x000F0000UL) >> 16;
+  unsigned long mode = (Pin & 0x0000F000UL) >> 12;
+  unsigned long otype = (Pin & 0x00000100UL) >> 8;
+  unsigned long set = (Pin & 0x00000200UL) >> 9;
+  
+  if (!(RCC->AHB2ENR & rcc)) RCC->AHB2ENR |= rcc;
+  
+  gpio->AFR[pin >> 3] &= ~(0x0000000FUL << ((pin & 0x07) * 4));
+  gpio->AFR[pin >> 3] |= af << ((pin & 0x07) * 4);
+  
+  gpio->OSPEEDR &= ~(0x00000003UL << (pin * 2));
+  gpio->OSPEEDR |= ospeed << (pin * 2);
+  
+  gpio->OTYPER &= ~(0x00000001UL << pin);
+  gpio->OTYPER |= otype << pin;
+  
+  gpio->PUPDR &= ~(0x00000003UL << (pin * 2));
+  gpio->PUPDR |= pupd << (pin * 2);
+  
+  gpio->BSRR = 1 << (set ? pin : pin+16);
+  
+  gpio->MODER &= ~(0x00000003UL << (pin * 2));
+  gpio->MODER |= mode << (pin * 2);
+}

Source/Drivers/GPIO.h

@@ -0,0 +1,74 @@
+#pragma once
+
+#ifndef GPIO_H
+#define GPIO_H
+
+#define GPIO_PIN_0  0x00000000UL
+#define GPIO_PIN_1  0x00000001UL
+#define GPIO_PIN_2  0x00000002UL
+#define GPIO_PIN_3  0x00000003UL
+#define GPIO_PIN_4  0x00000004UL
+#define GPIO_PIN_5  0x00000005UL
+#define GPIO_PIN_6  0x00000006UL
+#define GPIO_PIN_7  0x00000007UL
+#define GPIO_PIN_8  0x00000008UL
+#define GPIO_PIN_9  0x00000009UL
+#define GPIO_PIN_10 0x0000000AUL
+#define GPIO_PIN_11 0x0000000BUL
+#define GPIO_PIN_12 0x0000000CUL
+#define GPIO_PIN_13 0x0000000DUL
+#define GPIO_PIN_14 0x0000000EUL
+#define GPIO_PIN_15 0x0000000FUL
+
+#define GPIO_PORT_A 0x00000000UL
+#define GPIO_PORT_B 0x00000010UL
+#define GPIO_PORT_C 0x00000020UL
+#define GPIO_PORT_D 0x00000030UL
+#define GPIO_PORT_E 0x00000040UL
+#define GPIO_PORT_F 0x00000070UL
+#define GPIO_PORT_G 0x00000080UL
+#define GPIO_PORT_H 0x00000090UL
+#define GPIO_PORT_I 0x000000A0UL
+#define GPIO_PORT_J 0x000000B0UL
+#define GPIO_PORT_K 0x000000C0UL
+
+#define GPIO_PUSHPULL  0x00000000UL
+#define GPIO_OPENDRAIN 0x00000100UL
+
+#define GPIO_RESET 0x00000000UL
+#define GPIO_SET   0x00000200UL
+
+#define GPIO_INPUT  0x00000000UL
+#define GPIO_OUTPUT 0x00001000UL
+#define GPIO_ALTER  0x00002000UL
+#define GPIO_ANALOG 0x00003000UL
+
+#define GPIO_OSPEED_LOW    0x00000000UL
+#define GPIO_OSPEED_MEDIUM 0x00010000UL
+#define GPIO_OSPEED_FAST   0x00020000UL
+#define GPIO_OSPEED_HIGH   0x00030000UL
+
+#define GPIO_NOPUPD   0x00000000UL
+#define GPIO_PULLUP   0x00100000UL
+#define GPIO_PULLDOWN 0x00200000UL
+
+#define GPIO_AF0  0x00000000UL
+#define GPIO_AF1  0x01000000UL
+#define GPIO_AF2  0x02000000UL
+#define GPIO_AF3  0x03000000UL
+#define GPIO_AF4  0x04000000UL
+#define GPIO_AF5  0x05000000UL
+#define GPIO_AF6  0x06000000UL
+#define GPIO_AF7  0x07000000UL
+#define GPIO_AF8  0x08000000UL
+#define GPIO_AF9  0x09000000UL
+#define GPIO_AF10 0x0A000000UL
+#define GPIO_AF11 0x0B000000UL
+#define GPIO_AF12 0x0C000000UL
+#define GPIO_AF13 0x0D000000UL
+#define GPIO_AF14 0x0E000000UL
+#define GPIO_AF15 0x0F000000UL
+
+void GPIO_InitPin(unsigned long Pin);
+
+#endif

Source/Drivers/HAL_M/GPIO/Inc/HAL_GPIO.h

@@ -1,19 +0,0 @@
-#ifndef HAL_GPIO
-#define HAL_GPIO
-
-#include "stm32g431xx.h"
-
-// I/O MODES
-#define INPUT_MODE      ((uint32_t) 0x00)       // input mode
-#define OUTPUT_MODE     ((uint32_t) 0x01)       // general purpose output mode
-#define ALT_FUNC_MODE   ((uint32_t) 0x02)       // alternative function mode
-#define ANALOG_MODE     ((uint32_t) 0x03)       // alanog mode (reset state)
-
-// CLOCK ENABLING
-#define GPIO_CLOCK_EN_GPIOA     (RCC->AHB2ENR |= (1 << 0))
-#define GPIO_CLOCK_EN_GPIOB     (RCC->AHB2ENR |= (1 << 1))
-#define GPIO_CLOCK_EN_GPIOC     (RCC->AHB2ENR |= (1 << 2))
-
-
-
-#endif

Source/Drivers/HAL_M/GPIO/Src/HAL_GPIO.c

@@ -1,3 +0,0 @@
-#include "GPIO/Inc/HAL_GPIO.h"
-
-

Source/Drivers/I2C.cpp

@@ -0,0 +1,377 @@
+#include "I2C.h"
+#include "stm32g4xx.h"
+#include "GPIO.h"
+#include <string.h>
+
+struct I2C_Data
+{
+  I2C_TypeDef* I2C;
+  IRQn_Type IRQn;
+  
+  I2C_Request *Head, *Device;
+  
+  unsigned char Index;
+};
+
+static I2C_Data I2C1_Data = { I2C1, I2C1_EV_IRQn, 0, 0, 0 };
+
+
+static void EV_IRQHandler(I2C_Data& I2C)
+{
+  if ((I2C.I2C->CR1 & I2C_CR1_TXIE) && (I2C.I2C->ISR & I2C_ISR_TXE))
+  {
+    const I2C_Request& device = *I2C.Device;
+    
+    I2C.I2C->TXDR = device.Buffer[I2C.Index++];
+    if (I2C.Index < device.Write) return;
+    I2C.I2C->CR1 = I2C_CR1_TCIE | I2C_CR1_PE;
+    I2C.Index = 0;
+    return;
+  }
+  
+  if ((I2C.I2C->CR1 & I2C_CR1_TCIE) && (I2C.I2C->ISR & I2C_ISR_TC))
+  {
+    const I2C_Request& device = *I2C.Device;
+    
+    if (device.Read)
+    {
+      I2C.I2C->CR2 &= ~I2C_CR2_NBYTES_Msk;
+      I2C.I2C->CR2 |=  I2C_CR2_START | I2C_CR2_RD_WRN | (((unsigned long)device.Read)<<I2C_CR2_NBYTES_Pos);
+      I2C.I2C->CR1  =  I2C_CR1_RXIE | I2C_CR1_PE;
+    }
+    else
+    {
+      I2C.I2C->CR2 |=  I2C_CR2_STOP;
+      I2C.I2C->CR1  =  I2C_CR1_STOPIE | I2C_CR1_PE;
+    }
+    return;
+  }
+  
+  if ((I2C.I2C->CR1 & I2C_CR1_RXIE) && (I2C.I2C->ISR & I2C_ISR_RXNE))
+  {
+    const I2C_Request& device = *I2C.Device;
+    
+    device.Buffer[I2C.Index++] = I2C.I2C->RXDR;
+    if(I2C.Index<device.Read) return;
+    I2C.I2C->CR1  =  I2C_CR1_STOPIE | I2C_CR1_PE;
+    I2C.I2C->CR2 |=  I2C_CR2_STOP;
+    I2C.Index=0;
+    return;
+  }
+  
+  if ((I2C.I2C->CR1 & I2C_CR1_STOPIE) && (I2C.I2C->ISR & I2C_ISR_STOPF))
+  {
+    I2C_Request& device = *I2C.Device;
+    
+    I2C.I2C->ICR = I2C_ICR_STOPCF;
+    I2C.I2C->CR1 = I2C_CR1_PE;
+    I2C.I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
+    unsigned char addr = device.Address;
+    device.Address = 0;
+    
+    I2C.Device=device.Next;
+    
+    if(device.CallbackProc) device.CallbackProc(addr, device.Buffer, device.Read);
+  }
+    
+  if (I2C.I2C->CR1 == I2C_CR1_PE)
+  {
+    if(!I2C.Device)
+    {
+      do
+      {
+        I2C.Device = (I2C_Request*)__LDREXW((volatile unsigned int*)&I2C.Head);
+      } while(__STREXW(0, (volatile unsigned int*)&I2C.Head));
+      
+      if(!I2C.Device) return;
+    }
+    
+    const I2C_Request& device = *I2C.Device;
+    
+    if (!(I2C.I2C->CR2 & I2C_CR2_SADD_Msk))
+    {
+      I2C.I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
+      if(device.Write)
+      {
+        I2C.I2C->CR2 |= I2C_CR2_START | (device.Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)device.Write)<<I2C_CR2_NBYTES_Pos);
+        I2C.I2C->CR1 = I2C_CR1_TXIE | I2C_CR1_PE;
+      }
+      else 
+      {
+        I2C.I2C->CR2 |= I2C_CR2_START | (device.Address << (I2C_CR2_SADD_Pos + 1)) | I2C_CR2_RD_WRN | (((unsigned long)device.Read)<<I2C_CR2_NBYTES_Pos);
+        I2C.I2C->CR1 = I2C_CR1_RXIE | I2C_CR1_PE;
+      }
+    }
+    return;
+  }
+}
+
+
+extern "C" void I2C1_EV_IRQHandler()
+{
+  EV_IRQHandler(I2C1_Data);
+}
+
+
+static void Init(I2C_TypeDef* I2C, IRQn_Type IRQn)
+{
+  //I2C->TIMINGR = 0x00303D5BUL; // 100kHz
+  I2C->TIMINGR = 0x10802D9BUL; // 400kHz
+  
+  I2C->CR1 = I2C_CR1_PE;
+
+  while(I2C->ISR & I2C_ISR_BUSY) { }
+  
+  NVIC_SetPriority(IRQn, 1);
+  NVIC_EnableIRQ(IRQn);
+}
+
+
+static bool I2C_CheckDeviceWhoAmI(I2C_TypeDef* I2C, unsigned char Address, unsigned char WhoAmI_Reg, unsigned char Expected_ID)
+{
+  if (I2C->ISR & I2C_ISR_BUSY) return false;
+
+  I2C->ICR = I2C_ICR_STOPCF | I2C_ICR_NACKCF;
+
+  uint32_t timeout = 100000;
+
+  I2C->CR2 = (Address << 1) | (1 << 16) | I2C_CR2_START;
+
+  while (!(I2C->ISR & I2C_ISR_TXIS))
+  {
+    if (I2C->ISR & I2C_ISR_NACKF)
+    {
+      I2C->ICR = I2C_ICR_NACKCF;
+      return false;
+    }
+    if ((timeout--) == 0) return false;
+  }
+
+  I2C->TXDR = WhoAmI_Reg;
+
+  timeout = 100000;
+  while (!(I2C->ISR & I2C_ISR_TC))
+  {
+    if ((timeout--) == 0) return false;
+  }
+
+  I2C->CR2 = (Address << 1) | (1 << 16) | I2C_CR2_RD_WRN | I2C_CR2_START | I2C_CR2_AUTOEND;
+
+  timeout = 100000;
+  while (!(I2C->ISR & I2C_ISR_RXNE))
+  {
+    if (I2C->ISR & I2C_ISR_NACKF)
+    {
+      I2C->ICR = I2C_ICR_NACKCF;
+      return false;
+    }
+    if ((timeout--) == 0) return false;
+  }
+
+  unsigned char received_id = (unsigned char)I2C->RXDR;
+
+  timeout = 100000;
+  while (!(I2C->ISR & I2C_ISR_STOPF))
+  {
+    if ((timeout--) == 0) return false;
+  }
+
+  I2C->ICR = I2C_ICR_STOPCF;
+  I2C->CR2 = 0;
+
+  if (received_id == Expected_ID)
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+
+static bool I2C_CheckDevice(I2C_TypeDef* I2C, unsigned char Address)
+{
+  if (I2C->ISR & I2C_ISR_BUSY) return false;
+
+  I2C->ICR = I2C_ICR_STOPCF | I2C_ICR_NACKCF;
+
+  I2C->CR2 = (Address << 1) | I2C_CR2_AUTOEND | I2C_CR2_START;
+
+  uint32_t timeout = 100000;
+
+  while (!(I2C->ISR & I2C_ISR_STOPF))
+  {
+    if ((timeout--) == 0) return false;
+  }
+
+  bool device_present = false;
+
+  if (I2C->ISR & I2C_ISR_NACKF)
+  {
+    I2C->ICR = I2C_ICR_NACKCF;
+    device_present = false;
+  }
+  else device_present = true;
+
+  I2C->ICR = I2C_ICR_STOPCF;
+  I2C->CR2 = 0;
+
+  return device_present;
+}
+
+
+void I2C1_Init()
+{
+  if (RCC->APB1ENR1 & RCC_APB1ENR1_I2C1EN) return;
+
+  RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
+
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
+
+  GPIO_InitPin(GPIO_PIN_8 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
+  GPIO_InitPin(GPIO_PIN_9 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
+
+  Init(I2C1, I2C1_EV_IRQn);
+}
+
+
+static void Stop(I2C_TypeDef* I2C)
+{
+  I2C->CR2 |= I2C_CR2_STOP;
+  while (!(I2C->ISR & I2C_ISR_STOPF)) { asm volatile("NOP"); }
+  I2C->ICR = I2C_ICR_STOPCF;
+  I2C->CR2 = 0;
+}
+
+
+static void Read(I2C_TypeDef* I2C, unsigned char Address, unsigned char* Data, unsigned char Size)
+{
+  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_NBYTES_Msk);
+  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | I2C_CR2_RD_WRN | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
+  I2C->CR2 |= I2C_CR2_START;
+  
+  while (Size--)
+  {
+    while (!(I2C->ISR & I2C_ISR_RXNE)) { }
+    *Data++ = I2C->RXDR;
+  }
+}
+
+
+static void Write(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data, unsigned char Size)
+{
+  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
+  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
+  I2C->CR2 |= I2C_CR2_START;
+
+  while (I2C->CR2 & I2C_CR2_START) { asm volatile("NOP"); }
+  
+  while (Size--)
+  {
+    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
+    I2C->TXDR = *Data++;
+  }
+  
+  while(!(I2C->ISR & I2C_ISR_TC)) { asm volatile("NOP"); }
+}
+
+
+static void Write2(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data1, unsigned char Size1, const unsigned char* Data2, unsigned char Size2)
+{
+  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
+  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size1+Size2)<<I2C_CR2_NBYTES_Pos);
+  I2C->CR2 |= I2C_CR2_START;
+  
+  while (Size1--)
+  {
+    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
+    I2C->TXDR = *Data1++;
+  }
+  
+  while (Size2--)
+  {
+    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
+    I2C->TXDR = *Data2++;
+  }
+  
+  while (!(I2C->ISR & I2C_ISR_TC)) { asm volatile("NOP"); }
+}
+
+
+static bool Trans(I2C_Data& I2C, I2C_Request* Request, unsigned char Address, const unsigned char* Data, unsigned char SizeWrite, unsigned char SizeRead)
+{
+  if (Request->Address || SizeWrite>Request->Size || SizeRead>Request->Size || (SizeWrite==0 && SizeRead==0)) return false;
+  
+  if (SizeWrite) memcpy(Request->Buffer, Data, SizeWrite);
+  Request->Write = SizeWrite;
+  Request->Read = SizeRead;
+  
+  Request->Address = Address;
+  
+  do
+  {
+    I2C_Request* dev = (I2C_Request*)__LDREXW((volatile unsigned int*)&I2C.Head);
+    Request->Next = dev;
+  } while (__STREXW((unsigned int)Request, (volatile unsigned int*)&I2C.Head));
+  
+  if (I2C.I2C->CR1 == I2C_CR1_PE) NVIC_SetPendingIRQ(I2C.IRQn);
+  
+  return true;
+}
+
+
+bool I2C1_Trans(I2C_Request* Request, unsigned char Address, const void* Data, unsigned char SizeWrite, unsigned char SizeRead)
+{
+  return Trans(I2C1_Data, Request, Address, (unsigned char*)Data, SizeWrite, SizeRead);
+}
+
+
+void I2C1_Write(unsigned char Address, unsigned char Data)
+{
+  Write(I2C1, Address, &Data, 1);
+}
+
+
+void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size)
+{
+  Write(I2C1, Address, (unsigned char*)Data, Size);
+}
+
+
+void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2)
+{
+  Write2(I2C1, Address, (unsigned char*)Data1, Size1, (unsigned char*)Data2, Size2);
+}
+
+
+void I2C1_Read(unsigned char Address, void* Data, unsigned char Size)
+{
+  Read(I2C1, Address, (unsigned char*)Data, Size);
+}
+
+
+bool I2C1_CheckDevice(unsigned char Address)
+{
+  return I2C_CheckDevice(I2C1, Address);
+}
+
+
+bool I2C1_CheckDeviceWhoAmI(unsigned char Address, unsigned char WhoAmI_Reg, unsigned char Expected_ID)
+{
+  return I2C_CheckDeviceWhoAmI(I2C1, Address, WhoAmI_Reg, Expected_ID);
+}
+
+
+void I2C1_Stop()
+{
+  Stop(I2C1);
+}
+
+
+
+
+
+
+
+

Source/Drivers/I2C.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#ifndef I2C_H
+#define I2C_H
+
+struct I2C_Request
+{
+  void (*CallbackProc)(unsigned char Address, const unsigned char* Data, unsigned char Size);
+  unsigned char* Buffer;
+  unsigned char Size;
+  
+  unsigned char Address;
+  unsigned char Write;
+  unsigned char Read;
+  
+  I2C_Request* Next;
+};
+
+void I2C1_Init();
+bool I2C1_Trans(I2C_Request* Request, unsigned char Address, const void* Data, unsigned char SizeWrite, unsigned char SizeRead);
+void I2C1_Write(unsigned char Address, unsigned char Data);
+void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size);
+void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2);
+void I2C1_Read(unsigned char Address, void* Data, unsigned char Size);
+bool I2C1_CheckDevice(unsigned char Address);
+bool I2C1_CheckDeviceWhoAmI(unsigned char Address, unsigned char WhoAmI_Reg, unsigned char Expected_ID);
+void I2C1_Stop();
+
+#endif

Source/Drivers/Tick.cpp

@@ -0,0 +1,60 @@
+#include "Tick.h"
+#include "stm32g4xx.h"
+
+static unsigned long Tick = 0;
+
+extern "C" void SysTick_Handler()
+{
+  Tick++;
+}
+
+void TICK_Init()
+{
+  SysTick->LOAD = SystemCoreClock / 1000;
+  SysTick->VAL = 0UL;
+  SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;
+
+  NVIC_SetPriority(SysTick_IRQn, 0);
+}
+
+unsigned long TICK_GetCount() // ms
+{
+  return Tick;
+}
+
+float TICK_GetTime() // s
+{
+  return ((float)Tick)/1000.0f;
+}
+
+void TICK_Delay(unsigned long us)
+{
+  long tick=SysTick->VAL;
+  long wait=us*(SystemCoreClock/1000000);
+  
+  long load = SysTick->LOAD;
+  
+  long val;
+  
+  do
+  {
+    val=SysTick->VAL;
+    
+    if(val>tick) val-=load;
+    
+  } while(tick-val < wait);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+

Source/Drivers/Tick.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#ifndef TICK_H
+#define TICK_H
+
+void TICK_Init();
+unsigned long TICK_GetCount();
+float TICK_GetTime();
+void TICK_Delay(unsigned long us); // 900 us maximum
+
+#endif

Source/Drivers/Tim.cpp

@@ -0,0 +1,69 @@
+#include "Tim.h"
+#include "stm32g4xx.h"
+
+static ProcTIM TIM6_Proc1 = 0;
+static ProcTIM TIM6_Proc2 = 0;
+static ProcTIM TIM7_Proc  = 0;
+
+extern "C" void TIM6_DAC_IRQHandler()
+{
+  static int proc=0;
+  TIM6->SR = 0;
+  
+  
+  if(proc>3) 
+  {
+    TIM6_Proc2();
+    proc=0;
+  }
+  else proc++;
+  
+  TIM6_Proc1();
+}
+
+extern "C" void TIM7_IRQHandler()
+{
+  TIM7->SR = 0;
+  TIM7_Proc();
+}
+
+void TIM6_Init(long Priority, unsigned long Freq, const ProcTIM& Proc1, const ProcTIM& Proc2)
+{
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
+
+  TIM6->CR1 = 0;
+  TIM6->ARR = 1000 - 1;
+  TIM6->PSC = (SystemCoreClock / 1000 / Freq) - 1;
+  TIM6->DIER = TIM_DIER_UIE;
+
+  TIM6_Proc1 = Proc1;
+  TIM6_Proc2 = Proc2;
+  
+  NVIC_SetPriority(TIM6_DAC_IRQn, Priority);
+  NVIC_EnableIRQ(TIM6_DAC_IRQn);
+}
+
+void TIM7_Init(long Priority, unsigned long Freq, const ProcTIM& Proc)
+{
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
+
+  TIM7->CR1 = 0;
+  TIM7->ARR = 1000 - 1;
+  TIM7->PSC = (SystemCoreClock / 1000 / Freq) - 1;
+  TIM7->DIER = TIM_DIER_UIE;
+
+  TIM7_Proc = Proc;
+  
+  NVIC_SetPriority(TIM7_IRQn, Priority);
+  NVIC_EnableIRQ(TIM7_IRQn);
+}
+
+void TIM6_Enable()
+{
+  TIM6->CR1 = TIM_CR1_CEN;
+}
+
+void TIM7_Enable()
+{
+  TIM7->CR1 = TIM_CR1_CEN;
+}

Source/Drivers/Tim.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#ifndef TIM_H
+#define TIM_H
+
+typedef void (*ProcTIM)();
+
+void TIM6_Init(long Priority, unsigned long Freq, const ProcTIM& Proc1, const ProcTIM& Proc2);
+void TIM7_Init(long Priority, unsigned long Freq, const ProcTIM& Proc);
+
+void TIM6_Enable();
+void TIM7_Enable();
+
+#endif

Source/INS/IRS.c

@@ -1,128 +0,0 @@
-#include "irs.h"
-#include <math.h>
-
-void IRS_init(IRS* irs)
-{
-    irs->q.w = 1.0f;
-    irs->q.x = 0.0f;
-    irs->q.y = 0.0f;
-    irs->q.z = 0.0f;
-}
-
-void IRS_update(IRS* irs, float dt)
-{
-	Quaternion qConjugate = QuatConjugate(&irs->q);
-	
-	// gyro update
-	Vector3 gyro = 
-	{
-		irs->gyro.x * DEG2RAD,
-		irs->gyro.y * DEG2RAD,
-		irs->gyro.z * DEG2RAD
-	};
-	
-	Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
-	g = QuatProd(&irs->q, &g);
-	g = QuatConstProd(&g, dt * 0.5f);
-	irs->q = QuatSum(&irs->q, &g);
-	irs->q = QuatNormalize(&irs->q, 1.0f);
-	
-    // /gyro update
-	
-	// accel update
-	
-	Vector3 accel = {irs->accel.x, irs->accel.y, irs->accel.z};
-	restoreQuat(irs, &accel);
-	
-	// /accel update
-}
-
-void restoreQuat(IRS* irs, const Vector3* accel)
-{
-	float len = lengthV3(accel);
-	
-	static float quat_acc_alpha = 0.03f;
-	static float quat_acc_max = 0.02f;
-	
-	float dyn = fabsf(len - 1.0f);
-	if (dyn > quat_acc_max) dyn = 1.0f; else dyn /= quat_acc_max;
-	
-	float gain = quat_acc_alpha * (1.0f - dyn);
-	
-	if (gain < 0.0001f) return;
-	
-	Vector3 acc = normalizeV3(accel, 1.0f);
-	
-	Vector3 est;
-	est.x = 2.0f * (irs->q.x * irs->q.z - irs->q.w * irs->q.y);
-	est.y = 2.0f * (irs->q.w * irs->q.x - irs->q.y * irs->q.z);
-	est.z = irs->q.w * irs->q.w - irs->q.x * irs->q.x - irs->q.y * irs->q.y + irs->q.z * irs->q.z;
-	
-	Vector3 cross = Cross(&acc, &est);
-	float dot = DotV3(&acc, &est);
-	
-	if (dot < 0.0f)
-	{
-		float error_len = lengthV3(&cross);
-		if (error_len < 0.001f) {
-			cross.x = 1.0f;
-			cross.y = 0.0f;
-			cross.z = 0.0f;
-		}
-		else
-		{
-			cross = constProdV3(&cross, 1.0f / error_len);
-		}
-	}
-	
-	Vector3 axis = constProdV3(&cross, gain * 0.5f);
-	
-	Quaternion correction =
-	{
-		axis.x,
-		axis.y,
-		axis.z,
-		1.0f
-	};
-	
-	irs->q = QuatProd(&irs->q, &correction);
-	irs->q = QuatNormalize(&irs->q, 1.0f);
-}
-
-void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw)
-{
-	float h_roll = (roll * DEG2RAD) / 2.0f;
-	float h_pitch = (pitch * DEG2RAD) / 2.0f;
-	float h_yaw = (yaw * DEG2RAD) / 2.0f;
-	
-	Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
-	Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
-	Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)};
-	
-	Quaternion prProd = QuatProd(&q_pitch, &q_roll);
-	Quaternion pryProd = QuatProd(&prProd, &q_yaw);
-	
-	irs->shiftAccelPRY = pryProd;
-}
-
-Vector3 IRS_getGravity(const Quaternion* q)
-{
-    Vector3 g = 
-	{
-		2 * (q->x*q->z - q->w*q->y),
-		2 * (q->w*q->x + q->y*q->z),
-		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
-	};
-
-    return g;
-}
-
-
-
-
-
-
-
-
-
-

Source/INS/IRS.cpp

@@ -0,0 +1,269 @@
+#include "IRS.h"
+#include <math.h>
+
+
+static constexpr float PI = 3.14159265359f;
+static constexpr float TO_DEG = 180.0f / PI;
+static constexpr float TO_RAD = PI / 180.0f;
+static constexpr float G = 9.80665f; // m/c^2
+
+IRS::IRS():
+  RecordGyro(RecGyr.Rec),
+  RecordSpeed(RecSpd.Rec),
+  RecordPosit(RecPos.Rec),
+  RecordQuat(RecQua.Rec)
+{
+  RecordGyro.Init(RecGyr.Buffer, Freq, RecGyr.Past);
+  RecordSpeed.Init(RecSpd.Buffer, Freq, RecSpd.Past);
+  RecordPosit.Init(RecPos.Buffer, Freq, RecPos.Past);
+  RecordQuat.Init(RecQua.Buffer, Freq, RecQua.Past);
+}
+
+
+// ======================Orientation======================
+void IRS::UpdateGyro(const Vector3& Gyr)
+{
+  if(!Gyr.IsFinite()) return;
+  
+  IMU.Gyr = Gyr;
+
+  Vector3 gyr = Gyr * (Period * TO_RAD);
+
+  RecordGyro.Add(Gyr * TO_RAD);
+
+  Quaternion g = gyr;
+  g = (OriQuat * g);
+
+  OriQuat += g * 0.5f;
+  OriQuat = OriQuat.Norm();
+
+  RecordQuat.Add(OriQuat);
+
+  OriPRT = OriQuat.Conjugate() * Quaternion(0, 0, 1, 0) * OriQuat;
+}
+
+void IRS::UpdateAccel(const Vector3& Acc)
+{
+  if(!Acc.IsFinite()) return;
+  
+  Vector3 accel = ShiftAccelPRY * Acc * ShiftAccelPRY.Conjugate();
+  
+  IMU.Acc = accel;
+
+  Quaternion ine = OriQuat * accel * OriQuat.Conjugate();
+
+  Vector3 acc = { ine.X, ine.Y, ine.Z - 1.0f }; // без гравитации
+  
+  Inertial.Acc = acc;//OriQuat.RotateAroundZ(acc, true);
+
+  Inertial.Spd += acc * (G * Period); // интегрирование скорости
+  
+  Vector3 spd = Inertial.Spd * Period;
+  
+  Inertial.Pos += spd; // интегрирование позиции
+  
+  RecordSpeed.Add(Inertial.Spd);
+  RecordPosit.Add(Inertial.Pos);
+
+  RestoreQuat(accel);
+}
+
+void IRS::RestoreQuat(const Vector3& Acc)
+{
+  float len = Acc.Length();
+
+  static float quat_acc_alpha = 0.03f;
+  static float quat_acc_max = 0.02f;
+  
+  float dyn = fabsf(len - 1.0f);
+  if(dyn>quat_acc_max) dyn=1.0f; else dyn/=quat_acc_max;
+
+  float gain = quat_acc_alpha * (1.0f - dyn);
+  
+  if (gain < 0.0001f) return;
+  
+  Vector3 acc = Acc.Norm();
+  
+  Vector3 est;
+  est.X = 2.0f * (OriQuat.X * OriQuat.Z - OriQuat.W * OriQuat.Y);
+  est.Y = 2.0f * (OriQuat.W * OriQuat.X + OriQuat.Y * OriQuat.Z);
+  est.Z = OriQuat.W * OriQuat.W - OriQuat.X * OriQuat.X - OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z;
+
+  Vector3 cross = acc.Cross(est); 
+  float dot = acc.Dot(est);
+  
+  if (dot < 0.0f) 
+  {
+    float error_len = cross.Length();
+    if (error_len < 0.001f) cross = {1.0f, 0.0f, 0.0f}; 
+    else cross = cross * (1.0f / error_len);
+  }
+  
+  Vector3 axis = cross * (gain * 0.5f);
+  
+  Quaternion correction 
+  {
+    axis.X,
+    axis.Y,
+    axis.Z,
+    1.0f // Для малых углов cos(0) = 1. При нормализации это сработает корректно.
+  };
+
+  OriQuat = OriQuat * correction;
+  
+  OriQuat = OriQuat.Norm();
+}
+
+void IRS::UpdateMagnet(const Vector3& Mag)
+{
+  if(!Mag.IsFinite()) return;
+  
+  IMU.Mag = Mag; // Сохраняем сырые данные (если нужно для отладки)
+  
+  static int init_count = 100;
+  
+  float alpha = 0.001f; 
+  
+
+  if(init_count > 0) 
+  {
+    alpha = 0.05f;
+    init_count--;
+  }
+
+  Vector3 mag_norm = Mag.Norm();
+  
+  Quaternion mW = OriQuat * mag_norm * OriQuat.Conjugate();
+  
+  Vector3 mW_horizontal = { mW.X, mW.Y, 0.0f };
+  
+  if (mW_horizontal.LengthSquared() < 0.0001f) return;
+  mW_horizontal = mW_horizontal.Norm();
+
+  float error_z = mW_horizontal.Y * NorthDeclination.X - mW_horizontal.X * NorthDeclination.Y;
+  
+  float half_error_step = error_z * alpha * 0.5f;
+  
+  Quaternion corr
+  {
+      0.0f,
+      0.0f,
+      half_error_step, 
+      1.0f 
+  };
+  
+  OriQuat = corr * OriQuat;
+  OriQuat = OriQuat.Norm();
+}
+// ======================/Orientation======================
+
+
+void IRS::UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed)
+{
+  Shift.Pos += ShiftPosition;
+  Shift.Spd += ShiftSpeed;
+}
+
+void IRS::UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha)
+{
+  Shift.Pos = Shift.Qua * (1.0f - Alpha) + ShiftQuaternion * Alpha;
+}
+
+void IRS::RestoreAllShift(Vector3& ShiftPos)
+{
+  Vector3 spd = Shift.Spd;
+  Shift.Spd = 0;
+  RecordSpeed.Mix(spd);
+  Inertial.Spd += spd;
+
+  Vector3 pos = Shift.Pos;
+  Shift.Pos = 0;
+  
+  RecordPosit.Mix(pos);
+  Inertial.Pos += pos;
+  ShiftPos=pos;
+
+  Quaternion qua = Shift.Qua;
+  Shift.Qua = 0;
+  RecordQuat.Mix(qua);
+  OriQuat += qua;
+  OriQuat = OriQuat.Norm();
+}
+
+void IRS::SetAccelShift(float Pitch, float Roll, float Yaw)
+{
+  float h_roll  = (Roll * TO_RAD)  / 2.0f;
+  float h_pitch = (Pitch * TO_RAD) / 2.0f;
+  float h_yaw   = (Yaw * TO_RAD)   / 2.0f;
+  
+  Quaternion q_roll =  {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
+  Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
+  //   Как найти поворот: arcsin(ось/макс_ось) * TO_GRAD
+  Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)}; 
+  
+  ShiftAccelPRY = q_pitch * q_roll * q_yaw;
+}
+
+void IRS::SetNorthDeclination(const float Yaw)
+{
+  float dec = Yaw * TO_RAD; // Укажите ваше склонение
+  Vector3 mag_north_target;
+  NorthDeclination = { sinf(dec), cosf(dec) };
+}
+
+bool IRS::SetGroundHeight(float* Height, float MaxHeight, float Alpha)
+{
+  if(Height)
+  {
+    float shift = Inertial.Pos.Z - *Height;
+    GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
+  }
+  else
+  {
+    float height = Inertial.Pos.Z - GroundShift;
+    
+    if(height<MaxHeight)
+    {
+      float shift = Inertial.Pos.Z - MaxHeight;
+      GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
+    }
+  }
+  
+  float height = Inertial.Pos.Z - GroundShift;
+  if (height < 0.0f) GroundShift+=height;
+}
+
+void IRS::GetSinXYCosZ(Vector3& SinXYCosZ)
+{
+  SinXYCosZ = OriPRT;
+}
+
+void IRS::GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed)
+{
+  Reversed = OriPRT.Z < 0;
+  
+  float yaw = atan2f(2.0f * (OriQuat.X * OriQuat.Y - OriQuat.W * OriQuat.Z), 1.0f - 2.0f * (OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z)) * TO_DEG;
+  if (yaw < 0.0f) yaw += 360.0f;
+
+  PitchRollYaw =
+  {
+    GetAngle(OriPRT.Y, OriPRT.X, OriPRT.Z),  // Pitch
+    GetAngle(-OriPRT.X, OriPRT.Y, OriPRT.Z), // Roll
+    yaw                                      // Yaw
+  };
+}
+
+void IRS::GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd)
+{
+  if (Acc) *Acc = Inertial.Acc;
+  if (Spd) *Spd = Inertial.Spd;
+  if (Pos) *Pos = Inertial.Pos;
+  if (Gnd) *Gnd = GroundShift;
+}
+
+
+
+
+
+
+

Source/INS/IRS.h

@@ -1,30 +1,93 @@
+#pragma once
+
 #ifndef IRS_H
 #define IRS_H
 
-#include "quaternion.h"
+#include "Quaternion.h"
-#include "vector.h"
+#include "Vector.h"
+#include "util/Record.h"
 
-#define PI			3.14159265359f
+class IRS // Inertial Reference System
-#define DEG2RAD		PI / 180.0f
-
-typedef struct
 {
-    Quaternion q;   			// ориентация
+public:
-	Vector3 oriPRT; 			// orientation pitch roll tilts
+  IRS();
-    Vector3 gyro;   			// deg/s
-    Vector3 accel;  			// g
   
-	Quaternion shiftAccelPRY; 	// смещение акселерометра
+  static constexpr float Freq = 100.0f;
+  static constexpr float Period = 1.0f / Freq;
   
-} IRS;
+  Quaternion OriQuat; // Главный кватернион ориентации
+  Vector3 OriPRT;     // Локальные наклоны
   
-void IRS_init(IRS* irs);
+  struct { Vector3 Gyr, Acc, Mag; } IMU;      // Последние значения датчиков
+  struct { Vector3 Acc, Spd, Pos; } Inertial; // Инерциальные значения движения
   
-void IRS_update(IRS* irs, float dt);
+  float GroundShift = 0.0f; // Разница между высотой и поверхностью
-void restoreQuat(IRS* irs, const Vector3* accel);
   
-void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);
+  Quaternion ShiftAccelPRY; // Смещение акселерометра
+  Vector2 NorthDeclination; // Смещение магнитометра
   
-Vector3 IRS_getGravity(const Quaternion* q);
+private:
+  // Запись значений для расчётов в прошлом
+  struct StructRecGyr
+  {
+    static constexpr float Past = 0.1f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Vector3 Buffer[Count];
+    Record<Vector3> Rec;
+  } RecGyr;
+
+  struct StructRecAccSpdPos
+  {
+    static constexpr float Past = 0.6f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Vector3 Buffer[Count];
+    Record<Vector3> Rec;
+  } RecAcc, RecSpd, RecPos;
+
+  struct StructRecQua
+  {
+    static constexpr float Past = 0.6f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Quaternion Buffer[Count];
+    Record<Quaternion> Rec;
+  } RecQua;
+  
+  //---
+  // Сдвиг данных
+  struct { Vector3 Pos, Spd; Quaternion Qua = 0.0f; } Shift; // Значение смещения позиции, скорости и кватерниона
+  //---
+  
+public:
+  Record<Vector3>& RecordGyro;
+  Record<Vector3>& RecordSpeed;
+  Record<Vector3>& RecordPosit;
+  Record<Quaternion>& RecordQuat;
+
+private:
+  void RestoreQuat(const Vector3& Acc);
+  static float GetAngle(float X, float Y, float Z);
+  
+public:
+  void UpdateGyro(const Vector3& Gyr);    // deg/s
+  void UpdateAccel(const Vector3& Acc);   // g
+  void UpdateMagnet(const Vector3& Mag);  // gaus
+  
+  // Коэффициенты восстановления позиции и скорости в секунду
+  void SetShiftAlpha(const Vector3* Pos, const Vector3* Spd, const float* Qua); 
+
+  void UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed);
+  void UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha);
+
+  void RestoreAllShift(Vector3& ShiftPos);
+  
+  void SetAccelShift(float Pitch, float Roll, float Yaw);
+  void SetNorthDeclination(const float Yaw);
+  
+  bool SetGroundHeight(float* Height, float MaxHeight, float Alpha);
+  
+  void GetSinXYCosZ(Vector3& SinXYCos);
+  void GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed);
+  void GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd);
+};
 
 #endif

Source/INS/geometry/quaternion.c

@@ -1,169 +0,0 @@
-#include "quaternion.h"
-#include <math.h>
-
-#define PI		3.14159265359f
-
-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)
-{
-  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 = {q1->x + q2->x, q1->y + q2->y, q1->z + q2->z, 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 = 
-	{
-		q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
-		q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
-		q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
-		q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
-	};
-	
-	
-	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 QuatCreateFromEuler(const Vector3* eulerAngels)
-{
-	Quaternion res;
-	
-	float h_r = 0.5f * eulerAngels->y;
-	float h_p = 0.5f * eulerAngels->x;
-	float h_y = 0.5f * eulerAngels->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;
-}
-
-Vector3 QuatToEuler(const Quaternion* q)
-{
-	Vector3 e;
-
-    e.x = atan2f(2*(q->w*q->x + q->y*q->z),
-                 1 - 2*(q->x*q->x + q->y*q->y));
-
-    e.y = asinf(2*(q->w*q->y - q->z*q->x));
-
-    e.z = atan2f(2*(q->w*q->z + q->x*q->y),
-                 1 - 2*(q->y*q->y + q->z*q->z));
-
-    e.x *= 180.0f / PI;
-    e.y *= 180.0f / PI;
-    e.z *= 180.0f / PI;
-
-    return e;
-}
-
-
-
-
-
-
-
-
-
-
-
-
-

Source/INS/geometry/quaternion.h

@@ -1,31 +0,0 @@
-#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 Quaternion* q, const Vector3* vec, bool CCW);
-Quaternion QuatCreateFromEuler(const Vector3* eulerAngels);
-Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
-
-Vector3 QuatToEuler(const Quaternion* q);
-
-#endif
-

Source/INS/geometry/vector.c

@@ -1,174 +0,0 @@
-#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)
-{
-	Vector3 res = {0};
-	float n = lengthV3(v);
-	
-	if (n > 1e-12f)
-	{
-		n = gain / n;
-		res.x = v->x * n;
-		res.y = v->y * n;
-		res.z = v->z * n;
-	}
-	
-	return res;
-}
-
-float DotV2(const Vector2* v1, const Vector2* v2)
-{
-	float res = v1->x * v2->x + v1->y * v2->y;
-	return res;
-}
-
-float DotV3(const Vector3* v1, const Vector3* v2)
-{
-	float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->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;
-}
-
-
-Vector3 Cross(const Vector3* v1, const Vector3* v2)
-{
-	Vector3 res = 
-	{
-		v1->x * v2->z - v1->z * v2->y,
-		v1->z * v2->x - v1->x * v2->z,
-		v1->x * v2->y - v1->y * v2->x
-	};
-	
-	return res;
-}
-
-
-
-
-
-
-
-
-
-
-

Source/INS/geometry/vector.h

@@ -1,53 +0,0 @@
-#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);
-
-float DotV2(const Vector2* v1, const Vector2* v2);
-float DotV3(const Vector3* v1, const Vector3* v2);
-
-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 Cross(const Vector3* v1, const Vector3* v2);
-
-#endif

Source/INS/util/Record.cpp

@@ -0,0 +1,47 @@
+#include "Record.h"
+#include <math.h>
+
+template class Record<float>;
+template class Record<Vector2>;
+template class Record<Vector3>;
+template class Record<Quaternion>;
+
+template <class T>
+Record<T>::Record(T* RecordBuffer, float Frequency, float Period)
+{
+  Init(RecordBuffer, Frequency, Period);
+}
+
+template <class T>
+void Record<T>::Init(T* RecordBuffer, float Frequency, float Period)
+{
+  Freq = Frequency;
+  Buffer = RecordBuffer;
+  Count = (float)(Frequency * Period + 1.0f);
+}
+
+template <class T>
+T Record<T>::Past(float Time) const
+{
+  unsigned long move = (float)(Time * Freq);
+  if (move >= Count) move = Count - 1;
+  
+  if (Index >= move) move = Index - move;
+  else move = Index + (Count - move);
+
+  return Buffer[move];
+}
+
+template <class T>
+void Record<T>::Add(const T& Value)
+{
+  Index++;
+  if (Index >= Count) Index = 0;
+  Buffer[Index] = Value;
+}
+
+template <class T>
+void Record<T>::Mix(const T& Shift)
+{
+  for (unsigned int a = 0; a < Count; a++) Buffer[a] += Shift;
+}

Source/INS/util/Record.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#ifndef RECORD_H
+#define RECORD_H
+
+#include "Vector.h"
+#include "Quaternion.h"
+
+template <typename T>
+class Record
+{
+public:
+  Record() {};
+  Record(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1;
+  
+private:
+  float Freq;
+  unsigned long Count;
+  T* Buffer;
+  T Shift;
+  unsigned long Index = 0;
+
+public:
+  void Init(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1;
+
+  T Past(float Time) const;
+
+  void Add(const T& Value);
+
+  void Mix(const T& Shift);
+};
+
+template <typename T>
+class Average
+{
+private:
+  T Value;
+  float Count = 0;
+
+public:
+  void Set(const T& Val) { Value += Val; Count++; }
+  T Get() { if (Count == 0.0f) return T(); T ave = Value / Count; Count = 0; Value = 0.0f; return ave; }
+};
+
+#endif

Source/MathEnv/Biquad.cpp

@@ -0,0 +1,75 @@
+#include "Biquad.h"
+#include <math.h>
+
+static constexpr float PI = 3.14159265359f;
+
+void lpf2p_init(BiquadFilter *filter, float sample_freq, float cutoff_freq)
+{
+  // Некорректные параметры, фильтр не будет работать
+  if (cutoff_freq <= 0.0f || sample_freq <= 0.0f) return;
+  
+  float fr = sample_freq / cutoff_freq;
+  float ohm = tanf(PI / fr);
+  float c = 1.0f + 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm;
+  
+  filter->b0 = ohm * ohm / c;
+  filter->b1 = 2.0f * filter->b0;
+  filter->b2 = filter->b0;
+
+  filter->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  filter->a2 = (1.0f - 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm) / c;
+
+  // Сбрасываем историю
+  filter->x1 = filter->x2 = 0.0f;
+  filter->y1 = filter->y2 = 0.0f;
+}
+
+void notch_init(BiquadFilter *filter, float sample_freq, float center_freq, float bandwidth)
+{
+  // Некорректные параметры
+  if (center_freq <= 0.0f || sample_freq <= 0.0f || bandwidth <= 0.0f) return;
+  
+  // Промежуточные расчеты по стандартным формулам для biquad-фильтра
+  float w0 = 2.0f * PI * center_freq / sample_freq;
+  float alpha = sinf(w0) * sinhf(logf(2.0f) / 2.0f * bandwidth * w0 / sinf(w0));
+  
+  // Рассчитываем коэффициенты b (для входа x) и a (для выхода y)
+  float b0_temp = 1.0f;
+  float b1_temp = -2.0f * cosf(w0);
+  float b2_temp = 1.0f;
+  
+  float a0_temp = 1.0f + alpha;
+  float a1_temp = -2.0f * cosf(w0);
+  float a2_temp = 1.0f - alpha;
+  
+  // Нормализуем коэффициенты (делим на a0), чтобы использовать в уравнении
+  filter->b0 = b0_temp / a0_temp;
+  filter->b1 = b1_temp / a0_temp;
+  filter->b2 = b2_temp / a0_temp;
+  
+  filter->a1 = a1_temp / a0_temp;
+  filter->a2 = a2_temp / a0_temp;
+  
+  // Сбрасываем историю
+  filter->x1 = filter->x2 = 0.0f;
+  filter->y1 = filter->y2 = 0.0f;
+}
+
+float biquad_apply(BiquadFilter *filter, float input)
+{
+  // Применяем разностное уравнение: y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]
+  float output = filter->b0 * input +
+                filter->b1 * filter->x1 +
+                filter->b2 * filter->x2 -
+                filter->a1 * filter->y1 -
+                filter->a2 * filter->y2;
+  
+  // Обновляем историю для следующего шага
+  filter->x2 = filter->x1;
+  filter->x1 = input;
+  
+  filter->y2 = filter->y1;
+  filter->y1 = output;
+  
+  return output;
+}

Source/MathEnv/Biquad.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#ifndef BIQUAD_H
+#define BIQUAD_H
+
+typedef struct
+{
+  // Коэффициенты фильтра
+  float a1, a2;
+  float b0, b1, b2;
+  
+  float x1, x2; // Предыдущие входы
+  float y1, y2; // Предыдущие выходы
+} BiquadFilter;
+
+void lpf2p_init(BiquadFilter *filter, float sample_freq, float cutoff_freq);
+void notch_init(BiquadFilter *filter, float sample_freq, float center_freq, float bandwidth);
+float biquad_apply(BiquadFilter* filter, float input);
+
+
+#endif

Source/MathEnv/MathFunc.cpp

@@ -0,0 +1,14 @@
+#include "MathFunc.h"
+
+float Normalize(float Value, float Scale, float Min, float Max)
+{
+  const float len = (Max - Min) / 2.0f;
+  const float shift = (Max + Min) / 2.0f;
+  return (Value - shift) * Scale / len;
+}
+
+short Rev16(short v)
+{
+  asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
+  return v;
+}

Source/MathEnv/MathFunc.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#ifndef MATHFUNC_H
+#define MATHFUNC_H
+
+float Normalize(float Value, float Scale, float Min, float Max);
+short Rev16(short v);
+
+
+#endif

Source/MathEnv/Quaternion.cpp

@@ -0,0 +1,265 @@
+#include "Quaternion.h"
+#include <math.h>
+
+
+void Quaternion::Zero()
+{
+  X = 0.0f; Y = 0.0f; Z = 0.0f; W = 1.0f;
+}
+
+Quaternion Quaternion::Norm(float Gain) const
+{
+  float norm = sqrtf(X * X + Y * Y + Z * Z + W * W);
+
+  if (norm > 1e-6f)
+  {
+    norm = Gain / norm;
+
+    return
+    {
+      X * norm,
+      Y * norm,
+      Z * norm,
+      W * norm,
+    };
+  }
+  
+  return { 0.0f, 0.0f, 0.0f, 0.0f };
+}
+
+Quaternion Quaternion::Conjugate() const
+{
+  return { -X, -Y, -Z, W };
+}
+
+Quaternion Quaternion::Invert() const
+{
+  float nsq = X * X + Y * Y + Z * Z + W * W;
+
+  if (nsq > 1e-6f)
+  {
+    nsq = 1.0f / nsq;
+
+    return
+    {
+      -X * nsq,
+      -Y * nsq,
+      -Z * nsq,
+      W * nsq,
+    };
+  }
+
+  return { 0.0f, 0.0f, 0.0f, 0.0f };
+}
+
+Quaternion Quaternion::Negate() const
+{
+  return { -X, -Y, -Z, -W };
+}
+
+bool Quaternion::IsNAN() const
+{
+  return (X != X) || (Y != Y) || (Z != Z) || (W != W);
+}
+
+Quaternion& Quaternion::operator=(const Quaternion& Q)
+{
+  W = Q.W;
+  X = Q.X;
+  Y = Q.Y;
+  Z = Q.Z;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator+=(const Quaternion& Q)
+{
+  X += Q.X;
+  Y += Q.Y;
+  Z += Q.Z;
+  W += Q.W;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator-=(const Quaternion& Q)
+{
+  X -= Q.X;
+  Y -= Q.Y;
+  Z -= Q.Z;
+  W -= Q.W;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator*=(const float Value)
+{
+  X *= Value;
+  Y *= Value;
+  Z *= Value;
+  W *= Value;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator*=(const Quaternion& Q)
+{
+  const float x = X;
+  const float y = Y;
+  const float z = Z;
+  const float w = W;
+  
+  X = w * Q.X + x * Q.W + y * Q.Z - z * Q.Y;
+  Y = w * Q.Y - x * Q.Z + y * Q.W + z * Q.X;
+  Z = w * Q.Z + x * Q.Y - y * Q.X + z * Q.W;
+  W = w * Q.W - x * Q.X - y * Q.Y - z * Q.Z;
+
+  return *this;
+}
+
+Quaternion Quaternion::operator*(const float Value) const
+{
+  return
+  {
+    X * Value,
+    Y * Value,
+    Z * Value,
+    W * Value,
+  };
+}
+
+Quaternion Quaternion::operator*(const Quaternion& Q) const
+{
+  return
+  {
+    W * Q.X + X * Q.W + Y * Q.Z - Z * Q.Y,
+    W * Q.Y - X * Q.Z + Y * Q.W + Z * Q.X,
+    W * Q.Z + X * Q.Y - Y * Q.X + Z * Q.W,
+    W * Q.W - X * Q.X - Y * Q.Y - Z * Q.Z,
+  };
+}
+
+Quaternion Quaternion::operator+(const Quaternion& Q) const
+{
+  return
+  {
+    X + Q.X,
+    Y + Q.Y,
+    Z + Q.Z,
+    W + Q.W,
+  };
+}
+
+Quaternion Quaternion::operator-(const Quaternion& Q) const
+{
+  return
+  {
+    X - Q.X,
+    Y - Q.Y,
+    Z - Q.Z,
+    W - Q.W,
+  };
+}
+
+Vector3 Quaternion::Rotate(const Vector3& vec) const
+{
+  Quaternion p = { vec.X, vec.Y, vec.Z, 0.0f };
+  
+  // Вычисляем p' = q * p * q^ (q^ - сопряженный)
+  Quaternion rotated = *this * p * this->Conjugate();
+  
+  // Возвращаем векторную часть результата
+  return { rotated.X, rotated.Y, rotated.Z };
+}
+
+Vector3 Quaternion::RotateAroundZ(const Vector3& vec, bool CCW) const
+{
+  float yaw_sin_term = 2.0f * (W * Z + X * Y);
+  float yaw_cos_term = 1.0f - 2.0f * (Y * Y + Z * Z);
+  
+  float mag_sq = yaw_sin_term * yaw_sin_term + yaw_cos_term * yaw_cos_term;
+  
+  if (mag_sq < 1e-6f) return vec;
+  
+  float inv_mag = 1.0f / sqrtf(mag_sq); 
+  
+  float c = yaw_cos_term * inv_mag;
+  float s = yaw_sin_term * inv_mag;
+
+  if (CCW) s = -s;
+
+  return
+  {
+    vec.X * c - vec.Y * s,
+    vec.X * s + vec.Y * c,
+    vec.Z
+  };
+}
+
+Quaternion Quaternion::CreateYawPitchRoll(const Vector3& PitchRollYawRad) // Глобальный поворот
+{
+  float hp = 0.5f * PitchRollYawRad.X;
+  float hr = 0.5f * PitchRollYawRad.Y;
+  float hy = 0.5f * PitchRollYawRad.Z;
+
+  float cr = cosf(hr), sr = sinf(hr);
+  float cp = cosf(hp), sp = sinf(hp);
+  float cy = cosf(hy), sy = sinf(hy);
+  
+  return // Это эквивалент q_roll(Y) * q_pitch(X) * q_yaw(Z) [Yaw -> Pitch -> Roll]
+  {
+    cr * sp * cy - sr * cp * sy,
+    sr * cp * cy + cr * sp * sy,
+   -cr * cp * sy - sr * sp * cy,
+    cr * cp * cy - sr * sp * sy
+  };
+}
+
+Quaternion Quaternion::CreatePitchRollYaw(const Vector3& PitchRollYawRad) // Локальный поворот
+{
+  float hp = 0.5f * PitchRollYawRad.X;
+  float hr = 0.5f * PitchRollYawRad.Y;
+  float hy = 0.5f * PitchRollYawRad.Z;
+
+  float cr = cosf(hr), sr = sinf(hr);
+  float cp = cosf(hp), sp = sinf(hp);
+  float cy = cosf(hy), sy = sinf(hy);
+  
+  return // Это эквивалент  q_yaw(Z) * q_roll(Y) * q_pitch(X) [ Pitch -> Roll -> Yaw ]
+  {
+    cy * cr * sp + sy * sr * cp,
+    cy * sr * cp - sy * cr * sp,
+   -cr * cp * sy - cy * sr * sp,
+    cy * cr * cp - sy * sr * sp
+  };
+}
+
+Quaternion Quaternion::CreateYaw(const float YawRad)
+{
+  float hy = - 0.5f * YawRad;
+  return { 0.0f, 0.0f, sinf(hy), cosf(hy) };
+}
+
+Quaternion Quaternion::CreateDirection(const Vector2& Course)
+{
+  Vector2 xy = Course.Norm();
+  
+  if(xy.X < -0.999f) return { 0.0, 0.0, 1.0, 0.0 }; // Поворот на 180 градусов
+  float w = sqrtf((1.0f + xy.X) * 0.5f);
+  return { 0.0f, 0.0f, xy.Y / (2.0f * w), w };
+}
+
+Quaternion Quaternion::GetError(const Quaternion& Target, bool FastWay) const
+{
+  Quaternion error // Формула произведения Гамильтона с учетом инверсии current
+  {
+    W * Target.X - X * Target.W - Y * Target.Z + Z * Target.Y,
+    W * Target.Y + X * Target.Z - Y * Target.W - Z * Target.X,
+    W * Target.Z - X * Target.Y + Y * Target.X - Z * Target.W,
+    W * Target.W + X * Target.X + Y * Target.Y + Z * Target.Z
+  };
+  
+  if (FastWay && (error.W < 0.0f)) return error.Negate();
+  
+  return error;
+}

Source/MathEnv/Quaternion.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include "Vector.h"
+
+struct Quaternion
+{
+  float X, Y, Z, W;
+  
+  Quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) { }
+  Quaternion(float v) : X(v), Y(v), Z(v), W(v) { }
+  Quaternion(float x, float y, float z, float w) : X(x), Y(y), Z(z), W(w) { }
+  Quaternion(const Vector3& Vec, float w = 0.0f) : X(Vec.X), Y(Vec.Y), Z(Vec.Z), W(w) { }
+
+  void Zero();
+
+  Quaternion Norm(float Gain = 1.0f) const;
+  Quaternion Conjugate() const;
+  Quaternion Invert() const;
+  Quaternion Negate() const;
+  bool IsNAN() const;
+
+  Quaternion& operator=(const Quaternion& Q);
+  Quaternion& operator+=(const Quaternion& Q);
+  Quaternion& operator-=(const Quaternion& Q);
+  Quaternion& operator*=(const float Value);
+  Quaternion& operator*=(const Quaternion& Q);
+  Quaternion operator*(const float Value) const;
+  Quaternion operator*(const Quaternion& Q) const;
+  Quaternion operator+(const Quaternion& Q) const;
+  Quaternion operator-(const Quaternion& Q) const;
+
+  Vector3 Rotate(const Vector3& vec) const;
+  Vector3 RotateAroundZ(const Vector3& vec, bool CCW = false) const;
+  
+  static Quaternion CreateYawPitchRoll(const Vector3& PitchRollYawRad);
+  static Quaternion CreatePitchRollYaw(const Vector3& PitchRollYawRad);
+  static Quaternion CreateYaw(const float YawRad);
+  static Quaternion CreateDirection(const Vector2& Course);
+  
+  Quaternion GetError(const Quaternion& Target, bool FastWay) const;
+};
+
+#endif

Source/MathEnv/Vector.cpp

@@ -0,0 +1,458 @@
+#include "Vector.h"
+#include "Quaternion.h"
+#include <math.h>
+
+
+// ========================Vector2========================
+Vector2::Vector2(const Vector3& Vec) : X(Vec.X), Y(Vec.Y) {}
+
+void Vector2::Zero()
+{
+  X = Y = 0;
+}
+
+Vector2 Vector2::Norm(float Gain) const
+{
+  float n = sqrtf(X * X + Y * Y);
+
+  if (n > 1e-12f)
+  {
+    n = Gain / n;
+
+    return
+    {
+      X * n,
+      Y * n
+    };
+  }
+
+  return {0, 0};
+}
+
+Vector2 Vector2::Abs() const
+{
+  return { fabsf(X) , fabsf(Y) };
+}
+
+float Vector2::Length() const
+{
+  return sqrtf(X * X + Y * Y);
+}
+
+float Vector2::LengthSquared() const
+{
+  return X * X + Y * Y;
+}
+
+bool Vector2::IsNAN() const
+{
+  return (X != X) || (Y != Y);
+}
+
+bool Vector2::IsFinite() const 
+{
+  return (X - X == 0) && (Y - Y == 0);
+}
+
+Vector2 Vector2::Limit(float Max, float Min) const
+{
+  Vector2 lim;
+  
+  if (X > Max) lim.X = Max; else if (X < Min) lim.X = Min; else lim.X = X;
+  if (Y > Max) lim.Y = Max; else if (Y < Min) lim.Y = Min; else lim.Y = Y;
+  
+  return lim;
+}
+
+Vector2 Vector2::Power(float Pow) const
+{
+  return
+  {
+    powf(X, Pow),
+    powf(Y, Pow)
+  };
+}
+
+float Vector2::Dot(const Vector2& Vec) const
+{
+  return X * Vec.X + Y * Vec.Y;
+}
+
+Vector2& Vector2::operator+=(const Vector2& Vec)
+{
+  X += Vec.X;
+  Y += Vec.Y;
+
+  return *this;
+}
+
+Vector2& Vector2::operator-=(const Vector2& Vec)
+{
+  X -= Vec.X;
+  Y -= Vec.Y;
+
+  return *this;
+}
+
+Vector2& Vector2::operator*=(float Val)
+{
+  X *= Val;
+  Y *= Val;
+
+  return *this;
+}
+
+Vector2& Vector2::operator*=(const Vector2& Vec)
+{
+  X *= Vec.X;
+  Y *= Vec.Y;
+
+  return *this;
+}
+
+Vector2& Vector2::operator/=(float Val)
+{
+  X /= Val;
+  Y /= Val;
+
+  return *this;
+}
+
+Vector2& Vector2::operator=(float Val)
+{
+  X = Val;
+  Y = Val;
+
+  return *this;
+}
+
+Vector2 Vector2::operator*(float Val) const
+{
+  return
+  {
+    X * Val,
+    Y * Val
+  };
+}
+
+Vector2 Vector2::operator*(const Vector2& Vec) const
+{
+  return
+  {
+    X * Vec.X,
+    Y * Vec.Y
+  };
+}
+
+Vector2 Vector2::operator+(const Vector2& Vec) const
+{
+  return
+  {
+    X + Vec.X,
+    Y + Vec.Y
+  };
+}
+
+Vector2 Vector2::operator-(const Vector2& Vec) const
+{
+  return
+  {
+    X - Vec.X,
+    Y - Vec.Y
+  };
+}
+
+Vector2 Vector2::operator/(float Val) const
+{
+  return
+  {
+    X / Val,
+    Y / Val
+  };
+}
+
+Vector2 operator-(float Val, const Vector2& Vec)
+{
+  return
+  {
+    Val - Vec.X,
+    Val - Vec.Y
+  };
+}
+
+
+// ========================Vector3========================
+Vector3::Vector3(const struct Quaternion& q) : X(q.X),Y(q.Y),Z(q.Z) { }
+
+void Vector3::Zero()
+{
+  X = Y = Z = 0;
+}
+
+Vector3 Vector3::Norm(float Gain) const
+{
+  float n = sqrtf(X * X + Y * Y + Z * Z);
+
+  if (n > 1e-12f)
+  {
+    n = Gain / n;
+
+    return
+    {
+      X * n,
+      Y * n,
+      Z * n
+    };
+  }
+
+  return {0, 0};
+}
+
+Vector3 Vector3::Abs() const
+{
+  return { fabsf(X) , fabsf(Y), fabsf(Z) };
+}
+
+float Vector3::Length() const
+{
+  return sqrtf(X * X + Y * Y + Z * Z);
+}
+
+float Vector3::LengthSquared() const
+{
+  return X * X + Y * Y + Z * Z;
+}
+
+Vector3 Vector3::Copy() const
+{
+  return { X, Y, Z };
+}
+
+bool Vector3::IsNAN() const
+{
+  return (X != X) || (Y != Y) || (Z != Z);
+}
+
+bool Vector3::IsFinite() const 
+{
+  return (X - X == 0) && (Y - Y == 0) && (Z - Z == 0);
+}
+
+Vector3 Vector3::Limit(float Max, float Min) const
+{
+  Vector3 lim;
+  
+  if (X > Max) lim.X = Max; else if (X < Min) lim.X = Min; else lim.X = X;
+  if (Y > Max) lim.Y = Max; else if (Y < Min) lim.Y = Min; else lim.Y = Y;
+  if (Z > Max) lim.Z = Max; else if (Z < Min) lim.Z = Min; else lim.Z = Z;
+  
+  return lim;
+}
+
+Vector3 Vector3::Power(float Pow) const
+{
+  return
+  {
+    powf(X, Pow),
+    powf(Y, Pow),
+    powf(Z, Pow)
+  };
+}
+
+float Vector3::Dot(const Vector3& Vec) const
+{
+  return X * Vec.X + Y * Vec.Y + Z * Vec.Z;
+}
+
+Vector3 Vector3::Cross(const Vector3& Vec) const
+{
+  return
+  {
+    Y * Vec.Z - Z * Vec.Y,
+    Z * Vec.X - X * Vec.Z,
+    X * Vec.Y - Y * Vec.X
+  };
+}
+
+Vector3& Vector3::operator+=(const Vector3& Vec)
+{
+  X += Vec.X;
+  Y += Vec.Y;
+  Z += Vec.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator+=(float Val)
+{
+  X += Val;
+  Y += Val;
+  Z += Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator-=(const Vector3& Vec)
+{
+  X -= Vec.X;
+  Y -= Vec.Y;
+  Z -= Vec.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator-=(float Val)
+{
+  X -= Val;
+  Y -= Val;
+  Z -= Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator*=(float Val)
+{
+  X *= Val;
+  Y *= Val;
+  Z *= Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator*=(const Vector3& Vec)
+{
+  X *= Vec.X;
+  Y *= Vec.Y;
+  Z *= Vec.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator/=(float Val)
+{
+  X /= Val;
+  Y /= Val;
+  Z /= Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator=(const struct Quaternion& Quat)
+{
+  X = Quat.X;
+  Y = Quat.Y;
+  Z = Quat.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator=(float Val)
+{
+  X = Val;
+  Y = Val;
+  Z = Val;
+
+  return *this;
+}
+
+Vector3 Vector3::operator*(float Val) const
+{
+  return
+  {
+    X * Val,
+    Y * Val,
+    Z * Val,
+  };
+}
+
+Vector3 Vector3::operator*(const Vector3& Vec) const
+{
+  return
+  {
+    X * Vec.X,
+    Y * Vec.Y,
+    Z * Vec.Z,
+  };
+}
+
+Vector3 Vector3::operator+(float Val) const
+{
+  return
+  {
+    X + Val,
+    Y + Val,
+    Z + Val,
+  };
+}
+
+Vector3 Vector3::operator+(const Vector3& Vec) const
+{
+  return
+  {
+    X + Vec.X,
+    Y + Vec.Y,
+    Z + Vec.Z,
+  };
+}
+
+Vector3 Vector3::operator-(const Vector3& Vec) const
+{
+  return
+  {
+    X - Vec.X,
+    Y - Vec.Y,
+    Z - Vec.Z,
+  };
+}
+
+Vector3 Vector3::operator/(float Val) const
+{
+  return
+  {
+    X / Val,
+    Y / Val,
+    Z / Val,
+  };
+}
+
+Vector3 operator-(float Val, const Vector3& Vec)
+{
+  return
+  {
+    Val - Vec.X,
+    Val - Vec.Y,
+    Val - Vec.Z,
+  };
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

Source/MathEnv/Vector.h

@@ -0,0 +1,95 @@
+#pragma once
+
+#ifndef VECTOR_H
+#define VECTOR_H
+
+#include <functional>
+
+struct Vector2
+{
+  float X = 0.0f, Y = 0.0f;
+  
+  Vector2(const float v = 0) : X(v), Y(v) {}
+  Vector2(const struct Vector3& Vec);
+  Vector2(const Vector2& Vec) : X(Vec.X), Y(Vec.Y) {}
+  Vector2(float x, float y) : X(x), Y(y) {}
+  
+  void Zero();
+  
+  Vector2 Norm(float Gain = 1.0f) const;
+  Vector2 Abs() const;
+  float Length() const;
+  float LengthSquared() const;
+  bool IsNAN() const;
+  bool IsFinite() const;
+  Vector2 Limit(float Min, float Max) const;
+  Vector2 Power(float Pow) const;
+  float Dot(const Vector2& Vec) const;
+  
+  template<typename T> Vector2 Action(T Act) const { return { Act(X), Act(Y) }; };
+  
+  Vector2& operator+=(const Vector2& Vec);
+  Vector2& operator-=(const Vector2& Vec);
+  Vector2& operator*=(float Val);
+  Vector2& operator*=(const Vector2& Vec);
+  Vector2& operator/=(float Val);
+
+  Vector2& operator=(float Val);
+
+  Vector2 operator*(float Val) const;
+  Vector2 operator*(const Vector2& Vec) const;
+  Vector2 operator+(const Vector2& Vec) const;
+  Vector2 operator-(const Vector2& Vec) const;
+  Vector2 operator/(float Val) const;
+  
+  friend Vector2 operator-(float Val, const Vector2& Vec);
+};
+
+struct Vector3
+{
+  float X = 0.0f, Y = 0.0f, Z = 0.0f;
+  
+  Vector3(const float v = 0) : X(v), Y(v), Z(v) {}
+  Vector3(const Vector2& Vec, const float z = 0.0f) : X(Vec.X), Y(Vec.Y), Z(z) {}
+  Vector3(const Vector3& Vec) : X(Vec.X), Y(Vec.Y), Z(Vec.Z) {}
+  Vector3(float x, float y, float z): X(x), Y(y), Z(z) {}
+  Vector3(const struct Quaternion& q);
+  
+  void Zero();
+
+  Vector3 Norm(float Gain = 1.0f) const;
+  Vector3 Abs() const;
+  float Length() const;
+  float LengthSquared() const;
+  Vector3 Copy() const;
+  bool IsNAN() const;
+  bool IsFinite() const;
+  Vector3 Limit(float Max, float Min) const;
+  Vector3 Power(float Pow) const;
+  float Dot(const Vector3& Vec) const;
+  Vector3 Cross(const Vector3& Vec) const;
+  
+  template<typename T> Vector3 Action(T Act) const { return { Act(X), Act(Y), Act(Z) }; };
+
+  Vector3& operator+=(const Vector3& Vec);
+  Vector3& operator+=(float Val);
+  Vector3& operator-=(const Vector3& Vec);
+  Vector3& operator-=(float Val);
+  Vector3& operator*=(float Val);
+  Vector3& operator*=(const Vector3& Vec);
+  Vector3& operator/=(float Val);
+
+  Vector3& operator=(const struct Quaternion& Quat);
+  Vector3& operator=(float Val);
+
+  Vector3 operator*(float Val) const;
+  Vector3 operator*(const Vector3& Vec) const;
+  Vector3 operator+(float Val) const;
+  Vector3 operator+(const Vector3& Vec) const;
+  Vector3 operator-(const Vector3& Vec) const;
+  Vector3 operator/(float Val) const;
+
+  friend Vector3 operator-(float Val, const Vector3& Vec);
+};
+
+#endif

Source/main.c

@@ -1,121 +0,0 @@
-#include "stm32g431xx.h"
-#include "imu.h"
-#include "imu_processing.h"
-#include "IRS.h"
-#include "attitude.h"
-#include "radio_receiver.h"
-#include "motors.h"
-#include "pid.h"
-#include "lidar.h"
-
-void TIM6_DAC_IRQHandler();
-
-static uint8_t irs_update_flag = 0;
-static uint8_t control_update_flag = 0;
-
-imu_scaled_t imu;
-IRS irs;
-attitude_t attitude;
-rc_channels rx_chs_raw;
-rc_channels rx_chs_normalized;
-control_channels_t ctrl_chs;
-lidar_data lidar;
-
-Vector3 euler;
-
-void delay_ms(uint32_t ms);
-
-int main(void)
-{
-	__enable_irq();
-	
-	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
-	NVIC_SetPriority(USART3_IRQn, 1);
-	NVIC_SetPriority(LPUART1_IRQn, 0);
-	
-	imu_pow_init();
-	i2c_gpio_init();
-	i2c1_init();
-	imu_init();
-	
-	imu_processing_init();
-	imu_calibrate();
-	
-	imu_tim6_init();
-	
-	IRS_init(&irs);
-	
-	attitude_init(&attitude);
-	
-	receiver_init();
-	
-	motors_init();
-	
-	while (1)
-	{
-	    receiver_update(&rx_chs_raw);
-	    rx_chs_normalized = normalize_channels(rx_chs_raw);
-		
-		if (irs_update_flag)
-		{
-		    irs_update_flag = 0;
-			
-			imu_read_scaled(&imu);
-			
-			irs.gyro.x = imu.gx;
-			irs.gyro.y = imu.gy;
-			irs.gyro.z = imu.gz;
-			
-			irs.accel.x = imu.ax;
-			irs.accel.y = imu.ay;
-			irs.accel.z = imu.az;
-		
-		    IRS_update(&irs, IMU_DT);
-		}
-		
-		euler = QuatToEuler(&irs.q);
-		
-		if (control_update_flag)
-		{
-		    control_update_flag = 0;
-		
-		    attitude_controller_update(
-		        &ctrl_chs,
-		        &rx_chs_normalized,
-		        &irs.q,
-		        &irs.gyro
-		    );
-		}
-		
-		if (rx_chs_normalized.rc_armed)
-	    {
-	        motors_set_throttle_mix(
-	            rx_chs_normalized.rc_throttle,
-	            &ctrl_chs,
-	            rx_chs_normalized.rc_armed
-	        );
-	    }
-	    else
-		{
-			motors_turn_off();
-		}
-	}
-}
-
-void TIM6_DAC_IRQHandler()
-{
-	if (TIM6->SR & TIM_SR_UIF)
-	{
-		TIM6->SR &= ~TIM_SR_UIF;
-		irs_update_flag = 1;
-		control_update_flag = 1;
-	}
-}
-
-void delay_ms(uint32_t ms)
-{
-	for (uint32_t i = 0; i < ms * 4000; i++);
-}
-
-
-

Source/main.cpp

@@ -0,0 +1,189 @@
+#include "stm32g4xx.h"
+#include "Tick.h"
+#include "Tim.h"
+#include "IIMU.h"
+#include "ICM20948.h"
+#include "IRS.h"
+#include "Vector.h"
+#include "Autopilot.h"
+#include "RadioReceiver.h"
+#include "Motors.h"
+#include "Attitude.h"
+
+#define PI            3.14159265359f
+
+extern "C" void SystemClock_Config();
+
+constexpr long TIM_PRIORITY = 2;
+
+IMU_Data DataIMU;
+
+IRS MainIRS;
+
+IIMU* IMUObj;
+
+float GetCurrentTime()
+{
+  return ((float)TICK_GetCount()) / 1000.0f;
+}
+
+Vector3 QuatToEuler(const Quaternion& q)
+{
+  Vector3 e;
+  
+  e.X = atan2f(2*(q.W*q.X + q.Y*q.Z), 1 - 2*(q.X*q.X + q.Y*q.Y));
+
+  e.Y = asinf(2*(q.W*q.Y - q.Z*q.X));
+
+  e.Z = atan2f(2*(q.W*q.Z + q.X*q.Y), 1 - 2*(q.Y*q.Y + q.Z*q.Z));
+
+  e.X *= 180.0f / PI;
+  e.Y *= 180.0f / PI;
+  e.Z *= 180.0f / PI;
+
+  return e;
+}
+
+void DoneProcIMU(IMU_Data& Data)
+{
+  DataIMU = Data;
+  // MagReady=true;
+}
+
+void TimerUpdateSensor()
+{
+  //IMUObj->GetAsync(DoneProcIMU);
+  if(MainDrone.IMUInit) IMUObj->GetAsync(DoneProcIMU);
+  //if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
+}
+
+Vector3 Euler;
+
+void TimerUpdateMain()
+{
+  float time = GetCurrentTime();
+  
+  Vector3 gyr = {(float)DataIMU.Gyr.X, (float)DataIMU.Gyr.Y, (float)DataIMU.Gyr.Z};
+  Vector3 acc = {(float)DataIMU.Acc.X, (float)DataIMU.Acc.Y, (float)DataIMU.Acc.Z};
+  
+  // Vector3 mag;
+  // if(MainDrone.ExternMagInit) mag = {(float)DataMAG.X, (float)DataMAG.Y, (float)DataMAG.Z};
+  // else mag = {(float)DataIMU.Mag.X, (float)DataIMU.Mag.Y, (float)DataIMU.Mag.Z};
+  
+  MainIRS.UpdateGyro(gyr);
+  MainIRS.UpdateAccel(acc);
+  
+  Euler = QuatToEuler(MainIRS.OriQuat);
+  
+  /*if(MagReady)
+  {
+    MagReady=false;
+    MainIRS.UpdateMagnet(mag);
+  }
+  
+  MainGPS.UpdateAverage(time);
+  MainBar.UpdateAverage();
+  MainOF.UpdateAverage(IRS::Period);
+  MainLen.UpdateAverage();
+  
+  if(DataFLOW.Update)
+  {
+    DataFLOW.Update=false;
+    UpdateFlow(DataFLOW.OF, MainIRS.Inertial.Pos.Z - MainIRS.GroundShift, DataFLOW.Valid, DataFLOW.Quality);
+  }
+  
+  if(DataTOF.Update)
+  {
+    DataTOF.Update=false;
+    UpdateRange(DataTOF.Range, DataTOF.Valid && DataTOF.Range>0.02f && DataTOF.Strength>200.0f);
+  }
+  
+  UpdateBar(DataBAR.Pressure, DataBAR.Temp, DataBAR.Pressure>0.0f);
+  
+  if(DataGPS.Update)
+  {
+    DataGPS.Update=false;
+    Vector3 gps = { DataGPS.X, DataGPS.Y, DataGPS.Z };
+    UpdateGPS(gps, time, DataGPS.Valid);
+  }*/
+  
+  Vector3 pos;
+  MainIRS.RestoreAllShift(pos);
+  /*MainVel.PointBegin-=pos;
+  
+  AutoControl(gyr);
+  
+  ReadStateINS();
+  
+  if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
+}
+
+static bool control_update_flag = false;
+
+void TimerUpdateControl()
+{
+  control_update_flag = true;
+}
+
+int main()
+{
+  SystemClock_Config(); // 170MHz
+  IMU_InitPower();
+  
+  
+  
+  TICK_Init();
+  
+  TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
+  TIM7_Init(TIM_PRIORITY, 200, TimerUpdateControl);
+  
+  IMUObj = TryFindIMU(MainDrone.IMUInit);
+  
+  TIM6_Enable();
+  TIM7_Enable();
+  
+  static attitude_t attitude;
+  static rc_channels rx_chs_raw;
+  static rc_channels rx_chs_normalized;
+  static control_channels_t ctrl_chs;
+  
+  attitude_init(&attitude);
+  receiver_init();
+  motors_init();
+  
+  while (true)
+  {
+    receiver_update(&rx_chs_raw);
+	  rx_chs_normalized = normalize_channels(rx_chs_raw);
+    
+    if (control_update_flag)
+		{
+      control_update_flag = false;
+      
+      Vector3 Gyro = Vector3(DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z);
+      
+      attitude_controller_update(
+        &ctrl_chs,
+        &rx_chs_normalized,
+        &MainIRS.OriQuat,
+        &Gyro
+      );
+		}
+		
+		if (rx_chs_normalized.rc_armed)
+	  {
+      CalibDataIMU.AllowedCalib = false;
+      
+      motors_set_throttle_mix(
+        rx_chs_normalized.rc_throttle,
+        &ctrl_chs,
+        rx_chs_normalized.rc_armed
+      );
+	  }
+	  else
+		{
+			motors_turn_off();
+			CalibDataIMU.AllowedCalib = true;
+		}
+  }
+}

drone.ewp

@@ -356,14 +356,12 @@
                 </option>
                 <option>
                     <name>CCIncludePath2</name>
-                    <state>$PROJ_DIR$\Source\Drivers\CMSIS\Device\ST\STM32G4xx\Include</state>
+                    <state>$PROJ_DIR$\Source\Core</state>
-                    <state>$PROJ_DIR$\Source\Drivers\CMSIS\Include</state>
+                    <state>$PROJ_DIR$\Source\Drivers</state>
-                    <state>$PROJ_DIR$\Source\Drivers\HAL_M</state>
+                    <state>$PROJ_DIR$\Source\Devices</state>
-                    <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>
                     <state>$PROJ_DIR$\Source\INS</state>
+                    <state>$PROJ_DIR$\Source\Control</state>
+                    <state>$PROJ_DIR$\Source\MathEnv</state>
                 </option>
                 <option>
                     <name>CCStdIncCheck</name>
@@ -404,7 +402,7 @@
                 </option>
                 <option>
                     <name>IccLang</name>
-                    <state>2</state>
+                    <state>1</state>
                 </option>
                 <option>
                     <name>IccCDialect</name>
@@ -2240,161 +2238,149 @@
     </configuration>
     <group>
         <name>Source</name>
-        <group>
-            <name>BSP</name>
-            <group>
-                <name>Inc</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\imu.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\imu_processing.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\lidar.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\motors.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\radio_receiver.h</name>
-                </file>
-            </group>
-            <group>
-                <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\imu.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\imu_processing.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\lidar.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\motors.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\radio_receiver.c</name>
-                </file>
-            </group>
-        </group>
         <group>
             <name>Control</name>
-            <group>
-                <name>Inc</name>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Inc\attitude.h</name>
+                <name>$PROJ_DIR$\Source\Control\Attitude.cpp</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Inc\biquad_filter.h</name>
+                <name>$PROJ_DIR$\Source\Control\Attitude.h</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Inc\pid.h</name>
+                <name>$PROJ_DIR$\Source\Control\Autopilot.cpp</name>
-                </file>
-            </group>
-            <group>
-                <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\Control\Src\attitude.c</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Src\biquad_filter.c</name>
+                <name>$PROJ_DIR$\Source\Control\Autopilot.h</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Src\pid.c</name>
+                <name>$PROJ_DIR$\Source\Control\PID.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Control\PID.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Control\Protocol.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Control\Protocol.h</name>
             </file>
-            </group>
         </group>
         <group>
             <name>Core</name>
-            <group>
-                <name>Inc</name>
             <file>
-                    <name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
+                <name>$PROJ_DIR$\Source\Core\stm32g431xx.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Core\stm32g4xx.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Core\system_stm32g4xx.c</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Core\system_stm32g4xx.h</name>
             </file>
         </group>
         <group>
-                <name>Src</name>
+            <name>Devices</name>
             <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
+                <name>$PROJ_DIR$\Source\Devices\ICM20948.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\ICM20948.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\IIMU.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\IIMU.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\Motors.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\Motors.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\RadioReceiver.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\RadioReceiver.h</name>
             </file>
-            </group>
         </group>
         <group>
             <name>Drivers</name>
-            <group>
-                <name>CMSIS</name>
-                <group>
-                    <name>Device</name>
-                    <group>
-                        <name>ST</name>
-                        <group>
-                            <name>STM32G4xx</name>
-                            <group>
-                                <name>Include</name>
             <file>
-                                    <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g431xx.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\GPIO.cpp</name>
             </file>
             <file>
-                                    <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g4xx.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\GPIO.h</name>
             </file>
-                            </group>
-                        </group>
-                    </group>
-                </group>
-                <group>
-                    <name>Include</name>
             <file>
-                        <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Include\core_cm4.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\I2C.cpp</name>
             </file>
-                </group>
-            </group>
-            <group>
-                <name>HAL_M</name>
-                <group>
-                    <name>GPIO</name>
-                    <group>
-                        <name>Inc</name>
             <file>
-                            <name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Inc\HAL_GPIO.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\I2C.h</name>
             </file>
-                    </group>
-                    <group>
-                        <name>Src</name>
             <file>
-                            <name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Src\HAL_GPIO.c</name>
+                <name>$PROJ_DIR$\Source\Drivers\Tick.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Drivers\Tick.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Drivers\Tim.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Drivers\Tim.h</name>
             </file>
-                    </group>
-                </group>
-            </group>
         </group>
         <group>
             <name>INS</name>
             <group>
-                <name>geometry</name>
+                <name>util</name>
                 <file>
-                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.c</name>
+                    <name>$PROJ_DIR$\Source\INS\util\Record.cpp</name>
                 </file>
                 <file>
-                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.h</name>
+                    <name>$PROJ_DIR$\Source\INS\util\Record.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>
             <file>
-                <name>$PROJ_DIR$\Source\INS\IRS.c</name>
+                <name>$PROJ_DIR$\Source\INS\IRS.cpp</name>
             </file>
             <file>
                 <name>$PROJ_DIR$\Source\INS\IRS.h</name>
             </file>
         </group>
+        <group>
+            <name>MathEnv</name>
             <file>
-            <name>$PROJ_DIR$\Source\main.c</name>
+                <name>$PROJ_DIR$\Source\MathEnv\Biquad.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Biquad.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\MathFunc.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\MathFunc.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Quaternion.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Quaternion.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Vector.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Vector.h</name>
+            </file>
+        </group>
+        <file>
+            <name>$PROJ_DIR$\Source\main.cpp</name>
         </file>
         <file>
             <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Source\Templates\iar\startup_stm32g431xx.s</name>

drone.ewt

@@ -3423,161 +3423,149 @@
     </configuration>
     <group>
         <name>Source</name>
-        <group>
-            <name>BSP</name>
-            <group>
-                <name>Inc</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\imu.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\imu_processing.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\lidar.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\motors.h</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Inc\radio_receiver.h</name>
-                </file>
-            </group>
-            <group>
-                <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\imu.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\imu_processing.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\lidar.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\motors.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\BSP\Src\radio_receiver.c</name>
-                </file>
-            </group>
-        </group>
         <group>
             <name>Control</name>
-            <group>
-                <name>Inc</name>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Inc\attitude.h</name>
+                <name>$PROJ_DIR$\Source\Control\Attitude.cpp</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Inc\biquad_filter.h</name>
+                <name>$PROJ_DIR$\Source\Control\Attitude.h</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Inc\pid.h</name>
+                <name>$PROJ_DIR$\Source\Control\Autopilot.cpp</name>
-                </file>
-            </group>
-            <group>
-                <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\Control\Src\attitude.c</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Src\biquad_filter.c</name>
+                <name>$PROJ_DIR$\Source\Control\Autopilot.h</name>
             </file>
             <file>
-                    <name>$PROJ_DIR$\Source\Control\Src\pid.c</name>
+                <name>$PROJ_DIR$\Source\Control\PID.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Control\PID.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Control\Protocol.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Control\Protocol.h</name>
             </file>
-            </group>
         </group>
         <group>
             <name>Core</name>
-            <group>
-                <name>Inc</name>
             <file>
-                    <name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
+                <name>$PROJ_DIR$\Source\Core\stm32g431xx.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Core\stm32g4xx.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Core\system_stm32g4xx.c</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Core\system_stm32g4xx.h</name>
             </file>
         </group>
         <group>
-                <name>Src</name>
+            <name>Devices</name>
             <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
+                <name>$PROJ_DIR$\Source\Devices\ICM20948.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\ICM20948.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\IIMU.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\IIMU.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\Motors.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\Motors.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\RadioReceiver.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Devices\RadioReceiver.h</name>
             </file>
-            </group>
         </group>
         <group>
             <name>Drivers</name>
-            <group>
-                <name>CMSIS</name>
-                <group>
-                    <name>Device</name>
-                    <group>
-                        <name>ST</name>
-                        <group>
-                            <name>STM32G4xx</name>
-                            <group>
-                                <name>Include</name>
             <file>
-                                    <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g431xx.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\GPIO.cpp</name>
             </file>
             <file>
-                                    <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Include\stm32g4xx.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\GPIO.h</name>
             </file>
-                            </group>
-                        </group>
-                    </group>
-                </group>
-                <group>
-                    <name>Include</name>
             <file>
-                        <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Include\core_cm4.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\I2C.cpp</name>
             </file>
-                </group>
-            </group>
-            <group>
-                <name>HAL_M</name>
-                <group>
-                    <name>GPIO</name>
-                    <group>
-                        <name>Inc</name>
             <file>
-                            <name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Inc\HAL_GPIO.h</name>
+                <name>$PROJ_DIR$\Source\Drivers\I2C.h</name>
             </file>
-                    </group>
-                    <group>
-                        <name>Src</name>
             <file>
-                            <name>$PROJ_DIR$\Source\Drivers\HAL_M\GPIO\Src\HAL_GPIO.c</name>
+                <name>$PROJ_DIR$\Source\Drivers\Tick.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Drivers\Tick.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Drivers\Tim.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\Drivers\Tim.h</name>
             </file>
-                    </group>
-                </group>
-            </group>
         </group>
         <group>
             <name>INS</name>
             <group>
-                <name>geometry</name>
+                <name>util</name>
                 <file>
-                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.c</name>
+                    <name>$PROJ_DIR$\Source\INS\util\Record.cpp</name>
                 </file>
                 <file>
-                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.h</name>
+                    <name>$PROJ_DIR$\Source\INS\util\Record.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>
             <file>
-                <name>$PROJ_DIR$\Source\INS\IRS.c</name>
+                <name>$PROJ_DIR$\Source\INS\IRS.cpp</name>
             </file>
             <file>
                 <name>$PROJ_DIR$\Source\INS\IRS.h</name>
             </file>
         </group>
+        <group>
+            <name>MathEnv</name>
             <file>
-            <name>$PROJ_DIR$\Source\main.c</name>
+                <name>$PROJ_DIR$\Source\MathEnv\Biquad.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Biquad.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\MathFunc.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\MathFunc.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Quaternion.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Quaternion.h</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Vector.cpp</name>
+            </file>
+            <file>
+                <name>$PROJ_DIR$\Source\MathEnv\Vector.h</name>
+            </file>
+        </group>
+        <file>
+            <name>$PROJ_DIR$\Source\main.cpp</name>
         </file>
         <file>
             <name>$PROJ_DIR$\..\..\STM32G4\STM32CubeG4-master\Drivers\CMSIS\Device\ST\STM32G4xx\Source\Templates\iar\startup_stm32g431xx.s</name>