【Curie Nano试用】六轴姿态融合

我叫逗逗

看大家都更新了这么多教程，小的是在有愧啊，毕设也赶得紧，只能挤时间来搞curie nano了。

好了，废话不多说，我们看一下如何用curie nano来得到当前姿态。

首先我们在安装了curie的库之后会有一些简单的工程例子给我们，如下：
我们进入 curieIMU 里面，可以看到很多有关加速度计和陀螺仪的例程，我们就用这些例程来进行修改然后得到我们想要的姿态信息。
首先，我们需要安装Madgwick这个库，直接在arduino开发环境里面添加，具体怎么操作就不写了。

融合代码如下：

1. #include <CurieIMU.h>
   
2. #include <MadgwickAHRS.h>
   
3. 
   
4. Madgwick filter;
   
5. unsigned long microsPerReading, microsPrevious;
   
6. float accelScale, gyroScale;
   
7. 
   
8. void setup() {
   
9.   Serial.begin(9600);
   
10. 
    
11.   // start the IMU and filter
    
12.   CurieIMU.begin();
    
13. 
    
14. // Set data rate
    
15.   
    
16.   filter.begin(25);
    
17. 
    
18.   // Set the accelerometer range to 2G
    
19.   CurieIMU.setAccelerometerRange(2);
    
20.   // Set the gyroscope range to 250 degrees/second
    
21.   CurieIMU.setGyroRange(250);
    
22. 
    
23.   // initialize variables to pace updates to correct rate
    
24.   microsPerReading = 1000000 / 25;
    
25.   microsPrevious = micros();
    
26. }
    
27. 
    
28. void loop() {
    
29.   int aix, aiy, aiz;
    
30.   int gix, giy, giz;
    
31.   float ax, ay, az;
    
32.   float gx, gy, gz;
    
33.   float mx=0, my=0, mz=0;
    
34.   float roll, pitch, heading;
    
35.   unsigned long microsNow;
    
36. 
    
37.   // check if it's time to read data and update the filter
    
38.   microsNow = micros();
    
39.   if (microsNow - microsPrevious >= microsPerReading) {
    
40. 
    
41.     // read raw data from CurieIMU
    
42.     CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
    
43. //这个函数是读取原始值
    
44. 
    
45.     // convert from raw data to gravity and degrees/second units
    
46.     ax = convertRawAcceleration(aix);
    
47.     ay = convertRawAcceleration(aiy);
    
48.     az = convertRawAcceleration(aiz);
    
49.     gx = convertRawGyro(gix);
    
50.     gy = convertRawGyro(giy);
    
51.     gz = convertRawGyro(giz);
    
52. 
    
53.     // update the filter, which computes orientation
    
54.     filter.update(gx, gy, gz, ax, ay, az, mx, my ,mz);
    
55. //这个函数可以进去到Madgwick里面看
    
56. 
    
57.     // print the heading, pitch and roll
    
58.     roll = filter.getRoll();
    
59.     pitch = filter.getPitch();
    
60.     heading = filter.getYaw();
    
61.     Serial.print("Orientation: ");
    
62.     Serial.print(heading);
    
63.     Serial.print(" ");
    
64.     Serial.print(pitch);
    
65.     Serial.print(" ");
    
66.     Serial.println(roll);
    
67. 
    
68.     // increment previous time, so we keep proper pace
    
69.     microsPrevious = microsPrevious + microsPerReading;
    
70.   }
    
71. }
    
72. 
    
73. float convertRawAcceleration(int aRaw) {
    
74.   // since we are using 2G range
    
75.   // -2g maps to a raw value of -32768
    
76.   // +2g maps to a raw value of 32767
    
77. 
    
78.   float a = (aRaw * 2.0) / 32768.0;
    
79.   return a;
    
80. }
    
81. 
    
82. float convertRawGyro(int gRaw) {
    
83.   // since we are using 250 degrees/seconds range
    
84.   // -250 maps to a raw value of -32768
    
85.   // +250 maps to a raw value of 32767
    
86. 
    
87.   float g = (gRaw * 250.0) / 32768.0;
    
88.   return g;
    
89. }

复制代码

然后就可以通过串口助手来观察板子当前所处的状态：



但是通过这个算法得到的yaw值会漂，所以希望得到更精确的值，在板子上面有另一个模块hmc5883l电子罗盘，我们可以使用电子罗盘来校准yaw的值。
我目前为止没有什么进展，因为姿态融合这一块本来就比较难，相关资料也比较少，也希望有同方向一起研究的人。

后面更新就如何在在上述代码下启用hmc5883l，并通过这个电子罗盘来校准我们得到的姿态值。

feixiang20

多找点资料学呗