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