From f9b7277a33f97f2251e49381ea07b8fbd638edb6 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Wed, 18 Feb 2026 14:23:01 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9D=D0=B0=D0=B2=D0=B5=D0=B4=D1=91=D0=BD=20?= =?UTF-8?q?=D0=BF=D0=BE=D1=80=D1=8F=D0=B4=D0=BE=D0=BA=20=D0=B2=20=D0=BA?= =?UTF-8?q?=D0=BE=D0=B4=D0=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/BSP/Inc/imu.h | 25 ++- Source/BSP/Inc/imu_processing.h | 10 +- Source/BSP/Inc/radio_receiver.h | 10 +- Source/BSP/Src/imu.c | 203 ++++++++++-------- Source/BSP/Src/imu_processing.c | 120 +++++------ Source/BSP/Src/motors.c | 322 ++++++++++++++--------------- Source/BSP/Src/radio_receiver.c | 277 +++++++++++-------------- Source/Control/Inc/attitude.h | 8 +- Source/Control/Inc/biquad_filter.h | 12 +- Source/Control/Inc/pid.h | 18 +- Source/Control/Src/attitude.c | 120 +++++------ Source/Control/Src/biquad_filter.c | 68 +++--- Source/Control/Src/pid.c | 36 ++-- Source/Core/Inc/timer.h | 8 - Source/Core/Src/stm32g4xx_it.c | 166 --------------- Source/Core/Src/stm32g4xx_it.h | 49 ----- Source/Core/Src/timer.c | 14 -- Source/main.c | 85 ++++---- drone.ewp | 12 -- drone.ewt | 12 -- 20 files changed, 647 insertions(+), 928 deletions(-) delete mode 100644 Source/Core/Inc/timer.h delete mode 100644 Source/Core/Src/stm32g4xx_it.c delete mode 100644 Source/Core/Src/stm32g4xx_it.h delete mode 100644 Source/Core/Src/timer.c diff --git a/Source/BSP/Inc/imu.h b/Source/BSP/Inc/imu.h index 5e75ee7..c16dafa 100644 --- a/Source/BSP/Inc/imu.h +++ b/Source/BSP/Inc/imu.h @@ -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); diff --git a/Source/BSP/Inc/imu_processing.h b/Source/BSP/Inc/imu_processing.h index 1932c50..2bb14b7 100644 --- a/Source/BSP/Inc/imu_processing.h +++ b/Source/BSP/Inc/imu_processing.h @@ -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(); diff --git a/Source/BSP/Inc/radio_receiver.h b/Source/BSP/Inc/radio_receiver.h index f559230..a474332 100644 --- a/Source/BSP/Inc/radio_receiver.h +++ b/Source/BSP/Inc/radio_receiver.h @@ -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(); diff --git a/Source/BSP/Src/imu.c b/Source/BSP/Src/imu.c index 3495a33..7c91e88 100644 --- a/Source/BSP/Src/imu.c +++ b/Source/BSP/Src/imu.c @@ -1,119 +1,142 @@ #include "imu.h" +void imu_pow_init() +{ + RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN; + GPIOC->MODER &= ~(3 << (13 * 2)); + GPIOC->MODER |= 1 << (13 * 2); + GPIOC->OTYPER &= ~(1 << 13); + GPIOC->PUPDR &= ~(3 << (13 * 2)); + GPIOC->BSRR = 1 << (13 + 16); +} + void i2c_gpio_init() { - // enable GPIOB clock - 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; diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c index 218db67..7628ffa 100644 --- a/Source/BSP/Src/imu_processing.c +++ b/Source/BSP/Src/imu_processing.c @@ -20,72 +20,72 @@ static biquad_t gyro_z_lpf; void imu_processing_init() { - biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f); - biquad_init_lpf(&accel_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; } diff --git a/Source/BSP/Src/motors.c b/Source/BSP/Src/motors.c index 53ed5a5..83bcd8a 100644 --- a/Source/BSP/Src/motors.c +++ b/Source/BSP/Src/motors.c @@ -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; } diff --git a/Source/BSP/Src/radio_receiver.c b/Source/BSP/Src/radio_receiver.c index 9821e36..1d1a98b 100644 --- a/Source/BSP/Src/radio_receiver.c +++ b/Source/BSP/Src/radio_receiver.c @@ -10,196 +10,159 @@ volatile uint8_t sbus_frame_ready = 0; void receiver_gpio_init() { - RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN; - - 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; +} \ No newline at end of file diff --git a/Source/Control/Inc/attitude.h b/Source/Control/Inc/attitude.h index 80e688a..77680d2 100644 --- a/Source/Control/Inc/attitude.h +++ b/Source/Control/Inc/attitude.h @@ -5,13 +5,13 @@ #include "radio_receiver.h" #include "pid.h" -#define CF_ALPHA 0.99f +#define CF_ALPHA 0.99f typedef struct { - float roll; // deg - float 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); diff --git a/Source/Control/Inc/biquad_filter.h b/Source/Control/Inc/biquad_filter.h index ec04003..e8b9200 100644 --- a/Source/Control/Inc/biquad_filter.h +++ b/Source/Control/Inc/biquad_filter.h @@ -3,15 +3,15 @@ #include -#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); diff --git a/Source/Control/Inc/pid.h b/Source/Control/Inc/pid.h index 5d08470..847de22 100644 --- a/Source/Control/Inc/pid.h +++ b/Source/Control/Inc/pid.h @@ -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); diff --git a/Source/Control/Src/attitude.c b/Source/Control/Src/attitude.c index 0cfc361..aa8859e 100644 --- a/Source/Control/Src/attitude.c +++ b/Source/Control/Src/attitude.c @@ -28,34 +28,34 @@ float error_yaw = 0.0f; void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu) { - static float roll_acc; - static float 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; } \ No newline at end of file diff --git a/Source/Control/Src/biquad_filter.c b/Source/Control/Src/biquad_filter.c index eb14b35..8cf1b63 100644 --- a/Source/Control/Src/biquad_filter.c +++ b/Source/Control/Src/biquad_filter.c @@ -2,42 +2,42 @@ void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate) { - float omega = 2.0f * PI * cutoff / sample_rate; - float 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; } \ No newline at end of file diff --git a/Source/Control/Src/pid.c b/Source/Control/Src/pid.c index efe80a1..24cb2e0 100644 --- a/Source/Control/Src/pid.c +++ b/Source/Control/Src/pid.c @@ -2,22 +2,22 @@ float pid_update(pid_t* pid, float error, float gyro_rate, float dt) { - float p = pid->kp * error; - - 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; } \ No newline at end of file diff --git a/Source/Core/Inc/timer.h b/Source/Core/Inc/timer.h deleted file mode 100644 index 7eaade1..0000000 --- a/Source/Core/Inc/timer.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef TIMER_H -#define TIMER_H - -#include "stm32g431xx.h" - -void tim6_init(); - -#endif \ No newline at end of file diff --git a/Source/Core/Src/stm32g4xx_it.c b/Source/Core/Src/stm32g4xx_it.c deleted file mode 100644 index 2239d6e..0000000 --- a/Source/Core/Src/stm32g4xx_it.c +++ /dev/null @@ -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) -{ -}*/ - - -/** - * @} - */ - -/** - * @} - */ - - diff --git a/Source/Core/Src/stm32g4xx_it.h b/Source/Core/Src/stm32g4xx_it.h deleted file mode 100644 index ab13977..0000000 --- a/Source/Core/Src/stm32g4xx_it.h +++ /dev/null @@ -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 */ - - diff --git a/Source/Core/Src/timer.c b/Source/Core/Src/timer.c deleted file mode 100644 index cb87df5..0000000 --- a/Source/Core/Src/timer.c +++ /dev/null @@ -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); -} \ No newline at end of file diff --git a/Source/main.c b/Source/main.c index 87eb7c6..7e573a0 100644 --- a/Source/main.c +++ b/Source/main.c @@ -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++); } diff --git a/drone.ewp b/drone.ewp index 2b9c693..def56da 100644 --- a/drone.ewp +++ b/drone.ewp @@ -2311,24 +2311,12 @@ $PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h - - $PROJ_DIR$\Source\Core\Inc\timer.h - Src - - $PROJ_DIR$\Source\Core\Src\stm32g4xx_it.c - - - $PROJ_DIR$\Source\Core\Src\stm32g4xx_it.h - $PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c - - $PROJ_DIR$\Source\Core\Src\timer.c - diff --git a/drone.ewt b/drone.ewt index 995b1ce..d7c7a02 100644 --- a/drone.ewt +++ b/drone.ewt @@ -3496,24 +3496,12 @@ $PROJ_DIR$\Source\Core\Inc\system_stm32g4xx.h - - $PROJ_DIR$\Source\Core\Inc\timer.h - Src - - $PROJ_DIR$\Source\Core\Src\stm32g4xx_it.c - - - $PROJ_DIR$\Source\Core\Src\stm32g4xx_it.h - $PROJ_DIR$\Source\Core\Src\system_stm32g4xx.c - - $PROJ_DIR$\Source\Core\Src\timer.c -