From 55d36d3812b4bceab24c16e6247818996346d35a Mon Sep 17 00:00:00 2001 From: tuxmonkey Date: Wed, 11 Feb 2026 00:27:51 +0800 Subject: [PATCH] add imu deadbands to prevent 0piao --- User_Code/module/periph/imu/ins_task.c | 59 +++++++++++++++++++++++++- User_Code/module/periph/imu/ins_task.h | 8 +++- ozonedeb/windebnewwt.jdebug.user | 40 ++++++++++------- 3 files changed, 89 insertions(+), 18 deletions(-) diff --git a/User_Code/module/periph/imu/ins_task.c b/User_Code/module/periph/imu/ins_task.c index 486f1b5..65f7eeb 100644 --- a/User_Code/module/periph/imu/ins_task.c +++ b/User_Code/module/periph/imu/ins_task.c @@ -41,6 +41,47 @@ static void IMUPWMSet(uint16_t pwm) __HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, pwm); } +/** + * @brief 死区滤波函数 + * @param input 输入数据 + * @param dead_zone 死区阈值 + * @return 滤波后的数据 + */ +static float DeadZoneFilter(float input, float dead_zone) +{ + if (fabsf(input) < dead_zone) + { + return 0.0f; + } + else if (input > 0) + { + return input - dead_zone; + } + else + { + return input + dead_zone; + } +} + +/** + * @brief 应用死区滤波到IMU数据 + * @param gyro 陀螺仪数据 + * @param accel 加速度计数据 + */ +static void ApplyDeadZoneFilter(float gyro[3], float accel[3]) +{ + for (uint8_t i = 0; i < 3; ++i) + { + // 保存原始数据 + INS.GyroRaw[i] = gyro[i]; + INS.AccelRaw[i] = accel[i]; + + // 应用死区滤波 + gyro[i] = DeadZoneFilter(gyro[i], INS.GyroDeadZone[i]); + accel[i] = DeadZoneFilter(accel[i], INS.AccelDeadZone[i]); + } +} + /** * @brief 温度控制 * @@ -115,6 +156,18 @@ attitude_t *INS_Init(void) // noise of accel is relatively big and of high freq,thus lpf is used INS.AccelLPF = 0.0085; + + // 设置默认死区阈值 - 根据实际零漂情况调整这些值 + // 陀螺仪死区阈值 (单位: rad/s) + INS.GyroDeadZone[X] = 0.01f; // X轴死区 + INS.GyroDeadZone[Y] = 0.01f; // Y轴死区 + INS.GyroDeadZone[Z] = 0.01f; // Z轴死区 + + // 加速度计死区阈值 (单位: m/s²) + INS.AccelDeadZone[X] = 0.05f; // X轴死区 + INS.AccelDeadZone[Y] = 0.05f; // Y轴死区 + INS.AccelDeadZone[Z] = 0.05f; // Z轴死区 + DWT_GetDeltaT(&INS_DWT_Count); return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复. } @@ -133,6 +186,7 @@ void INS_Task(void) { BMI088_Read(&BMI088); + // 读取原始IMU数据 INS.Accel[X] = BMI088.Accel[X]; INS.Accel[Y] = BMI088.Accel[Y]; INS.Accel[Z] = BMI088.Accel[Z]; @@ -140,6 +194,9 @@ void INS_Task(void) INS.Gyro[Y] = BMI088.Gyro[Y]; INS.Gyro[Z] = BMI088.Gyro[Z]; + // 应用死区滤波处理零漂 + ApplyDeadZoneFilter(INS.Gyro, INS.Accel); + // demo function,用于修正安装误差,可以不管,本demo暂时没用 IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel); @@ -356,4 +413,4 @@ void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) q[1] = sinPitch * cosRoll * cosYaw - cosPitch * sinRoll * sinYaw; q[2] = sinPitch * cosRoll * sinYaw + cosPitch * sinRoll * cosYaw; q[3] = cosPitch * cosRoll * sinYaw - sinPitch * sinRoll * cosYaw; -} +} \ No newline at end of file diff --git a/User_Code/module/periph/imu/ins_task.h b/User_Code/module/periph/imu/ins_task.h index 89c567c..ea9e426 100644 --- a/User_Code/module/periph/imu/ins_task.h +++ b/User_Code/module/periph/imu/ins_task.h @@ -45,6 +45,12 @@ typedef struct float AccelLPF; // 加速度低通滤波系数 + // 死区滤波参数 - 用于处理零漂 + float GyroDeadZone[3]; // 陀螺仪死区阈值 + float AccelDeadZone[3]; // 加速度计死区阈值 + float GyroRaw[3]; // 原始陀螺仪数据 + float AccelRaw[3]; // 原始加速度计数据 + // bodyframe在绝对系的向量表示 float xn[3]; float yn[3]; @@ -140,4 +146,4 @@ void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q); */ void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q); -#endif +#endif \ No newline at end of file diff --git a/ozonedeb/windebnewwt.jdebug.user b/ozonedeb/windebnewwt.jdebug.user index 9ec7e66..c581b28 100644 --- a/ozonedeb/windebnewwt.jdebug.user +++ b/ozonedeb/windebnewwt.jdebug.user @@ -1,32 +1,40 @@ -OpenDocument="main.c", FilePath="C:/Users/esqwt/CLionProjects/tronone-h7-scaffold/Core/Src/main.c", Line=89 +GraphedExpression="(INS).Yaw", Color=#a00909 +GraphedExpression="(INS).Pitch", Color=#09a01b +GraphedExpression="(INS).Roll", Color=#09a087 +OpenDocument="delayticks.c", FilePath="C:/Users/esqwt/CLionProjects/tronone-h7-scaffold/User_Code/bsp/delayticks/delayticks.c", Line=14 +OpenDocument="ins_task.c", FilePath="C:/Users/esqwt/CLionProjects/tronone-h7-scaffold/User_Code/module/periph/imu/ins_task.c", Line=217 +OpenDocument="tasks.c", FilePath="C:/Users/esqwt/CLionProjects/tronone-h7-scaffold/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=2308 +OpenDocument="main.c", FilePath="C:/Users/esqwt/CLionProjects/tronone-h7-scaffold/Core/Src/main.c", Line=74 OpenToolbar="Debug", Floating=0, x=0, y=0 OpenToolbar="Breakpoints", Floating=0, x=1, y=0 -OpenWindow="Call Stack", DockArea=RIGHT, x=0, y=0, w=681, h=175, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Call Stack", DockArea=LEFT, x=0, y=3, w=551, h=158, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Registers 1", DockArea=BOTTOM, x=1, y=0, w=271, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=551, h=187, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Disassembly", DockArea=BOTTOM, x=2, y=0, w=472, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Break & Tracepoints", DockArea=LEFT, x=0, y=1, w=551, h=194, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VectorCatchIndexMask=254 OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=348, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x20005E30 -OpenWindow="Global Data", DockArea=RIGHT, x=0, y=3, w=681, h=211, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=2, w=551, h=193, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Functions", DockArea=LEFT, x=0, y=3, w=551, h=156, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Call Graph", DockArea=LEFT, x=0, y=3, w=551, h=156, TabPos=2, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Global Data", DockArea=RIGHT, x=0, y=2, w=614, h=271, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=2, w=551, h=191, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Functions", DockArea=LEFT, x=0, y=3, w=551, h=158, TabPos=2, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Call Graph", DockArea=LEFT, x=0, y=3, w=551, h=158, TabPos=3, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=826, h=181, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=RIGHT, x=0, y=2, w=681, h=207, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="1 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="435;0", PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="321;-65", CodeGraphLegendShown=0, CodeGraphLegendPosition="336;0" -OpenWindow="Console", DockArea=LEFT, x=0, y=3, w=551, h=156, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="FreeRTOS", DockArea=RIGHT, x=0, y=1, w=681, h=157, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing="Task List" +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=614, h=268, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="368;0", PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="396;-65", CodeGraphLegendShown=0, CodeGraphLegendPosition="412;0" +OpenWindow="Console", DockArea=LEFT, x=0, y=3, w=551, h=158, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="FreeRTOS", DockArea=RIGHT, x=0, y=0, w=614, h=212, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing="Task List" +TableHeader="Call Graph", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Stack Total";"Stack Local";"Code Total";"Code Local";"Depth";"Called From"], ColWidths=[384;100;100;100;100;100;113] TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1164;100;100;100;100] TableHeader="Global Data", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Location";"Size";"Type";"Scope"], ColWidths=[222;130;100;54;95;100] TableHeader="Vector Catches", SortCol="None", SortOrder="ASCENDING", VisibleCols=["";"Vector Catch";"Description"], ColWidths=[50;300;500] TableHeader="Break & Tracepoints", SortCol="None", SortOrder="ASCENDING", VisibleCols=["";"Type";"Location";"Extras"], ColWidths=[100;100;142;209] -TableHeader="Call Stack", SortCol="Function", SortOrder="ASCENDING", VisibleCols=["Function";"Stack Frame";"Source";"PC";"Return Address";"Stack Used"], ColWidths=[110;126;190;100;190;100] TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[229;100;100;100;1046] -TableHeader="Data Sampling Table", SortCol="Index", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100] -TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[320;100;100;100;100;100;100;113;113] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (INS).Yaw";" (INS).Pitch";" (INS).Roll"], ColWidths=[100;100;100;100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[320;100;118;102;100;118;100;113;113] TableHeader="Power Sampling", SortCol="Index", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] -TableHeader="Task List", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info (Free / Size)";"ID";"Mutex Count";"Notified Value";"Notify State";"Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info (Free / Size)";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110;110;110;110;110;110;110;110;110;110;110] -TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;105;198] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location"], ColWidths=[171;101;279] +TableHeader="Task List", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info (Free / Size)";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110] +TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;105;294] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location"], ColWidths=[171;101;262] TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] -TableHeader="Call Graph", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Stack Total";"Stack Local";"Code Total";"Code Local";"Depth";"Called From"], ColWidths=[384;100;100;100;100;100;113] \ No newline at end of file +TableHeader="Call Stack", SortCol="Function", SortOrder="ASCENDING", VisibleCols=["Function";"Stack Frame";"Source";"PC";"Return Address";"Stack Used"], ColWidths=[110;126;190;100;190;100] +TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[26;26;26;37] +WatchedExpression="INS", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file