From 0faafbf089870fa17ab9d43db4b937474429ec50 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Fri, 17 Apr 2026 13:40:27 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=BE=D0=BB=D0=BD=D1=8B=D0=B9=20=D0=BF?= =?UTF-8?q?=D0=B5=D1=80=D0=B5=D1=85=D0=BE=D0=B4=20=D0=BD=D0=B0=20C++?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit *Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке. *Определение вращения работает корректно. --- Source/BSP/Inc/imu.h | 93 ------ Source/BSP/Inc/imu_processing.h | 30 -- Source/BSP/Inc/lidar.h | 50 ---- Source/BSP/Inc/motors.h | 24 -- Source/BSP/Src/imu.c | 281 ------------------ Source/BSP/Src/imu_processing.c | 159 ---------- Source/BSP/Src/lidar.c | 214 ------------- Source/Control/Attitude.cpp | 73 +++++ Source/Control/{Inc/attitude.h => Attitude.h} | 17 +- Source/Control/Inc/biquad_filter.h | 20 -- Source/Control/{Src/pid.c => PID.cpp} | 2 +- Source/Control/{Inc/pid.h => PID.h} | 12 +- Source/Control/Src/attitude.c | 69 ----- Source/Control/Src/biquad_filter.c | 43 --- .../{BSP/Src/motors.c => Devices/Motors.cpp} | 61 ++-- Source/Devices/Motors.h | 21 ++ .../RadioReceiver.cpp} | 63 ++-- .../RadioReceiver.h} | 20 +- Source/INS/{IRS.c => IRS.cpp} | 0 Source/main.c | 136 --------- Source/main.cpp | 69 +++++ 21 files changed, 247 insertions(+), 1210 deletions(-) delete mode 100644 Source/BSP/Inc/imu.h delete mode 100644 Source/BSP/Inc/imu_processing.h delete mode 100644 Source/BSP/Inc/lidar.h delete mode 100644 Source/BSP/Inc/motors.h delete mode 100644 Source/BSP/Src/imu.c delete mode 100644 Source/BSP/Src/imu_processing.c delete mode 100644 Source/BSP/Src/lidar.c create mode 100644 Source/Control/Attitude.cpp rename Source/Control/{Inc/attitude.h => Attitude.h} (72%) delete mode 100644 Source/Control/Inc/biquad_filter.h rename Source/Control/{Src/pid.c => PID.cpp} (96%) rename Source/Control/{Inc/pid.h => PID.h} (72%) delete mode 100644 Source/Control/Src/attitude.c delete mode 100644 Source/Control/Src/biquad_filter.c rename Source/{BSP/Src/motors.c => Devices/Motors.cpp} (79%) create mode 100644 Source/Devices/Motors.h rename Source/{BSP/Src/radio_receiver.c => Devices/RadioReceiver.cpp} (57%) rename Source/{BSP/Inc/radio_receiver.h => Devices/RadioReceiver.h} (56%) rename Source/INS/{IRS.c => IRS.cpp} (100%) delete mode 100644 Source/main.c diff --git a/Source/BSP/Inc/imu.h b/Source/BSP/Inc/imu.h deleted file mode 100644 index bfdd46a..0000000 --- a/Source/BSP/Inc/imu.h +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#ifndef IMU_H -#define IMU_H - -#include "stm32g431xx.h" - -#define ICM_ADDR 0x68 -#define REG_PWR_MGMT_1 0x06 -#define REG_BANK_SEL 0x7F -#define REG_GYRO_CONFIG_1 0x01 -#define REG_ACCEL_CONFIG 0x14 -#define FREQUENCY 100.0f -#define IMU_DT 1.0f / FREQUENCY - -#define GYRO_FS_SEL_2000 3 << 1 -#define GYRO_DLPFCFG_73 3 << 3 -#define GYRO_FCHOICE_ON 1 -#define GYRO_FCHOICE_OFF 0 - -#define ACCEL_FS_SEL_4 1 << 1 -#define ACCEL_FS_SEL_8 2 << 1 -#define ACCEL_DLPFCFG_69 3 << 3 -#define ACCEL_FCHOICE_ON 1 -#define ACCEL_FCHOICE_OFF 0 - -static volatile uint8_t i2c_busy = 0; -static uint8_t i2c_buf[16]; -static uint8_t i2c_len = 0; -static uint8_t i2c_reg = 0; -static uint8_t i2c_addr = 0; - -typedef struct -{ - int16_t ax, ay, az; // lsb - int16_t gx, gy, gz; // lsb -} imu_raw_t; - -/*typedef struct I2C_Request -{ - uint8_t addr; - uint8_t reg; - uint8_t *buf; - uint8_t len; - - void (*callback)(uint8_t*); - - struct I2C_Request* next; -} I2C_Request; - -static I2C_Request* i2c_head = 0; -static uint8_t i2c_busy = 0;*/ - -/*typedef struct I2C_Request -{ - void (*Callback)(uint8_t* data, uint8_t size); - - uint8_t* Buffer; - uint8_t Size; - - uint8_t Address; - uint8_t Write; - uint8_t Read; - - struct I2C_Request* Next; -} I2C_Request;*/ - -static void (*i2c_callback)(uint8_t* buf) = 0; - -void imu_pow_init(); - -void i2c_gpio_init(); - -void i2c1_init(); - -void imu_init(); - -void imu_tim6_init(const uint16_t freq); - -void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len); - -//void i2c_enqueue(I2C_Request* req); -void i2c_start_next(); - -void imu_get_async(void (*cb)(uint8_t* data, uint8_t size)); - -void i2c_write(uint8_t addr, uint8_t reg, uint8_t data); - -void imu_read_raw(imu_raw_t* data); - -static void i2c_wait_idle(I2C_TypeDef* i2c); - -#endif \ No newline at end of file diff --git a/Source/BSP/Inc/imu_processing.h b/Source/BSP/Inc/imu_processing.h deleted file mode 100644 index c4129b8..0000000 --- a/Source/BSP/Inc/imu_processing.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef IMU_PROCESSING_H -#define IMU_PROCESSING_H - -#include "imu.h" - - -#define ACCEL_SENS_SCALE_FACTOR 4096.0f -#define GYRO_SENS_SCALE_FACTOR 16.384f -#define PI 3.14159265359f -#define DEG2RAD PI / 180.0f - -typedef struct -{ - float ax, ay, az; // g - float gx, gy, gz; // dps -} imu_scaled_t; - -void imu_processing_init(); - -void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib); -void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib); -void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax); -uint16_t Rev16(uint16_t v); - -void imu_accel_calibrate(); - -float normalize(float value, float scale, float min, float max); -long absl(long value); - -#endif \ No newline at end of file diff --git a/Source/BSP/Inc/lidar.h b/Source/BSP/Inc/lidar.h deleted file mode 100644 index 07c0fc0..0000000 --- a/Source/BSP/Inc/lidar.h +++ /dev/null @@ -1,50 +0,0 @@ -#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 \ No newline at end of file diff --git a/Source/BSP/Inc/motors.h b/Source/BSP/Inc/motors.h deleted file mode 100644 index 9514075..0000000 --- a/Source/BSP/Inc/motors.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef MOTORS_H -#define MOTORS_H - -#include -#include "pid.h" - -void motors_init(); -void motor_gpio_tim1_ch3_init(); -void motor_gpio_tim1_ch4_init(); -void motors_tim1_ch3_4_init(); - -void motor_gpio_tim2_ch1_init(); -void motor_gpio_tim2_ch2_init(); -void motors_tim2_ch1_2_init(); - -void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed); -void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed); -void motors_turn_off(); -void motor1_set_throttle(int16_t us); -void motor2_set_throttle(int16_t us); -void motor3_set_throttle(int16_t us); -void motor4_set_throttle(int16_t us); - -#endif \ No newline at end of file diff --git a/Source/BSP/Src/imu.c b/Source/BSP/Src/imu.c deleted file mode 100644 index 0adcc44..0000000 --- a/Source/BSP/Src/imu.c +++ /dev/null @@ -1,281 +0,0 @@ -#include "imu.h" - -/*static I2C_Request* current_req = 0; -static uint8_t i2c_buf[16]; -static uint8_t i2c_index = 0;*/ - -static I2C_Request* i2c_head = 0; -static I2C_Request* i2c_current = 0; - -static uint8_t imu_buffer[16]; - -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))); - - // high speed - GPIOB->OSPEEDR |= (2 << (8 * 2)) | (2 << (9 * 2)); - - // 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() -{ - - RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN; // enable I2C1 - - I2C1->TIMINGR = 0x10802D9BUL; // 400 kHz @ 16 MHz - - I2C1->CR1 |= I2C_CR1_PE; -} - -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, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON); - - // accel 8g, FS_SEL = 2 - i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON); - - // back to bank 0 - i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4)); -} - -void imu_tim6_init(const uint16_t freq) -{ - RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN; - - TIM6->CR1 = 0; - TIM6->ARR = 1000 - 1; - TIM6->PSC = (SystemCoreClock / 1000 / freq) - 1; - - 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; -} - -static void i2c_enqueue(I2C_Request* req) -{ - __disable_irq(); - - req->Next = i2c_head; - i2c_head = req; - - __enable_irq(); - - // если I2C свободен — запускаем - if (!i2c_busy) - { - NVIC_SetPendingIRQ(I2C1_EV_IRQn); - } -} - -static void i2c_start_next() -{ - if (!i2c_head) - { - i2c_busy = 0; - return; - } - - i2c_busy = 1; - - i2c_current = i2c_head; - i2c_head = i2c_head->Next; - - I2C1->CR1 |= I2C_CR1_TXIE | - I2C_CR1_RXIE | - I2C_CR1_TCIE | - I2C_CR1_STOPIE; - - NVIC_EnableIRQ(I2C1_EV_IRQn); - - // старт записи регистра - I2C1->CR2 = (i2c_current->Address << 1) | - (1 << I2C_CR2_NBYTES_Pos) | - I2C_CR2_START; -} - -void imu_get_async(void (*cb)(uint8_t* data, uint8_t size)) -{ - static I2C_Request req; - - req.Callback = cb; - req.Buffer = imu_buffer; - req.Size = sizeof(imu_buffer); - - req.Address = ICM_ADDR; - req.Write = 1; - req.Read = 12; - - imu_buffer[0] = 0x2D; // регистр - - i2c_enqueue(&req); -} - -void i2c_write(uint8_t addr, uint8_t reg, uint8_t data) -{ - i2c_wait_idle(I2C1); - - // write register address - I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START; - - while (!(I2C1->ISR & I2C_ISR_TXIS)); - I2C1->TXDR = reg; - - while (!(I2C1->ISR & I2C_ISR_TXIS)); - I2C1->TXDR = data; - - while (!(I2C1->ISR & I2C_ISR_TC)); - I2C1->CR2 |= I2C_CR2_STOP; -} - -void I2C1_EV_IRQHandler() -{ - static int test_irq = 0; - test_irq++; - - - uint32_t isr = I2C1->ISR; - - if (!i2c_current) - { - i2c_start_next(); - return; - } - - static uint8_t index = 0; - - // TXIS - if (isr & I2C_ISR_TXIS) - { - I2C1->TXDR = i2c_current->Buffer[0]; - } - - // TC → старт чтения - else if (isr & I2C_ISR_TC) - { - I2C1->CR2 = (i2c_current->Address << 1) | - I2C_CR2_RD_WRN | - (i2c_current->Read << I2C_CR2_NBYTES_Pos) | - I2C_CR2_AUTOEND | - I2C_CR2_START; - } - - // RXNE - else if (isr & I2C_ISR_RXNE) - { - - - i2c_current->Buffer[index++] = I2C1->RXDR; - - if (index >= i2c_current->Read) - index = 0; - } - - // STOP - else if (isr & I2C_ISR_STOPF) - { - I2C1->ICR |= I2C_ICR_STOPCF; - - if (i2c_current->Callback) - i2c_current->Callback(i2c_current->Buffer, i2c_current->Read); - - i2c_current = 0; - - i2c_start_next(); - } -} - -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]; -} - -static void i2c_wait_idle(I2C_TypeDef* i2c) -{ - int timeout = 100000; - while ((i2c->ISR & I2C_ISR_BUSY) && --timeout); - - if (timeout == 0) - { - // сброс I2C - i2c->CR1 &= ~I2C_CR1_PE; - i2c->CR1 |= I2C_CR1_PE; - } - - // 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 deleted file mode 100644 index aab9e10..0000000 --- a/Source/BSP/Src/imu_processing.c +++ /dev/null @@ -1,159 +0,0 @@ -#include "imu_processing.h" -#include "biquad_filter.h" -#include "math.h" - -static float accel_bias_x = 0.0f; -static float accel_bias_y = 0.0f; - -static biquad_t accel_x_lpf; -static biquad_t accel_y_lpf; -static biquad_t accel_z_lpf; - -static biquad_t gyro_x_lpf; -static biquad_t gyro_y_lpf; -static biquad_t gyro_z_lpf; - -const float accel_min = -4096.0f; -const float accel_max = 4096.0f; - -static int16_t x_calib; -static int16_t y_calib; -static int16_t z_calib; -static long gyro_shift[3] = {0, 0, 0}; -static float acc; - -void imu_processing_init() -{ - biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f); - biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f); - biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f); - - biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f); - biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f); - biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f); -} - -void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib) -{ - static imu_raw_t raw; - - imu_read_raw(&raw); - - raw.ax -= accel_bias_x; - raw.ay -= accel_bias_y; - - raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax); - raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay); - raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az); - - raw.gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw.gx); - raw.gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw.gy); - raw.gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw.gz); - - if (allowed_calib) imu_calib(&raw, 50, 4096, 500); - - out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max); - out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max); - out->az = normalize(raw.az, 1.0f, accel_min, accel_max); - - out->gx = (raw.gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR; - out->gy = (raw.gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR; - out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR; -} - -void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib) -{ - // bias - raw->ax -= accel_bias_x; - raw->ay -= accel_bias_y; - - // фильтры - raw->ax = (uint16_t) biquad_apply(&accel_x_lpf, raw->ax); - raw->ay = (uint16_t) biquad_apply(&accel_y_lpf, raw->ay); - raw->az = (uint16_t) biquad_apply(&accel_z_lpf, raw->az); - - raw->gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw->gx); - raw->gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw->gy); - raw->gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw->gz); - - // калибровка - if (allowed_calib) - imu_calib(raw, 50, 4096, 500); - - // масштабирование - out->ax = normalize(raw->ax, 1.0f, accel_min, accel_max); - out->ay = normalize(raw->ay, 1.0f, accel_min, accel_max); - out->az = normalize(raw->az, 1.0f, accel_min, accel_max); - - out->gx = (raw->gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR; - out->gy = (raw->gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR; - out->gz = (raw->gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR; -} - -void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax) -{ - long len = imu->gx*imu->gx + imu->gy*imu->gy + imu->gz*imu->gz; - if (len > gyrLim*gyrLim) return; - - const float alpha = 0.001f; - x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz; - - x_calib = x_calib * (1.0f - alpha) + imu->gx * alpha; - y_calib = y_calib * (1.0f - alpha) + imu->gy * alpha; - z_calib = z_calib * (1.0f - alpha) + imu->gz * alpha; - - gyro_shift[0] = x_calib; - gyro_shift[1] = y_calib; - gyro_shift[2] = z_calib; - - len = imu->ax*imu->ax + imu->ay*imu->ay + imu->az*imu->az; - if (absl(len - accZero*accZero) > accMax*accMax) return; - - len = sqrtf(len); - - acc = len; - - acc = acc * (1.0f - alpha) + len * alpha; -} - -uint16_t Rev16(uint16_t v) -{ - asm("REV16 %1, %0" : "=r" (v) : "r" (v)); - return v; -} - -void imu_accel_calibrate() -{ - const int samples = 1000; - - float sum_ax = 0; - float sum_ay = 0; - - imu_raw_t imu; - - for (uint16_t i = 0; i < samples; ++i) - { - imu_read_raw(&imu); - - sum_ax += imu.ax; - sum_ay += imu.ay; - - for (volatile uint16_t i = 0; i < 5000; ++i) { asm volatile("NOP"); }; - } - - accel_bias_x = sum_ax / samples; - accel_bias_y = sum_ay / samples; -} - -float normalize(float value, float scale, float min, float max) -{ - const float len = (max - min) / 2.0f; - const float shift = (max + min) / 2.0f; - return (value - shift) * scale / len; -} - -long absl(long value) -{ - if (value < 0) return -value; - return value; -} diff --git a/Source/BSP/Src/lidar.c b/Source/BSP/Src/lidar.c deleted file mode 100644 index 4ff2ea1..0000000 --- a/Source/BSP/Src/lidar.c +++ /dev/null @@ -1,214 +0,0 @@ -#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++); -} - - - - - - diff --git a/Source/Control/Attitude.cpp b/Source/Control/Attitude.cpp new file mode 100644 index 0000000..fd52043 --- /dev/null +++ b/Source/Control/Attitude.cpp @@ -0,0 +1,73 @@ +#include "Attitude.h" +#include "PID.h" +#include + +#define FREQUENCY 100.0f +#define PERIOD 1.0f / FREQUENCY + +#define PI 3.14159265359f +#define DEG2RAD PI / 180.0f +#define RATE_LIM 300.0f + +constexpr float angle_kp_pitch = 2.5f; +constexpr float angle_kp_roll = 2.5f; +constexpr float angle_kp_yaw = 2.0f; + +pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f}; +pid_t pid_roll = {.kp = 0.6f, .kd = 0.025f}; +pid_t pid_yaw = {.kp = 0.6f}; + +void attitude_init(attitude_t* att) +{ + att->gyro = (Vector3){}; +} + +void attitude_controller_update(control_channels_t* control, + const rc_channels* rx, + const Quaternion* current_q, + const Vector3* gyro) +{ + Quaternion q_target = rx_to_quaternion(rx); + + Quaternion q_error = current_q->GetError(q_target, true); + + Vector3 angle_error = + { + 2.0f * q_error.X, + 2.0f * q_error.Y, + 2.0f * q_error.Z + }; + + Vector3 desired_rate = + { + angle_error.X * angle_kp_pitch, + angle_error.Y * angle_kp_roll, + angle_error.Z * angle_kp_yaw + }; + + desired_rate.X = constrain(desired_rate.X, -RATE_LIM, RATE_LIM); + desired_rate.Y = constrain(desired_rate.Y, -RATE_LIM, RATE_LIM); + desired_rate.Z = constrain(desired_rate.Z, -RATE_LIM, RATE_LIM); + + control->pitch = pid_update(&pid_pitch, desired_rate.X - gyro->X, gyro->X, PERIOD); + control->roll = pid_update(&pid_roll, desired_rate.Y - gyro->Y, gyro->Y, PERIOD); + control->yaw = pid_update(&pid_yaw, desired_rate.Z - gyro->Z, gyro->Z, PERIOD); +} + +Quaternion rx_to_quaternion(const rc_channels* rx) +{ + Quaternion q; + float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD; + float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD; + float yaw = 0; + + Vector3 pry = {pitch, roll, yaw}; + q.CreatePitchRollYaw(pry); + return q; +} + +float constrain(float x, float min, float max) +{ + if (x < min) x = min; else if (x > max) x = max; + return x; +} \ No newline at end of file diff --git a/Source/Control/Inc/attitude.h b/Source/Control/Attitude.h similarity index 72% rename from Source/Control/Inc/attitude.h rename to Source/Control/Attitude.h index b6128cf..73b60b0 100644 --- a/Source/Control/Inc/attitude.h +++ b/Source/Control/Attitude.h @@ -1,17 +1,18 @@ +#pragma once + #ifndef ATTITUDE_H #define ATTITUDE_H -#include "quaternion.h" -#include "vector.h" -#include "pid.h" -#include "radio_receiver.h" -#include "imu_processing.h" +#include "Quaternion.h" +#include "Vector.h" +#include "PID.h" +#include "RadioReceiver.h" #include "IRS.h" -typedef struct +struct attitude_t { - Vector3 gyro; -} attitude_t; + Vector3 gyro; +}; void attitude_init(attitude_t* att); diff --git a/Source/Control/Inc/biquad_filter.h b/Source/Control/Inc/biquad_filter.h deleted file mode 100644 index e8b9200..0000000 --- a/Source/Control/Inc/biquad_filter.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef BIQUAD_FILTER_H -#define BIQUAD_FILTER_H - -#include - -#define PI 3.14159265359f - -typedef struct -{ - float b0, b1, b2; - float a1, a2; - - float x1, x2; - float y1, y2; -} biquad_t; - -void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate); -float biquad_apply(biquad_t* f, float input); - -#endif \ No newline at end of file diff --git a/Source/Control/Src/pid.c b/Source/Control/PID.cpp similarity index 96% rename from Source/Control/Src/pid.c rename to Source/Control/PID.cpp index 24cb2e0..87fbe0d 100644 --- a/Source/Control/Src/pid.c +++ b/Source/Control/PID.cpp @@ -1,4 +1,4 @@ -#include "pid.h" +#include "PID.h" float pid_update(pid_t* pid, float error, float gyro_rate, float dt) { diff --git a/Source/Control/Inc/pid.h b/Source/Control/PID.h similarity index 72% rename from Source/Control/Inc/pid.h rename to Source/Control/PID.h index 847de22..9b90f54 100644 --- a/Source/Control/Inc/pid.h +++ b/Source/Control/PID.h @@ -1,9 +1,11 @@ +#pragma once + #ifndef PID_H #define PID_H -#include "stm32g431xx.h" +#include "stm32g4xx.h" -typedef struct +struct pid_t { float kp; float ki; @@ -11,14 +13,14 @@ typedef struct float integral; float prev_error; -} pid_t; +}; -typedef struct +struct control_channels_t { 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 deleted file mode 100644 index 9501cb5..0000000 --- a/Source/Control/Src/attitude.c +++ /dev/null @@ -1,69 +0,0 @@ -#include "attitude.h" -#include "pid.h" -#include "imu.h" -#include "imu_processing.h" -#include - -#define RATE_LIM 300.0f - -float angle_kp_pitch = 2.5f; -float angle_kp_roll = 2.5f; -float angle_kp_yaw = 2.0f; - -pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f}; -pid_t pid_roll = {.kp = 0.6f, .kd = 0.025f}; -pid_t pid_yaw = {.kp = 0.6f}; - -void attitude_init(attitude_t* att) -{ - att->gyro = (Vector3){0}; -} - -void attitude_controller_update(control_channels_t* control, - const rc_channels* rx, - const Quaternion* current_q, - const Vector3* gyro) -{ - Quaternion q_target = rx_to_quaternion(rx); - - Quaternion q_error = QuatGetError(current_q, &q_target, true); - - Vector3 angle_error = - { - 2.0f * q_error.x, - 2.0f * q_error.y, - 2.0f * q_error.z - }; - - Vector3 desired_rate = - { - angle_error.x * angle_kp_pitch, - angle_error.y * angle_kp_roll, - angle_error.z * angle_kp_yaw - }; - - desired_rate.x = constrain(desired_rate.x, -RATE_LIM, RATE_LIM); - desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM); - desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM); - - control->pitch = pid_update(&pid_pitch, desired_rate.x - gyro->x, gyro->x, IMU_DT); - control->roll = pid_update(&pid_roll, desired_rate.y - gyro->y, gyro->y, IMU_DT); - control->yaw = pid_update(&pid_yaw, desired_rate.z - gyro->z, gyro->z, IMU_DT); -} - -Quaternion rx_to_quaternion(const rc_channels* rx) -{ - float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD; - float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD; - float yaw = 0; - - Vector3 pry = {pitch, roll, yaw}; - - return QuatCreateFromEuler(&pry); -} - -float constrain(float x, float min, float max) -{ - if (x < min) x = min; else if (x > max) x = max; - return x; -} \ No newline at end of file diff --git a/Source/Control/Src/biquad_filter.c b/Source/Control/Src/biquad_filter.c deleted file mode 100644 index 8cf1b63..0000000 --- a/Source/Control/Src/biquad_filter.c +++ /dev/null @@ -1,43 +0,0 @@ -#include "biquad_filter.h" - -void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate) -{ - float omega = 2.0f * PI * cutoff / sample_rate; - float sin_omega = sinf(omega); - float cos_omega = cosf(omega); - - float alpha = sin_omega / sqrtf(2.0f); - - float b0 = (1 - cos_omega) / 2; - float b1 = 1 - cos_omega; - float b2 = (1 - cos_omega) / 2; - float a0 = 1 + alpha; - float a1 = -2 * cos_omega; - float a2 = 1 - alpha; - - f->b0 = b0 / a0; - f->b1 = b1 / a0; - f->b2 = b2 / a0; - f->a1 = a1 / a0; - f->a2 = a2 / a0; - - f->x1 = f->x2 = 0; - f->y1 = f->y2 = 0; -} - -float biquad_apply(biquad_t* f, float input) -{ - float output = f->b0 * input - + f->b1 * f->x1 - + f->b2 * f->x2 - - f->a1 * f->y1 - - f->a2 * f->y2; - - f->x2 = f->x1; - f->x1 = input; - - f->y2 = f->y1; - f->y1 = output; - - return output; -} \ No newline at end of file diff --git a/Source/BSP/Src/motors.c b/Source/Devices/Motors.cpp similarity index 79% rename from Source/BSP/Src/motors.c rename to Source/Devices/Motors.cpp index 83bcd8a..da66064 100644 --- a/Source/BSP/Src/motors.c +++ b/Source/Devices/Motors.cpp @@ -1,6 +1,7 @@ -#include "motors.h" -#include "stm32g431xx.h" +#include "Motors.h" + +//======================Init====================== void motors_init() { RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN; @@ -141,18 +142,20 @@ void motors_tim2_ch1_2_init() // set counter enable TIM2->CR1 |= TIM_CR1_CEN; } +//======================/Init====================== -int16_t T; -int16_t P; -int16_t R; -int16_t Y; -int16_t m1; -int16_t m2; -int16_t m3; -int16_t m4; +short T; +short P; +short R; +short Y; -void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed) +short m1; +short m2; +short m3; +short m4; + +void motors_set_throttle_mix(const short throttle, const control_channels_t* chs, const bool armed) { T = throttle; P = (int16_t) chs->pitch; @@ -170,7 +173,7 @@ void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* c motor_set_throttle(4, m4, armed); } -void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed) +void motor_set_throttle(unsigned char motor_number, unsigned short us, bool armed) { if (armed && us < 1050) us = 1050; if (us > 2000) us = 2000; @@ -200,31 +203,17 @@ void motors_turn_off() motor_set_throttle(4, 900, 0); } -void motor1_set_throttle(int16_t throttle) -{ - if (throttle < 1000) throttle = 1000; - if (throttle > 2000) throttle = 2000; - TIM1->CCR3 = throttle; -} -void motor2_set_throttle(int16_t throttle) -{ - if (throttle < 1000) throttle = 1000; - if (throttle > 2000) throttle = 2000; - TIM1->CCR4 = throttle; -} -void motor3_set_throttle(int16_t throttle) -{ - if (throttle < 1000) throttle = 1000; - if (throttle > 2000) throttle = 2000; - TIM2->CCR1 = throttle; -} -void motor4_set_throttle(int16_t throttle) -{ - if (throttle < 1000) throttle = 1000; - if (throttle > 2000) throttle = 2000; - TIM2->CCR2 = throttle; -} + + + + + + + + + + diff --git a/Source/Devices/Motors.h b/Source/Devices/Motors.h new file mode 100644 index 0000000..192d39a --- /dev/null +++ b/Source/Devices/Motors.h @@ -0,0 +1,21 @@ +#pragma once + +#ifndef MOTORS_H +#define MOTORS_H + +#include "pid.h" + +void motors_init(); +void motor_gpio_tim1_ch3_init(); +void motor_gpio_tim1_ch4_init(); +void motors_tim1_ch3_4_init(); + +void motor_gpio_tim2_ch1_init(); +void motor_gpio_tim2_ch2_init(); +void motors_tim2_ch1_2_init(); + +void motors_set_throttle_mix(const short throttle, const control_channels_t* chs, const bool armed); +void motor_set_throttle(unsigned char motor_number, unsigned short us, bool armed); +void motors_turn_off(); + +#endif \ No newline at end of file diff --git a/Source/BSP/Src/radio_receiver.c b/Source/Devices/RadioReceiver.cpp similarity index 57% rename from Source/BSP/Src/radio_receiver.c rename to Source/Devices/RadioReceiver.cpp index 1d1a98b..841c9ef 100644 --- a/Source/BSP/Src/radio_receiver.c +++ b/Source/Devices/RadioReceiver.cpp @@ -1,12 +1,11 @@ -#include "radio_receiver.h" +#include "RadioReceiver.h" - -volatile uint16_t sbus_channels[16] = {0}; -volatile uint8_t sbus_buffer[SBUS_FRAME_SIZE] = {0}; -volatile uint8_t sbus_index = 0; -volatile uint8_t sbus_failsafe = 0; -volatile uint8_t sbus_frame_lost = 0; -volatile uint8_t sbus_frame_ready = 0; +volatile unsigned short sbus_channels[16] = {0}; +volatile unsigned char sbus_buffer[SBUS_FRAME_SIZE] = {0}; +volatile unsigned char sbus_index = 0; +volatile unsigned char sbus_failsafe = 0; +volatile unsigned char sbus_frame_lost = 0; +volatile bool sbus_frame_ready = 0; void receiver_gpio_init() { @@ -119,31 +118,31 @@ void receiver_update(rc_channels* chs) void receiver_parse_frame() { - uint16_t b[22]; - - for (uint8_t i = 0; i < 22; ++i) - b[i] = sbus_buffer[i + 1]; + 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[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) @@ -157,12 +156,12 @@ rc_channels normalize_channels(rc_channels 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) +short int_mapping(short x, short in_min, short in_max, short out_min, short out_max) { return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min); } -int8_t bool_mapping_gt(int16_t x, int16_t boundary) +bool bool_mapping_gt(short x, short boundary) { return x >= boundary; } \ No newline at end of file diff --git a/Source/BSP/Inc/radio_receiver.h b/Source/Devices/RadioReceiver.h similarity index 56% rename from Source/BSP/Inc/radio_receiver.h rename to Source/Devices/RadioReceiver.h index a474332..fd0c79c 100644 --- a/Source/BSP/Inc/radio_receiver.h +++ b/Source/Devices/RadioReceiver.h @@ -1,3 +1,5 @@ +#pragma once + #ifndef RADIO_RECEIVER_H #define RADIO_RECEIVER_H @@ -7,14 +9,14 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_START_BYTE 0X0F -typedef struct +struct rc_channels { - 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; + short rc_roll; // -500 - 500 + short rc_pitch; // -500 - 500 + short rc_throttle; // 1000 - 2000 + short rc_yaw; // -500 - 500 + bool rc_armed; // 0/1 +}; void receiver_gpio_init(); void receiver_lpuart_clock_init(); @@ -24,8 +26,8 @@ void LPUART1_IRQHandler(); void receiver_update(rc_channels* chs); void receiver_parse_frame(); rc_channels normalize_channels(rc_channels chs); -int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); -int8_t bool_mapping_gt(int16_t x, int16_t boundary); +short int_mapping(short x, short in_min, short in_max, short out_min, short out_max); +bool bool_mapping_gt(short x, short boundary); void led_init(); diff --git a/Source/INS/IRS.c b/Source/INS/IRS.cpp similarity index 100% rename from Source/INS/IRS.c rename to Source/INS/IRS.cpp diff --git a/Source/main.c b/Source/main.c deleted file mode 100644 index 5c96fc3..0000000 --- a/Source/main.c +++ /dev/null @@ -1,136 +0,0 @@ -#include "stm32g431xx.h" -#include "imu.h" -#include "imu_processing.h" -#include "IRS.h" -#include "attitude.h" -#include "radio_receiver.h" -#include "motors.h" -#include "pid.h" -#include "lidar.h" - -void TIM6_DAC_IRQHandler(); - -//static uint8_t irs_update_flag = 0; -static uint8_t control_update_flag = 0; -static uint8_t allowed_calib = 0; - -imu_raw_t imu_raw; -imu_scaled_t imu; -IRS irs; -attitude_t attitude; -rc_channels rx_chs_raw; -rc_channels rx_chs_normalized; -control_channels_t ctrl_chs; -//lidar_data lidar; - -Vector3 euler; - -void imu_callback(uint8_t* buf, uint8_t size); -void delay_ms(uint32_t ms); - -int main(void) -{ - SystemClock_Config(); // 170 MHz - - __enable_irq(); - - NVIC_SetPriority(TIM6_DAC_IRQn, 2); - NVIC_SetPriority(USART3_IRQn, 1); - NVIC_SetPriority(LPUART1_IRQn, 0); - - imu_pow_init(); - i2c_gpio_init(); - i2c1_init(); - imu_init(); - - imu_processing_init(); - //imu_accel_calibrate(); - - imu_tim6_init(500); - - IRS_init(&irs); - - attitude_init(&attitude); - - receiver_init(); - - motors_init(); - - while (1) - { - receiver_update(&rx_chs_raw); - rx_chs_normalized = normalize_channels(rx_chs_raw); - - if (control_update_flag) - { - control_update_flag = 0; - - attitude_controller_update( - &ctrl_chs, - &rx_chs_normalized, - &irs.q, - &irs.gyro - ); - } - - if (rx_chs_normalized.rc_armed) - { - allowed_calib = 0; - - motors_set_throttle_mix( - rx_chs_normalized.rc_throttle, - &ctrl_chs, - rx_chs_normalized.rc_armed - ); - } - else - { - motors_turn_off(); - allowed_calib = 1; - } - } -} - -void imu_callback(uint8_t* buf, uint8_t size) -{ - imu_raw.ax = Rev16((buf[0] << 8) | buf[1]); - imu_raw.ay = Rev16((buf[2] << 8) | buf[3]); - imu_raw.az = Rev16((buf[4] << 8) | buf[5]); - - imu_raw.gx = Rev16((buf[6] << 8) | buf[7]); - imu_raw.gy = Rev16((buf[8] << 8) | buf[9]); - imu_raw.gz = Rev16((buf[10] << 8) | buf[11]); - - imu_process_raw(&imu, &imu_raw, &allowed_calib); - - irs.gyro.x = imu.gx; - irs.gyro.y = imu.gy; - irs.gyro.z = imu.gz; - - irs.accel.x = imu.ax; - irs.accel.y = imu.ay; - irs.accel.z = imu.az; - - IRS_update(&irs, IMU_DT); - - euler = QuatToEuler(&irs.q); -} - -void TIM6_DAC_IRQHandler() -{ - if (TIM6->SR & TIM_SR_UIF) - { - TIM6->SR &= ~TIM_SR_UIF; - - imu_get_async(imu_callback); - control_update_flag = 1; - } -} - -void delay_ms(uint32_t ms) -{ - for (uint32_t i = 0; i < ms * 4000; i++); -} - - - diff --git a/Source/main.cpp b/Source/main.cpp index 75e98f1..ada59c8 100644 --- a/Source/main.cpp +++ b/Source/main.cpp @@ -6,6 +6,11 @@ #include "IRS.h" #include "Vector.h" #include "Autopilot.h" +#include "RadioReceiver.h" +#include "Motors.h" +#include "Attitude.h" + +#define PI 3.14159265359f extern "C" void SystemClock_Config(); @@ -22,6 +27,23 @@ float GetCurrentTime() return ((float)TICK_GetCount()) / 1000.0f; } +Vector3 QuatToEuler(const Quaternion& q) +{ + Vector3 e; + + e.X = atan2f(2*(q.W*q.X + q.Y*q.Z), 1 - 2*(q.X*q.X + q.Y*q.Y)); + + e.Y = asinf(2*(q.W*q.Y - q.Z*q.X)); + + e.Z = atan2f(2*(q.W*q.Z + q.X*q.Y), 1 - 2*(q.Y*q.Y + q.Z*q.Z)); + + e.X *= 180.0f / PI; + e.Y *= 180.0f / PI; + e.Z *= 180.0f / PI; + + return e; +} + void DoneProcIMU(IMU_Data& Data) { DataIMU = Data; @@ -35,6 +57,8 @@ void TimerUpdateSensor() //if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR); } +Vector3 Euler; + void TimerUpdateMain() { float time = GetCurrentTime(); @@ -48,6 +72,9 @@ void TimerUpdateMain() MainIRS.UpdateGyro(gyr); MainIRS.UpdateAccel(acc); + + Euler = QuatToEuler(MainIRS.OriQuat); + /*if(MagReady) { MagReady=false; @@ -91,6 +118,8 @@ void TimerUpdateMain() if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/ } +static bool control_update_flag = 0; + int main() { SystemClock_Config(); // 170MHz @@ -104,8 +133,48 @@ int main() TIM6_Enable(); + attitude_t attitude; + rc_channels rx_chs_raw; + rc_channels rx_chs_normalized; + control_channels_t ctrl_chs; + + attitude_init(&attitude); + receiver_init(); + motors_init(); + while (true) { + receiver_update(&rx_chs_raw); + rx_chs_normalized = normalize_channels(rx_chs_raw); + if (control_update_flag) + { + control_update_flag = 0; + + Vector3 Gyro = Vector3(DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z); + + attitude_controller_update( + &ctrl_chs, + &rx_chs_normalized, + &MainIRS.OriQuat, + &Gyro + ); + } + + if (rx_chs_normalized.rc_armed) + { + CalibDataIMU.AllowedCalib = false; + + motors_set_throttle_mix( + rx_chs_normalized.rc_throttle, + &ctrl_chs, + rx_chs_normalized.rc_armed + ); + } + else + { + motors_turn_off(); + CalibDataIMU.AllowedCalib = true; + } } } \ No newline at end of file