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

Source/BSP/Inc/imu.h

@@ -3,31 +3,34 @@
 
 #include "stm32g431xx.h"
 
-#define ICM_ADDR                0x68
+#define ICM_ADDR				0x68
-#define REG_PWR_MGMT_1          0x06
+#define REG_PWR_MGMT_1			0x06
-#define REG_BANK_SEL            0x7F
+#define REG_BANK_SEL			0x7F
-#define REG_GYRO_CONFIG_1       0x01
+#define REG_GYRO_CONFIG_1		0x01
-#define REG_ACCEL_CONFIG        0x14
+#define REG_ACCEL_CONFIG		0x14
-#define IMU_RATE_HZ             1000                    // 1 ms
+#define IMU_DT					0.002f   // 2 ms
-#define IMU_DT                  0.002f   // 2 ms
 
 typedef struct
 {
-  int16_t ax, ay, az; // lsb
+	int16_t ax, ay, az; // lsb
-  int16_t gx, gy, gz; // lsb
+	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
+void imu_pow_init();
+
 void i2c_gpio_init();
 
 void i2c1_init();
 
-void icm_init();
+void imu_init();
+
+void imu_tim6_init();
 
 void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
 
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);
 
-void icm_read_raw(imu_raw_t* data);
+void imu_read_raw(imu_raw_t* data);
 
 static void i2c_wait_idle(I2C_TypeDef* i2c);
 

Source/BSP/Inc/imu_processing.h

@@ -4,14 +4,14 @@
 #include "imu.h"
 
 
-#define ACCEL_SENS_SCALE_FACTOR         8192.0f
+#define ACCEL_SENS_SCALE_FACTOR		8192.0f
-#define GYRO_SENS_SCALE_FACTOR          16.4f
+#define GYRO_SENS_SCALE_FACTOR		16.4f
-#define PI                              3.14159265359f
+#define PI							3.14159265359f
 
 typedef struct
 {
-  float ax, ay, az; // g
+	float ax, ay, az; // g
-  float gx, gy, gz; // dps
+	float gx, gy, gz; // dps
 } imu_scaled_t;
 
 void imu_processing_init();

Source/BSP/Inc/lidar.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#ifndef LIDAR_H
+#define LIDAR_H
+
+#include "stm32g431xx.h"
+
+#define USART3_START_BYTE	0x59
+#define USART3_BUF_SIZE		64
+#define USART3_FRAME_SIZE	9
+#define LIDAR_MIN_DIST		0.01f
+#define LIDAR_MAX_DIST		40.0f
+#define TF02_I2C_ADDR		0x10
+
+typedef struct
+{
+	uint8_t header1;		// 0x59
+	uint8_t header2;		// 0x59
+	uint8_t distance_l;		// cm
+ 	uint8_t distance_h;		// cm
+ 	uint8_t strength_l;
+ 	uint8_t strength_h;
+ 	uint8_t temp_l;
+ 	uint8_t temp_h;
+ 	uint8_t checksum;
+} lidar_data_buf;
+
+typedef struct
+{
+	uint16_t distance;		// cm
+	uint16_t strength;
+	uint16_t temperature;
+} lidar_data;
+
+void lidar_init();
+void lidar_tim7_init();
+void TIM7_DAC_IRQHandler();
+void USART3_IRQHandler();
+void lidar_update(lidar_data* lidar);
+
+uint8_t usart_available();
+uint8_t usart_read();
+
+void lidar_i2c2_init();
+static void i2c2_wait_txis();
+static void i2c2_wait_stop();
+static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size);
+void tf02_force_uart();
+
+#endif

Source/BSP/Inc/radio_receiver.h

@@ -9,11 +9,11 @@
 
 typedef struct
 {
-  int16_t rc_roll;     // -500 - 500
+	int16_t rc_roll;     // -500 - 500
-  int16_t rc_pitch;    // -500 - 500
+	int16_t rc_pitch;    // -500 - 500
-  int16_t rc_throttle; // 1000 - 2000
+	int16_t rc_throttle; // 1000 - 2000
-  int16_t rc_yaw;      // -500 - 500
+	int16_t rc_yaw;      // -500 - 500
-  int16_t rc_armed;    // 0/1
+	int16_t rc_armed;    // 0/1
 } rc_channels;
 
 void receiver_gpio_init();

Source/BSP/Src/imu.c

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

Source/BSP/Src/imu_processing.c

@@ -20,72 +20,72 @@ static biquad_t gyro_z_lpf;
 
 void imu_processing_init()
 {
-  biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
-  biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
-  biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
-  
+	
-  biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
+	biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
-  biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
+	biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
-  biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
+	biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
 }
 
 void imu_read_scaled(imu_scaled_t* out)
 {
-  static imu_raw_t raw;
+	static imu_raw_t raw;
-  
+	
-  icm_read_raw(&raw);
+	imu_read_raw(&raw);
-  
+	
-  out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
+	out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
-  out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
+	out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
-  out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
+	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
-  
+	
-  out->ax = biquad_apply(&accel_x_lpf, out->ax);
+	out->ax = biquad_apply(&accel_x_lpf, out->ax);
-  out->ay = biquad_apply(&accel_y_lpf, out->ay);
+	out->ay = biquad_apply(&accel_y_lpf, out->ay);
-  out->az = biquad_apply(&accel_z_lpf, out->az);
+	out->az = biquad_apply(&accel_z_lpf, out->az);
-  
+	
-  out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
+	out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
-  out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
+	out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
-  out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
+	out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
-  
+	
-  out->gx = biquad_apply(&gyro_x_lpf, out->gx);
+	out->gx = biquad_apply(&gyro_x_lpf, out->gx);
-  out->gy = biquad_apply(&gyro_y_lpf, out->gy);
+	out->gy = biquad_apply(&gyro_y_lpf, out->gy);
-  out->gz = biquad_apply(&gyro_z_lpf, out->gz);
+	out->gz = biquad_apply(&gyro_z_lpf, out->gz);
 }
 
 void imu_calibrate()
 {
-  const int samples = 1000;
+	const int samples = 1000;
-  
+	
-  float sum_ax = 0;
+	float sum_ax = 0;
-  float sum_ay = 0;
+	float sum_ay = 0;
-  float sum_az = 0;
+	float sum_az = 0;
-  
+	
-  float sum_gx = 0;
+	float sum_gx = 0;
-  float sum_gy = 0;
+	float sum_gy = 0;
-  float sum_gz = 0;
+	float sum_gz = 0;
-  
+	
-  imu_raw_t imu;
+	imu_raw_t imu;
-  
+	
-  for (uint16_t i = 0; i < samples; ++i)
+	for (uint16_t i = 0; i < samples; ++i)
-  {
+	{
-    icm_read_raw(&imu);
+		imu_read_raw(&imu);
-    
+		
-    sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
+		sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
-    sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
+		sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
-    sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
+		sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
-    
+		
-    sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
+		sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
-    sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
+		sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
-    sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
+		sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
-    
+		
-    for (volatile uint16_t d = 0; d < 5000; ++d);
+		for (volatile uint16_t d = 0; d < 5000; ++d);
-  }
+	}
-  
+	
-  accel_bias_x = sum_ax / samples;
+	accel_bias_x = sum_ax / samples;
-  accel_bias_y = sum_ay / samples;
+	accel_bias_y = sum_ay / samples;
-  accel_bias_z = (sum_az / samples) + 1.0f;
+	accel_bias_z = (sum_az / samples) + 1.0f;
-  
+	
-  gyro_bias_x = sum_gx / samples;
+	gyro_bias_x = sum_gx / samples;
-  gyro_bias_y = sum_gy / samples;
+	gyro_bias_y = sum_gy / samples;
-  gyro_bias_z = sum_gz / samples;
+	gyro_bias_z = sum_gz / samples;
 }

Source/BSP/Src/lidar.c

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

Source/BSP/Src/motors.c

@@ -3,143 +3,143 @@
 
 void motors_init()
 {
-  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
-  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+	RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
-  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
+	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
-  
+	
-  motor_gpio_tim1_ch3_init();
+	motor_gpio_tim1_ch3_init();
-  motor_gpio_tim1_ch4_init();
+	motor_gpio_tim1_ch4_init();
-  motors_tim1_ch3_4_init();
+	motors_tim1_ch3_4_init();
-  
+	
-  motor_gpio_tim2_ch1_init();
+	motor_gpio_tim2_ch1_init();
-  motor_gpio_tim2_ch2_init();
+	motor_gpio_tim2_ch2_init();
-  motors_tim2_ch1_2_init();
+	motors_tim2_ch1_2_init();
 }
 
 void motor_gpio_tim1_ch3_init()
 {
-  // set alt function mode PA10
+	// set alt function mode PA10
-  GPIOA->MODER &= ~(3 << (10 * 2));
+	GPIOA->MODER &= ~(3 << (10 * 2));
-  GPIOA->MODER |= 2 << (10 * 2);
+	GPIOA->MODER |= 2 << (10 * 2);
-  
+	
-  // AF6 for PA10
+	// AF6 for PA10
-  GPIOA->AFR[1] &= ~(0xF << 8);
+	GPIOA->AFR[1] &= ~(0xF << 8);
-  GPIOA->AFR[1] |= 6 << 8;
+	GPIOA->AFR[1] |= 6 << 8;
-  
+	
-  // very high speed
+	// very high speed
-  GPIOA->OSPEEDR |= 3 << (10 * 2);
+	GPIOA->OSPEEDR |= 3 << (10 * 2);
 }
 
 void motor_gpio_tim1_ch4_init()
 {
-  // set alt function mode PA11
+	// set alt function mode PA11
-  GPIOA->MODER &= ~(3 << (11 * 2));
+	GPIOA->MODER &= ~(3 << (11 * 2));
-  GPIOA->MODER |= 2 << (11 * 2);
+	GPIOA->MODER |= 2 << (11 * 2);
-  
+	
-  // AF11 for PA11
+	// AF11 for PA11
-  GPIOA->AFR[1] &= ~(0xF << 12);
+	GPIOA->AFR[1] &= ~(0xF << 12);
-  GPIOA->AFR[1] |= 11 << 12;
+	GPIOA->AFR[1] |= 11 << 12;
-  
+	
-  // very high speed
+	// very high speed
-  GPIOA->OSPEEDR |= 3 << (11 * 2);
+	GPIOA->OSPEEDR |= 3 << (11 * 2);
 }
 
 void motors_tim1_ch3_4_init()
 {
-  // PWM mode 1
+	// PWM mode 1
-  TIM1->CCMR2 &= ~TIM_CCMR2_OC3M;
+	TIM1->CCMR2 &= ~TIM_CCMR2_OC3M;
-  TIM1->CCMR2 |= TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2;
+	TIM1->CCMR2 |= TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2;
-  
+	
-  TIM1->CCMR2 &= ~TIM_CCMR2_OC4M;
+	TIM1->CCMR2 &= ~TIM_CCMR2_OC4M;
-  TIM1->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2;
+	TIM1->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2;
-  
+	
-  // preload enable
+	// preload enable
-  TIM1->CCMR2 |= TIM_CCMR2_OC3PE;
+	TIM1->CCMR2 |= TIM_CCMR2_OC3PE;
-  TIM1->CCMR2 |= TIM_CCMR2_OC4PE;
+	TIM1->CCMR2 |= TIM_CCMR2_OC4PE;
-  
+	
-  // enable capture/compare 3 output
+	// enable capture/compare 3 output
-  TIM1->CCER |= TIM_CCER_CC3E;
+	TIM1->CCER |= TIM_CCER_CC3E;
-  TIM1->CCER |= TIM_CCER_CC4E;
+	TIM1->CCER |= TIM_CCER_CC4E;
-  
+	
-  TIM1->PSC = 16 - 1;
+	TIM1->PSC = 16 - 1;
-  TIM1->ARR = 20000 - 1;
+	TIM1->ARR = 20000 - 1;
-  TIM1->CCR3 = 900;
+	TIM1->CCR3 = 900;
-  TIM1->CCR4 = 900;
+	TIM1->CCR4 = 900;
-  
+	
-  // TIM1_ARR is buffered
+	// TIM1_ARR is buffered
-  TIM1->CR1 |= TIM_CR1_ARPE;
+	TIM1->CR1 |= TIM_CR1_ARPE;
-  
+	
-  // set main output enable
+	// set main output enable
-  TIM1->BDTR |= TIM_BDTR_MOE;
+	TIM1->BDTR |= TIM_BDTR_MOE;
-  
+	
-  // set update generation
+	// set update generation
-  TIM1->EGR |= TIM_EGR_UG;
+	TIM1->EGR |= TIM_EGR_UG;
-  
+	
-  // set counter enable
+	// set counter enable
-  TIM1->CR1 |= TIM_CR1_CEN;
+	TIM1->CR1 |= TIM_CR1_CEN;
 }
 
 void motor_gpio_tim2_ch1_init()
 {
-  // set alt function mode PA0
+	// set alt function mode PA0
-  GPIOA->MODER &= ~(3 << (0 * 2));
+	GPIOA->MODER &= ~(3 << (0 * 2));
-  GPIOA->MODER |= 2 << (0 * 2);
+	GPIOA->MODER |= 2 << (0 * 2);
-  
+	
-  // AF1 for PA0
+	// AF1 for PA0
-  GPIOA->AFR[0] &= ~(0xF << 0);
+	GPIOA->AFR[0] &= ~(0xF << 0);
-  GPIOA->AFR[0] |= 1 << 0;
+	GPIOA->AFR[0] |= 1 << 0;
-  
+	
-  // very high speed
+	// very high speed
-  GPIOA->OSPEEDR |= 3 << (0 * 2);
+	GPIOA->OSPEEDR |= 3 << (0 * 2);
 }
 
 void motor_gpio_tim2_ch2_init()
 {
-  // set alt function mode PA1
+	// set alt function mode PA1
-  GPIOA->MODER &= ~(3 << (1 * 2));
+	GPIOA->MODER &= ~(3 << (1 * 2));
-  GPIOA->MODER |= 2 << (1 * 2);
+	GPIOA->MODER |= 2 << (1 * 2);
-  
+	
-  // AF1 for PA1
+	// AF1 for PA1
-  GPIOA->AFR[0] &= ~(0xF << 4);
+	GPIOA->AFR[0] &= ~(0xF << 4);
-  GPIOA->AFR[0] |= 1 << 4;
+	GPIOA->AFR[0] |= 1 << 4;
-  
+	
-  // very high speed
+	// very high speed
-  GPIOA->OSPEEDR |= 3 << (1 * 2);
+	 GPIOA->OSPEEDR |= 3 << (1 * 2);
 }
 
 void motors_tim2_ch1_2_init()
 {
-  // PWM mode 1
+	// PWM mode 1
-  TIM2->CCMR1 &= ~TIM_CCMR1_OC1M;
+	TIM2->CCMR1 &= ~TIM_CCMR1_OC1M;
-  TIM2->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
+	TIM2->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
-  
+	
-  TIM2->CCMR1 &= ~TIM_CCMR1_OC2M;
+	TIM2->CCMR1 &= ~TIM_CCMR1_OC2M;
-  TIM2->CCMR1 |= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2;
+	TIM2->CCMR1 |= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2;
-  
+	
-  // preload enable
+	// preload enable
-  TIM2->CCMR1 |= TIM_CCMR1_OC1PE;
+	TIM2->CCMR1 |= TIM_CCMR1_OC1PE;
-  TIM2->CCMR1 |= TIM_CCMR1_OC2PE;
+	TIM2->CCMR1 |= TIM_CCMR1_OC2PE;
-  
+	
-  // enable capture/compare 3 output
+	// enable capture/compare 3 output
-  TIM2->CCER |= TIM_CCER_CC1E;
+	TIM2->CCER |= TIM_CCER_CC1E;
-  TIM2->CCER |= TIM_CCER_CC2E;
+	TIM2->CCER |= TIM_CCER_CC2E;
-  
+	
-  TIM2->PSC = 16 - 1;
+	TIM2->PSC = 16 - 1;
-  TIM2->ARR = 20000 - 1;
+	TIM2->ARR = 20000 - 1;
-  TIM2->CCR1 = 900;
+	TIM2->CCR1 = 900;
-  TIM2->CCR2 = 900;
+	TIM2->CCR2 = 900;
-  
+	
-  // TIM2_ARR is buffered
+	// TIM2_ARR is buffered
-  TIM2->CR1 |= TIM_CR1_ARPE;
+	TIM2->CR1 |= TIM_CR1_ARPE;
-  
+	
-  // set main output enable
+	// set main output enable
-  TIM2->BDTR |= TIM_BDTR_MOE;
+	TIM2->BDTR |= TIM_BDTR_MOE;
-  
+	
-  // set update generation
+	// set update generation
-  TIM2->EGR |= TIM_EGR_UG;
+	TIM2->EGR |= TIM_EGR_UG;
-  
+	
-  // set counter enable
+	// set counter enable
-  TIM2->CR1 |= TIM_CR1_CEN;
+	TIM2->CR1 |= TIM_CR1_CEN;
 }
 
 int16_t T;
@@ -154,77 +154,77 @@ int16_t m4;
 
 void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed)
 {
-  T = throttle;
+	T = throttle;
-  P = (int16_t) chs->pitch;
+	P = (int16_t) chs->pitch;
-  R = (int16_t) chs->roll;
+	R = (int16_t) chs->roll;
-  Y = (int16_t) chs->yaw;
+	Y = (int16_t) chs->yaw;
-  
+	
-  m1 = T - P + R - Y;
+	m1 = T - P + R - Y;
-  m2 = T - P - R + Y;
+	m2 = T - P - R + Y;
-  m3 = T + P + R + Y;
+	m3 = T + P + R + Y;
-  m4 = T + P - R - Y;
+	m4 = T + P - R - Y;
-  
+	
-  motor_set_throttle(1, m1, armed);
+	motor_set_throttle(1, m1, armed);
-  motor_set_throttle(2, m2, armed);
+	motor_set_throttle(2, m2, armed);
-  motor_set_throttle(3, m3, armed);
+	motor_set_throttle(3, m3, armed);
-  motor_set_throttle(4, m4, armed);
+	motor_set_throttle(4, m4, armed);
 }
 
 void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed)
 {
-  if (armed && us < 1050) us = 1050;
+	if (armed && us < 1050) us = 1050;
-  if (us > 2000) us = 2000;
+	if (us > 2000) us = 2000;
-  
+	
-  switch (motor_number)
+	switch (motor_number)
-  {
+	{
-  case 1:
+	case 1:
-    TIM1->CCR3 = us;
+	  TIM1->CCR3 = us;
-    break;
+	  break;
-  case 2:
+	case 2:
-    TIM2->CCR2 = us;
+	  TIM2->CCR2 = us;
-    break;
+	  break;
-  case 3:
+	case 3:
-    TIM1->CCR4 = us;
+	  TIM1->CCR4 = us;
-    break;
+	  break;
-  case 4:
+	case 4:
-    TIM2->CCR1 = us;
+	  TIM2->CCR1 = us;
-    break;
+	  break;
-  }
+	}
 }
 
 void motors_turn_off()
 {
-  motor_set_throttle(1, 900, 0);
+	motor_set_throttle(1, 900, 0);
-  motor_set_throttle(2, 900, 0);
+	motor_set_throttle(2, 900, 0);
-  motor_set_throttle(3, 900, 0);
+	motor_set_throttle(3, 900, 0);
-  motor_set_throttle(4, 900, 0);
+	motor_set_throttle(4, 900, 0);
 }
 
 void motor1_set_throttle(int16_t throttle)
 {
-  if (throttle < 1000) throttle = 1000;
+	if (throttle < 1000) throttle = 1000;
-  if (throttle > 2000) throttle = 2000;
+	if (throttle > 2000) throttle = 2000;
-  TIM1->CCR3 = throttle;
+	TIM1->CCR3 = throttle;
 }
 
 void motor2_set_throttle(int16_t throttle)
 {
-  if (throttle < 1000) throttle = 1000;
+	if (throttle < 1000) throttle = 1000;
-  if (throttle > 2000) throttle = 2000;
+	if (throttle > 2000) throttle = 2000;
-  TIM1->CCR4 = throttle;
+	TIM1->CCR4 = throttle;
 }
 
 void motor3_set_throttle(int16_t throttle)
 {
-  if (throttle < 1000) throttle = 1000;
+	if (throttle < 1000) throttle = 1000;
-  if (throttle > 2000) throttle = 2000;
+	if (throttle > 2000) throttle = 2000;
-  TIM2->CCR1 = throttle;
+	TIM2->CCR1 = throttle;
 }
 
 void motor4_set_throttle(int16_t throttle)
 {
-  if (throttle < 1000) throttle = 1000;
+	if (throttle < 1000) throttle = 1000;
-  if (throttle > 2000) throttle = 2000;
+	if (throttle > 2000) throttle = 2000;
-  TIM2->CCR2 = throttle;
+	TIM2->CCR2 = throttle;
 }
 

Source/BSP/Src/radio_receiver.c

@@ -10,196 +10,159 @@ volatile uint8_t sbus_frame_ready = 0;
 
 void receiver_gpio_init()
 {
-  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
-  
+	
-  GPIOA->MODER &= ~(3 << (3 * 2));
+	GPIOA->MODER &= ~(3 << (3 * 2));
-  GPIOA->MODER |= 2 << (3 * 2);
+	GPIOA->MODER |= 2 << (3 * 2);
-  
+	
-  GPIOA->AFR[0] &= ~(0xF << (3 * 4));
+	GPIOA->AFR[0] &= ~(0xF << (3 * 4));
-  GPIOA->AFR[0] |= 12 << (3 * 4);
+	GPIOA->AFR[0] |= 12 << (3 * 4);
-  
+	
-  // pull-up
+	// pull-up
-  GPIOA->PUPDR &= ~(3 << (3 * 2));
+	GPIOA->PUPDR &= ~(3 << (3 * 2));
-  GPIOA->PUPDR |= 1 << (3 * 2);
+	GPIOA->PUPDR |= 1 << (3 * 2);
-  
 }
 
 void receiver_lpuart_clock_init()
 {
-  RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
+	RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
-  RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
+	RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
-  RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
+	RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
 }
 
 void receiver_uart_init()
 {
-  receiver_lpuart_clock_init();
+	receiver_lpuart_clock_init();
-  
+	
-  LPUART1->CR1 = 0;
+	LPUART1->CR1 = 0;
-  LPUART1->CR2 = 0;
+	LPUART1->CR2 = 0;
-  LPUART1->CR3 = 0;
+	LPUART1->CR3 = 0;
-  
+	
-  LPUART1->BRR = (256 * 16000000UL) / 100000UL;
+	LPUART1->BRR = (256 * 16000000UL) / 100000UL;
-  
+	
-  // parity control enable
+	// parity control enable
-  LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
+	LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
-  
+	
-  // word length M = 01 - 9 bit
+	// word length M = 01 - 9 bit
-  LPUART1->CR1 &= ~USART_CR1_M1;
+	LPUART1->CR1 &= ~USART_CR1_M1;
-  LPUART1->CR1 |= USART_CR1_M0;
+	LPUART1->CR1 |= USART_CR1_M0;
-  
+	
-  // even parity
+	// even parity
-  LPUART1->CR1 &= ~USART_CR1_PS;
+	LPUART1->CR1 &= ~USART_CR1_PS;
-  
+	
-  // 2 stop bits
+	// 2 stop bits
-  LPUART1->CR2 &= ~USART_CR2_STOP;
+	LPUART1->CR2 &= ~USART_CR2_STOP;
-  LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
+	LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
-  
+	
-  // invertion enabled
+	// invertion enabled
-  LPUART1->CR2 |= USART_CR2_RXINV;
+	LPUART1->CR2 |= USART_CR2_RXINV;
-  
+	
-  // receiver enable
+	// receiver enable
-  // interrupt generated whenever ORE = 1 or RXNE = 1
+	// interrupt generated whenever ORE = 1 or RXNE = 1
-  LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
+	LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
-  
+	
-  // uart enable
+	// uart enable
-  LPUART1->CR1 |= USART_CR1_UE;
+	LPUART1->CR1 |= USART_CR1_UE;
-  
+	
-  NVIC_EnableIRQ(LPUART1_IRQn);
+	NVIC_EnableIRQ(LPUART1_IRQn);
 }
 
 void receiver_init()
 {
-  receiver_gpio_init();
+	receiver_gpio_init();
-  receiver_uart_init();
+	receiver_uart_init();
 }
 
 void LPUART1_IRQHandler()
 { 
-  if (LPUART1->ISR & USART_ISR_RXNE)
+	if (LPUART1->ISR & USART_ISR_RXNE)
-  {
+	{
-    uint8_t b = LPUART1->RDR;
+		uint8_t b = LPUART1->RDR;
-    
+		
-    if (b == SBUS_START_BYTE)
+		if (b == SBUS_START_BYTE)
-      sbus_index = 0;
+			sbus_index = 0;
-    
+		
-    if (sbus_index < SBUS_FRAME_SIZE)
+		if (sbus_index < SBUS_FRAME_SIZE)
-      sbus_buffer[sbus_index++] = b;
+			sbus_buffer[sbus_index++] = b;
-    
+		
-    if (sbus_index == SBUS_FRAME_SIZE)
+		if (sbus_index == SBUS_FRAME_SIZE)
-    {
+		{
-      sbus_index = 0;
+			sbus_index = 0;
-      sbus_frame_ready = 1;
+			sbus_frame_ready = 1;
-    }
+		}
-  }
+	}
 }
 
 void receiver_update(rc_channels* chs)
 {
-  if (!sbus_frame_ready)
+	if (!sbus_frame_ready)
-    return;
+		return;
-    
+	  
-  sbus_frame_ready = 0;
+	sbus_frame_ready = 0;
-  
+	
-  if (sbus_buffer[0] != SBUS_START_BYTE)
+	if (sbus_buffer[0] != SBUS_START_BYTE)
-    return;
+		return;
-  
+	
-  sbus_failsafe   = sbus_buffer[23] & (1 << 3);
+	sbus_failsafe   = sbus_buffer[23] & (1 << 3);
-  sbus_frame_lost = sbus_buffer[23] & (1 << 2);
+	sbus_frame_lost = sbus_buffer[23] & (1 << 2);
-  
+	
-  if (sbus_failsafe || sbus_frame_lost)
+	if (sbus_failsafe || sbus_frame_lost)
-    return;
+		return;
-      
+	    
-  receiver_parse_frame();
+	receiver_parse_frame();
-  
+	
-  chs->rc_roll = sbus_channels[0];
+	chs->rc_roll = sbus_channels[0];
-  chs->rc_pitch = sbus_channels[1];
+	chs->rc_pitch = sbus_channels[1];
-  chs->rc_throttle = sbus_channels[2];
+	chs->rc_throttle = sbus_channels[2];
-  chs->rc_armed = sbus_channels[4];
+	chs->rc_armed = sbus_channels[4];
 }
 
 void receiver_parse_frame()
 {
-  uint16_t b[22];
+  	uint16_t b[22];
-  
+  	
-  for (uint8_t i = 0; i < 22; ++i)
+  	for (uint8_t i = 0; i < 22; ++i)
-    b[i] = sbus_buffer[i + 1];
+		b[i] = sbus_buffer[i + 1];
-
+	
-  sbus_channels[0]  = ( b[0]        | (b[1] << 8) )                  & 0x07FF;
+  	sbus_channels[0]  = ( b[0]        | (b[1] << 8) )                  & 0x07FF;
-  sbus_channels[1]  = ( (b[1] >> 3) | (b[2] << 5) )                  & 0x07FF;
+  	sbus_channels[1]  = ( (b[1] >> 3) | (b[2] << 5) )                  & 0x07FF;
-  sbus_channels[2]  = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) )   & 0x07FF;
+  	sbus_channels[2]  = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) )   & 0x07FF;
-  sbus_channels[3]  = ( (b[4] >> 1) | (b[5] << 7) )                  & 0x07FF;
+  	sbus_channels[3]  = ( (b[4] >> 1) | (b[5] << 7) )                  & 0x07FF;
-  sbus_channels[4]  = ( (b[5] >> 4) | (b[6] << 4) )                  & 0x07FF;
+  	sbus_channels[4]  = ( (b[5] >> 4) | (b[6] << 4) )                  & 0x07FF;
-  sbus_channels[5]  = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) )    & 0x07FF;
+  	sbus_channels[5]  = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) )    & 0x07FF;
-  sbus_channels[6]  = ( (b[8] >> 2) | (b[9] << 6) )                  & 0x07FF;
+  	sbus_channels[6]  = ( (b[8] >> 2) | (b[9] << 6) )                  & 0x07FF;
-  sbus_channels[7]  = ( (b[9] >> 5) | (b[10] << 3) )                 & 0x07FF;
+  	sbus_channels[7]  = ( (b[9] >> 5) | (b[10] << 3) )                 & 0x07FF;
-
+	
-  sbus_channels[8]  = ( b[11]       | (b[12] << 8) )                 & 0x07FF;
+  	sbus_channels[8]  = ( b[11]       | (b[12] << 8) )                 & 0x07FF;
-  sbus_channels[9]  = ( (b[12] >> 3)| (b[13] << 5) )                 & 0x07FF;
+  	sbus_channels[9]  = ( (b[12] >> 3)| (b[13] << 5) )                 & 0x07FF;
-  sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
+  	sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
-  sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) )                 & 0x07FF;
+  	sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) )                 & 0x07FF;
-  sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) )                 & 0x07FF;
+  	sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) )                 & 0x07FF;
-  sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) )  & 0x07FF;
+  	sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) )  & 0x07FF;
-  sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) )                 & 0x07FF;
+  	sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) )                 & 0x07FF;
-  sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) )                 & 0x07FF;
+  	sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) )                 & 0x07FF;
-
+	
-  sbus_frame_lost = sbus_buffer[23] & (1 << 2);
+  	sbus_frame_lost = sbus_buffer[23] & (1 << 2);
-  sbus_failsafe   = sbus_buffer[23] & (1 << 3);
+  	sbus_failsafe   = sbus_buffer[23] & (1 << 3);
 }
 
 rc_channels normalize_channels(rc_channels chs)
 {
-  chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
+	chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
-  chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
+	chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
-  chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
+	chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
-  //chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
+	//chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
-  chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
+	chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
-  
+	
-  return 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)
 {
-  return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
+	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)
 {
-  return x >= boundary;
+	return x >= boundary;
-}
+}
-
-//------------------------------------------------------------------------------
-
-void toggle_led()
-{
-    if (GPIOA->ODR & (1 << 15)) 
-    {
-        GPIOA->BSRR = 1 << (15 + 16);
-    }
-    else
-    {
-        GPIOA->BSRR = 1 << 15;
-    }
-}
-
-void led_init(void)
-{
-    /* Enable GPIOA clock */
-    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
-
-    /* PA15 -> Output mode */
-    GPIOA->MODER &= ~(3U << (15 * 2));
-    GPIOA->MODER |=  (1U << (15 * 2));
-
-    /* Push-pull */
-    GPIOA->OTYPER &= ~(1U << 15);
-
-    /* Low speed (?????????? ??? LED) */
-    GPIOA->OSPEEDR &= ~(3U << (15 * 2));
-
-    /* No pull-up / pull-down */
-    GPIOA->PUPDR &= ~(3U << (15 * 2));
-
-    /* Start with LED OFF */
-    GPIOA->BSRR = (1U << (15 + 16));
-}

Source/Control/Inc/attitude.h

@@ -5,13 +5,13 @@
 #include "radio_receiver.h"
 #include "pid.h"
 
-#define CF_ALPHA        0.99f
+#define CF_ALPHA		0.99f
 
 typedef struct
 {
-  float roll;   // deg
+	float roll;			// deg
-  float pitch;  // deg
+	float pitch;		// deg
-  float yaw_rate;    // deg/s
+	float yaw_rate;		// deg/s
 } attitude_t;
 
 void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);

Source/Control/Inc/biquad_filter.h

@@ -3,15 +3,15 @@
 
 #include <math.h>
 
-#define PI      3.14159265359f
+#define PI			3.14159265359f
 
 typedef struct
 {
-  float b0, b1, b2;
+	float b0, b1, b2;
-  float a1, a2;
+	float a1, a2;
-
+	
-  float x1, x2;
+	float x1, x2;
-  float y1, y2;
+	float y1, y2;
 } biquad_t;
 
 void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate);

Source/Control/Inc/pid.h

@@ -5,19 +5,19 @@
 
 typedef struct
 {
-  float kp;
+	float kp;
-  float ki;
+	float ki;
-  float kd;
+	float kd;
-  
+	
-  float integral;
+	float integral;
-  float prev_error;
+	float prev_error;
 } pid_t;
 
 typedef struct
 {
-  float roll;
+	float roll;
-  float pitch;
+	float pitch;
-  float yaw;
+	float yaw;
 } control_channels_t;
 
 float pid_update(pid_t* pid, float error, float gyro_rate, float dt);

Source/Control/Src/attitude.c

@@ -28,34 +28,34 @@ float error_yaw = 0.0f;
 
 void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
 {
-  static float roll_acc;
+	static float roll_acc;
-  static float pitch_acc;
+	static float pitch_acc;
-  
+	
-  roll_acc = accel_roll_deg(imu);
+	roll_acc = accel_roll_deg(imu);
-  pitch_acc = accel_pitch_deg(imu);
+	pitch_acc = accel_pitch_deg(imu);
-  
+	
-  integrate_gyro_roll_deg(&att->roll, imu);
+	integrate_gyro_roll_deg(&att->roll, imu);
-  integrate_gyro_pitch_deg(&att->pitch, imu);
+	integrate_gyro_pitch_deg(&att->pitch, imu);
-  
+	
-  att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc;
+	att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc;
-  att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc;
+	att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc;
 }
 
 void attitude_update(attitude_t* attitude, imu_scaled_t* imu)
 {
-  if (imu_update_flag)
+	if (imu_update_flag)
-  {
+	{
-    imu_update_flag = 0;
+		imu_update_flag = 0;
-    
+		
-    imu_read_scaled(imu);
+		imu_read_scaled(imu);
-    complementary_filter_update(attitude, imu);
+		complementary_filter_update(attitude, imu);
-    yaw_rate_update(attitude, imu);
+		yaw_rate_update(attitude, imu);
-  }
+	}
 }
 
 void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
 {
-  attitude->yaw_rate = imu->gz;
+	attitude->yaw_rate = imu->gz;
 }
 
 void attitude_pid_update(control_channels_t* control, 
@@ -63,67 +63,67 @@ void attitude_pid_update(control_channels_t* control,
                          const attitude_t* att,
                          const imu_scaled_t* imu)
 {
-  if (pid_update_flag)
+	if (pid_update_flag)
-  {
+	{
-    pid_update_flag = 0;
+		pid_update_flag = 0;
-    
+		
-    desired_roll  = int_mapping(rx->rc_roll, -500, 500, -45, 45);
+		desired_roll  = int_mapping(rx->rc_roll, -500, 500, -45, 45);
-    desired_pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45);
+		desired_pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45);
-    
+		
-    desired_roll_rate  = angle_kp_roll  * (desired_roll - att->roll);
+		desired_roll_rate  = angle_kp_roll  * (desired_roll - att->roll);
-    desired_pitch_rate = angle_kp_pitch * (desired_pitch - att->pitch);
+		desired_pitch_rate = angle_kp_pitch * (desired_pitch - att->pitch);
-    
+		
-    if (desired_roll_rate > 200) desired_roll_rate = 200;
+		if (desired_roll_rate > 200) desired_roll_rate = 200;
-    if (desired_roll_rate < -200) desired_roll_rate = -200;
+		if (desired_roll_rate < -200) desired_roll_rate = -200;
-
+		
-    if (desired_pitch_rate > 200)  desired_pitch_rate = 200;
+		if (desired_pitch_rate > 200)  desired_pitch_rate = 200;
-    if (desired_pitch_rate < -200) desired_pitch_rate = -200;
+		if (desired_pitch_rate < -200) desired_pitch_rate = -200;
-    
+		
-    roll_rate_error = desired_roll_rate - imu->gy;
+		roll_rate_error = desired_roll_rate - imu->gy;
-    pitch_rate_error = desired_pitch_rate - imu->gx;
+		pitch_rate_error = desired_pitch_rate - imu->gx;
-    yaw_rate_error = - imu->gz;
+		yaw_rate_error = - imu->gz;
-    
+		
-    control->roll  = pid_update(&pid_roll, roll_rate_error, imu->gy, IMU_DT);
+		control->roll  = pid_update(&pid_roll, roll_rate_error, imu->gy, IMU_DT);
-    control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT);
+		control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT);
-    control->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
+		control->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
-  }
+	}
 }
 
 void TIM6_DAC_IRQHandler()
 {
-  if (TIM6->SR & TIM_SR_UIF)
+	if (TIM6->SR & TIM_SR_UIF)
-  {
+	{
-    TIM6->SR &= ~TIM_SR_UIF;
+		TIM6->SR &= ~TIM_SR_UIF;
-    imu_update_flag = 1;
+		imu_update_flag = 1;
-    pid_update_flag = 1;
+		pid_update_flag = 1;
-  }
+	}
 }
 
 float accel_roll_deg(const imu_scaled_t* imu) {
-  // right-left
+	// right-left
-  return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI;
+	return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI;
 }
 
 float accel_pitch_deg(const imu_scaled_t* imu)
 {
-  // forward-backward
+	// forward-backward
-  return atan2f(-imu->ay, sqrtf(imu->ax * imu->ax + imu->az * imu->az)) * 180.0f / PI;
+	return atan2f(-imu->ay, sqrtf(imu->ax * imu->ax + imu->az * imu->az)) * 180.0f / PI;
 }
 
 void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu)
 {
-  // right-left
+	// right-left
-  *roll += imu->gy * IMU_DT;
+	*roll += imu->gy * IMU_DT;
 }
 
 void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu)
 {
-  // forward-backward
+	// forward-backward
-  *pitch += imu->gx * IMU_DT;
+	*pitch += imu->gx * IMU_DT;
 }
 
 void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu)
 {
-  // forward-backward
+	// forward-backward
-  *yaw += imu->gz * IMU_DT;
+	*yaw += imu->gz * IMU_DT;
 }

Source/Control/Src/biquad_filter.c

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

Source/Control/Src/pid.c

@@ -2,22 +2,22 @@
 
 float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
 {
-  float p = pid->kp * error;
+	float p = pid->kp * error;
-  
+	
-  pid->integral += error * dt;
+	pid->integral += error * dt;
-  if (pid->integral > 100) pid->integral = 100;
+	if (pid->integral > 100) pid->integral = 100;
-  if (pid->integral < -100) pid->integral = -100;
+	if (pid->integral < -100) pid->integral = -100;
-  
+	
-  float i = pid->ki * pid->integral;
+	float i = pid->ki * pid->integral;
-  
+	
-  float d = - pid->kd * gyro_rate;
+	float d = - pid->kd * gyro_rate;
-  
+	
-  pid->prev_error = error;
+	pid->prev_error = error;
-  
+	
-  float spid = p + i + d;
+	float spid = p + i + d;
-  
+	
-  if (spid > 300) spid = 300;
+	if (spid > 300) spid = 300;
-  if (spid < -300) spid = -300;
+	if (spid < -300) spid = -300;
-  
+	
-  return spid;
+	return spid;
 }

Source/Core/Inc/timer.h

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

Source/Core/Src/stm32g4xx_it.c

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

Source/Core/Src/stm32g4xx_it.h

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

Source/Core/Src/timer.c

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

Source/INS/geometry/quaternion.c

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

Source/INS/geometry/quaternion.h

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

Source/INS/geometry/vector.c

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

Source/INS/geometry/vector.h

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

Source/main.c

@@ -1,11 +1,11 @@
 #include "stm32g431xx.h"
 #include "imu.h"
 #include "imu_processing.h"
-#include "timer.h"
 #include "attitude.h"
 #include "radio_receiver.h"
 #include "motors.h"
 #include "pid.h"
+#include "lidar.h"
 
 
 imu_scaled_t imu;
@@ -13,60 +13,56 @@ attitude_t attitude;
 rc_channels rx_chs_raw;
 rc_channels rx_chs_normalized;
 control_channels_t ctrl_chs;
+lidar_data lidar;
 
 void delay_ms(uint32_t ms);
 
-  int main(void)
+int main(void)
 {
-  __enable_irq();
+	__enable_irq();
-  
+	
-  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
+	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
-  GPIOC->MODER &= ~(3 << (13 * 2));
+	NVIC_SetPriority(USART3_IRQn, 1);
-  GPIOC->MODER |= 1 << (13 * 2);
+	NVIC_SetPriority(LPUART1_IRQn, 0);
-  GPIOC->OTYPER &= ~(1 << 13);
+	
-  GPIOC->PUPDR &= ~(3 << (13 * 2));
+	imu_pow_init();
-  GPIOC->BSRR = 1 << (13 + 16);
+	i2c_gpio_init();
-  
+	i2c1_init();
-  delay_ms(200);
+	imu_init();
-  
+	imu_tim6_init();
-  NVIC_SetPriority(TIM6_DAC_IRQn, 1);
+	imu_processing_init();
-  NVIC_SetPriority(LPUART1_IRQn, 0);
+	
-  
+	imu_calibrate();
-  i2c_gpio_init();
+	
-  i2c1_init();
+	receiver_init();
-  icm_init();
+	
-  imu_processing_init();
+	motors_init();
-  tim6_init();
+	
-  
+	while (1)
-  imu_calibrate();
+	{
-  
+		attitude_update(&attitude, &imu);
-  receiver_init();
+		
-  
+		receiver_update(&rx_chs_raw);
-  motors_init();
+		rx_chs_normalized = normalize_channels(rx_chs_raw);
-  
+		
-  while (1)
+		attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
-  {
+		
-    attitude_update(&attitude, &imu);
+		lidar_update(&lidar);
-    
+		
-    receiver_update(&rx_chs_raw);
+		if (rx_chs_normalized.rc_armed)
-    rx_chs_normalized = normalize_channels(rx_chs_raw);
+		{
-    
+			motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed);
-    attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
+		}
-    
+		else
-    if (rx_chs_normalized.rc_armed)
+		{
-    {
+			motors_turn_off();
-      motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed);
+		}
-    }
+	}
-    else
-    {
-      motors_turn_off();
-    }
-  }
 }
 
 void delay_ms(uint32_t ms)
 {
-    for (uint32_t i = 0; i < ms * 4000; i++);
+	for (uint32_t i = 0; i < ms * 4000; i++);
 }
 
 

drone.ewp

@@ -362,6 +362,7 @@
                     <state>$PROJ_DIR$\Source\Core\Inc</state>
                     <state>$PROJ_DIR$\Source\BSP\Inc</state>
                     <state>$PROJ_DIR$\Source\Control\Inc</state>
+                    <state>$PROJ_DIR$\Source\INS\geometry</state>
                 </option>
                 <option>
                     <name>CCStdIncCheck</name>
@@ -2311,24 +2312,12 @@
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Inc\timer.h</name>
-                </file>
             </group>
             <group>
                 <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.h</name>
-                </file>
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\timer.c</name>
-                </file>
             </group>
         </group>
         <group>
@@ -2379,6 +2368,24 @@
                 </group>
             </group>
         </group>
+        <group>
+            <name>INS</name>
+            <group>
+                <name>geometry</name>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.h</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.h</name>
+                </file>
+            </group>
+        </group>
         <file>
             <name>$PROJ_DIR$\Source\main.c</name>
         </file>

drone.ewt

@@ -3496,24 +3496,12 @@
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Inc\timer.h</name>
-                </file>
             </group>
             <group>
                 <name>Src</name>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.c</name>
-                </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\stm32g4xx_it.h</name>
-                </file>
                 <file>
                     <name>$PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c</name>
                 </file>
-                <file>
-                    <name>$PROJ_DIR$\Source\Core\Src\timer.c</name>
-                </file>
             </group>
         </group>
         <group>
@@ -3564,6 +3552,24 @@
                 </group>
             </group>
         </group>
+        <group>
+            <name>INS</name>
+            <group>
+                <name>geometry</name>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\quaternion.h</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.c</name>
+                </file>
+                <file>
+                    <name>$PROJ_DIR$\Source\INS\geometry\vector.h</name>
+                </file>
+            </group>
+        </group>
         <file>
             <name>$PROJ_DIR$\Source\main.c</name>
         </file>