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