LAURUS
read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle
Dependencies: mbed
Diff: main.cpp
- Revision:
- 3:40559ebef0f1
- Parent:
- 2:4a6b46653abf
--- a/main.cpp Mon Apr 20 14:54:55 2015 +0000
+++ b/main.cpp Wed May 13 04:02:27 2015 +0000
@@ -4,20 +4,23 @@
#include "ErrorLogger.h"
#include "Matrix.h"
#include "Vector.h"
+#include "MPU6050.h"
+#include "HMC5883L.h"
/********** private define **********/
/********** private macro **********/
/********** private typedef **********/
-/********** public variables **********/
-/********** public functions **********/
/********** private variables **********/
+const static float dt = 0.01f;
DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
I2C i2c(D14, D15); // mpu6050用I2Cオブジェクト
Ticker INS_ticker; // 割り込み用タイマー
+MPU6050 mpu6050(&i2c);
+HMC5883L hmc5883l(&i2c);
-const int mpu6050_addr = 0xd0; // mpu6050アドレス
-const int hmc5883l_addr = 0x3C; // hmc5883lアドレス
+//const int mpu6050_addr = 0xd0; // mpu6050アドレス
+//const int hmc5883l_addr = 0x3C; // hmc5883lアドレス
volatile int ret = 0; // I2C関数の返り値保存用
uint8_t cmd[2] = {}; // I2C送信データ
uint8_t data[14] = {}; // I2C受信データ
@@ -27,7 +30,11 @@
//int16_t mag[3] = {}; // 地磁気
Vector acc(3);
Vector gyro(3);
+Vector mag(3);
Vector g(3);
+Vector n(3);
+Vector v_acc(3);
+Vector v(3);
float theta[2] = {}; // ロール、ピッチ角
char text[256]; // デバッグ用文字列
@@ -41,23 +48,52 @@
int main() {
i2c.frequency(400000); // mpu6050との通信は400kHz
+ mpu6050.init();
+ hmc5883l.init();
// 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
- cmd[0] = 0x6b;
+ /*cmd[0] = 0x6b;
cmd[1] = 0x00;
ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
- pc.printf("ret = %d ", ret);
+ pc.printf("ret = %d ", ret);*/
+
+ // 地磁気センサの初期値を取得
+ hmc5883l.read();
+ mag.SetComp(1, (float)hmc5883l.data.value[0]);
+ mag.SetComp(2, (float)hmc5883l.data.value[1]);
+ mag.SetComp(3, (float)hmc5883l.data.value[2]);
+
+ mag = mag.Normalize();
+
+ v.SetComp(1, 0.0f);
+ v.SetComp(2, 0.0f);
+ v.SetComp(3, 0.0f);
+
+ g.SetComp(1, 0.0f);
+ g.SetComp(2, 0.0f);
+ g.SetComp(3, 1.0f);
// センサー値の取得・計算は割り込み関数内で行う。
// 割り込み周期は10ms（10000μs）
- INS_ticker.attach_us(&INS_IntFunc, 10000);
+ INS_ticker.attach_us(&INS_IntFunc, 1000000 * dt);
while(1) {
// メインループではひたすらLEDチカチカ
myled = 1; // LED is ON
- wait(0.2); // 200 ms
+ wait(0.05); // 50 ms
+
+ pc.printf("%.3f\t", g.GetComp(1));
+ pc.printf("%.3f\t", g.GetComp(2));
+ pc.printf("%.3f\t", g.GetComp(3));
+ pc.printf("%.3f\t", n.GetComp(1));
+ pc.printf("%.3f\t", n.GetComp(2));
+ pc.printf("%.3f\t", n.GetComp(3));
+ pc.printf("%.3f\t", v.GetComp(1));
+ pc.printf("%.3f\t", v.GetComp(2));
+ pc.printf("%.3f\r\n", v.GetComp(3));
+
myled = 0; // LED is OFF
- wait(1.0); // 1 sec
+ wait(0.05); // 1 sec
}
}
@@ -65,7 +101,7 @@
void INS_IntFunc() {
// 0x3bレジスタからデータの読み取りを行う
- cmd[0] = 0x3b;
+ /*cmd[0] = 0x3b;
ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
@@ -80,23 +116,65 @@
int16_t gyro_temp = 0;
gyro_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
gyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+ }*/
+
+ mpu6050.read();
+ hmc5883l.read();
+
+ for(int i=0; i<3; i++) {
+ acc.SetComp(i+1, (float)mpu6050.data.value.acc[i] * ACC_LSB_TO_G);
+ gyro.SetComp(i+1, (float)mpu6050.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+ mag.SetComp(i+1, (float)hmc5883l.data.value[i]);
}
+ //acc = acc.Normalize(); // 欲しいのは方向のみなので単位ベクトル化
+ //mag = mag.Normalize(); // 欲しいのは方向のみなので単位ベクトル化
+
+ /*pc.printf("%.4f\t", acc.GetComp(1));
+ pc.printf("%.4f\t", acc.GetComp(2));
+ pc.printf("%.4f\t", acc.GetComp(3));
+ pc.printf("%.4f\t", gyro.GetComp(1));
+ pc.printf("%.4f\t", gyro.GetComp(2));
+ pc.printf("%.4f\t", gyro.GetComp(3));
+ pc.printf("%.4f\t", mag.GetComp(1));
+ pc.printf("%.4f\t", mag.GetComp(2));
+ pc.printf("%.4f\r\n", mag.GetComp(3));
+ */
// 重力ベクトルを推定
{
- acc = acc.GetUnit(); // 欲しいのは方向のみなので、単位ベクトル化
- Vector delta = Cross(gyro, g); // Δg = ω × g
+ Vector delta_g = Cross(gyro, g); // Δg = ω × g
+ Vector delta_n = Cross(gyro, n); // Δf = ω × f
+
// 相補フィルタを使ってみる
- g = 0.9f * (g + 0.01f * delta) + 0.1f * acc;
- g = g.GetUnit();
+ //g = g + delta * dt;
+ g = 0.9f * (g + delta_g * dt) + 0.1f * acc.Normalize();
+ g = g.Normalize();
+ n = 0.9f * (n + delta_n * dt) + 0.1f * mag.Normalize();
+ n = n.Normalize();
+
+ v_acc = G_TO_MPSS * (acc - (acc * g) * g);
+
+ v += v_acc * dt;
// 推定結果をPCに送信
- pc.printf("%.4f\t", g.GetComp(1));
- pc.printf("%.4f\t", g.GetComp(2));
- pc.printf("%.4f\r\n", g.GetComp(3));
+ /*pc.printf("%.3f\t", g.GetComp(1));
+ pc.printf("%.3f\t", g.GetComp(2));
+ pc.printf("%.3f\t", g.GetComp(3));
+ pc.printf("%.3f\t", n.GetComp(1));
+ pc.printf("%.3f\t", n.GetComp(2));
+ pc.printf("%.3f\t", n.GetComp(3));
+ pc.printf("%.3f\t", v.GetComp(1));
+ pc.printf("%.3f\t", v.GetComp(2));
+ pc.printf("%.3f\r\n", v.GetComp(3));*/
- }
+
+
+ //pc.printf("%f\t", (float)mpu6050.data.value.gyro[0] * GYRO_LSB_TO_DEG);
+ //pc.printf("%f\t", (float)mpu6050.data.value.gyro[1] * GYRO_LSB_TO_DEG);
+ //pc.printf("%f\r\n", (float)mpu6050.data.value.gyro[2] * GYRO_LSB_TO_DEG);
+
+ }
/*
// 各センサー値からセンサーの姿勢・角速度を計算