init:全链路闭环控制调通
This commit is contained in:
SkyLi
@@ -0,0 +1,33 @@
|
||||
#include "Beep.h"
|
||||
#include "zf_driver_gpio.h"
|
||||
|
||||
static int beep_time=0;
|
||||
static int beep_T;
|
||||
void beep_init(int t){
|
||||
//gpio_init(P33_10, GPO, 0, GPO_PUSH_PULL); // 蜂鸣器初始化
|
||||
gpio_init(A14, GPO, 0, GPO_PUSH_PULL);
|
||||
beep_T=t;
|
||||
}
|
||||
void beep_on(void){
|
||||
// pwm_set_duty(ATOM2_CH0_P33_10,5000);
|
||||
//gpio_set_level(P33_10,1);
|
||||
gpio_set_level(A14,1);
|
||||
}
|
||||
void beep_off(void){
|
||||
// pwm_set_duty(ATOM2_CH0_P33_10,0);
|
||||
//gpio_set_level(P33_10,0);
|
||||
gpio_set_level(A14,0);
|
||||
}
|
||||
void beep_on_time(int ms){
|
||||
beep_time=ms/beep_T;
|
||||
}
|
||||
|
||||
void beep_actuator(void){
|
||||
if(beep_time>0){
|
||||
beep_on();
|
||||
beep_time--;
|
||||
}
|
||||
else{
|
||||
beep_off();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
#ifndef __BEEP_H
|
||||
#define __BEEP_H
|
||||
|
||||
//初始化，参数为周期单位ms
|
||||
void beep_init(int t);
|
||||
void beep_on(void);
|
||||
void beep_off(void);
|
||||
//蜂鸣器鸣叫指定时间，参数为时间单位ms
|
||||
void beep_on_time(int ms);
|
||||
void beep_actuator(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,66 @@
|
||||
#include "Command.h"
|
||||
#include "zf_driver_uart.h"
|
||||
#include "zf_common_interrupt.h"
|
||||
#include "ti/driverlib/dl_uart_main.h"
|
||||
#include "zf_common_fifo.h"
|
||||
|
||||
volatile fifo_struct command_fifo = {0};
|
||||
volatile uint8_t command_fifo_buffer[COMMAND_FIFO_BUFFER_SIZE] = {0};
|
||||
uint8_t command_sum(uint8_t* data,int len);
|
||||
|
||||
void command_callback(uint32 state, void *ptr)
|
||||
{
|
||||
while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据
|
||||
{
|
||||
fifo_write_element((fifo_struct*)&command_fifo,DL_UART_Main_receiveData(UART2));//写入软件FIFO
|
||||
}
|
||||
}
|
||||
void command_init()
|
||||
{
|
||||
uart_init(UART_2,115200,UART2_TX_B15,UART2_RX_B16);
|
||||
uart_set_interrupt_config(UART_2,UART_INTERRUPT_CONFIG_RX_ENABLE);
|
||||
interrupt_set_priority(UART2_INT_IRQn,1);//设置为与主要任务平行的优先级
|
||||
|
||||
//开启发送DMA
|
||||
|
||||
//开启硬件FIFO
|
||||
DL_UART_Main_enableFIFOs(UART2);
|
||||
DL_UART_Main_setRXFIFOThreshold(UART2,DL_UART_MAIN_RX_FIFO_LEVEL_1_2_FULL);
|
||||
|
||||
//设置软件FIFO
|
||||
fifo_init((fifo_struct *)&command_fifo,FIFO_DATA_8BIT,(void*)command_fifo_buffer,COMMAND_FIFO_BUFFER_SIZE);
|
||||
//设置中断回调
|
||||
uart_set_callback(UART_2,command_callback,NULL);
|
||||
}
|
||||
//从软件FIFO中解析数据包并执行
|
||||
void command_task()
|
||||
{
|
||||
|
||||
}
|
||||
//DMA发送使用的地址
|
||||
uint8_t angle_velocity_data[6] = {0};
|
||||
//发送速度前馈
|
||||
void command_send_angle_velocity(float* pointer_angle_velocity)
|
||||
{
|
||||
//等待上一次DMA发送完成
|
||||
|
||||
//填充数据
|
||||
uint8_t* p = (uint8_t*)pointer_angle_velocity;
|
||||
angle_velocity_data[0]=0xA1;
|
||||
angle_velocity_data[1]=*(p++);
|
||||
angle_velocity_data[2]=*(p++);
|
||||
angle_velocity_data[3]=*(p++);
|
||||
angle_velocity_data[4]=*(p++);
|
||||
angle_velocity_data[5]=command_sum(angle_velocity_data,5);
|
||||
//DMA发送（非阻塞）
|
||||
|
||||
}
|
||||
uint8_t command_sum(uint8_t* data,int len)
|
||||
{
|
||||
uint8_t sum = 0;
|
||||
for(int i=0;i<len;i++)
|
||||
{
|
||||
sum+=data[i];
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
#ifndef __COMMAND_H
|
||||
#define __COMMAND_H
|
||||
|
||||
#define COMMAND_FIFO_BUFFER_SIZE 16
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
void command_init();
|
||||
void command_task();
|
||||
void command_send_angle_velocity(float* pointer_angle_velocity);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,122 @@
|
||||
#include "GrayscaleSensor.h"
|
||||
#include "zf_driver_adc.h"
|
||||
#include "zf_driver_gpio.h"
|
||||
#include "zf_driver_delay.h"
|
||||
|
||||
#define AD2 B8
|
||||
#define AD1 B9
|
||||
#define AD0 B14
|
||||
|
||||
#define FILTER_ALPHA 0.5f //位置滤波系数，范围 [0, 1]，值越大响应越快但噪声越大
|
||||
|
||||
volatile static uint16 adc_white[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的白色参考值，初始值为满量程
|
||||
volatile static uint16 adc_black[8] = {0,0,0,0,0,0,0,0}; // 存储8路灰度传感器的黑色参考值，初始值为0
|
||||
volatile static uint16 adc_range[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的动态范围，初始值为满量程
|
||||
volatile static uint16 adc_raw_value[8] = {0}; // 存储8路灰度传感器的原始 ADC 值
|
||||
static float adc_normalized[8] = {0}; // 存储8路灰度传感器的归一化值，范围 [0, 1]
|
||||
static float adc_normalized_sum = 0; // 归一化值的总和，用于计算位置
|
||||
volatile float grayscale_sensor_position_prev = 0; // 存储小车相对于黑线的位置
|
||||
volatile float grayscale_sensor_position = 0; // 存储小车相对于黑线的位置
|
||||
static const int position[8] = {-4, -3, -2, -1, 1, 2, 3, 4}; // 每个传感器对应的位置
|
||||
|
||||
void grayscale_sensor_set_white(void)
|
||||
{
|
||||
for(uint8 i=0;i<8;i++)
|
||||
{
|
||||
adc_white[i] = adc_raw_value[i];
|
||||
adc_range[i] = adc_white[i] - adc_black[i];
|
||||
}
|
||||
}
|
||||
void grayscale_sensor_set_black(void)
|
||||
{
|
||||
for(uint8 i=0;i<8;i++)
|
||||
{
|
||||
adc_black[i] = adc_raw_value[i];
|
||||
adc_range[i] = adc_white[i] - adc_black[i];
|
||||
}
|
||||
}
|
||||
|
||||
void grayscale_sensor_init(void)
|
||||
{
|
||||
adc_init(ADC1_CH5_B18, ADC_12BIT);
|
||||
gpio_init(AD2, GPO, GPIO_LOW, GPO_PUSH_PULL);
|
||||
gpio_init(AD1, GPO, GPIO_LOW, GPO_PUSH_PULL);
|
||||
gpio_init(AD0, GPO, GPIO_LOW, GPO_PUSH_PULL);
|
||||
gpio_set_level(AD2, 1);
|
||||
gpio_set_level(AD1, 1);
|
||||
gpio_set_level(AD0, 1);
|
||||
}
|
||||
/*真值表
|
||||
AD2 AD1 AD0
|
||||
0 0 0 CH1
|
||||
0 0 1 CH2
|
||||
0 1 0 CH3
|
||||
0 1 1 CH4
|
||||
1 0 0 CH5
|
||||
1 0 1 CH6
|
||||
1 1 0 CH7
|
||||
1 1 1 CH8
|
||||
*/
|
||||
void grayscale_sensor_scan(void)
|
||||
{
|
||||
gpio_set_level(AD2, 0);
|
||||
gpio_set_level(AD1, 0);
|
||||
gpio_set_level(AD0, 0); // CH1
|
||||
system_delay_us(1);
|
||||
adc_raw_value[0]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 0);
|
||||
gpio_set_level(AD1, 0);
|
||||
gpio_set_level(AD0, 1); // CH2
|
||||
system_delay_us(1);
|
||||
adc_raw_value[1]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 0);
|
||||
gpio_set_level(AD1, 1);
|
||||
gpio_set_level(AD0, 0); // CH3
|
||||
system_delay_us(1);
|
||||
adc_raw_value[2]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 0);
|
||||
gpio_set_level(AD1, 1);
|
||||
gpio_set_level(AD0, 1); // CH4
|
||||
system_delay_us(1);
|
||||
adc_raw_value[3]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 1);
|
||||
gpio_set_level(AD1, 0);
|
||||
gpio_set_level(AD0, 0); // CH5
|
||||
system_delay_us(1);
|
||||
adc_raw_value[4]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 1);
|
||||
gpio_set_level(AD1, 0);
|
||||
gpio_set_level(AD0, 1); // CH6
|
||||
system_delay_us(1);
|
||||
adc_raw_value[5]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 1);
|
||||
gpio_set_level(AD1, 1);
|
||||
gpio_set_level(AD0, 0); // CH7
|
||||
system_delay_us(1);
|
||||
adc_raw_value[6]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
gpio_set_level(AD2, 1);
|
||||
gpio_set_level(AD1, 1);
|
||||
gpio_set_level(AD0, 1); // CH8
|
||||
system_delay_us(1);
|
||||
adc_raw_value[7]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
||||
|
||||
// 计算小车相对于黑线的位置
|
||||
grayscale_sensor_position = 0;
|
||||
adc_normalized_sum = 0;
|
||||
for(uint8 i=0;i<8;i++)
|
||||
{
|
||||
adc_normalized[i] = ((float)(adc_white[i] - adc_raw_value[i])) / adc_range[i];
|
||||
grayscale_sensor_position += adc_normalized[i] * position[i];
|
||||
adc_normalized_sum += adc_normalized[i];
|
||||
}
|
||||
if(adc_normalized_sum > 0)
|
||||
{
|
||||
grayscale_sensor_position /= adc_normalized_sum;
|
||||
}
|
||||
else
|
||||
{
|
||||
grayscale_sensor_position = 0; // 避免除以零，默认位置为0
|
||||
}
|
||||
grayscale_sensor_position = grayscale_sensor_position_prev + FILTER_ALPHA * (grayscale_sensor_position - grayscale_sensor_position_prev);
|
||||
grayscale_sensor_position_prev = grayscale_sensor_position;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef GRAYSCALESENSOR_H
|
||||
#define GRAYSCALESENSOR_H
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
extern volatile float grayscale_sensor_position; // 存储小车相对于黑线的位置
|
||||
|
||||
void grayscale_sensor_init(void);
|
||||
void grayscale_sensor_scan(void);
|
||||
void grayscale_sensor_set_white(void);
|
||||
void grayscale_sensor_set_black(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,172 @@
|
||||
#include "ICM_42688.h"
|
||||
|
||||
//==================================================================================================
|
||||
// 内部变量
|
||||
//==================================================================================================
|
||||
|
||||
static float accel_scale = 1.0f; // 加速度计 LSB → g
|
||||
static float gyro_scale = 1.0f; // 陀螺仪 LSB → °/s
|
||||
|
||||
//==================================================================================================
|
||||
// SPI 底层操作宏
|
||||
//==================================================================================================
|
||||
|
||||
#define ICM42688_WRITE_REG(reg, data) spi_write_8bit_register(ICM42688_SPI, reg, data)
|
||||
#define ICM42688_READ_REGS(reg, data, num) spi_read_8bit_registers(ICM42688_SPI, (reg) | 0x80, data, num)
|
||||
|
||||
//==================================================================================================
|
||||
// 静态函数 — 寄存器级 SPI 操作
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 向指定寄存器写入一字节
|
||||
* @param reg 寄存器地址
|
||||
* @param data 写入值
|
||||
* 片选 → 写寄存器 → 释放片选
|
||||
*/
|
||||
static void icm42688_write_reg(uint8_t reg, uint8_t data)
|
||||
{
|
||||
ICM42688_CS_LEVEL(0);
|
||||
ICM42688_WRITE_REG(reg, data);
|
||||
ICM42688_CS_LEVEL(1);
|
||||
}
|
||||
|
||||
/** @brief 从指定寄存器连续读取多字节
|
||||
* @param reg 起始寄存器地址
|
||||
* @param data 输出缓冲区指针
|
||||
* @param num 读取字节数
|
||||
* 片选 → 读多字节 → 释放片选
|
||||
*/
|
||||
static void icm42688_read_regs(uint8_t reg, uint8_t *data, uint8_t num)
|
||||
{
|
||||
ICM42688_CS_LEVEL(0);
|
||||
ICM42688_READ_REGS(reg, data, num);
|
||||
ICM42688_CS_LEVEL(1);
|
||||
}
|
||||
|
||||
/** @brief 将两个 uint8 拼接为 int16（大端序）
|
||||
* @param hi 高字节
|
||||
* @param lo 低字节
|
||||
* @return 有符号 16 位整数
|
||||
*/
|
||||
static int16_t icm42688_combine_bytes(uint8_t hi, uint8_t lo)
|
||||
{
|
||||
return (int16_t)(((uint16_t)hi << 8) | lo);
|
||||
}
|
||||
|
||||
/** @brief 配置加速度计/陀螺仪的量程和输出速率
|
||||
* @param afs 加速度计量程
|
||||
* @param aodr 加速度计输出速率
|
||||
* @param gfs 陀螺仪量程
|
||||
* @param godr 陀螺仪输出速率
|
||||
* 同时更新内部比例系数 accel_scale / gyro_scale
|
||||
*/
|
||||
static void icm42688_set_range_odr(enum icm42688_afs afs, enum icm42688_aodr aodr,
|
||||
enum icm42688_gfs gfs, enum icm42688_godr godr)
|
||||
{
|
||||
icm42688_write_reg(ICM42688_ACCEL_CONFIG0, (afs << 5) | (aodr + 1));
|
||||
icm42688_write_reg(ICM42688_GYRO_CONFIG0, (gfs << 5) | (godr + 1));
|
||||
|
||||
switch (afs) {
|
||||
case ICM42688_AFS_2G: accel_scale = 2000.0f / 32768.0f; break;
|
||||
case ICM42688_AFS_4G: accel_scale = 4000.0f / 32768.0f; break;
|
||||
case ICM42688_AFS_8G: accel_scale = 8000.0f / 32768.0f; break;
|
||||
case ICM42688_AFS_16G: accel_scale = 16000.0f / 32768.0f; break;
|
||||
default: accel_scale = 1.0f; break;
|
||||
}
|
||||
|
||||
switch (gfs) {
|
||||
case ICM42688_GFS_15_625DPS: gyro_scale = 15.625f / 32768.0f; break;
|
||||
case ICM42688_GFS_31_25DPS: gyro_scale = 31.25f / 32768.0f; break;
|
||||
case ICM42688_GFS_62_5DPS: gyro_scale = 62.5f / 32768.0f; break;
|
||||
case ICM42688_GFS_125DPS: gyro_scale = 125.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_250DPS: gyro_scale = 250.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_500DPS: gyro_scale = 500.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_1000DPS: gyro_scale = 1000.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_2000DPS: gyro_scale = 2000.0f / 32768.0f; break;
|
||||
default: gyro_scale = 1.0f; break;
|
||||
}
|
||||
}
|
||||
|
||||
//==================================================================================================
|
||||
// 公开接口
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 返回当前陀螺仪比例系数 (LSB → °/s)
|
||||
* 供 imu_calibrate_gyro() 将原始 ADC 均值转换为物理偏移量
|
||||
*/
|
||||
float icm42688_gyro_scale(void)
|
||||
{
|
||||
return gyro_scale;
|
||||
}
|
||||
|
||||
/** @brief 初始化 ICM-42688
|
||||
* 1. 初始化 SPI 外设和 CS 引脚
|
||||
* 2. 读取 WHO_AM_I 进行自检（超时 500ms 后报错死循环）
|
||||
* 3. 复位并配置量程/速率（默认 ±16G / ±2000dps / 200Hz ODR）
|
||||
* 4. 设置为低噪声模式
|
||||
* 由 imu_init() 内部调用，用户一般不直接调用
|
||||
*/
|
||||
void icm42688_init(void)
|
||||
{
|
||||
system_delay_ms(10);
|
||||
spi_init(ICM42688_SPI, SPI_MODE0, ICM42688_SPI_SPEED,
|
||||
ICM42688_SPC_PIN, ICM42688_SDI_PIN, ICM42688_SDO_PIN, SPI_CS_NULL);
|
||||
gpio_init(ICM42688_CS_PIN, GPO, GPIO_HIGH, GPO_PUSH_PULL);
|
||||
|
||||
uint8_t model = 0xFF;
|
||||
int timeout = 50;
|
||||
while (1) {
|
||||
icm42688_read_regs(ICM42688_WHO_AM_I, &model, 1);
|
||||
if (model == 0x47) {
|
||||
break;
|
||||
}
|
||||
ICM42688_DELAY_MS(10);
|
||||
if (--timeout < 0) {
|
||||
zf_log(0, "ICM42688 Init Error!");
|
||||
while (1);
|
||||
}
|
||||
}
|
||||
|
||||
icm42688_write_reg(ICM42688_PWR_MGMT0, 0x00);
|
||||
ICM42688_DELAY_MS(20);
|
||||
|
||||
// 如果修改了采样频率（定时器中断周期），则需要对应修改下方的配置
|
||||
icm42688_set_range_odr(ICM42688_AFS_16G, ICM42688_AODR_200HZ,
|
||||
ICM42688_GFS_2000DPS, ICM42688_GODR_200HZ);
|
||||
|
||||
icm42688_write_reg(ICM42688_PWR_MGMT0, 0x0F);
|
||||
ICM42688_DELAY_MS(10);
|
||||
}
|
||||
|
||||
/** @brief 读取全部传感器数据（加速度 + 角速度）
|
||||
* @param data 输出结构体指针
|
||||
* 一次 12 字节 SPI 突发从 ACCEL_DATA_X1 开始连续读取
|
||||
* 读取顺序: AX→AY→AZ→GX→GY→GZ（与传感器内部 FIFO 顺序一致）
|
||||
*/
|
||||
void icm42688_read_all(icm42688_data_t *data)
|
||||
{
|
||||
uint8_t buf[12];
|
||||
icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buf, 12);
|
||||
|
||||
data->accel_x = accel_scale * icm42688_combine_bytes(buf[0], buf[1]);
|
||||
data->accel_y = accel_scale * icm42688_combine_bytes(buf[2], buf[3]);
|
||||
data->accel_z = accel_scale * icm42688_combine_bytes(buf[4], buf[5]);
|
||||
|
||||
data->gyro_x = gyro_scale * icm42688_combine_bytes(buf[6], buf[7]);
|
||||
data->gyro_y = gyro_scale * icm42688_combine_bytes(buf[8], buf[9]);
|
||||
data->gyro_z = gyro_scale * icm42688_combine_bytes(buf[10], buf[11]);
|
||||
}
|
||||
|
||||
/** @brief 读取陀螺仪原始 ADC 值（不做比例换算）
|
||||
* @param gx/gy/gz 输出 int16 原始值指针
|
||||
* 供 imu_calibrate_gyro() 在静止状态下采集原始数据计算零偏
|
||||
*/
|
||||
void icm42688_read_gyro_raw(int16_t *gx, int16_t *gy, int16_t *gz)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
icm42688_read_regs(ICM42688_GYRO_DATA_X1, buf, 6);
|
||||
|
||||
*gx = icm42688_combine_bytes(buf[0], buf[1]);
|
||||
*gy = icm42688_combine_bytes(buf[2], buf[3]);
|
||||
*gz = icm42688_combine_bytes(buf[4], buf[5]);
|
||||
}
|
||||
@@ -0,0 +1,198 @@
|
||||
#ifndef __ICM_42688_H
|
||||
#define __ICM_42688_H
|
||||
|
||||
#include "zf_common_headfile.h"
|
||||
|
||||
//==================================================================================================
|
||||
// ICM-42688-P 6轴惯性传感器 硬件抽象层
|
||||
// 本文件仅负责 SPI 通信、寄存器读写、原始数据获取，不涉及姿态解算
|
||||
//==================================================================================================
|
||||
|
||||
// SPI 引脚定义
|
||||
#define ICM42688_SPI (SPI_1)
|
||||
#define ICM42688_SPI_SPEED (8 * 1000 * 1000)
|
||||
#define ICM42688_SPC_PIN (SPI1_SCK_B23)
|
||||
#define ICM42688_SDI_PIN (SPI1_MOSI_B22)
|
||||
#define ICM42688_SDO_PIN (SPI1_MISO_B21)
|
||||
#define ICM42688_CS_PIN (B19)
|
||||
|
||||
#define ICM42688_DELAY_MS(time) (system_delay_ms(time))
|
||||
#define ICM42688_CS_LEVEL(level) gpio_set_level(ICM42688_CS_PIN, level)
|
||||
|
||||
// 传感器原始数据（物理单位：加速度 g，角速度 °/s，传感器坐标系）
|
||||
typedef struct {
|
||||
float accel_x, accel_y, accel_z;
|
||||
float gyro_x, gyro_y, gyro_z;
|
||||
} icm42688_data_t;
|
||||
|
||||
// 加速度计量程
|
||||
enum icm42688_afs {
|
||||
ICM42688_AFS_16G,
|
||||
ICM42688_AFS_8G,
|
||||
ICM42688_AFS_4G,
|
||||
ICM42688_AFS_2G,
|
||||
};
|
||||
|
||||
// 加速度计输出速率 (ODR)
|
||||
enum icm42688_aodr {
|
||||
ICM42688_AODR_32000HZ,
|
||||
ICM42688_AODR_16000HZ,
|
||||
ICM42688_AODR_8000HZ,
|
||||
ICM42688_AODR_4000HZ,
|
||||
ICM42688_AODR_2000HZ,
|
||||
ICM42688_AODR_1000HZ,
|
||||
ICM42688_AODR_200HZ,
|
||||
ICM42688_AODR_100HZ,
|
||||
ICM42688_AODR_50HZ,
|
||||
ICM42688_AODR_25HZ,
|
||||
ICM42688_AODR_12_5HZ,
|
||||
ICM42688_AODR_6_25HZ,
|
||||
ICM42688_AODR_3_125HZ,
|
||||
ICM42688_AODR_1_5625HZ,
|
||||
ICM42688_AODR_500HZ,
|
||||
};
|
||||
|
||||
// 陀螺仪量程
|
||||
enum icm42688_gfs {
|
||||
ICM42688_GFS_2000DPS,
|
||||
ICM42688_GFS_1000DPS,
|
||||
ICM42688_GFS_500DPS,
|
||||
ICM42688_GFS_250DPS,
|
||||
ICM42688_GFS_125DPS,
|
||||
ICM42688_GFS_62_5DPS,
|
||||
ICM42688_GFS_31_25DPS,
|
||||
ICM42688_GFS_15_625DPS,
|
||||
};
|
||||
|
||||
// 陀螺仪输出速率 (ODR)
|
||||
enum icm42688_godr {
|
||||
ICM42688_GODR_32000HZ,
|
||||
ICM42688_GODR_16000HZ,
|
||||
ICM42688_GODR_8000HZ,
|
||||
ICM42688_GODR_4000HZ,
|
||||
ICM42688_GODR_2000HZ,
|
||||
ICM42688_GODR_1000HZ,
|
||||
ICM42688_GODR_200HZ,
|
||||
ICM42688_GODR_100HZ,
|
||||
ICM42688_GODR_50HZ,
|
||||
ICM42688_GODR_25HZ,
|
||||
ICM42688_GODR_12_5HZ,
|
||||
ICM42688_GODR_X0HZ,
|
||||
ICM42688_GODR_X1HZ,
|
||||
ICM42688_GODR_X2HZ,
|
||||
ICM42688_GODR_500HZ,
|
||||
};
|
||||
|
||||
//==================================================================================================
|
||||
// 公开接口（由 IMU 层调用，用户一般不直接使用）
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 初始化 ICM-42688（SPI 配置 + 寄存器设置 + 自检）
|
||||
* 由 imu_init() 内部调用，用户无需单独调用
|
||||
*/
|
||||
void icm42688_init(void);
|
||||
|
||||
/** @brief 读取加速度和角速度（物理单位，传感器坐标系）
|
||||
* @param data 输出结构体指针
|
||||
* 一次 12 字节 SPI 突发读取，节省通信时间
|
||||
*/
|
||||
void icm42688_read_all(icm42688_data_t *data);
|
||||
|
||||
/** @brief 读取陀螺仪原始 ADC 值（不含比例换算）
|
||||
* @param gx/gy/gz 输出 int16 原始值指针
|
||||
* 供校准函数使用
|
||||
*/
|
||||
void icm42688_read_gyro_raw(int16_t *gx, int16_t *gy, int16_t *gz);
|
||||
|
||||
/** @brief 返回当前陀螺仪比例系数 (LSB → °/s)
|
||||
* 供校准函数计算零偏
|
||||
*/
|
||||
float icm42688_gyro_scale(void);
|
||||
|
||||
//==================================================================================================
|
||||
// 寄存器地址
|
||||
//==================================================================================================
|
||||
|
||||
// Bank 0
|
||||
#define ICM42688_DEVICE_CONFIG 0x11
|
||||
#define ICM42688_DRIVE_CONFIG 0x13
|
||||
#define ICM42688_INT_CONFIG 0x14
|
||||
#define ICM42688_FIFO_CONFIG 0x16
|
||||
#define ICM42688_TEMP_DATA1 0x1D
|
||||
#define ICM42688_TEMP_DATA0 0x1E
|
||||
#define ICM42688_ACCEL_DATA_X1 0x1F
|
||||
#define ICM42688_ACCEL_DATA_X0 0x20
|
||||
#define ICM42688_ACCEL_DATA_Y1 0x21
|
||||
#define ICM42688_ACCEL_DATA_Y0 0x22
|
||||
#define ICM42688_ACCEL_DATA_Z1 0x23
|
||||
#define ICM42688_ACCEL_DATA_Z0 0x24
|
||||
#define ICM42688_GYRO_DATA_X1 0x25
|
||||
#define ICM42688_GYRO_DATA_X0 0x26
|
||||
#define ICM42688_GYRO_DATA_Y1 0x27
|
||||
#define ICM42688_GYRO_DATA_Y0 0x28
|
||||
#define ICM42688_GYRO_DATA_Z1 0x29
|
||||
#define ICM42688_GYRO_DATA_Z0 0x2A
|
||||
#define ICM42688_TMST_FSYNCH 0x2B
|
||||
#define ICM42688_TMST_FSYNCL 0x2C
|
||||
#define ICM42688_INT_STATUS 0x2D
|
||||
#define ICM42688_FIFO_COUNTH 0x2E
|
||||
#define ICM42688_FIFO_COUNTL 0x2F
|
||||
#define ICM42688_FIFO_DATA 0x30
|
||||
#define ICM42688_APEX_DATA0 0x31
|
||||
#define ICM42688_APEX_DATA1 0x32
|
||||
#define ICM42688_APEX_DATA2 0x33
|
||||
#define ICM42688_APEX_DATA3 0x34
|
||||
#define ICM42688_APEX_DATA4 0x35
|
||||
#define ICM42688_APEX_DATA5 0x36
|
||||
#define ICM42688_INT_STATUS2 0x37
|
||||
#define ICM42688_INT_STATUS3 0x38
|
||||
#define ICM42688_SIGNAL_PATH_RESET 0x4B
|
||||
#define ICM42688_INTF_CONFIG0 0x4C
|
||||
#define ICM42688_INTF_CONFIG1 0x4D
|
||||
#define ICM42688_PWR_MGMT0 0x4E
|
||||
#define ICM42688_GYRO_CONFIG0 0x4F
|
||||
#define ICM42688_ACCEL_CONFIG0 0x50
|
||||
#define ICM42688_GYRO_CONFIG1 0x51
|
||||
#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
|
||||
#define ICM42688_ACCEL_CONFIG1 0x53
|
||||
#define ICM42688_TMST_CONFIG 0x54
|
||||
#define ICM42688_APEX_CONFIG0 0x56
|
||||
#define ICM42688_SMD_CONFIG 0x57
|
||||
#define ICM42688_FIFO_CONFIG1 0x5F
|
||||
#define ICM42688_FIFO_CONFIG2 0x60
|
||||
#define ICM42688_FIFO_CONFIG3 0x61
|
||||
#define ICM42688_FSYNC_CONFIG 0x62
|
||||
#define ICM42688_INT_CONFIG0 0x63
|
||||
#define ICM42688_INT_CONFIG1 0x64
|
||||
#define ICM42688_INT_SOURCE0 0x65
|
||||
#define ICM42688_INT_SOURCE1 0x66
|
||||
#define ICM42688_INT_SOURCE3 0x68
|
||||
#define ICM42688_INT_SOURCE4 0x69
|
||||
#define ICM42688_FIFO_LOST_PKT0 0x6C
|
||||
#define ICM42688_FIFO_LOST_PKT1 0x6D
|
||||
#define ICM42688_SELF_TEST_CONFIG 0x70
|
||||
#define ICM42688_WHO_AM_I 0x75
|
||||
#define ICM42688_REG_BANK_SEL 0x76
|
||||
|
||||
// Bank 1
|
||||
#define ICM42688_SENSOR_CONFIG0 0x03
|
||||
#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
|
||||
#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
|
||||
#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
|
||||
#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
|
||||
#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
|
||||
#define ICM42688_GYRO_CONFIG_STATIC7 0x10
|
||||
#define ICM42688_GYRO_CONFIG_STATIC8 0x11
|
||||
#define ICM42688_GYRO_CONFIG_STATIC9 0x12
|
||||
#define ICM42688_GYRO_CONFIG_STATIC10 0x13
|
||||
#define ICM42688_XG_ST_DATA 0x5F
|
||||
#define ICM42688_YG_ST_DATA 0x60
|
||||
#define ICM42688_ZG_ST_DATA 0x61
|
||||
#define ICM42688_TMSTVAL0 0x62
|
||||
#define ICM42688_TMSTVAL1 0x63
|
||||
#define ICM42688_TMSTVAL2 0x64
|
||||
#define ICM42688_INTF_CONFIG4 0x7A
|
||||
#define ICM42688_INTF_CONFIG5 0x7B
|
||||
#define ICM42688_INTF_CONFIG6 0x7C
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,374 @@
|
||||
#include "IMU.h"
|
||||
#include <math.h>
|
||||
|
||||
//==================================================================================================
|
||||
// 常量
|
||||
//==================================================================================================
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.141592654f
|
||||
#endif
|
||||
|
||||
#define TWO_KP_DEF (2.0f * 0.5f) // Mahony 比例增益（默认 1.0）
|
||||
#define TWO_KI_DEF (2.0f * 0.0f) // Mahony 积分增益（默认 0, 关闭）
|
||||
|
||||
//==================================================================================================
|
||||
// Mahony 滤波器状态（静态，本文件内可见）
|
||||
//==================================================================================================
|
||||
|
||||
static float inv_sample_freq; // 1 / 采样频率
|
||||
static float two_kp = TWO_KP_DEF; // 比例增益
|
||||
static float two_ki = TWO_KI_DEF; // 积分增益
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 姿态四元数
|
||||
static float integral_fbx = 0.0f, integral_fby = 0.0f, integral_fbz = 0.0f; // 积分误差项
|
||||
|
||||
//==================================================================================================
|
||||
// 陀螺仪零偏（由 imu_calibrate_gyro 填入）
|
||||
//==================================================================================================
|
||||
|
||||
static float gyro_bias_x = 0.0f;
|
||||
static float gyro_bias_y = 0.0f;
|
||||
static float gyro_bias_z = 0.0f;
|
||||
|
||||
//==================================================================================================
|
||||
// 姿态输出
|
||||
//==================================================================================================
|
||||
|
||||
volatile imu_attitude_t imu_angle;
|
||||
|
||||
volatile int yaw_round_count = 0;
|
||||
|
||||
volatile static float yaw_angle_last = 0.0f;
|
||||
|
||||
|
||||
//==================================================================================================
|
||||
// 双二阶滤波器状态
|
||||
//==================================================================================================
|
||||
|
||||
static biquad_state_t bq_gyro_x, bq_gyro_y, bq_gyro_z;
|
||||
static biquad_state_t bq_accel_x, bq_accel_y, bq_accel_z;
|
||||
|
||||
//==================================================================================================
|
||||
// 公开接口实现
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 初始化 IMU
|
||||
* @param sample_freq_hz 采样频率（Hz），等于定时器中断频率
|
||||
* 1. 初始化 ICM-42688 硬件
|
||||
* 2. 配置 Mahony 采样周期
|
||||
* 3. 初始化各轴陀螺仪/加速度计双二阶滤波器
|
||||
* 滤波器参数:
|
||||
* 陀螺仪 X/Y/Z: 低通 30/20/30Hz → 滤除高频振动
|
||||
* 加速度计 X/Y/Z: 低通 10Hz → 重力矢量提取（响应慢但稳）
|
||||
*/
|
||||
void imu_init(int sample_freq_hz)
|
||||
{
|
||||
icm42688_init();
|
||||
|
||||
inv_sample_freq = 1.0f / sample_freq_hz;
|
||||
|
||||
biquad_init(&bq_gyro_x, BIQUAD_LOWPASS, sample_freq_hz, 30, 0.7071f);
|
||||
biquad_init(&bq_gyro_y, BIQUAD_LOWPASS, sample_freq_hz, 20, 0.5773f);
|
||||
biquad_init(&bq_gyro_z, BIQUAD_LOWPASS, sample_freq_hz, 30, 0.5773f);
|
||||
biquad_init(&bq_accel_x, BIQUAD_LOWPASS, sample_freq_hz, 10, 0.5773f);
|
||||
biquad_init(&bq_accel_y, BIQUAD_LOWPASS, sample_freq_hz, 10, 0.5773f);
|
||||
biquad_init(&bq_accel_z, BIQUAD_LOWPASS, sample_freq_hz, 10, 0.5773f);
|
||||
}
|
||||
|
||||
/** @brief 更新姿态（定时器 ISR 中按固定频率调用）
|
||||
* 数据流：传感器 → 零偏修正 → 双二阶滤波 → Mahony → 欧拉角 → Yaw 连续角度
|
||||
* 传感器坐标系直通，无坐标轴转换
|
||||
* 陀螺仪数据不进 Mahony 前只做偏置修正（Mahony 内部自带角速度积分处理）
|
||||
* 加速度数据经低通滤波后送入 Mahony 做重力矢量参考
|
||||
*/
|
||||
void imu_update(void)
|
||||
{
|
||||
icm42688_data_t raw;
|
||||
icm42688_read_all(&raw);
|
||||
|
||||
// 零偏修正
|
||||
raw.gyro_x -= gyro_bias_x;
|
||||
raw.gyro_y -= gyro_bias_y;
|
||||
raw.gyro_z -= gyro_bias_z;
|
||||
|
||||
// 滤波（传感器坐标系，不做轴转换）
|
||||
imu_angle.gyro_x = biquad_apply(&bq_gyro_x, raw.gyro_x);
|
||||
imu_angle.gyro_y = biquad_apply(&bq_gyro_y, raw.gyro_y);
|
||||
imu_angle.gyro_z = biquad_apply(&bq_gyro_z, raw.gyro_z);
|
||||
|
||||
float ax_f = biquad_apply(&bq_accel_x, raw.accel_x);
|
||||
float ay_f = biquad_apply(&bq_accel_y, raw.accel_y);
|
||||
float az_f = biquad_apply(&bq_accel_z, raw.accel_z);
|
||||
|
||||
// Mahony AHRS 姿态解算
|
||||
mahony_ahrs_update(raw.gyro_x, raw.gyro_y, raw.gyro_z, ax_f, ay_f, az_f);
|
||||
|
||||
// Yaw 连续角度跟踪（处理 ±180° 跳变）
|
||||
if (imu_angle.yaw - yaw_angle_last > 180.0f) {
|
||||
yaw_round_count--;
|
||||
} else if (imu_angle.yaw - yaw_angle_last < -180.0f) {
|
||||
yaw_round_count++;
|
||||
}
|
||||
yaw_angle_last = imu_angle.yaw;
|
||||
}
|
||||
|
||||
/** @brief 陀螺仪零偏校准
|
||||
* 必须在设备完全静止时调用，采集 3 秒陀螺仪原始数据
|
||||
* 每秒采样 1000 点取均值，3 秒的均值再取平均，乘以比例系数得到 °/s 零偏
|
||||
* 校准结果自动存入 gyro_bias_x/y/z，在后续 imu_update() 中生效
|
||||
*/
|
||||
void imu_calibrate_gyro(void)
|
||||
{
|
||||
#define CALIB_SECONDS 3
|
||||
#define CALIB_SAMPLES 1000
|
||||
|
||||
static float gyro_history[3][CALIB_SECONDS];
|
||||
int32_t gyro_sum[3] = {0, 0, 0};
|
||||
uint32_t sec_cnt = 0, sample_cnt = 0;
|
||||
|
||||
system_delay_ms(1000);
|
||||
system_delay_ms(1000);
|
||||
|
||||
do {
|
||||
system_delay_us(1000);
|
||||
int16_t gx_r, gy_r, gz_r;
|
||||
icm42688_read_gyro_raw(&gx_r, &gy_r, &gz_r);
|
||||
|
||||
if (sample_cnt < CALIB_SAMPLES) {
|
||||
gyro_sum[0] += gx_r;
|
||||
gyro_sum[1] += gy_r;
|
||||
gyro_sum[2] += gz_r;
|
||||
sample_cnt++;
|
||||
} else {
|
||||
gyro_history[0][sec_cnt] = (float)gyro_sum[0] / CALIB_SAMPLES;
|
||||
gyro_history[1][sec_cnt] = (float)gyro_sum[1] / CALIB_SAMPLES;
|
||||
gyro_history[2][sec_cnt] = (float)gyro_sum[2] / CALIB_SAMPLES;
|
||||
gyro_sum[0] = 0;
|
||||
gyro_sum[1] = 0;
|
||||
gyro_sum[2] = 0;
|
||||
sample_cnt = 0;
|
||||
sec_cnt++;
|
||||
}
|
||||
} while (sec_cnt < CALIB_SECONDS);
|
||||
|
||||
float sum[3] = {0, 0, 0};
|
||||
float k = icm42688_gyro_scale() / CALIB_SECONDS;
|
||||
for (uint8_t i = 0; i < CALIB_SECONDS; i++) {
|
||||
sum[0] += gyro_history[0][i];
|
||||
sum[1] += gyro_history[1][i];
|
||||
sum[2] += gyro_history[2][i];
|
||||
}
|
||||
|
||||
gyro_bias_x = k * sum[0];
|
||||
gyro_bias_y = k * sum[1];
|
||||
gyro_bias_z = k * sum[2];
|
||||
|
||||
}
|
||||
|
||||
//==================================================================================================
|
||||
// 算法实现
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief Mahony AHRS 6轴姿态解算
|
||||
* @param gx/gy/gz 陀螺仪角速度 (°/s)
|
||||
* @param ax/ay/az 加速度计测量值 (g)
|
||||
*
|
||||
* 算法流程:
|
||||
* 1. 角速度转弧度，加速度归一化
|
||||
* 2. 由当前四元数估计重力方向
|
||||
* 3. 估计重力与测量重力的叉积 → PI 反馈修正陀螺仪
|
||||
* 4. 一阶龙格-库塔积分更新四元数
|
||||
* 5. 四元数归一化
|
||||
* 6. 提取欧拉角写入 imu_angle
|
||||
*
|
||||
* 参考: S.O.H. Madgwick "An efficient orientation filter for IMU and MARG sensor arrays"
|
||||
*/
|
||||
void mahony_ahrs_update(float gx, float gy, float gz, float ax, float ay, float az)
|
||||
{
|
||||
float recip_norm;
|
||||
float halfvx, halfvy, halfvz;
|
||||
float halfex, halfey, halfez;
|
||||
float qa, qb, qc;
|
||||
|
||||
// 仅当加速度计有读数时执行修正（静止或匀加速时加速度计才可靠）
|
||||
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
// 角速度 → rad/s
|
||||
gx *= ANG2RAD;
|
||||
gy *= ANG2RAD;
|
||||
gz *= ANG2RAD;
|
||||
|
||||
// 加速度归一化
|
||||
recip_norm = inv_sqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recip_norm;
|
||||
ay *= recip_norm;
|
||||
az *= recip_norm;
|
||||
|
||||
// 由当前四元数估计重力方向（世界系 Z 轴在传感器系的投影）
|
||||
halfvx = q1 * q3 - q0 * q2;
|
||||
halfvy = q0 * q1 + q2 * q3;
|
||||
halfvz = q0 * q0 - 0.5f + q3 * q3;
|
||||
|
||||
// 叉积 = 估计方向 × 测量方向 → 姿态误差
|
||||
halfex = (ay * halfvz - az * halfvy);
|
||||
halfey = (az * halfvx - ax * halfvz);
|
||||
halfez = (ax * halfvy - ay * halfvx);
|
||||
|
||||
// 积分反馈（默认 ki=0, 关闭）
|
||||
if (two_ki > 0.0f) {
|
||||
integral_fbx += two_ki * halfex * inv_sample_freq;
|
||||
integral_fby += two_ki * halfey * inv_sample_freq;
|
||||
integral_fbz += two_ki * halfez * inv_sample_freq;
|
||||
gx += integral_fbx;
|
||||
gy += integral_fby;
|
||||
gz += integral_fbz;
|
||||
} else {
|
||||
integral_fbx = 0.0f;
|
||||
integral_fby = 0.0f;
|
||||
integral_fbz = 0.0f;
|
||||
}
|
||||
|
||||
// 比例反馈
|
||||
gx += two_kp * halfex;
|
||||
gy += two_kp * halfey;
|
||||
gz += two_kp * halfez;
|
||||
}
|
||||
|
||||
// 四元数一阶积分 (RK1)
|
||||
gx *= (0.5f * inv_sample_freq);
|
||||
gy *= (0.5f * inv_sample_freq);
|
||||
gz *= (0.5f * inv_sample_freq);
|
||||
qa = q0; qb = q1; qc = q2;
|
||||
q0 += (-qb * gx - qc * gy - q3 * gz);
|
||||
q1 += ( qa * gx + qc * gz - q3 * gy);
|
||||
q2 += ( qa * gy - qb * gz + q3 * gx);
|
||||
q3 += ( qa * gz + qb * gy - qc * gx);
|
||||
|
||||
// 四元数归一化
|
||||
recip_norm = inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recip_norm;
|
||||
q1 *= recip_norm;
|
||||
q2 *= recip_norm;
|
||||
q3 *= recip_norm;
|
||||
|
||||
// 旋转矩阵 → 欧拉角（传感器坐标系）
|
||||
float r11 = 2.0f * (q0 * q1 + q2 * q3);
|
||||
float r12 = 1.0f - 2.0f * (q1 * q1 + q2 * q2);
|
||||
float r21 = 2.0f * (q0 * q2 - q3 * q1);
|
||||
float r31 = 2.0f * (q0 * q3 + q1 * q2);
|
||||
float r32 = 1.0f - 2.0f * (q2 * q2 + q3 * q3);
|
||||
|
||||
imu_angle.pitch = RAD2ANG * asinf(r21);
|
||||
imu_angle.roll = RAD2ANG * atan2f(r11, r12);
|
||||
imu_angle.yaw = RAD2ANG * atan2f(r31, r32);
|
||||
}
|
||||
|
||||
/** @brief 双二阶滤波器系数初始化
|
||||
* @param state 滤波器状态
|
||||
* @param type 滤波器类型
|
||||
* @param fs 采样率 (Hz)
|
||||
* @param fc 截止/中心频率 (Hz)
|
||||
* @param q 品质因数
|
||||
*
|
||||
* 通用双二阶传递函数:
|
||||
* H(z) = (b0 + b1*z^-1 + b2*z^-2) / (a0 + a1*z^-1 + a2*z^-2)
|
||||
*
|
||||
* Q 值参考:
|
||||
* 0.5773 Thomson-Bessel → 最平坦群延迟, 相位失真最小
|
||||
* 0.7071 Butterworth → 通带最平坦, 常用默认值
|
||||
* 0.9565 Chebyshev 1dB → 陡峭过渡带, 允许 1dB 通带波纹
|
||||
*/
|
||||
void biquad_init(biquad_state_t *state, biquad_type_t type, int fs, float fc, float q)
|
||||
{
|
||||
float w0 = 2.0f * PI * fc / fs;
|
||||
float sin_w0 = sinf(w0);
|
||||
float cos_w0 = cosf(w0);
|
||||
float alpha = sin_w0 / (2.0f * q);
|
||||
|
||||
float b0, b1, b2, a0, a1, a2;
|
||||
|
||||
switch (type) {
|
||||
case BIQUAD_LOWPASS:
|
||||
b0 = (1.0f - cos_w0) / 2.0f;
|
||||
b1 = b0 * 2.0f;
|
||||
b2 = b0;
|
||||
a0 = 1.0f + alpha;
|
||||
a1 = -2.0f * cos_w0;
|
||||
a2 = 1.0f - alpha;
|
||||
break;
|
||||
case BIQUAD_HIGHPASS:
|
||||
b0 = (1.0f + cos_w0) / 2.0f;
|
||||
b1 = -b0 * 2.0f;
|
||||
b2 = b0;
|
||||
a0 = 1.0f + alpha;
|
||||
a1 = -2.0f * cos_w0;
|
||||
a2 = 1.0f - alpha;
|
||||
break;
|
||||
case BIQUAD_BANDPASS_PEAK:
|
||||
b0 = alpha;
|
||||
b1 = 0.0f;
|
||||
b2 = -alpha;
|
||||
a0 = 1.0f + alpha;
|
||||
a1 = -2.0f * cos_w0;
|
||||
a2 = 1.0f - alpha;
|
||||
break;
|
||||
case BIQUAD_BANDSTOP_NOTCH:
|
||||
b0 = 1.0f;
|
||||
b1 = -2.0f * cos_w0;
|
||||
b2 = 1.0f;
|
||||
a0 = 1.0f + alpha;
|
||||
a1 = -2.0f * cos_w0;
|
||||
a2 = 1.0f - alpha;
|
||||
break;
|
||||
}
|
||||
|
||||
// 归一化系数（除以 a0）
|
||||
state->a0 = b0 / a0;
|
||||
state->a1 = b1 / a0;
|
||||
state->a2 = b2 / a0;
|
||||
state->a3 = a1 / a0;
|
||||
state->a4 = a2 / a0;
|
||||
state->x1 = 0.0f;
|
||||
state->x2 = 0.0f;
|
||||
state->y1 = 0.0f;
|
||||
state->y2 = 0.0f;
|
||||
}
|
||||
|
||||
/** @brief 双二阶滤波器单步迭代
|
||||
* @param state 滤波器状态
|
||||
* @param data 当前输入
|
||||
* @return 当前输出
|
||||
*
|
||||
* 差分方程:
|
||||
* y[n] = a0*x[n] + a1*x[n-1] + a2*x[n-2] - a3*y[n-1] - a4*y[n-2]
|
||||
*/
|
||||
float biquad_apply(biquad_state_t *state, float data)
|
||||
{
|
||||
float result = state->a0 * data
|
||||
+ state->a1 * state->x1
|
||||
+ state->a2 * state->x2
|
||||
- state->a3 * state->y1
|
||||
- state->a4 * state->y2;
|
||||
state->x2 = state->x1;
|
||||
state->x1 = data;
|
||||
state->y2 = state->y1;
|
||||
state->y1 = result;
|
||||
return result;
|
||||
}
|
||||
|
||||
/** @brief 快速平方根倒数 (Fast Inverse Square Root)
|
||||
* @param x 输入正数
|
||||
* @return 1/sqrt(x) 近似值
|
||||
*
|
||||
* 使用 Quake III 的经典 magic number 0x5f3759df
|
||||
* 精度约 0.1%, 在无 FPU 的 M0+ 上远快于 1.0f/sqrtf(x)
|
||||
*/
|
||||
float inv_sqrt(float x)
|
||||
{
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long *)&y;
|
||||
i = 0x5f3759df - (i >> 1);
|
||||
y = *(float *)&i;
|
||||
y = y * (1.5f - (halfx * y * y)); // 一次 Newton 迭代
|
||||
return y;
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
#ifndef __IMU_H
|
||||
#define __IMU_H
|
||||
|
||||
#include "ICM_42688.h"
|
||||
|
||||
//==================================================================================================
|
||||
// IMU 姿态解算层
|
||||
// 负责：传感器数据滤波 → Mahony AHRS 姿态解算 → 欧拉角输出
|
||||
// 所有数据基于传感器坐标系（无机体坐标转换）
|
||||
//==================================================================================================
|
||||
|
||||
#define ANG2RAD 0.01745329252f // 角度制 → 弧度制
|
||||
#define RAD2ANG 57.295779513f // 弧度制 → 角度制
|
||||
|
||||
//==================================================================================================
|
||||
// 双二阶滤波器 (Biquad Filter)
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 滤波器类型 */
|
||||
typedef enum {
|
||||
BIQUAD_LOWPASS, // 低通
|
||||
BIQUAD_HIGHPASS, // 高通
|
||||
BIQUAD_BANDPASS_PEAK, // 带通（峰值）
|
||||
BIQUAD_BANDSTOP_NOTCH, // 带阻（陷波）
|
||||
} biquad_type_t;
|
||||
|
||||
/** @brief 滤波器状态（系数 + 历史值）
|
||||
* 典型 Q 值: 0.5773 (Thomson-Bessel, 相位线性度好)
|
||||
* 0.7071 (Butterworth, 最平坦)
|
||||
* 0.9565 (Chebyshev 1dB, 阻带衰减快)
|
||||
*/
|
||||
typedef struct {
|
||||
float a0, a1, a2, a3, a4; // 传递函数系数
|
||||
float x1, x2, y1, y2; // 历史输入/输出
|
||||
} biquad_state_t;
|
||||
|
||||
//==================================================================================================
|
||||
// 姿态输出
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 姿态数据（传感器坐标系，volatile 因为 ISR 写入、主循环读取） */
|
||||
typedef struct {
|
||||
float pitch, roll, yaw; // 欧拉角 (°)
|
||||
float gyro_x, gyro_y, gyro_z; // 滤波后角速度 (°/s)
|
||||
} imu_attitude_t;
|
||||
|
||||
extern volatile imu_attitude_t imu_angle; // 当前姿态
|
||||
extern volatile int yaw_round_count;
|
||||
|
||||
//==================================================================================================
|
||||
// 用户接口
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 初始化 IMU（传感器 + 滤波器 + Mahony）
|
||||
* @param sample_freq_hz 采样频率（Hz），需与定时器中断频率一致，典型值 200
|
||||
* 用户只需调用此函数，内部自动调用 icm42688_init()
|
||||
*/
|
||||
void imu_init(int sample_freq_hz);
|
||||
|
||||
/** @brief 更新姿态（在定时器 ISR 中以固定频率调用）
|
||||
* 读取传感器 → 零偏修正 → 双二阶滤波 → Mahony 解算 → 写入 imu_angle
|
||||
* 执行时间约 100~150us（80MHz Cortex-M0+ 估算），需保证在中断周期内完成
|
||||
*/
|
||||
void imu_update(void);
|
||||
|
||||
/** @brief 陀螺仪零偏校准（静止状态下调用）
|
||||
* 采集 3 秒静止数据，计算各轴零偏，自动应用于后续 imu_update()
|
||||
* 调用时设备必须完全静止
|
||||
*/
|
||||
void imu_calibrate_gyro(void);
|
||||
|
||||
//==================================================================================================
|
||||
// 算法接口（也可被外部直接调用）
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 双二阶滤波器初始化
|
||||
* @param state 滤波器状态指针
|
||||
* @param type 滤波器类型
|
||||
* @param fs 采样频率 (Hz)
|
||||
* @param fc 截止频率 (Hz)
|
||||
* @param q 品质因数
|
||||
*/
|
||||
void biquad_init(biquad_state_t *state, biquad_type_t type, int fs, float fc, float q);
|
||||
|
||||
/** @brief 双二阶滤波器单步计算
|
||||
* @param state 滤波器状态指针
|
||||
* @param data 当前输入值
|
||||
* @return 滤波后输出值
|
||||
*/
|
||||
float biquad_apply(biquad_state_t *state, float data);
|
||||
|
||||
/** @brief Mahony AHRS 姿态解算（6轴 IMU 模式）
|
||||
* @param gx/gy/gz 陀螺仪角速度 (°/s, 传感器坐标系)
|
||||
* @param ax/ay/az 加速度计测量值 (g, 传感器坐标系)
|
||||
* 输出写入全局变量 imu_angle.pitch/roll/yaw
|
||||
*/
|
||||
void mahony_ahrs_update(float gx, float gy, float gz, float ax, float ay, float az);
|
||||
|
||||
/** @brief 快速平方根倒数（Quake III 算法）
|
||||
* @param x 输入值
|
||||
* @return 1/sqrt(x) 的近似值
|
||||
*/
|
||||
float inv_sqrt(float x);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,33 @@
|
||||
#include "Motor.h"
|
||||
#include "zf_driver_gpio.h"
|
||||
#include "zf_driver_pwm.h"
|
||||
#include "PID_Controller.h"
|
||||
#include "MotorEncoder.h"
|
||||
|
||||
void motor_init(void)
|
||||
{
|
||||
gpio_init(DIR_L, GPO, GPIO_HIGH, GPO_PUSH_PULL);
|
||||
pwm_init(PWM_L, 17000, 0);
|
||||
|
||||
gpio_init(DIR_R, GPO, GPIO_HIGH, GPO_PUSH_PULL);
|
||||
pwm_init(PWM_R, 17000, 0);
|
||||
}
|
||||
// 右电机速度设置
|
||||
void motor_set_pwm_right(int32 pwm)
|
||||
{
|
||||
uint8_t dir = (pwm >= 0) ? 1 : 0;//根据电机接线方式调整方向
|
||||
uint32 abs_pwm = abs(pwm);
|
||||
if(pwm > MOTOR_MAX) pwm = MOTOR_MAX;
|
||||
gpio_set_level(DIR_R, dir);//设置方向
|
||||
pwm_set_duty(PWM_R, abs_pwm);//设置PWM占空比
|
||||
}
|
||||
|
||||
// 左电机速度设置 - 需要根据实际硬件修改
|
||||
void motor_set_pwm_left(int32 pwm)
|
||||
{
|
||||
uint8_t dir = (pwm >= 0) ? 0 : 1;//根据电机接线方式调整方向
|
||||
uint32 abs_pwm = abs(pwm);
|
||||
if(pwm > MOTOR_MAX) pwm = MOTOR_MAX;
|
||||
gpio_set_level(DIR_L, dir);//设置方向
|
||||
pwm_set_duty(PWM_L, abs_pwm);//设置PWM占空比
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
#ifndef __MOTOR_H
|
||||
#define __MOTOR_H
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
#define MOTOR_MAX (3000)
|
||||
|
||||
#define DIR_R (B11)
|
||||
#define PWM_R (PWM_TIM_G0_CH0_B10)
|
||||
|
||||
#define DIR_L (A27)
|
||||
#define PWM_L (PWM_TIM_G7_CH0_A26)
|
||||
|
||||
void motor_init(void);
|
||||
void motor_set_pwm_right(int32 pwm);
|
||||
void motor_set_pwm_left(int32 pwm);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,76 @@
|
||||
#include "MotorEncoder.h"
|
||||
#include "ti_msp_dl_config.h"
|
||||
#include "ti/driverlib/dl_timerg.h"
|
||||
#include "zf_common_interrupt.h"
|
||||
#include "zf_driver_exti.h"
|
||||
#include "zf_driver_gpio.h"
|
||||
|
||||
#define MOTOR_ENCODER_LEFT_A_PIN A9
|
||||
#define MOTOR_ENCODER_LEFT_B_PIN A12
|
||||
|
||||
volatile motor_encoder_data_t motor_speed_right = {.raw = 0};
|
||||
volatile motor_encoder_data_t motor_speed_left = {.raw = 0};
|
||||
/*
|
||||
由于MSPM0G3507只有一个QEI，因此只能测量一个电机的编码器速度，另一个电机的速度需要通过GPIO中断模拟正交编码器来测量。
|
||||
分配如下：
|
||||
- QEI0：右电机编码器
|
||||
- GPIO中断：左电机编码器
|
||||
*/
|
||||
|
||||
//用中断模拟一个正交编码器(或者用输入捕获)
|
||||
volatile int16_t left_encoder_count = 0;
|
||||
//A相中断
|
||||
void motor_encoder_exit_handler_A(uint32 state, void *ptr)
|
||||
{
|
||||
// 读取A相和B相的状态
|
||||
uint8_t A_state = gpio_get_level(MOTOR_ENCODER_LEFT_A_PIN);
|
||||
uint8_t B_state = gpio_get_level(MOTOR_ENCODER_LEFT_B_PIN);
|
||||
// 根据A相和B相的状态更新计数
|
||||
if(A_state == B_state)
|
||||
{
|
||||
left_encoder_count++;
|
||||
}
|
||||
else
|
||||
{
|
||||
left_encoder_count--;
|
||||
}
|
||||
|
||||
}
|
||||
//B相中断
|
||||
void motor_encoder_exit_handler_B(uint32 state, void *ptr)
|
||||
{
|
||||
// 读取A相和B相的状态
|
||||
uint8_t A_state = gpio_get_level(MOTOR_ENCODER_LEFT_A_PIN);
|
||||
uint8_t B_state = gpio_get_level(MOTOR_ENCODER_LEFT_B_PIN);
|
||||
// 根据A相和B相的状态更新计数
|
||||
if(A_state == B_state)
|
||||
{
|
||||
left_encoder_count--;
|
||||
}
|
||||
else
|
||||
{
|
||||
left_encoder_count++;
|
||||
}
|
||||
}
|
||||
|
||||
void motor_encoder_init(void)
|
||||
{
|
||||
//使用QEI
|
||||
DL_TimerG_startCounter(QEI_0_INST);
|
||||
//使用GPIO中断模拟正交编码器
|
||||
exti_init(MOTOR_ENCODER_LEFT_A_PIN, EXTI_TRIGGER_BOTH, motor_encoder_exit_handler_A, NULL);
|
||||
exti_init(MOTOR_ENCODER_LEFT_B_PIN, EXTI_TRIGGER_BOTH, motor_encoder_exit_handler_B, NULL);
|
||||
interrupt_set_priority(GPIOA_INT_IRQn, 0);
|
||||
}
|
||||
|
||||
void motor_encoder_update(void)
|
||||
{
|
||||
// 读取右电机编码器
|
||||
motor_speed_right.raw = (uint16_t)DL_TimerG_getTimerCount(QEI_0_INST);
|
||||
DL_TimerG_setTimerCount(QEI_0_INST, 0); // 读取后清零
|
||||
|
||||
// 读取左电机编码器
|
||||
motor_speed_left.value = left_encoder_count;
|
||||
left_encoder_count = 0; // 读取后清零
|
||||
}
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef MOTORENCODER_H
|
||||
#define MOTORENCODER_H
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
typedef union{
|
||||
uint16_t raw;
|
||||
int16_t value;
|
||||
} motor_encoder_data_t;
|
||||
|
||||
void motor_encoder_init(void);
|
||||
void motor_encoder_update(void);
|
||||
|
||||
extern volatile motor_encoder_data_t motor_speed_right;
|
||||
extern volatile motor_encoder_data_t motor_speed_left;
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,232 @@
|
||||
/**
|
||||
* @file controller.c
|
||||
* @author wanghongxi
|
||||
* @author modified by neozng
|
||||
* @brief PID控制器定义
|
||||
* @version beta
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
|
||||
*/
|
||||
#include "PID_Controller.h"
|
||||
#include <math.h>
|
||||
|
||||
/* ----------------------------下面是pid优化环节的实现---------------------------- */
|
||||
|
||||
// 梯形积分
|
||||
static void f_Trapezoid_Intergral(PIDInstance *pid)
|
||||
{
|
||||
// 计算梯形的面积,(上底+下底)*高/2
|
||||
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
|
||||
}
|
||||
|
||||
// 变速积分(误差小时积分作用更强)
|
||||
static void f_Changing_Integration_Rate(PIDInstance *pid)
|
||||
{
|
||||
if (pid->Err * pid->Iout > 0)
|
||||
{
|
||||
// 积分呈累积趋势
|
||||
if (fabsf(pid->Err) <= pid->CoefB)
|
||||
return; // Full integral
|
||||
if (fabsf(pid->Err) <= (pid->CoefB + pid->CoefA))
|
||||
pid->ITerm *= (pid->CoefB + pid->CoefA - fabsf(pid->Err)) / pid->CoefA;
|
||||
else // 最大阈值,不使用积分
|
||||
pid->ITerm = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void f_Integral_Limit(PIDInstance *pid)
|
||||
{
|
||||
static float temp_Output, temp_Iout;
|
||||
temp_Iout = pid->Iout + pid->ITerm;
|
||||
temp_Output = pid->Pout + pid->Iout + pid->Dout;
|
||||
if (fabsf(temp_Output) > pid->MaxOut)
|
||||
{
|
||||
if (pid->Err * pid->Iout > 0) // 积分却还在累积
|
||||
{
|
||||
pid->ITerm = 0; // 当前积分项置零
|
||||
}
|
||||
}
|
||||
|
||||
if (temp_Iout > pid->IntegralLimit)
|
||||
{
|
||||
pid->ITerm = 0;
|
||||
pid->Iout = pid->IntegralLimit;
|
||||
}
|
||||
if (temp_Iout < -pid->IntegralLimit)
|
||||
{
|
||||
pid->ITerm = 0;
|
||||
pid->Iout = -pid->IntegralLimit;
|
||||
}
|
||||
}
|
||||
|
||||
// 微分先行(仅使用反馈值而不计参考输入的微分)
|
||||
static void f_Derivative_On_Measurement(PIDInstance *pid)
|
||||
{
|
||||
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
|
||||
}
|
||||
|
||||
// 微分滤波(采集微分时,滤除高频噪声)
|
||||
static void f_Derivative_Filter(PIDInstance *pid)
|
||||
{
|
||||
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
|
||||
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
|
||||
}
|
||||
|
||||
// 输出滤波
|
||||
static void f_Output_Filter(PIDInstance *pid)
|
||||
{
|
||||
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
|
||||
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
|
||||
}
|
||||
|
||||
// 输出限幅
|
||||
static void f_Output_Limit(PIDInstance *pid)
|
||||
{
|
||||
if (pid->Output > pid->MaxOut)
|
||||
{
|
||||
pid->Output = pid->MaxOut;
|
||||
}
|
||||
if (pid->Output < -(pid->MaxOut))
|
||||
{
|
||||
pid->Output = -(pid->MaxOut);
|
||||
}
|
||||
}
|
||||
|
||||
// 电机堵转检测
|
||||
static void f_PID_ErrorHandle(PIDInstance *pid)
|
||||
{
|
||||
/*Motor Blocked Handle*/
|
||||
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
|
||||
return;
|
||||
|
||||
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
|
||||
{
|
||||
// Motor blocked counting
|
||||
pid->ERRORHandler.ERRORCount++;
|
||||
}
|
||||
else
|
||||
{
|
||||
pid->ERRORHandler.ERRORCount = 0;
|
||||
}
|
||||
|
||||
if (pid->ERRORHandler.ERRORCount > 500)
|
||||
{
|
||||
// Motor blocked over 1000times
|
||||
pid->ERRORHandler.ERRORType = PID_MOTOR_BLOCKED_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---------------------------下面是PID的外部算法接口--------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
|
||||
*
|
||||
* @param pid PID实例
|
||||
* @param config PID初始化设置
|
||||
*/
|
||||
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
|
||||
{
|
||||
memset(pid, 0, sizeof(PIDInstance));
|
||||
|
||||
pid->Kp = config->Kp;
|
||||
pid->Ki = config->Ki;
|
||||
pid->Kd = config->Kd;
|
||||
pid->MaxOut = config->MaxOut;
|
||||
pid->DeadBand = config->DeadBand;
|
||||
pid->Improve = config->Improve;
|
||||
pid->IntegralLimit = config->IntegralLimit;
|
||||
pid->CoefA = config->CoefA;
|
||||
pid->CoefB = config->CoefB;
|
||||
pid->Output_LPF_RC = config->Output_LPF_RC;
|
||||
pid->Derivative_LPF_RC = config->Derivative_LPF_RC;
|
||||
pid->dt = config->T;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief PID计算
|
||||
* @param[in] PID结构体
|
||||
* @param[in] 测量值
|
||||
* @param[in] 期望值
|
||||
* @retval 返回空
|
||||
*/
|
||||
float PIDCalculate(PIDInstance *pid, float measure, float ref)
|
||||
{
|
||||
// 堵转检测
|
||||
if (pid->Improve & PID_ErrorHandle)
|
||||
f_PID_ErrorHandle(pid);
|
||||
|
||||
if (pid->dt < 0.000001f) // 避免除零
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
// 保存上次的测量值和误差,计算当前error
|
||||
pid->Measure = measure;
|
||||
pid->Ref = ref;
|
||||
pid->Err = pid->Ref - pid->Measure;
|
||||
/*
|
||||
Yaw角误差修正
|
||||
若err大于180，则应该取反向，负数：-(360-err)=err-360
|
||||
若err小于-180，则应该取反向，正数：360-(-err)=err+360
|
||||
*/
|
||||
if(pid->Improve & PID_Yaw_Error_Corretion)
|
||||
{
|
||||
if(pid->Err > 180)
|
||||
{
|
||||
pid->Err = pid->Err - 360;
|
||||
}
|
||||
else if(pid->Err < -180)
|
||||
{
|
||||
pid->Err = pid->Err +360;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果在死区外,则计算PID
|
||||
if (fabsf(pid->Err) > pid->DeadBand)
|
||||
{
|
||||
// 基本的pid计算,使用位置式
|
||||
pid->Pout = pid->Kp * pid->Err;
|
||||
pid->ITerm = pid->Ki * pid->Err * pid->dt;
|
||||
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
|
||||
|
||||
// 梯形积分
|
||||
if (pid->Improve & PID_Trapezoid_Intergral)
|
||||
f_Trapezoid_Intergral(pid);
|
||||
// 变速积分
|
||||
if (pid->Improve & PID_ChangingIntegrationRate)
|
||||
f_Changing_Integration_Rate(pid);
|
||||
// 微分先行
|
||||
if (pid->Improve & PID_Derivative_On_Measurement)
|
||||
f_Derivative_On_Measurement(pid);
|
||||
// 微分滤波器
|
||||
if (pid->Improve & PID_DerivativeFilter)
|
||||
f_Derivative_Filter(pid);
|
||||
// 积分限幅
|
||||
if (pid->Improve & PID_Integral_Limit)
|
||||
f_Integral_Limit(pid);
|
||||
|
||||
pid->Iout += pid->ITerm; // 累加积分
|
||||
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
|
||||
|
||||
// 输出滤波
|
||||
if (pid->Improve & PID_OutputFilter)
|
||||
f_Output_Filter(pid);
|
||||
|
||||
// 输出限幅
|
||||
f_Output_Limit(pid);
|
||||
}
|
||||
else // 进入死区, 则清空积分和输出
|
||||
{
|
||||
pid->Output = 0;
|
||||
pid->ITerm = 0;
|
||||
}
|
||||
|
||||
// 保存当前数据,用于下次计算
|
||||
pid->Last_Measure = pid->Measure;
|
||||
pid->Last_Output = pid->Output;
|
||||
pid->Last_Dout = pid->Dout;
|
||||
pid->Last_Err = pid->Err;
|
||||
pid->Last_ITerm = pid->ITerm;
|
||||
|
||||
return pid->Output;
|
||||
}
|
||||
@@ -0,0 +1,114 @@
|
||||
#ifndef _PID_CONTROLLER_H
|
||||
#define _PID_CONTROLLER_H
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
|
||||
typedef enum
|
||||
{
|
||||
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
|
||||
PID_Integral_Limit = 0b00000001, // 0000 0001
|
||||
PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
|
||||
PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
|
||||
PID_Yaw_Error_Corretion = 0b00001000, // 0000 1000
|
||||
PID_OutputFilter = 0b00010000, // 0001 0000
|
||||
PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
|
||||
PID_DerivativeFilter = 0b01000000, // 0100 0000
|
||||
PID_ErrorHandle = 0b10000000, // 1000 0000
|
||||
} PID_Improvement_e;
|
||||
|
||||
/* PID 报错类型枚举*/
|
||||
typedef enum errorType_e
|
||||
{
|
||||
PID_ERROR_NONE = 0x00U,
|
||||
PID_MOTOR_BLOCKED_ERROR = 0x01U
|
||||
} ErrorType_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint64_t ERRORCount;
|
||||
ErrorType_e ERRORType;
|
||||
} PID_ErrorHandler_t;
|
||||
|
||||
/* PID结构体 */
|
||||
typedef struct
|
||||
{
|
||||
//---------------------------------- init config block
|
||||
// config parameter
|
||||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
float MaxOut;
|
||||
float DeadBand;
|
||||
|
||||
// improve parameter
|
||||
PID_Improvement_e Improve;
|
||||
float IntegralLimit; // 积分限幅
|
||||
float CoefA; // 变速积分 For Changing Integral
|
||||
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
|
||||
float Derivative_LPF_RC; // 微分滤波器系数
|
||||
|
||||
//-----------------------------------
|
||||
// for calculating
|
||||
float Measure;
|
||||
float Last_Measure;
|
||||
float Err;
|
||||
float Last_Err;
|
||||
float Last_ITerm;
|
||||
|
||||
float Pout;
|
||||
float Iout;
|
||||
float Dout;
|
||||
float ITerm;
|
||||
|
||||
float Output;
|
||||
float Last_Output;
|
||||
float Last_Dout;
|
||||
|
||||
float Ref;
|
||||
|
||||
float dt;
|
||||
|
||||
PID_ErrorHandler_t ERRORHandler;
|
||||
} PIDInstance;
|
||||
|
||||
/* 用于PID初始化的结构体*/
|
||||
typedef struct // config parameter
|
||||
{
|
||||
// basic parameter
|
||||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
float MaxOut; // 输出限幅
|
||||
float DeadBand; // 死区
|
||||
float T;
|
||||
|
||||
// improve parameter
|
||||
PID_Improvement_e Improve;
|
||||
float IntegralLimit; // 积分限幅
|
||||
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
|
||||
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float Output_LPF_RC; // RC = 1/omegac
|
||||
float Derivative_LPF_RC;
|
||||
} PID_Init_Config_s;
|
||||
|
||||
/**
|
||||
* @brief 初始化PID实例
|
||||
* @todo 待修改为统一的PIDRegister风格
|
||||
* @param pid PID实例指针
|
||||
* @param config PID初始化配置
|
||||
*/
|
||||
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
|
||||
|
||||
/**
|
||||
* @brief 计算PID输出
|
||||
*
|
||||
* @param pid PID实例指针
|
||||
* @param measure 反馈值
|
||||
* @param ref 设定值
|
||||
* @return float PID计算输出
|
||||
*/
|
||||
float PIDCalculate(PIDInstance *pid, float measure, float ref);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,158 @@
|
||||
#include "Task.h"
|
||||
#include "zf_driver_pit.h"
|
||||
#include "zf_device_key.h"
|
||||
#include "zf_common_interrupt.h"
|
||||
#include "IMU.h"
|
||||
#include "Beep.h"
|
||||
#include "GrayscaleSensor.h"
|
||||
#include "Motor.h"
|
||||
#include "MotorEncoder.h"
|
||||
#include "PID_Controller.h"
|
||||
#include "Command.h"
|
||||
|
||||
|
||||
volatile uint8_t motor_control_task_enable = 0;
|
||||
volatile uint8_t angle_control_task_enable = 0;
|
||||
volatile uint8_t position_control_task_enable = 0;
|
||||
|
||||
/*速度环的输入，开启角度环后由角度环接管(范围-50到+50)*/
|
||||
volatile int16_t motor_speed_right_ref = 0;
|
||||
volatile int16_t motor_speed_left_ref = 0;
|
||||
/*角度环接管速度之后的基础直行速度*/
|
||||
volatile int16_t motor_speed_base = 0;
|
||||
/*角度环输入，开启位置环后由位置环接管*/
|
||||
volatile float yaw_ref = 0;
|
||||
/*位置环的输出*/
|
||||
volatile float angle_velocity = 0;
|
||||
|
||||
volatile PIDInstance pid_speed_right={0};
|
||||
volatile PIDInstance pid_speed_left={0};
|
||||
volatile PIDInstance pid_yaw = {0};
|
||||
volatile PIDInstance pid_position = {0};
|
||||
|
||||
|
||||
void motor_control_init(float T);
|
||||
void angle_control_init(float T);
|
||||
void position_control_init(float T);
|
||||
void angle_control_task(void);
|
||||
void position_control_task(void);
|
||||
void motor_control_task(void);
|
||||
void Task_10ms(uint32 state, void *ptr)
|
||||
{
|
||||
// 10ms 执行一次的任务代码(100Hz)
|
||||
motor_encoder_update();
|
||||
if(motor_control_task_enable)
|
||||
motor_control_task();
|
||||
|
||||
}
|
||||
void Task_20ms(uint32 state, void *ptr)
|
||||
{
|
||||
// 20ms 执行一次的任务代码(50Hz)
|
||||
grayscale_sensor_scan();
|
||||
if(position_control_task_enable)
|
||||
position_control_task();
|
||||
|
||||
imu_update();
|
||||
if(angle_control_task_enable)
|
||||
angle_control_task();
|
||||
}
|
||||
void Task_50ms(uint32 state, void *ptr)
|
||||
{
|
||||
// 50ms 执行一次的任务代码(20Hz)
|
||||
key_scanner();
|
||||
beep_actuator();
|
||||
}
|
||||
void task_init(void)
|
||||
{
|
||||
motor_control_init(0.01f);
|
||||
angle_control_init(0.02f);
|
||||
position_control_init(0.02f);
|
||||
|
||||
pit_ms_init(PIT_TIM_A0,10,Task_10ms,NULL);
|
||||
interrupt_set_priority(TIMA0_INT_IRQn,1);
|
||||
pit_ms_init(PIT_TIM_A1,20,Task_20ms,NULL);
|
||||
interrupt_set_priority(TIMA1_INT_IRQn,1);
|
||||
pit_ms_init(PIT_TIM_G6,50,Task_50ms,NULL);
|
||||
interrupt_set_priority(TIMG6_INT_IRQn,7);
|
||||
}
|
||||
|
||||
void motor_control_init(float T)
|
||||
{
|
||||
PID_Init_Config_s config_right = {
|
||||
.Kp = 300.0f,
|
||||
.Ki = 150.0f,
|
||||
.Kd = 0.0f,
|
||||
.T = T,
|
||||
.MaxOut = MOTOR_MAX
|
||||
};
|
||||
PIDInit((PIDInstance *)&pid_speed_right, &config_right);
|
||||
PID_Init_Config_s config_left = {
|
||||
.Kp = 300.0f,
|
||||
.Ki = 150.0f,
|
||||
.Kd = 0.0f,
|
||||
.T = T,
|
||||
.MaxOut = MOTOR_MAX
|
||||
};
|
||||
PIDInit((PIDInstance *)&pid_speed_left, &config_left);
|
||||
}
|
||||
void motor_control_task(void)
|
||||
{
|
||||
float pwm_right = PIDCalculate((PIDInstance *)&pid_speed_right,motor_speed_right.value,motor_speed_right_ref);
|
||||
float pwm_left = PIDCalculate((PIDInstance *)&pid_speed_left,motor_speed_left.value,motor_speed_left_ref);
|
||||
|
||||
motor_set_pwm_right(pwm_right);
|
||||
motor_set_pwm_left(pwm_left);
|
||||
}
|
||||
void angle_control_init(float T)
|
||||
{
|
||||
PID_Init_Config_s config_yaw = {
|
||||
.Kp = 1.0f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.0f,
|
||||
.T = T,
|
||||
.MaxOut = 25,
|
||||
.Improve = PID_Yaw_Error_Corretion
|
||||
};
|
||||
PIDInit((PIDInstance *)&pid_yaw, &config_yaw);
|
||||
}
|
||||
void angle_control_task(void)
|
||||
{
|
||||
float speed_dif = PIDCalculate((PIDInstance *)&pid_yaw,imu_angle.yaw,yaw_ref);
|
||||
motor_speed_left_ref = motor_speed_base - speed_dif;
|
||||
motor_speed_right_ref = motor_speed_base + speed_dif;
|
||||
}
|
||||
void position_control_init(float T)
|
||||
{
|
||||
PID_Init_Config_s config_position = {
|
||||
.Kp = 0.5f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.0f,
|
||||
.T = T,
|
||||
.MaxOut = 90,
|
||||
};
|
||||
PIDInit((PIDInstance *)&pid_position, &config_position);
|
||||
}
|
||||
void position_control_task(void)
|
||||
{
|
||||
//角度速率（单位度/秒）
|
||||
angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
|
||||
|
||||
//发送给云台，角度速率前馈
|
||||
command_send_angle_velocity((float*)&angle_velocity);
|
||||
|
||||
set_yaw_ref(yaw_ref + angle_velocity*pid_position.dt);
|
||||
}
|
||||
void set_yaw_ref(float set_value)
|
||||
{
|
||||
if(set_value>180)
|
||||
{
|
||||
while(set_value > 180)
|
||||
set_value -= 360;
|
||||
}
|
||||
else if(set_value<-180)
|
||||
{
|
||||
while(set_value < -180)
|
||||
set_value += 360;
|
||||
}
|
||||
yaw_ref = set_value;
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#ifndef __TASK_H
|
||||
#define __TASK_H
|
||||
|
||||
#include "zf_common_typedef.h"
|
||||
|
||||
void task_init(void);
|
||||
|
||||
//建议不直接修改此变量，而是通过set_yaw_ref(float set_value)设置，以防预期之外的值
|
||||
extern volatile float yaw_ref;
|
||||
void set_yaw_ref(float set_value);
|
||||
|
||||
//供外部控制使用
|
||||
extern volatile uint8_t motor_control_task_enable;
|
||||
extern volatile uint8_t angle_control_task_enable;
|
||||
extern volatile uint8_t position_control_task_enable;
|
||||
|
||||
|
||||
//配置项
|
||||
/*自动模式下的基础直行速度*/
|
||||
#define AUTO_MODE_BASE_SPEED 10
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user