Ken Hira / Mbed 2 deprecated ICM20948

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

Dependencies:   mbed MadgwickFilter

ICM20948.cpp@0:ce9707156696, 2021-10-18 (annotated)

Committer:
hiramitsu
Date:
Mon Oct 18 13:41:49 2021 +0000
Revision:
0:ce9707156696
ICM20948 (use acce and gyro, omit mag)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiramitsu 0:ce9707156696 1 #include "mbed.h"
hiramitsu 0:ce9707156696 2 #include "ICM20948.hpp"
hiramitsu 0:ce9707156696 3 extern Serial pc1(USBTX,USBRX);
hiramitsu 0:ce9707156696 4 I2C ICM20948_i2c(p9,p10); // I2C0_SDA, I2C0_SCL
hiramitsu 0:ce9707156696 5
hiramitsu 0:ce9707156696 6
hiramitsu 0:ce9707156696 7 ICM20948::ICM20948()
hiramitsu 0:ce9707156696 8 {
hiramitsu 0:ce9707156696 9 ICM20948_i2c.frequency(400000);
hiramitsu 0:ce9707156696 10 gyroBias[0] = 0;
hiramitsu 0:ce9707156696 11 gyroBias[1] = 0;
hiramitsu 0:ce9707156696 12 gyroBias[2] = 0;
hiramitsu 0:ce9707156696 13 }
hiramitsu 0:ce9707156696 14
hiramitsu 0:ce9707156696 15 ICM20948::~ICM20948() {}
hiramitsu 0:ce9707156696 16
hiramitsu 0:ce9707156696 17 void ICM20948::ICM_WriteByte(uint8_t ICM20948_reg, uint8_t ICM20948_data)
hiramitsu 0:ce9707156696 18 {
hiramitsu 0:ce9707156696 19 char data_out[2];
hiramitsu 0:ce9707156696 20 data_out[0]=ICM20948_reg;
hiramitsu 0:ce9707156696 21 data_out[1]=ICM20948_data;
hiramitsu 0:ce9707156696 22 ICM20948_i2c.write(ICM20948_slave_addr, data_out, 2, 0);
hiramitsu 0:ce9707156696 23 }
hiramitsu 0:ce9707156696 24
hiramitsu 0:ce9707156696 25 uint8_t ICM20948::ICM_ReadByte(uint8_t ICM20948_reg)
hiramitsu 0:ce9707156696 26 {
hiramitsu 0:ce9707156696 27 char data_out[1], data_in[1];
hiramitsu 0:ce9707156696 28 data_out[0] = ICM20948_reg;
hiramitsu 0:ce9707156696 29 ICM20948_i2c.write(ICM20948_slave_addr, data_out, 1, 1);
hiramitsu 0:ce9707156696 30 ICM20948_i2c.read(ICM20948_slave_addr, data_in, 1, 0);
hiramitsu 0:ce9707156696 31 return (data_in[0]);
hiramitsu 0:ce9707156696 32 }
hiramitsu 0:ce9707156696 33
hiramitsu 0:ce9707156696 34 // Communication test: WHO_AM_I register reading
hiramitsu 0:ce9707156696 35 void ICM20948::whoAmI()
hiramitsu 0:ce9707156696 36 {
hiramitsu 0:ce9707156696 37 uint8_t whoAmI = ICM_ReadByte(ICM20948_WHO_AM_I); // Should return 0x68
hiramitsu 0:ce9707156696 38 pc.printf("I AM ICM20948: 0x%x \r\n",whoAmI);
hiramitsu 0:ce9707156696 39
hiramitsu 0:ce9707156696 40 if(whoAmI==0xea) {
hiramitsu 0:ce9707156696 41 pc.printf("WHO_AM_I OK! ICM20948 is detected!\r\n");
hiramitsu 0:ce9707156696 42 } else {
hiramitsu 0:ce9707156696 43 pc.printf("WHO_AM_I FAILED! Could not connect to ICM20948 \r\nCheck the connections... \r\n");
hiramitsu 0:ce9707156696 44 }
hiramitsu 0:ce9707156696 45
hiramitsu 0:ce9707156696 46 }
hiramitsu 0:ce9707156696 47
hiramitsu 0:ce9707156696 48 void ICM20948::powerOn()
hiramitsu 0:ce9707156696 49 {
hiramitsu 0:ce9707156696 50 // USER_BANK_0 を使用
hiramitsu 0:ce9707156696 51 ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_0);
hiramitsu 0:ce9707156696 52 ICM_WriteByte(ICM20948_PWR_MGMT_1, 0x01);
hiramitsu 0:ce9707156696 53 // 一旦アクセル，ジャイロの電源を落して，
hiramitsu 0:ce9707156696 54 //ICM_WriteOneByte(ICM20948_PWR_MGMT_2, (0x38 | 0x07));
hiramitsu 0:ce9707156696 55 ICM_WriteByte(ICM20948_PWR_MGMT_2, (0x38 | 0x07));
hiramitsu 0:ce9707156696 56 wait_ms(10);
hiramitsu 0:ce9707156696 57 // からの再起動でリセット
hiramitsu 0:ce9707156696 58 //ICM_WriteOneByte(ICM20948_PWR_MGMT_2, (0x00 | 0x00));
hiramitsu 0:ce9707156696 59 ICM_WriteByte(ICM20948_PWR_MGMT_2, (0x00 | 0x00));
hiramitsu 0:ce9707156696 60 }
hiramitsu 0:ce9707156696 61
hiramitsu 0:ce9707156696 62 void ICM20948::init()
hiramitsu 0:ce9707156696 63 {
hiramitsu 0:ce9707156696 64 powerOn();
hiramitsu 0:ce9707156696 65
hiramitsu 0:ce9707156696 66 // USER_BANK_2 を使用
hiramitsu 0:ce9707156696 67 ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_2);
hiramitsu 0:ce9707156696 68
hiramitsu 0:ce9707156696 69 // ジャイロのレンジとLPFの使用有無を設定
hiramitsu 0:ce9707156696 70 //ICM_WriteOneByte(GYRO_CONFIG_1, (GYRO_RATE_250 | GYRO_LPF_230Hz));
hiramitsu 0:ce9707156696 71 ICM_WriteByte(ICM20948_GYRO_CONFIG_1, (GYRO_RATE_250 | GYRO_LPF_17Hz));
hiramitsu 0:ce9707156696 72 gRes = 250.0/32768.0 * 2.0 * 3.1415926 / 360.0; // 設定した Ndps / 32768.0
hiramitsu 0:ce9707156696 73 // ジャイロのサンプリングレートを設定
hiramitsu 0:ce9707156696 74 //ICM_WriteOneByte(ICM20948_GYRO_SMPLRT_DIV, GYRO_SMPLRT_100Hz);
hiramitsu 0:ce9707156696 75 ICM_WriteByte(ICM20948_GYRO_SMPLRT_DIV, GYRO_SMPLRT_100Hz);
hiramitsu 0:ce9707156696 76
hiramitsu 0:ce9707156696 77 // 加速度のレンジとLPFの使用有無を設定
hiramitsu 0:ce9707156696 78 //ICM_WriteOneByte(ACCEL_CONFIG, (ACC_RATE_2g | ACC_LPF_136HZ));
hiramitsu 0:ce9707156696 79 ICM_WriteByte(ICM20948_ACCEL_CONFIG, (ACC_RATE_2g | ACC_LPF_136HZ));
hiramitsu 0:ce9707156696 80 aRes = 2.0/32768.0; // 選択した Ng / 32768.0
hiramitsu 0:ce9707156696 81 // 加速度のサンプリングレートを設定
hiramitsu 0:ce9707156696 82 //ICM_WriteOneByte(ICM20948_ACCEL_SMPLRT_DIV_2, ACC_SMPLRT_100Hz);
hiramitsu 0:ce9707156696 83 ICM_WriteByte(ICM20948_ACCEL_SMPLRT_DIV_2, ACC_SMPLRT_100Hz);
hiramitsu 0:ce9707156696 84
hiramitsu 0:ce9707156696 85 // USER_BANK_0 に戻す
hiramitsu 0:ce9707156696 86 ICM_WriteByte(ICM20948_REG_BANK_SEL, USER_BANK_0);
hiramitsu 0:ce9707156696 87 wait_ms(10);
hiramitsu 0:ce9707156696 88
hiramitsu 0:ce9707156696 89 // ジャイロのキャリブレーション
hiramitsu 0:ce9707156696 90 gyroCalib();
hiramitsu 0:ce9707156696 91 }
hiramitsu 0:ce9707156696 92
hiramitsu 0:ce9707156696 93 // ジャイロのキャリブレーション．100回の平均をとって引く
hiramitsu 0:ce9707156696 94 void ICM20948::gyroCalib(){
hiramitsu 0:ce9707156696 95 pc.printf("Calibrating Gyro ... Wait for 5s\n\r");
hiramitsu 0:ce9707156696 96 int16_t gxtmp, gytmp, gztmp;
hiramitsu 0:ce9707156696 97 float gx, gy, gz;
hiramitsu 0:ce9707156696 98 gx = 0.0; gy = 0.0; gz = 0.0;
hiramitsu 0:ce9707156696 99 uint8_t LoByte, HiByte;
hiramitsu 0:ce9707156696 100 for(int i = 0; i < 100; i++){
hiramitsu 0:ce9707156696 101 LoByte = ICM_ReadByte(ICM20948_GYRO_XOUT_L); // read Gyrometer X_Low value
hiramitsu 0:ce9707156696 102 HiByte = ICM_ReadByte(ICM20948_GYRO_XOUT_H); // read Gyrometer X_High value
hiramitsu 0:ce9707156696 103 gxtmp = ((HiByte<<8) | LoByte);
hiramitsu 0:ce9707156696 104 gx += (float)(gxtmp) * gRes;
hiramitsu 0:ce9707156696 105
hiramitsu 0:ce9707156696 106 LoByte = ICM_ReadByte(ICM20948_GYRO_YOUT_L);
hiramitsu 0:ce9707156696 107 HiByte = ICM_ReadByte(ICM20948_GYRO_YOUT_H);
hiramitsu 0:ce9707156696 108 gytmp = ((HiByte<<8) | LoByte);
hiramitsu 0:ce9707156696 109 gy += (float)(gytmp) * gRes;
hiramitsu 0:ce9707156696 110
hiramitsu 0:ce9707156696 111 LoByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_L);
hiramitsu 0:ce9707156696 112 HiByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_H);
hiramitsu 0:ce9707156696 113 gztmp = ((HiByte<<8) | LoByte);
hiramitsu 0:ce9707156696 114 gz += (float)(gztmp) * gRes;
hiramitsu 0:ce9707156696 115 //pc.printf("%d %f, %d %f, %d %f\n\r", gxtmp, gx, gytmp, gy, gztmp, gz);
hiramitsu 0:ce9707156696 116 wait_ms(50);
hiramitsu 0:ce9707156696 117 }
hiramitsu 0:ce9707156696 118
hiramitsu 0:ce9707156696 119 gyroBias[0] = gx / 100.0;
hiramitsu 0:ce9707156696 120 gyroBias[1] = gy / 100.0;
hiramitsu 0:ce9707156696 121 gyroBias[2] = gz / 100.0;
hiramitsu 0:ce9707156696 122
hiramitsu 0:ce9707156696 123 pc.printf("Gyro Bias: %f %f %f\n\r", gyroBias[0], gyroBias[1], gyroBias[2]);
hiramitsu 0:ce9707156696 124 }
hiramitsu 0:ce9707156696 125
hiramitsu 0:ce9707156696 126 void ICM20948::getAccGyro(float acc[3], float gyro[3]){
hiramitsu 0:ce9707156696 127 int16_t ax, ay, az, gx, gy, gz;
hiramitsu 0:ce9707156696 128 uint8_t LoByte, HiByte;
hiramitsu 0:ce9707156696 129
hiramitsu 0:ce9707156696 130 LoByte = ICM_ReadByte(ICM20948_ACCEL_XOUT_L); // read Accelerometer X_Low value
hiramitsu 0:ce9707156696 131 HiByte = ICM_ReadByte(ICM20948_ACCEL_XOUT_H); // read Accelerometer X_High value
hiramitsu 0:ce9707156696 132 ax = (HiByte<<8) | LoByte;
hiramitsu 0:ce9707156696 133 acc[0] = ((float)(ax)) * aRes;
hiramitsu 0:ce9707156696 134
hiramitsu 0:ce9707156696 135 LoByte = ICM_ReadByte(ICM20948_ACCEL_YOUT_L);
hiramitsu 0:ce9707156696 136 HiByte = ICM_ReadByte(ICM20948_ACCEL_YOUT_H);
hiramitsu 0:ce9707156696 137 ay = (HiByte<<8) | LoByte;
hiramitsu 0:ce9707156696 138 acc[1] = ((float)(ay)) * aRes;
hiramitsu 0:ce9707156696 139
hiramitsu 0:ce9707156696 140 LoByte = ICM_ReadByte(ICM20948_ACCEL_ZOUT_L);
hiramitsu 0:ce9707156696 141 HiByte = ICM_ReadByte(ICM20948_ACCEL_ZOUT_H);
hiramitsu 0:ce9707156696 142 az = (HiByte<<8) | LoByte;
hiramitsu 0:ce9707156696 143 acc[2] = ((float)(az)) * aRes;
hiramitsu 0:ce9707156696 144
hiramitsu 0:ce9707156696 145 LoByte = ICM_ReadByte(ICM20948_GYRO_XOUT_L); // read Gyrometer X_Low value
hiramitsu 0:ce9707156696 146 HiByte = ICM_ReadByte(ICM20948_GYRO_XOUT_H); // read Gyrometer X_High value
hiramitsu 0:ce9707156696 147 gx = (HiByte<<8) | LoByte;
hiramitsu 0:ce9707156696 148 gyro[0] = ((float)(gx)) * gRes - gyroBias[0];
hiramitsu 0:ce9707156696 149
hiramitsu 0:ce9707156696 150 LoByte = ICM_ReadByte(ICM20948_GYRO_YOUT_L);
hiramitsu 0:ce9707156696 151 HiByte = ICM_ReadByte(ICM20948_GYRO_YOUT_H);
hiramitsu 0:ce9707156696 152 gy = (HiByte<<8) | LoByte;
hiramitsu 0:ce9707156696 153 gyro[1] = ((float)(gy)) * gRes - gyroBias[1];
hiramitsu 0:ce9707156696 154
hiramitsu 0:ce9707156696 155 LoByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_L);
hiramitsu 0:ce9707156696 156 HiByte = ICM_ReadByte(ICM20948_GYRO_ZOUT_H);
hiramitsu 0:ce9707156696 157 gz = (HiByte<<8) | LoByte;
hiramitsu 0:ce9707156696 158 gyro[2] = ((float)(gz)) * gRes - gyroBias[2];
hiramitsu 0:ce9707156696 159
hiramitsu 0:ce9707156696 160 }
hiramitsu 0:ce9707156696 161
hiramitsu 0:ce9707156696 162 int16_t ICM20948::getIMUTemp()
hiramitsu 0:ce9707156696 163 {
hiramitsu 0:ce9707156696 164 uint8_t LoByte, HiByte;
hiramitsu 0:ce9707156696 165 LoByte = ICM_ReadByte(ICM20948_TEMP_OUT_L); // read Accelerometer X_Low value
hiramitsu 0:ce9707156696 166 HiByte = ICM_ReadByte(ICM20948_TEMP_OUT_H); // read Accelerometer X_High value
hiramitsu 0:ce9707156696 167 return ((HiByte<<8) | LoByte);
hiramitsu 0:ce9707156696 168 }