Полный переход на C++
*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке. *Определение вращения работает корректно.
This commit is contained in:
@@ -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
|
|
||||||
@@ -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
|
|
||||||
@@ -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
|
|
||||||
@@ -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
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
@@ -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
73
Source/Control/Attitude.cpp
Normal 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;
|
||||||
|
}
|
||||||
@@ -1,17 +1,18 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
#ifndef ATTITUDE_H
|
#ifndef ATTITUDE_H
|
||||||
#define ATTITUDE_H
|
#define ATTITUDE_H
|
||||||
|
|
||||||
#include "quaternion.h"
|
#include "Quaternion.h"
|
||||||
#include "vector.h"
|
#include "Vector.h"
|
||||||
#include "pid.h"
|
#include "PID.h"
|
||||||
#include "radio_receiver.h"
|
#include "RadioReceiver.h"
|
||||||
#include "imu_processing.h"
|
|
||||||
#include "IRS.h"
|
#include "IRS.h"
|
||||||
|
|
||||||
typedef struct
|
struct attitude_t
|
||||||
{
|
{
|
||||||
Vector3 gyro;
|
Vector3 gyro;
|
||||||
} attitude_t;
|
};
|
||||||
|
|
||||||
void attitude_init(attitude_t* att);
|
void attitude_init(attitude_t* att);
|
||||||
|
|
||||||
@@ -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
|
|
||||||
@@ -1,4 +1,4 @@
|
|||||||
#include "pid.h"
|
#include "PID.h"
|
||||||
|
|
||||||
float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
|
float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
|
||||||
{
|
{
|
||||||
@@ -1,9 +1,11 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
#ifndef PID_H
|
#ifndef PID_H
|
||||||
#define PID_H
|
#define PID_H
|
||||||
|
|
||||||
#include "stm32g431xx.h"
|
#include "stm32g4xx.h"
|
||||||
|
|
||||||
typedef struct
|
struct pid_t
|
||||||
{
|
{
|
||||||
float kp;
|
float kp;
|
||||||
float ki;
|
float ki;
|
||||||
@@ -11,14 +13,14 @@ typedef struct
|
|||||||
|
|
||||||
float integral;
|
float integral;
|
||||||
float prev_error;
|
float prev_error;
|
||||||
} pid_t;
|
};
|
||||||
|
|
||||||
typedef struct
|
struct control_channels_t
|
||||||
{
|
{
|
||||||
float roll;
|
float roll;
|
||||||
float pitch;
|
float pitch;
|
||||||
float yaw;
|
float yaw;
|
||||||
} control_channels_t;
|
};
|
||||||
|
|
||||||
float pid_update(pid_t* pid, float error, float gyro_rate, float dt);
|
float pid_update(pid_t* pid, float error, float gyro_rate, float dt);
|
||||||
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
@@ -1,6 +1,7 @@
|
|||||||
#include "motors.h"
|
#include "Motors.h"
|
||||||
#include "stm32g431xx.h"
|
|
||||||
|
|
||||||
|
|
||||||
|
//======================Init======================
|
||||||
void motors_init()
|
void motors_init()
|
||||||
{
|
{
|
||||||
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
|
||||||
@@ -141,18 +142,20 @@ void motors_tim2_ch1_2_init()
|
|||||||
// set counter enable
|
// set counter enable
|
||||||
TIM2->CR1 |= TIM_CR1_CEN;
|
TIM2->CR1 |= TIM_CR1_CEN;
|
||||||
}
|
}
|
||||||
|
//======================/Init======================
|
||||||
|
|
||||||
int16_t T;
|
|
||||||
int16_t P;
|
|
||||||
int16_t R;
|
|
||||||
int16_t Y;
|
|
||||||
|
|
||||||
int16_t m1;
|
short T;
|
||||||
int16_t m2;
|
short P;
|
||||||
int16_t m3;
|
short R;
|
||||||
int16_t m4;
|
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;
|
T = throttle;
|
||||||
P = (int16_t) chs->pitch;
|
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);
|
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 (armed && us < 1050) us = 1050;
|
||||||
if (us > 2000) us = 2000;
|
if (us > 2000) us = 2000;
|
||||||
@@ -200,31 +203,17 @@ void motors_turn_off()
|
|||||||
motor_set_throttle(4, 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
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
21
Source/Devices/Motors.h
Normal 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
|
||||||
@@ -1,12 +1,11 @@
|
|||||||
#include "radio_receiver.h"
|
#include "RadioReceiver.h"
|
||||||
|
|
||||||
|
volatile unsigned short sbus_channels[16] = {0};
|
||||||
volatile uint16_t sbus_channels[16] = {0};
|
volatile unsigned char sbus_buffer[SBUS_FRAME_SIZE] = {0};
|
||||||
volatile uint8_t sbus_buffer[SBUS_FRAME_SIZE] = {0};
|
volatile unsigned char sbus_index = 0;
|
||||||
volatile uint8_t sbus_index = 0;
|
volatile unsigned char sbus_failsafe = 0;
|
||||||
volatile uint8_t sbus_failsafe = 0;
|
volatile unsigned char sbus_frame_lost = 0;
|
||||||
volatile uint8_t sbus_frame_lost = 0;
|
volatile bool sbus_frame_ready = 0;
|
||||||
volatile uint8_t sbus_frame_ready = 0;
|
|
||||||
|
|
||||||
void receiver_gpio_init()
|
void receiver_gpio_init()
|
||||||
{
|
{
|
||||||
@@ -157,12 +156,12 @@ rc_channels normalize_channels(rc_channels chs)
|
|||||||
return chs;
|
return chs;
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
|
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);
|
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;
|
return x >= boundary;
|
||||||
}
|
}
|
||||||
@@ -1,3 +1,5 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
#ifndef RADIO_RECEIVER_H
|
#ifndef RADIO_RECEIVER_H
|
||||||
#define RADIO_RECEIVER_H
|
#define RADIO_RECEIVER_H
|
||||||
|
|
||||||
@@ -7,14 +9,14 @@
|
|||||||
#define SBUS_FRAME_SIZE 25
|
#define SBUS_FRAME_SIZE 25
|
||||||
#define SBUS_START_BYTE 0X0F
|
#define SBUS_START_BYTE 0X0F
|
||||||
|
|
||||||
typedef struct
|
struct rc_channels
|
||||||
{
|
{
|
||||||
int16_t rc_roll; // -500 - 500
|
short rc_roll; // -500 - 500
|
||||||
int16_t rc_pitch; // -500 - 500
|
short rc_pitch; // -500 - 500
|
||||||
int16_t rc_throttle; // 1000 - 2000
|
short rc_throttle; // 1000 - 2000
|
||||||
int16_t rc_yaw; // -500 - 500
|
short rc_yaw; // -500 - 500
|
||||||
int16_t rc_armed; // 0/1
|
bool rc_armed; // 0/1
|
||||||
} rc_channels;
|
};
|
||||||
|
|
||||||
void receiver_gpio_init();
|
void receiver_gpio_init();
|
||||||
void receiver_lpuart_clock_init();
|
void receiver_lpuart_clock_init();
|
||||||
@@ -24,8 +26,8 @@ void LPUART1_IRQHandler();
|
|||||||
void receiver_update(rc_channels* chs);
|
void receiver_update(rc_channels* chs);
|
||||||
void receiver_parse_frame();
|
void receiver_parse_frame();
|
||||||
rc_channels normalize_channels(rc_channels chs);
|
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);
|
short int_mapping(short x, short in_min, short in_max, short out_min, short out_max);
|
||||||
int8_t bool_mapping_gt(int16_t x, int16_t boundary);
|
bool bool_mapping_gt(short x, short boundary);
|
||||||
|
|
||||||
|
|
||||||
void led_init();
|
void led_init();
|
||||||
136
Source/main.c
136
Source/main.c
@@ -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++);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -6,6 +6,11 @@
|
|||||||
#include "IRS.h"
|
#include "IRS.h"
|
||||||
#include "Vector.h"
|
#include "Vector.h"
|
||||||
#include "Autopilot.h"
|
#include "Autopilot.h"
|
||||||
|
#include "RadioReceiver.h"
|
||||||
|
#include "Motors.h"
|
||||||
|
#include "Attitude.h"
|
||||||
|
|
||||||
|
#define PI 3.14159265359f
|
||||||
|
|
||||||
extern "C" void SystemClock_Config();
|
extern "C" void SystemClock_Config();
|
||||||
|
|
||||||
@@ -22,6 +27,23 @@ float GetCurrentTime()
|
|||||||
return ((float)TICK_GetCount()) / 1000.0f;
|
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)
|
void DoneProcIMU(IMU_Data& Data)
|
||||||
{
|
{
|
||||||
DataIMU = Data;
|
DataIMU = Data;
|
||||||
@@ -35,6 +57,8 @@ void TimerUpdateSensor()
|
|||||||
//if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
|
//if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Vector3 Euler;
|
||||||
|
|
||||||
void TimerUpdateMain()
|
void TimerUpdateMain()
|
||||||
{
|
{
|
||||||
float time = GetCurrentTime();
|
float time = GetCurrentTime();
|
||||||
@@ -48,6 +72,9 @@ void TimerUpdateMain()
|
|||||||
|
|
||||||
MainIRS.UpdateGyro(gyr);
|
MainIRS.UpdateGyro(gyr);
|
||||||
MainIRS.UpdateAccel(acc);
|
MainIRS.UpdateAccel(acc);
|
||||||
|
|
||||||
|
Euler = QuatToEuler(MainIRS.OriQuat);
|
||||||
|
|
||||||
/*if(MagReady)
|
/*if(MagReady)
|
||||||
{
|
{
|
||||||
MagReady=false;
|
MagReady=false;
|
||||||
@@ -91,6 +118,8 @@ void TimerUpdateMain()
|
|||||||
if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
|
if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool control_update_flag = 0;
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
SystemClock_Config(); // 170MHz
|
SystemClock_Config(); // 170MHz
|
||||||
@@ -104,8 +133,48 @@ int main()
|
|||||||
|
|
||||||
TIM6_Enable();
|
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)
|
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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
Reference in New Issue
Block a user