Revision 1:6cd6d2760856, committed 2015-05-24

Comitter:

ojan

Date:

Sun May 24 17:32:47 2015 +0000

Parent:

0:bc6f14fc60c7

Child:

2:d2b60a1d0cd9

Commit message:

Send all sensor's data to PC

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GMS6_CR6/GMS6_CR6.cpp	Sun May 24 17:32:47 2015 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "GMS6_CR6.h"
+
+GMS6_CR6::GMS6_CR6(Serial* ps, Serial* pc): p_port(ps), p_pc(pc) {
+    p_port->baud(4800);
+    pointer = 0;
+    INT_flag = 0;
+    p_port->attach(this, &GMS6_CR6::INT_Rx, Serial::RxIrq);
+}
+
+GMS6_CR6::~GMS6_CR6() {
+    p_port = NULL;
+}
+
+void GMS6_CR6::read() {
+    while(INT_flag);
+    int ret = sscanf(buff2, "$GPGGA,%f,%f,%c,%f,%c,%d,%d", 
+                &time, &raw_latitude, &lat_hem, &raw_longitude, &lng_hem);
+    if(!ret) {
+        p_pc->printf("sscanf Error\r\n");
+        return;
+    }
+    
+    int deg_lat = (int)raw_latitude / 100;
+    float min_lat = (raw_latitude - (float)(deg_lat*100)) / 60.0f;
+    latitude = (float)deg_lat + min_lat;
+    
+    int deg_lng = (int)raw_longitude / 100;
+    float min_lng = (raw_longitude - (float)(deg_lng*100)) / 60.0f;
+    longitude = (float)deg_lng + min_lng;
+    
+    
+}
+
+void GMS6_CR6::INT_Rx() {
+    buff1[pointer] = p_port->getc();
+    
+    if(buff1[pointer] != '\r') {
+        pointer = (pointer+1)%BuffSize;
+    } else {
+        if(strstr((const char*)buff1, "GPGGA") != NULL) {
+            buff1[pointer] = '\0';
+            INT_flag = 1;
+            strcpy(buff2, (const char*)buff1);
+            INT_flag = 0;
+            pointer = 0;
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GMS6_CR6/GMS6_CR6.h	Sun May 24 17:32:47 2015 +0000
@@ -0,0 +1,25 @@
+#pragma once
+
+const int BuffSize = 256;
+
+class GMS6_CR6 {
+    
+public: 
+    GMS6_CR6(Serial* ps, Serial* pc);
+    ~GMS6_CR6();
+    void INT_Rx();
+    void read();
+    
+    char lat_hem, lng_hem;
+    float raw_longitude, raw_latitude;
+    float longitude, latitude;
+    float time;
+      
+private: 
+    Serial* p_port;
+    Serial* p_pc;
+    char buff1[BuffSize];
+    char buff2[BuffSize];
+    int pointer;
+    volatile int INT_flag;
+};
\ No newline at end of file

--- a/HMC5883L/HMC5883L.cpp	Fri May 15 17:24:32 2015 +0000
+++ b/HMC5883L/HMC5883L.cpp	Sun May 24 17:32:47 2015 +0000
@@ -1,9 +1,7 @@
 #include "mbed.h"
 #include "HMC5883L.h"
 
-HMC5883L::HMC5883L(I2C* i2c) {
-    this->i2c = i2c;
-}
+HMC5883L::HMC5883L(I2C* p_i2c): i2c(p_i2c) {}
 
 HMC5883L::~HMC5883L() {
     i2c = NULL;
@@ -30,9 +28,15 @@
         data.reg[i*2+1] = temp;
     }
     
+    // 軸を加速度センサーとあわせる
     int16_t temp = data.value[1];
     data.value[1] = data.value[2];
     data.value[2] = temp;
     
+    temp = data.value[1];
+    data.value[1] = -data.value[0];
+    data.value[0] = temp;
+    
+    
     return 1;
 }
\ No newline at end of file

--- a/HMC5883L/HMC5883L.h	Fri May 15 17:24:32 2015 +0000
+++ b/HMC5883L/HMC5883L.h	Sun May 24 17:32:47 2015 +0000
@@ -19,7 +19,7 @@
 
 class HMC5883L {
 public:
-    HMC5883L(I2C* i2c);
+    HMC5883L(I2C* p_i2c);
     ~HMC5883L();
     
     int init();

--- a/MPU6050/MPU6050.cpp	Fri May 15 17:24:32 2015 +0000
+++ b/MPU6050/MPU6050.cpp	Sun May 24 17:32:47 2015 +0000
@@ -1,8 +1,7 @@
 #include "mbed.h"
 #include "MPU6050.h"
 
-MPU6050::MPU6050(I2C* i2c) {
-    this->i2c = i2c;
+MPU6050::MPU6050(I2C* p_i2c): i2c(p_i2c){
 }
 
 MPU6050::~MPU6050() {
@@ -10,10 +9,21 @@
 }
 
 int MPU6050::init() {
+    // スリープモード解除
     char cmd[2] = {0x6b, 0x00};
     int ret = i2c->write(mpu_addr, cmd, 2);
+    if(ret != 0) return 0;
     
+    // ジャイロのレンジを500deg/sに設定
+    char data = 0;
+    cmd[0] = 0x1b;
+    ret = i2c->write(mpu_addr, cmd, 1, true);
     if(ret != 0) return 0;
+    i2c->read(mpu_addr | 0x01, &data, 1, false);
+    cmd[1] = data | 0x08;
+    ret = i2c->write(mpu_addr, cmd, 2);
+    if(ret != 0) return 0;
+    
     return 1;
 }
 

--- a/MPU6050/MPU6050.h	Fri May 15 17:24:32 2015 +0000
+++ b/MPU6050/MPU6050.h	Sun May 24 17:32:47 2015 +0000
@@ -34,7 +34,7 @@
 
 class MPU6050 {
 public:
-    MPU6050(I2C* i2c);
+    MPU6050(I2C* p_i2c);
     ~MPU6050();
     
     int init();

--- a/main.cpp	Fri May 15 17:24:32 2015 +0000
+++ b/main.cpp	Sun May 24 17:32:47 2015 +0000
@@ -2,6 +2,7 @@
 #include "MPU6050.h"
 #include "HMC5883L.h"
 #include "LPS25H.h"
+#include "GMS6_CR6.h"
 #include "ErrorLogger.h"
 #include "Vector.h"
 #include "myConstants.h"
@@ -10,25 +11,35 @@
 #define TRUE    1
 #define FALSE   0
 /********** private macro     **********/
+
 /********** private typedef   **********/
+
 /********** private variables **********/
-I2C         i2c(I2C_SDA, I2C_SCL);  // I2Cポート
-MPU6050     mpu(&i2c);              // 加速度・角速度センサ
-HMC5883L    hmc(&i2c);              // 地磁気センサ
-LPS25H      lps(&i2c);              // 気圧センサ
-Serial pc(SERIAL_TX, SERIAL_RX);    // PC通信用シリアルポート
-Ticker timer;                       // 割り込みタイマー
+I2C         i2c(I2C_SDA, I2C_SCL);      // I2Cポート
+MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
+HMC5883L    hmc(&i2c);                  // 地磁気センサ
+LPS25H      lps(&i2c);                  // 気圧センサ
+Serial      gps(D8, D2);             // GPS通信用シリアルポート
+Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
+GMS6_CR6    gms(&gps, &pc);             // GPS
+Ticker      timer;                      // 割り込みタイマー
 
-const float freq = 0.01f;           // 割り込み周期（s）
+const float Freq = 0.01f;           // 割り込み周期（s）
 
 int lps_cnt = 0;                    // 気圧センサ読み取りカウント
-uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
-float acc[3];                       // 加速度(m/s^2)
-float gyro[3];                      // 角速度(deg/s)
-float geomag[3];                    // 地磁気(?)
+uint8_t INT_flag = FALSE;            // 割り込み可否フラグ
+Vector acc(3);                       // 加速度(m/s^2)
+Vector gyro(3);                      // 角速度(deg/s)
+Vector geomag(3);                    // 地磁気(?)
 float press;                        // 気圧(hPa)
+
+Vector g(3);                        // 重力ベクトル
+Vector n(3);                        // 地磁気ベクトル
+Vector bias(3);                     // 地磁気センサのバイアスベクトル
+
 /********** private functions **********/
 void INT_func();                    // 割り込み用関数
+
 /********** main function     **********/
 
 int main() {
@@ -38,23 +49,31 @@
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
     
-    timer.attach(&INT_func, freq);  // 割り込み有効化
+    timer.attach(&INT_func, Freq);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+    
+    //重力ベクトルの初期化
+    g.SetComp(1, 0.0f);
+    g.SetComp(2, 0.0f);
+    g.SetComp(3, 1.0f);
     
     while(1) {
         
+        // 1秒おきにセンサーの出力をpcへ出力
+        wait(1.0f);
+        
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
         // センサ類の全出力値をPCに送信
-        printf("%f\t", acc[0]);
-        printf("%f\t", acc[1]);
-        printf("%f\t", acc[2]);
-        printf("%f\t", gyro[0]);
-        printf("%f\t", gyro[1]);
-        printf("%f\t", gyro[2]);
-        printf("%f\t", geomag[0]);
-        printf("%f\t", geomag[1]);
-        printf("%f\t", geomag[2]);
-        printf("%f\r\n", press);
+        pc.printf("%f,", acc.GetComp(1));
+        pc.printf("%f,", acc.GetComp(2));
+        pc.printf("%f,", acc.GetComp(3));
+        pc.printf("%f,", gyro.GetComp(1));
+        pc.printf("%f,", gyro.GetComp(2));
+        pc.printf("%f,", gyro.GetComp(3));
+        pc.printf("%f,", geomag.GetComp(1));
+        pc.printf("%f,", geomag.GetComp(2));
+        pc.printf("%f,", geomag.GetComp(3));
+        pc.printf("%f\r\n", press);
         
         INT_flag = TRUE;            // 割り込み許可
         
@@ -64,15 +83,24 @@
 void INT_func() {
     if(INT_flag == FALSE) {
         
+        // センサーの値を更新
         mpu.read();
         hmc.read();
         
         for(int i=0; i<3; i++) {
-            acc[i] = (float)mpu.data.value.acc[i] * ACC_LSB_TO_G * G_TO_MPSS;
-            gyro[i] = mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG;
-            geomag[i] = hmc.data.value[i];
+            acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
+            gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+            geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
         }
         
+        Vector delta_g = Cross(gyro, g);                            // Δg = ω × g
+        g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize();   // 相補フィルタ
+        g = g.Normalize();
+        
+        Vector delta_n = Cross(gyro,n);
+        n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize();
+        n = n.Normalize();
+        
         // LPS25Hによる気圧の取得は10Hz
         lps_cnt = (lps_cnt+1)%10;
         if(lps_cnt == 0) {

--- a/myConstants.h	Fri May 15 17:24:32 2015 +0000
+++ b/myConstants.h	Sun May 24 17:32:47 2015 +0000
@@ -6,6 +6,11 @@
 #define DEG_TO_RAD          0.0174532925f           // π / 180
 #define ACC_LSB_TO_G        0.0000610351562f        // g/LSB (1/2^14
 #define G_TO_MPSS           9.8f                    // (m/s^2)/g
-//#define GYRO_LSB_TO_DEG     0.0152671755f          // deg/LSB (1/65.5
-#define GYRO_LSB_TO_DEG     0.00763358778f          // deg/LSB (1/131
-#define PRES_LSB_TO_HPA     0.000244140625f         // hPa/LSB (1/4096
\ No newline at end of file
+//#define GYRO_LSB_TO_DEG     0.0304878048f          // deg/LSB (1/32.8
+#define GYRO_LSB_TO_DEG     0.0152671755f          // deg/LSB (1/65.5
+//#define GYRO_LSB_TO_DEG     0.00763358778f          // deg/LSB (1/131
+#define PRES_LSB_TO_HPA     0.000244140625f         // hPa/LSB (1/4096
+
+#define MAG_LSB_TO_GAUSS    0.00092f                // Gauss/LSB
+#define MAG_MAGNITUDE       0.46f                   // Magnitude of GeoMagnetism (Gauss)
+#define MAG_SIN             -0.754709580f            // Sin-Value of Inclination
\ No newline at end of file