bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: bisultanov:eecf8f2cc2a0144ac81b87c5d1312e682f005c5a
Branches Tags
bisultanov:main
...
compare: bisultanov:main
Branches Tags
bisultanov:main

16 Commits
eecf8f2cc2 ... main

Author SHA1 Message Date
Radzhab Bisultanov
906e5c0610 Определены и реализованы основные функции кватернионов 2026-03-05 21:56:56 +03:00
Radzhab Bisultanov
9975e3f24c Реализованы функции вектора 2026-03-05 21:23:41 +03:00
Radzhab Bisultanov
4dde6cf046 Вектор определён 2026-03-05 17:41:09 +03:00
Radzhab Bisultanov
637dd9296f Созданы файлы для реализации векторов 2026-03-05 15:37:00 +03:00
Radzhab Bisultanov
8faf1fb3af Созданы файлы для реализации кватернионов 2026-03-05 15:34:49 +03:00
Radzhab Bisultanov
fed22e5fd7 Возвращение main в полное рабочее состояние 2026-03-04 12:14:50 +03:00
Radzhab Bisultanov
eaad822677 Изменена логика работы с UART и обработки данных. Добавлен кольцевой буфер 2026-03-03 17:51:24 +03:00
Radzhab Bisultanov
63df753fa8 Обновление данных лидара поставлено на таймер 7 2026-02-27 13:14:07 +03:00
Radzhab Bisultanov
484dcf5843 Данные с лидара читаются 2026-02-27 12:18:24 +03:00
Radzhab Bisultanov
bbd0bd2004 Доблена запись по I2C, лидар переведён на UART 2026-02-27 12:10:05 +03:00
Radzhab Bisultanov
385aa66ffc Добавлена инициализация I2C для лидара 2026-02-26 17:01:03 +03:00
Radzhab Bisultanov
8a3336c994 Лидар интегрирован в основной цикл 2026-02-25 17:07:59 +03:00
Radzhab Bisultanov
2b3e4129e8 Реализована функция lidar_update(), обновляющая данные 2026-02-20 15:16:59 +03:00
Radzhab Bisultanov
a6f47a2e73 Реализован обработчик прерывания USART3 2026-02-20 14:51:25 +03:00
Radzhab Bisultanov
6c9c1ba35b Реализована инциализация GPIOB11 и USART3 2026-02-20 12:07:24 +03:00
Radzhab Bisultanov
f9b7277a33 Наведён порядок в коде 2026-02-18 14:23:01 +03:00
26 changed files with 1235 additions and 928 deletions
Expand all files Collapse all files

25
Source/BSP/Inc/imu.h
View File

@@ -3,31 +3,34 @@
#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
#define ICM_ADDR 0x68
#define REG_PWR_MGMT_1 0x06
#define REG_BANK_SEL 0x7F
#define REG_GYRO_CONFIG_1 0x01
#define REG_ACCEL_CONFIG 0x14
#define IMU_DT 0.002f // 2 ms
typedef struct
{
int16_t ax, ay, az; // lsb
int16_t gx, gy, gz; // lsb
int16_t ax, ay, az; // lsb
int16_t gx, gy, gz; // lsb
} imu_raw_t;
void imu_pow_init();
void i2c_gpio_init();
void i2c1_init();
void 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);

10
Source/BSP/Inc/imu_processing.h
View File

@@ -4,14 +4,14 @@
#include "imu.h"
#define ACCEL_SENS_SCALE_FACTOR 8192.0f
#define GYRO_SENS_SCALE_FACTOR 16.4f
#define PI 3.14159265359f
#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
float ax, ay, az; // g
float gx, gy, gz; // dps
} imu_scaled_t;
void imu_processing_init();

50
Source/BSP/Inc/lidar.h
View File

@@ -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

10
Source/BSP/Inc/radio_receiver.h
View File

@@ -9,11 +9,11 @@
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
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();

153
Source/BSP/Src/imu.c
View File

@@ -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
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
// 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));
// 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)));
// 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);
// 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);
// 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;
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
I2C1->CR1 |= I2C_CR1_PE;
I2C1->CR1 |= I2C_CR1_PE;
}
void icm_init()
void imu_init()
{
// select bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
// 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);
// 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);
// 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));
// 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));
// 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));
// back to bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
}
void imu_tim6_init()
{
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
TIM6->PSC = 16000 - 1; // 16 MHz / 16000 = 1000 Hz (1 ms)
TIM6->ARR = 2 - 1; // 2 ms
TIM6->DIER |= TIM_DIER_UIE; // interrupt enable
TIM6->CR1 |= TIM_CR1_CEN; // counter enable
NVIC_EnableIRQ(TIM6_DAC_IRQn);
}
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
{
i2c_wait_idle(I2C1);
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
// 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_TXIS));
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)
{
while (!(I2C1->ISR & I2C_ISR_RXNE));
buf[i] = I2C1->RXDR;
}
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;
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);
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
// 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 = reg;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = data;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = data;
while (!(I2C1->ISR & I2C_ISR_TC));
I2C1->CR2 |= I2C_CR2_STOP;
while (!(I2C1->ISR & I2C_ISR_TC));
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->ay = (buf[2] << 8) | buf[3];
data->az = (buf[4] << 8) | buf[5];
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];
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);
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;

90
Source/BSP/Src/imu_processing.c
View File

@@ -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_y_lpf, 30.0f, 500.0f);
biquad_init_lpf(&accel_z_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_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);
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;
static imu_raw_t raw;
icm_read_raw(&raw);
imu_read_raw(&raw);
out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
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->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 = 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);
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;
const int samples = 1000;
float sum_ax = 0;
float sum_ay = 0;
float sum_az = 0;
float sum_ax = 0;
float sum_ay = 0;
float sum_az = 0;
float sum_gx = 0;
float sum_gy = 0;
float sum_gz = 0;
float sum_gx = 0;
float sum_gy = 0;
float sum_gz = 0;
imu_raw_t imu;
imu_raw_t imu;
for (uint16_t i = 0; i < samples; ++i)
{
icm_read_raw(&imu);
for (uint16_t i = 0; i < samples; ++i)
{
imu_read_raw(&imu);
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
sum_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;
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);
}
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;
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;
gyro_bias_x = sum_gx / samples;
gyro_bias_y = sum_gy / samples;
gyro_bias_z = sum_gz / samples;
}

214
Source/BSP/Src/lidar.c
View File

@@ -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++);
}

264
Source/BSP/Src/motors.c
View File

@@ -3,143 +3,143 @@
void motors_init()
{
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
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_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();
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);
// 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;
// AF6 for PA10
GPIOA->AFR[1] &= ~(0xF << 8);
GPIOA->AFR[1] |= 6 << 8;
// very high speed
GPIOA->OSPEEDR |= 3 << (10 * 2);
// 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);
// 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;
// AF11 for PA11
GPIOA->AFR[1] &= ~(0xF << 12);
GPIOA->AFR[1] |= 11 << 12;
// very high speed
GPIOA->OSPEEDR |= 3 << (11 * 2);
// 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;
// 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;
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;
// 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;
// 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->PSC = 16 - 1;
TIM1->ARR = 20000 - 1;
TIM1->CCR3 = 900;
TIM1->CCR4 = 900;
// TIM1_ARR is buffered
TIM1->CR1 |= TIM_CR1_ARPE;
// TIM1_ARR is buffered
TIM1->CR1 |= TIM_CR1_ARPE;
// set main output enable
TIM1->BDTR |= TIM_BDTR_MOE;
// set main output enable
TIM1->BDTR |= TIM_BDTR_MOE;
// set update generation
TIM1->EGR |= TIM_EGR_UG;
// set update generation
TIM1->EGR |= TIM_EGR_UG;
// set counter enable
TIM1->CR1 |= TIM_CR1_CEN;
// 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);
// 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;
// AF1 for PA0
GPIOA->AFR[0] &= ~(0xF << 0);
GPIOA->AFR[0] |= 1 << 0;
// very high speed
GPIOA->OSPEEDR |= 3 << (0 * 2);
// 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);
// 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;
// AF1 for PA1
GPIOA->AFR[0] &= ~(0xF << 4);
GPIOA->AFR[0] |= 1 << 4;
// very high speed
GPIOA->OSPEEDR |= 3 << (1 * 2);
// 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;
// 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;
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;
// 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;
// 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->PSC = 16 - 1;
TIM2->ARR = 20000 - 1;
TIM2->CCR1 = 900;
TIM2->CCR2 = 900;
// TIM2_ARR is buffered
TIM2->CR1 |= TIM_CR1_ARPE;
// TIM2_ARR is buffered
TIM2->CR1 |= TIM_CR1_ARPE;
// set main output enable
TIM2->BDTR |= TIM_BDTR_MOE;
// set main output enable
TIM2->BDTR |= TIM_BDTR_MOE;
// set update generation
TIM2->EGR |= TIM_EGR_UG;
// set update generation
TIM2->EGR |= TIM_EGR_UG;
// set counter enable
TIM2->CR1 |= TIM_CR1_CEN;
// set counter enable
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;
P = (int16_t) chs->pitch;
R = (int16_t) chs->roll;
Y = (int16_t) chs->yaw;
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;
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);
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;
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;
}
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);
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;
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;
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;
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;
if (throttle < 1000) throttle = 1000;
if (throttle > 2000) throttle = 2000;
TIM2->CCR2 = throttle;
}

221
Source/BSP/Src/radio_receiver.c
View File

@@ -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 |= 2 << (3 * 2);
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);
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;
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();
receiver_lpuart_clock_init();
LPUART1->CR1 = 0;
LPUART1->CR2 = 0;
LPUART1->CR3 = 0;
LPUART1->CR1 = 0;
LPUART1->CR2 = 0;
LPUART1->CR3 = 0;
LPUART1->BRR = (256 * 16000000UL) / 100000UL;
LPUART1->BRR = (256 * 16000000UL) / 100000UL;
// parity control enable
LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
// 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;
// word length M = 01 - 9 bit
LPUART1->CR1 &= ~USART_CR1_M1;
LPUART1->CR1 |= USART_CR1_M0;
// even parity
LPUART1->CR1 &= ~USART_CR1_PS;
// even parity
LPUART1->CR1 &= ~USART_CR1_PS;
// 2 stop bits
LPUART1->CR2 &= ~USART_CR2_STOP;
LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
// 2 stop bits
LPUART1->CR2 &= ~USART_CR2_STOP;
LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
// invertion enabled
LPUART1->CR2 |= USART_CR2_RXINV;
// 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;
// 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;
// uart enable
LPUART1->CR1 |= USART_CR1_UE;
NVIC_EnableIRQ(LPUART1_IRQn);
NVIC_EnableIRQ(LPUART1_IRQn);
}
void receiver_init()
{
receiver_gpio_init();
receiver_uart_init();
receiver_gpio_init();
receiver_uart_init();
}
void LPUART1_IRQHandler()
{
if (LPUART1->ISR & USART_ISR_RXNE)
{
uint8_t b = LPUART1->RDR;
if (LPUART1->ISR & USART_ISR_RXNE)
{
uint8_t b = LPUART1->RDR;
if (b == SBUS_START_BYTE)
sbus_index = 0;
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_buffer[sbus_index++] = b;
if (sbus_index == SBUS_FRAME_SIZE)
{
sbus_index = 0;
sbus_frame_ready = 1;
}
}
if (sbus_index == SBUS_FRAME_SIZE)
{
sbus_index = 0;
sbus_frame_ready = 1;
}
}
}
void receiver_update(rc_channels* chs)
{
if (!sbus_frame_ready)
return;
if (!sbus_frame_ready)
return;
sbus_frame_ready = 0;
sbus_frame_ready = 0;
if (sbus_buffer[0] != SBUS_START_BYTE)
return;
if (sbus_buffer[0] != SBUS_START_BYTE)
return;
sbus_failsafe = sbus_buffer[23] & (1 << 3);
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
sbus_failsafe = sbus_buffer[23] & (1 << 3);
sbus_frame_lost = sbus_buffer[23] & (1 << 2);
if (sbus_failsafe || sbus_frame_lost)
return;
if (sbus_failsafe || sbus_frame_lost)
return;
receiver_parse_frame();
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];
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];
uint16_t b[22];
for (uint8_t i = 0; i < 22; ++i)
b[i] = sbus_buffer[i + 1];
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[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_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);
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);
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;
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;
}
//------------------------------------------------------------------------------
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));
return x >= boundary;
}

8
Source/Control/Inc/attitude.h
View File

@@ -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 pitch; // deg
float yaw_rate; // deg/s
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);

10
Source/Control/Inc/biquad_filter.h
View File

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

16
Source/Control/Inc/pid.h
View File

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

100
Source/Control/Src/attitude.c
View File

@@ -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 pitch_acc;
static float roll_acc;
static float pitch_acc;
roll_acc = accel_roll_deg(imu);
pitch_acc = accel_pitch_deg(imu);
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);
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;
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;
if (imu_update_flag)
{
imu_update_flag = 0;
imu_read_scaled(imu);
complementary_filter_update(attitude, imu);
yaw_rate_update(attitude, imu);
}
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;
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)
{
pid_update_flag = 0;
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 = 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);
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_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;
pitch_rate_error = desired_pitch_rate - imu->gx;
yaw_rate_error = - imu->gz;
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);
}
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;
}
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;
// 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;
// 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;
// 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;
// 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;
// forward-backward
*yaw += imu->gz * IMU_DT;
}

54
Source/Control/Src/biquad_filter.c
View File

@@ -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 sin_omega = sinf(omega);
float cos_omega = cosf(omega);
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 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;
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->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;
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;
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->x2 = f->x1;
f->x1 = input;
f->y2 = f->y1;
f->y1 = output;
f->y2 = f->y1;
f->y1 = output;
return output;
return output;
}

22
Source/Control/Src/pid.c
View File

@@ -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;
if (pid->integral > 100) pid->integral = 100;
if (pid->integral < -100) pid->integral = -100;
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 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;
}

8
Source/Core/Inc/timer.h
View File

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

166
Source/Core/Src/stm32g4xx_it.c
View File

@@ -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)
{
}*/
/**
* @}
*/
/**
* @}
*/

49
Source/Core/Src/stm32g4xx_it.h
View File

@@ -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 */

14
Source/Core/Src/timer.c
View File

@@ -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);
}

64
Source/INS/geometry/quaternion.c Normal file
View File

@@ -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;
}

30
Source/INS/geometry/quaternion.h Normal file
View File

@@ -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

138
Source/INS/geometry/vector.c Normal file
View File

@@ -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;
}

50
Source/INS/geometry/vector.h Normal file
View File

@@ -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

70
Source/main.c
View File

@@ -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;
GPIOC->MODER &= ~(3 << (13 * 2));
GPIOC->MODER |= 1 << (13 * 2);
GPIOC->OTYPER &= ~(1 << 13);
GPIOC->PUPDR &= ~(3 << (13 * 2));
GPIOC->BSRR = 1 << (13 + 16);
NVIC_SetPriority(TIM6_DAC_IRQn, 2);
NVIC_SetPriority(USART3_IRQn, 1);
NVIC_SetPriority(LPUART1_IRQn, 0);
delay_ms(200);
imu_pow_init();
i2c_gpio_init();
i2c1_init();
imu_init();
imu_tim6_init();
imu_processing_init();
NVIC_SetPriority(TIM6_DAC_IRQn, 1);
NVIC_SetPriority(LPUART1_IRQn, 0);
imu_calibrate();
i2c_gpio_init();
i2c1_init();
icm_init();
imu_processing_init();
tim6_init();
receiver_init();
imu_calibrate();
motors_init();
receiver_init();
while (1)
{
attitude_update(&attitude, &imu);
motors_init();
receiver_update(&rx_chs_raw);
rx_chs_normalized = normalize_channels(rx_chs_raw);
while (1)
{
attitude_update(&attitude, &imu);
attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
receiver_update(&rx_chs_raw);
rx_chs_normalized = normalize_channels(rx_chs_raw);
lidar_update(&lidar);
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();
}
}
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++);
for (uint32_t i = 0; i < ms * 4000; i++);
}

31
drone.ewp
View File

@@ -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>

30
drone.ewt
View File

@@ -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>