Initial commit

.gitignore

@@ -0,0 +1,8 @@
+.metadata/
+settings/
+Debug/
+.vs/
+obj/
+bin/
+x64/
+x86/

Source/BSP/Inc/imu.h

@@ -0,0 +1,34 @@
+#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_RATE_HZ             1000                    // 1 ms
+#define IMU_DT                  0.002f   // 2 ms
+
+typedef struct
+{
+  int16_t ax, ay, az; // lsb
+  int16_t gx, gy, gz; // lsb
+} imu_raw_t;
+
+void i2c_gpio_init();
+
+void i2c1_init();
+
+void icm_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);
+
+static void i2c_wait_idle(I2C_TypeDef* i2c);
+
+#endif

Source/BSP/Inc/imu_processing.h

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

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

@@ -0,0 +1,34 @@
+#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
+{
+  int16_t rc_roll;     // -500 - 500
+  int16_t rc_pitch;    // -500 - 500
+  int16_t rc_throttle; // 1000 - 2000
+  int16_t rc_yaw;      // -500 - 500
+  int16_t 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);
+int8_t bool_mapping_gt(int16_t x, int16_t boundary);
+
+
+void led_init();
+void toggle_led();
+
+#endif

Source/BSP/Src/imu.c

@@ -0,0 +1,120 @@
+#include "imu.h"
+
+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 icm_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, (3 << 1));
+  
+  // accel ~4g, FS_SEL = 1
+  i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
+  
+  // back to bank 0
+  i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
+}
+
+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 icm_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

@@ -0,0 +1,91 @@
+#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, 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(&gyro_x_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);
+}
+
+void imu_read_scaled(imu_scaled_t* out)
+{
+  static imu_raw_t raw;
+  
+  icm_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;
+  
+  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)
+  {
+    icm_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/motors.c

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

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

Source/Control/Inc/attitude.h

@@ -0,0 +1,35 @@
+#ifndef ATTITUDE_H
+#define ATTITUDE_H
+
+#include "imu_processing.h"
+#include "radio_receiver.h"
+#include "pid.h"
+
+#define CF_ALPHA        0.99f
+
+typedef struct
+{
+  float roll;   // deg
+  float pitch;  // deg
+  float yaw_rate;    // deg/s
+} attitude_t;
+
+void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);
+void attitude_update(attitude_t* attitude, imu_scaled_t* imu);
+void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu);
+void attitude_pid_update(control_channels_t* control, 
+                         const rc_channels* rx, 
+                         const attitude_t* att, 
+                         const imu_scaled_t* imu);
+void TIM6_DAC_IRQHandler();
+
+float accel_roll_deg(const imu_scaled_t* imu);
+float accel_pitch_deg(const imu_scaled_t* imu);
+
+void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu);
+void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu);
+void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu);
+
+
+
+#endif

Source/Control/Inc/biquad_filter.h

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

@@ -0,0 +1,26 @@
+#ifndef PID_H
+#define PID_H
+
+#include "stm32g431xx.h"
+
+typedef struct
+{
+  float kp;
+  float ki;
+  float kd;
+  
+  float integral;
+  float prev_error;
+} pid_t;
+
+typedef struct
+{
+  float roll;
+  float pitch;
+  float yaw;
+} control_channels_t;
+
+float pid_update(pid_t* pid, float error, float gyro_rate, float dt);
+
+
+#endif

Source/Control/Src/attitude.c

@@ -0,0 +1,129 @@
+#include "attitude.h"
+#include "pid.h"
+#include <math.h>
+
+volatile uint8_t imu_update_flag = 0;
+volatile uint8_t pid_update_flag = 0;
+
+float angle_kp_roll = 2.5f;
+float angle_kp_pitch = 2.5f;
+
+pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
+pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
+pid_t pid_yaw = {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f};
+
+int16_t desired_roll = 0;
+int16_t desired_pitch = 0;
+
+float desired_roll_rate = 0.0f;
+float desired_pitch_rate = 0.0f;
+
+float roll_rate_error = 0.0f;
+float pitch_rate_error = 0.0f;
+float yaw_rate_error = 0.0f;
+
+float error_roll = 0.0f;
+float error_pitch = 0.0f;
+float error_yaw = 0.0f;
+
+void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
+{
+  static float roll_acc;
+  static float pitch_acc;
+  
+  roll_acc = accel_roll_deg(imu);
+  pitch_acc = accel_pitch_deg(imu);
+  
+  integrate_gyro_roll_deg(&att->roll, imu);
+  integrate_gyro_pitch_deg(&att->pitch, imu);
+  
+  att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_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)
+  {
+    imu_update_flag = 0;
+    
+    imu_read_scaled(imu);
+    complementary_filter_update(attitude, imu);
+    yaw_rate_update(attitude, imu);
+  }
+}
+
+void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
+{
+  attitude->yaw_rate = imu->gz;
+}
+
+void attitude_pid_update(control_channels_t* control, 
+                         const rc_channels* rx, 
+                         const attitude_t* att,
+                         const imu_scaled_t* imu)
+{
+  if (pid_update_flag)
+  {
+    pid_update_flag = 0;
+    
+    desired_roll  = int_mapping(rx->rc_roll, -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_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_pitch_rate > 200)  desired_pitch_rate = 200;
+    if (desired_pitch_rate < -200) desired_pitch_rate = -200;
+    
+    roll_rate_error = desired_roll_rate - imu->gy;
+    pitch_rate_error = desired_pitch_rate - imu->gx;
+    yaw_rate_error = - imu->gz;
+    
+    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->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
+  }
+}
+
+void TIM6_DAC_IRQHandler()
+{
+  if (TIM6->SR & TIM_SR_UIF)
+  {
+    TIM6->SR &= ~TIM_SR_UIF;
+    imu_update_flag = 1;
+    pid_update_flag = 1;
+  }
+}
+
+float accel_roll_deg(const imu_scaled_t* imu) {
+  // right-left
+  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
+  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
+  *roll += imu->gy * IMU_DT;
+}
+
+void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu)
+{
+  // forward-backward
+  *pitch += imu->gx * IMU_DT;
+}
+
+void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu)
+{
+  // forward-backward
+  *yaw += imu->gz * IMU_DT;
+}

Source/Control/Src/biquad_filter.c

@@ -0,0 +1,43 @@
+#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/Control/Src/pid.c

@@ -0,0 +1,23 @@
+#include "pid.h"
+
+float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
+{
+  float p = pid->kp * error;
+  
+  pid->integral += error * dt;
+  if (pid->integral > 100) pid->integral = 100;
+  if (pid->integral < -100) pid->integral = -100;
+  
+  float i = pid->ki * pid->integral;
+  
+  float d = - pid->kd * gyro_rate;
+  
+  pid->prev_error = error;
+  
+  float spid = p + i + d;
+  
+  if (spid > 300) spid = 300;
+  if (spid < -300) spid = -300;
+  
+  return spid;
+}

Source/Core/Inc/system_stm32g4xx.h

@@ -0,0 +1,104 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32g4xx.h
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M4 Device System Source File for STM32G4xx devices.
+  ******************************************************************************
+  * @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.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32g4xx_system
+  * @{
+  */
+
+/**
+  * @brief Define to prevent recursive inclusion
+  */
+#ifndef __SYSTEM_STM32G4XX_H
+#define __SYSTEM_STM32G4XX_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/** @addtogroup STM32G4xx_System_Includes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+
+/** @addtogroup STM32G4xx_System_Exported_Variables
+  * @{
+  */
+  /* The SystemCoreClock variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetSysClockFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+extern uint32_t SystemCoreClock;            /*!< System Clock Frequency (Core Clock) */
+
+extern const uint8_t  AHBPrescTable[16];    /*!< AHB prescalers table values */
+extern const uint8_t  APBPrescTable[8];     /*!< APB prescalers table values */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Exported_Constants
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Exported_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Exported_Functions
+  * @{
+  */
+
+extern void SystemInit(void);
+extern void SystemCoreClockUpdate(void);
+/**
+  * @}
+  */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_STM32G4XX_H */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */

Source/Core/Inc/timer.h

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

Source/Core/Src/stm32g4xx_it.c

@@ -0,0 +1,166 @@
+/**
+  ******************************************************************************
+  * @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

@@ -0,0 +1,49 @@
+/**
+  ******************************************************************************
+  * @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/system_stm32g4xx.c

@@ -0,0 +1,285 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32g4xx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File
+  *
+  *   This file provides two functions and one global variable to be called from
+  *   user application:
+  *      - SystemInit(): This function is called at startup just after reset and
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32g4xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick
+  *                                  timer or configure other parameters.
+  *
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  *   After each device reset the HSI (16 MHz) is used as system clock source.
+  *   Then SystemInit() function is called, in "startup_stm32g4xx.s" file, to
+  *   configure the system clock before to branch to main program.
+  *
+  *   This file configures the system clock as follows:
+  *=============================================================================
+  *-----------------------------------------------------------------------------
+  *        System Clock source                    | HSI
+  *-----------------------------------------------------------------------------
+  *        SYSCLK(Hz)                             | 16000000
+  *-----------------------------------------------------------------------------
+  *        HCLK(Hz)                               | 16000000
+  *-----------------------------------------------------------------------------
+  *        AHB Prescaler                          | 1
+  *-----------------------------------------------------------------------------
+  *        APB1 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        APB2 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        PLL_M                                  | 1
+  *-----------------------------------------------------------------------------
+  *        PLL_N                                  | 16
+  *-----------------------------------------------------------------------------
+  *        PLL_P                                  | 7
+  *-----------------------------------------------------------------------------
+  *        PLL_Q                                  | 2
+  *-----------------------------------------------------------------------------
+  *        PLL_R                                  | 2
+  *-----------------------------------------------------------------------------
+  *        Require 48MHz for RNG                  | Disabled
+  *-----------------------------------------------------------------------------
+  *=============================================================================
+  ******************************************************************************
+  * @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.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32g4xx_system
+  * @{
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32g4xx.h"
+
+#if !defined  (HSE_VALUE)
+  #define HSE_VALUE     24000000U /*!< Value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    16000000U /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Defines
+  * @{
+  */
+
+/************************* Miscellaneous Configuration ************************/
+/* Note: Following vector table addresses must be defined in line with linker
+         configuration. */
+/*!< Uncomment the following line if you need to relocate the vector table
+     anywhere in Flash or Sram, else the vector table is kept at the automatic
+     remap of boot address selected */
+/* #define USER_VECT_TAB_ADDRESS */
+
+#if defined(USER_VECT_TAB_ADDRESS)
+/*!< Uncomment the following line if you need to relocate your vector Table
+     in Sram else user remap will be done in Flash. */
+/* #define VECT_TAB_SRAM */
+#if defined(VECT_TAB_SRAM)
+#define VECT_TAB_BASE_ADDRESS   SRAM_BASE       /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#else
+#define VECT_TAB_BASE_ADDRESS   FLASH_BASE      /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_SRAM */
+#if !defined(VECT_TAB_OFFSET)
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table offset field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_OFFSET */
+#endif /* USER_VECT_TAB_ADDRESS */
+/******************************************************************************/
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Variables
+  * @{
+  */
+  /* The SystemCoreClock variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+  uint32_t SystemCoreClock = HSI_VALUE;
+
+  const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U};
+  const uint8_t APBPrescTable[8] =  {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system.
+  * @param  None
+  * @retval None
+  */
+
+void SystemInit(void)
+{
+  /* FPU settings ------------------------------------------------------------*/
+  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+    SCB->CPACR |= ((3UL << (10*2))|(3UL << (11*2)));  /* set CP10 and CP11 Full Access */
+  #endif
+
+  /* Configure the Vector Table location add offset address ------------------*/
+#if defined(USER_VECT_TAB_ADDRESS)
+  SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
+#endif /* USER_VECT_TAB_ADDRESS */
+}
+
+/**
+  * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.
+  *
+  * @note   - The system frequency computed by this function is not the real
+  *           frequency in the chip. It is calculated based on the predefined
+  *           constant and the selected clock source:
+  *
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**)
+  *
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***)
+  *
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***)
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *
+  *         (**) HSI_VALUE is a constant defined in stm32g4xx_hal.h file (default value
+  *              16 MHz) but the real value may vary depending on the variations
+  *              in voltage and temperature.
+  *
+  *         (***) HSE_VALUE is a constant defined in stm32g4xx_hal.h file (default value
+  *              24 MHz), user has to ensure that HSE_VALUE is same as the real
+  *              frequency of the crystal used. Otherwise, this function may
+  *              have wrong result.
+  *
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate(void)
+{
+  uint32_t tmp, pllvco, pllr, pllsource, pllm;
+
+  /* Get SYSCLK source -------------------------------------------------------*/
+  switch (RCC->CFGR & RCC_CFGR_SWS)
+  {
+    case 0x04:  /* HSI used as system clock source */
+      SystemCoreClock = HSI_VALUE;
+      break;
+
+    case 0x08:  /* HSE used as system clock source */
+      SystemCoreClock = HSE_VALUE;
+      break;
+
+    case 0x0C:  /* PLL used as system clock  source */
+      /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN
+         SYSCLK = PLL_VCO / PLLR
+         */
+      pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
+      pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U ;
+      if (pllsource == 0x02UL) /* HSI used as PLL clock source */
+      {
+        pllvco = (HSI_VALUE / pllm);
+      }
+      else                   /* HSE used as PLL clock source */
+      {
+        pllvco = (HSE_VALUE / pllm);
+      }
+      pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8);
+      pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U;
+      SystemCoreClock = pllvco/pllr;
+      break;
+
+    default:
+      break;
+  }
+  /* Compute HCLK clock frequency --------------------------------------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;
+}
+
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+

Source/Core/Src/timer.c

@@ -0,0 +1,14 @@
+#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/Drivers/CMSIS/Device/ST/STM32G4xx/Include/stm32g4xx.h

@@ -0,0 +1,269 @@
+/**
+  ******************************************************************************
+  * @file    stm32g4xx.h
+  * @author  MCD Application Team
+  * @brief   CMSIS STM32G4xx Device Peripheral Access Layer Header File.
+  *
+  *          The file is the unique include file that the application programmer
+  *          is using in the C source code, usually in main.c. This file contains:
+  *           - Configuration section that allows to select:
+  *              - The STM32G4xx device used in the target application
+  *              - To use or not the peripheral<61>s drivers in application code(i.e.
+  *                code will be based on direct access to peripheral<61>s registers
+  *                rather than drivers API), this option is controlled by
+  *                "#define USE_HAL_DRIVER"
+  *
+  ******************************************************************************
+  * @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.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32g4xx
+  * @{
+  */
+
+#ifndef __STM32G4xx_H
+#define __STM32G4xx_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif /* __cplusplus */
+
+/** @addtogroup Library_configuration_section
+  * @{
+  */
+
+/**
+  * @brief STM32 Family
+  */
+#if !defined (STM32G4)
+#define STM32G4
+#endif /* STM32G4 */
+
+/* Uncomment the line below according to the target STM32G4 device used in your
+   application
+  */
+
+#if !defined (STM32G431xx) && !defined (STM32G441xx) && !defined (STM32G471xx) && \
+    !defined (STM32G473xx) && !defined (STM32G474xx) && !defined (STM32G484xx) && \
+    !defined (STM32GBK1CB) && !defined (STM32G491xx) && !defined (STM32G4A1xx) && \
+    !defined (STM32G411xB) && !defined (STM32G411xC) && !defined (STM32G414xx)
+  /* #define STM32G411xB */   /*!< STM32G411xB Devices */
+  /* #define STM32G411xC */   /*!< STM32G411xC Devices */
+  /* #define STM32G414xx */   /*!< STM32G414xx Devices */
+  #define STM32G431xx    /*!< STM32G431xx Devices */
+  /* #define STM32G441xx */   /*!< STM32G441xx Devices */
+  /* #define STM32G471xx */   /*!< STM32G471xx Devices */
+  /* #define STM32G473xx */   /*!< STM32G473xx Devices */
+  /* #define STM32G483xx */   /*!< STM32G483xx Devices */
+  /* #define STM32G474xx */   /*!< STM32G474xx Devices */
+  /* #define STM32G484xx */   /*!< STM32G484xx Devices */
+  /* #define STM32G491xx */   /*!< STM32G491xx Devices */
+  /* #define STM32G4A1xx */   /*!< STM32G4A1xx Devices */
+  /* #define STM32GBK1CB */   /*!< STM32GBK1CB Devices */
+#endif
+
+/*  Tip: To avoid modifying this file each time you need to switch between these
+        devices, you can define the device in your toolchain compiler preprocessor.
+  */
+#if !defined  (USE_HAL_DRIVER)
+/**
+ * @brief Comment the line below if you will not use the peripherals drivers.
+   In this case, these drivers will not be included and the application code will
+   be based on direct access to peripherals registers
+   */
+  /*#define USE_HAL_DRIVER */
+#endif /* USE_HAL_DRIVER */
+
+/**
+  * @brief CMSIS Device version number V1.2.5
+  */
+#define __STM32G4_CMSIS_VERSION_MAIN   (0x01U) /*!< [31:24] main version */
+#define __STM32G4_CMSIS_VERSION_SUB1   (0x02U) /*!< [23:16] sub1 version */
+#define __STM32G4_CMSIS_VERSION_SUB2   (0x05U) /*!< [15:8]  sub2 version */
+#define __STM32G4_CMSIS_VERSION_RC     (0x00U) /*!< [7:0]  release candidate */
+#define __STM32G4_CMSIS_VERSION        ((__STM32G4_CMSIS_VERSION_MAIN << 24)\
+                                       |(__STM32G4_CMSIS_VERSION_SUB1 << 16)\
+                                       |(__STM32G4_CMSIS_VERSION_SUB2 << 8 )\
+                                       |(__STM32G4_CMSIS_VERSION_RC))
+
+/**
+  * @}
+  */
+
+/** @addtogroup Device_Included
+  * @{
+  */
+
+#if defined(STM32G431xx)
+  #include "stm32g431xx.h"
+#elif defined(STM32G441xx)
+  #include "stm32g441xx.h"
+#elif defined(STM32G471xx)
+  #include "stm32g471xx.h"
+#elif defined(STM32G473xx)
+  #include "stm32g473xx.h"
+#elif defined(STM32G483xx)
+  #include "stm32g483xx.h"
+#elif defined(STM32G474xx)
+  #include "stm32g474xx.h"
+#elif defined(STM32G484xx)
+  #include "stm32g484xx.h"
+#elif defined(STM32G491xx)
+  #include "stm32g491xx.h"
+#elif defined(STM32G4A1xx)
+  #include "stm32g4a1xx.h"
+#elif defined(STM32GBK1CB)
+  #include "stm32gbk1cb.h"
+#elif defined(STM32G411xB)
+  #include "stm32g411xb.h"
+#elif defined(STM32G411xC)
+  #include "stm32g411xc.h"
+#elif defined(STM32G414xx)
+  #include "stm32g414xx.h"
+#else
+  #error "Please select first the target STM32G4xx device used in your application (in stm32g4xx.h file)"
+#endif
+
+/**
+  * @}
+  */
+
+/** @addtogroup Exported_types
+  * @{
+  */
+typedef enum
+{
+  RESET = 0,
+  SET = !RESET
+} FlagStatus, ITStatus;
+
+typedef enum
+{
+  DISABLE = 0,
+  ENABLE = !DISABLE
+} FunctionalState;
+#define IS_FUNCTIONAL_STATE(STATE) (((STATE) == DISABLE) || ((STATE) == ENABLE))
+
+typedef enum
+{
+  SUCCESS = 0,
+  ERROR = !SUCCESS
+} ErrorStatus;
+
+/**
+  * @}
+  */
+
+
+/** @addtogroup Exported_macros
+  * @{
+  */
+#define SET_BIT(REG, BIT)     ((REG) |= (BIT))
+
+#define CLEAR_BIT(REG, BIT)   ((REG) &= ~(BIT))
+
+#define READ_BIT(REG, BIT)    ((REG) & (BIT))
+
+#define CLEAR_REG(REG)        ((REG) = (0x0))
+
+#define WRITE_REG(REG, VAL)   ((REG) = (VAL))
+
+#define READ_REG(REG)         ((REG))
+
+#define MODIFY_REG(REG, CLEARMASK, SETMASK)  WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK)))
+
+#define POSITION_VAL(VAL)     (__CLZ(__RBIT(VAL)))
+
+/* Use of CMSIS compiler intrinsics for register exclusive access */
+/* Atomic 32-bit register access macro to set one or several bits */
+#define ATOMIC_SET_BIT(REG, BIT)                             \
+  do {                                                       \
+    uint32_t val;                                            \
+    do {                                                     \
+      val = __LDREXW((__IO uint32_t *)&(REG)) | (BIT);       \
+    } while ((__STREXW(val,(__IO uint32_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 32-bit register access macro to clear one or several bits */
+#define ATOMIC_CLEAR_BIT(REG, BIT)                           \
+  do {                                                       \
+    uint32_t val;                                            \
+    do {                                                     \
+      val = __LDREXW((__IO uint32_t *)&(REG)) & ~(BIT);      \
+    } while ((__STREXW(val,(__IO uint32_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 32-bit register access macro to clear and set one or several bits */
+#define ATOMIC_MODIFY_REG(REG, CLEARMSK, SETMASK)                          \
+  do {                                                                     \
+    uint32_t val;                                                          \
+    do {                                                                   \
+      val = (__LDREXW((__IO uint32_t *)&(REG)) & ~(CLEARMSK)) | (SETMASK); \
+    } while ((__STREXW(val,(__IO uint32_t *)&(REG))) != 0U);               \
+  } while(0)
+
+/* Atomic 16-bit register access macro to set one or several bits */
+#define ATOMIC_SETH_BIT(REG, BIT)                            \
+  do {                                                       \
+    uint16_t val;                                            \
+    do {                                                     \
+      val = __LDREXH((__IO uint16_t *)&(REG)) | (BIT);       \
+    } while ((__STREXH(val,(__IO uint16_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 16-bit register access macro to clear one or several bits */
+#define ATOMIC_CLEARH_BIT(REG, BIT)                          \
+  do {                                                       \
+    uint16_t val;                                            \
+    do {                                                     \
+      val = __LDREXH((__IO uint16_t *)&(REG)) & ~(BIT);      \
+    } while ((__STREXH(val,(__IO uint16_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 16-bit register access macro to clear and set one or several bits */
+#define ATOMIC_MODIFYH_REG(REG, CLEARMSK, SETMASK)                         \
+  do {                                                                     \
+    uint16_t val;                                                          \
+    do {                                                                   \
+      val = (__LDREXH((__IO uint16_t *)&(REG)) & ~(CLEARMSK)) | (SETMASK); \
+    } while ((__STREXH(val,(__IO uint16_t *)&(REG))) != 0U);               \
+  } while(0)
+
+
+/**
+  * @}
+  */
+
+#if defined (USE_HAL_DRIVER)
+ #include "stm32g4xx_hal.h"
+#endif /* USE_HAL_DRIVER */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* __STM32G4xx_H */
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+
+
+

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

@@ -0,0 +1,19 @@
+#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

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

Source/main.c

@@ -0,0 +1,73 @@
+#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"
+
+
+imu_scaled_t imu;
+attitude_t attitude;
+rc_channels rx_chs_raw;
+rc_channels rx_chs_normalized;
+control_channels_t ctrl_chs;
+
+void delay_ms(uint32_t ms);
+
+  int main(void)
+{
+  __enable_irq();
+  
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
+  GPIOC->MODER &= ~(3 << (13 * 2));
+  GPIOC->MODER |= 1 << (13 * 2);
+  GPIOC->OTYPER &= ~(1 << 13);
+  GPIOC->PUPDR &= ~(3 << (13 * 2));
+  GPIOC->BSRR = 1 << (13 + 16);
+  
+  delay_ms(200);
+  
+  NVIC_SetPriority(TIM6_DAC_IRQn, 1);
+  NVIC_SetPriority(LPUART1_IRQn, 0);
+  
+  i2c_gpio_init();
+  i2c1_init();
+  icm_init();
+  imu_processing_init();
+  tim6_init();
+  
+  imu_calibrate();
+  
+  receiver_init();
+  
+  motors_init();
+  
+  while (1)
+  {
+    attitude_update(&attitude, &imu);
+    
+    receiver_update(&rx_chs_raw);
+    rx_chs_normalized = normalize_channels(rx_chs_raw);
+    
+    attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
+    
+    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 delay_ms(uint32_t ms)
+{
+    for (uint32_t i = 0; i < ms * 4000; i++);
+}
+
+
+

Source/startup_stm32g431xx.s

@@ -0,0 +1,585 @@
+;*******************************************************************************
+;* @File Name          : startup_stm32g431xx.s
+;* @Author             : MCD Application Team
+;* @Brief              : STM32G431xx Devices vector
+;*******************************************************************************
+;* Description        : This module performs:
+;*                      - Set the initial SP
+;*                      - Set the initial PC == _iar_program_start,
+;*                      - Set the vector table entries with the exceptions ISR
+;*                        address.
+;*                      - Branches to main in the C library (which eventually
+;*                        calls main()).
+;*                      After Reset the Cortex-M4 processor is in Thread mode,
+;*                      priority is Privileged, and the Stack is set to Main.
+;********************************************************************************
+;* @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.
+;
+;*******************************************************************************
+;
+; The modules in this file are included in the libraries, and may be replaced
+; by any user-defined modules that define the PUBLIC symbol _program_start or
+; a user defined start symbol.
+; To override the cstartup defined in the library, simply add your modified
+; version to the workbench project.
+;
+; The vector table is normally located at address 0.
+; When debugging in RAM, it can be located in RAM, aligned to at least 2^6.
+; The name "__vector_table" has special meaning for C-SPY:
+; it is where the SP start value is found, and the NVIC vector
+; table register (VTOR) is initialized to this address if != 0.
+;
+; Cortex-M version
+;
+
+        MODULE  ?cstartup
+
+        ;; Forward declaration of sections.
+        SECTION CSTACK:DATA:NOROOT(3)
+
+        SECTION .intvec:CODE:NOROOT(2)
+
+        EXTERN  __iar_program_start
+        EXTERN  SystemInit
+        PUBLIC  __vector_table
+
+        DATA
+__vector_table
+        DCD     sfe(CSTACK)
+        DCD     Reset_Handler             ; Reset Handler
+
+        DCD     NMI_Handler               ; NMI Handler
+        DCD     HardFault_Handler         ; Hard Fault Handler
+        DCD     MemManage_Handler         ; MPU Fault Handler
+        DCD     BusFault_Handler          ; Bus Fault Handler
+        DCD     UsageFault_Handler        ; Usage Fault Handler
+        DCD     0                         ; Reserved
+        DCD     0                         ; Reserved
+        DCD     0                         ; Reserved
+        DCD     0                         ; Reserved
+        DCD     SVC_Handler               ; SVCall Handler
+        DCD     DebugMon_Handler          ; Debug Monitor Handler
+        DCD     0                         ; Reserved
+        DCD     PendSV_Handler            ; PendSV Handler
+        DCD     SysTick_Handler           ; SysTick Handler
+
+         ; External Interrupts
+        DCD     WWDG_IRQHandler                   ; Window WatchDog
+        DCD     PVD_PVM_IRQHandler                ; PVD/PVM1/PVM2/PVM3/PVM4 through EXTI Line detection
+        DCD     RTC_TAMP_LSECSS_IRQHandler        ; RTC, TAMP and RCC LSE_CSS through the EXTI line
+        DCD     RTC_WKUP_IRQHandler               ; RTC Wakeup through the EXTI line
+        DCD     FLASH_IRQHandler                  ; FLASH
+        DCD     RCC_IRQHandler                    ; RCC
+        DCD     EXTI0_IRQHandler                  ; EXTI Line0
+        DCD     EXTI1_IRQHandler                  ; EXTI Line1
+        DCD     EXTI2_IRQHandler                  ; EXTI Line2
+        DCD     EXTI3_IRQHandler                  ; EXTI Line3
+        DCD     EXTI4_IRQHandler                  ; EXTI Line4
+        DCD     DMA1_Channel1_IRQHandler          ; DMA1 Channel 1
+        DCD     DMA1_Channel2_IRQHandler          ; DMA1 Channel 2
+        DCD     DMA1_Channel3_IRQHandler          ; DMA1 Channel 3
+        DCD     DMA1_Channel4_IRQHandler          ; DMA1 Channel 4
+        DCD     DMA1_Channel5_IRQHandler          ; DMA1 Channel 5
+        DCD     DMA1_Channel6_IRQHandler          ; DMA1 Channel 6
+        DCD     0                                 ; Reserved
+        DCD     ADC1_2_IRQHandler                 ; ADC1 and ADC2
+        DCD     USB_HP_IRQHandler                 ; USB Device High Priority
+        DCD     USB_LP_IRQHandler                 ; USB Device Low Priority
+        DCD     FDCAN1_IT0_IRQHandler             ; FDCAN1 interrupt line 0
+        DCD     FDCAN1_IT1_IRQHandler             ; FDCAN1 interrupt line 1
+        DCD     EXTI9_5_IRQHandler                ; External Line[9:5]s
+        DCD     TIM1_BRK_TIM15_IRQHandler         ; TIM1 Break, Transition error, Index error and TIM15
+        DCD     TIM1_UP_TIM16_IRQHandler          ; TIM1 Update and TIM16
+        DCD     TIM1_TRG_COM_TIM17_IRQHandler     ; TIM1 Trigger, Commutation, Direction change, Index and TIM17
+        DCD     TIM1_CC_IRQHandler                ; TIM1 Capture Compare
+        DCD     TIM2_IRQHandler                   ; TIM2
+        DCD     TIM3_IRQHandler                   ; TIM3
+        DCD     TIM4_IRQHandler                   ; TIM4
+        DCD     I2C1_EV_IRQHandler                ; I2C1 Event
+        DCD     I2C1_ER_IRQHandler                ; I2C1 Error
+        DCD     I2C2_EV_IRQHandler                ; I2C2 Event
+        DCD     I2C2_ER_IRQHandler                ; I2C2 Error
+        DCD     SPI1_IRQHandler                   ; SPI1
+        DCD     SPI2_IRQHandler                   ; SPI2
+        DCD     USART1_IRQHandler                 ; USART1
+        DCD     USART2_IRQHandler                 ; USART2
+        DCD     USART3_IRQHandler                 ; USART3
+        DCD     EXTI15_10_IRQHandler              ; External Line[15:10]
+        DCD     RTC_Alarm_IRQHandler              ; RTC Alarm (A and B) through EXTI Line
+        DCD     USBWakeUp_IRQHandler              ; USB Wakeup through EXTI line
+        DCD     TIM8_BRK_IRQHandler               ; TIM8 Break, Transition error and Index error Interrupt
+        DCD     TIM8_UP_IRQHandler                ; TIM8 Update Interrupt
+        DCD     TIM8_TRG_COM_IRQHandler           ; TIM8 Trigger, Commutation, Direction change and Index Interrupt
+        DCD     TIM8_CC_IRQHandler                ; TIM8 Capture Compare Interrupt
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     LPTIM1_IRQHandler                 ; LP TIM1 interrupt
+        DCD     0                                 ; Reserved
+        DCD     SPI3_IRQHandler                   ; SPI3
+        DCD     UART4_IRQHandler                  ; UART4
+        DCD     0                                 ; Reserved
+        DCD     TIM6_DAC_IRQHandler               ; TIM6 and DAC1&3 underrun errors
+        DCD     TIM7_IRQHandler                   ; TIM7
+        DCD     DMA2_Channel1_IRQHandler          ; DMA2 Channel 1
+        DCD     DMA2_Channel2_IRQHandler          ; DMA2 Channel 2
+        DCD     DMA2_Channel3_IRQHandler          ; DMA2 Channel 3
+        DCD     DMA2_Channel4_IRQHandler          ; DMA2 Channel 4
+        DCD     DMA2_Channel5_IRQHandler          ; DMA2 Channel 5
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     UCPD1_IRQHandler                  ; UCPD1
+        DCD     COMP1_2_3_IRQHandler              ; COMP1, COMP2 and COMP3
+        DCD     COMP4_IRQHandler                  ; COMP4
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     CRS_IRQHandler                    ; CRS Interrupt
+        DCD     SAI1_IRQHandler                   ; Serial Audio Interface 1 global interrupt
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     FPU_IRQHandler                    ; FPU
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     RNG_IRQHandler                    ; RNG global interrupt
+        DCD     LPUART1_IRQHandler                ; LP UART 1 interrupt
+        DCD     I2C3_EV_IRQHandler                ; I2C3 Event
+        DCD     I2C3_ER_IRQHandler                ; I2C3 Error
+        DCD     DMAMUX_OVR_IRQHandler             ; DMAMUX overrun global interrupt
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     DMA2_Channel6_IRQHandler          ; DMA2 Channel 6
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     CORDIC_IRQHandler                 ; CORDIC
+        DCD     FMAC_IRQHandler                   ; FMAC
+
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;
+;; Default interrupt handlers.
+;;
+        THUMB
+        PUBWEAK Reset_Handler
+        SECTION .text:CODE:NOROOT:REORDER(2)
+Reset_Handler
+        LDR     R0, =SystemInit
+        BLX     R0
+        LDR     R0, =__iar_program_start
+        BX      R0
+
+        PUBWEAK NMI_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+NMI_Handler
+        B NMI_Handler
+
+        PUBWEAK HardFault_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+HardFault_Handler
+        B HardFault_Handler
+
+        PUBWEAK MemManage_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+MemManage_Handler
+        B MemManage_Handler
+
+        PUBWEAK BusFault_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+BusFault_Handler
+        B BusFault_Handler
+
+        PUBWEAK UsageFault_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+UsageFault_Handler
+        B UsageFault_Handler
+
+        PUBWEAK SVC_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SVC_Handler
+        B SVC_Handler
+
+        PUBWEAK DebugMon_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DebugMon_Handler
+        B DebugMon_Handler
+
+        PUBWEAK PendSV_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+PendSV_Handler
+        B PendSV_Handler
+
+        PUBWEAK SysTick_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SysTick_Handler
+        B SysTick_Handler
+
+        PUBWEAK WWDG_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+WWDG_IRQHandler
+        B WWDG_IRQHandler
+
+        PUBWEAK PVD_PVM_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+PVD_PVM_IRQHandler
+        B PVD_PVM_IRQHandler
+
+        PUBWEAK RTC_TAMP_LSECSS_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RTC_TAMP_LSECSS_IRQHandler
+        B RTC_TAMP_LSECSS_IRQHandler
+
+        PUBWEAK RTC_WKUP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RTC_WKUP_IRQHandler
+        B RTC_WKUP_IRQHandler
+
+        PUBWEAK FLASH_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FLASH_IRQHandler
+        B FLASH_IRQHandler
+
+        PUBWEAK RCC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RCC_IRQHandler
+        B RCC_IRQHandler
+
+        PUBWEAK EXTI0_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI0_IRQHandler
+        B EXTI0_IRQHandler
+
+        PUBWEAK EXTI1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI1_IRQHandler
+        B EXTI1_IRQHandler
+
+        PUBWEAK EXTI2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI2_IRQHandler
+        B EXTI2_IRQHandler
+
+        PUBWEAK EXTI3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI3_IRQHandler
+        B EXTI3_IRQHandler
+
+        PUBWEAK EXTI4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI4_IRQHandler
+        B EXTI4_IRQHandler
+
+        PUBWEAK DMA1_Channel1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel1_IRQHandler
+        B DMA1_Channel1_IRQHandler
+
+        PUBWEAK DMA1_Channel2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel2_IRQHandler
+        B DMA1_Channel2_IRQHandler
+
+        PUBWEAK DMA1_Channel3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel3_IRQHandler
+        B DMA1_Channel3_IRQHandler
+
+        PUBWEAK DMA1_Channel4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel4_IRQHandler
+        B DMA1_Channel4_IRQHandler
+
+        PUBWEAK DMA1_Channel5_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel5_IRQHandler
+        B DMA1_Channel5_IRQHandler
+
+        PUBWEAK DMA1_Channel6_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel6_IRQHandler
+        B DMA1_Channel6_IRQHandler
+
+        PUBWEAK ADC1_2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+ADC1_2_IRQHandler
+        B ADC1_2_IRQHandler
+
+        PUBWEAK USB_HP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USB_HP_IRQHandler
+        B USB_HP_IRQHandler
+
+        PUBWEAK USB_LP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USB_LP_IRQHandler
+        B USB_LP_IRQHandler
+
+        PUBWEAK FDCAN1_IT0_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FDCAN1_IT0_IRQHandler
+        B FDCAN1_IT0_IRQHandler
+
+        PUBWEAK FDCAN1_IT1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FDCAN1_IT1_IRQHandler
+        B FDCAN1_IT1_IRQHandler
+
+        PUBWEAK EXTI9_5_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI9_5_IRQHandler
+        B EXTI9_5_IRQHandler
+
+        PUBWEAK TIM1_BRK_TIM15_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_BRK_TIM15_IRQHandler
+        B TIM1_BRK_TIM15_IRQHandler
+
+        PUBWEAK TIM1_UP_TIM16_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_UP_TIM16_IRQHandler
+        B TIM1_UP_TIM16_IRQHandler
+
+        PUBWEAK TIM1_TRG_COM_TIM17_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_TRG_COM_TIM17_IRQHandler
+        B TIM1_TRG_COM_TIM17_IRQHandler
+
+        PUBWEAK TIM1_CC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_CC_IRQHandler
+        B TIM1_CC_IRQHandler
+
+        PUBWEAK TIM2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM2_IRQHandler
+        B TIM2_IRQHandler
+
+        PUBWEAK TIM3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM3_IRQHandler
+        B TIM3_IRQHandler
+
+        PUBWEAK TIM4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM4_IRQHandler
+        B TIM4_IRQHandler
+
+        PUBWEAK I2C1_EV_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C1_EV_IRQHandler
+        B I2C1_EV_IRQHandler
+
+        PUBWEAK I2C1_ER_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C1_ER_IRQHandler
+        B I2C1_ER_IRQHandler
+
+        PUBWEAK I2C2_EV_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C2_EV_IRQHandler
+        B I2C2_EV_IRQHandler
+
+        PUBWEAK I2C2_ER_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C2_ER_IRQHandler
+        B I2C2_ER_IRQHandler
+
+        PUBWEAK SPI1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SPI1_IRQHandler
+        B SPI1_IRQHandler
+
+        PUBWEAK SPI2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SPI2_IRQHandler
+        B SPI2_IRQHandler
+
+        PUBWEAK USART1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USART1_IRQHandler
+        B USART1_IRQHandler
+
+        PUBWEAK USART2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USART2_IRQHandler
+        B USART2_IRQHandler
+
+        PUBWEAK USART3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USART3_IRQHandler
+        B USART3_IRQHandler
+
+        PUBWEAK EXTI15_10_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI15_10_IRQHandler
+        B EXTI15_10_IRQHandler
+
+        PUBWEAK RTC_Alarm_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RTC_Alarm_IRQHandler
+        B RTC_Alarm_IRQHandler
+
+        PUBWEAK USBWakeUp_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USBWakeUp_IRQHandler
+        B USBWakeUp_IRQHandler
+
+        PUBWEAK TIM8_BRK_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_BRK_IRQHandler
+        B TIM8_BRK_IRQHandler
+
+        PUBWEAK TIM8_UP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_UP_IRQHandler
+        B TIM8_UP_IRQHandler
+
+        PUBWEAK TIM8_TRG_COM_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_TRG_COM_IRQHandler
+        B TIM8_TRG_COM_IRQHandler
+
+        PUBWEAK TIM8_CC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_CC_IRQHandler
+        B TIM8_CC_IRQHandler
+
+        PUBWEAK LPTIM1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+LPTIM1_IRQHandler
+        B LPTIM1_IRQHandler
+
+        PUBWEAK SPI3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SPI3_IRQHandler
+        B SPI3_IRQHandler
+
+        PUBWEAK UART4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+UART4_IRQHandler
+        B UART4_IRQHandler
+
+        PUBWEAK TIM6_DAC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM6_DAC_IRQHandler
+        B TIM6_DAC_IRQHandler
+
+        PUBWEAK TIM7_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM7_IRQHandler
+        B TIM7_IRQHandler
+
+        PUBWEAK DMA2_Channel1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel1_IRQHandler
+        B DMA2_Channel1_IRQHandler
+
+        PUBWEAK DMA2_Channel2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel2_IRQHandler
+        B DMA2_Channel2_IRQHandler
+
+        PUBWEAK DMA2_Channel3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel3_IRQHandler
+        B DMA2_Channel3_IRQHandler
+
+        PUBWEAK DMA2_Channel4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel4_IRQHandler
+        B DMA2_Channel4_IRQHandler
+
+        PUBWEAK DMA2_Channel5_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel5_IRQHandler
+        B DMA2_Channel5_IRQHandler
+
+        PUBWEAK UCPD1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+UCPD1_IRQHandler
+        B UCPD1_IRQHandler
+
+        PUBWEAK COMP1_2_3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+COMP1_2_3_IRQHandler
+        B COMP1_2_3_IRQHandler
+
+        PUBWEAK COMP4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+COMP4_IRQHandler
+        B COMP4_IRQHandler
+
+        PUBWEAK CRS_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+CRS_IRQHandler
+        B CRS_IRQHandler
+
+        PUBWEAK SAI1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SAI1_IRQHandler
+        B SAI1_IRQHandler
+
+        PUBWEAK FPU_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FPU_IRQHandler
+        B FPU_IRQHandler
+
+        PUBWEAK RNG_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RNG_IRQHandler
+        B RNG_IRQHandler
+
+        PUBWEAK LPUART1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+LPUART1_IRQHandler
+        B LPUART1_IRQHandler
+
+        PUBWEAK I2C3_EV_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C3_EV_IRQHandler
+        B I2C3_EV_IRQHandler
+
+        PUBWEAK I2C3_ER_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C3_ER_IRQHandler
+        B I2C3_ER_IRQHandler
+
+        PUBWEAK DMAMUX_OVR_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMAMUX_OVR_IRQHandler
+        B DMAMUX_OVR_IRQHandler
+
+        PUBWEAK DMA2_Channel6_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel6_IRQHandler
+        B DMA2_Channel6_IRQHandler
+
+        PUBWEAK CORDIC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+CORDIC_IRQHandler
+        B CORDIC_IRQHandler
+
+        PUBWEAK FMAC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FMAC_IRQHandler
+        B FMAC_IRQHandler
+
+        END

drone.eww

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<workspace>
+    <project>
+        <path>$WS_DIR$\drone.ewp</path>
+    </project>
+    <batchBuild />
+</workspace>