Revision 7:da63be95c693, committed 2019-12-17

Comitter:

tanahashi

Date:

Tue Dec 17 14:08:27 2019 +0000

Parent:

6:a7d11c40a920

Commit message:

fix

Changed in this revision

diff -r a7d11c40a920 -r da63be95c693 PQMPU9250.cpp
--- a/PQMPU9250.cpp	Sun Nov 24 06:08:54 2019 +0000
+++ b/PQMPU9250.cpp	Tue Dec 17 14:08:27 2019 +0000
@@ -10,17 +10,18 @@
 
 void MPU9250::begin()
 {
-    cmd[0] = PWR_MGMT_1;
+    cmd[0] = MPU9250_PWR_MGMT_1;
     cmd[1] = 0x00;
     _i2c->write(_addr, cmd, 2);
-    cmd[0] = INT_PIN_CFG;
+    cmd[0] = MPU9250_INT_PIN_CFG;
     cmd[1] = 0x02;
     _i2c->write(_addr, cmd, 2);
     set(_2G, _250DPS, _16B_8HZ);
 }
 
-bool MPU9250::test(){
-    cmd[0] = WHO_AM_I;
+bool MPU9250::test()
+{
+    cmd[0] = MPU9250_WHO_AM_I;
     _i2c->write(_addr, cmd, 1);
     _i2c->read(_addr, buff, 1);
     if (buff[0] == 0x71) {
@@ -32,9 +33,9 @@
 
 bool MPU9250::test_AK8963()
 {
-    cmd[0] = WIA;
-    _i2c->write(AK8963_ADDR, cmd, 1);
-    _i2c->read(AK8963_ADDR, buff, 1);
+    cmd[0] = MPU9250_WIA;
+    _i2c->write(MPU9250_AK8963_ADDR, cmd, 1);
+    _i2c->read(MPU9250_AK8963_ADDR, buff, 1);
     if (buff[0] == 0x48) {
         return true;
     } else {
@@ -42,7 +43,8 @@
     }
 }
 
-void MPU9250::set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config){
+void MPU9250::set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config)
+{
     set_accel(accel_config);
     set_gyro(gyro_config);
     set_mag(mag_config);
@@ -50,42 +52,42 @@
 
 void MPU9250::set_accel(AccelConfig_t accel_config)
 {
-    cmd[0] = ACCEL_CONFIG;
+    cmd[0] = MPU9250_ACCEL_CONFIG;
     cmd[1] = accel_config;
     _i2c->write(_addr, cmd, 2);
     if (accel_config == _2G) {
-        accel_LSB = ACCEL_LSB;
+        accel_LSB = MPU9250_ACCEL_LSB;
     } else if (accel_config == _4G) {
-        accel_LSB = ACCEL_LSB * 2;
+        accel_LSB = MPU9250_ACCEL_LSB * 2;
     } else if (accel_config == _8G) {
-        accel_LSB = ACCEL_LSB * 4;
+        accel_LSB = MPU9250_ACCEL_LSB * 4;
     } else if (accel_config == _16G) {
-        accel_LSB = ACCEL_LSB * 8;
+        accel_LSB = MPU9250_ACCEL_LSB * 8;
     }
 }
 
 void MPU9250::set_gyro(GyroConfig_t gyro_config)
 {
-    cmd[0] = GYRO_CONFIG;
+    cmd[0] = MPU9250_GYRO_CONFIG;
     cmd[1] = gyro_config;
     _i2c->write(_addr, cmd, 2);
     if (gyro_config == _250DPS) {
-        gyro_LSB = GYRO_LSB;
+        gyro_LSB = MPU9250_GYRO_LSB;
     } else if (gyro_config == _500DPS) {
-        gyro_LSB = GYRO_LSB * 2;
+        gyro_LSB = MPU9250_GYRO_LSB * 2;
     } else if (gyro_config == _1000DPS) {
-        gyro_LSB = GYRO_LSB * 4;
+        gyro_LSB = MPU9250_GYRO_LSB * 4;
     } else if (gyro_config == _2000DPS) {
-        gyro_LSB = GYRO_LSB * 8;
+        gyro_LSB = MPU9250_GYRO_LSB * 8;
     }
 }
 
 void MPU9250::set_mag(MagConfig_t mag_config)
 {
-    cmd[0] = CNTL1;
+    cmd[0] = MPU9250_CNTL1;
     cmd[1] = mag_config;
-    _i2c->write(AK8963_ADDR, cmd, 2);
-    mag_LSB = MAG_LSB;
+    _i2c->write(MPU9250_AK8963_ADDR, cmd, 2);
+    mag_LSB = MPU9250_MAG_LSB;
 }
 
 void MPU9250::offset(float *accel, float *gyro, float *mag)
@@ -125,7 +127,7 @@
 
 void MPU9250::read_accel(float *accel)
 {
-    cmd[0] = ACCEL_XOUT_H;
+    cmd[0] = MPU9250_ACCEL_XOUT_H;
     _i2c->write(_addr, cmd, 1);
     _i2c->read(_addr, buff, 6);
     accel[0] = (short)(buff[0] << 8 | buff[1]) * accel_LSB - accel_offset[0];
@@ -135,7 +137,7 @@
 
 void MPU9250::read_gyro(float *gyro)
 {
-    cmd[0] = GYRO_XOUT_H;
+    cmd[0] = MPU9250_GYRO_XOUT_H;
     _i2c->write(_addr, cmd, 1);
     _i2c->read(_addr, buff, 6);
     gyro[0] = (short)(buff[0] << 8 | buff[1]) * gyro_LSB - gyro_offset[0];
@@ -145,9 +147,9 @@
 
 void MPU9250::read_mag(float *mag)
 {
-    cmd[0] = HXL;
-    _i2c->write(AK8963_ADDR, cmd, 1);
-    _i2c->read(AK8963_ADDR, buff, 7);
+    cmd[0] = MPU9250_HXL;
+    _i2c->write(MPU9250_AK8963_ADDR, cmd, 1);
+    _i2c->read(MPU9250_AK8963_ADDR, buff, 7);
     mag[0] = (short)(buff[0] | buff[1] << 8) * mag_LSB - mag_offset[0];
     mag[1] = (short)(buff[2] | buff[3] << 8) * mag_LSB - mag_offset[1];
     mag[2] = (short)(buff[4] | buff[5] << 8) * mag_LSB - mag_offset[2];

diff -r a7d11c40a920 -r da63be95c693 PQMPU9250.h
--- a/PQMPU9250.h	Sun Nov 24 06:08:54 2019 +0000
+++ b/PQMPU9250.h	Tue Dec 17 14:08:27 2019 +0000
@@ -1,49 +1,23 @@
 #ifndef PQMPU9250_H
-#define PQMPU9250_H
+#define MPU9250_PQMPU9250_H
 
 #define MPU9250_ADDR_HIGH 0b1101001<<1
 #define MPU9250_ADDR_LOW 0b1101000<<1
-#define AK8963_ADDR 0b0001100<<1
-#define WIA 0x00
-#define ST1 0x02
-#define HXL 0x03
-#define CNTL1 0x0A
-#define GYRO_CONFIG 0x1B
-#define ACCEL_CONFIG 0x1C
-#define INT_PIN_CFG 0x37
-#define ACCEL_XOUT_H 0x3B
-#define GYRO_XOUT_H 0x43
-#define PWR_MGMT_1 0x6B
-#define WHO_AM_I 0x75
-#define ACCEL_LSB 0.0000610
-#define GYRO_LSB 0.00763
-#define MAG_LSB 0.150
-
-typedef enum {
-    AD0_HIGH = MPU9250_ADDR_HIGH,
-    AD0_LOW  = MPU9250_ADDR_LOW
-} AD0_t;
-
-typedef enum {
-    _2G  = 0b00000000,
-    _4G  = 0b00001000,
-    _8G  = 0b00010000,
-    _16G = 0b00011000
-} AccelConfig_t;
-
-typedef enum {
-    _250DPS  = 0b00000000,
-    _500DPS  = 0b00001000,
-    _1000DPS = 0b00010000,
-    _2000DPS = 0b00011000
-} GyroConfig_t;
-
-typedef enum {
-    _16B_8HZ   = 0b00010010,
-    _16B_100HZ = 0b00010110,
-    _14B_8HZ   = 0b00000010,
-    _14B_100HZ = 0b00000100
-} MagConfig_t;
+#define MPU9250_AK8963_ADDR 0b0001100<<1
+#define MPU9250_WIA 0x00
+#define MPU9250_ST1 0x02
+#define MPU9250_HXL 0x03
+#define MPU9250_CNTL1 0x0A
+#define MPU9250_GYRO_CONFIG 0x1B
+#define MPU9250_ACCEL_CONFIG 0x1C
+#define MPU9250_INT_PIN_CFG 0x37
+#define MPU9250_ACCEL_XOUT_H 0x3B
+#define MPU9250_GYRO_XOUT_H 0x43
+#define MPU9250_PWR_MGMT_1 0x6B
+#define MPU9250_WHO_AM_I 0x75
+#define MPU9250_ACCEL_LSB 0.0000610
+#define MPU9250_GYRO_LSB 0.00763
+#define MPU9250_MAG_LSB 0.150
 
 /**
  * 9軸センサMPU9250のライブラリ
@@ -54,7 +28,7 @@
 Serial pc(USBTX, USBRX, 115200);
 I2C i2c(p9, p10);
 
-MPU9250 mpu(i2c, AD0_HIGH);
+MPU9250 mpu(i2c, MPU9250::AD0_HIGH);
 
 float accel_offset[] = {0, 0, 0};
 float gyro_offset[] = {0, 0, 0};
@@ -69,12 +43,10 @@
     mpu.offset(accel_offset, gyro_offset, mag_offset);
     if(!mpu.test()){
         pc.printf("[  FAIL  ] MPU9250 cannot be reached.\r\n");
-        return 0;
     }
     if(!mpu.test_AK8963()){
         pc.printf("[  FAIL  ] AK8963 cannot be reached.\r\n");
-        return 0;
-    }        
+    }
     while(1) {
         mpu.read(accel, gyro, mag);
         pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], mag[0], mag[1], mag[2]);
@@ -84,7 +56,34 @@
  */
 class MPU9250
 {
-private:    
+public:
+    typedef enum {
+        AD0_HIGH = MPU9250_ADDR_HIGH,
+        AD0_LOW  = MPU9250_ADDR_LOW
+    } AD0_t;
+
+    typedef enum {
+        _2G  = 0b00000000,
+        _4G  = 0b00001000,
+        _8G  = 0b00010000,
+        _16G = 0b00011000
+    } AccelConfig_t;
+
+    typedef enum {
+        _250DPS  = 0b00000000,
+        _500DPS  = 0b00001000,
+        _1000DPS = 0b00010000,
+        _2000DPS = 0b00011000
+    } GyroConfig_t;
+
+    typedef enum {
+        _16B_8HZ   = 0b00010010,
+        _16B_100HZ = 0b00010110,
+        _14B_8HZ   = 0b00000010,
+        _14B_100HZ = 0b00000100
+    } MagConfig_t;
+
+private:
     I2C *_i2c;
     int _addr;
     char cmd[2];
@@ -102,26 +101,26 @@
      * @param AD0 AD0ピンのH/Lレベル
      */
     MPU9250(I2C &i2c, AD0_t AD0);
-    
+
     /**
      * センサ動作開始
      */
     void begin();
-    
+
     /**
      * センサ通信テスト
      * @retval true 通信成功
      * @retval false 通信失敗
      */
     bool test();
-    
+
     /**
      * 磁気センサAK8963通信テスト
      * @retval true 通信成功
      * @retval false 通信失敗
      */
     bool test_AK8963();
-    
+
     /**
      * センサ設定
      * @param accel_config 加速度の測定レンジ
@@ -129,25 +128,25 @@
      * @param mag_config 磁気センサの分解能/データレート
      */
     void set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config);
-    
+
     /**
      * 加速度センサ設定
      * @param accel_config 角速度センサの測定レンジ
      */
     void set_accel(AccelConfig_t accel_config);
-    
+
     /**
      * 角速度センサ設定
      * @param gyro_config 角速度の測定レンジ
      */
     void set_gyro(GyroConfig_t gyro_config);
-    
+
     /**
      * 磁気センサ設定
      * @param mag_config 磁気センサの分解能/データレート
      */
     void set_mag(MagConfig_t mag_config);
-    
+
     /**
      * ゼロ点補正
      * @param accel 加速度のオフセット配列
@@ -155,25 +154,25 @@
      * @param mag 磁気のオフセット配列
      */
     void offset(float *accel, float *gyro, float *mag);
-    
+
     /**
      * 加速度のゼロ点補正
      * @param accel 加速度のオフセット配列
      */
     void offset_accel(float *accel);
-    
+
     /**
      * 角速度のゼロ点補正
      * @param gyro 角速度のオフセット配列
      */
     void offset_gyro(float *gyro);
-    
+
     /**
      * 磁気のゼロ点補正
      * @param mag 磁気のオフセット配列
-     */ 
+     */
     void offset_mag(float *mag);
-    
+
     /**
      * 測定値の読み取り
      * @param accel 加速度を格納する配列
@@ -181,19 +180,19 @@
      * @param mag 磁気を格納する配列
      */
     void read(float *accel, float *gyro, float *mag);
-    
+
     /**
      * 加速度測定値の読み取り
      * @param accel 加速度を格納する配列
      */
     void read_accel(float *accel);
-    
+
     /**
      * 角速度測定値の読み取り
      * @param gyro 角速度を格納する配列
      */
     void read_gyro(float *gyro);
-    
+
     /**
      * 磁気測定値の読み取り
      * @param mag 磁気を格納する配列