【求助帖】前辈们帮忙看看这个姿态解算代码哪里写的问题

"main.c"，思路是用SPI读取加速度计和陀螺仪的数据，分别存放在accelerometer[3]和gyro[3]中，然后用MahonyAHRSupdateIMU和get_angle函数计算得到姿态角。

"MahonyAHRS.c"，sampleFreq和C板教程上给的一致，也是1000.0f，MahonyAHRSupdateIMU函数也和C板例程一致，直接Copy过来的。

最终的效果有些无法描述，因为不知道搞出来的到底是啥。
静止测试：板子放在桌子上，官方的程序运行出来INS_angle[1]接近于0，但是我这个运行出来会增长到0.5左右。
运动测试：晃几下板子，在把板子静止放在桌子上，INS_angle[1]又变成其他。
看到官方的程序用到了Freertos还有dma，而我这个没用到，不知道是不是这个的原因。

这是因为积分频率不正确导致的吧，把计算的函数频率接近1khz。官方所用的方法是丢到任务里跑，你也可以放在定时器当中。由于分频系数的影响实际频率会大一点，1次运行大概0.998ms左右

找到解决办法了。
根据C板教程第3章，把TIM1搞到1000HZ，把附件里While(1)里的代码放到定时器回调函数HAL_TIM_PeriodElapsedCallback()里，main函数里别忘了开启定时中断HAL_TIM_Base_Start_IT(&htim1)。
运行，效果还可以，比不上官方代码的效果，但是大致数据已经是正确的了。