diff --git a/lib/ICM20948.c b/lib/ICM20948.c
new file mode 100644
index 0000000000000000000000000000000000000000..992f3566698ab467a3f9bd0ccc63eb77697d5cf7
--- /dev/null
+++ b/lib/ICM20948.c
@@ -0,0 +1,493 @@
+#include "ICM20948.h"
+#include <hardware/gpio.h>
+
+#define I2C_PORT i2c0
+IMU_ST_SENSOR_DATA gstGyroOffset = { 0, 0, 0 };
+
+char I2C_ReadOneByte(uint8_t reg) {
+ uint8_t buf;
+ i2c_write_blocking(I2C_PORT, I2C_ADD_ICM20948, ®, 1, true);
+ i2c_read_blocking(I2C_PORT, I2C_ADD_ICM20948, &buf, 1, false);
+ return buf;
+}
+
+void I2C_WriteOneByte(uint8_t reg, uint8_t value) {
+ uint8_t buf[] = { reg, value };
+ i2c_write_blocking(I2C_PORT, I2C_ADD_ICM20948, buf, 2, false);
+}
+
+/******************************************************************************
+ * IMU module *
+ ******************************************************************************/
+#define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases
+
+float angles[3];
+float q0, q1, q2, q3;
+
+bool reserved_addr(uint8_t addr) {
+ return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
+}
+
+void imuInit(IMU_EN_SENSOR_TYPE *penMotionSensorType) {
+ bool bRet = false;
+
+#if 0
+ printf("\nI2C Bus Scan\n");
+ printf(" 0 1 2 3 4 5 6 7 8 9 A B C D E F\n");
+ for (int addr = 0; addr < (1 << 7); ++addr) {
+ if (addr % 16 == 0) {
+ printf("%02x ", addr);
+ }
+
+ // Perform a 1-byte dummy read from the probe address. If a slave
+ // acknowledges this address, the function returns the number of bytes
+ // transferred. If the address byte is ignored, the function returns
+ // -1.
+ //
+ // Skip over any reserved addresses.
+ int ret;
+ uint8_t rxdata;
+ if (reserved_addr(addr))
+ ret = PICO_ERROR_GENERIC;
+ else
+ ret = i2c_read_blocking(I2C_PORT, addr, &rxdata, 1, false);
+
+ printf(ret < 0 ? "." : "@");
+ printf(addr % 16 == 15 ? "\n" : " ");
+ }
+ printf("Done.\n");
+#endif
+
+ bRet = icm20948Check();
+ if (true == bRet) {
+ *penMotionSensorType = IMU_EN_SENSOR_TYPE_ICM20948;
+ icm20948init();
+ }
+ else {
+ *penMotionSensorType = IMU_EN_SENSOR_TYPE_NULL;
+ }
+ q0 = 1.0f;
+ q1 = 0.0f;
+ q2 = 0.0f;
+ q3 = 0.0f;
+}
+
+bool imuDataGet(IMU_ST_ANGLES_DATA *pstAngles,
+ IMU_ST_SENSOR_DATA *pstGyroRawData,
+ IMU_ST_SENSOR_DATA *pstAccelRawData,
+ IMU_ST_SENSOR_DATA *pstMagnRawData) {
+ float MotionVal[9];
+ float s16Accel[3], s16Gyro[3], s16Magn[3];
+ icm20948AccelRead(&s16Accel[0], &s16Accel[1], &s16Accel[2]);
+ icm20948GyroRead(&s16Gyro[0], &s16Gyro[1], &s16Gyro[2]);
+ icm20948MagRead(&s16Magn[0], &s16Magn[1], &s16Magn[2]);
+
+ MotionVal[0] = s16Gyro[0] / 32.8;
+ MotionVal[1] = s16Gyro[1] / 32.8;
+ MotionVal[2] = s16Gyro[2] / 32.8;
+ MotionVal[3] = s16Accel[0];
+ MotionVal[4] = s16Accel[1];
+ MotionVal[5] = s16Accel[2];
+ MotionVal[6] = s16Magn[0];
+ MotionVal[7] = s16Magn[1];
+ MotionVal[8] = s16Magn[2];
+ imuAHRSupdate(MotionVal[0] * 0.0175, MotionVal[1] * 0.0175,
+ MotionVal[2] * 0.0175, MotionVal[3], MotionVal[4],
+ MotionVal[5], MotionVal[6], MotionVal[7],
+ MotionVal[8]);
+
+ pstAngles->fPitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // pitch
+ pstAngles->fRoll =
+ atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // roll
+ pstAngles->fYaw =
+ atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;
+
+ pstGyroRawData->s16X = s16Gyro[0];
+ pstGyroRawData->s16Y = s16Gyro[1];
+ pstGyroRawData->s16Z = s16Gyro[2];
+
+ pstAccelRawData->s16X = s16Accel[0];
+ pstAccelRawData->s16Y = s16Accel[1];
+ pstAccelRawData->s16Z = s16Accel[2];
+
+ pstMagnRawData->s16X = s16Magn[0];
+ pstMagnRawData->s16Y = s16Magn[1];
+ pstMagnRawData->s16Z = s16Magn[2];
+
+ return true;
+}
+
+void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az,
+ float mx, float my, float mz) {
+ float norm;
+ float hx, hy, hz, bx, bz;
+ float vx, vy, vz, wx, wy, wz;
+ float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
+ float ex, ey, ez, halfT = 0.024f;
+
+ float q0q0 = q0 * q0;
+ float q0q1 = q0 * q1;
+ float q0q2 = q0 * q2;
+ float q0q3 = q0 * q3;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q3q3 = q3 * q3;
+
+ norm = invSqrt(ax * ax + ay * ay + az * az);
+ ax = ax * norm;
+ ay = ay * norm;
+ az = az * norm;
+
+ norm = invSqrt(mx * mx + my * my + mz * mz);
+ mx = mx * norm;
+ my = my * norm;
+ mz = mz * norm;
+
+ // compute reference direction of flux
+ hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
+ hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
+ hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
+ bx = sqrt((hx * hx) + (hy * hy));
+ bz = hz;
+
+ // estimated direction of gravity and flux (v and w)
+ vx = 2 * (q1q3 - q0q2);
+ vy = 2 * (q0q1 + q2q3);
+ vz = q0q0 - q1q1 - q2q2 + q3q3;
+ wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
+ wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
+ wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);
+
+ // error is sum of cross product between reference direction of fields and direction
+ // measured by sensors
+ ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+ ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+ ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+
+ if (ex != 0.0f && ey != 0.0f && ez != 0.0f) {
+ exInt = exInt + ex * Ki * halfT;
+ eyInt = eyInt + ey * Ki * halfT;
+ ezInt = ezInt + ez * Ki * halfT;
+
+ gx = gx + Kp * ex + exInt;
+ gy = gy + Kp * ey + eyInt;
+ gz = gz + Kp * ez + ezInt;
+ }
+
+ q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
+ q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
+ q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
+ q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
+
+ norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 = q0 * norm;
+ q1 = q1 * norm;
+ q2 = q2 * norm;
+ q3 = q3 * norm;
+}
+
+float invSqrt(float x) {
+ float halfx = 0.5f * x;
+ float y = x;
+
+ long i = *(long *)&y; // get bits for floating value
+ i = 0x5f3759df - (i >> 1); // gives initial guss you
+ y = *(float *)&i; // convert bits back to float
+ y = y * (1.5f - (halfx * y * y)); // newtop step, repeating increases accuracy
+
+ return y;
+}
+
+/******************************************************************************
+ * ICM20948 sensor device *
+ ******************************************************************************/
+
+int dataReady() {
+// return I2C_ReadOneByte(REG_ADD_ACCEL_XOUT_L);
+ return I2C_ReadOneByte(0x1A);
+}
+
+void setContinuousMode(){
+ // Enable FIFO
+ I2C_WriteOneByte(REG_ADD_USER_CTRL, REG_VAL_BIT_FIFO_EN);
+ // Set continuous mode
+// I2C_WriteOneByte()
+}
+
+void icm20948init() {
+
+ /* user bank 0 register */
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
+ I2C_WriteOneByte(REG_ADD_PWR_MIGMT_1, REG_VAL_ALL_RGE_RESET);
+ sleep_ms(10);
+ I2C_WriteOneByte(REG_ADD_PWR_MIGMT_1, REG_VAL_RUN_MODE);
+
+ /* user bank 2 register */
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_2);
+ I2C_WriteOneByte(REG_ADD_GYRO_SMPLRT_DIV, 0x08);
+ I2C_WriteOneByte(REG_ADD_GYRO_CONFIG_1, REG_VAL_BIT_GYRO_DLPCFG_6
+ | REG_VAL_BIT_GYRO_FS_2000DPS
+ | REG_VAL_BIT_GYRO_DLPF);
+ I2C_WriteOneByte( REG_ADD_ACCEL_SMPLRT_DIV_1, 0x00); // 119 Hz
+ I2C_WriteOneByte( REG_ADD_ACCEL_SMPLRT_DIV_2, 0x08);
+ I2C_WriteOneByte(REG_ADD_ACCEL_CONFIG, REG_VAL_BIT_ACCEL_DLPCFG_6
+ | REG_VAL_BIT_ACCEL_FS_4g
+ | REG_VAL_BIT_ACCEL_DLPF);
+
+
+ /* user bank 0 register */
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
+
+ sleep_ms(100);
+ /* offset */
+ icm20948GyroOffset();
+
+ icm20948MagCheck();
+
+ icm20948WriteSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_WRITE,
+ REG_ADD_MAG_CNTL2, REG_VAL_MAG_MODE_20HZ);
+}
+
+bool icm20948Check() {
+ bool bRet = false;
+ if (REG_VAL_WIA == I2C_ReadOneByte(REG_ADD_WIA)) {
+ bRet = true;
+ }
+ return bRet;
+}
+
+bool icm20948GyroRead(float *ps16X, float *ps16Y, float *ps16Z) {
+ uint8_t u8Buf[6];
+ int16_t s16Buf[3] = { 0 };
+ uint8_t i;
+ int32_t s32OutBuf[3] = { 0 };
+ //
+ static ICM20948_ST_AVG_DATA sstAvgBuf[3];
+ static int16_t ss16c = 0;
+ ss16c++;
+
+ u8Buf[0] = I2C_ReadOneByte(REG_ADD_GYRO_XOUT_L);
+ u8Buf[1] = I2C_ReadOneByte(REG_ADD_GYRO_XOUT_H);
+ s16Buf[0] = (u8Buf[1] << 8) | u8Buf[0];
+
+ u8Buf[0] = I2C_ReadOneByte(REG_ADD_GYRO_YOUT_L);
+ u8Buf[1] = I2C_ReadOneByte(REG_ADD_GYRO_YOUT_H);
+ s16Buf[1] = (u8Buf[1] << 8) | u8Buf[0];
+
+ u8Buf[0] = I2C_ReadOneByte(REG_ADD_GYRO_ZOUT_L);
+ u8Buf[1] = I2C_ReadOneByte(REG_ADD_GYRO_ZOUT_H);
+ s16Buf[2] = (u8Buf[1] << 8) | u8Buf[0];
+
+ *ps16X = s16Buf[0] * 2000.0 / 32768.0;
+ *ps16Y = s16Buf[1] * 2000.0 / 32768.0;
+ *ps16Z = s16Buf[2] * 2000.0 / 32768.0;
+
+ /*
+ for (i = 0; i < 3; i++) {
+ icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i],
+ s32OutBuf + i);
+ }
+ *ps16X = s32OutBuf[0] - gstGyroOffset.s16X;
+ *ps16Y = s32OutBuf[1] - gstGyroOffset.s16Y;
+ *ps16Z = s32OutBuf[2] - gstGyroOffset.s16Z;
+*/
+
+ if (*ps16X == 0 && *ps16Y == 0 && *ps16Z == 0) {
+ return false;
+ }
+ return true;
+
+}
+
+bool icm20948AccelRead(float *ps16X, float *ps16Y, float *ps16Z) {
+ uint8_t u8Buf[2];
+ int16_t s16Buf[3] = { 0 };
+ uint8_t i;
+ int32_t s32OutBuf[3] = { 0 };
+ //
+ static ICM20948_ST_AVG_DATA sstAvgBuf[3];
+
+ u8Buf[0] = I2C_ReadOneByte(REG_ADD_ACCEL_XOUT_L);
+ u8Buf[1] = I2C_ReadOneByte(REG_ADD_ACCEL_XOUT_H);
+ s16Buf[0] = (u8Buf[1] << 8) | u8Buf[0];
+
+ u8Buf[0] = I2C_ReadOneByte(REG_ADD_ACCEL_YOUT_L);
+ u8Buf[1] = I2C_ReadOneByte(REG_ADD_ACCEL_YOUT_H);
+ s16Buf[1] = (u8Buf[1] << 8) | u8Buf[0];
+
+ u8Buf[0] = I2C_ReadOneByte(REG_ADD_ACCEL_ZOUT_L);
+ u8Buf[1] = I2C_ReadOneByte(REG_ADD_ACCEL_ZOUT_H);
+ s16Buf[2] = (u8Buf[1] << 8) | u8Buf[0];
+
+ *ps16X = s16Buf[0] * 4.0 / 32768.0;
+ *ps16Y = s16Buf[1] * 4.0 / 32768.0;
+ *ps16Z = s16Buf[2] * 4.0 / 32768.0;
+
+ // for (i = 0; i < 3; i++)
+ // {
+ // icm20948CalAvgValue (&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer,
+ // s16Buf[i], s32OutBuf + i);
+ // }
+ //
+ // *ps16X = s32OutBuf[0];
+ // *ps16Y = s32OutBuf[1];
+ // *ps16Z = s32OutBuf[2];
+
+ if (*ps16X == 0 && *ps16Y == 0 && *ps16Z == 0) {
+ return false;
+ }
+ return true;
+}
+
+bool icm20948MagRead(float *ps16X, float *ps16Y, float *ps16Z) {
+ uint8_t counter = 20;
+ uint8_t u8Data[MAG_DATA_LEN];
+ int16_t s16Buf[3] = { 0 };
+ uint8_t i;
+ int32_t s32OutBuf[3] = { 0 };
+ //
+ static ICM20948_ST_AVG_DATA sstAvgBuf[3];
+ while (counter > 0) {
+ sleep_ms(1);
+ icm20948ReadSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_READ,
+ REG_ADD_MAG_ST2, 1, u8Data);
+
+ if ((u8Data[0] & 0x01) != 0)
+ break;
+
+ counter--;
+ }
+
+ if (counter != 0) {
+ icm20948ReadSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_READ,
+ REG_ADD_MAG_DATA, MAG_DATA_LEN, u8Data);
+ s16Buf[0] = ((int16_t)u8Data[1] << 8) | u8Data[0];
+ s16Buf[1] = ((int16_t)u8Data[3] << 8) | u8Data[2];
+ s16Buf[2] = ((int16_t)u8Data[5] << 8) | u8Data[4];
+ }
+
+ *ps16X = s16Buf[0] * 4.0 * 100.0 / 32768.0;
+ *ps16Y = s16Buf[1] * 4.0 * 100.0 / 32768.0;
+ *ps16Z = s16Buf[2] * 4.0 * 100.0 / 32768.0;
+
+/*
+ for (i = 0; i < 3; i++) {
+ icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i],
+ s32OutBuf + i);
+ }
+ *ps16X = s32OutBuf[0];
+ *ps16Y = -s32OutBuf[1];
+ *ps16Z = -s32OutBuf[2];
+ */
+ if (*ps16X == 0 && *ps16Y == 0 && *ps16Z == 0) {
+ return false;
+ }
+ return true;
+}
+
+void icm20948ReadSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr,
+ uint8_t u8Len, uint8_t *pu8data) {
+ uint8_t i;
+ uint8_t u8Temp;
+
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); // swtich bank3
+ I2C_WriteOneByte(REG_ADD_I2C_SLV0_ADDR, u8I2CAddr);
+ I2C_WriteOneByte(REG_ADD_I2C_SLV0_REG, u8RegAddr);
+ I2C_WriteOneByte(REG_ADD_I2C_SLV0_CTRL, REG_VAL_BIT_SLV0_EN | u8Len);
+
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); // swtich bank0
+
+ u8Temp = I2C_ReadOneByte(REG_ADD_USER_CTRL);
+ u8Temp |= REG_VAL_BIT_I2C_MST_EN;
+ I2C_WriteOneByte(REG_ADD_USER_CTRL, u8Temp);
+ sleep_ms(5);
+ u8Temp &= ~REG_VAL_BIT_I2C_MST_EN;
+ I2C_WriteOneByte(REG_ADD_USER_CTRL, u8Temp);
+
+ for (i = 0; i < u8Len; i++) {
+ *(pu8data + i) = I2C_ReadOneByte(REG_ADD_EXT_SENS_DATA_00 + i);
+ }
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); // swtich bank3
+
+ u8Temp = I2C_ReadOneByte(REG_ADD_I2C_SLV0_CTRL);
+ u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN) & (REG_VAL_BIT_MASK_LEN));
+ I2C_WriteOneByte(REG_ADD_I2C_SLV0_CTRL, u8Temp);
+
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); // swtich bank0
+}
+
+void icm20948WriteSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr,
+ uint8_t u8data) {
+ uint8_t u8Temp;
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); // swtich bank3
+ I2C_WriteOneByte(REG_ADD_I2C_SLV1_ADDR, u8I2CAddr);
+ I2C_WriteOneByte(REG_ADD_I2C_SLV1_REG, u8RegAddr);
+ I2C_WriteOneByte(REG_ADD_I2C_SLV1_DO, u8data);
+ I2C_WriteOneByte(REG_ADD_I2C_SLV1_CTRL, REG_VAL_BIT_SLV0_EN | 1);
+
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); // swtich bank0
+
+ u8Temp = I2C_ReadOneByte(REG_ADD_USER_CTRL);
+ u8Temp |= REG_VAL_BIT_I2C_MST_EN;
+ I2C_WriteOneByte(REG_ADD_USER_CTRL, u8Temp);
+ sleep_ms(5);
+ u8Temp &= ~REG_VAL_BIT_I2C_MST_EN;
+ I2C_WriteOneByte(REG_ADD_USER_CTRL, u8Temp);
+
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); // swtich bank3
+
+ u8Temp = I2C_ReadOneByte(REG_ADD_I2C_SLV0_CTRL);
+ u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN) & (REG_VAL_BIT_MASK_LEN));
+ I2C_WriteOneByte(REG_ADD_I2C_SLV0_CTRL, u8Temp);
+
+ I2C_WriteOneByte(REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
+}
+
+void icm20948CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal,
+ int32_t *pOutVal) {
+ uint8_t i;
+
+ *(pAvgBuffer + ((*pIndex)++)) = InVal;
+ *pIndex &= 0x07;
+
+ *pOutVal = 0;
+ for (i = 0; i < 8; i++) {
+ *pOutVal += *(pAvgBuffer + i);
+ }
+ *pOutVal >>= 3;
+}
+
+void icm20948GyroOffset() {
+ uint8_t i;
+ float s16Gx = 0, s16Gy = 0, s16Gz = 0;
+ int32_t s32TempGx = 0, s32TempGy = 0, s32TempGz = 0;
+ for (i = 0; i < 32; i++) {
+ icm20948GyroRead(&s16Gx, &s16Gy, &s16Gz);
+ s32TempGx += (uint16_t)s16Gx;
+ s32TempGy += (uint16_t)s16Gy;
+ s32TempGz += (uint16_t)s16Gz;
+ sleep_ms(10);
+ }
+ gstGyroOffset.s16X = s32TempGx >> 5;
+ gstGyroOffset.s16Y = s32TempGy >> 5;
+ gstGyroOffset.s16Z = s32TempGz >> 5;
+}
+
+bool icm20948MagCheck() {
+ bool bRet = false;
+ uint8_t u8Ret[2];
+
+ icm20948ReadSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_READ,
+ REG_ADD_MAG_WIA1, 2, u8Ret);
+ if ((u8Ret[0] == REG_VAL_MAG_WIA1) && (u8Ret[1] == REG_VAL_MAG_WIA2)) {
+ bRet = true;
+ }
+
+ return bRet;
+}
+
+// ICM20948 IMU;
\ No newline at end of file