入门教程二最后说教你用数据来干坏事,想了想,不能干,万一被警察叔叔抓去了就不好了,要做个良好市民。但是数据都读出来了,不用它干点啥多可惜,下面就用它来计算刚体的姿态角,这样,前面下载的库文件就派上用场了。
先热热身,扯一会,姿态角,这个词对玩四轴或者对四轴感兴趣的人一定不陌生,因为比较穷,菜鸟就默默做了一个感兴趣的人,先让我哭一会。
哭完了,继续,在我刚接触姿态角,并且想把它算出来的时候,是很困难的,先把加速度计,陀螺仪和电子托盘获取的数据折腾出来,再对数据滤波得到更加稳定有效的数据,用到互补滤波,卡尔曼滤波什么的,再对滤波后的数据处理计算出欧拉角,虽然有现成算法,但是弄明白这些算法的原理相当难!你可以试试哦,欢迎入坑!:victory:
咳咳,扯完了,干正事:
这里,如果撇开算法的理论,站在巨人的肩膀上,想得到姿态角还是挺容易的,直接调用库函数:MadgwickAHRS(四元数梯度下降算法),最后我们将得到的四元数转换成欧拉角。但是,要想得到比较直观的欧拉角,这里提供的库函数我们需要修改一下,因为现在的计算公式是为了另一个好玩的示例准备的,下个教程教你用,这回不骗你!:lol:
【产品链接】: Arduino 101
找到库文件MadgwickAHRS,如果你在前面安装了库Madgwick打开Arduino IDE的 文件 -> 首选项
复制上面的文件夹位置,在电脑里面搜索,会在文件夹下找到libraries/Madgwick,里面就有算法的源代码(我的完整目录是:C:\Users\LG\Documents\Arduino\libraries\Madgwick\src):
打开MadgwickAHRS.h文件,在头文件中我们能找到三个成员函数:getPitch(), getRoll(), getYaw() :
修改:
我们可以在原来三个成员函数的返回值各乘以57.3,也能得到欧拉角,因为后面我们还需要用到这个库,所以我们再添加三个成员函数,计算公式稍作改变,这三个获取欧拉角的计算公式我有点纠结,因为查资料我得到了不同版本的计算公式,我用了下面的,如果这个公式有问题,还望哪位大神及时纠正,非常感谢!:
float mygetPitch(){return atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;};
float mygetRoll(){return asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;};
float mygetYaw(){return atan2f( 2 * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3 )*57.3;};
最后的有文件MadgwickAHRS.h是这样的,保存一下:
下载测试程序,在源码的基础上,修改了获取欧拉角的三个成员函数:
- /*
- ===============================================
- Example sketch for CurieIMU library for Intel(R) Curie(TM) devices.
- Copyright (c) 2015 Intel Corporation. All rights reserved.
-
- Based on I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050
- class by Jeff Rowberg: https://github.com/jrowberg/i2cdevlib
-
- ===============================================
- I2Cdev device library code is placed under the MIT license
- Copyright (c) 2011 Jeff Rowberg
-
- Permission is hereby granted, free of charge, to any person obtaining a copy
- of this software and associated documentation files (the "Software"), to deal
- in the Software without restriction, including without limitation the rights
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- copies of the Software, and to permit persons to whom the Software is
- furnished to do so, subject to the following conditions:
-
- The above copyright notice and this permission notice shall be included in
- all copies or substantial portions of the Software.
-
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- THE SOFTWARE.
- ===============================================
-
- Genuino 101 CurieIMU Orientation Visualiser
- Hardware Required:
- * Arduino/Genuino 101
-
- Modified Nov 2015
- by Helena Bisby <support@arduino.cc>
- This example code is in the public domain
- https://arduino.cc/en/Tutorial/Genuino101CurieIMUOrientationVisualiser
- */
-
- #include <CurieIMU.h>
- #include <MadgwickAHRS.h>
-
- Madgwick filter; // initialise Madgwick object
- int ax, ay, az;
- int gx, gy, gz;
- float yaw;
- float pitch;
- float roll;
- int factor = 800; // variable by which to divide gyroscope values, used to control sensitivity
- // note that an increased baud rate requires an increase in value of factor
-
- int calibrateOffsets = 1; // int to determine whether calibration takes place or not
-
-
- void setup() {
- // initialize Serial communication
- Serial.begin(9600);
-
- // initialize device
- CurieIMU.begin();
-
- if (calibrateOffsets == 1) {
- // use the code below to calibrate accel/gyro offset values
- Serial.println("Internal sensor offsets BEFORE calibration...");
- Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
- Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
- Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
- Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
- Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
- Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
- Serial.println("");
-
- Serial.print("Starting Gyroscope calibration...");
- CurieIMU.autoCalibrateGyroOffset();
- Serial.println(" Done");
- Serial.print("Starting Acceleration calibration...");
- CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
- CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
- CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
- Serial.println(" Done");
- }
- }
-
- void loop() {
- // read raw accel/gyro measurements from device
- CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz);
-
- // use function from MagdwickAHRS.h to return quaternions
- filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
-
- // functions to find yaw roll and pitch from quaternions
- //yaw = filter.mygetYaw();
- roll = filter.mygetRoll();
- pitch = filter.mygetPitch();
-
- // print gyro and accel values for debugging only, comment out when running Processing
-
- Serial.print(0);
- Serial.print(","); // print comma so values can be parsed
- Serial.print(pitch);
- Serial.print(","); // print comma so values can be parsed
- Serial.println(roll);
-
-
- /* Serial.print(ax); Serial.print("\t");
- Serial.print(ay); Serial.print("\t");
- Serial.print(az); Serial.print("\t");
- Serial.print(gx); Serial.print("\t");
- Serial.print(gy); Serial.print("\t");
- Serial.print(gz); Serial.print("\t");
- Serial.println("");
-
-
- if (Serial.available() > 0) {
- int val = Serial.read();
- if (val == 's') { // if incoming serial is "s"
- Serial.print(yaw);
- Serial.print(","); // print comma so values can be parsed
- Serial.print(pitch);
- Serial.print(","); // print comma so values can be parsed
- Serial.println(roll);
- }
- }*/
- }
复制代码
串口得到的数据,因为没有电子罗盘对Yaw的数据进行修正,这个数据是会飘的,不准确,所以注释了对他的计算,你可以打开试试,我把板子绕X轴旋转90度得到的数据。
OK,到这里我们能得到板子的姿态角了,这里有些问题,希望大神能指点一二:
1.在库函数原来的计算公式后面乘以57.3也能得到欧拉角,两个欧拉角的计算公式不同,都对吗,还是都存在问题?
2.在单独获取Pitch数据的时候比较稳定,基本对Roll没有影响,但是最后数据收敛的速度有点慢,不知道为什么?
3.在单独获取Roll数据的时候,Pitch数据也有变动,不应该是这样的,这是为什么?
拓展:
在CurieIMU这个驱动库的examples里面还有很多示例可以玩起来,下面没有写进教程的例子简单说明一下:
AccelerometerOrientation:当你转动开发板的时候,示例会告诉你你现在开发板是处于什么状态,例如:正面并且数字引脚向上,串口会显示:
up
digital pins up"
RawImuDataSerial:读取加速度计和陀螺仪的原始数据,可以读出来看看
ShockDetect:检测来自坐标轴方向的震动,体验不是很好
StepDetect:简单计步器算法,能工作,体验不是很好
TapDetect:检测来自坐标轴方向的敲击,体验不是很好
补充一下:
你可能会发现,在计算四元数的时候用到了下面的成员函数:
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
代码中是这样调用的:
filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
问题来了,这里的参数只有加速度计和陀螺仪的,那么,如果我们拿到了集成了电子罗盘的DFRobot 版Inter Curie,怎么把电子罗盘的数据融合进去呢?不用担心哦,在算法库里面还有个成员函数能帮你解决问题:
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
现在你可以玩九轴了,是不是很开心!
看累了没,开心一刻:
朋友们一起去爬山,到山顶,一女生面对秀丽山河高喊:祖国啊!我的母亲!一暗恋她的男生赶紧跟着大喊:祖国啊!我的丈母娘!
【Arduino/Genuino 101 入门教程】
* DF创客社区版权所有,欢迎转载。
转载请务必标注来源: DF创客社区+作者姓名+原文网址。
|
|