275 lines
8.2 KiB
C
275 lines
8.2 KiB
C
|
/*
|
||
|
|
MPU6500 + IST8310 组合惯性测量单元。
|
||
|
|
|
||
|
|
*/
|
||
|
|
|
||
|
|
/* Includes ----------------------------------------------------------------- */
|
||
|
|
#include "imu.h"
|
||
|
|
|
||
|
|
/* Include 标准库 */
|
||
|
|
#include <stdbool.h>
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
/* 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;
|
||
|
|
}
|