Diff: ICM20948.cpp

Revision:

0:ce9707156696

--- /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