LAURUS / Mbed 2 deprecated Laurus_acc_gyro

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

Dependencies:   mbed

main.cpp@0:5e220b09d315, 2015-04-13 (annotated)

Committer:
ojan
Date:
Mon Apr 13 08:10:05 2015 +0000
Revision:
0:5e220b09d315
Child:
1:2eca9b376580
read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle

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