Ken Hira / Mbed 2 deprecated ICM20948

ICM20948+madgwickフィルター 地磁気は不使用で6軸のみ使用 最初5s静置してジャイロのオフセットを求めキャリブレーション 地磁気を測らないためヨー軸ドリフトがあるが，緩やかに動かせば2,3分程度はヨー軸も正しい値を測定できた

Dependencies:   mbed MadgwickFilter

Files at this revision

API Documentation at this revision

Revision 0:ce9707156696, committed 2021-10-18

Comitter:
hiramitsu
Date:
Mon Oct 18 13:41:49 2021 +0000
Commit message:
ICM20948 (use acce and gyro, omit mag)

Changed in this revision

ICM20948.cpp Show annotated file Show diff for this revision Revisions of this file
ICM20948.hpp Show annotated file Show diff for this revision Revisions of this file
MadgwickFilter.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r ce9707156696 ICM20948.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ICM20948.cpp	Mon Oct 18 13:41:49 2021 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "ICM20948.hpp"
+extern Serial pc1(USBTX,USBRX);
+I2C  ICM20948_i2c(p9,p10); // I2C0_SDA, I2C0_SCL
+
+
+ICM20948::ICM20948()
+{
+    ICM20948_i2c.frequency(400000);
+    gyroBias[0] = 0;
+    gyroBias[1] = 0;
+    gyroBias[2] = 0;
+}
+
+ICM20948::~ICM20948() {}
+
+void ICM20948::ICM_WriteByte(uint8_t ICM20948_reg, uint8_t ICM20948_data)
+{
+    char data_out[2];
+    data_out[0]=ICM20948_reg;
+    data_out[1]=ICM20948_data;
+    ICM20948_i2c.write(ICM20948_slave_addr, data_out, 2, 0);
+}
+
+uint8_t ICM20948::ICM_ReadByte(uint8_t ICM20948_reg)
+{
+    char data_out[1], data_in[1];
+    data_out[0] = ICM20948_reg;
+    ICM20948_i2c.write(ICM20948_slave_addr, data_out, 1, 1);
+    ICM20948_i2c.read(ICM20948_slave_addr, data_in, 1, 0);
+    return (data_in[0]);
+}
+
+// Communication test: WHO_AM_I register reading
+void ICM20948::whoAmI()
+{
+    uint8_t whoAmI = ICM_ReadByte(ICM20948_WHO_AM_I);   // Should return 0x68
+    pc.printf("I AM ICM20948: 0x%x \r\n",whoAmI);
+
+    if(whoAmI==0xea) {
+        pc.printf("WHO_AM_I OK! ICM20948 is detected!\r\n");
+    } else {
+        pc.printf("WHO_AM_I FAILED! Could not connect to ICM20948 \r\nCheck the connections... \r\n");
+    }
+
+}
+
+void ICM20948::powerOn()
+{
+    // USER_BANK_0 を使用
+    ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_0);
+    ICM_WriteByte(ICM20948_PWR_MGMT_1, 0x01);
+    // 一旦アクセル，ジャイロの電源を落して，
+    //ICM_WriteOneByte(ICM20948_PWR_MGMT_2, (0x38 | 0x07));
+    ICM_WriteByte(ICM20948_PWR_MGMT_2, (0x38 | 0x07));
+    wait_ms(10);
+    // からの再起動でリセット
+    //ICM_WriteOneByte(ICM20948_PWR_MGMT_2, (0x00 | 0x00));
+    ICM_WriteByte(ICM20948_PWR_MGMT_2, (0x00 | 0x00));
+}
+
+void ICM20948::init()
+{
+    powerOn();
+
+    // USER_BANK_2 を使用
+    ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_2);
+
+    // ジャイロのレンジとLPFの使用有無を設定
+    //ICM_WriteOneByte(GYRO_CONFIG_1, (GYRO_RATE_250 | GYRO_LPF_230Hz));
+    ICM_WriteByte(ICM20948_GYRO_CONFIG_1, (GYRO_RATE_250 | GYRO_LPF_17Hz));
+    gRes = 250.0/32768.0 * 2.0 * 3.1415926 / 360.0; // 設定した Ndps / 32768.0
+    // ジャイロのサンプリングレートを設定
+    //ICM_WriteOneByte(ICM20948_GYRO_SMPLRT_DIV, GYRO_SMPLRT_100Hz);
+    ICM_WriteByte(ICM20948_GYRO_SMPLRT_DIV, GYRO_SMPLRT_100Hz);
+
+    // 加速度のレンジとLPFの使用有無を設定
+    //ICM_WriteOneByte(ACCEL_CONFIG, (ACC_RATE_2g | ACC_LPF_136HZ));
+    ICM_WriteByte(ICM20948_ACCEL_CONFIG, (ACC_RATE_2g | ACC_LPF_136HZ));
+    aRes = 2.0/32768.0; // 選択した Ng / 32768.0
+    // 加速度のサンプリングレートを設定
+    //ICM_WriteOneByte(ICM20948_ACCEL_SMPLRT_DIV_2, ACC_SMPLRT_100Hz);
+    ICM_WriteByte(ICM20948_ACCEL_SMPLRT_DIV_2, ACC_SMPLRT_100Hz);
+
+    // USER_BANK_0 に戻す
+    ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_0);
+    wait_ms(10);
+    
+    // ジャイロのキャリブレーション
+    gyroCalib();
+}
+
+// ジャイロのキャリブレーション．100回の平均をとって引く
+void ICM20948::gyroCalib(){
+    pc.printf("Calibrating Gyro ... Wait for 5s\n\r");
+    int16_t gxtmp, gytmp, gztmp;
+    float gx, gy, gz;
+    gx = 0.0; gy = 0.0; gz = 0.0;
+    uint8_t LoByte, HiByte;
+    for(int i = 0; i < 100; i++){
+        LoByte = ICM_ReadByte(ICM20948_GYRO_XOUT_L); // read Gyrometer X_Low  value
+        HiByte = ICM_ReadByte(ICM20948_GYRO_XOUT_H); // read Gyrometer X_High value
+        gxtmp = ((HiByte<<8) | LoByte);
+        gx += (float)(gxtmp) * gRes;
+        
+        LoByte = ICM_ReadByte(ICM20948_GYRO_YOUT_L);
+        HiByte = ICM_ReadByte(ICM20948_GYRO_YOUT_H);
+        gytmp = ((HiByte<<8) | LoByte);
+        gy += (float)(gytmp) * gRes;
+        
+        LoByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_L);
+        HiByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_H);
+        gztmp = ((HiByte<<8) | LoByte);
+        gz += (float)(gztmp) * gRes;
+        //pc.printf("%d %f, %d %f, %d %f\n\r", gxtmp, gx, gytmp, gy, gztmp, gz);
+        wait_ms(50);
+    }
+    
+    gyroBias[0] = gx / 100.0;
+    gyroBias[1] = gy / 100.0;
+    gyroBias[2] = gz / 100.0;
+    
+    pc.printf("Gyro Bias: %f %f %f\n\r", gyroBias[0], gyroBias[1], gyroBias[2]);
+}
+
+void ICM20948::getAccGyro(float acc[3], float gyro[3]){
+    int16_t ax, ay, az, gx, gy, gz;
+    uint8_t LoByte, HiByte;
+    
+    LoByte = ICM_ReadByte(ICM20948_ACCEL_XOUT_L); // read Accelerometer X_Low  value
+    HiByte = ICM_ReadByte(ICM20948_ACCEL_XOUT_H); // read Accelerometer X_High value
+    ax = (HiByte<<8) | LoByte;
+    acc[0] = ((float)(ax)) * aRes;
+    
+    LoByte = ICM_ReadByte(ICM20948_ACCEL_YOUT_L);
+    HiByte = ICM_ReadByte(ICM20948_ACCEL_YOUT_H);
+    ay = (HiByte<<8) | LoByte;
+    acc[1] = ((float)(ay)) * aRes;
+    
+    LoByte = ICM_ReadByte(ICM20948_ACCEL_ZOUT_L);
+    HiByte = ICM_ReadByte(ICM20948_ACCEL_ZOUT_H);
+    az = (HiByte<<8) | LoByte;
+    acc[2] = ((float)(az)) * aRes;
+    
+    LoByte = ICM_ReadByte(ICM20948_GYRO_XOUT_L); // read Gyrometer X_Low  value
+    HiByte = ICM_ReadByte(ICM20948_GYRO_XOUT_H); // read Gyrometer X_High value
+    gx = (HiByte<<8) | LoByte;
+    gyro[0] = ((float)(gx)) * gRes - gyroBias[0];
+    
+    LoByte = ICM_ReadByte(ICM20948_GYRO_YOUT_L);
+    HiByte = ICM_ReadByte(ICM20948_GYRO_YOUT_H);
+    gy = (HiByte<<8) | LoByte;
+    gyro[1] = ((float)(gy)) * gRes - gyroBias[1];
+    
+    LoByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_L);
+    HiByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_H);
+    gz = (HiByte<<8) | LoByte;
+    gyro[2] = ((float)(gz)) * gRes - gyroBias[2];
+    
+}
+
+int16_t ICM20948::getIMUTemp()
+{
+    uint8_t LoByte, HiByte;
+    LoByte = ICM_ReadByte(ICM20948_TEMP_OUT_L); // read Accelerometer X_Low  value
+    HiByte = ICM_ReadByte(ICM20948_TEMP_OUT_H); // read Accelerometer X_High value
+    return ((HiByte<<8) | LoByte);
+}
\ No newline at end of file

diff -r 000000000000 -r ce9707156696 ICM20948.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ICM20948.hpp	Mon Oct 18 13:41:49 2021 +0000
@@ -0,0 +1,95 @@
+#ifndef ICM20948_HPP
+#define ICM20948_HPP
+
+
+// USER BANK 0
+#define ICM20948_WHO_AM_I         0x00 // Should return 0xEA
+#define ICM20948_PWR_MGMT_1       0x06  // Device defaults to the SLEEP mode
+#define ICM20948_PWR_MGMT_2       0x07
+#define ICM20948_ACCEL_XOUT_H     0x2D
+#define ICM20948_ACCEL_XOUT_L     0x2E
+#define ICM20948_ACCEL_YOUT_H     0x2F
+#define ICM20948_ACCEL_YOUT_L     0x30
+#define ICM20948_ACCEL_ZOUT_H     0x31
+#define ICM20948_ACCEL_ZOUT_L     0x32
+#define ICM20948_GYRO_XOUT_H      0x33
+#define ICM20948_GYRO_XOUT_L      0x34
+#define ICM20948_GYRO_YOUT_H      0x35
+#define ICM20948_GYRO_YOUT_L      0x36
+#define ICM20948_GYRO_ZOUT_H      0x37
+#define ICM20948_GYRO_ZOUT_L      0x38
+#define ICM20948_TEMP_OUT_H       0x39
+#define ICM20948_TEMP_OUT_L       0x3A
+
+// USER BANK 2
+#define ICM20948_GYRO_SMPLRT_DIV  0x00
+#define ICM20948_GYRO_CONFIG_1    0x01
+#define ICM20948_ACCEL_SMPLRT_DIV_2 0x11
+#define ICM20948_ACCEL_CONFIG     0x14
+
+// COMMON
+#define ICM20948_REG_BANK_SEL     0x7F
+
+// OTHER
+#define USER_BANK_0               0x00
+#define USER_BANK_1               0x10
+#define USER_BANK_2               0x20
+#define USER_BANK_3               0x30
+
+// ジャイロのレンジは2:1bitの値に相当
+// ジャイロのモジュール内ローパスフィルタは5:3,0bitに相当
+#define GYRO_RATE_250             0x00
+#define GYRO_RATE_500             0x02
+#define GYRO_RATE_1000            0x04
+#define GYRO_RATE_2000            0x06
+#define GYRO_LPF_OFF              0x00
+#define GYRO_LPF_230Hz            0x01
+#define GYRO_LPF_17Hz             0x29
+#define GYRO_SMPLRT_100Hz         0x0A
+
+// 加速度のレンジは2:1bitの値に相当
+// 加速度のモジュール内ローパスフィルタは5:3,0bitに相当
+#define ACC_RATE_2g               0x00
+#define ACC_RATE_4g               0x02
+#define ACC_RATE_8g               0x04
+#define ACC_RATE_16g              0x06
+#define ACC_LPF_OFF               0x00
+#define ACC_LPF_136HZ             0x11
+#define ACC_SMPLRT_100Hz          0x0A
+
+
+
+ 
+// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+#define ADO 0
+#if ADO
+#define ICM20948_slave_addr 0x69<<1  // Device address when ADO = 1
+#else
+#define ICM20948_slave_addr 0x68<<1  // Device address when ADO = 0
+#endif
+ 
+#define IMU_ONE_G 9.80665
+//#define ICM20948_slave_addr          0xD0
+extern float aRes, gRes; 
+extern Serial pc;
+ 
+class ICM20948 {
+    public:
+        ICM20948();
+        ~ICM20948();
+         
+        void ICM_WriteByte(uint8_t ICM20948_reg, uint8_t ICM20948_data);
+        uint8_t ICM_ReadByte(uint8_t ICM20948_reg);
+        void    whoAmI();
+        void    powerOn();
+        void    init();
+        void    gyroCalib();
+        void    getAccGyro(float acc[3], float gyro[3]);
+        int16_t getIMUTemp();
+    private:
+        float aRes, gRes;
+        float gyroBias[3];
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r ce9707156696 MadgwickFilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MadgwickFilter.lib	Mon Oct 18 13:41:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/PQ_Hybrid_Electrical_Equipment_Team/code/MadgwickFilter/#c20656d96585

diff -r 000000000000 -r ce9707156696 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 18 13:41:49 2021 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+#include "ICM20948.hpp"
+#include "MadgwickFilter.hpp"
+
+Serial pc(USBTX, USBRX);
+
+int main()
+{
+    pc.baud(115200);
+    ICM20948 imu;
+    MadgwickFilter madgwick = MadgwickFilter();
+    
+    imu.whoAmI();
+    imu.init();
+    float acc[3];
+    float gyro[3];
+    float eulerVals[3];
+    int count = 0;
+
+    while(1) {
+        //imu.getAccGyro(acc, gyro);
+        //pc.printf("Acc: %f %f %f, Gyro: %f %f %f\n\r", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2]);
+        //wait_ms(500);
+        imu.getAccGyro(acc, gyro);
+        madgwick.MadgwickAHRSupdateIMU(gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2]);
+        count++;
+        if(count == 100){
+            madgwick.getEulerAngle(eulerVals);
+            pc.printf("r: %f, p: %f, y: %f\n\r", eulerVals[0], eulerVals[1], eulerVals[2]);
+            count = 0;
+        }
+        wait_ms(10);
+    }
+}

diff -r 000000000000 -r ce9707156696 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Oct 18 13:41:49 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file