TuxMonkey/tronone-h7-scaffold
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/tronone-h7-scaffold synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

imu test

Browse Source
This commit is contained in:
TuxMonkey
2025-11-16 17:19:14 +08:00
parent de8a0d5117
commit ab01747052
20 changed files with 10796 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
User_Code/application/robot.c Normal file
View File

@@ -0,0 +1,86 @@
/**
* @file robot.c
* @brief
* @author TuxMonkey (nqx2004@gmail.com)
* @version 1.0
* @date 2025-07-08
*
* @copyright Copyright (c) 2025 DLMU-C.ONE
*
* @par 修改日志:
* <table>
* <tr><th>Date <th>Version <th>Author <th>Description
* <tr><td>2025-07-08 <td>1.0 <td>TuxMonkey <td>内容
* </table>
*/
#include "bsp_init.h"
#include "robot.h"
#include "cmsis_gcc.h"
// #include "robot_def.h"
// #include "robot_task.h"
// // 编译warning,提醒开发者修改机器人参数
// #ifndef ROBOT_DEF_PARAM_WARNING
// #define ROBOT_DEF_PARAM_WARNING
// #pragma message "check if you have configured the parameters in robot_def.h, IF NOT, please refer to the comments AND DO IT, otherwise the robot will have FATAL ERRORS!!!"
// #endif // !ROBOT_DEF_PARAM_WARNING
// #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
// #include "chassis.h"
// #endif
// #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
// #include "gimbal.h"
// #include "shoot.h"
// #include "robot_cmd.h"
// #endif
/**
* @brief 机器人初始化函数
*
* @note 该函数在系统启动时调用,用于初始化机器人各个模块
*
* @attention 请不要在初始化过程中使用中断和延时函数！
* 若必须,则只允许使用DWT_Delay()
*/
void RobotInit()
{
// 关闭中断,防止在初始化过程中发生中断
// 请不要在初始化过程中使用中断和延时函数！
// 若必须,则只允许使用DWT_Delay()
__disable_irq();
BSPInit();
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit();
// GimbalInit();
// ShootInit();
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
ChassisInit();
#endif
// OSTaskInit(); // 创建基础任务
// 初始化完成,开启中断
__enable_irq();
}
// void RobotTask()
// {
// #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
// RobotCMDTask();
// // GimbalTask();
// // ShootTask();
// #endif
// #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
// ChassisTask();
// #endif
// }

18
User_Code/application/robot.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef ROBOT_H
#define ROBOT_H
/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
/**
* @brief 机器人初始化,请在开启rtos之前调用.这也是唯一需要放入main函数的函数
*
*/
void RobotInit();
/**
* @brief 机器人任务,放入实时系统以一定频率运行,内部会调用各个应用的任务
*
*/
void RobotTask();
#endif

22
User_Code/bsp/bsp_init.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef BSP_INIT_h
#define BSP_INIT_h
#include "bsp_init.h"
// #include "bsp_log.h"
#include "bsp_dwt.h"
// #include "bsp_usb.h"
/**
* @brief bsp层初始化统一入口,这里仅初始化必须的bsp组件,其他组件的初始化在各自的模块中进行
* 需在实时系统启动前调用,目前由RobotoInit()调用
*
* @note 其他实例型的外设如CAN和串口会在注册实例的时候自动初始化,不注册不初始化
*/
inline void BSPInit()
{
DWT_Init(480); //Todo:修改这里以适配喵板cpufreq
// BSPLogInit();
}
#endif // !BSP_INIT_h

142
User_Code/bsp/dwt/bsp_dwt.c
View File

@@ -1,5 +1,139 @@
//
// Created by tuxmonkey on 2025/10/28.
//
/**
* @file bsp_dwt.c
* @brief DWT延时 软件延时库
* @author TuxMonkey (nqx2004@gmail.com)
* @version 1.0
* @date 2025-07-08
*
* @copyright Copyright (c) 2025 DLMU-C.ONE
*
* @par 修改日志:
* <table>
* <tr><th>Date <th>Version <th>Author <th>Description
* <tr><td>2025-07-08 <td>1.0 <td>TuxMonkey <td>内容
* </table>
*/
#include "bsp_dwt.h"
#include "bsp_dwt.h"
#include "cmsis_os.h"
static DWT_Time_t SysTime;
static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
static uint32_t CYCCNT_RountCount;
static uint32_t CYCCNT_LAST;
static uint64_t CYCCNT64;
/**
* @brief 私有函数,用于检查DWT CYCCNT寄存器是否溢出,并更新CYCCNT_RountCount
* @attention 此函数假设两次调用之间的时间间隔不超过一次溢出
*
* @todo 更好的方案是为dwt的时间更新单独设置一个任务?
* 不过,使用dwt的初衷是定时不被中断/任务等因素影响,因此该实现仍然有其存在的意义
*
*/
static void DWT_CNT_Update(void)
{
static volatile uint8_t bit_locker = 0;
if (!bit_locker)
{
bit_locker = 1;
volatile uint32_t cnt_now = DWT->CYCCNT;
if (cnt_now < CYCCNT_LAST)
CYCCNT_RountCount++;
CYCCNT_LAST = DWT->CYCCNT;
bit_locker = 0;
}
}
void DWT_Init(uint32_t CPU_Freq_mHz)
{
/* 使能DWT外设 */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
/* DWT CYCCNT寄存器计数清0 */
DWT->CYCCNT = (uint32_t) 0u;
/* 使能Cortex-M DWT CYCCNT寄存器 */
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
CPU_FREQ_Hz = CPU_Freq_mHz * 1000000;
CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000;
CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000;
CYCCNT_RountCount = 0;
DWT_CNT_Update();
}
float DWT_GetDeltaT(uint32_t *cnt_last)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
float dt = ((uint32_t) (cnt_now - *cnt_last)) / ((float) (CPU_FREQ_Hz));
*cnt_last = cnt_now;
DWT_CNT_Update();
return dt;
}
double DWT_GetDeltaT64(uint32_t *cnt_last)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
double dt = ((uint32_t) (cnt_now - *cnt_last)) / ((double) (CPU_FREQ_Hz));
*cnt_last = cnt_now;
DWT_CNT_Update();
return dt;
}
void DWT_SysTimeUpdate(void)
{
volatile uint32_t cnt_now = DWT->CYCCNT;
static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3;
DWT_CNT_Update();
CYCCNT64 = (uint64_t) CYCCNT_RountCount * (uint64_t) UINT32_MAX + (uint64_t) cnt_now;
CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz;
CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz;
SysTime.s = CNT_TEMP1;
SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms;
CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms;
SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us;
}
float DWT_GetTimeline_s(void)
{
DWT_SysTimeUpdate();
float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f;
return DWT_Timelinef32;
}
float DWT_GetTimeline_ms(void)
{
DWT_SysTimeUpdate();
float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f;
return DWT_Timelinef32;
}
uint64_t DWT_GetTimeline_us(void)
{
DWT_SysTimeUpdate();
uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us;
return DWT_Timelinef32;
}
void DWT_Delay(float Delay)
{
uint32_t tickstart = DWT->CYCCNT;
float wait = Delay;
while ((DWT->CYCCNT - tickstart) < wait * (float) CPU_FREQ_Hz);
}

108
User_Code/bsp/dwt/bsp_dwt.h
View File

@@ -1,8 +1,104 @@
//
// Created by tuxmonkey on 2025/10/28.
//
/**
* @file bsp_dwt.h
* @brief DWT延时 软件延时库头文件
* @author TuxMonkey (nqx2004@gmail.com)
* @version 1.0
* @date 2025-07-08
*
* @copyright Copyright (c) 2025 DLMU-C.ONE
*
* @par 修改日志:
* <table>
* <tr><th>Date <th>Version <th>Author <th>Description
* <tr><td>2025-07-08 <td>1.0 <td>TuxMonkey <td>内容
* </table>
*/
#ifndef TRONONEH7_SCAFFOLD_BSP_DWT_H
#define TRONONEH7_SCAFFOLD_BSP_DWT_H
#ifndef _BSP_DWT_H
#define _BSP_DWT_H
#endif //TRONONEH7_SCAFFOLD_BSP_DWT_H
#include "main.h"
#include "stdint.h"
#include "bsp_log.h"
typedef struct
{
uint32_t s;
uint16_t ms;
uint16_t us;
} DWT_Time_t;
/**
* @brief 该宏用于计算代码段执行时间,单位为秒/s,返回值为float类型
* 首先需要创建一个float类型的变量,用于存储时间间隔
* 计算得到的时间间隔同时还会通过RTT打印到日志终端,你也可以将你的dt变量添加到查看
*/
#define TIME_ELAPSE(dt, code) \
do \
{ \
float tstart = DWT_GetTimeline_s(); \
code; \
dt = DWT_GetTimeline_s() - tstart; \
LOGINFO("[DWT] " #dt " = %f s\r\n", dt); \
} while (0)
/**
* @brief 初始化DWT,传入参数为CPU频率,单位MHz
*
* @param CPU_Freq_mHz c板为168MHz,A板为180MHz
*/
void DWT_Init(uint32_t CPU_Freq_mHz);
/**
* @brief 获取两次调用之间的时间间隔,单位为秒/s
*
* @param cnt_last 上一次调用的时间戳
* @return float 时间间隔,单位为秒/s
*/
float DWT_GetDeltaT(uint32_t *cnt_last);
/**
* @brief 获取两次调用之间的时间间隔,单位为秒/s,高精度
*
* @param cnt_last 上一次调用的时间戳
* @return double 时间间隔,单位为秒/s
*/
double DWT_GetDeltaT64(uint32_t *cnt_last);
/**
* @brief 获取当前时间,单位为秒/s,即初始化后的时间
*
* @return float 时间轴
*/
float DWT_GetTimeline_s(void);
/**
* @brief 获取当前时间,单位为毫秒/ms,即初始化后的时间
*
* @return float
*/
float DWT_GetTimeline_ms(void);
/**
* @brief 获取当前时间,单位为微秒/us,即初始化后的时间
*
* @return uint64_t
*/
uint64_t DWT_GetTimeline_us(void);
/**
* @brief DWT延时函数,单位为秒/s
* @attention 该函数不受中断是否开启的影响,可以在临界区和关闭中断时使用
* @note 禁止在__disable_irq()和__enable_irq()之间使用HAL_Delay()函数,应使用本函数
*
* @param Delay 延时时间,单位为秒/s
*/
void DWT_Delay(float Delay);
/**
* @brief DWT更新时间轴函数,会被三个timeline函数调用
* @attention 如果长时间不调用timeline函数,则需要手动调用该函数更新时间轴,否则CYCCNT溢出后定时和时间轴不准确
*/
void DWT_SysTimeUpdate(void);
#endif /* BSP_DWT_H_ */

54
User_Code/bsp/dwt/bsp_dwt.md Normal file
View File

@@ -0,0 +1,54 @@
# bsp_dwt
DWT是stm32内部的一个"隐藏资源",他的用途是给下载器提供准确的定时,从而为调试信息加上时间戳.并在固定的时间间隔将调试数据发送到你的xxlink上.
## 常用功能
### 计算两次进入同一个函数的时间间隔
```c
static uint32_t cnt;
float deltaT;
deltaT=DWT_GetDeltaT(&cnt);
```
### 计算执行某部分代码的耗时
```c
float start,end;
start=DWT_DetTimeline_ms();
// some proc to go...
for(uint8_t i=0;i<10;i++)
foo();
end = DWT_DetTimeline_ms()-start;
```
我们还提供了一个宏用于调试计时:
```c
#define TIME_ELAPSE(dt, code) \
do \
{ \
float tstart = DWT_GetTimeline_s(); \
code; \
dt = DWT_GetTimeline_s() - tstart; \
LOGINFO("[DWT] " #dt " = %f s\r\n", dt); \
} while (0)
```
传入一个float类型的变量,并将你要执行的代码写入第二个参数:
```c
static float my_func_dt;
TIME_ELAPSE(my_func_dt,
Function1(vara);
Function2(some, var);
Function3(your,param);
// something more
);
// my_func_dt can be used for other purpose then;
```

6
User_Code/bsp/fdcan/bsp_fdcan.c
View File

@@ -150,7 +150,7 @@ FDCANInstance *FDCANRegister(FDCAN_Init_Config_s *config)
{
while (1)
{
// LOGERROR("[bsp_fdcan] FDCAN instance exceeded MAX num, consider balance the load of FDCAN bus");
// Todo:LOGERROR("[bsp_fdcan] FDCAN instance exceeded MAX num, consider balance the load of FDCAN bus");
}
}
for (size_t i = 0; i < idx; i++)
@@ -160,7 +160,7 @@ FDCANInstance *FDCANRegister(FDCAN_Init_Config_s *config)
{
while (1)
{
// LOGERROR("[bsp_fdcan] FDCAN id crash ,tx [%d] or rx [%d] already registered", config->tx_id, config->rx_id);
// Todo:LOGERROR("[bsp_fdcan] FDCAN id crash ,tx [%d] or rx [%d] already registered", config->tx_id, config->rx_id);
}
}
}
@@ -243,7 +243,7 @@ void FDCANSetDLC(FDCANInstance *_instance, uint8_t length)
if (length > 64 || length == 0) // 安全检查
while (1)
{
//LOGERROR("[bsp_fdcan] FDCAN DLC error! check your code or wild pointer");
//Todo:LOGERROR("[bsp_fdcan] FDCAN DLC error! check your code or wild pointer");
}
_instance->txconf.DataLength = FDCANLenToDlc(length);
}

211
User_Code/module/algorithm/controller/controller.c
View File

@@ -1,5 +1,208 @@
//
// Created by ASUS on 2025/11/15.
//
/**
* @file controller.c
* @author wanghongxi
* @author modified by neozng
* @brief PID控制器定义
* @version beta
* @date 2022-11-01
*
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h"
#include "memory.h"
/* ----------------------------下面是pid优化环节的实现---------------------------- */
// 梯形积分
static void f_Trapezoid_Intergral(PIDInstance *pid)
{
// 计算梯形的面积,(上底+下底)*高/2
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
// 变速积分(误差小时积分作用更强)
static void f_Changing_Integration_Rate(PIDInstance *pid)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
else // 最大阈值,不使用积分
pid->ITerm = 0;
}
}
static void f_Integral_Limit(PIDInstance *pid)
{
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
pid->ITerm = 0; // 当前积分项置零
}
}
if (temp_Iout > pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = pid->IntegralLimit;
}
if (temp_Iout < -pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = -pid->IntegralLimit;
}
}
// 微分先行(仅使用反馈值而不计参考输入的微分)
static void f_Derivative_On_Measurement(PIDInstance *pid)
{
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
// 微分滤波(采集微分时,滤除高频噪声)
static void f_Derivative_Filter(PIDInstance *pid)
{
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}
// 输出滤波
static void f_Output_Filter(PIDInstance *pid)
{
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 输出限幅
static void f_Output_Limit(PIDInstance *pid)
{
if (pid->Output > pid->MaxOut)
{
pid->Output = pid->MaxOut;
}
if (pid->Output < -(pid->MaxOut))
{
pid->Output = -(pid->MaxOut);
}
}
// 电机堵转检测
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
{
// Motor blocked counting
pid->ERRORHandler.ERRORCount++;
}
else
{
pid->ERRORHandler.ERRORCount = 0;
}
if (pid->ERRORHandler.ERRORCount > 500)
{
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = PID_MOTOR_BLOCKED_ERROR;
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
*
* @param pid PID实例
* @param config PID初始化设置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
// 后续修改为逐个赋值
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_Config_s));
// set rest of memory to 0
DWT_GetDeltaT(&pid->DWT_CNT);
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref)
{
// 堵转检测
if (pid->Improve & PID_ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
// 如果在死区外,则计算PID
if (abs(pid->Err) > pid->DeadBand)
{
// 基本的pid计算,使用位置式
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & PID_Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & PID_ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & PID_Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & PID_DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & PID_Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 输出滤波
if (pid->Improve & PID_OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
}
else // 进入死区, 则清空积分和输出
{
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}

140
User_Code/module/algorithm/controller/controller.h
View File

@@ -1,8 +1,136 @@
//
// Created by ASUS on 2025/11/15.
//
/**
******************************************************************************
* @file controller.h
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _CONTROLLER_H
#define _CONTROLLER_H
#ifndef TRONONEH7_SCAFFOLD_CONTROLLER_H
#define TRONONEH7_SCAFFOLD_CONTROLLER_H
#include "main.h"
#include "stdint.h"
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include <math.h>
#endif //TRONONEH7_SCAFFOLD_CONTROLLER_H
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
typedef enum
{
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
PID_Integral_Limit = 0b00000001, // 0000 0001
PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
PID_Proportional_On_Measurement = 0b00001000, // 0000 1000
PID_OutputFilter = 0b00010000, // 0001 0000
PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
PID_DerivativeFilter = 0b01000000, // 0100 0000
PID_ErrorHandle = 0b10000000, // 1000 0000
} PID_Improvement_e;
/* PID 报错类型枚举*/
typedef enum errorType_e
{
PID_ERROR_NONE = 0x00U,
PID_MOTOR_BLOCKED_ERROR = 0x01U
} ErrorType_e;
typedef struct
{
uint64_t ERRORCount;
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
/* PID结构体 */
typedef struct
{
//---------------------------------- init config block
// config parameter
float Kp;
float Ki;
float Kd;
float MaxOut;
float DeadBand;
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // 变速积分 For Changing Integral
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
float Derivative_LPF_RC; // 微分滤波器系数
//-----------------------------------
// for calculating
float Measure;
float Last_Measure;
float Err;
float Last_Err;
float Last_ITerm;
float Pout;
float Iout;
float Dout;
float ITerm;
float Output;
float Last_Output;
float Last_Dout;
float Ref;
uint32_t DWT_CNT;
float dt;
PID_ErrorHandler_t ERRORHandler;
} PIDInstance;
/* 用于PID初始化的结构体*/
typedef struct // config parameter
{
// basic parameter
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
float DeadBand; // 死区
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
} PID_Init_Config_s;
/**
* @brief 初始化PID实例
* @todo 待修改为统一的PIDRegister风格
* @param pid PID实例指针
* @param config PID初始化配置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
/**
* @brief 计算PID输出
*
* @param pid PID实例指针
* @param measure 反馈值
* @param ref 设定值
* @return float PID计算输出
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif

487
User_Code/module/algorithm/kalman_filter/kalman_filter.c
View File

@@ -1,5 +1,486 @@
//
// Created by ASUS on 2025/11/15.
//
/**
******************************************************************************
* @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;
}

139
User_Code/module/algorithm/kalman_filter/kalman_filter.h
View File

@@ -1,8 +1,135 @@
//
// Created by ASUS on 2025/11/15.
//
/**
******************************************************************************
* @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
#ifndef TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
#define TRONONEH7_SCAFFOLD_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
*/
#endif //TRONONEH7_SCAFFOLD_KALMAN_FILTER_H
#include "stm32h723xx.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
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#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

88
User_Code/module/algorithm/user_lib/user_lib.h
View File

@@ -10,24 +10,14 @@
*
******************************************************************************
*/
#ifndef _USER_LIB_H
#define _USER_LIB_H
#ifndef USER_LIB_H
#define USER_LIB_H
#include "stdint.h"
#include "main.h"
#include "cmsis_os.h"
enum
{
CHASSIS_DEBUG = 1,
GIMBAL_DEBUG,
INS_DEBUG,
RC_DEBUG,
IMU_HEAT_DEBUG,
SHOOT_DEBUG,
AIMASSIST_DEBUG,
};
extern uint8_t GlobalDebugMode;
#include "stm32h723xx.h"
#include "arm_math.h"
#ifndef user_malloc
#ifdef _CMSIS_OS_H
@@ -37,6 +27,19 @@ extern uint8_t GlobalDebugMode;
#endif
#endif
#define msin(x) (arm_sin_f32(x))
#define mcos(x) (arm_cos_f32(x))
typedef arm_matrix_instance_f32 mat;
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define MatAdd arm_mat_add_f32
#define MatSubtract arm_mat_sub_f32
#define MatMultiply arm_mat_mult_f32
#define MatTranspose arm_mat_trans_f32
#define MatInverse arm_mat_inverse_f32
void MatInit(mat *m, uint8_t row, uint8_t col);
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
@@ -87,40 +90,9 @@ extern uint8_t GlobalDebugMode;
#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
typedef struct
{
float input; //输入数据
float out; //输出数据
float min_value; //限幅最小值
float max_value; //限幅最大值
float frame_period; //时间间隔
} ramp_function_source_t;
typedef __packed struct
{
uint16_t Order;
uint32_t Count;
float *x;
float *y;
float k;
float b;
float StandardDeviation;
float t[4];
} Ordinary_Least_Squares_t;
//快速开方
float Sqrt(float x);
//斜波函数初始化
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
//斜波函数计算
float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
//绝对限制
float abs_limit(float num, float Limit);
@@ -142,24 +114,20 @@ int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
//循环限幅函数
float loop_float_constrain(float Input, float minValue, float maxValue);
//角度 °限幅 180 ~ -180
float theta_format(float Ang);
int float_rounding(float raw);
//弧度格式化为-PI~PI
float *Norm3d(float *v);
float NormOf3d(float *v);
void Cross3d(float *v1, float *v2, float *res);
float Dot3d(float *v1, float *v2);
float AverageFilter(float new_data, float *buf, uint8_t len);
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order);
void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS);
float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS);
#endif

17
User_Code/module/general_def.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef GENERAL_DEF_H
#define GENERAL_DEF_H
// 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
#ifndef PI
#define PI 3.1415926535f
#endif
#define PI2 (PI * 2.0f) // 2 pi
#define RAD_2_DEGREE 57.2957795f // 180/pi
#define DEGREE_2_RAD 0.01745329252f // pi/180
#define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec
#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
#endif // !GENERAL_DEF_H

12
User_Code/module/periph/imu/BMI088driver.c
View File

@@ -5,7 +5,7 @@
#include "bsp_log.h"
#include <math.h>
#pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
// Todo: #pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
float BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN;
float BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN;
@@ -64,7 +64,9 @@ IMU_Data_t BMI088;
}
static void BMI088_write_single_reg(uint8_t reg, uint8_t data);
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data);
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len);
#elif defined(BMI088_USE_IIC)
@@ -138,7 +140,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
bmi088->GyroOffset[2] = GzOFFSET;
bmi088->gNorm = gNORM;
bmi088->TempWhenCali = 40;
LOGERROR("[BMI088] Calibrate Failed! Use offline params");
// Todo: LOGERROR("[BMI088] Calibrate Failed! Use offline params");
break;
}
@@ -209,7 +211,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
gyroDiff[1] > 0.5f || //0.15
gyroDiff[2] > 0.5f) //0.15
{
LOGWARNING("[bmi088] calibration was interrupted\n");
// LOGWARNING("[bmi088] calibration was interrupted\n");
break;
}
@@ -264,7 +266,7 @@ uint8_t bmi088_accel_init(void)
// check the "who am I"
if (res != BMI088_ACC_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 acc chip id");
// Todo: LOGERROR("[bmi088] Can not read bmi088 acc chip id");
return BMI088_NO_SENSOR;
}
@@ -312,7 +314,7 @@ uint8_t bmi088_gyro_init(void)
// check the "who am I"
if (res != BMI088_GYRO_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
//Todo: LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
return BMI088_NO_SENSOR;
}