/* MPU6500 + IST8310 组合惯性测量单元。 */ /* Includes ----------------------------------------------------------------- */ #include "imu.h" /* Include 标准库 */ #include #include /* Include BSP相关的头文件 */ #include "bsp_delay.h" #include "bsp_spi.h" /* Include Component相关的头文件 */ #include "user_math.h" /* Private define ----------------------------------------------------------- */ #define MPU6500_SELF_TEST_XG (0x00) #define MPU6500_SELF_TEST_YG (0x01) #define MPU6500_SELF_TEST_ZG (0x02) #define MPU6500_SELF_TEST_XA (0x0D) #define MPU6500_SELF_TEST_YA (0x0E) #define MPU6500_SEFLF_TEST_ZA (0x0F) #define MPU6500_XG_OFFSET_H (0x13) #define MPU6500_XG_OFFSET_L (0x14) #define MPU6500_YG_OFFSET_H (0x15) #define MPU6500_YG_OFFSET_L (0x16) #define MPU6500_ZG_OFFSET_H (0x17) #define MPU6500_ZG_OFFSET_L (0x18) #define MPU6500_SMPLRT_DIV (0x19) #define MPU6500_CONFIG (0x1A) #define MPU6500_GYRO_CONFIG (0x1B) #define MPU6500_ACCEL_CONFIG (0x1C) #define MPU6500_ACCEL_CONFIG_2 (0x1D) #define MPU6500_LP_ACCEL_ODR (0x1E) #define MPU6500_MOT_THR (0x1F) #define MPU6500_FIFO_EN (0x23) #define MPU6500_I2C_MST_CTRL (0x24) #define MPU6500_I2C_SLV0_ADDR (0x25) #define MPU6500_I2C_SLV0_REG (0x26) #define MPU6500_I2C_SLV0_CTRL (0x27) #define MPU6500_I2C_SLV1_ADDR (0x28) #define MPU6500_I2C_SLV1_REG (0x29) #define MPU6500_I2C_SLV1_CTRL (0x2A) #define MPU6500_I2C_SLV2_ADDR (0x2B) #define MPU6500_I2C_SLV2_REG (0x2C) #define MPU6500_I2C_SLV2_CTRL (0x2D) #define MPU6500_I2C_SLV3_ADDR (0x2E) #define MPU6500_I2C_SLV3_REG (0x2F) #define MPU6500_I2C_SLV3_CTRL (0x30) #define MPU6500_I2C_SLV4_ADDR (0x31) #define MPU6500_I2C_SLV4_REG (0x32) #define MPU6500_I2C_SLV4_DO (0x33) #define MPU6500_I2C_SLV4_CTRL (0x34) #define MPU6500_I2C_SLV4_DI (0x35) #define MPU6500_I2C_MST_STATUS (0x36) #define MPU6500_INT_PIN_CFG (0x37) #define MPU6500_INT_ENABLE (0x38) #define MPU6500_INT_STATUS (0x3A) #define MPU6500_ACCEL_XOUT_H (0x3B) #define MPU6500_ACCEL_XOUT_L (0x3C) #define MPU6500_ACCEL_YOUT_H (0x3D) #define MPU6500_ACCEL_YOUT_L (0x3E) #define MPU6500_ACCEL_ZOUT_H (0x3F) #define MPU6500_ACCEL_ZOUT_L (0x40) #define MPU6500_TEMP_OUT_H (0x41) #define MPU6500_TEMP_OUT_L (0x42) #define MPU6500_GYRO_XOUT_H (0x43) #define MPU6500_GYRO_XOUT_L (0x44) #define MPU6500_GYRO_YOUT_H (0x45) #define MPU6500_GYRO_YOUT_L (0x46) #define MPU6500_GYRO_ZOUT_H (0x47) #define MPU6500_GYRO_ZOUT_L (0x48) #define MPU6500_EXT_SENS_DATA_00 (0x49) #define MPU6500_EXT_SENS_DATA_01 (0x4A) #define MPU6500_EXT_SENS_DATA_02 (0x4B) #define MPU6500_EXT_SENS_DATA_03 (0x4C) #define MPU6500_EXT_SENS_DATA_04 (0x4D) #define MPU6500_EXT_SENS_DATA_05 (0x4E) #define MPU6500_EXT_SENS_DATA_06 (0x4F) #define MPU6500_EXT_SENS_DATA_07 (0x50) #define MPU6500_EXT_SENS_DATA_08 (0x51) #define MPU6500_EXT_SENS_DATA_09 (0x52) #define MPU6500_EXT_SENS_DATA_10 (0x53) #define MPU6500_EXT_SENS_DATA_11 (0x54) #define MPU6500_EXT_SENS_DATA_12 (0x55) #define MPU6500_EXT_SENS_DATA_13 (0x56) #define MPU6500_EXT_SENS_DATA_14 (0x57) #define MPU6500_EXT_SENS_DATA_15 (0x58) #define MPU6500_EXT_SENS_DATA_16 (0x59) #define MPU6500_EXT_SENS_DATA_17 (0x5A) #define MPU6500_EXT_SENS_DATA_18 (0x5B) #define MPU6500_EXT_SENS_DATA_19 (0x5C) #define MPU6500_EXT_SENS_DATA_20 (0x5D) #define MPU6500_EXT_SENS_DATA_21 (0x5E) #define MPU6500_EXT_SENS_DATA_22 (0x5F) #define MPU6500_EXT_SENS_DATA_23 (0x60) #define MPU6500_I2C_SLV0_DO (0x63) #define MPU6500_I2C_SLV1_DO (0x64) #define MPU6500_I2C_SLV2_DO (0x65) #define MPU6500_I2C_SLV3_DO (0x66) #define MPU6500_I2C_MST_DELAY_CTRL (0x67) #define MPU6500_SIGNAL_PATH_RESET (0x68) #define MPU6500_MOT_DETECT_CTRL (0x69) #define MPU6500_USER_CTRL (0x6A) #define MPU6500_PWR_MGMT_1 (0x6B) #define MPU6500_PWR_MGMT_2 (0x6C) #define MPU6500_FIFO_COUNTH (0x72) #define MPU6500_FIFO_COUNTL (0x73) #define MPU6500_FIFO_R_W (0x74) #define MPU6500_WHO_AM_I (0x75) #define MPU6500_XA_OFFSET_H (0x77) #define MPU6500_XA_OFFSET_L (0x78) #define MPU6500_YA_OFFSET_H (0x7A) #define MPU6500_YA_OFFSET_L (0x7B) #define MPU6500_ZA_OFFSET_H (0x7D) #define MPU6500_ZA_OFFSET_L (0x7E) #define MPU6500_ID (0x70) /* Private macro ------------------------------------------------------------ */ /* Private typedef ---------------------------------------------------------- */ __packed struct { __packed struct { int16_t x; int16_t y; int16_t z; } accl; int16_t temp; __packed struct { int16_t x; int16_t y; int16_t z; } gyro; __packed struct { int16_t x; int16_t y; int16_t z; } magn; } packed; /* Private variables -------------------------------------------------------- */ static uint8_t buffer[2]; static IMU_t *gimu; static bool inited = false; /* Private function -------------------------------------------------------- */ void IMU_RxCpltCallback(void) { osThreadFlagsSet(gimu->received_alert, IMU_SIGNAL_RAW_ACCL_REDY | IMU_SIGNAL_RAW_GYRO_REDY); } void IMU_ExtIntCallback(void) { IMU_StartReceiving(gimu); } static void MPU_WriteSingle(uint8_t reg, uint8_t data) { buffer[0] = (reg & 0x7f); buffer[1] = data; BSP_SPI_Transmit(BSP_SPI_IMU, buffer, 2); BSP_Delay(5); } static uint8_t MPU_ReadSingle(uint8_t reg) { buffer[0] = (reg | 0x80); BSP_SPI_Transmit(BSP_SPI_IMU, buffer, 1); BSP_SPI_Receive(BSP_SPI_IMU, buffer, 1); return buffer[0]; } static void MPU_Read(uint8_t reg, uint8_t *data, uint8_t len) { if (data == NULL) return; buffer[0] = (reg | 0x80); BSP_SPI_Transmit(BSP_SPI_IMU, buffer, 1); BSP_SPI_Receive(BSP_SPI_IMU, data, len); } /* Exported functions ------------------------------------------------------- */ int8_t IMU_Init(IMU_t *imu) { if (imu == NULL) return -1; if (inited) return -1; if (MPU_ReadSingle(MPU6500_WHO_AM_I) != MPU6500_ID) return -1; gimu = imu; /* MPU6500 init. */ MPU_WriteSingle(MPU6500_PWR_MGMT_1, 0x80); /* Reset device */ BSP_Delay(100); MPU_WriteSingle(MPU6500_SIGNAL_PATH_RESET, 0x0f); /* Reset device */ BSP_Delay(100); MPU_WriteSingle(MPU6500_PWR_MGMT_1, 0x03); /* Clock source -> gyro-z */ MPU_WriteSingle(MPU6500_PWR_MGMT_2, 0x00); /* Enable acc & gyro */ MPU_WriteSingle(MPU6500_CONFIG, 0x04); /* Disable acc LPF */ MPU_WriteSingle(MPU6500_GYRO_CONFIG, 0x18); /* gyro range 0x10:+-1000dps 0x18:+-2000dps */ MPU_WriteSingle(MPU6500_ACCEL_CONFIG, 0x10); /* +-8G */ MPU_WriteSingle(MPU6500_ACCEL_CONFIG_2, 0x10); /* Disable acc LPF */ BSP_Delay(10); BSP_SPI_RegisterCallback(BSP_SPI_IMU, BSP_SPI_RX_COMPLETE_CB, IMU_RxCpltCallback); inited = true; return 0; } IMU_t *IMU_GetDevice(void) { if (inited) { return gimu; } return NULL; } uint8_t IMU_StartReceiving(IMU_t *imu) { MPU_Read(MPU6500_ACCEL_XOUT_H, (uint8_t *)&(imu->raw), 20); return 0; } /* magn_scale[3] is initially zero. So data from uncalibrated magnentmeter is * ignored. */ /* Sensor use right-handed coordinate system. */ /* * x < R (RoboMaster trademark) * y * UP is z */ int8_t IMU_Parse(IMU_t *imu) { if (imu == NULL) return -1; uint8_t raw[20]; memcpy(raw, &(imu->raw), sizeof(imu->raw)); for (uint8_t i = 0; i < 20; i += 2) ((int16_t *)&imu->raw)[i / 2] = (raw[i] << 8) | raw[i + 1]; imu->data.temp = 21.0f + (float)imu->raw.temp / 333.87f; /* TODO: Strage accl data. Potential sensor damage. */ imu->data.accl.x = (float)imu->raw.accl.x / 4096.0f; imu->data.accl.y = (float)imu->raw.accl.y / 4096.0f; imu->data.accl.z = (float)imu->raw.accl.z / 4096.0f; /* Convert gyroscope imu_raw to degrees/sec, then, to radians/sec */ imu->data.gyro.x = (float)(imu->raw.gyro.x - imu->cali.gyro_offset[0]) / 16.384f / 180.0f * M_PI; imu->data.gyro.y = (float)(imu->raw.gyro.y - imu->cali.gyro_offset[1]) / 16.384f / 180.0f * M_PI; imu->data.gyro.z = (float)(imu->raw.gyro.z - imu->cali.gyro_offset[2]) / 16.384f / 180.0f * M_PI; #if 0 imu->data.magn.x = (float)((imu->raw.magn.x - imu->cali.magn_offset[0]) * imu->cali.magn_scale[0]); imu->data.magn.y = (float)((imu->raw.magn.y - imu->cali.magn_offset[1]) * imu->cali.magn_scale[1]); imu->data.magn.z = (float)((imu->raw.magn.z - imu->cali.magn_offset[2]) * imu->cali.magn_scale[2]); #else imu->data.magn.x = 0.0f; imu->data.magn.y = 0.0f; imu->data.magn.z = 0.0f; #endif return 0; }