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

Наведён порядок в коде

Browse Source
This commit is contained in:
Radzhab Bisultanov
2026-02-18 14:23:01 +03:00
parent eecf8f2cc2
commit f9b7277a33
20 changed files with 647 additions and 928 deletions
Download Patch File Download Diff File 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();

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();

203
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;
// 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);
// 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;
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
I2C1->CR1 |= I2C_CR1_PE;
}
void icm_init()
void imu_init()
{
// select bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
// wake up, auto clock
i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
// select bank 2
i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
// gyro ~2000 dps, FS_SEL = 3
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (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));
// 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 imu_tim6_init()
{
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
TIM6->PSC = 16000 - 1; // 16 MHz / 16000 = 1000 Hz (1 ms)
TIM6->ARR = 2 - 1; // 2 ms
TIM6->DIER |= TIM_DIER_UIE; // interrupt enable
TIM6->CR1 |= TIM_CR1_CEN; // counter enable
NVIC_EnableIRQ(TIM6_DAC_IRQn);
}
void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
{
i2c_wait_idle(I2C1);
// write register address
I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
while (!(I2C1->ISR & I2C_ISR_TXIS));
I2C1->TXDR = reg;
while (!(I2C1->ISR & I2C_ISR_TC));
I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
for (uint8_t i = 0; i < len; ++i)
{
while (!(I2C1->ISR & I2C_ISR_RXNE));
buf[i] = I2C1->RXDR;
}
while (!(I2C1->ISR & I2C_ISR_STOPF));
I2C1->ICR |= I2C_ICR_STOPCF;
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;
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)
void imu_read_raw(imu_raw_t* data)
{
uint8_t buf[12];
i2c_read(ICM_ADDR, 0x2D, buf, 12);
data->ax = (buf[0] << 8) | buf[1];
data->ay = (buf[2] << 8) | buf[3];
data->az = (buf[4] << 8) | buf[5];
data->gx = (buf[6] << 8) | buf[7];
data->gy = (buf[8] << 8) | buf[9];
data->gz = (buf[10] << 8) | buf[11];
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 |
while (i2c->ISR & I2C_ISR_BUSY);
i2c->ICR = I2C_ICR_STOPCF |
I2C_ICR_NACKCF |
I2C_ICR_BERRCF |
I2C_ICR_ARLOCF;

120
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(&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(&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);
static imu_raw_t raw;
imu_read_raw(&raw);
out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
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;
const int samples = 1000;
float sum_ax = 0;
float sum_ay = 0;
float sum_az = 0;
float sum_gx = 0;
float sum_gy = 0;
float sum_gz = 0;
imu_raw_t imu;
for (uint16_t i = 0; i < samples; ++i)
{
imu_read_raw(&imu);
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
for (volatile uint16_t d = 0; d < 5000; ++d);
}
accel_bias_x = sum_ax / samples;
accel_bias_y = sum_ay / samples;
accel_bias_z = (sum_az / samples) + 1.0f;
gyro_bias_x = sum_gx / samples;
gyro_bias_y = sum_gy / samples;
gyro_bias_z = sum_gz / samples;
}

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

277
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;
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);
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;
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);
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();
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;
}
}
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];
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);
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;
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);
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);

12
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 x1, x2;
float y1, y2;
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);

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

@@ -5,19 +5,19 @@
typedef struct
{
float kp;
float ki;
float kd;
float integral;
float prev_error;
float kp;
float ki;
float kd;
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);

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

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

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

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

85
Source/main.c
View File

@@ -1,7 +1,6 @@
#include "stm32g431xx.h"
#include "imu.h"
#include "imu_processing.h"
#include "timer.h"
#include "attitude.h"
#include "radio_receiver.h"
#include "motors.h"
@@ -16,57 +15,49 @@ control_channels_t ctrl_chs;
void delay_ms(uint32_t ms);
int main(void)
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();
}
}
__enable_irq();
NVIC_SetPriority(TIM6_DAC_IRQn, 1);
NVIC_SetPriority(LPUART1_IRQn, 0);
imu_pow_init();
i2c_gpio_init();
i2c1_init();
imu_init();
imu_tim6_init();
imu_processing_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++);
for (uint32_t i = 0; i < ms * 4000; i++);
}