LAURUS / Mbed 2 deprecated Laurus_acc_gyro

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

Dependencies:   mbed

main.cpp@1:2eca9b376580, 2015-04-16 (annotated)

Committer:
ojan
Date:
Thu Apr 16 08:51:04 2015 +0000
Revision:
1:2eca9b376580
Parent:
0:5e220b09d315
Child:
2:4a6b46653abf
???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 0:5e220b09d315 1 #include "mbed.h"
ojan 0:5e220b09d315 2 #include "toString.h"
ojan 1:2eca9b376580 3 #include "ErrorLogger.h"
ojan 0:5e220b09d315 4 /********** private typedef **********/
ojan 0:5e220b09d315 5 /********** public variables **********/
ojan 0:5e220b09d315 6 /********** public functions **********/
ojan 0:5e220b09d315 7 /********** private variables **********/
ojan 0:5e220b09d315 8
ojan 0:5e220b09d315 9 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
ojan 0:5e220b09d315 10 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 0:5e220b09d315 11 I2C mpu6050(D14, D15); // mpu6050用I2Cオブジェクト
ojan 0:5e220b09d315 12 Ticker INS_ticker; // 割り込み用タイマー
ojan 0:5e220b09d315 13
ojan 0:5e220b09d315 14 const int addr = 0xd0; // mpu6050アドレス
ojan 0:5e220b09d315 15 volatile int ret = 0; // I2C関数の返り値保存用
ojan 0:5e220b09d315 16 uint8_t cmd[2] = {}; // I2C送信データ
ojan 0:5e220b09d315 17 uint8_t data[14] = {}; // I2C受信データ
ojan 0:5e220b09d315 18 uint16_t Tempr = 0; // 温度
ojan 0:5e220b09d315 19 int16_t acc[3] = {}; // 加速度
ojan 0:5e220b09d315 20 int16_t gyro[3] = {}; // 角速度
ojan 0:5e220b09d315 21 float theta[2] = {}; // ロール、ピッチ角
ojan 0:5e220b09d315 22
ojan 0:5e220b09d315 23 char text[256]; // デバッグ用文字列
ojan 0:5e220b09d315 24
ojan 0:5e220b09d315 25 /********** private functions **********/
ojan 0:5e220b09d315 26
ojan 0:5e220b09d315 27 void INS_IntFunc(); // センサー値取得用割り込み関数
ojan 0:5e220b09d315 28
ojan 0:5e220b09d315 29 /********** main function **********/
ojan 0:5e220b09d315 30
ojan 0:5e220b09d315 31 int main() {
ojan 0:5e220b09d315 32
ojan 0:5e220b09d315 33 mpu6050.frequency(400000); // mpu6050との通信は400kHz
ojan 0:5e220b09d315 34
ojan 0:5e220b09d315 35 // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
ojan 0:5e220b09d315 36 cmd[0] = 0x6b;
ojan 0:5e220b09d315 37 cmd[1] = 0x00;
ojan 0:5e220b09d315 38 ret = mpu6050.write(addr, (char*)cmd, 2);
ojan 0:5e220b09d315 39 pc.printf("ret = %d ", ret);
ojan 0:5e220b09d315 40
ojan 0:5e220b09d315 41 // センサー値の取得・計算は割り込み関数内で行う。
ojan 0:5e220b09d315 42 // 割り込み周期は10ms（10000μs）
ojan 0:5e220b09d315 43 INS_ticker.attach_us(&INS_IntFunc, 10000);
ojan 0:5e220b09d315 44
ojan 0:5e220b09d315 45 while(1) {
ojan 0:5e220b09d315 46 // メインループではひたすらLEDチカチカ
ojan 0:5e220b09d315 47 myled = 1; // LED is ON
ojan 0:5e220b09d315 48 wait(0.2); // 200 ms
ojan 0:5e220b09d315 49 myled = 0; // LED is OFF
ojan 0:5e220b09d315 50 wait(1.0); // 1 sec
ojan 0:5e220b09d315 51
ojan 0:5e220b09d315 52 }
ojan 0:5e220b09d315 53 }
ojan 0:5e220b09d315 54
ojan 0:5e220b09d315 55 void INS_IntFunc() {
ojan 0:5e220b09d315 56
ojan 0:5e220b09d315 57 // 0x3bレジスタからデータの読み取りを行う
ojan 0:5e220b09d315 58 cmd[0] = 0x3b;
ojan 0:5e220b09d315 59 ret = mpu6050.write(addr, (char*)cmd, 1, true);
ojan 0:5e220b09d315 60 mpu6050.read(addr | 0x01, (char*)data, 14, false);
ojan 0:5e220b09d315 61
ojan 0:5e220b09d315 62 // 各データを加速度、角速度にそれぞれ突っ込む
ojan 0:5e220b09d315 63 for(int i=0; i<3; i++) {
ojan 0:5e220b09d315 64 acc[i] = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
ojan 0:5e220b09d315 65 }
ojan 0:5e220b09d315 66 Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
ojan 0:5e220b09d315 67 for(int i=4; i<7; i++) {
ojan 0:5e220b09d315 68 gyro[i-4] = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
ojan 0:5e220b09d315 69 }
ojan 0:5e220b09d315 70
ojan 0:5e220b09d315 71 // 各センサー値からセンサーの姿勢・角速度を計算
ojan 0:5e220b09d315 72 float roll_acc = (180.0f * atan2((float)acc[0], (float)acc[2]) / 3.1415f);
ojan 0:5e220b09d315 73 float roll_ratio_gyro = (float)gyro[1] / 65.5f;
ojan 0:5e220b09d315 74 float pitch_acc = (180.0f * atan2((float)acc[1], (float)acc[2]) / 3.1415f);
ojan 0:5e220b09d315 75 float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
ojan 0:5e220b09d315 76
ojan 0:5e220b09d315 77 // 相補フィルタを用いて角度を更新
ojan 0:5e220b09d315 78 theta[0] = 0.98f * (theta[0] + roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
ojan 0:5e220b09d315 79 theta[1] = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;
ojan 0:5e220b09d315 80
ojan 0:5e220b09d315 81 // 推定された角度をパソコンに送信
ojan 0:5e220b09d315 82 pc.printf("%.4f\t", theta[0]);
ojan 0:5e220b09d315 83 pc.printf("%.4f\r\n", theta[1]);
ojan 0:5e220b09d315 84 }