add imu deadbands to prevent 0piao

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;
-}
+}

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

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="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=193, 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=156, TabPos=1, TopOfStack=0, 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=156, TabPos=2, TopOfStack=1, 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="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=156, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=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=1, w=681, h=157, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, Showing="Task List"
+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 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;100;100;100;100;100;113;113]
+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="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;198]
+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;279]
+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]
+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