chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)
- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
This commit is contained in:
@@ -0,0 +1,305 @@
|
||||
/* =============================================================================
|
||||
* bmi088.c - C 板板载六轴 IMU BMI088 驱动(HAL 版,架构与 legacy 一致)
|
||||
* 初始化(bmi088_init,任务上下文):
|
||||
* 1. 先关 EXTI4/EXTI9_5 NVIC(数据就绪中断若在初始化期间触发 DMA 读,
|
||||
* 会把 SPI 永久占在 BUSY,导致读 ID 全部失败--疯转/绿灯闪的根因)
|
||||
* 2. 重配 SPI1 为模式3(CPOL=1,CPHA=2边沿)/16 分频(5.25MHz,
|
||||
* CubeMX 生成的是模式0/2分频 42MHz,BMI088 不支持)
|
||||
* 3. 有界重试读 ID(acc=0x1E, gyro=0x0F),成功后按 legacy 序列配置寄存器
|
||||
* 4. 清 EXTI 挂起标志后放行中断
|
||||
* 运行: EXTI 下降沿 -> start_dma_read() SPI DMA 读取 -> TxRx 完成回调解析
|
||||
* (加速度 8 字节 -> 链式读温度 4 字节;陀螺 7 字节),量程 ±3g/±2000dps
|
||||
* ========================================================================== */
|
||||
#include "bmi088.h"
|
||||
#include "main.h"
|
||||
#include "spi.h"
|
||||
#include "led.h"
|
||||
|
||||
/* ---- 寄存器(与 legacy 一致) ---- */
|
||||
#define BMI088_ACC_CHIP_ID 0x00
|
||||
#define BMI088_ACC_PWR_CTRL 0x7D
|
||||
#define BMI088_ACC_PWR_CONF 0x7C
|
||||
#define BMI088_ACC_CONF 0x40
|
||||
#define BMI088_ACC_RANGE 0x41
|
||||
#define BMI088_INT1_IO_CONF 0x53
|
||||
#define BMI088_INT1_INT2_MAP_DATA 0x58
|
||||
#define BMI088_ACC_SOFTRESET 0x7E
|
||||
#define BMI088_ACC_DATA_START 0x12
|
||||
#define BMI088_ACC_TEMP_MSB 0x22
|
||||
|
||||
#define BMI088_GYRO_CHIP_ID 0x00
|
||||
#define BMI088_GYRO_RANGE 0x0F
|
||||
#define BMI088_GYRO_BANDWIDTH 0x10
|
||||
#define BMI088_GYRO_LPM1 0x11
|
||||
#define BMI088_GYRO_INT_CTRL 0x15
|
||||
#define BMI088_INT3_INT4_IO_CONF 0x16
|
||||
#define BMI088_INT3_INT4_IO_MAP 0x18
|
||||
#define BMI088_GYRO_SOFTRESET 0x14
|
||||
#define BMI088_GYRO_DATA_START 0x02
|
||||
|
||||
#define BMI088_ACC_ID_VALUE 0x1E
|
||||
#define BMI088_GYRO_ID_VALUE 0x0F
|
||||
|
||||
#define BMI088_CS_ACC(x) HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)
|
||||
#define BMI088_CS_GYRO(x) HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)
|
||||
|
||||
float bmi088_accel[3];
|
||||
float bmi088_gyro[3];
|
||||
float bmi088_temperature;
|
||||
uint8_t bmi088_online = 0;
|
||||
uint8_t bmi088_last_acc_id = 0;
|
||||
uint8_t bmi088_last_gyro_id = 0;
|
||||
|
||||
static uint8_t spi_tx_buf[8];
|
||||
static uint8_t spi_rx_buf[8];
|
||||
|
||||
/* 读数阶段(当前 SPI DMA 正在搬运的数据类型) */
|
||||
typedef enum
|
||||
{
|
||||
BMI088_PHASE_NONE = 0,
|
||||
BMI088_PHASE_ACCEL,
|
||||
BMI088_PHASE_TEMP,
|
||||
BMI088_PHASE_GYRO,
|
||||
} bmi088_phase_e;
|
||||
|
||||
static volatile bmi088_phase_e read_phase = BMI088_PHASE_NONE;
|
||||
|
||||
extern SPI_HandleTypeDef hspi1;
|
||||
|
||||
/* ---- 阻塞式寄存器读写 ---- */
|
||||
|
||||
static void accel_write_reg(uint8_t reg, uint8_t data)
|
||||
{
|
||||
uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
|
||||
BMI088_CS_ACC(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
|
||||
BMI088_CS_ACC(1);
|
||||
}
|
||||
|
||||
static uint8_t accel_read_reg(uint8_t reg)
|
||||
{
|
||||
uint8_t tx[3] = {(uint8_t)(reg | 0x80), 0x00, 0x00}; // 加速度计首字节回读为乱码
|
||||
uint8_t rx[3];
|
||||
BMI088_CS_ACC(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 3, 10);
|
||||
BMI088_CS_ACC(1);
|
||||
return rx[2];
|
||||
}
|
||||
|
||||
static void gyro_write_reg(uint8_t reg, uint8_t data)
|
||||
{
|
||||
uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
|
||||
BMI088_CS_GYRO(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
|
||||
BMI088_CS_GYRO(1);
|
||||
}
|
||||
|
||||
static uint8_t gyro_read_reg(uint8_t reg)
|
||||
{
|
||||
uint8_t tx[2] = {(uint8_t)(reg | 0x80), 0x00};
|
||||
uint8_t rx[2];
|
||||
BMI088_CS_GYRO(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 10);
|
||||
BMI088_CS_GYRO(1);
|
||||
return rx[1];
|
||||
}
|
||||
|
||||
/* ---- 数据解析(换算系数与 legacy 一致) ---- */
|
||||
|
||||
static void process_accel(void)
|
||||
{
|
||||
int16_t raw[3];
|
||||
raw[0] = (int16_t)(((uint16_t)spi_rx_buf[3] << 8) | spi_rx_buf[2]);
|
||||
raw[1] = (int16_t)(((uint16_t)spi_rx_buf[5] << 8) | spi_rx_buf[4]);
|
||||
raw[2] = (int16_t)(((uint16_t)spi_rx_buf[7] << 8) | spi_rx_buf[6]);
|
||||
bmi088_accel[0] = raw[0] * 0.0008974358974f; // (raw/32768*3)*9.8
|
||||
bmi088_accel[1] = raw[1] * 0.0008974358974f;
|
||||
bmi088_accel[2] = raw[2] * 0.0008974358974f;
|
||||
}
|
||||
|
||||
static void process_temperature(void)
|
||||
{
|
||||
uint16_t raw = ((uint16_t)spi_rx_buf[2] << 3) | (spi_rx_buf[3] >> 5); // 11bits
|
||||
int16_t temp = (raw > 1023) ? (int16_t)(raw - 2048) : (int16_t)raw;
|
||||
bmi088_temperature = temp * 0.125f + 23.0f;
|
||||
}
|
||||
|
||||
static void process_gyro(void)
|
||||
{
|
||||
int16_t raw[3];
|
||||
raw[0] = (int16_t)(((uint16_t)spi_rx_buf[2] << 8) | spi_rx_buf[1]);
|
||||
raw[1] = (int16_t)(((uint16_t)spi_rx_buf[4] << 8) | spi_rx_buf[3]);
|
||||
raw[2] = (int16_t)(((uint16_t)spi_rx_buf[6] << 8) | spi_rx_buf[5]);
|
||||
bmi088_gyro[0] = raw[0] * 0.0010652644360316952f; // (raw/32768*2000)/180*π
|
||||
bmi088_gyro[1] = raw[1] * 0.0010652644360316952f;
|
||||
bmi088_gyro[2] = raw[2] * 0.0010652644360316952f;
|
||||
}
|
||||
|
||||
/* 启动一次 SPI DMA 读(若 SPI 忙则丢弃本次,等下一个数据就绪沿) */
|
||||
static void start_dma_read(bmi088_phase_e phase, uint8_t reg, uint8_t len)
|
||||
{
|
||||
if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
|
||||
return;
|
||||
read_phase = phase;
|
||||
spi_tx_buf[0] = (uint8_t)(reg | 0x80);
|
||||
for (uint8_t i = 1; i < len; i++)
|
||||
spi_tx_buf[i] = 0x00;
|
||||
|
||||
if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
|
||||
BMI088_CS_ACC(0);
|
||||
else
|
||||
BMI088_CS_GYRO(0);
|
||||
|
||||
HAL_SPI_TransmitReceive_DMA(&hspi1, spi_tx_buf, spi_rx_buf, len);
|
||||
}
|
||||
|
||||
/* ---- 初始化 ---- */
|
||||
|
||||
static void error_blink(void)
|
||||
{
|
||||
/* BMI088 连接异常报警:绿灯连闪两下(经 TIM5 PWM,引脚在复用模式下
|
||||
* 直接写 GPIO 寄存器无效,必须走 LED 模块) */
|
||||
static uint8_t counter = 0;
|
||||
counter++;
|
||||
if (counter == 1)
|
||||
led_set_green(1);
|
||||
else if (counter == 5)
|
||||
led_set_green(0);
|
||||
else if (counter == 13)
|
||||
led_set_green(1);
|
||||
else if (counter == 17)
|
||||
led_set_green(0);
|
||||
else if (counter == 40)
|
||||
counter = 0;
|
||||
HAL_Delay(25);
|
||||
}
|
||||
|
||||
void bmi088_init(void)
|
||||
{
|
||||
/* 关键:数据就绪中断(EXTI)在芯片上电后就会持续触发,若不屏蔽,
|
||||
* EXTI 回调里的 SPI DMA 读会把 SPI 永远占在 BUSY,初始化的阻塞式
|
||||
* 读 ID 全部失败(legacy 靠初始化期间关闭同款 NVIC 规避,此处一致) */
|
||||
HAL_NVIC_DisableIRQ(EXTI4_IRQn);
|
||||
HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);
|
||||
|
||||
/* CubeMX 生成的 SPI1 为模式0/2分频(42MHz),BMI088 需要模式3 且 <=10MHz,
|
||||
* 此处重新配置为模式3/16分频(5.25MHz,与 legacy 相同) */
|
||||
HAL_SPI_DeInit(&hspi1);
|
||||
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
|
||||
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
|
||||
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
|
||||
if (HAL_SPI_Init(&hspi1) != HAL_OK)
|
||||
return; // 放弃本次初始化,保持系统可运行
|
||||
|
||||
BMI088_CS_ACC(1);
|
||||
BMI088_CS_GYRO(1);
|
||||
|
||||
/* 检测 ID 并配置寄存器(与 legacy 序列一致)
|
||||
* 有界重试:最多 40 次(约 1s),失败则放弃并保持系统可运行 */
|
||||
for (uint8_t retry = 0; retry < 40; retry++)
|
||||
{
|
||||
bmi088_last_acc_id = accel_read_reg(BMI088_ACC_CHIP_ID);
|
||||
bmi088_last_gyro_id = gyro_read_reg(BMI088_GYRO_CHIP_ID);
|
||||
if (bmi088_last_acc_id == BMI088_ACC_ID_VALUE &&
|
||||
bmi088_last_gyro_id == BMI088_GYRO_ID_VALUE)
|
||||
{
|
||||
bmi088_online = 1;
|
||||
accel_write_reg(BMI088_ACC_SOFTRESET, 0xB6);
|
||||
HAL_Delay(1); // 加速度计软重启 1ms
|
||||
gyro_write_reg(BMI088_GYRO_SOFTRESET, 0xB6);
|
||||
HAL_Delay(30); // 陀螺仪软重启 30ms
|
||||
|
||||
accel_read_reg(BMI088_ACC_CHIP_ID); // 软重启后首笔读写有 bug,丢弃
|
||||
gyro_read_reg(BMI088_GYRO_CHIP_ID);
|
||||
|
||||
accel_write_reg(BMI088_ACC_PWR_CTRL, 0x04);
|
||||
HAL_Delay(1); // 上电等待(legacy 300us,放宽到 1ms)
|
||||
accel_write_reg(BMI088_ACC_PWR_CONF, 0x00);
|
||||
HAL_Delay(1);
|
||||
accel_write_reg(BMI088_ACC_CONF, 0xAB); // 800Hz ODR,正常低通
|
||||
HAL_Delay(1);
|
||||
accel_write_reg(BMI088_ACC_RANGE, 0x00); // ±3g
|
||||
HAL_Delay(1);
|
||||
accel_write_reg(BMI088_INT1_IO_CONF, 0x08); // INT1 推挽,低有效
|
||||
HAL_Delay(1);
|
||||
accel_write_reg(BMI088_INT1_INT2_MAP_DATA, 0x04); // INT1=数据就绪
|
||||
HAL_Delay(1);
|
||||
|
||||
gyro_write_reg(BMI088_GYRO_RANGE, 0x00); // ±2000°/s
|
||||
HAL_Delay(1);
|
||||
gyro_write_reg(BMI088_GYRO_BANDWIDTH, 0x82); // 116Hz 带宽,1kHz ODR
|
||||
HAL_Delay(1);
|
||||
gyro_write_reg(BMI088_GYRO_LPM1, 0x00); // 正常模式
|
||||
HAL_Delay(1);
|
||||
gyro_write_reg(BMI088_GYRO_INT_CTRL, 0x80); // 使能数据就绪中断
|
||||
HAL_Delay(1);
|
||||
gyro_write_reg(BMI088_INT3_INT4_IO_CONF, 0x00); // INT3 推挽,低有效
|
||||
HAL_Delay(1);
|
||||
gyro_write_reg(BMI088_INT3_INT4_IO_MAP, 0x01); // INT3=数据就绪
|
||||
HAL_Delay(1);
|
||||
break;
|
||||
}
|
||||
error_blink();
|
||||
}
|
||||
|
||||
/* 配置成功后清除挂起的数据就绪标志,再放行 EXTI 中断 */
|
||||
if (bmi088_online)
|
||||
{
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);
|
||||
HAL_NVIC_EnableIRQ(EXTI4_IRQn);
|
||||
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- 中断回调 ---- */
|
||||
|
||||
/* EXTI4(加速度计数据就绪)/ EXTI5(陀螺仪数据就绪) */
|
||||
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
|
||||
{
|
||||
if (!bmi088_online)
|
||||
return; // 初始化完成前不启动 DMA 读,避免占用 SPI
|
||||
|
||||
if (GPIO_Pin == INT1_ACCEL_Pin)
|
||||
{
|
||||
if (read_phase == BMI088_PHASE_NONE)
|
||||
start_dma_read(BMI088_PHASE_ACCEL, BMI088_ACC_DATA_START, 8);
|
||||
}
|
||||
else if (GPIO_Pin == INT1_GYRO_Pin)
|
||||
{
|
||||
if (read_phase == BMI088_PHASE_NONE)
|
||||
start_dma_read(BMI088_PHASE_GYRO, BMI088_GYRO_DATA_START, 7);
|
||||
}
|
||||
}
|
||||
|
||||
/* SPI 收发完成 */
|
||||
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
|
||||
{
|
||||
if (hspi->Instance != SPI1)
|
||||
return;
|
||||
|
||||
switch (read_phase)
|
||||
{
|
||||
case BMI088_PHASE_ACCEL:
|
||||
BMI088_CS_ACC(1);
|
||||
process_accel();
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
start_dma_read(BMI088_PHASE_TEMP, BMI088_ACC_TEMP_MSB, 4); // 链式读取温度
|
||||
break;
|
||||
|
||||
case BMI088_PHASE_TEMP:
|
||||
BMI088_CS_ACC(1);
|
||||
process_temperature();
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
break;
|
||||
|
||||
case BMI088_PHASE_GYRO:
|
||||
BMI088_CS_GYRO(1);
|
||||
process_gyro();
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
#ifndef BMI088_H
|
||||
#define BMI088_H
|
||||
|
||||
/* C 板板载六轴 IMU BMI088 驱动(HAL 版,架构与 legacy 一致)
|
||||
* SPI1 模式3 + DMA2(Stream2 RX/Stream3 TX),INT1_Accel=PC4(EXTI4)、INT1_Gyro=PC5(EXTI5)
|
||||
* 数据就绪下降沿触发 SPI DMA 读取,在完成回调中解析
|
||||
* 量程:加速度 ±3g,角速度 ±2000°/s,陀螺 1kHz 输出 */
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
extern float bmi088_accel[3]; // (m/s^2)
|
||||
extern float bmi088_gyro[3]; // (rad/s)
|
||||
extern float bmi088_temperature; // (℃)
|
||||
extern uint8_t bmi088_online; // 1-初始化成功 0-检测失败(有界重试后放弃,系统继续运行)
|
||||
extern uint8_t bmi088_last_acc_id; // 最近一次读到的加速度计 ID(诊断用,正常 0x1E)
|
||||
extern uint8_t bmi088_last_gyro_id; // 最近一次读到的陀螺仪 ID(诊断用,正常 0x0F)
|
||||
|
||||
/* 初始化(任务上下文调用,含阻塞式寄存器配置与 ID 检测循环;失败时绿灯报警并重试) */
|
||||
extern void bmi088_init(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,44 @@
|
||||
/* =============================================================================
|
||||
* buzzer.c - C 板无源蜂鸣器(TIM4_CH3, PD14)
|
||||
* 说明: 音调表与 legacy 完全一致(ARR 固定 10 分频,改 PSC 变调,
|
||||
* 装载用 HAL_TIM_GenerateEvent 立即生效);buzzer_init 必须在
|
||||
* CAN_Task 中调用一次启动 PWM 输出,否则所有报警/音效无声
|
||||
* ========================================================================== */
|
||||
#include "buzzer.h"
|
||||
#include "tim.h"
|
||||
|
||||
extern TIM_HandleTypeDef htim4;
|
||||
|
||||
/* 蜂鸣器音调频率表(ARR 固定 10 分频下的 PSC 值,与 legacy 一致) */
|
||||
static const uint16_t buzzer_tone_freq[37] = {
|
||||
0,
|
||||
32107, 30305, 28604, 26999, 25483, 24053, 22703, 21429, 20226, 19091, 18019, 17008,
|
||||
16053, 15152, 14302, 13499, 12742, 12026, 11352, 10714, 10113, 9545, 9010, 8504,
|
||||
8027, 7576, 7151, 6750, 6371, 6013, 5676, 5357, 5056, 4773, 4505, 4252,
|
||||
};
|
||||
|
||||
void buzzer_init(void)
|
||||
{
|
||||
/* 与 legacy 相同:ARR=10 分频,CubeMX 的 ARR=65535 改为 9 */
|
||||
__HAL_TIM_SET_AUTORELOAD(&htim4, 10 - 1);
|
||||
__HAL_TIM_SET_PRESCALER(&htim4, 19091);
|
||||
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
|
||||
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
|
||||
}
|
||||
|
||||
void buzzer_on(buzzer_tone_e tone)
|
||||
{
|
||||
if (tone != BUZZER_P)
|
||||
{
|
||||
__HAL_TIM_SET_PRESCALER(&htim4, buzzer_tone_freq[tone]);
|
||||
HAL_TIM_GenerateEvent(&htim4, TIM_EVENTSOURCE_UPDATE); // 立即装载 PSC
|
||||
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 5); // 占空比 50%
|
||||
}
|
||||
else
|
||||
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
|
||||
}
|
||||
|
||||
void buzzer_off(void)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef BUZZER_H
|
||||
#define BUZZER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* C 板无源蜂鸣器(TIM4_CH3, PD14),音调表与 legacy 完全一致 */
|
||||
|
||||
typedef enum
|
||||
{
|
||||
BUZZER_P = 0, // 空拍
|
||||
BUZZER_L1, BUZZER_L1_, BUZZER_L2, BUZZER_L2_, BUZZER_L3, BUZZER_L4, BUZZER_L4_, BUZZER_L5, BUZZER_L5_, BUZZER_L6, BUZZER_L6_, BUZZER_L7, // 低音
|
||||
BUZZER_M1, BUZZER_M1_, BUZZER_M2, BUZZER_M2_, BUZZER_M3, BUZZER_M4, BUZZER_M4_, BUZZER_M5, BUZZER_M5_, BUZZER_M6, BUZZER_M6_, BUZZER_M7, // 中音
|
||||
BUZZER_H1, BUZZER_H1_, BUZZER_H2, BUZZER_H2_, BUZZER_H3, BUZZER_H4, BUZZER_H4_, BUZZER_H5, BUZZER_H5_, BUZZER_H6, BUZZER_H6_, BUZZER_H7, // 高音
|
||||
} buzzer_tone_e;
|
||||
|
||||
extern void buzzer_init(void);
|
||||
extern void buzzer_on(buzzer_tone_e tone);
|
||||
extern void buzzer_off(void);
|
||||
#endif
|
||||
@@ -0,0 +1,79 @@
|
||||
/* =============================================================================
|
||||
* dr16.c - DT7 遥控器 + 图传键鼠数据解析
|
||||
* 输入: usart3 整帧回调(18 字节,100000-8E1,每 14ms 一帧) --> dr16_parse()
|
||||
* 输出: dr16 结构体(摇杆 364~1684 中值 1024 / 拨杆 1上2下3中 / 键鼠 /
|
||||
* 按键翻转边沿 key_push_*)/ dr16_status / dr16_start_flag
|
||||
* 边沿语义(与 legacy 一致): Q/E/Shift/Ctrl 按下瞬间翻转保持;鼠标右键按下瞬间置1
|
||||
* dr16_start_flag: 1-未连接 2-收到首帧(robot_cmd 借此触发云台角度复位)
|
||||
* 掉线: dr16_offline() = 超过 DR16_OFFLINE_TIME_MS(25ms) 无帧
|
||||
* ========================================================================== */
|
||||
#include "dr16.h"
|
||||
|
||||
dr16_t dr16;
|
||||
uint8_t dr16_status = 0;
|
||||
uint8_t dr16_start_flag = 1;
|
||||
uint32_t dr16_last_tick = 0;
|
||||
uint32_t dr16_frame_count = 0;
|
||||
|
||||
void dr16_parse(const uint8_t * data)
|
||||
{
|
||||
uint8_t last_key_q = dr16.key_q;
|
||||
uint8_t last_key_e = dr16.key_e;
|
||||
uint8_t last_key_shift = dr16.key_shift;
|
||||
uint8_t last_key_ctrl = dr16.key_ctrl;
|
||||
uint8_t last_mouse_r = dr16.mouse_key_r;
|
||||
|
||||
dr16.rc_r_RL = (((uint16_t)data[1] << 8) | data[0]) & 0x07FF; // B[0:10]
|
||||
dr16.rc_r_UD = (((uint16_t)data[2] << 5) | (data[1] >> 3)) & 0x07FF; // B[11:21]
|
||||
dr16.rc_l_RL = (((uint16_t)data[4] << 10) | (((uint16_t)data[3] << 2) | (data[2] >> 6))) & 0x07FF; // B[22:32]
|
||||
dr16.rc_l_UD = (((uint16_t)data[5] << 7) | (data[4] >> 1)) & 0x07FF; // B[33:43]
|
||||
|
||||
dr16.s2 = (data[5] >> 4) & 0x03; // B[44:45]
|
||||
dr16.s1 = (data[5] >> 6) & 0x03; // B[46:47]
|
||||
|
||||
dr16.mouse_rl = (int16_t)(((uint16_t)data[7] << 8) | data[6]); // B[48:63]
|
||||
dr16.mouse_du = (int16_t)(((uint16_t)data[9] << 8) | data[8]); // B[64:79]
|
||||
dr16.mouse_wheel = (int16_t)(((uint16_t)data[11] << 8) | data[10]); // B[80:95]
|
||||
dr16.mouse_key_l = data[12];
|
||||
dr16.mouse_key_r = data[13];
|
||||
|
||||
dr16.key_w = data[14] & 0x01;
|
||||
dr16.key_s = (data[14] >> 1) & 0x01;
|
||||
dr16.key_a = (data[14] >> 2) & 0x01;
|
||||
dr16.key_d = (data[14] >> 3) & 0x01;
|
||||
dr16.key_shift = (data[14] >> 4) & 0x01;
|
||||
dr16.key_ctrl = (data[14] >> 5) & 0x01;
|
||||
dr16.key_q = (data[14] >> 6) & 0x01;
|
||||
dr16.key_e = (data[14] >> 7) & 0x01;
|
||||
dr16.key_alpha1 = (data[14] >> 9) & 0x01;
|
||||
dr16.key_alpha2 = (data[14] >> 10) & 0x01;
|
||||
|
||||
dr16.thumb_wheel = (int16_t)(((uint16_t)data[17] << 8) | data[16]); // B[120:135]
|
||||
|
||||
/* 按键边沿检测(legacy 语义:Q/E/Shift/Ctrl 按下翻转,鼠标右键按下瞬间置 1) */
|
||||
if (last_key_q == 0 && dr16.key_q == 1)
|
||||
dr16.key_push_q = !dr16.key_push_q;
|
||||
if (last_key_e == 0 && dr16.key_e == 1)
|
||||
dr16.key_push_e = !dr16.key_push_e;
|
||||
if (last_key_shift == 0 && dr16.key_shift == 1)
|
||||
dr16.key_push_shift = !dr16.key_push_shift;
|
||||
if (last_key_ctrl == 0 && dr16.key_ctrl == 1)
|
||||
dr16.key_push_ctrl = !dr16.key_push_ctrl;
|
||||
if (last_mouse_r == 0 && dr16.mouse_key_r == 1)
|
||||
dr16.mouse_key_push_r = 1;
|
||||
else
|
||||
dr16.mouse_key_push_r = 0;
|
||||
|
||||
dr16_last_tick = HAL_GetTick();
|
||||
dr16_status = 1;
|
||||
dr16_frame_count++;
|
||||
|
||||
/* 首次收到数据:置启动标志,触发云台角度复位(与 legacy Remote_StartFlag 语义一致) */
|
||||
if (dr16_start_flag == 1)
|
||||
dr16_start_flag = 2;
|
||||
}
|
||||
|
||||
uint8_t dr16_offline(void)
|
||||
{
|
||||
return (dr16_status == 0 || HAL_GetTick() - dr16_last_tick > DR16_OFFLINE_TIME_MS);
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
#ifndef DR16_H
|
||||
#define DR16_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "stm32f4xx_hal.h"
|
||||
|
||||
#define DR16_OFFLINE_TIME_MS 25 // 遥控器掉线判定时间(legacy 25ms)
|
||||
|
||||
/* DT7 遥控器 + 图传键鼠数据结构,自 legacy Remote 模块移植
|
||||
* S1/S2 拨杆:1-上 2-下 3-中(与 legacy 解析保持一致)
|
||||
* 摇杆:364~1684,中值 1024 */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t rc_r_RL; // 通道0-右摇杆左右
|
||||
uint16_t rc_r_UD; // 通道1-右摇杆上下
|
||||
uint16_t rc_l_RL; // 通道2-左摇杆左右
|
||||
uint16_t rc_l_UD; // 通道3-左摇杆上下
|
||||
|
||||
uint8_t s1; // 左侧拨杆
|
||||
uint8_t s2; // 右侧拨杆
|
||||
|
||||
int16_t mouse_rl; // 鼠标X轴速度
|
||||
int16_t mouse_du; // 鼠标Y轴速度
|
||||
int16_t mouse_wheel; // 鼠标滚轮
|
||||
uint8_t mouse_key_l;
|
||||
uint8_t mouse_key_r;
|
||||
|
||||
uint8_t key_w;
|
||||
uint8_t key_s;
|
||||
uint8_t key_a;
|
||||
uint8_t key_d;
|
||||
uint8_t key_shift;
|
||||
uint8_t key_ctrl;
|
||||
uint8_t key_q;
|
||||
uint8_t key_e;
|
||||
uint8_t key_alpha1; // 图传扩展键
|
||||
uint8_t key_alpha2;
|
||||
|
||||
int16_t thumb_wheel; // 遥控器拨轮
|
||||
|
||||
/* 按键边沿检测(legacy 语义) */
|
||||
uint8_t key_push_q; // Q 键按下翻转
|
||||
uint8_t key_push_e; // E 键按下翻转
|
||||
uint8_t key_push_shift; // Shift 按下翻转
|
||||
uint8_t key_push_ctrl; // Ctrl 按下翻转
|
||||
uint8_t mouse_key_push_r; // 鼠标右键按下瞬间为 1
|
||||
} dr16_t;
|
||||
|
||||
extern dr16_t dr16;
|
||||
extern uint8_t dr16_status; // 1-已连接 0-掉线
|
||||
extern uint8_t dr16_start_flag; // 1-准备启动 2-收到首帧(与 legacy Remote_StartFlag 一致)
|
||||
|
||||
/* 解析一帧 18 字节原始数据(usart3 接收回调/中断上下文调用) */
|
||||
extern void dr16_parse(const uint8_t * data);
|
||||
|
||||
/* 最近收帧 tick(HAL_GetTick),用于掉线检测 */
|
||||
extern uint32_t dr16_last_tick;
|
||||
extern uint32_t dr16_frame_count; // 收到的帧计数(诊断用)
|
||||
|
||||
/* 判定遥控器是否掉线 */
|
||||
extern uint8_t dr16_offline(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,124 @@
|
||||
/* =============================================================================
|
||||
* ist8310.c - C 板板载磁力计 IST8310 —— 硬件 I2C3 版
|
||||
* 总线: I2C3(PA8=SCL, PC9=SDA, CubeMX 已配置 100kHz 标准模式),
|
||||
* 替代 legacy 的软件模拟 I2C 与早期移植版的 bsp_i2c_soft
|
||||
* 初始化: 必须先把 RSTN(PG6) 拉高再检测——CubeMX 默认输出低电平会把芯片
|
||||
* 按在复位态,WHO_AM_I(0x10) 永远读不到(绿灯慢闪的根因);
|
||||
* 之后配置 CNTL1(连续 200Hz)/CNTL2(DRDY 下降沿)/AVGCNTL(4 次平均)
|
||||
* 数据: ist8310_poll() 每 5ms 阻塞读一次 6 字节(约 0.7ms@100kHz),
|
||||
* 由 IMU_Task 调用;DRDY(EXTI3) 不再使用(数据仅做健康监测与
|
||||
* 9 轴扩展预留,无需中断实时性)
|
||||
* 报警: 检测失败/连续读失败 -> 绿灯慢闪(见 monitor_task)
|
||||
* ========================================================================== */
|
||||
#include "ist8310.h"
|
||||
#include "i2c.h"
|
||||
#include "main.h"
|
||||
#include "led.h"
|
||||
|
||||
#define IST8310_ADDR (0x0E << 1) // 7 位地址 0x0E,HAL 使用 8 位格式
|
||||
|
||||
#define IST8310_WHO_AM_I 0x00
|
||||
#define IST8310_CNTL1 0x0A
|
||||
#define IST8310_CNTL2 0x0B
|
||||
#define IST8310_AVGCNTL 0x41
|
||||
#define IST8310_PDCNTL 0x42
|
||||
#define IST8310_DATA_START 0x03
|
||||
|
||||
#define IST8310_WHO_AM_I_VALUE 0x10
|
||||
|
||||
#define IST8310_POLL_PERIOD_MS 5 // 200Hz 读取节奏
|
||||
|
||||
float ist8310_magnetic_field[3];
|
||||
float ist8310_temperature;
|
||||
uint8_t ist8310_online = 0;
|
||||
uint8_t ist8310_last_who = 0;
|
||||
|
||||
static uint8_t raw_buf[6];
|
||||
static uint8_t read_error_count = 0; // 连续读失败计数(运行中总线异常监测)
|
||||
|
||||
/* RSTN(PG6):低电平复位芯片,每次改变需等待生效 */
|
||||
static void ist8310_rstn(uint8_t level)
|
||||
{
|
||||
HAL_GPIO_WritePin(RSTN_IST8310_GPIO_Port, RSTN_IST8310_Pin,
|
||||
level ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
HAL_Delay(1);
|
||||
}
|
||||
|
||||
static uint8_t ist8310_read_who(void)
|
||||
{
|
||||
uint8_t who = 0;
|
||||
if (HAL_I2C_Mem_Read(&hi2c3, IST8310_ADDR, IST8310_WHO_AM_I,
|
||||
I2C_MEMADD_SIZE_8BIT, &who, 1, 10) != HAL_OK)
|
||||
return 0;
|
||||
return who;
|
||||
}
|
||||
|
||||
void ist8310_init(void)
|
||||
{
|
||||
/* 关键:先释放 RSTN(CubeMX 默认输出低电平会把芯片按在复位态,
|
||||
* WHO_AM_I 将永远读不到) */
|
||||
HAL_GPIO_WritePin(RSTN_IST8310_GPIO_Port, RSTN_IST8310_Pin, GPIO_PIN_SET);
|
||||
HAL_Delay(2);
|
||||
|
||||
/* 有界重试:最多 40 次(约 1s),失败则放弃并保持系统可运行 */
|
||||
for (uint8_t retry = 0; retry < 40; retry++)
|
||||
{
|
||||
ist8310_last_who = ist8310_read_who();
|
||||
if (ist8310_last_who == IST8310_WHO_AM_I_VALUE)
|
||||
{
|
||||
ist8310_online = 1;
|
||||
HAL_I2C_Mem_Write(&hi2c3, IST8310_ADDR, IST8310_CNTL1,
|
||||
I2C_MEMADD_SIZE_8BIT, (uint8_t[]){0x03}, 1, 10); // 连续模式 200Hz
|
||||
HAL_I2C_Mem_Write(&hi2c3, IST8310_ADDR, IST8310_CNTL2,
|
||||
I2C_MEMADD_SIZE_8BIT, (uint8_t[]){0x08}, 1, 10); // DRDY 下降沿
|
||||
HAL_I2C_Mem_Write(&hi2c3, IST8310_ADDR, IST8310_AVGCNTL,
|
||||
I2C_MEMADD_SIZE_8BIT, (uint8_t[]){0x12}, 1, 10); // 4 次采样平均
|
||||
HAL_I2C_Mem_Write(&hi2c3, IST8310_ADDR, IST8310_PDCNTL,
|
||||
I2C_MEMADD_SIZE_8BIT, (uint8_t[]){0xC0}, 1, 10); // 正常脉冲时长
|
||||
ist8310_rstn(0);
|
||||
ist8310_rstn(1);
|
||||
return;
|
||||
}
|
||||
|
||||
/* IST8310 连接异常报警:绿灯 1s 周期闪烁(经 TIM5 PWM,
|
||||
* 引脚复用模式下直接写 GPIO 寄存器无效) */
|
||||
static uint8_t counter = 0;
|
||||
counter++;
|
||||
if (counter == 1)
|
||||
led_set_green(1);
|
||||
else if (counter == 5)
|
||||
led_set_green(0);
|
||||
else if (counter == 40)
|
||||
counter = 0;
|
||||
HAL_Delay(25);
|
||||
}
|
||||
}
|
||||
|
||||
void ist8310_poll(void)
|
||||
{
|
||||
static uint32_t last_poll_tick = 0;
|
||||
uint32_t now = HAL_GetTick();
|
||||
if (now - last_poll_tick < IST8310_POLL_PERIOD_MS)
|
||||
return;
|
||||
last_poll_tick = now;
|
||||
|
||||
if (!ist8310_online)
|
||||
return;
|
||||
|
||||
if (HAL_I2C_Mem_Read(&hi2c3, IST8310_ADDR, IST8310_DATA_START,
|
||||
I2C_MEMADD_SIZE_8BIT, raw_buf, 6, 2) != HAL_OK)
|
||||
{
|
||||
/* 连续 100 次(约 0.5s)失败判定离线,交由监控任务绿灯报警 */
|
||||
if (++read_error_count > 100)
|
||||
ist8310_online = 0;
|
||||
return;
|
||||
}
|
||||
read_error_count = 0;
|
||||
|
||||
int16_t raw_x = (int16_t)(((uint16_t)raw_buf[1] << 8) | raw_buf[0]);
|
||||
int16_t raw_y = (int16_t)(((uint16_t)raw_buf[3] << 8) | raw_buf[2]);
|
||||
int16_t raw_z = (int16_t)(((uint16_t)raw_buf[5] << 8) | raw_buf[4]);
|
||||
ist8310_magnetic_field[0] = raw_x * 0.3f;
|
||||
ist8310_magnetic_field[1] = raw_y * 0.3f;
|
||||
ist8310_magnetic_field[2] = raw_z * 0.3f;
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
#ifndef IST8310_H
|
||||
#define IST8310_H
|
||||
|
||||
/* C 板板载磁力计 IST8310 —— 硬件 I2C3(PA8/PC9, CubeMX 已配置)版
|
||||
* DRDY=PG3(EXTI3), RSTN=PG6
|
||||
* 注:当前 AHRS 仅做 6 轴融合(磁力计数据未参与,与 legacy 一致),此模块用于
|
||||
* 健康检查与后续 9 轴扩展;
|
||||
* legacy/早期移植版用软件模拟 I2C,现已改为硬件 I2C3 */
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
extern float ist8310_magnetic_field[3]; // (uT)
|
||||
extern float ist8310_temperature; // (℃)
|
||||
extern uint8_t ist8310_online; // 1-初始化成功 0-检测失败(有界重试后放弃,系统继续运行)
|
||||
extern uint8_t ist8310_last_who; // 最近一次读到的 WHO_AM_I(诊断用,正常 0x10)
|
||||
|
||||
/* 初始化(任务上下文调用;失败时绿灯报警并重试,有界重试后放弃) */
|
||||
extern void ist8310_init(void);
|
||||
|
||||
/* 轮询读取(在 IMU_Task 1kHz 循环中调用,内部按 5ms/200Hz 分频,
|
||||
* 阻塞式 I2C 读取约 0.7ms@100kHz,不影响姿态解算) */
|
||||
extern void ist8310_poll(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,65 @@
|
||||
/* =============================================================================
|
||||
* led.c - C 板 RGB LED(TIM5 CH1/2/3 -> PH10 蓝/PH11 绿/PH12 红,高电平亮)
|
||||
* 说明: CubeMX 配置 TIM5 PSC=0/ARR=65535,此处改为 legacy 的 PSC=83/ARR=999
|
||||
* (1kHz PWM 千分位);所有接口带懒初始化,任何任务/初始化阶段都能安全
|
||||
* 调用用于报警(引脚处于复用模式时直接写 GPIO 寄存器是无效的)
|
||||
* ========================================================================== */
|
||||
#include "led.h"
|
||||
#include "tim.h"
|
||||
|
||||
extern TIM_HandleTypeDef htim5;
|
||||
|
||||
static uint8_t led_inited = 0;
|
||||
|
||||
void led_ensure_init(void)
|
||||
{
|
||||
if (!led_inited)
|
||||
led_init();
|
||||
}
|
||||
|
||||
void led_init(void)
|
||||
{
|
||||
led_inited = 1;
|
||||
/* CubeMX 生成的 PSC=0/ARR=65535 改为 legacy 的 PSC=83/ARR=999(1kHz PWM,千分位) */
|
||||
__HAL_TIM_SET_PRESCALER(&htim5, 84 - 1);
|
||||
__HAL_TIM_SET_AUTORELOAD(&htim5, 1000 - 1);
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, 0);
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, 0);
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, 0);
|
||||
HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_1);
|
||||
HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_2);
|
||||
HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3);
|
||||
}
|
||||
|
||||
/* 通道与颜色的对应关系同 legacy:CCR1=蓝,CCR2=绿,CCR3=红 */
|
||||
void led_set_color(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
led_ensure_init();
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, (uint32_t)b * 1000 / 255);
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, (uint32_t)g * 1000 / 255);
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, (uint32_t)r * 1000 / 255);
|
||||
}
|
||||
|
||||
void led_set_all_off(void)
|
||||
{
|
||||
led_ensure_init();
|
||||
led_set_color(0, 0, 0);
|
||||
}
|
||||
|
||||
void led_set_red(uint8_t on)
|
||||
{
|
||||
led_ensure_init();
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_3, on ? 1000 : 0);
|
||||
}
|
||||
|
||||
void led_set_green(uint8_t on)
|
||||
{
|
||||
led_ensure_init();
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_2, on ? 1000 : 0);
|
||||
}
|
||||
|
||||
void led_set_blue(uint8_t on)
|
||||
{
|
||||
led_ensure_init();
|
||||
__HAL_TIM_SET_COMPARE(&htim5, TIM_CHANNEL_1, on ? 1000 : 0);
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef LED_H
|
||||
#define LED_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* C 板 RGB LED(TIM5 CH1/2/3 -> PH10 蓝 / PH11 绿 / PH12 红,低电平点亮经 PWM 实现)
|
||||
* 与 legacy LED_SetColor 映射一致 */
|
||||
|
||||
extern void led_init(void); /* 配置 TIM5 为 1kHz PWM */
|
||||
/* 若尚未初始化则先初始化(可在任意上下文安全调用,供传感器报警等早期使用) */
|
||||
extern void led_ensure_init(void);
|
||||
extern void led_set_color(uint8_t r, uint8_t g, uint8_t b); /* 0~255 */
|
||||
extern void led_set_all_off(void);
|
||||
extern void led_set_red(uint8_t on);
|
||||
extern void led_set_green(uint8_t on);
|
||||
extern void led_set_blue(uint8_t on);
|
||||
#endif
|
||||
@@ -0,0 +1,126 @@
|
||||
/* =============================================================================
|
||||
* pid.c - 位置式/增量式 PID(自 legacy Control/PID 原样移植)
|
||||
* 用法:
|
||||
* PID_PositionStructureInit(pid, need) 初始化
|
||||
* PID_PositionSetParameter(pid, kp, ki, kd) 设置增益
|
||||
* PID_PositionSetEkRange(pid, lo, hi) 误差死区(|ek|在区间内视为0)
|
||||
* PID_PositionSetOUTRange(pid, lo, hi) 输出限幅
|
||||
* PID_PositionCalc(pid, now) 每周期计算,结果在 pid->OUT
|
||||
* PID_PositionClean(pid) 清误差与积分(模式切换时)
|
||||
* 注意: 积分无独立限幅,靠输出限幅+死区约束;所有云台/底盘 PID 参数集中在
|
||||
* robot_param.h,控制周期 2ms(与 legacy 调参时一致,勿随意改周期)
|
||||
* ========================================================================== */
|
||||
#include "pid.h"
|
||||
|
||||
void PID_PositionStructureInit(PID_PositionTypeDef * pid, float need_value)
|
||||
{
|
||||
pid->Need_Value = need_value;
|
||||
pid->Ek = 0;
|
||||
pid->Sum_Ek = 0;
|
||||
pid->Ek_low = 0;
|
||||
pid->Ek_up = 0;
|
||||
pid->Kp = 0;
|
||||
pid->Ki = 0;
|
||||
pid->Kd = 0;
|
||||
pid->OUT_low = -1e10;
|
||||
pid->OUT_up = 1e10;
|
||||
}
|
||||
|
||||
void PID_PositionSetParameter(PID_PositionTypeDef * pid, float kp, float ki, float kd)
|
||||
{
|
||||
pid->Kp = kp;
|
||||
pid->Ki = ki;
|
||||
pid->Kd = kd;
|
||||
}
|
||||
|
||||
void PID_PositionSetEkRange(PID_PositionTypeDef * pid, float ek_low, float ek_up)
|
||||
{
|
||||
pid->Ek_low = ek_low;
|
||||
pid->Ek_up = ek_up;
|
||||
}
|
||||
|
||||
void PID_PositionSetOUTRange(PID_PositionTypeDef * pid, float out_low, float out_up)
|
||||
{
|
||||
pid->OUT_low = out_low;
|
||||
pid->OUT_up = out_up;
|
||||
}
|
||||
|
||||
void PID_PositionClean(PID_PositionTypeDef * pid)
|
||||
{
|
||||
pid->Ek = 0;
|
||||
pid->Sum_Ek = 0;
|
||||
}
|
||||
|
||||
void PID_PositionCalc(PID_PositionTypeDef * pid, float now_value)
|
||||
{
|
||||
pid->Now_Value = now_value;
|
||||
pid->Ek_1 = pid->Ek;
|
||||
pid->Ek = pid->Need_Value - pid->Now_Value;
|
||||
if (pid->Ek_low < pid->Ek && pid->Ek < pid->Ek_up) // 误差为0检测
|
||||
pid->Ek = 0;
|
||||
pid->Sum_Ek += pid->Ek;
|
||||
pid->Del_Ek = pid->Ek - pid->Ek_1;
|
||||
|
||||
pid->P_OUT = pid->Kp * pid->Ek;
|
||||
pid->I_OUT = pid->Ki * pid->Sum_Ek;
|
||||
pid->D_OUT = pid->Kd * pid->Del_Ek;
|
||||
pid->OUT = pid->P_OUT + pid->I_OUT + pid->D_OUT;
|
||||
|
||||
if (pid->OUT < pid->OUT_low) // 输出限幅
|
||||
pid->OUT = pid->OUT_low;
|
||||
if (pid->OUT > pid->OUT_up)
|
||||
pid->OUT = pid->OUT_up;
|
||||
}
|
||||
|
||||
void PID_IncrementalStructureInit(PID_IncrementalTypeDef * pid, float need_value)
|
||||
{
|
||||
pid->Need_Value = need_value;
|
||||
pid->Ek = 0;
|
||||
pid->Ek_1 = 0;
|
||||
pid->Ek_low = 0;
|
||||
pid->Ek_up = 0;
|
||||
pid->Kp = 0;
|
||||
pid->Ki = 0;
|
||||
pid->Kd = 0;
|
||||
pid->OUT_low = -1e10;
|
||||
pid->OUT_up = 1e10;
|
||||
}
|
||||
|
||||
void PID_IncrementalSetParameter(PID_IncrementalTypeDef * pid, float kp, float ki, float kd)
|
||||
{
|
||||
pid->Kp = kp;
|
||||
pid->Ki = ki;
|
||||
pid->Kd = kd;
|
||||
}
|
||||
|
||||
void PID_IncrementalSetEkRange(PID_IncrementalTypeDef * pid, float ek_low, float ek_up)
|
||||
{
|
||||
pid->Ek_low = ek_low;
|
||||
pid->Ek_up = ek_up;
|
||||
}
|
||||
|
||||
void PID_IncrementalSetOUTRange(PID_IncrementalTypeDef * pid, float out_low, float out_up)
|
||||
{
|
||||
pid->OUT_low = out_low;
|
||||
pid->OUT_up = out_up;
|
||||
}
|
||||
|
||||
void PID_IncrementalCalc(PID_IncrementalTypeDef * pid, float now_value)
|
||||
{
|
||||
pid->Now_Value = now_value;
|
||||
pid->Ek_2 = pid->Ek_1;
|
||||
pid->Ek_1 = pid->Ek;
|
||||
pid->Ek = pid->Need_Value - pid->Now_Value;
|
||||
if (pid->Ek_low < pid->Ek && pid->Ek < pid->Ek_up) // 误差为0检测
|
||||
pid->Ek = 0;
|
||||
|
||||
pid->P_OUT = pid->Kp * (pid->Ek - pid->Ek_1);
|
||||
pid->I_OUT = pid->Ki * pid->Ek;
|
||||
pid->D_OUT = pid->Kd * (pid->Ek - 2 * pid->Ek_1 + pid->Ek_2);
|
||||
pid->OUT = pid->P_OUT + pid->I_OUT + pid->D_OUT;
|
||||
|
||||
if (pid->OUT < pid->OUT_low) // 输出限幅
|
||||
pid->OUT = pid->OUT_low;
|
||||
if (pid->OUT > pid->OUT_up)
|
||||
pid->OUT = pid->OUT_up;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
#ifndef PID_H
|
||||
#define PID_H
|
||||
|
||||
/* 位置式/增量式 PID,自 legacy 工程 Control/PID 原样移植 */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float Need_Value; // 预期值
|
||||
float Now_Value; // 当前值
|
||||
|
||||
float Ek; // 本次误差
|
||||
float Ek_1; // 上一次误差
|
||||
float Sum_Ek; // 误差积分
|
||||
float Del_Ek; // 误差差分
|
||||
float Ek_low; // 误差为0阈值下限
|
||||
float Ek_up; // 误差为0阈值上限
|
||||
|
||||
float Kp; // Kp
|
||||
float Ki; // Ki
|
||||
float Kd; // Kd
|
||||
|
||||
float P_OUT; // 比例输出
|
||||
float I_OUT; // 积分输出
|
||||
float D_OUT; // 微分输出
|
||||
float OUT; // 总输出
|
||||
float OUT_low; // 输出限幅下限
|
||||
float OUT_up; // 输出限幅上限
|
||||
} PID_PositionTypeDef; // 位置式PID参数结构体
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float Need_Value; // 预期值
|
||||
float Now_Value; // 当前值
|
||||
|
||||
float Ek; // 本次误差
|
||||
float Ek_1; // 上一次误差
|
||||
float Ek_2; // 上两次误差
|
||||
float Ek_low; // 误差为0阈值下限
|
||||
float Ek_up; // 误差为0阈值上限
|
||||
|
||||
float Kp; // Kp
|
||||
float Ki; // Ki
|
||||
float Kd; // Kd
|
||||
|
||||
float P_OUT; // 比例输出
|
||||
float I_OUT; // 积分输出
|
||||
float D_OUT; // 微分输出
|
||||
float OUT; // 总输出
|
||||
float OUT_low; // 输出限幅下限
|
||||
float OUT_up; // 输出限幅上限
|
||||
} PID_IncrementalTypeDef; // 增量式PID参数结构体
|
||||
|
||||
void PID_PositionStructureInit(PID_PositionTypeDef * pid, float need_value);
|
||||
void PID_PositionSetParameter(PID_PositionTypeDef * pid, float kp, float ki, float kd);
|
||||
void PID_PositionSetEkRange(PID_PositionTypeDef * pid, float ek_low, float ek_up);
|
||||
void PID_PositionSetOUTRange(PID_PositionTypeDef * pid, float out_low, float out_up);
|
||||
void PID_PositionClean(PID_PositionTypeDef * pid);
|
||||
void PID_PositionCalc(PID_PositionTypeDef * pid, float now_value);
|
||||
|
||||
void PID_IncrementalStructureInit(PID_IncrementalTypeDef * pid, float need_value);
|
||||
void PID_IncrementalSetParameter(PID_IncrementalTypeDef * pid, float kp, float ki, float kd);
|
||||
void PID_IncrementalSetEkRange(PID_IncrementalTypeDef * pid, float ek_low, float ek_up);
|
||||
void PID_IncrementalSetOUTRange(PID_IncrementalTypeDef * pid, float out_low, float out_up);
|
||||
void PID_IncrementalCalc(PID_IncrementalTypeDef * pid, float now_value);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,50 @@
|
||||
/* =============================================================================
|
||||
* attitude.c - 姿态解算封装(BMI088 + QuaternionEKF 惯导)
|
||||
* 2026-08-31 起由 Mahony 互补滤波更换为 Wang Hongxi 的 QuaternionEKF:
|
||||
* - 陀螺零偏在线估计,不再需要上电静止校准(1s 阻塞校准已移除)
|
||||
* - 卡方检验剔除加速度异常帧,动态响应/抗震动干扰优于互补滤波
|
||||
* - 欧拉角映射(与旧 Mahony get_yaw/pitch/roll 语义一一对应,勿改):
|
||||
* att_deg_yaw <- QEKF_INS.Yaw (标准偏航;取多圈累计 YawTotalAngle)
|
||||
* att_deg_roll <- QEKF_INS.Pitch (绕 x 轴;本云台 Pitch 环反馈用它)
|
||||
* att_deg_pitch <- QEKF_INS.Roll (绕 y 轴)
|
||||
* 原因:QEKF 内部变量命名与航空惯例互换,见 ins.c 文件头
|
||||
* 数据链: bmi088.c(SPI DMA 后台更新) -> ins_update(1kHz) -> att_deg_*
|
||||
* 健康位: bmi088_online=0 时姿态数据不可用(云台任务会拒绝输出);
|
||||
* 姿态收敛需数秒,上电初期 ConvergeFlag=0 属正常
|
||||
* ========================================================================== */
|
||||
#include "attitude.h"
|
||||
#include "bmi088.h"
|
||||
#include "ist8310.h"
|
||||
#include "ins.h"
|
||||
#include "tim.h"
|
||||
|
||||
extern TIM_HandleTypeDef htim10;
|
||||
|
||||
float att_deg_yaw; // 多圈连续偏航角(°)
|
||||
float att_deg_pitch; // 绕 y 轴角(°)
|
||||
float att_deg_roll; // 绕 x 轴角(°,本云台 Pitch 环反馈)
|
||||
uint8_t att_ready = 0; // 1-标定完成且姿态已开始输出(控制任务门控用)
|
||||
|
||||
void attitude_init(void)
|
||||
{
|
||||
ist8310_init();
|
||||
bmi088_init();
|
||||
ins_init();
|
||||
}
|
||||
|
||||
void attitude_update(void)
|
||||
{
|
||||
ins_update(0.001f); // 1kHz 固定节拍
|
||||
|
||||
att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用
|
||||
att_deg_roll = INS.Pitch; // QEKF 命名互换,见文件头
|
||||
att_deg_pitch = INS.Roll;
|
||||
|
||||
if (INS.UpdateCount > 0)
|
||||
att_ready = 1;
|
||||
}
|
||||
|
||||
void imu_heater_off(void)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, 0);
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef ATTITUDE_H
|
||||
#define ATTITUDE_H
|
||||
|
||||
/* 姿态解算封装(BMI088 + QuaternionEKF 惯导 + 多圈 Yaw)
|
||||
* 2026-08-31 由 Mahony 更换为 QuaternionEKF(见 attitude.c 文件头)
|
||||
* 注意:本云台的 Pitch 环反馈使用 att_deg_roll(绕 x 轴角,安装方向所致),
|
||||
* 而 att_deg_roll 映射自 QEKF_INS.Pitch(QEKF 命名与航空惯例互换) */
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
extern float att_deg_yaw; // 多圈连续偏航角(°)
|
||||
extern float att_deg_pitch; // 绕 y 轴角(°)
|
||||
extern float att_deg_roll; // 绕 x 轴角(°,本云台 Pitch 环反馈)
|
||||
|
||||
/* 初始化:配置 IST8310/BMI088(阻塞,有界重试)+ z 轴静止零偏标定(约 1s,
|
||||
* 车体必须静止;QEKF 不估计 z 偏置)+ QuaternionEKF 初值 */
|
||||
extern void attitude_init(void);
|
||||
|
||||
/* 姿态更新(1kHz 调用,dt=0.001s) */
|
||||
extern void attitude_update(void);
|
||||
|
||||
/* 1-z 轴标定完成且姿态已有效输出(标定期间姿态为 0,控制任务须门控) */
|
||||
extern uint8_t att_ready;
|
||||
|
||||
/* IMU 加热器(TIM10 CH1/PF6 PWM):monitor 检测到过温时调用强制关闭 */
|
||||
extern void imu_heater_off(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,197 @@
|
||||
/* =============================================================================
|
||||
* c_comm.c - 云台<->底盘 板间通讯(CToC,CAN2,帧 ID 与 legacy 完全一致)
|
||||
* 帧: 0x149 云->底 摇杆(4 个大端 u16) 0x189 云->底 控制状态
|
||||
* 0x019 底->云 发射机构上电状态
|
||||
* 云台板(主机): c_comm_master_send() 每 2ms 发摇杆帧+状态帧;
|
||||
* 键鼠在线时按 legacy 规则把 WASD/鼠标合成进摇杆量;
|
||||
* Data[0] 必须实时判定 dr16_offline()(dr16_status 掉线后不会自己清零)
|
||||
* 底盘板(从机): c_comm_slave_send() 每 2ms 回传发射机构状态;
|
||||
* 中断内解析 0x149/0x189 填充 ctoc_remote(底盘的"虚拟遥控器")
|
||||
* ========================================================================== */
|
||||
#include "c_comm.h"
|
||||
#include "robot_param.h"
|
||||
#include "referee.h"
|
||||
#include <string.h>
|
||||
|
||||
volatile ctoc_remote_t ctoc_remote;
|
||||
volatile uint8_t ctoc_shooter_power = 1;
|
||||
volatile uint32_t ctoc_reply_last_tick = 0;
|
||||
volatile uint8_t ctoc_remote_fault = 0;
|
||||
volatile float ctoc_gimbal_yaw = 0.0f;
|
||||
volatile uint8_t ctoc_att_ready = 0;
|
||||
static volatile uint32_t ctoc_att_last_tick = 0;
|
||||
/* 云台自检下发的编码器方向:+1=tick增向CCW -1=CW 0=未检测(0x1A9 flags) */
|
||||
volatile int8_t ctoc_encoder_ccw = 0;
|
||||
|
||||
/* 0x1A9 帧新鲜判定(两板公共:底盘跟随用它门控,云台留作诊断) */
|
||||
uint8_t ctoc_att_fresh(void)
|
||||
{
|
||||
return (ctoc_att_last_tick != 0 &&
|
||||
HAL_GetTick() - ctoc_att_last_tick < CTOC_OFFLINE_TIME_MS);
|
||||
}
|
||||
|
||||
static void c_comm_pack_uint16(uint8_t data[8], const uint16_t val[4])
|
||||
{
|
||||
for (uint8_t i = 0; i < 4; i++)
|
||||
{
|
||||
data[2 * i] = (uint8_t)(val[i] >> 8);
|
||||
data[2 * i + 1] = (uint8_t)(val[i] & 0xFF);
|
||||
}
|
||||
}
|
||||
|
||||
static void c_comm_send_frame(hcan_t * hcan, uint32_t std_id, const uint8_t data[8])
|
||||
{
|
||||
CanCtrlData_s msg;
|
||||
msg.hcan = hcan;
|
||||
msg.tx_header.StdId = std_id;
|
||||
msg.tx_header.IDE = CAN_ID_STD;
|
||||
msg.tx_header.RTR = CAN_RTR_DATA;
|
||||
msg.tx_header.DLC = 8;
|
||||
msg.tx_header.TransmitGlobalTime = DISABLE;
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
msg.tx_data[i] = data[i];
|
||||
CAN_SendTxMessage(&msg);
|
||||
}
|
||||
|
||||
#if defined(ROBOT_TYPE_GIMBAL)
|
||||
|
||||
/* ============================ 云台板(主机) ============================ */
|
||||
|
||||
static void c_comm_rx_0x019(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
|
||||
{
|
||||
(void)std_id;
|
||||
(void)dlc;
|
||||
ctoc_shooter_power = rx_data[0]; // 发射机构上电状态
|
||||
ctoc_remote_fault = rx_data[1] & 0x01; // 底盘电机故障位
|
||||
ctoc_reply_last_tick = HAL_GetTick();
|
||||
}
|
||||
|
||||
void c_comm_init(void)
|
||||
{
|
||||
can_rx_register(&hcan2, CTOC_MASTER_ID1, c_comm_rx_0x019);
|
||||
}
|
||||
|
||||
uint8_t ctoc_chassis_fault(void)
|
||||
{
|
||||
/* 从未收到过 0x019 视为底盘不在场,不判故障(便于仅云台台架调试);
|
||||
* 收到过之后,故障位或帧超时都判故障 */
|
||||
if (ctoc_reply_last_tick == 0)
|
||||
return 0;
|
||||
if (HAL_GetTick() - ctoc_reply_last_tick > CTOC_OFFLINE_TIME_MS)
|
||||
return 1;
|
||||
return ctoc_remote_fault;
|
||||
}
|
||||
|
||||
/* 云台板:0x1A9 姿态帧 */
|
||||
void c_comm_master_send_attitude(float yaw_deg, uint8_t flags)
|
||||
{
|
||||
uint8_t data[8] = {0};
|
||||
memcpy(data, &yaw_deg, 4); // float 小端
|
||||
data[4] = flags;
|
||||
c_comm_send_frame(&hcan2, CTOC_SLAVE_ID3, data);
|
||||
}
|
||||
|
||||
void c_comm_master_send(uint8_t keyboard_online, const uint8_t kb[12], uint8_t motor_fault, uint8_t spin_enable)
|
||||
{
|
||||
uint8_t data[8];
|
||||
|
||||
if (!keyboard_online)
|
||||
{
|
||||
// 遥控器控制:直接转发原始摇杆值
|
||||
const uint16_t sticks[4] = {dr16.rc_r_RL, dr16.rc_r_UD, dr16.rc_l_RL, dr16.rc_l_UD};
|
||||
c_comm_pack_uint16(data, sticks);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 键鼠控制:合成摇杆量(与 legacy 一致)
|
||||
uint8_t key_go = kb[8] & 0x01;
|
||||
uint8_t key_back = (kb[8] >> 1) & 0x01;
|
||||
uint8_t key_left = (kb[8] >> 2) & 0x01;
|
||||
uint8_t key_right = (kb[8] >> 3) & 0x01;
|
||||
int16_t mouse_rl = (int16_t)(((uint16_t)kb[1] << 8) | kb[0]);
|
||||
int16_t mouse_du = (int16_t)(((uint16_t)kb[3] << 8) | kb[2]);
|
||||
|
||||
const uint16_t sticks[4] = {
|
||||
(uint16_t)(1024 + (key_right - key_left) * 660), // AD->右摇杆RL
|
||||
(uint16_t)(1024 + (key_go - key_back) * 660), // WS->右摇杆UD
|
||||
(uint16_t)(1024 + mouse_rl * PC_MOUSE_RL_SENSITIVITY), // 鼠标->左摇杆RL
|
||||
(uint16_t)(1024 + mouse_du * PC_MOUSE_DU_SENSITIVITY), // 鼠标->左摇杆UD
|
||||
};
|
||||
c_comm_pack_uint16(data, sticks);
|
||||
}
|
||||
c_comm_send_frame(&hcan2, CTOC_SLAVE_ID1, data);
|
||||
|
||||
/* 在线状态必须实时判定:dr16_status 只在收帧时置 1,掉线后不会自己清零,
|
||||
* 若直接转发会让底盘永远认为遥控器在线(停车保护失效) */
|
||||
data[0] = (dr16_status && !dr16_offline()) ? 1 : 0;
|
||||
data[1] = dr16.s2;
|
||||
data[2] = dr16.key_push_ctrl;
|
||||
data[3] = dr16.key_push_shift;
|
||||
data[4] = dr16_start_flag;
|
||||
data[5] = dr16.s1;
|
||||
data[6] = (motor_fault ? 0x80 : 0) | (spin_enable ? 0x04 : 0) |
|
||||
(dr16.key_alpha1 ? 0x01 : 0) |
|
||||
(dr16.key_alpha2 ? 0x02 : 0); // bit7=故障 bit2=陀螺使能(扩展)
|
||||
data[7] = 0;
|
||||
c_comm_send_frame(&hcan2, CTOC_SLAVE_ID2, data);
|
||||
}
|
||||
|
||||
#elif defined(ROBOT_TYPE_CHASSIS)
|
||||
|
||||
/* ============================ 底盘板(从机) ============================ */
|
||||
|
||||
static void c_comm_rx_0x149(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
|
||||
{
|
||||
(void)std_id;
|
||||
(void)dlc;
|
||||
ctoc_remote.rc_r_rl = ((uint16_t)rx_data[0] << 8) | rx_data[1];
|
||||
ctoc_remote.rc_r_ud = ((uint16_t)rx_data[2] << 8) | rx_data[3];
|
||||
ctoc_remote.rc_l_rl = ((uint16_t)rx_data[4] << 8) | rx_data[5];
|
||||
ctoc_remote.rc_l_ud = ((uint16_t)rx_data[6] << 8) | rx_data[7];
|
||||
ctoc_remote.last_tick = HAL_GetTick();
|
||||
}
|
||||
|
||||
static void c_comm_rx_0x189(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
|
||||
{
|
||||
(void)std_id;
|
||||
(void)dlc;
|
||||
ctoc_remote.status = rx_data[0];
|
||||
ctoc_remote.s2 = rx_data[1];
|
||||
ctoc_remote.ctrl_push = rx_data[2];
|
||||
ctoc_remote.shift_push = rx_data[3];
|
||||
ctoc_remote.start_flag = rx_data[4];
|
||||
ctoc_remote.s1 = rx_data[5];
|
||||
ctoc_remote.spin_flag = (rx_data[6] >> 2) & 0x01; // 小陀螺使能位
|
||||
ctoc_remote.gimbal_fault = (rx_data[6] & 0x80) ? 1 : 0; // 云台电机故障位
|
||||
ctoc_remote.last_tick = HAL_GetTick();
|
||||
}
|
||||
|
||||
static void c_comm_rx_0x1a9(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
|
||||
{
|
||||
(void)std_id;
|
||||
(void)dlc;
|
||||
float yaw;
|
||||
memcpy(&yaw, rx_data, 4);
|
||||
ctoc_gimbal_yaw = yaw;
|
||||
ctoc_att_ready = rx_data[4] & 0x01;
|
||||
if (rx_data[4] & 0x04) // 云台自检完成位
|
||||
ctoc_encoder_ccw = (rx_data[4] & 0x02) ? 1 : -1; // tick 增方向
|
||||
ctoc_att_last_tick = HAL_GetTick();
|
||||
}
|
||||
|
||||
void c_comm_init(void)
|
||||
{
|
||||
can_rx_register(&hcan2, CTOC_SLAVE_ID1, c_comm_rx_0x149);
|
||||
can_rx_register(&hcan2, CTOC_SLAVE_ID2, c_comm_rx_0x189);
|
||||
can_rx_register(&hcan2, CTOC_SLAVE_ID3, c_comm_rx_0x1a9);
|
||||
}
|
||||
|
||||
void c_comm_slave_send(uint8_t motor_fault)
|
||||
{
|
||||
uint8_t data[8] = {0};
|
||||
data[0] = referee.shooter_power_on; // 发射机构上电状态
|
||||
data[1] = motor_fault ? 0x01 : 0; // 底盘电机故障位(扩展)
|
||||
c_comm_send_frame(&hcan2, CTOC_MASTER_ID1, data);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,79 @@
|
||||
#ifndef C_COMM_H
|
||||
#define C_COMM_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "bsp_can.h"
|
||||
#include "dr16.h"
|
||||
|
||||
/* 云台<->底盘 板间通讯(CToC),帧 ID 与 legacy 完全一致
|
||||
* 0x149 云台->底盘:Data[0..1]右摇杆RL [2..3]右摇杆UD [4..5]左摇杆RL [6..7]左摇杆UD(大端16bit)
|
||||
* 0x189 云台->底盘:Data[0]遥控状态 [1]S2 [3]Shift翻转 [4]启动标志 [5]S1
|
||||
* Data[2] 保留(原 Ctrl 翻转态——沿检测已移至云台侧,
|
||||
* 底盘误判沿会误开小陀螺,勿再使用)
|
||||
* Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展)
|
||||
* bit7=云台电机故障(本工程扩展)
|
||||
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) Data[4]=att_ready
|
||||
* (双 INS 跟随方案的数据源,2026-09 新增)
|
||||
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE)
|
||||
* Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW
|
||||
* bit2=该位有效(自检完成) [跟随符号自动综合的数据源]
|
||||
* 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展)
|
||||
*
|
||||
* 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */
|
||||
|
||||
/* 底盘板:从 0x149/0x189 解析出的"虚拟遥控器"数据(布局同 dr16_t 的子集) */
|
||||
typedef struct
|
||||
{
|
||||
uint16_t rc_r_rl;
|
||||
uint16_t rc_r_ud;
|
||||
uint16_t rc_l_rl;
|
||||
uint16_t rc_l_ud;
|
||||
uint8_t s1;
|
||||
uint8_t s2;
|
||||
uint8_t ctrl_push;
|
||||
uint8_t shift_push;
|
||||
uint8_t start_flag;
|
||||
uint8_t status; // 云台侧遥控器在线状态
|
||||
uint8_t spin_flag; // 小陀螺使能(0x189 Data[6] bit2,云台侧沿检测)
|
||||
uint8_t gimbal_fault; // 云台电机故障标志(0x189 Data[6] bit7)
|
||||
uint32_t last_tick; // 最近收帧 tick(掉线检测)
|
||||
} ctoc_remote_t;
|
||||
|
||||
extern volatile ctoc_remote_t ctoc_remote;
|
||||
|
||||
/* 云台板:底盘回传的发射机构上电状态 */
|
||||
extern volatile uint8_t ctoc_shooter_power;
|
||||
/* 云台板:最近收到 0x019 的 tick(掉线检测/诊断) */
|
||||
extern volatile uint32_t ctoc_reply_last_tick;
|
||||
/* 云台板:底盘电机故障标志(0x019 Data[1] bit0,ISR 内更新) */
|
||||
extern volatile uint8_t ctoc_remote_fault;
|
||||
|
||||
/* 云台板:底盘故障综合判定(故障位置位 或 0x019 超时;从未收到过 0x019
|
||||
* 视为底盘不在场不判故障,便于仅云台台架调试) */
|
||||
extern uint8_t ctoc_chassis_fault(void);
|
||||
|
||||
/* 云台板:发送 0x1A9 姿态帧(2ms 周期,c_comm_task 调用)
|
||||
* yaw_deg 为世界系多圈角(att_deg_yaw),flags bit0=att_ready */
|
||||
extern void c_comm_master_send_attitude(float yaw_deg, uint8_t flags);
|
||||
|
||||
/* 底盘板:云台姿态(0x1A9 解析,ISR 内更新) */
|
||||
extern volatile float ctoc_gimbal_yaw;
|
||||
extern volatile uint8_t ctoc_att_ready;
|
||||
/* 云台自检的编码器方向:+1=tick增向CCW -1=CW 0=未检测 */
|
||||
extern volatile int8_t ctoc_encoder_ccw;
|
||||
/* 底盘板:0x1A9 帧是否新鲜(掉线检测) */
|
||||
extern uint8_t ctoc_att_fresh(void);
|
||||
|
||||
/* 注册 CAN 接收回调(can_start() 之后调用) */
|
||||
extern void c_comm_init(void);
|
||||
|
||||
/* 云台板:发送摇杆帧与控制状态帧(2ms 周期,keyboard_online=1 时用键鼠合成量替代摇杆)
|
||||
* kb 参数为图传 0x0304 解析出的键鼠数据(keyboard_online 时有效),布局:
|
||||
* [0..1]鼠标X [2..3]鼠标Y [4..5]滚轮 [6]左键 [7]右键 [8]键位bitmap
|
||||
* motor_fault:本板电机掉线标志,随 0x189 Data[6] bit7 广播 */
|
||||
extern void c_comm_master_send(uint8_t keyboard_online, const uint8_t kb[12], uint8_t motor_fault, uint8_t spin_enable);
|
||||
|
||||
/* 底盘板:回传发射机构状态与底盘电机故障位(2ms 周期) */
|
||||
extern void c_comm_slave_send(uint8_t motor_fault);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,118 @@
|
||||
/* =============================================================================
|
||||
* filter.c - 滤波与数学工具集(自 legacy user_lib / WaveFiltering 移植)
|
||||
* 内容:
|
||||
* kalmanFilter() 一维卡尔曼(R 越大越平滑),用于姿态角平滑
|
||||
* ramp_calc() 斜波(缓启动)
|
||||
* first_order_filter 一阶低通
|
||||
* invSqrt/abs_limit/fp32_constrain/loop_fp32_constrain/theta_format
|
||||
* rad_format() 弧度归一化到 -PI~PI(角度环误差计算用)
|
||||
* ========================================================================== */
|
||||
#include "filter.h"
|
||||
#include <math.h>
|
||||
|
||||
void kalman_filter_init(KFP * kfp, float q, float r)
|
||||
{
|
||||
kfp->LastP = 0.02f;
|
||||
kfp->Now_P = 0.0f;
|
||||
kfp->out = 0.0f;
|
||||
kfp->Kg = 0.0f;
|
||||
kfp->Q = q;
|
||||
kfp->R = r;
|
||||
}
|
||||
|
||||
float kalmanFilter(KFP * kfp, float input)
|
||||
{
|
||||
kfp->Now_P = kfp->LastP + kfp->Q; // 预测协方差
|
||||
kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R); // 卡尔曼增益
|
||||
kfp->out += kfp->Kg * (input - kfp->out); // 更新最优值
|
||||
kfp->LastP = (1 - kfp->Kg) * kfp->Now_P; // 更新协方差
|
||||
return kfp->out;
|
||||
}
|
||||
|
||||
void ramp_init(ramp_t * ramp, float frame_period, float max, float min)
|
||||
{
|
||||
ramp->frame_period = frame_period;
|
||||
ramp->max_value = max;
|
||||
ramp->min_value = min;
|
||||
ramp->input = 0.0f;
|
||||
ramp->out = 0.0f;
|
||||
}
|
||||
|
||||
void ramp_calc(ramp_t * ramp, float input)
|
||||
{
|
||||
ramp->input = input;
|
||||
ramp->out += ramp->input * ramp->frame_period;
|
||||
if (ramp->out > ramp->max_value)
|
||||
ramp->out = ramp->max_value;
|
||||
else if (ramp->out < ramp->min_value)
|
||||
ramp->out = ramp->min_value;
|
||||
}
|
||||
|
||||
void first_order_filter_init(first_order_filter_t * f, float frame_period, const float num[1])
|
||||
{
|
||||
f->frame_period = frame_period;
|
||||
f->num[0] = num[0];
|
||||
f->input = 0.0f;
|
||||
f->out = 0.0f;
|
||||
}
|
||||
|
||||
void first_order_filter_cali(first_order_filter_t * f, float input)
|
||||
{
|
||||
f->input = input;
|
||||
f->out = f->num[0] / (f->num[0] + f->frame_period) * f->out +
|
||||
f->frame_period / (f->num[0] + f->frame_period) * f->input;
|
||||
}
|
||||
|
||||
float invSqrt(float num)
|
||||
{
|
||||
return 1 / sqrtf(num);
|
||||
}
|
||||
|
||||
void abs_limit(float * num, float limit)
|
||||
{
|
||||
if (*num > limit)
|
||||
*num = limit;
|
||||
else if (*num < -limit)
|
||||
*num = -limit;
|
||||
}
|
||||
|
||||
float fp32_constrain(float value, float min_value, float max_value)
|
||||
{
|
||||
if (value > max_value)
|
||||
return max_value;
|
||||
if (value < min_value)
|
||||
return min_value;
|
||||
return value;
|
||||
}
|
||||
|
||||
int16_t int16_constrain(int16_t value, int16_t min_value, int16_t max_value)
|
||||
{
|
||||
if (value > max_value)
|
||||
return max_value;
|
||||
if (value < min_value)
|
||||
return min_value;
|
||||
return value;
|
||||
}
|
||||
|
||||
float loop_fp32_constrain(float input, float min_value, float max_value)
|
||||
{
|
||||
if (max_value < min_value)
|
||||
return input;
|
||||
|
||||
if (input > max_value)
|
||||
{
|
||||
float len = max_value - min_value;
|
||||
return input - len * ((int)((input - min_value) / len) + 1);
|
||||
}
|
||||
else if (input < min_value)
|
||||
{
|
||||
float len = max_value - min_value;
|
||||
return input + len * ((int)((min_value - input) / len) + 1);
|
||||
}
|
||||
return input;
|
||||
}
|
||||
|
||||
float theta_format(float angle)
|
||||
{
|
||||
return loop_fp32_constrain(angle, -180.0f, 180.0f);
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
#ifndef FILTER_H
|
||||
#define FILTER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* 滤波/数学工具集,自 legacy 的 user_lib 与 WaveFiltering_Kalman_Filtering 移植 */
|
||||
|
||||
#define PI 3.14159265358979f
|
||||
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
|
||||
|
||||
/* 一维卡尔曼滤波器 */
|
||||
typedef struct
|
||||
{
|
||||
float LastP; // 上次估算协方差 初始化值为0.02
|
||||
float Now_P; // 当前估算协方差 初始化值为0
|
||||
float out; // 卡尔曼滤波器输出 初始化值为0
|
||||
float Kg; // 卡尔曼增益 初始化值为0
|
||||
float Q; // 过程噪声协方差 初始化值为0.001
|
||||
float R; // 观测噪声协方差 初始化值为0.543
|
||||
} KFP;
|
||||
|
||||
void kalman_filter_init(KFP * kfp, float q, float r);
|
||||
float kalmanFilter(KFP * kfp, float input);
|
||||
|
||||
/* 斜波函数 */
|
||||
typedef struct
|
||||
{
|
||||
float input;
|
||||
float out;
|
||||
float min_value;
|
||||
float max_value;
|
||||
float frame_period;
|
||||
} ramp_t;
|
||||
|
||||
void ramp_init(ramp_t * ramp, float frame_period, float max, float min);
|
||||
void ramp_calc(ramp_t * ramp, float input);
|
||||
|
||||
/* 一阶低通滤波 */
|
||||
typedef struct
|
||||
{
|
||||
float input;
|
||||
float out;
|
||||
float num[1];
|
||||
float frame_period;
|
||||
} first_order_filter_t;
|
||||
|
||||
void first_order_filter_init(first_order_filter_t * f, float frame_period, const float num[1]);
|
||||
void first_order_filter_cali(first_order_filter_t * f, float input);
|
||||
|
||||
/* 快速开方 */
|
||||
float invSqrt(float num);
|
||||
|
||||
/* 限幅/死区/格式化 */
|
||||
void abs_limit(float * num, float limit);
|
||||
float fp32_constrain(float value, float min_value, float max_value);
|
||||
int16_t int16_constrain(int16_t value, int16_t min_value, int16_t max_value);
|
||||
float loop_fp32_constrain(float input, float min_value, float max_value);
|
||||
float theta_format(float angle); /* 角度格式化为 -180~180 */
|
||||
#define rad_format(Ang) loop_fp32_constrain((Ang), -PI, PI)
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,205 @@
|
||||
/* =============================================================================
|
||||
* gimbal.c - 云台双轴控制(串级 PID)
|
||||
* Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明
|
||||
* 位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅)
|
||||
* 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电压(限幅)
|
||||
* 稳定性条件:SIGN*G>0,G 为"电压->陀螺 z 角速度"的装配增益,
|
||||
* 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
|
||||
* Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
|
||||
* 发送:CAN2 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
|
||||
* ========================================================================== */
|
||||
#include "gimbal.h"
|
||||
#include "shoot.h"
|
||||
#include "pid.h"
|
||||
#include "attitude.h"
|
||||
#include "bmi088.h"
|
||||
#include "can.h"
|
||||
#include "robot_param.h"
|
||||
#include "filter.h"
|
||||
|
||||
#ifndef RAD_TO_ANGLE
|
||||
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
|
||||
#endif
|
||||
|
||||
dji_motor_t motor_yaw_gimbal;
|
||||
dji_motor_t motor_pitch_gimbal;
|
||||
dji_motor_t motor_friction_l;
|
||||
dji_motor_t motor_friction_r;
|
||||
dji_motor_t motor_rammer;
|
||||
|
||||
static PID_PositionTypeDef yaw_angle_pid, yaw_speed_pid;
|
||||
static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid;
|
||||
|
||||
static float yaw_target_deg; // 世界系目标角(°)
|
||||
static float pitch_target_deg; // 目标角(°)
|
||||
static float yaw_voltage;
|
||||
static float pitch_voltage;
|
||||
|
||||
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
|
||||
static float yaw_sign_f = 0.0f;
|
||||
static uint8_t yaw_sign_valid = 0;
|
||||
|
||||
/* 拨弹电流由 shoot 模块写入,云台与拨弹共帧(0x1FF CAN1)发送 */
|
||||
static int16_t rammer_current_shared = 0;
|
||||
|
||||
void shoot_share_rammer_current(int16_t current);
|
||||
|
||||
void gimbal_motor_init(void)
|
||||
{
|
||||
dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020);
|
||||
dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020);
|
||||
dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508);
|
||||
dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508);
|
||||
dji_motor_init(&motor_rammer, &hcan1, 0x207, DJI_MOTOR_2006);
|
||||
|
||||
/* Yaw 位置环:° -> °/s */
|
||||
PID_PositionStructureInit(&yaw_angle_pid, 0);
|
||||
PID_PositionSetParameter(&yaw_angle_pid, GIMBAL_YAW_ANGLE_KP, GIMBAL_YAW_ANGLE_KI, GIMBAL_YAW_ANGLE_KD);
|
||||
PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND);
|
||||
PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT);
|
||||
|
||||
/* Yaw 速度环:°/s -> 电压 */
|
||||
PID_PositionStructureInit(&yaw_speed_pid, 0);
|
||||
PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD);
|
||||
PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT);
|
||||
|
||||
/* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */
|
||||
PID_PositionStructureInit(&pitch_angle_pid, 0);
|
||||
PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD);
|
||||
PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND);
|
||||
PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200);
|
||||
|
||||
PID_PositionStructureInit(&pitch_speed_pid, 300);
|
||||
PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD);
|
||||
PID_PositionSetEkRange(&pitch_speed_pid, -5, 5);
|
||||
PID_PositionSetOUTRange(&pitch_speed_pid, -30000, 30000);
|
||||
}
|
||||
|
||||
uint8_t gimbal_detect_yaw_sign(void)
|
||||
{
|
||||
if (gimbal_motor_offline())
|
||||
return 0;
|
||||
|
||||
/* 静默期:输出清零等待机械/陀螺稳定 */
|
||||
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
|
||||
HAL_Delay(50);
|
||||
|
||||
/* 正向电压脉冲 60ms,期间平均陀螺 z 角速度 */
|
||||
float sum = 0.0f;
|
||||
dji_motor_send_current_high(&hcan2, GIMBAL_YAW_DETECT_VOLTAGE, 0, 0, 0);
|
||||
for (uint16_t i = 0; i < 60; i++)
|
||||
{
|
||||
sum += bmi088_gyro[2]; // rad/s(z 轴向上为正)
|
||||
HAL_Delay(1);
|
||||
}
|
||||
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
|
||||
HAL_Delay(80); // 回稳,随后 gimbal_task 的首次控制会同步目标角
|
||||
|
||||
const float rate_dps = sum / 60.0f * RAD_TO_ANGLE;
|
||||
if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN)
|
||||
{
|
||||
yaw_sign_f = 1.0f; // 正电压->陀螺正转,装配增益 G>0
|
||||
yaw_sign_valid = 1;
|
||||
}
|
||||
else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN)
|
||||
{
|
||||
yaw_sign_f = -1.0f; // 皮带交叉等反向装配
|
||||
yaw_sign_valid = 1;
|
||||
}
|
||||
else
|
||||
return 0; // 被按住/没转:不给结论,由调用方决定重试或回退
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
float gimbal_yaw_sign(void)
|
||||
{
|
||||
return yaw_sign_valid ? yaw_sign_f : GIMBAL_YAW_SIGN_FALLBACK;
|
||||
}
|
||||
|
||||
int8_t gimbal_encoder_ccw(void)
|
||||
{
|
||||
/* 自检:电压正脉冲 -> 陀螺 z 正转(云台 CCW),而电压正方向即编码器
|
||||
* tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */
|
||||
if (!yaw_sign_valid)
|
||||
return 0;
|
||||
return (yaw_sign_f > 0.0f) ? 1 : -1;
|
||||
}
|
||||
|
||||
void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
|
||||
{
|
||||
/* 双环共用装配符号 */
|
||||
const float sign = gimbal_yaw_sign();
|
||||
const float yaw_meas = sign * att_deg_yaw; // °
|
||||
const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE); // °/s
|
||||
|
||||
/* 首次进入控制:目标角同步为当前姿态(否则 Yaw 目标仍为初始 0°,
|
||||
* 一闭环就会朝 0° 猛转) */
|
||||
static uint8_t first_run = 1;
|
||||
if (first_run)
|
||||
{
|
||||
first_run = 0;
|
||||
yaw_target_deg = yaw_meas;
|
||||
pitch_target_deg = 0;
|
||||
}
|
||||
|
||||
if (reset_flag)
|
||||
{
|
||||
yaw_target_deg = yaw_meas; // Yaw 复位为当前角度
|
||||
pitch_target_deg = 0; // Pitch 复位为上电位置
|
||||
PID_PositionClean(&yaw_angle_pid);
|
||||
PID_PositionClean(&yaw_speed_pid);
|
||||
PID_PositionClean(&pitch_angle_pid);
|
||||
PID_PositionClean(&pitch_speed_pid);
|
||||
}
|
||||
|
||||
yaw_target_deg += yaw_delta;
|
||||
pitch_target_deg += pitch_delta;
|
||||
|
||||
/* Yaw 位置环:° -> °/s(反馈为带符号姿态角) */
|
||||
yaw_angle_pid.Need_Value = yaw_target_deg;
|
||||
PID_PositionCalc(&yaw_angle_pid, yaw_meas);
|
||||
|
||||
/* Yaw 速度环:°/s -> 电压(反馈为带符号陀螺角速度,双环符号自洽) */
|
||||
yaw_speed_pid.Need_Value = yaw_angle_pid.OUT;
|
||||
PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas);
|
||||
yaw_voltage = yaw_speed_pid.OUT;
|
||||
|
||||
/* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */
|
||||
pitch_angle_pid.Need_Value = pitch_target_deg;
|
||||
PID_PositionCalc(&pitch_angle_pid, att_deg_roll);
|
||||
pitch_speed_pid.Need_Value = pitch_angle_pid.OUT;
|
||||
PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed);
|
||||
pitch_voltage = pitch_speed_pid.OUT;
|
||||
|
||||
/* 发送:CAN2 0x1FF 槽1 = Yaw 电压;CAN1 0x1FF 槽2 = Pitch 电压,槽3 = 拨弹电流 */
|
||||
dji_motor_send_current_high(&hcan2, (int16_t)yaw_voltage, 0, 0, 0);
|
||||
dji_motor_send_current_high(&hcan1, 0, (int16_t)pitch_voltage, rammer_current_shared, 0);
|
||||
}
|
||||
|
||||
void shoot_share_rammer_current(int16_t current)
|
||||
{
|
||||
rammer_current_shared = current;
|
||||
}
|
||||
|
||||
uint8_t gimbal_motor_offline(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
uint8_t rammer_off = 0;
|
||||
#if GIMBAL_RAMMER_INSTALLED
|
||||
rammer_off = (motor_rammer.fdb.last_online_tick == 0 ||
|
||||
now - motor_rammer.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
|
||||
#endif
|
||||
return (motor_yaw_gimbal.fdb.last_online_tick == 0 || now - motor_yaw_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
||||
motor_pitch_gimbal.fdb.last_online_tick == 0 || now - motor_pitch_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
||||
motor_friction_l.fdb.last_online_tick == 0 || now - motor_friction_l.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
||||
motor_friction_r.fdb.last_online_tick == 0 || now - motor_friction_r.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
|
||||
rammer_off);
|
||||
}
|
||||
|
||||
void gimbal_output_zero(void)
|
||||
{
|
||||
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
|
||||
dji_motor_send_current_high(&hcan1, 0, 0, 0, 0);
|
||||
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
#ifndef GIMBAL_H
|
||||
#define GIMBAL_H
|
||||
|
||||
/* 云台双轴控制(串级 PID)
|
||||
* Yaw (2026-09 重写):双环都以陀螺仪为反馈——
|
||||
* 位置环:QEKF 多圈 yaw(°) -> 目标角速度(°/s)
|
||||
* 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电压
|
||||
* 装配符号由上电自检自动测定(gimbal_detect_yaw_sign),
|
||||
* 两环共用同一符号,不可能再出现"两传感器符号打架"
|
||||
* Pitch:位置环用 QEKF 的 Roll(安装方向所致,勿改),速度环转子转速
|
||||
* 电机: Yaw=GM6020 CAN2 0x205(电压,0x1FF CAN2 槽1)
|
||||
* Pitch=GM6020 CAN1 0x206(电压,与拨弹共帧 0x1FF CAN1 槽2/3,legacy 方案)
|
||||
* 摩擦轮 M3508 0x201/0x202 与拨弹 M2006 0x207 的实例也在此初始化 */
|
||||
|
||||
#include "dji_motor.h"
|
||||
|
||||
/* 电机实例(由 gimbal_task 初始化,其他模块可读取反馈) */
|
||||
extern dji_motor_t motor_yaw_gimbal; // Yaw GM6020,CAN2 0x205
|
||||
extern dji_motor_t motor_pitch_gimbal; // Pitch GM6020,CAN1 0x206
|
||||
extern dji_motor_t motor_friction_l; // 左摩擦轮 M3508,CAN1 0x201
|
||||
extern dji_motor_t motor_friction_r; // 右摩擦轮 M3508,CAN1 0x202
|
||||
extern dji_motor_t motor_rammer; // 拨弹 M2006,CAN1 0x207
|
||||
|
||||
/* 初始化电机实例与 PID(can_start 之后调用) */
|
||||
extern void gimbal_motor_init(void);
|
||||
|
||||
/* Yaw 装配符号自检:给 Yaw 电机正向小电压脉冲,按陀螺 z 轴响应方向
|
||||
* 测定符号。阻塞约 200ms(HAL_Delay),检测期间云台会轻转几度。
|
||||
* 返回 1-成功(gimbal_yaw_sign() 可用) 0-失败(被按住/电机离线/无响应) */
|
||||
extern uint8_t gimbal_detect_yaw_sign(void);
|
||||
|
||||
/* 自检测得的装配符号(+1/-1;未检测时为 FALLBACK 参数值) */
|
||||
extern float gimbal_yaw_sign(void);
|
||||
|
||||
/* 编码器 tick 增大方向对应云台旋转方向:+1=逆时针(CCW) -1=顺时针(CW)
|
||||
* 0=尚未自检。随 0x1A9 flags bit1 下发,供底盘跟随符号自动综合 */
|
||||
extern int8_t gimbal_encoder_ccw(void);
|
||||
|
||||
/* 云台轴控制:根据 cmd 增量更新目标角并计算电压输出(2ms 调用)
|
||||
* 内部发送 CAN2 0x1FF(Yaw)与 CAN1 0x1FF(Pitch+拨弹) */
|
||||
extern void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag);
|
||||
|
||||
/* 云台任一电机掉线返回 1 */
|
||||
extern uint8_t gimbal_motor_offline(void);
|
||||
|
||||
/* 云台输出清零(停车) */
|
||||
extern void gimbal_output_zero(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,492 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file QuaternionEKF.c
|
||||
* @author Wang Hongxi
|
||||
* @version V1.2.0
|
||||
* @date 2022/3/8
|
||||
* @brief attitude update with gyro bias estimate and chi-square test
|
||||
******************************************************************************
|
||||
* @attention
|
||||
* 1st order LPF transfer function:
|
||||
* 1
|
||||
* ———————
|
||||
* as + 1
|
||||
******************************************************************************
|
||||
*/
|
||||
#include "QuaternionEKF.h"
|
||||
|
||||
QEKF_INS_t QEKF_INS;
|
||||
|
||||
const float IMU_QuaternionEKF_F[36] = {1, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 1, 0, 0,
|
||||
0, 0, 0, 0, 1, 0,
|
||||
0, 0, 0, 0, 0, 1};
|
||||
float IMU_QuaternionEKF_P[36] = {100000, 0.1, 0.1, 0.1, 0.1, 0.1,
|
||||
0.1, 100000, 0.1, 0.1, 0.1, 0.1,
|
||||
0.1, 0.1, 100000, 0.1, 0.1, 0.1,
|
||||
0.1, 0.1, 0.1, 100000, 0.1, 0.1,
|
||||
0.1, 0.1, 0.1, 0.1, 100, 0.1,
|
||||
0.1, 0.1, 0.1, 0.1, 0.1, 100};
|
||||
float IMU_QuaternionEKF_K[18];
|
||||
float IMU_QuaternionEKF_H[18];
|
||||
|
||||
static float invSqrt(float x);
|
||||
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf);
|
||||
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
|
||||
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
|
||||
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
|
||||
|
||||
/**
|
||||
* @brief Quaternion EKF initialization and some reference value
|
||||
* @param[in] process_noise1 quaternion process noise 10
|
||||
* @param[in] process_noise2 gyro bias process noise 0.001
|
||||
* @param[in] measure_noise accel measure noise 1000000
|
||||
* @param[in] lambda fading coefficient 0.9996
|
||||
* @param[in] lpf lowpass filter coefficient 0
|
||||
*/
|
||||
void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf)
|
||||
{
|
||||
QEKF_INS.Initialized = 1;
|
||||
QEKF_INS.Q1 = process_noise1;
|
||||
QEKF_INS.Q2 = process_noise2;
|
||||
QEKF_INS.R = measure_noise;
|
||||
QEKF_INS.ChiSquareTestThreshold = 1e-8;
|
||||
QEKF_INS.ConvergeFlag = 0;
|
||||
QEKF_INS.ErrorCount = 0;
|
||||
QEKF_INS.UpdateCount = 0;
|
||||
if (lambda > 1)
|
||||
{
|
||||
lambda = 1;
|
||||
}
|
||||
QEKF_INS.lambda = lambda;
|
||||
QEKF_INS.accLPFcoef = lpf;
|
||||
|
||||
// 初始化矩阵维度信息
|
||||
Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3);
|
||||
Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
|
||||
|
||||
// 姿态初始化
|
||||
QEKF_INS.IMU_QuaternionEKF.xhat_data[0] = 1;
|
||||
QEKF_INS.IMU_QuaternionEKF.xhat_data[1] = 0;
|
||||
QEKF_INS.IMU_QuaternionEKF.xhat_data[2] = 0;
|
||||
QEKF_INS.IMU_QuaternionEKF.xhat_data[3] = 0;
|
||||
|
||||
// 自定义函数初始化,用于扩展或增加kf的基础功能
|
||||
QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
|
||||
QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading;
|
||||
QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH;
|
||||
QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate;
|
||||
|
||||
// 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合)
|
||||
QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE;
|
||||
QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE;
|
||||
|
||||
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
|
||||
memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Quaternion EKF update
|
||||
* @param[in] gyro x y z in rad/s
|
||||
* @param[in] accel x y z in m/s²
|
||||
* @param[in] update period in s
|
||||
*/
|
||||
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt)
|
||||
{
|
||||
// 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
|
||||
static float halfgxdt, halfgydt, halfgzdt;
|
||||
static float accelInvNorm;
|
||||
if (!QEKF_INS.Initialized)
|
||||
{
|
||||
IMU_QuaternionEKF_Init(10, 0.001, 1000000 * 10, 0.9996 * 0 + 1, 0);
|
||||
}
|
||||
|
||||
/* F, number with * represent vals to be set
|
||||
0 1* 2* 3* 4 5
|
||||
6* 7 8* 9* 10 11
|
||||
12* 13* 14 15* 16 17
|
||||
18* 19* 20* 21 22 23
|
||||
24 25 26 27 28 29
|
||||
30 31 32 33 34 35
|
||||
*/
|
||||
QEKF_INS.dt = dt;
|
||||
|
||||
QEKF_INS.Gyro[0] = gx - QEKF_INS.GyroBias[0];
|
||||
QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
|
||||
QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];
|
||||
|
||||
// set F
|
||||
halfgxdt = 0.5f * QEKF_INS.Gyro[0] * dt;
|
||||
halfgydt = 0.5f * QEKF_INS.Gyro[1] * dt;
|
||||
halfgzdt = 0.5f * QEKF_INS.Gyro[2] * dt;
|
||||
|
||||
// 此部分设定状态转移矩阵F的左上角部分 4x4子矩阵,即0.5(Ohm-Ohm^bias)*deltaT,右下角有一个2x2单位阵已经初始化好了
|
||||
// 注意在predict步F的右上角是4x2的零矩阵,因此每次predict的时候都会调用memcpy用单位阵覆盖前一轮线性化后的矩阵
|
||||
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
|
||||
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt;
|
||||
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt;
|
||||
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt;
|
||||
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt;
|
||||
QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
|
||||
|
||||
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
|
||||
if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
|
||||
{
|
||||
QEKF_INS.Accel[0] = ax;
|
||||
QEKF_INS.Accel[1] = ay;
|
||||
QEKF_INS.Accel[2] = az;
|
||||
}
|
||||
QEKF_INS.Accel[0] = QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
|
||||
QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
|
||||
QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
|
||||
|
||||
// set z,单位化重力加速度向量
|
||||
accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]);
|
||||
for (uint8_t i = 0; i < 3; i++)
|
||||
{
|
||||
QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
|
||||
}
|
||||
|
||||
// get body state
|
||||
QEKF_INS.gyro_norm = 1.0f / invSqrt(QEKF_INS.Gyro[0] * QEKF_INS.Gyro[0] +
|
||||
QEKF_INS.Gyro[1] * QEKF_INS.Gyro[1] +
|
||||
QEKF_INS.Gyro[2] * QEKF_INS.Gyro[2]);
|
||||
QEKF_INS.accl_norm = 1.0f / accelInvNorm;
|
||||
|
||||
// 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
|
||||
// 稍后在最后的姿态更新部分会利用StableFlag来确定
|
||||
if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f && QEKF_INS.accl_norm < 9.8f + 0.5f)
|
||||
{
|
||||
QEKF_INS.StableFlag = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
QEKF_INS.StableFlag = 0;
|
||||
}
|
||||
|
||||
// set Q R,过程噪声和观测噪声矩阵
|
||||
QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
|
||||
QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
|
||||
QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
|
||||
QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
|
||||
QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
|
||||
QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
|
||||
QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R;
|
||||
QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
|
||||
QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;
|
||||
|
||||
// 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
|
||||
Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);
|
||||
|
||||
// 获取融合后的数据,包括四元数和xy零飘值
|
||||
QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
|
||||
QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
|
||||
QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
|
||||
QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];
|
||||
QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
|
||||
QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
|
||||
QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移
|
||||
|
||||
// 利用四元数反解欧拉角
|
||||
QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
|
||||
QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
|
||||
QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
|
||||
|
||||
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
|
||||
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
|
||||
{
|
||||
QEKF_INS.YawRoundCount--;
|
||||
}
|
||||
else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f)
|
||||
{
|
||||
QEKF_INS.YawRoundCount++;
|
||||
}
|
||||
QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
|
||||
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
|
||||
QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新;
|
||||
* 并对零漂的方差进行限制,防止过度收敛并限幅防止发散
|
||||
*
|
||||
* @param kf
|
||||
*/
|
||||
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
|
||||
{
|
||||
static float q0, q1, q2, q3;
|
||||
static float qInvNorm;
|
||||
|
||||
q0 = kf->xhatminus_data[0];
|
||||
q1 = kf->xhatminus_data[1];
|
||||
q2 = kf->xhatminus_data[2];
|
||||
q3 = kf->xhatminus_data[3];
|
||||
|
||||
// quaternion normalize
|
||||
qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
for (uint8_t i = 0; i < 4; i++)
|
||||
{
|
||||
kf->xhatminus_data[i] *= qInvNorm;
|
||||
}
|
||||
/* F, number with * represent vals to be set
|
||||
0 1 2 3 4* 5*
|
||||
6 7 8 9 10* 11*
|
||||
12 13 14 15 16* 17*
|
||||
18 19 20 21 22* 23*
|
||||
24 25 26 27 28 29
|
||||
30 31 32 33 34 35
|
||||
*/
|
||||
// set F
|
||||
kf->F_data[4] = q1 * QEKF_INS.dt / 2;
|
||||
kf->F_data[5] = q2 * QEKF_INS.dt / 2;
|
||||
|
||||
kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
|
||||
kf->F_data[11] = q3 * QEKF_INS.dt / 2;
|
||||
|
||||
kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
|
||||
kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
|
||||
|
||||
kf->F_data[22] = q2 * QEKF_INS.dt / 2;
|
||||
kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
|
||||
|
||||
// fading filter,防止零飘参数过度收敛
|
||||
kf->P_data[28] /= QEKF_INS.lambda;
|
||||
kf->P_data[35] /= QEKF_INS.lambda;
|
||||
|
||||
// 限幅,防止发散
|
||||
if (kf->P_data[28] > 10000)
|
||||
{
|
||||
kf->P_data[28] = 10000;
|
||||
}
|
||||
if (kf->P_data[35] > 10000)
|
||||
{
|
||||
kf->P_data[35] = 10000;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
|
||||
*
|
||||
* @param kf
|
||||
*/
|
||||
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf)
|
||||
{
|
||||
static float doubleq0, doubleq1, doubleq2, doubleq3;
|
||||
/* H
|
||||
0 1 2 3 4 5
|
||||
6 7 8 9 10 11
|
||||
12 13 14 15 16 17
|
||||
last two cols are zero
|
||||
*/
|
||||
// set H
|
||||
doubleq0 = 2 * kf->xhatminus_data[0];
|
||||
doubleq1 = 2 * kf->xhatminus_data[1];
|
||||
doubleq2 = 2 * kf->xhatminus_data[2];
|
||||
doubleq3 = 2 * kf->xhatminus_data[3];
|
||||
|
||||
memset(kf->H_data, 0, sizeof_float * kf->zSize * kf->xhatSize);
|
||||
|
||||
kf->H_data[0] = -doubleq2;
|
||||
kf->H_data[1] = doubleq3;
|
||||
kf->H_data[2] = -doubleq0;
|
||||
kf->H_data[3] = doubleq1;
|
||||
|
||||
kf->H_data[6] = doubleq1;
|
||||
kf->H_data[7] = doubleq0;
|
||||
kf->H_data[8] = doubleq3;
|
||||
kf->H_data[9] = doubleq2;
|
||||
|
||||
kf->H_data[12] = doubleq0;
|
||||
kf->H_data[13] = -doubleq1;
|
||||
kf->H_data[14] = -doubleq2;
|
||||
kf->H_data[15] = doubleq3;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 利用观测值和先验估计得到最优的后验估计
|
||||
* 加入了卡方检验以判断融合加速度的条件是否满足
|
||||
* 同时引入发散保护保证恶劣工况下的必要量测更新
|
||||
*
|
||||
* @param kf
|
||||
*/
|
||||
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
|
||||
{
|
||||
static float q0, q1, q2, q3;
|
||||
|
||||
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
|
||||
kf->temp_matrix.numRows = kf->H.numRows;
|
||||
kf->temp_matrix.numCols = kf->Pminus.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
|
||||
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
|
||||
kf->temp_matrix1.numCols = kf->HT.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
|
||||
kf->S.numRows = kf->R.numRows;
|
||||
kf->S.numCols = kf->R.numCols;
|
||||
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
|
||||
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
|
||||
|
||||
q0 = kf->xhatminus_data[0];
|
||||
q1 = kf->xhatminus_data[1];
|
||||
q2 = kf->xhatminus_data[2];
|
||||
q3 = kf->xhatminus_data[3];
|
||||
|
||||
kf->temp_vector.numRows = kf->H.numRows;
|
||||
kf->temp_vector.numCols = 1;
|
||||
// 计算预测得到的重力加速度方向(通过姿态获取的)
|
||||
kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
|
||||
kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
|
||||
kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
|
||||
|
||||
// 计算预测值和各个轴的方向余弦
|
||||
for (uint8_t i = 0; i < 3; i++)
|
||||
{
|
||||
QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
|
||||
}
|
||||
|
||||
// 利用加速度计数据修正
|
||||
kf->temp_vector1.numRows = kf->z.numRows;
|
||||
kf->temp_vector1.numCols = 1;
|
||||
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k))
|
||||
|
||||
// chi-square test,卡方检验
|
||||
kf->temp_matrix.numRows = kf->temp_vector1.numRows;
|
||||
kf->temp_matrix.numCols = 1;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix); // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k)))
|
||||
kf->temp_vector.numRows = 1;
|
||||
kf->temp_vector.numCols = kf->temp_vector1.numRows;
|
||||
kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))'
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare);
|
||||
// rk is small,filter converged/converging
|
||||
if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold)
|
||||
{
|
||||
QEKF_INS.ConvergeFlag = 1;
|
||||
}
|
||||
// rk is bigger than thre but once converged
|
||||
if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
|
||||
{
|
||||
if (QEKF_INS.StableFlag)
|
||||
{
|
||||
QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验
|
||||
}
|
||||
else
|
||||
{
|
||||
QEKF_INS.ErrorCount = 0;
|
||||
}
|
||||
|
||||
if (QEKF_INS.ErrorCount > 50)
|
||||
{
|
||||
// 滤波器发散
|
||||
QEKF_INS.ConvergeFlag = 0;
|
||||
kf->SkipEq5 = FALSE; // step-5 is cov mat P updating
|
||||
}
|
||||
else
|
||||
{
|
||||
// 残差未通过卡方检验 仅预测
|
||||
// xhat(k) = xhat'(k)
|
||||
// P(k) = P'(k)
|
||||
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
|
||||
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||
kf->SkipEq5 = TRUE; // part5 is P updating
|
||||
return;
|
||||
}
|
||||
}
|
||||
else // if divergent or rk is not that big/acceptable,use adaptive gain
|
||||
{
|
||||
// scale adaptive,rk越小则增益越大,否则更相信预测值
|
||||
if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
|
||||
{
|
||||
QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (0.9f * QEKF_INS.ChiSquareTestThreshold);
|
||||
}
|
||||
else
|
||||
{
|
||||
QEKF_INS.AdaptiveGainScale = 1;
|
||||
}
|
||||
QEKF_INS.ErrorCount = 0;
|
||||
kf->SkipEq5 = FALSE;
|
||||
}
|
||||
|
||||
// cal kf-gain K
|
||||
kf->temp_matrix.numRows = kf->Pminus.numRows;
|
||||
kf->temp_matrix.numCols = kf->HT.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
|
||||
|
||||
// implement adaptive
|
||||
for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++)
|
||||
{
|
||||
kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
|
||||
}
|
||||
for (uint8_t i = 4; i < 6; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < 3; j++)
|
||||
{
|
||||
kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
|
||||
}
|
||||
}
|
||||
|
||||
kf->temp_vector.numRows = kf->K.numRows;
|
||||
kf->temp_vector.numCols = 1;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
|
||||
|
||||
// 零漂修正限幅,一般不会有过大的漂移
|
||||
if (QEKF_INS.ConvergeFlag)
|
||||
{
|
||||
for (uint8_t i = 4; i < 6; i++)
|
||||
{
|
||||
if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt)
|
||||
{
|
||||
kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
|
||||
}
|
||||
if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt)
|
||||
{
|
||||
kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 不修正yaw轴数据
|
||||
kf->temp_vector.pData[3] = 0;
|
||||
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief EKF观测环节,其实就是把数据复制一下
|
||||
*
|
||||
* @param kf kf类型定义
|
||||
*/
|
||||
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf)
|
||||
{
|
||||
memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P));
|
||||
memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K));
|
||||
memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 自定义1/sqrt(x),速度更快
|
||||
*
|
||||
* @param x x
|
||||
* @return float
|
||||
*/
|
||||
static float invSqrt(float x)
|
||||
{
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long *)&y;
|
||||
i = 0x5f375a86 - (i >> 1);
|
||||
y = *(float *)&i;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
return y;
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file QuaternionEKF.h
|
||||
* @author Wang Hongxi
|
||||
* @version V1.2.0
|
||||
* @date 2022/3/8
|
||||
* @brief attitude update with gyro bias estimate and chi-square test
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
#ifndef _QUAT_EKF_H
|
||||
#define _QUAT_EKF_H
|
||||
#include "kalman_filter.h"
|
||||
|
||||
/* boolean type definitions */
|
||||
#ifndef TRUE
|
||||
#define TRUE 1 /**< boolean true */
|
||||
#endif
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE 0 /**< boolean fails */
|
||||
#endif
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t Initialized;
|
||||
KalmanFilter_t IMU_QuaternionEKF;
|
||||
uint8_t ConvergeFlag;
|
||||
uint8_t StableFlag;
|
||||
uint64_t ErrorCount;
|
||||
uint64_t UpdateCount;
|
||||
|
||||
float q[4]; // 四元数估计值
|
||||
float GyroBias[3]; // 陀螺仪零偏估计值
|
||||
|
||||
float Gyro[3];
|
||||
float Accel[3];
|
||||
|
||||
float OrientationCosine[3];
|
||||
|
||||
float accLPFcoef;
|
||||
float gyro_norm;
|
||||
float accl_norm;
|
||||
float AdaptiveGainScale;
|
||||
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
|
||||
float YawTotalAngle;
|
||||
|
||||
float Q1; // 四元数更新过程噪声
|
||||
float Q2; // 陀螺仪零偏过程噪声
|
||||
float R; // 加速度计量测噪声
|
||||
|
||||
float dt; // 姿态更新周期
|
||||
mat ChiSquare;
|
||||
float ChiSquare_Data[1]; // 卡方检验检测函数
|
||||
float ChiSquareTestThreshold; // 卡方检验阈值
|
||||
float lambda; // 渐消因子
|
||||
|
||||
int16_t YawRoundCount;
|
||||
|
||||
float YawAngleLast;
|
||||
} QEKF_INS_t;
|
||||
|
||||
extern QEKF_INS_t QEKF_INS;
|
||||
extern float chiSquare;
|
||||
extern float ChiSquareTestThreshold;
|
||||
void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
|
||||
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,152 @@
|
||||
/* =============================================================================
|
||||
* ins.c - 惯导姿态解算封装实现(见 ins.h 说明)
|
||||
* 移植自 RoboMaster-C-Board-INS-Example-master 的 ins_task.c:
|
||||
* - 去除 BMI088driver/controller/bsp_PWM/bsp_dwt 依赖,数据源改为本工程
|
||||
* 的 bmi088 驱动全局量,温度控制仍由 IMU_Task 负责
|
||||
* - dt 由调用方传入(IMU_Task 固定 1ms),不再使用 DWT 计时
|
||||
* - 保留运动加速度解算(低通 0.0085),供后续功能扩展(如功率/跌倒检测)
|
||||
* 欧拉角约定(重要):
|
||||
* QEKF 内部变量命名与航空惯例互换——QEKF_INS.Pitch 是绕 x 轴的角,
|
||||
* QEKF_INS.Roll 是绕 y 轴的角;attitude.c 按"本工程 Pitch 反馈=QEKF.Pitch"
|
||||
* 映射(与旧 Mahony 方案的 get_roll 语义一致),勿凭名字想当然
|
||||
* ========================================================================== */
|
||||
#include "ins.h"
|
||||
#include "QuaternionEKF.h"
|
||||
#include "bmi088.h"
|
||||
#include "main.h"
|
||||
#include <string.h>
|
||||
|
||||
/* z 轴静止零偏标定参数:QEKF 无法观测 z 偏置(见 ins.h),上电采 1s */
|
||||
#define INS_Z_CAL_SAMPLES 1000
|
||||
static float gyro_z_offset = 0;
|
||||
static uint8_t z_cal_done = 0;
|
||||
|
||||
INS_t INS;
|
||||
|
||||
static const float xb[3] = {1, 0, 0};
|
||||
static const float yb[3] = {0, 1, 0};
|
||||
static const float zb[3] = {0, 0, 1};
|
||||
|
||||
void ins_init(void)
|
||||
{
|
||||
memset(&INS, 0, sizeof(INS));
|
||||
|
||||
/* z 轴静止零偏标定:上电静止采 1s 取平均(x/y 偏置由 EKF 在线估计,
|
||||
* 无需校准;此期间 ins_update() 不输出,控制任务由 ins_ready() 门控) */
|
||||
float sum = 0;
|
||||
for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
|
||||
{
|
||||
sum += bmi088_gyro[2];
|
||||
HAL_Delay(1);
|
||||
}
|
||||
gyro_z_offset = sum / INS_Z_CAL_SAMPLES;
|
||||
z_cal_done = 1;
|
||||
|
||||
INS.Initialized = 1;
|
||||
|
||||
/* 与 INS 例程 demo 相同的参数组合:
|
||||
* 过程噪声1=10(四元数) 过程噪声2=0.001(零偏) 观测噪声=1e7(加速度)
|
||||
* 渐消因子=1(不渐消) 加速度低通=0(内部另有运动加速度低通) */
|
||||
IMU_QuaternionEKF_Init(10, 0.001f, 10000000, 1, 0);
|
||||
}
|
||||
|
||||
uint8_t ins_ready(void)
|
||||
{
|
||||
return z_cal_done;
|
||||
}
|
||||
|
||||
void ins_update(float dt)
|
||||
{
|
||||
if (!z_cal_done)
|
||||
return;
|
||||
|
||||
/* BMI088 驱动在 SPI DMA 完成回调中后台更新,这里直接取最新值;
|
||||
* z 轴减去上电标定的零偏(x/y 由 EKF 在线估计,不再静态校准) */
|
||||
float gyro[3] = {bmi088_gyro[0], bmi088_gyro[1],
|
||||
bmi088_gyro[2] - gyro_z_offset};
|
||||
float accel[3] = {bmi088_accel[0], bmi088_accel[1], bmi088_accel[2]};
|
||||
|
||||
/* 核心函数,EKF 更新四元数(内部含零偏估计与卡方检验) */
|
||||
IMU_QuaternionEKF_Update(gyro[0], gyro[1], gyro[2],
|
||||
accel[0], accel[1], accel[2], dt);
|
||||
|
||||
memcpy(INS.q, QEKF_INS.q, sizeof(INS.q));
|
||||
|
||||
/* 机体系基向量转换到导航系(惯性系=导航系),供扩展功能使用 */
|
||||
float xn[3], yn[3], zn[3];
|
||||
BodyFrameToEarthFrame(xb, xn, INS.q);
|
||||
BodyFrameToEarthFrame(yb, yn, INS.q);
|
||||
BodyFrameToEarthFrame(zb, zn, INS.q);
|
||||
|
||||
/* 将重力从导航系转到机体系,计算运动加速度(低通),供扩展使用 */
|
||||
const float gravity[3] = {0, 0, 9.81f};
|
||||
float gravity_b[3];
|
||||
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
|
||||
const float AccelLPF = 0.0085f;
|
||||
for (uint8_t i = 0; i < 3; i++)
|
||||
{
|
||||
INS.MotionAccel_b[i] = (accel[i] - gravity_b[i]) * dt / (AccelLPF + dt) +
|
||||
INS.MotionAccel_b[i] * AccelLPF / (AccelLPF + dt);
|
||||
}
|
||||
|
||||
/* 获取最终数据(QEKF 内部已含多圈 Yaw 处理) */
|
||||
INS.Yaw = QEKF_INS.Yaw;
|
||||
INS.Pitch = QEKF_INS.Pitch;
|
||||
INS.Roll = QEKF_INS.Roll;
|
||||
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
|
||||
|
||||
INS.GyroBias[0] = QEKF_INS.GyroBias[0];
|
||||
INS.GyroBias[1] = QEKF_INS.GyroBias[1];
|
||||
INS.GyroBias[2] = QEKF_INS.GyroBias[2];
|
||||
INS.StableFlag = QEKF_INS.StableFlag;
|
||||
INS.ConvergeFlag = QEKF_INS.ConvergeFlag;
|
||||
INS.ChiSquare = QEKF_INS.ChiSquare_Data[0];
|
||||
INS.UpdateCount = QEKF_INS.UpdateCount;
|
||||
|
||||
(void)xn;
|
||||
(void)yn;
|
||||
(void)zn;
|
||||
(void)zb;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Transform 3dvector from BodyFrame to EarthFrame
|
||||
* @param[1] vector in BodyFrame
|
||||
* @param[2] vector in EarthFrame
|
||||
* @param[3] quaternion
|
||||
*/
|
||||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
|
||||
{
|
||||
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
|
||||
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
|
||||
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
|
||||
|
||||
vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
|
||||
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
|
||||
(q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
|
||||
|
||||
vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
|
||||
(q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
|
||||
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Transform 3dvector from EarthFrame to BodyFrame
|
||||
* @param[1] vector in EarthFrame
|
||||
* @param[2] vector in BodyFrame
|
||||
* @param[3] quaternion
|
||||
*/
|
||||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
|
||||
{
|
||||
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
|
||||
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
|
||||
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
|
||||
|
||||
vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
|
||||
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
|
||||
(q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
|
||||
|
||||
vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
|
||||
(q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
|
||||
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
/* =============================================================================
|
||||
* ins.h - 惯导姿态解算封装(移植自 RoboMaster-C-Board-INS-Example, Wang Hongxi)
|
||||
* 核心: QuaternionEKF 四元数 EKF——陀螺积分预测 + 加速度重力方向观测,
|
||||
* 在线估计 x/y 陀螺零偏,卡方检验剔除加速度异常(撞击/运动)帧
|
||||
* 对比旧 Mahony 方案: 收敛更快、零偏在线估计(无需上电静止校准)、
|
||||
* 动态加速度干扰抑制更好(卡方检验)
|
||||
* 数据源: 本工程 BMI088 驱动的 bmi088_accel/gyro(SPI DMA 后台更新)
|
||||
* 调用: ins_init() 一次,ins_update(dt) 每 1ms;dt 用固定 0.001
|
||||
* (IMU_Task 为 vTaskDelayUntil 精确 1kHz 节拍,无需 DWT 计时)
|
||||
* ========================================================================== */
|
||||
#ifndef INS_H
|
||||
#define INS_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t Initialized;
|
||||
|
||||
float Gyro[3]; // rad/s(零偏修正后)
|
||||
float Accel[3]; // m/s^2
|
||||
float q[4]; // 四元数 w x y z
|
||||
|
||||
float Yaw; // -180~180(°)
|
||||
float Pitch; // QEKF 命名(实际为绕 x 轴角,本工程映射见 attitude.c)
|
||||
float Roll; // 同上
|
||||
float YawTotalAngle; // 多圈连续 Yaw(°)
|
||||
|
||||
float GyroBias[3]; // EKF 在线估计的陀螺零偏(rad/s)
|
||||
float MotionAccel_b[3]; // 运动加速度(机体系,去除重力,低通)
|
||||
uint8_t StableFlag; // 1-载体静止且加速度正常(可用于 ZUPT 等扩展)
|
||||
uint8_t ConvergeFlag; // 1-卡方检验收敛(姿态可信)
|
||||
float ChiSquare; // 最近一次卡方检验值(诊断用)
|
||||
uint32_t UpdateCount; // 更新计数(诊断用)
|
||||
} INS_t;
|
||||
|
||||
extern INS_t INS;
|
||||
|
||||
/* 初始化:先做 z 轴静止零偏标定(约 1s 阻塞,车体必须静止——QEKF 的
|
||||
* 卡方观测无法感知 z 偏置,GyroBias[2] 恒为 0,不标定会导致 Yaw 持续漂移),
|
||||
* 再初始化 EKF(参数为 INS 例程 demo 值,调参见 QuaternionEKF.c 头注释) */
|
||||
extern void ins_init(void);
|
||||
|
||||
/* 姿态更新(1kHz 调用,dt 单位秒;内部读 bmi088_accel/gyro 全局量)
|
||||
* 标定完成前直接返回,不更新 */
|
||||
extern void ins_update(float dt);
|
||||
|
||||
/* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */
|
||||
extern uint8_t ins_ready(void);
|
||||
|
||||
/* 坐标系变换工具(机体系 <-> 导航系) */
|
||||
extern void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
|
||||
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,480 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file kalman filter.c
|
||||
* @author Wang Hongxi
|
||||
* @version V1.2.2
|
||||
* @date 2022/1/8
|
||||
* @brief C implementation of kalman filter
|
||||
******************************************************************************
|
||||
* @attention
|
||||
* 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。
|
||||
* This implementation of kalman filter can dynamically adjust dimension and
|
||||
* value of matrix H R and K according to the measurement validity under any
|
||||
* circumstance that the sampling rate of component sensors are different.
|
||||
*
|
||||
* 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
|
||||
* 入传感器量测所对应的状态与这个量测的方式，详情请见例程
|
||||
* Therefore, the initialization of matrix P, F, and Q is sometimes different
|
||||
* from that of matrices H R. when initialization. Additionally, the corresponding
|
||||
* state and the method of the measurement should be provided when initializing
|
||||
* measurement vector z. For more details, please see the example.
|
||||
*
|
||||
* 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初
|
||||
* 始化矩阵P那样用常规方式初始化z H R即可。
|
||||
* If automatic adjustment is not required, assign zero to the UseAutoAdjustment
|
||||
* and initialize z H R in the normal way as matrix P.
|
||||
*
|
||||
* 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼
|
||||
* 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
|
||||
* MeasuredVector and ControlVector are required to be updated in the sensor
|
||||
* callback function. Integer 0 in measurement vector z indicates the invalidity
|
||||
* of current measurement, so MeasuredVector and ControlVector will be reset
|
||||
* (to 0) during each update.
|
||||
*
|
||||
* 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法
|
||||
* 通过限制矩阵P最小值的方法，可有效抑制滤波器的过度收敛，详情请见例程。
|
||||
* Additionally, the excessive convergence of matrix P will make filter incapable
|
||||
* of adopting the slowly changing state. This implementation can effectively
|
||||
* suppress filter excessive convergence through boundary limiting for matrix P.
|
||||
* For more details, please see the example.
|
||||
*
|
||||
* @example:
|
||||
* x =
|
||||
* | height |
|
||||
* | velocity |
|
||||
* |acceleration|
|
||||
*
|
||||
* KalmanFilter_t Height_KF;
|
||||
*
|
||||
* void INS_Task_Init(void)
|
||||
* {
|
||||
* static float P_Init[9] =
|
||||
* {
|
||||
* 10, 0, 0,
|
||||
* 0, 30, 0,
|
||||
* 0, 0, 10,
|
||||
* };
|
||||
* static float F_Init[9] =
|
||||
* {
|
||||
* 1, dt, 0.5*dt*dt,
|
||||
* 0, 1, dt,
|
||||
* 0, 0, 1,
|
||||
* };
|
||||
* static float Q_Init[9] =
|
||||
* {
|
||||
* 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
|
||||
* 0.5*dt*dt*dt, dt*dt, dt,
|
||||
* 0.5*dt*dt, dt, 1,
|
||||
* };
|
||||
*
|
||||
* // 设置最小方差
|
||||
* static float state_min_variance[3] = {0.03, 0.005, 0.1};
|
||||
*
|
||||
* // 开启自动调整
|
||||
* Height_KF.UseAutoAdjustment = 1;
|
||||
*
|
||||
* // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度
|
||||
* static uint8_t measurement_reference[3] = {1, 1, 3}
|
||||
*
|
||||
* static float measurement_degree[3] = {1, 1, 1}
|
||||
* // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下）
|
||||
* |1 0 0|
|
||||
* |1 0 0|
|
||||
* |0 0 1|
|
||||
*
|
||||
* static float mat_R_diagonal_elements = {30, 25, 35}
|
||||
* //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下）
|
||||
* |30 0 0|
|
||||
* | 0 25 0|
|
||||
* | 0 0 35|
|
||||
*
|
||||
* Kalman_Filter_Init(&Height_KF, 3, 0, 3);
|
||||
*
|
||||
* // 设置矩阵值
|
||||
* memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
|
||||
* memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
|
||||
* memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
|
||||
* memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference));
|
||||
* memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
|
||||
* memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements));
|
||||
* memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance));
|
||||
* }
|
||||
*
|
||||
* void INS_Task(void const *pvParameters)
|
||||
* {
|
||||
* // 循环更新
|
||||
* Kalman_Filter_Update(&Height_KF);
|
||||
* vTaskDelay(ts);
|
||||
* }
|
||||
*
|
||||
* // 测量数据更新应按照以下形式 即向MeasuredVector赋值
|
||||
* void Barometer_Read_Over(void)
|
||||
* {
|
||||
* ......
|
||||
* INS_KF.MeasuredVector[0] = baro_height;
|
||||
* }
|
||||
* void GPS_Read_Over(void)
|
||||
* {
|
||||
* ......
|
||||
* INS_KF.MeasuredVector[1] = GPS_height;
|
||||
* }
|
||||
* void Acc_Data_Process(void)
|
||||
* {
|
||||
* ......
|
||||
* INS_KF.MeasuredVector[2] = acc.z;
|
||||
* }
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#include "kalman_filter.h"
|
||||
|
||||
uint16_t sizeof_float, sizeof_double;
|
||||
|
||||
static void H_K_R_Adjustment(KalmanFilter_t *kf);
|
||||
|
||||
/**
|
||||
* @brief 初始化矩阵维度信息并为矩阵分配空间
|
||||
*
|
||||
* @param kf kf类型定义
|
||||
* @param xhatSize 状态变量维度
|
||||
* @param uSize 控制变量维度
|
||||
* @param zSize 观测量维度
|
||||
*/
|
||||
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize)
|
||||
{
|
||||
sizeof_float = sizeof(float);
|
||||
sizeof_double = sizeof(double);
|
||||
|
||||
kf->xhatSize = xhatSize;
|
||||
kf->uSize = uSize;
|
||||
kf->zSize = zSize;
|
||||
|
||||
kf->MeasurementValidNum = 0;
|
||||
|
||||
// measurement flags
|
||||
kf->MeasurementMap = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
|
||||
memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize);
|
||||
kf->MeasurementDegree = (float *)user_malloc(sizeof_float * zSize);
|
||||
memset(kf->MeasurementDegree, 0, sizeof_float * zSize);
|
||||
kf->MatR_DiagonalElements = (float *)user_malloc(sizeof_float * zSize);
|
||||
memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize);
|
||||
kf->StateMinVariance = (float *)user_malloc(sizeof_float * xhatSize);
|
||||
memset(kf->StateMinVariance, 0, sizeof_float * xhatSize);
|
||||
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
|
||||
memset(kf->temp, 0, sizeof(uint8_t) * zSize);
|
||||
|
||||
// filter data
|
||||
kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize);
|
||||
memset(kf->FilteredValue, 0, sizeof_float * xhatSize);
|
||||
kf->MeasuredVector = (float *)user_malloc(sizeof_float * zSize);
|
||||
memset(kf->MeasuredVector, 0, sizeof_float * zSize);
|
||||
kf->ControlVector = (float *)user_malloc(sizeof_float * uSize);
|
||||
memset(kf->ControlVector, 0, sizeof_float * uSize);
|
||||
|
||||
// xhat x(k|k)
|
||||
kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize);
|
||||
memset(kf->xhat_data, 0, sizeof_float * xhatSize);
|
||||
Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *)kf->xhat_data);
|
||||
|
||||
// xhatminus x(k|k-1)
|
||||
kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize);
|
||||
memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
|
||||
Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data);
|
||||
|
||||
if (uSize != 0)
|
||||
{
|
||||
// control vector u
|
||||
kf->u_data = (float *)user_malloc(sizeof_float * uSize);
|
||||
memset(kf->u_data, 0, sizeof_float * uSize);
|
||||
Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data);
|
||||
}
|
||||
|
||||
// measurement vector z
|
||||
kf->z_data = (float *)user_malloc(sizeof_float * zSize);
|
||||
memset(kf->z_data, 0, sizeof_float * zSize);
|
||||
Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data);
|
||||
|
||||
// covariance matrix P(k|k)
|
||||
kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||
memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||
Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data);
|
||||
|
||||
// create covariance matrix P(k|k-1)
|
||||
kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||
memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||
Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data);
|
||||
|
||||
// state transition matrix F FT
|
||||
kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||
kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||
memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||
memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||
Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data);
|
||||
Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data);
|
||||
|
||||
if (uSize != 0)
|
||||
{
|
||||
// control matrix B
|
||||
kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize);
|
||||
memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
|
||||
Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
|
||||
}
|
||||
|
||||
// measurement matrix H
|
||||
kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize);
|
||||
kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
|
||||
memset(kf->H_data, 0, sizeof_float * zSize * xhatSize);
|
||||
memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize);
|
||||
Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data);
|
||||
Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data);
|
||||
|
||||
// process noise covariance matrix Q
|
||||
kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||
memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||
Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data);
|
||||
|
||||
// measurement noise covariance matrix R
|
||||
kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize);
|
||||
memset(kf->R_data, 0, sizeof_float * zSize * zSize);
|
||||
Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data);
|
||||
|
||||
// kalman gain K
|
||||
kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
|
||||
memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
|
||||
Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data);
|
||||
|
||||
kf->S_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||
kf->temp_matrix_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||
kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||
kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize);
|
||||
kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize);
|
||||
Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data);
|
||||
Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data);
|
||||
Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1);
|
||||
Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data);
|
||||
Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1);
|
||||
|
||||
kf->SkipEq1 = 0;
|
||||
kf->SkipEq2 = 0;
|
||||
kf->SkipEq3 = 0;
|
||||
kf->SkipEq4 = 0;
|
||||
kf->SkipEq5 = 0;
|
||||
}
|
||||
|
||||
void Kalman_Filter_Measure(KalmanFilter_t *kf)
|
||||
{
|
||||
// 矩阵H K R根据量测情况自动调整
|
||||
// matrix H K R auto adjustment
|
||||
if (kf->UseAutoAdjustment != 0)
|
||||
H_K_R_Adjustment(kf);
|
||||
else
|
||||
{
|
||||
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
|
||||
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
|
||||
}
|
||||
|
||||
memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize);
|
||||
}
|
||||
|
||||
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf)
|
||||
{
|
||||
if (!kf->SkipEq1)
|
||||
{
|
||||
if (kf->uSize > 0)
|
||||
{
|
||||
kf->temp_vector.numRows = kf->xhatSize;
|
||||
kf->temp_vector.numCols = 1;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
|
||||
kf->temp_vector1.numRows = kf->xhatSize;
|
||||
kf->temp_vector1.numCols = 1;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1);
|
||||
kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
|
||||
}
|
||||
else
|
||||
{
|
||||
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf)
|
||||
{
|
||||
if (!kf->SkipEq2)
|
||||
{
|
||||
kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT);
|
||||
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus);
|
||||
kf->temp_matrix.numRows = kf->Pminus.numRows;
|
||||
kf->temp_matrix.numCols = kf->FT.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT
|
||||
kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus);
|
||||
}
|
||||
}
|
||||
void Kalman_Filter_SetK(KalmanFilter_t *kf)
|
||||
{
|
||||
if (!kf->SkipEq3)
|
||||
{
|
||||
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
|
||||
kf->temp_matrix.numRows = kf->H.numRows;
|
||||
kf->temp_matrix.numCols = kf->Pminus.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
|
||||
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
|
||||
kf->temp_matrix1.numCols = kf->HT.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
|
||||
kf->S.numRows = kf->R.numRows;
|
||||
kf->S.numCols = kf->R.numCols;
|
||||
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
|
||||
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
|
||||
kf->temp_matrix.numRows = kf->Pminus.numRows;
|
||||
kf->temp_matrix.numCols = kf->HT.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
|
||||
}
|
||||
}
|
||||
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
|
||||
{
|
||||
if (!kf->SkipEq4)
|
||||
{
|
||||
kf->temp_vector.numRows = kf->H.numRows;
|
||||
kf->temp_vector.numCols = 1;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k)
|
||||
kf->temp_vector1.numRows = kf->z.numRows;
|
||||
kf->temp_vector1.numCols = 1;
|
||||
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H·xhat'(k)
|
||||
kf->temp_vector.numRows = kf->K.numRows;
|
||||
kf->temp_vector.numCols = 1;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
|
||||
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
|
||||
}
|
||||
}
|
||||
void Kalman_Filter_P_Update(KalmanFilter_t *kf)
|
||||
{
|
||||
if (!kf->SkipEq5)
|
||||
{
|
||||
kf->temp_matrix.numRows = kf->K.numRows;
|
||||
kf->temp_matrix.numCols = kf->H.numCols;
|
||||
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
|
||||
kf->temp_matrix1.numCols = kf->Pminus.numCols;
|
||||
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H
|
||||
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)·H·P'(k)
|
||||
kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
|
||||
*
|
||||
* @param kf kf类型定义
|
||||
* @return float* 返回滤波值
|
||||
*/
|
||||
float *Kalman_Filter_Update(KalmanFilter_t *kf)
|
||||
{
|
||||
// 0. 获取量测信息
|
||||
Kalman_Filter_Measure(kf);
|
||||
if (kf->User_Func0_f != NULL)
|
||||
kf->User_Func0_f(kf);
|
||||
|
||||
// 先验估计
|
||||
// 1. xhat'(k)= A·xhat(k-1) + B·u
|
||||
Kalman_Filter_xhatMinusUpdate(kf);
|
||||
if (kf->User_Func1_f != NULL)
|
||||
kf->User_Func1_f(kf);
|
||||
|
||||
// 预测更新
|
||||
// 2. P'(k) = A·P(k-1)·AT + Q
|
||||
Kalman_Filter_PminusUpdate(kf);
|
||||
if (kf->User_Func2_f != NULL)
|
||||
kf->User_Func2_f(kf);
|
||||
|
||||
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
|
||||
{
|
||||
// 量测更新
|
||||
// 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
|
||||
Kalman_Filter_SetK(kf);
|
||||
|
||||
if (kf->User_Func3_f != NULL)
|
||||
kf->User_Func3_f(kf);
|
||||
|
||||
// 融合
|
||||
// 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k))
|
||||
Kalman_Filter_xhatUpdate(kf);
|
||||
|
||||
if (kf->User_Func4_f != NULL)
|
||||
kf->User_Func4_f(kf);
|
||||
|
||||
// 修正方差
|
||||
// 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k)
|
||||
Kalman_Filter_P_Update(kf);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 无有效量测,仅预测
|
||||
// xhat(k) = xhat'(k)
|
||||
// P(k) = P'(k)
|
||||
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
|
||||
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||
}
|
||||
|
||||
// 自定义函数,可以提供后处理等
|
||||
if (kf->User_Func5_f != NULL)
|
||||
kf->User_Func5_f(kf);
|
||||
|
||||
// 避免滤波器过度收敛
|
||||
// suppress filter excessive convergence
|
||||
for (uint8_t i = 0; i < kf->xhatSize; i++)
|
||||
{
|
||||
if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
|
||||
kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
|
||||
}
|
||||
|
||||
memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize);
|
||||
|
||||
if (kf->User_Func6_f != NULL)
|
||||
kf->User_Func6_f(kf);
|
||||
|
||||
return kf->FilteredValue;
|
||||
}
|
||||
|
||||
static void H_K_R_Adjustment(KalmanFilter_t *kf)
|
||||
{
|
||||
kf->MeasurementValidNum = 0;
|
||||
|
||||
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
|
||||
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
|
||||
|
||||
// 识别量测数据有效性并调整矩阵H R K
|
||||
// recognize measurement validity and adjust matrices H R K
|
||||
memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
|
||||
memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
|
||||
for (uint8_t i = 0; i < kf->zSize; i++)
|
||||
{
|
||||
if (kf->z_data[i] != 0)
|
||||
{
|
||||
// 重构向量z
|
||||
// rebuild vector z
|
||||
kf->z_data[kf->MeasurementValidNum] = kf->z_data[i];
|
||||
kf->temp[kf->MeasurementValidNum] = i;
|
||||
// 重构矩阵H
|
||||
// rebuild matrix H
|
||||
kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i];
|
||||
kf->MeasurementValidNum++;
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < kf->MeasurementValidNum; i++)
|
||||
{
|
||||
// 重构矩阵R
|
||||
// rebuild matrix R
|
||||
kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]];
|
||||
}
|
||||
|
||||
// 调整矩阵维数
|
||||
// adjust the dimensions of system matrices
|
||||
kf->H.numRows = kf->MeasurementValidNum;
|
||||
kf->H.numCols = kf->xhatSize;
|
||||
kf->HT.numRows = kf->xhatSize;
|
||||
kf->HT.numCols = kf->MeasurementValidNum;
|
||||
kf->R.numRows = kf->MeasurementValidNum;
|
||||
kf->R.numCols = kf->MeasurementValidNum;
|
||||
kf->K.numRows = kf->xhatSize;
|
||||
kf->K.numCols = kf->MeasurementValidNum;
|
||||
kf->z.numRows = kf->MeasurementValidNum;
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file kalman filter.h
|
||||
* @author Wang Hongxi
|
||||
* @version V1.2.2
|
||||
* @date 2022/1/8
|
||||
* @brief
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
#ifndef __KALMAN_FILTER_H
|
||||
#define __KALMAN_FILTER_H
|
||||
|
||||
// cortex-m4 DSP lib
|
||||
/*
|
||||
#define __CC_ARM // Keil
|
||||
#define ARM_MATH_CM4
|
||||
#define ARM_MATH_MATRIX_CHECK
|
||||
#define ARM_MATH_ROUNDING
|
||||
#define ARM_MATH_DSP // define in arm_math.h
|
||||
*/
|
||||
|
||||
#include "stm32f407xx.h"
|
||||
#include "arm_math.h"
|
||||
//#include "dsp/matrix_functions.h"
|
||||
#include "math.h"
|
||||
#include "stdint.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
#ifndef user_malloc
|
||||
#ifdef _CMSIS_OS_H
|
||||
#define user_malloc pvPortMalloc
|
||||
#else
|
||||
#define user_malloc malloc
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#define mat arm_matrix_instance_f32
|
||||
#define Matrix_Init arm_mat_init_f32
|
||||
#define Matrix_Add arm_mat_add_f32
|
||||
#define Matrix_Subtract arm_mat_sub_f32
|
||||
#define Matrix_Multiply arm_mat_mult_f32
|
||||
#define Matrix_Transpose arm_mat_trans_f32
|
||||
#define Matrix_Inverse arm_mat_inverse_f32
|
||||
|
||||
typedef struct kf_t
|
||||
{
|
||||
float *FilteredValue;
|
||||
float *MeasuredVector;
|
||||
float *ControlVector;
|
||||
|
||||
uint8_t xhatSize;
|
||||
uint8_t uSize;
|
||||
uint8_t zSize;
|
||||
|
||||
uint8_t UseAutoAdjustment;
|
||||
uint8_t MeasurementValidNum;
|
||||
|
||||
uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state
|
||||
float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H
|
||||
float *MatR_DiagonalElements; // 量测方差 variance for each measurement
|
||||
float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence
|
||||
uint8_t *temp;
|
||||
|
||||
// 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个
|
||||
uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5;
|
||||
|
||||
// definiion of struct mat: rows & cols & pointer to vars
|
||||
mat xhat; // x(k|k)
|
||||
mat xhatminus; // x(k|k-1)
|
||||
mat u; // control vector u
|
||||
mat z; // measurement vector z
|
||||
mat P; // covariance matrix P(k|k)
|
||||
mat Pminus; // covariance matrix P(k|k-1)
|
||||
mat F, FT; // state transition matrix F FT
|
||||
mat B; // control matrix B
|
||||
mat H, HT; // measurement matrix H
|
||||
mat Q; // process noise covariance matrix Q
|
||||
mat R; // measurement noise covariance matrix R
|
||||
mat K; // kalman gain K
|
||||
mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1;
|
||||
|
||||
int8_t MatStatus;
|
||||
|
||||
// 用户定义函数,可以替换或扩展基准KF的功能
|
||||
void (*User_Func0_f)(struct kf_t *kf);
|
||||
void (*User_Func1_f)(struct kf_t *kf);
|
||||
void (*User_Func2_f)(struct kf_t *kf);
|
||||
void (*User_Func3_f)(struct kf_t *kf);
|
||||
void (*User_Func4_f)(struct kf_t *kf);
|
||||
void (*User_Func5_f)(struct kf_t *kf);
|
||||
void (*User_Func6_f)(struct kf_t *kf);
|
||||
|
||||
// 矩阵存储空间指针
|
||||
float *xhat_data, *xhatminus_data;
|
||||
float *u_data;
|
||||
float *z_data;
|
||||
float *P_data, *Pminus_data;
|
||||
float *F_data, *FT_data;
|
||||
float *B_data;
|
||||
float *H_data, *HT_data;
|
||||
float *Q_data;
|
||||
float *R_data;
|
||||
float *K_data;
|
||||
float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1;
|
||||
} KalmanFilter_t;
|
||||
|
||||
extern uint16_t sizeof_float, sizeof_double;
|
||||
|
||||
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize);
|
||||
void Kalman_Filter_Measure(KalmanFilter_t *kf);
|
||||
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf);
|
||||
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf);
|
||||
void Kalman_Filter_SetK(KalmanFilter_t *kf);
|
||||
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf);
|
||||
void Kalman_Filter_P_Update(KalmanFilter_t *kf);
|
||||
float *Kalman_Filter_Update(KalmanFilter_t *kf);
|
||||
|
||||
#endif //__KALMAN_FILTER_H
|
||||
@@ -0,0 +1,126 @@
|
||||
/* =============================================================================
|
||||
* dji_motor.c - DJI 电机统一驱动(M3508/M2006/GM6020)
|
||||
* 反馈路径(中断上下文):
|
||||
* dji_motor_init() 注册 CAN 回调 --> can_rx_callback() 按 rx_id 分发
|
||||
* --> dji_motor_parse(): 角度过零判定(+/-4096)多圈累计 total_tick
|
||||
* (无 legacy 的 +-1 圈限制,底盘小陀螺时 Yaw 可连续旋转),
|
||||
* 记录 last_online_tick 供掉线检测
|
||||
* 控制路径(任务上下文):
|
||||
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
|
||||
* dji_motor_send_voltage_high() 0x2FF 电压帧(GM6020 ID5~7)
|
||||
* 注意 0x1FF 帧为 M3508/M2006 高位电流与 GM6020 低位电压共用,按槽位区分
|
||||
* ========================================================================== */
|
||||
#include "dji_motor.h"
|
||||
|
||||
#define DJI_MOTOR_MAX 12
|
||||
|
||||
/* legacy 中的多圈判断以 4000 为阈值;这里改用对称的 ±4096 半圈阈值 */
|
||||
#define DJI_MOTOR_HALF_TURN 4096
|
||||
|
||||
static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
|
||||
static uint8_t dji_motor_count = 0;
|
||||
|
||||
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
|
||||
|
||||
/* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
|
||||
static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
|
||||
{
|
||||
dji_motor_feedback_t * fdb = &motor->fdb;
|
||||
uint16_t now_angle = (uint16_t)(((uint16_t)data[0] << 8) | data[1]);
|
||||
|
||||
if (fdb->last_online_tick != 0)
|
||||
{
|
||||
int16_t diff = (int16_t)now_angle - (int16_t)fdb->raw_angle;
|
||||
if (diff > DJI_MOTOR_HALF_TURN) // 反向过零(如 8100 -> 100)
|
||||
fdb->total_tick += diff - 8192;
|
||||
else if (diff < -DJI_MOTOR_HALF_TURN) // 正向过零(如 100 -> 8100)
|
||||
fdb->total_tick += diff + 8192;
|
||||
else
|
||||
fdb->total_tick += diff;
|
||||
}
|
||||
fdb->raw_angle = now_angle;
|
||||
fdb->rotor_angle_deg = (float)fdb->total_tick * 0.0439453125f; // /8192*360
|
||||
fdb->rotor_speed = (int16_t)(((uint16_t)data[2] << 8) | data[3]);
|
||||
fdb->raw_torque = (int16_t)(((uint16_t)data[4] << 8) | data[5]);
|
||||
fdb->temperature = data[6];
|
||||
|
||||
switch (motor->type)
|
||||
{
|
||||
case DJI_MOTOR_3508:
|
||||
fdb->torque_current = fdb->raw_torque * 0.001220703125f; // /16384*20
|
||||
break;
|
||||
case DJI_MOTOR_2006:
|
||||
fdb->torque_current = fdb->raw_torque * 0.001f; // /10000*10
|
||||
break;
|
||||
case DJI_MOTOR_6020:
|
||||
fdb->torque_current = fdb->raw_torque * -3.0f; // 实际转矩电流,±3A 量程
|
||||
break;
|
||||
}
|
||||
fdb->last_online_tick = HAL_GetTick();
|
||||
}
|
||||
|
||||
void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_motor_type_e type)
|
||||
{
|
||||
motor->type = type;
|
||||
motor->hcan = hcan;
|
||||
motor->rx_id = rx_id;
|
||||
motor->fdb.raw_angle = 0;
|
||||
motor->fdb.total_tick = 0;
|
||||
motor->fdb.last_online_tick = 0; // 0 表示尚未收到反馈
|
||||
|
||||
if (dji_motor_count < DJI_MOTOR_MAX)
|
||||
{
|
||||
dji_motor_list[dji_motor_count++] = motor;
|
||||
can_rx_register(hcan, rx_id, dji_motor_rx_callback);
|
||||
}
|
||||
}
|
||||
|
||||
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
|
||||
{
|
||||
(void)dlc;
|
||||
for (uint8_t i = 0; i < dji_motor_count; i++)
|
||||
{
|
||||
if (dji_motor_list[i]->rx_id == std_id)
|
||||
{
|
||||
dji_motor_parse(dji_motor_list[i], rx_data);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- 控制帧 ---- */
|
||||
|
||||
void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4])
|
||||
{
|
||||
CanCtrlData_s msg;
|
||||
msg.hcan = hcan;
|
||||
msg.tx_header.StdId = std_id;
|
||||
msg.tx_header.IDE = CAN_ID_STD;
|
||||
msg.tx_header.RTR = CAN_RTR_DATA;
|
||||
msg.tx_header.DLC = 8;
|
||||
msg.tx_header.TransmitGlobalTime = DISABLE;
|
||||
for (uint8_t i = 0; i < 4; i++)
|
||||
{
|
||||
msg.tx_data[2 * i] = (uint8_t)(val[i] >> 8);
|
||||
msg.tx_data[2 * i + 1] = (uint8_t)(val[i] & 0xFF);
|
||||
}
|
||||
CAN_SendTxMessage(&msg);
|
||||
}
|
||||
|
||||
void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
|
||||
{
|
||||
const int16_t v[4] = {i1, i2, i3, i4};
|
||||
dji_motor_send_int16x4(hcan, 0x200, v);
|
||||
}
|
||||
|
||||
void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8)
|
||||
{
|
||||
const int16_t v[4] = {i5, i6, i7, i8};
|
||||
dji_motor_send_int16x4(hcan, 0x1FF, v);
|
||||
}
|
||||
|
||||
void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7)
|
||||
{
|
||||
const int16_t v[4] = {v5, v6, v7, 0};
|
||||
dji_motor_send_int16x4(hcan, 0x2FF, v);
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
#ifndef DJI_MOTOR_H
|
||||
#define DJI_MOTOR_H
|
||||
|
||||
#include "bsp_can.h"
|
||||
|
||||
/* DJI 电机(M3508/M2006/GM6020)统一驱动
|
||||
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
|
||||
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */
|
||||
|
||||
typedef enum
|
||||
{
|
||||
DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
|
||||
DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A
|
||||
DJI_MOTOR_6020, // 直驱, 电压控制
|
||||
} dji_motor_type_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t raw_angle; // 本次机械角度原始数据(0~8191 对应 0~360°)
|
||||
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
|
||||
float rotor_angle_deg; // 累计转子机械角(°)
|
||||
int16_t rotor_speed; // 转子转速(RPM)
|
||||
int16_t raw_torque; // 转矩电流/转矩电压原始数据
|
||||
float torque_current; // 转矩电流(A)
|
||||
uint8_t temperature; // 电机温度(℃)
|
||||
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
|
||||
} dji_motor_feedback_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
dji_motor_type_e type;
|
||||
hcan_t * hcan;
|
||||
uint16_t rx_id; // 反馈 ID:M3508/M2006 0x201~0x208,GM6020 0x205~0x20B
|
||||
dji_motor_feedback_t fdb;
|
||||
} dji_motor_t;
|
||||
|
||||
/* 初始化并注册 CAN 接收回调(can_start() 之后调用) */
|
||||
extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_motor_type_e type);
|
||||
|
||||
/* ---- 控制帧发送 ----
|
||||
* M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
|
||||
* M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000)
|
||||
* GM6020 ID5~7:0x2FF 电压帧 */
|
||||
extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
|
||||
extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
|
||||
extern void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7);
|
||||
|
||||
/* 通用 int16x4 发送(板间通讯等复用) */
|
||||
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,218 @@
|
||||
/* =============================================================================
|
||||
* referee.c - 裁判系统协议解析(0x0201 机器人状态 / 0x0202 实时功率 /
|
||||
* 0x0304 自定义控制器键鼠)
|
||||
* 输入: referee_parse_byte() 流式喂字节(USART6 整帧回调里循环调用)
|
||||
* 校验: CRC8(多项式0x31,初值0xFF) / CRC16(0x1021,0xFFFF),位运算实现,
|
||||
* 与 legacy 查表算法等价
|
||||
* 字段偏移与 legacy 逐一对应:
|
||||
* 0x0201: data[0]=robot_id, data[10..11]=功率上限, data[12]&0x04=发射机构上电
|
||||
* 0x0202: data[4..7]=实时功率(float), data[8..9]=缓冲能量(J)
|
||||
* 0x0304: 12 字节键鼠数据 -> keyboard 钩子(仅云台板注册)
|
||||
* 解析在接收中断内完成,任务层只读 referee_data_t 全局量
|
||||
* ========================================================================== */
|
||||
#include "referee.h"
|
||||
#include "main.h"
|
||||
|
||||
/* 帧结构: SOF(1) data_len(2) seq(1) CRC8(1) | cmd_id(2) data(n) CRC16(2)
|
||||
* 校验算法与 DJI 裁判系统协议一致:
|
||||
* CRC8 多项式 0x31,初值 0xFF,输入输出不反转
|
||||
* CRC16 多项式 0x1021,初值 0xFFFF,输入输出不反转 */
|
||||
|
||||
#define REF_FRAME_MAX_LEN 160
|
||||
|
||||
static uint8_t crc8_update(uint8_t crc, uint8_t data)
|
||||
{
|
||||
crc ^= data;
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x31) : (uint8_t)(crc << 1);
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint16_t crc16_update(uint16_t crc, uint8_t data)
|
||||
{
|
||||
crc ^= (uint16_t)data << 8;
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
crc = (crc & 0x8000) ? (uint16_t)((crc << 1) ^ 0x1021) : (uint16_t)(crc << 1);
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint8_t verify_crc8(const uint8_t * data, uint16_t len)
|
||||
{
|
||||
uint8_t crc = 0xFF;
|
||||
for (uint16_t i = 0; i < len - 1; i++)
|
||||
crc = crc8_update(crc, data[i]);
|
||||
return crc == data[len - 1];
|
||||
}
|
||||
|
||||
static uint8_t verify_crc16(const uint8_t * data, uint16_t len)
|
||||
{
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (uint16_t i = 0; i < len - 2; i++)
|
||||
crc = crc16_update(crc, data[i]);
|
||||
return (uint8_t)(crc & 0xFF) == data[len - 2] && (uint8_t)(crc >> 8) == data[len - 1];
|
||||
}
|
||||
|
||||
referee_data_t referee = {
|
||||
.robot_id = 0,
|
||||
.shooter_power_on = 1,
|
||||
.chassis_power_limit = 60,
|
||||
.chassis_power = 0,
|
||||
.buffer_energy = 60.0f,
|
||||
.link_status = 0,
|
||||
.last_update_tick = 0,
|
||||
};
|
||||
|
||||
static referee_keyboard_hook_f keyboard_hook = NULL;
|
||||
|
||||
void referee_set_keyboard_hook(referee_keyboard_hook_f hook)
|
||||
{
|
||||
keyboard_hook = hook;
|
||||
}
|
||||
|
||||
/* 解析状态机 */
|
||||
typedef enum
|
||||
{
|
||||
REF_STATE_SOF = 0,
|
||||
REF_STATE_LEN_LOW,
|
||||
REF_STATE_LEN_HIGH,
|
||||
REF_STATE_SEQ,
|
||||
REF_STATE_HEADER_CRC8,
|
||||
REF_STATE_CMD_LOW,
|
||||
REF_STATE_CMD_HIGH,
|
||||
REF_STATE_DATA,
|
||||
REF_STATE_CRC16_LOW,
|
||||
REF_STATE_CRC16_HIGH,
|
||||
} ref_state_e;
|
||||
|
||||
static ref_state_e ref_state = REF_STATE_SOF;
|
||||
static uint16_t ref_data_len = 0;
|
||||
static uint16_t ref_cmd_id = 0;
|
||||
static uint16_t ref_data_index = 0;
|
||||
static uint8_t ref_frame_buf[REF_FRAME_MAX_LEN];
|
||||
static uint16_t ref_frame_index = 0;
|
||||
|
||||
static void referee_dispatch(uint16_t cmd_id, const uint8_t * data, uint16_t len)
|
||||
{
|
||||
switch (cmd_id)
|
||||
{
|
||||
case 0x0201: // 机器人状态(与 legacy 偏移一致)
|
||||
if (len >= 13)
|
||||
{
|
||||
referee.robot_id = data[0];
|
||||
referee.chassis_power_limit = (uint16_t)(data[10] | (data[11] << 8));
|
||||
referee.shooter_power_on = (data[12] & 0x04) >> 2;
|
||||
referee.last_update_tick = HAL_GetTick();
|
||||
referee.link_status = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x0202: // 实时功率(与 legacy 偏移一致)
|
||||
if (len >= 10)
|
||||
{
|
||||
float power;
|
||||
memcpy(&power, &data[4], 4);
|
||||
referee.chassis_power = power;
|
||||
referee.buffer_energy = (float)((uint16_t)(data[8] | (data[9] << 8)));
|
||||
referee.last_update_tick = HAL_GetTick();
|
||||
referee.link_status = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x0304: // 自定义控制器(经图传链路转发)
|
||||
if (len >= 12 && keyboard_hook != NULL)
|
||||
keyboard_hook(data);
|
||||
referee.last_update_tick = HAL_GetTick();
|
||||
referee.link_status = 1;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void referee_parse_byte(uint8_t byte)
|
||||
{
|
||||
switch (ref_state)
|
||||
{
|
||||
case REF_STATE_SOF:
|
||||
if (byte == 0xA5)
|
||||
{
|
||||
ref_frame_index = 0;
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
ref_state = REF_STATE_LEN_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case REF_STATE_LEN_LOW:
|
||||
ref_data_len = byte;
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
ref_state = REF_STATE_LEN_HIGH;
|
||||
break;
|
||||
|
||||
case REF_STATE_LEN_HIGH:
|
||||
ref_data_len |= (uint16_t)byte << 8;
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
if (ref_data_len > REF_FRAME_MAX_LEN - 9) // 帧过长,重新找帧头
|
||||
ref_state = REF_STATE_SOF;
|
||||
else
|
||||
ref_state = REF_STATE_SEQ;
|
||||
break;
|
||||
|
||||
case REF_STATE_SEQ:
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
ref_state = REF_STATE_HEADER_CRC8;
|
||||
break;
|
||||
|
||||
case REF_STATE_HEADER_CRC8:
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
if (verify_crc8(ref_frame_buf, 5))
|
||||
ref_state = REF_STATE_CMD_LOW;
|
||||
else
|
||||
ref_state = REF_STATE_SOF;
|
||||
break;
|
||||
|
||||
case REF_STATE_CMD_LOW:
|
||||
ref_cmd_id = byte;
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
ref_state = REF_STATE_CMD_HIGH;
|
||||
break;
|
||||
|
||||
case REF_STATE_CMD_HIGH:
|
||||
ref_cmd_id |= (uint16_t)byte << 8;
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
ref_data_index = 0;
|
||||
if (ref_data_len == 0)
|
||||
ref_state = REF_STATE_CRC16_LOW;
|
||||
else
|
||||
ref_state = REF_STATE_DATA;
|
||||
break;
|
||||
|
||||
case REF_STATE_DATA:
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
if (++ref_data_index >= ref_data_len)
|
||||
ref_state = REF_STATE_CRC16_LOW;
|
||||
break;
|
||||
|
||||
case REF_STATE_CRC16_LOW:
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
ref_state = REF_STATE_CRC16_HIGH;
|
||||
break;
|
||||
|
||||
case REF_STATE_CRC16_HIGH:
|
||||
ref_frame_buf[ref_frame_index++] = byte;
|
||||
if (verify_crc16(ref_frame_buf, ref_frame_index))
|
||||
referee_dispatch(ref_cmd_id, &ref_frame_buf[7], ref_data_len);
|
||||
ref_state = REF_STATE_SOF;
|
||||
break;
|
||||
|
||||
default:
|
||||
ref_state = REF_STATE_SOF;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void referee_parse_bytes(const uint8_t * data, uint16_t len)
|
||||
{
|
||||
while (len--)
|
||||
referee_parse_byte(*data++);
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
#ifndef REFEREE_H
|
||||
#define REFEREE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
/* 裁判系统协议解析(0x0201 机器人状态 / 0x0202 实时功率 / 0x0304 自定义控制器)
|
||||
* CRC 校验与字段偏移和 legacy RefereeSystem 模块保持一致 */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t robot_id; // 机器人 ID(无裁判系统时为 0,4 号步兵为 4/104)
|
||||
uint8_t shooter_power_on; // 发射机构上电状态(0x0201)
|
||||
uint16_t chassis_power_limit; // 底盘功率上限(W)
|
||||
float chassis_power; // 底盘实时功率(W)
|
||||
float buffer_energy; // 底盘缓冲能量(J)
|
||||
uint8_t link_status; // 收到过有效帧置 1,掉线检测配合 last_update_tick
|
||||
uint32_t last_update_tick;
|
||||
} referee_data_t;
|
||||
|
||||
extern referee_data_t referee;
|
||||
|
||||
/* 流式解析一个字节(中断上下文调用;把串口收到的每个字节依次喂进来) */
|
||||
extern void referee_parse_byte(uint8_t byte);
|
||||
|
||||
/* 批量喂入(usart 整帧回调里循环调用 referee_parse_byte 即可,此函数为便捷封装) */
|
||||
extern void referee_parse_bytes(const uint8_t * data, uint16_t len);
|
||||
|
||||
/* 0x0304(自定义控制器)数据回调,在中断上下文执行 */
|
||||
typedef void (*referee_keyboard_hook_f)(const uint8_t data[12]);
|
||||
extern void referee_set_keyboard_hook(referee_keyboard_hook_f hook);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,101 @@
|
||||
/* =============================================================================
|
||||
* shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006
|
||||
* 逻辑(与 legacy 一致):
|
||||
* 摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过),
|
||||
* 开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开)
|
||||
* 拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向
|
||||
* 拨弹电流经 shoot_share_rammer_current() 交给云台共帧发送(0x1FF CAN1)
|
||||
* ========================================================================== */
|
||||
#include "shoot.h"
|
||||
#include "gimbal.h"
|
||||
#include "pid.h"
|
||||
#include "tim.h"
|
||||
#include "robot_param.h"
|
||||
|
||||
extern TIM_HandleTypeDef htim3;
|
||||
|
||||
uint8_t shoot_friction_flag = 0;
|
||||
|
||||
static PID_PositionTypeDef friction_l_pid, friction_r_pid, rammer_pid;
|
||||
|
||||
/* 激光:PC8 = TIM3_CH3(10kHz PWM,ARR=8399),占空比 0/满 对应关/开 */
|
||||
#define LASER_ON() __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 8399)
|
||||
#define LASER_OFF() __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0)
|
||||
|
||||
static void laser_init(void)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0);
|
||||
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
|
||||
}
|
||||
|
||||
void shoot_init(void)
|
||||
{
|
||||
PID_PositionStructureInit(&friction_l_pid, 0);
|
||||
PID_PositionSetParameter(&friction_l_pid, GIMBAL_FRICTION_WHEEL_KP, GIMBAL_FRICTION_WHEEL_KI, GIMBAL_FRICTION_WHEEL_KD);
|
||||
PID_PositionSetEkRange(&friction_l_pid, -5, 5);
|
||||
PID_PositionSetOUTRange(&friction_l_pid, -15000, 15000);
|
||||
|
||||
PID_PositionStructureInit(&friction_r_pid, 0);
|
||||
PID_PositionSetParameter(&friction_r_pid, GIMBAL_FRICTION_WHEEL_KP, GIMBAL_FRICTION_WHEEL_KI, GIMBAL_FRICTION_WHEEL_KD);
|
||||
PID_PositionSetEkRange(&friction_r_pid, -5, 5);
|
||||
PID_PositionSetOUTRange(&friction_r_pid, -15000, 15000);
|
||||
|
||||
PID_PositionStructureInit(&rammer_pid, GIMBAL_RAMMER_SPEED);
|
||||
PID_PositionSetParameter(&rammer_pid, GIMBAL_RAMMER_KP, GIMBAL_RAMMER_KI, GIMBAL_RAMMER_KD);
|
||||
PID_PositionSetEkRange(&rammer_pid, -20, 20);
|
||||
PID_PositionSetOUTRange(&rammer_pid, -30000, 30000);
|
||||
|
||||
laser_init();
|
||||
}
|
||||
|
||||
void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on)
|
||||
{
|
||||
/* 摩擦轮(与 legacy 逻辑一致:发射机构上电才允许开启;
|
||||
* 台架调试时可经 DEBUG_SHOOT_WITHOUT_REFEREE 绕过裁判系统检查) */
|
||||
if (friction_on && (shooter_power_on || DEBUG_SHOOT_WITHOUT_REFEREE))
|
||||
{
|
||||
friction_l_pid.Need_Value = -GIMBAL_FRICTION_WHEEL_SPEED;
|
||||
friction_r_pid.Need_Value = GIMBAL_FRICTION_WHEEL_SPEED;
|
||||
LASER_ON();
|
||||
shoot_friction_flag = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
friction_l_pid.Need_Value = 0;
|
||||
friction_r_pid.Need_Value = 0;
|
||||
LASER_OFF();
|
||||
shoot_friction_flag = 0;
|
||||
}
|
||||
|
||||
/* 拨弹:仅在摩擦轮开启时允许 */
|
||||
if (shoot_friction_flag)
|
||||
{
|
||||
if (fire)
|
||||
rammer_pid.Need_Value = GIMBAL_RAMMER_SPEED;
|
||||
else if (eject)
|
||||
rammer_pid.Need_Value = -GIMBAL_RAMMER_SPEED;
|
||||
else
|
||||
rammer_pid.Need_Value = 0;
|
||||
}
|
||||
else
|
||||
rammer_pid.Need_Value = 0;
|
||||
|
||||
PID_PositionCalc(&friction_l_pid, motor_friction_l.fdb.rotor_speed);
|
||||
PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed);
|
||||
PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed);
|
||||
|
||||
/* 拨弹电流交给云台控制帧统一发送(0x1FF CAN1 共帧) */
|
||||
shoot_share_rammer_current((int16_t)rammer_pid.OUT);
|
||||
}
|
||||
|
||||
void shoot_send_cmd(void)
|
||||
{
|
||||
dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0);
|
||||
}
|
||||
|
||||
void shoot_output_zero(void)
|
||||
{
|
||||
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
|
||||
LASER_OFF();
|
||||
shoot_friction_flag = 0;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef SHOOT_H
|
||||
#define SHOOT_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* 发射机构:摩擦轮 2×M3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(电流由云台共帧发送)
|
||||
* 摩擦轮开启时联动激光(PC8,经 TIM3 CH3 PWM) */
|
||||
|
||||
/* 初始化摩擦轮/拨弹 PID(can_start 之后调用) */
|
||||
extern void shoot_init(void);
|
||||
|
||||
/* 发射控制(2ms 调用)
|
||||
* friction_on:摩擦轮开关;fire:发射;eject:退弹
|
||||
* shooter_power_on:发射机构上电状态(来自底盘 0x019) */
|
||||
extern void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on);
|
||||
|
||||
/* 发送摩擦轮电流帧(0x200 CAN1) */
|
||||
extern void shoot_send_cmd(void);
|
||||
|
||||
/* 摩擦轮输出清零 */
|
||||
extern void shoot_output_zero(void);
|
||||
|
||||
/* 摩擦轮当前开关状态(供激光联动与调试) */
|
||||
extern uint8_t shoot_friction_flag;
|
||||
|
||||
/* 拨弹电流写入共享变量(由 shoot 模块调用,云台共帧发送) */
|
||||
extern void shoot_share_rammer_current(int16_t current);
|
||||
#endif
|
||||
Reference in New Issue
Block a user