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