LAURUS / Mbed 2 deprecated Laurus_acc_gyro

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

Dependencies:   mbed

main.cpp@3:40559ebef0f1, 2015-05-13 (annotated)

Committer:
ojan
Date:
Wed May 13 04:02:27 2015 +0000
Revision:
3:40559ebef0f1
Parent:
2:4a6b46653abf 
estimate gravity vector & geomagnetic vector

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 3:40559ebef0f1 7 #include "MPU6050.h"
ojan 3:40559ebef0f1 8 #include "HMC5883L.h"
ojan 2:4a6b46653abf 9 /********** private define **********/
ojan 2:4a6b46653abf 10 /********** private macro **********/
ojan 0:5e220b09d315 11 /********** private typedef **********/
ojan 0:5e220b09d315 12 /********** private variables **********/
ojan 3:40559ebef0f1 13 const static float dt = 0.01f;
ojan 0:5e220b09d315 14
ojan 0:5e220b09d315 15 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
ojan 0:5e220b09d315 16 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 2:4a6b46653abf 17 I2C i2c(D14, D15); // mpu6050用I2Cオブジェクト
ojan 0:5e220b09d315 18 Ticker INS_ticker; // 割り込み用タイマー
ojan 3:40559ebef0f1 19 MPU6050 mpu6050(&i2c);
ojan 3:40559ebef0f1 20 HMC5883L hmc5883l(&i2c);
ojan 0:5e220b09d315 21
ojan 3:40559ebef0f1 22 //const int mpu6050_addr = 0xd0; // mpu6050アドレス
ojan 3:40559ebef0f1 23 //const int hmc5883l_addr = 0x3C; // hmc5883lアドレス
ojan 0:5e220b09d315 24 volatile int ret = 0; // I2C関数の返り値保存用
ojan 0:5e220b09d315 25 uint8_t cmd[2] = {}; // I2C送信データ
ojan 0:5e220b09d315 26 uint8_t data[14] = {}; // I2C受信データ
ojan 0:5e220b09d315 27 uint16_t Tempr = 0; // 温度
ojan 2:4a6b46653abf 28 //int16_t acc[3] = {}; // 加速度
ojan 2:4a6b46653abf 29 //int16_t gyro[3] = {}; // 角速度
ojan 2:4a6b46653abf 30 //int16_t mag[3] = {}; // 地磁気
ojan 2:4a6b46653abf 31 Vector acc(3);
ojan 2:4a6b46653abf 32 Vector gyro(3);
ojan 3:40559ebef0f1 33 Vector mag(3);
ojan 2:4a6b46653abf 34 Vector g(3);
ojan 3:40559ebef0f1 35 Vector n(3);
ojan 3:40559ebef0f1 36 Vector v_acc(3);
ojan 3:40559ebef0f1 37 Vector v(3);
ojan 0:5e220b09d315 38 float theta[2] = {}; // ロール、ピッチ角
ojan 0:5e220b09d315 39
ojan 0:5e220b09d315 40 char text[256]; // デバッグ用文字列
ojan 0:5e220b09d315 41
ojan 0:5e220b09d315 42 /********** private functions **********/
ojan 0:5e220b09d315 43
ojan 0:5e220b09d315 44 void INS_IntFunc(); // センサー値取得用割り込み関数
ojan 0:5e220b09d315 45
ojan 0:5e220b09d315 46 /********** main function **********/
ojan 0:5e220b09d315 47
ojan 0:5e220b09d315 48 int main() {
ojan 0:5e220b09d315 49
ojan 2:4a6b46653abf 50 i2c.frequency(400000); // mpu6050との通信は400kHz
ojan 3:40559ebef0f1 51 mpu6050.init();
ojan 3:40559ebef0f1 52 hmc5883l.init();
ojan 0:5e220b09d315 53
ojan 0:5e220b09d315 54 // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
ojan 3:40559ebef0f1 55 /*cmd[0] = 0x6b;
ojan 0:5e220b09d315 56 cmd[1] = 0x00;
ojan 2:4a6b46653abf 57 ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
ojan 3:40559ebef0f1 58 pc.printf("ret = %d ", ret);*/
ojan 3:40559ebef0f1 59
ojan 3:40559ebef0f1 60 // 地磁気センサの初期値を取得
ojan 3:40559ebef0f1 61 hmc5883l.read();
ojan 3:40559ebef0f1 62 mag.SetComp(1, (float)hmc5883l.data.value[0]);
ojan 3:40559ebef0f1 63 mag.SetComp(2, (float)hmc5883l.data.value[1]);
ojan 3:40559ebef0f1 64 mag.SetComp(3, (float)hmc5883l.data.value[2]);
ojan 3:40559ebef0f1 65
ojan 3:40559ebef0f1 66 mag = mag.Normalize();
ojan 3:40559ebef0f1 67
ojan 3:40559ebef0f1 68 v.SetComp(1, 0.0f);
ojan 3:40559ebef0f1 69 v.SetComp(2, 0.0f);
ojan 3:40559ebef0f1 70 v.SetComp(3, 0.0f);
ojan 3:40559ebef0f1 71
ojan 3:40559ebef0f1 72 g.SetComp(1, 0.0f);
ojan 3:40559ebef0f1 73 g.SetComp(2, 0.0f);
ojan 3:40559ebef0f1 74 g.SetComp(3, 1.0f);
ojan 0:5e220b09d315 75
ojan 0:5e220b09d315 76 // センサー値の取得・計算は割り込み関数内で行う。
ojan 0:5e220b09d315 77 // 割り込み周期は10ms（10000μs）
ojan 3:40559ebef0f1 78 INS_ticker.attach_us(&INS_IntFunc, 1000000 * dt);
ojan 0:5e220b09d315 79
ojan 0:5e220b09d315 80 while(1) {
ojan 0:5e220b09d315 81 // メインループではひたすらLEDチカチカ
ojan 0:5e220b09d315 82 myled = 1; // LED is ON
ojan 3:40559ebef0f1 83 wait(0.05); // 50 ms
ojan 3:40559ebef0f1 84
ojan 3:40559ebef0f1 85 pc.printf("%.3f\t", g.GetComp(1));
ojan 3:40559ebef0f1 86 pc.printf("%.3f\t", g.GetComp(2));
ojan 3:40559ebef0f1 87 pc.printf("%.3f\t", g.GetComp(3));
ojan 3:40559ebef0f1 88 pc.printf("%.3f\t", n.GetComp(1));
ojan 3:40559ebef0f1 89 pc.printf("%.3f\t", n.GetComp(2));
ojan 3:40559ebef0f1 90 pc.printf("%.3f\t", n.GetComp(3));
ojan 3:40559ebef0f1 91 pc.printf("%.3f\t", v.GetComp(1));
ojan 3:40559ebef0f1 92 pc.printf("%.3f\t", v.GetComp(2));
ojan 3:40559ebef0f1 93 pc.printf("%.3f\r\n", v.GetComp(3));
ojan 3:40559ebef0f1 94
ojan 0:5e220b09d315 95 myled = 0; // LED is OFF
ojan 3:40559ebef0f1 96 wait(0.05); // 1 sec
ojan 0:5e220b09d315 97
ojan 0:5e220b09d315 98 }
ojan 0:5e220b09d315 99 }
ojan 0:5e220b09d315 100
ojan 0:5e220b09d315 101 void INS_IntFunc() {
ojan 0:5e220b09d315 102
ojan 0:5e220b09d315 103 // 0x3bレジスタからデータの読み取りを行う
ojan 3:40559ebef0f1 104 /*cmd[0] = 0x3b;
ojan 2:4a6b46653abf 105 ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
ojan 2:4a6b46653abf 106 i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
ojan 0:5e220b09d315 107
ojan 0:5e220b09d315 108 // 各データを加速度、角速度にそれぞれ突っ込む
ojan 0:5e220b09d315 109 for(int i=0; i<3; i++) {
ojan 2:4a6b46653abf 110 int16_t acc_temp = 0;
ojan 2:4a6b46653abf 111 acc_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
ojan 2:4a6b46653abf 112 acc.SetComp(i+1, -(float)acc_temp * ACC_LSB_TO_G);
ojan 0:5e220b09d315 113 }
ojan 0:5e220b09d315 114 Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
ojan 0:5e220b09d315 115 for(int i=4; i<7; i++) {
ojan 2:4a6b46653abf 116 int16_t gyro_temp = 0;
ojan 2:4a6b46653abf 117 gyro_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
ojan 2:4a6b46653abf 118 gyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 3:40559ebef0f1 119 }*/
ojan 3:40559ebef0f1 120
ojan 3:40559ebef0f1 121 mpu6050.read();
ojan 3:40559ebef0f1 122 hmc5883l.read();
ojan 3:40559ebef0f1 123
ojan 3:40559ebef0f1 124 for(int i=0; i<3; i++) {
ojan 3:40559ebef0f1 125 acc.SetComp(i+1, (float)mpu6050.data.value.acc[i] * ACC_LSB_TO_G);
ojan 3:40559ebef0f1 126 gyro.SetComp(i+1, (float)mpu6050.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 3:40559ebef0f1 127 mag.SetComp(i+1, (float)hmc5883l.data.value[i]);
ojan 0:5e220b09d315 128 }
ojan 0:5e220b09d315 129
ojan 3:40559ebef0f1 130 //acc = acc.Normalize(); // 欲しいのは方向のみなので単位ベクトル化
ojan 3:40559ebef0f1 131 //mag = mag.Normalize(); // 欲しいのは方向のみなので単位ベクトル化
ojan 3:40559ebef0f1 132
ojan 3:40559ebef0f1 133 /*pc.printf("%.4f\t", acc.GetComp(1));
ojan 3:40559ebef0f1 134 pc.printf("%.4f\t", acc.GetComp(2));
ojan 3:40559ebef0f1 135 pc.printf("%.4f\t", acc.GetComp(3));
ojan 3:40559ebef0f1 136 pc.printf("%.4f\t", gyro.GetComp(1));
ojan 3:40559ebef0f1 137 pc.printf("%.4f\t", gyro.GetComp(2));
ojan 3:40559ebef0f1 138 pc.printf("%.4f\t", gyro.GetComp(3));
ojan 3:40559ebef0f1 139 pc.printf("%.4f\t", mag.GetComp(1));
ojan 3:40559ebef0f1 140 pc.printf("%.4f\t", mag.GetComp(2));
ojan 3:40559ebef0f1 141 pc.printf("%.4f\r\n", mag.GetComp(3));
ojan 3:40559ebef0f1 142 */
ojan 2:4a6b46653abf 143 // 重力ベクトルを推定
ojan 2:4a6b46653abf 144 {
ojan 3:40559ebef0f1 145 Vector delta_g = Cross(gyro, g); // Δg = ω × g
ojan 3:40559ebef0f1 146 Vector delta_n = Cross(gyro, n); // Δf = ω × f
ojan 3:40559ebef0f1 147
ojan 2:4a6b46653abf 148
ojan 2:4a6b46653abf 149 // 相補フィルタを使ってみる
ojan 3:40559ebef0f1 150 //g = g + delta * dt;
ojan 3:40559ebef0f1 151 g = 0.9f * (g + delta_g * dt) + 0.1f * acc.Normalize();
ojan 3:40559ebef0f1 152 g = g.Normalize();
ojan 3:40559ebef0f1 153 n = 0.9f * (n + delta_n * dt) + 0.1f * mag.Normalize();
ojan 3:40559ebef0f1 154 n = n.Normalize();
ojan 3:40559ebef0f1 155
ojan 3:40559ebef0f1 156 v_acc = G_TO_MPSS * (acc - (acc * g) * g);
ojan 3:40559ebef0f1 157
ojan 3:40559ebef0f1 158 v += v_acc * dt;
ojan 2:4a6b46653abf 159
ojan 2:4a6b46653abf 160 // 推定結果をPCに送信
ojan 3:40559ebef0f1 161 /*pc.printf("%.3f\t", g.GetComp(1));
ojan 3:40559ebef0f1 162 pc.printf("%.3f\t", g.GetComp(2));
ojan 3:40559ebef0f1 163 pc.printf("%.3f\t", g.GetComp(3));
ojan 3:40559ebef0f1 164 pc.printf("%.3f\t", n.GetComp(1));
ojan 3:40559ebef0f1 165 pc.printf("%.3f\t", n.GetComp(2));
ojan 3:40559ebef0f1 166 pc.printf("%.3f\t", n.GetComp(3));
ojan 3:40559ebef0f1 167 pc.printf("%.3f\t", v.GetComp(1));
ojan 3:40559ebef0f1 168 pc.printf("%.3f\t", v.GetComp(2));
ojan 3:40559ebef0f1 169 pc.printf("%.3f\r\n", v.GetComp(3));*/
ojan 2:4a6b46653abf 170
ojan 3:40559ebef0f1 171
ojan 3:40559ebef0f1 172
ojan 3:40559ebef0f1 173 //pc.printf("%f\t", (float)mpu6050.data.value.gyro[0] * GYRO_LSB_TO_DEG);
ojan 3:40559ebef0f1 174 //pc.printf("%f\t", (float)mpu6050.data.value.gyro[1] * GYRO_LSB_TO_DEG);
ojan 3:40559ebef0f1 175 //pc.printf("%f\r\n", (float)mpu6050.data.value.gyro[2] * GYRO_LSB_TO_DEG);
ojan 3:40559ebef0f1 176
ojan 3:40559ebef0f1 177 }
ojan 2:4a6b46653abf 178
ojan 2:4a6b46653abf 179 /*
ojan 0:5e220b09d315 180 // 各センサー値からセンサーの姿勢・角速度を計算
ojan 2:4a6b46653abf 181 float roll_acc = (atan2((float)acc[0], (float)acc[2]) * RAD_TO_DEG);
ojan 0:5e220b09d315 182 float roll_ratio_gyro = (float)gyro[1] / 65.5f;
ojan 2:4a6b46653abf 183 float pitch_acc = (atan2((float)acc[1], (float)acc[2]) * RAD_TO_DEG);
ojan 0:5e220b09d315 184 float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
ojan 0:5e220b09d315 185
ojan 0:5e220b09d315 186 // 相補フィルタを用いて角度を更新
ojan 2:4a6b46653abf 187 theta[0] = 0.98f * (theta[0] - roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
ojan 0:5e220b09d315 188 theta[1] = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;
ojan 0:5e220b09d315 189
ojan 0:5e220b09d315 190 // 推定された角度をパソコンに送信
ojan 0:5e220b09d315 191 pc.printf("%.4f\t", theta[0]);
ojan 0:5e220b09d315 192 pc.printf("%.4f\r\n", theta[1]);
ojan 2:4a6b46653abf 193 */
ojan 0:5e220b09d315 194 }