132 lines
3.6 KiB
C
132 lines
3.6 KiB
C
#include "mpu6500.h"
|
|
|
|
#define MPU6500_REG_GYRO_CONFIG 0x1BU
|
|
#define MPU6500_REG_ACCEL_CONFIG 0x1CU
|
|
#define MPU6500_REG_ACCEL_XOUT_H 0x3BU
|
|
#define MPU6500_REG_WHO_AM_I 0x75U
|
|
#define MPU6500_REG_SIGNAL_PATH_RESET 0x68U
|
|
#define MPU6500_REG_PWR_MGMT_1 0x6BU
|
|
#define mPU6500_REG_PWR_MGMT_2 0x6CU
|
|
|
|
#define MPU6500_CONST_WHOAMI 0x70U
|
|
|
|
#define MPU6500_BTW_CPU(h, l) (((h & 0xFFU) << 8U) | (l & 0xFFU))
|
|
|
|
/**
|
|
* @brief Reset MPU6500.
|
|
* Refer to description field of the PWR_MGMT1 register in RegMap manual
|
|
* @param mpu Pointer to mpu6500_t struct
|
|
* @return mpu6500_ret_t MPU6500_RET_SUCCESS for success, others for failure.
|
|
*/
|
|
static mpu6500_ret_t mpu6500_reset(mpu6500_t *mpu) {
|
|
mpu6500_ret_t ret = MPU6500_RET_SUCCESS;
|
|
|
|
uint8_t buf[1];
|
|
|
|
ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_PWR_MGMT_1, buf, 0x01);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
buf[0] |= 0x80; /* H_RESET */
|
|
ret = mpu->ops.write_register(mpu->user_data, MPU6500_REG_PWR_MGMT_1, buf, 0x01);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
ret = mpu->ops.delay(mpu->user_data, 100);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
buf[0] = 0x07; /* GYRO_RST | ACCEL_RST | TEMP_RST */
|
|
ret = mpu->ops.write_register(mpu->user_data, MPU6500_REG_SIGNAL_PATH_RESET, buf, 0x01);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
ret = mpu->ops.delay(mpu->user_data, 100);
|
|
return ret;
|
|
}
|
|
|
|
mpu6500_ret_t mpu6500_init(mpu6500_t *mpu) {
|
|
mpu6500_ret_t ret = MPU6500_RET_SUCCESS;
|
|
|
|
uint8_t rx_buf[1];
|
|
|
|
ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_WHO_AM_I, rx_buf, 0x01);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
ret = mpu6500_reset(mpu);
|
|
return ret;
|
|
}
|
|
|
|
mpu6500_ret_t mpu6500_read_channel(mpu6500_t *mpu, mpu6500_channel_t ch, int16_t *result) {
|
|
mpu6500_ret_t ret = MPU6500_RET_SUCCESS;
|
|
|
|
uint8_t buf[2];
|
|
|
|
ret = mpu->ops.read_register(mpu->user_data, ch, buf, 0x02);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
*result = MPU6500_BTW_CPU(buf[0], buf[1]);
|
|
|
|
return ret;
|
|
}
|
|
|
|
mpu6500_ret_t mpu6500_read_result(mpu6500_t *mpu, mpu6500_result_t *result) {
|
|
mpu6500_ret_t ret = MPU6500_RET_SUCCESS;
|
|
|
|
uint8_t buf[14];
|
|
ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_ACCEL_XOUT_H, buf, 14);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
result->accel_x_raw = MPU6500_BTW_CPU(buf[0], buf[1]);
|
|
result->accel_y_raw = MPU6500_BTW_CPU(buf[2], buf[3]);
|
|
result->accel_z_raw = MPU6500_BTW_CPU(buf[4], buf[5]);
|
|
result->temp_raw = MPU6500_BTW_CPU(buf[6], buf[7]);
|
|
result->gyro_x_raw = MPU6500_BTW_CPU(buf[8], buf[9]);
|
|
result->gyro_y_raw = MPU6500_BTW_CPU(buf[10], buf[11]);
|
|
result->gyro_z_raw = MPU6500_BTW_CPU(buf[12], buf[13]);
|
|
|
|
return ret;
|
|
}
|
|
|
|
mpu6500_ret_t mpu6500_get_accel_fullscale(mpu6500_t *mpu, uint16_t *f_scale) {
|
|
mpu6500_ret_t ret = MPU6500_RET_SUCCESS;
|
|
|
|
uint8_t buf[1];
|
|
ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_ACCEL_CONFIG, buf, 0x01);
|
|
if (ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
uint8_t fs = (buf[0] >> 3U) & 0x03U;
|
|
|
|
*f_scale = 1 << (fs + 1); /* 00: 2g, 01: 4g, 10: 8g, 11: 16g */
|
|
|
|
return ret;
|
|
}
|
|
|
|
mpu6500_ret_t mpu6500_get_gyro_fullscale(mpu6500_t *mpu, uint16_t *f_scale) {
|
|
mpu6500_ret_t ret = MPU6500_RET_SUCCESS;
|
|
|
|
uint8_t buf[1];
|
|
ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_GYRO_CONFIG, buf, 0x01);
|
|
if(ret != MPU6500_RET_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
uint8_t fs = (buf[0] >> 3U) & 0x03U;
|
|
|
|
*f_scale = 250 * (1 << fs);
|
|
|
|
return ret;
|
|
}
|