LAURUS / Mbed 2 deprecated Laurus_acc_gyro

read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle

Dependencies:   mbed

main.cpp@2:4a6b46653abf, 2015-04-20 (annotated)

Committer:
ojan
Date:
Mon Apr 20 14:54:55 2015 +0000
Revision:
2:4a6b46653abf
Parent:
1:2eca9b376580
Child:
3:40559ebef0f1
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 0:5e220b09d315 1 #include "mbed.h"
ojan 2:4a6b46653abf 2 #include "myConstants.h"
ojan 0:5e220b09d315 3 #include "toString.h"
ojan 1:2eca9b376580 4 #include "ErrorLogger.h"
ojan 2:4a6b46653abf 5 #include "Matrix.h"
ojan 2:4a6b46653abf 6 #include "Vector.h"
ojan 2:4a6b46653abf 7 /********** private define **********/
ojan 2:4a6b46653abf 8 /********** private macro **********/
ojan 0:5e220b09d315 9 /********** private typedef **********/
ojan 0:5e220b09d315 10 /********** public variables **********/
ojan 0:5e220b09d315 11 /********** public functions **********/
ojan 0:5e220b09d315 12 /********** private variables **********/
ojan 0:5e220b09d315 13
ojan 0:5e220b09d315 14 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
ojan 0:5e220b09d315 15 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 2:4a6b46653abf 16 I2C i2c(D14, D15); // mpu6050用I2Cオブジェクト
ojan 0:5e220b09d315 17 Ticker INS_ticker; // 割り込み用タイマー
ojan 0:5e220b09d315 18
ojan 2:4a6b46653abf 19 const int mpu6050_addr = 0xd0; // mpu6050アドレス
ojan 2:4a6b46653abf 20 const int hmc5883l_addr = 0x3C; // hmc5883lアドレス
ojan 0:5e220b09d315 21 volatile int ret = 0; // I2C関数の返り値保存用
ojan 0:5e220b09d315 22 uint8_t cmd[2] = {}; // I2C送信データ
ojan 0:5e220b09d315 23 uint8_t data[14] = {}; // I2C受信データ
ojan 0:5e220b09d315 24 uint16_t Tempr = 0; // 温度
ojan 2:4a6b46653abf 25 //int16_t acc[3] = {}; // 加速度
ojan 2:4a6b46653abf 26 //int16_t gyro[3] = {}; // 角速度
ojan 2:4a6b46653abf 27 //int16_t mag[3] = {}; // 地磁気
ojan 2:4a6b46653abf 28 Vector acc(3);
ojan 2:4a6b46653abf 29 Vector gyro(3);
ojan 2:4a6b46653abf 30 Vector g(3);
ojan 0:5e220b09d315 31 float theta[2] = {}; // ロール、ピッチ角
ojan 0:5e220b09d315 32
ojan 0:5e220b09d315 33 char text[256]; // デバッグ用文字列
ojan 0:5e220b09d315 34
ojan 0:5e220b09d315 35 /********** private functions **********/
ojan 0:5e220b09d315 36
ojan 0:5e220b09d315 37 void INS_IntFunc(); // センサー値取得用割り込み関数
ojan 0:5e220b09d315 38
ojan 0:5e220b09d315 39 /********** main function **********/
ojan 0:5e220b09d315 40
ojan 0:5e220b09d315 41 int main() {
ojan 0:5e220b09d315 42
ojan 2:4a6b46653abf 43 i2c.frequency(400000); // mpu6050との通信は400kHz
ojan 0:5e220b09d315 44
ojan 0:5e220b09d315 45 // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
ojan 0:5e220b09d315 46 cmd[0] = 0x6b;
ojan 0:5e220b09d315 47 cmd[1] = 0x00;
ojan 2:4a6b46653abf 48 ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
ojan 0:5e220b09d315 49 pc.printf("ret = %d ", ret);
ojan 0:5e220b09d315 50
ojan 0:5e220b09d315 51 // センサー値の取得・計算は割り込み関数内で行う。
ojan 0:5e220b09d315 52 // 割り込み周期は10ms（10000μs）
ojan 2:4a6b46653abf 53 INS_ticker.attach_us(&INS_IntFunc, 10000);
ojan 0:5e220b09d315 54
ojan 0:5e220b09d315 55 while(1) {
ojan 0:5e220b09d315 56 // メインループではひたすらLEDチカチカ
ojan 0:5e220b09d315 57 myled = 1; // LED is ON
ojan 0:5e220b09d315 58 wait(0.2); // 200 ms
ojan 0:5e220b09d315 59 myled = 0; // LED is OFF
ojan 0:5e220b09d315 60 wait(1.0); // 1 sec
ojan 0:5e220b09d315 61
ojan 0:5e220b09d315 62 }
ojan 0:5e220b09d315 63 }
ojan 0:5e220b09d315 64
ojan 0:5e220b09d315 65 void INS_IntFunc() {
ojan 0:5e220b09d315 66
ojan 0:5e220b09d315 67 // 0x3bレジスタからデータの読み取りを行う
ojan 0:5e220b09d315 68 cmd[0] = 0x3b;
ojan 2:4a6b46653abf 69 ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
ojan 2:4a6b46653abf 70 i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
ojan 0:5e220b09d315 71
ojan 0:5e220b09d315 72 // 各データを加速度、角速度にそれぞれ突っ込む
ojan 0:5e220b09d315 73 for(int i=0; i<3; i++) {
ojan 2:4a6b46653abf 74 int16_t acc_temp = 0;
ojan 2:4a6b46653abf 75 acc_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
ojan 2:4a6b46653abf 76 acc.SetComp(i+1, -(float)acc_temp * ACC_LSB_TO_G);
ojan 0:5e220b09d315 77 }
ojan 0:5e220b09d315 78 Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
ojan 0:5e220b09d315 79 for(int i=4; i<7; i++) {
ojan 2:4a6b46653abf 80 int16_t gyro_temp = 0;
ojan 2:4a6b46653abf 81 gyro_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
ojan 2:4a6b46653abf 82 gyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 0:5e220b09d315 83 }
ojan 0:5e220b09d315 84
ojan 2:4a6b46653abf 85 // 重力ベクトルを推定
ojan 2:4a6b46653abf 86 {
ojan 2:4a6b46653abf 87 acc = acc.GetUnit(); // 欲しいのは方向のみなので、単位ベクトル化
ojan 2:4a6b46653abf 88 Vector delta = Cross(gyro, g); // Δg = ω × g
ojan 2:4a6b46653abf 89
ojan 2:4a6b46653abf 90 // 相補フィルタを使ってみる
ojan 2:4a6b46653abf 91 g = 0.9f * (g + 0.01f * delta) + 0.1f * acc;
ojan 2:4a6b46653abf 92 g = g.GetUnit();
ojan 2:4a6b46653abf 93
ojan 2:4a6b46653abf 94 // 推定結果をPCに送信
ojan 2:4a6b46653abf 95 pc.printf("%.4f\t", g.GetComp(1));
ojan 2:4a6b46653abf 96 pc.printf("%.4f\t", g.GetComp(2));
ojan 2:4a6b46653abf 97 pc.printf("%.4f\r\n", g.GetComp(3));
ojan 2:4a6b46653abf 98
ojan 2:4a6b46653abf 99 }
ojan 2:4a6b46653abf 100
ojan 2:4a6b46653abf 101 /*
ojan 0:5e220b09d315 102 // 各センサー値からセンサーの姿勢・角速度を計算
ojan 2:4a6b46653abf 103 float roll_acc = (atan2((float)acc[0], (float)acc[2]) * RAD_TO_DEG);
ojan 0:5e220b09d315 104 float roll_ratio_gyro = (float)gyro[1] / 65.5f;
ojan 2:4a6b46653abf 105 float pitch_acc = (atan2((float)acc[1], (float)acc[2]) * RAD_TO_DEG);
ojan 0:5e220b09d315 106 float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
ojan 0:5e220b09d315 107
ojan 0:5e220b09d315 108 // 相補フィルタを用いて角度を更新
ojan 2:4a6b46653abf 109 theta[0] = 0.98f * (theta[0] - roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
ojan 0:5e220b09d315 110 theta[1] = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;
ojan 0:5e220b09d315 111
ojan 0:5e220b09d315 112 // 推定された角度をパソコンに送信
ojan 0:5e220b09d315 113 pc.printf("%.4f\t", theta[0]);
ojan 0:5e220b09d315 114 pc.printf("%.4f\r\n", theta[1]);
ojan 2:4a6b46653abf 115 */
ojan 0:5e220b09d315 116 }