我的陀螺仪旋转了90度，计算出来姿态旋转只有30度

majianjia

如题…L3G4200D 输出 采样率250Hz 即周期0.004s
然后 ：Degree = data * 8.75 / 1000 * 0.004
把Degree带入四元数算法，计算周期250Hz
得到的值如题…比如z轴实际转动90度，数据只显示30度 ，X Y轴类似。
可能的原因1：因为sprintf转换三个浮点数成字符串，DMA输出到串口。会不会是sprintf太耗时间导致计算周期达不到250HZ
可能的原因2：stm32振荡器设置有问题，系统频率不是72MHz(串口能正常通讯怎么解释？)
可能的原因3：传感器的寄存器设置有问题（L3G4200D设置应该注意什么？）

我用定时器测出来算法耗时（假设系统时钟正确）

陀螺仪加速度提取 200us
四元数据计算 587us
（不包括浮点转换为字符串输出的时间）

四元数纯浮点计算，加起来还不到1ms 用的是自己网上收集的快速三角，反三角，平方，开平方 math.h

不知道是什么问题唉…课多没时间去测试，中午不睡觉了…

121212

直接积分  呢？

是不是 四元数据  算法有问题？

majianjia

直接积分没试过。。
系统时钟没问题

majianjia

问题找到了，在rt_kprintf()
sprintf转换三个浮点数 用了300~500us不等，这个不足以影响系统。
但是如果转换完成后输出，马上就变成 7749us 至8065 us


yaw =   2.3   roll =   4.2   prith =  -3.4  
姿态计算 耗时： 7749 us

yaw =   2.3   roll =   4.2   prith =  -3.5  
姿态计算 耗时： 7704 us

yaw = -151.6   roll = -120.8   prith = -52.5  //角度偏差大的时候sprintf转换更耗时
姿态计算 耗时?8065 us

7.7ms 已经大大超过整个系统周期4ms了
看来我要早点写好发送线程，丢掉串口助手了，不能再用rt_kprintf做输出了


但是就上面的时间测试来说，数据采集，初步的滤波 ，然后单精度计算四元数，用STM32可以做得很好，整个过程不超过1ms
不知道卡尔曼滤波加上之后如何。我预计应该在1ms内能完成卡尔曼滤波的计算。
如果都可以做到，全单精度浮点计算，用STM32是没什么压力，只做姿态的话，应该可以上500HZ。

以下是测试的源代码

start_measure_time();          //开始计时
#ifdef print_posture
                if(acc.count % 16 == 1)                                                                          //调试时输出数据
                {
                        sprintf(f_buf1, "%5.1f ", final_angle.fYaw  *180/ONE_PI);
                        sprintf(f_buf2, "%5.1f ", final_angle.fRoll *180/ONE_PI);
                        sprintf(f_buf3, "%5.1f ", final_angle.fPitch*180/ONE_PI);
                        rt_kprintf("\nyaw = %s  roll = %s  prith = %s ", f_buf1, f_buf2, f_buf3);
                }
#endif       

mb_temp = (int)stop_measure_time();    //结束计时
rt_kprintf("\n姿态计算 耗时：%5d us",mb_temp);
        }

gzhuli

rt_kprintf默认是轮询发送的。

majianjia

回复【4楼】gzhuli  咕唧霖
-----------------------------------------------------------------------

原来是这样。。怪不得那么花时间。是不是缓冲不够了，所以在等发送完成？

Lziyoutian

想问一下LZ  怎么配置l3g4200的 我现在可以读写 读ID也是０x43 但是读三轴数据的时候 就是不会变化 无论我怎么动板子！
求解啊！！！

gzhuli

你应该是直接用serial.c的吧？
初始化rt_serial_init()时，dev->flag有没有RT_DEVICE_FLAG_DMA_TX？

majianjia

是的，有没有就不知道了，我没有看过太多关于rt-thread的源码。
我就直接用rt_kprintf输出，其他的没有关注。
我已经将数据改成用结构体输出了，用USART2发送，没有注_册设备，用的是中断的方式。这样做方便和我的以前做的上位机通信。每帧数据32字节，就是一个完整的结构体。上位机可以绘图，这样观察数据好一些，也不会有延迟。中午回去我看一下DMA，换成DMA发送就更好了。

gzhuli

serial.c的串口初始化函数rt_serial_init()是由你调用的，dev结构体是你初始化传进去的，你怎么说不知道有没有……
只要rt_serial_init()传进去的dev->flag带有RT_DEVICE_FLAG_DMA_TX标志，就会启用DMA发送模式，如果没有就是轮询。

ITG3205

专业提供 ST InvenSense 各种陀螺仪