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

Полный переход на C++

Browse Source 
*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке.
*Определение вращения работает корректно.
This commit is contained in:
Раджаб Бисултанов
2026-04-17 13:40:27 +03:00
parent a3d845df9e
commit 0faafbf089
21 changed files with 247 additions and 1210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

24
Source/BSP/Inc/motors.h
View File

@@ -1,24 +0,0 @@
#ifndef MOTORS_H
#define MOTORS_H
#include <stdint.h>
#include "pid.h"
void motors_init();
void motor_gpio_tim1_ch3_init();
void motor_gpio_tim1_ch4_init();
void motors_tim1_ch3_4_init();
void motor_gpio_tim2_ch1_init();
void motor_gpio_tim2_ch2_init();
void motors_tim2_ch1_2_init();
void motors_set_throttle_mix(const int16_t throttle, const control_channels_t* chs, const int8_t armed);
void motor_set_throttle(int8_t motor_number, int16_t us, int8_t armed);
void motors_turn_off();
void motor1_set_throttle(int16_t us);
void motor2_set_throttle(int16_t us);
void motor3_set_throttle(int16_t us);
void motor4_set_throttle(int16_t us);
#endif

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

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

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

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

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

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

73
Source/Control/Attitude.cpp Normal file
View File

@@ -0,0 +1,73 @@
#include "Attitude.h"
#include "PID.h"
#include <math.h>
#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;
}

17
Source/Control/Inc/attitude.h → Source/Control/Attitude.h
View File

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

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

@@ -1,20 +0,0 @@
#ifndef BIQUAD_FILTER_H
#define BIQUAD_FILTER_H
#include <math.h>
#define PI 3.14159265359f
typedef struct
{
float b0, b1, b2;
float a1, a2;
float x1, x2;
float y1, y2;
} biquad_t;
void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate);
float biquad_apply(biquad_t* f, float input);
#endif

2
Source/Control/Src/pid.c → Source/Control/PID.cpp
View File

@@ -1,4 +1,4 @@
#include "pid.h"
#include "PID.h"
float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
{

12
Source/Control/Inc/pid.h → Source/Control/PID.h
View File

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

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

@@ -1,69 +0,0 @@
#include "attitude.h"
#include "pid.h"
#include "imu.h"
#include "imu_processing.h"
#include <math.h>
#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;
}

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

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

61
Source/BSP/Src/motors.c → Source/Devices/Motors.cpp
View File

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

21
Source/Devices/Motors.h Normal file
View File

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

63
Source/BSP/Src/radio_receiver.c → Source/Devices/RadioReceiver.cpp
View File

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

20
Source/BSP/Inc/radio_receiver.h → Source/Devices/RadioReceiver.h
View File

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

0
Source/INS/IRS.c → Source/INS/IRS.cpp
View File

136
Source/main.c
View File

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

69
Source/main.cpp
View File

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