albatross / Mbed 2 deprecated Laurus_acc_gyro

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Laurus_acc_gyro by LAURUS

main.cpp@4:8df0fc5dfd81, 2015-12-05 (annotated)

Committer:
taurin
Date:
Sat Dec 05 05:07:00 2015 +0000
Revision:
4:8df0fc5dfd81
Parent:
3:40559ebef0f1
Child:
5:a0e50699bfca
MPU6050??????????;

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