135 lines
3.9 KiB
C
135 lines
3.9 KiB
C
|
#include "gSensor/mpu6050.h"
|
|||
|
|
#include "gSensor/gSensor_manage.h"
|
|||
|
|
#include "app_config.h"
|
|||
|
|
#include "imuSensor_manage.h"
|
|||
|
|
|
|||
|
|
#if TCFG_MPU6050_EN
|
|||
|
|
|
|||
|
|
int gsensorlen = 32;
|
|||
|
|
OS_MUTEX SENSOR_IIC_MUTEX;
|
|||
|
|
spinlock_t sensor_iic;
|
|||
|
|
u8 sensor_iic_init_status = 0;
|
|||
|
|
|
|||
|
|
#define ACCEL_ONLY_LOW_POWER 0
|
|||
|
|
|
|||
|
|
u8 mpu6050_register_read(u8 addr, u8 *data)
|
|||
|
|
{
|
|||
|
|
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, data, 1);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
u8 mpu6050_register_write(u8 addr, u8 data)
|
|||
|
|
{
|
|||
|
|
gravity_sensor_command(I2C_ADDR_MPU6050_W, addr, data);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
u8 mpu6050_read_nbyte_data(u8 addr, u8 *data, u8 len)
|
|||
|
|
{
|
|||
|
|
return _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, data, len);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
u8 mpu6050_get_xyz_data(u8 addr, void *xyz_data)
|
|||
|
|
{
|
|||
|
|
u8 buf[6], read_len;
|
|||
|
|
axis_info_t *data = (axis_info_t *)xyz_data;
|
|||
|
|
read_len = mpu6050_read_nbyte_data(addr, buf, 6);
|
|||
|
|
if (read_len == 6) {
|
|||
|
|
data->x = ((u16)buf[0] << 8) | buf[1];
|
|||
|
|
data->y = ((u16)buf[2] << 8) | buf[3];
|
|||
|
|
data->z = ((u16)buf[4] << 8) | buf[5];
|
|||
|
|
|
|||
|
|
data->x = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->x / 32768;
|
|||
|
|
data->y = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->y / 32768;
|
|||
|
|
data->z = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->z / 32768;
|
|||
|
|
}
|
|||
|
|
return read_len;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
extern void step_cal_init();
|
|||
|
|
extern int step_cal();
|
|||
|
|
u8 mpu6050_init()
|
|||
|
|
{
|
|||
|
|
u8 res = 0;
|
|||
|
|
u8 data;
|
|||
|
|
res = mpu6050_register_read(MPU6050_RA_WHO_AM_I, &data); //读ID
|
|||
|
|
if (data == MPU_ADDR) {
|
|||
|
|
g_printf("read MPU6050 ID suss");
|
|||
|
|
} else {
|
|||
|
|
g_printf("read MPU6050 ID err");
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
data = 0x80;
|
|||
|
|
mpu6050_register_write(MPU6050_RA_PWR_MGMT_1, data); //复位
|
|||
|
|
os_time_dly(10);
|
|||
|
|
|
|||
|
|
data = 0x00;
|
|||
|
|
data |= BIT(3); //关闭温度传感器
|
|||
|
|
#if ACCEL_ONLY_LOW_POWER
|
|||
|
|
data |= BIT(5); //设置accel-only低功耗模式
|
|||
|
|
#endif
|
|||
|
|
mpu6050_register_write(MPU6050_RA_PWR_MGMT_1, data); //退出休眠
|
|||
|
|
|
|||
|
|
data = 0x00;
|
|||
|
|
data |= (BIT(0) | BIT(1) | BIT(2)); //关闭陀螺仪
|
|||
|
|
#if ACCEL_ONLY_LOW_POWER
|
|||
|
|
data |= (3 << 6); //accel-only低功耗模式下，唤醒频率为40Hz
|
|||
|
|
#endif
|
|||
|
|
/*mpu6050_register_write(MPU6050_RA_PWR_MGMT_2, data);*/
|
|||
|
|
|
|||
|
|
data = ACCEL_RANGE_2G << 3;
|
|||
|
|
mpu6050_register_write(MPU6050_RA_ACCEL_CONFIG, data); //加速度计量程
|
|||
|
|
|
|||
|
|
#if (!ACCEL_ONLY_LOW_POWER)
|
|||
|
|
data = 0x06;
|
|||
|
|
mpu6050_register_write(MPU6050_RA_CONFIG, data); //设置陀螺仪输出速率
|
|||
|
|
|
|||
|
|
data = MPU6050_GYRO_OUT_RATE / MPU6050_SAMPLE_RATE - 1;
|
|||
|
|
mpu6050_register_write(MPU6050_RA_SMPLRT_DIV, data); //设置采样率, 采样率=陀螺仪输出速率/(1+SMPLRT_DIV)
|
|||
|
|
|
|||
|
|
data = 0x00;
|
|||
|
|
mpu6050_register_write(MPU6050_RA_INT_ENABLE, data); //关闭中断
|
|||
|
|
|
|||
|
|
data = 0x00;
|
|||
|
|
mpu6050_register_write(MPU6050_RA_USER_CTRL, data); //关闭主机IIC
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/*step_cal_init();*/
|
|||
|
|
/*sys_timer_add(NULL, step_cal, 50);*/
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void mpu6050_ctl(u8 cmd, void *arg)
|
|||
|
|
{
|
|||
|
|
switch (cmd) {
|
|||
|
|
case GET_ACCEL_DATA:
|
|||
|
|
case IMU_GET_ACCEL_DATA:
|
|||
|
|
mpu6050_get_xyz_data(MPU6050_RA_ACCEL_XOUT_H, arg);
|
|||
|
|
break;
|
|||
|
|
case IMU_GET_GYRO_DATA:
|
|||
|
|
mpu6050_get_xyz_data(MPU6050_RA_GYRO_XOUT_H, arg);
|
|||
|
|
break;
|
|||
|
|
case READ_GSENSOR_DATA:
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
REGISTER_GRAVITY_SENSOR(gSensor) = {
|
|||
|
|
.logo = "mpu6050",
|
|||
|
|
.gravity_sensor_init = mpu6050_init,
|
|||
|
|
.gravity_sensor_check = NULL,
|
|||
|
|
.gravity_sensor_ctl = mpu6050_ctl,
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
//未合到imu: init:gravity_sensor_init(&motion_sensor_data);
|
|||
|
|
REGISTER_IMU_SENSOR(mpu6050_sensor) = {
|
|||
|
|
.logo = "mpu6050",
|
|||
|
|
.imu_sensor_init = mpu6050_init,
|
|||
|
|
.imu_sensor_check = NULL,
|
|||
|
|
.imu_sensor_ctl = mpu6050_ctl,
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
#endif //TCFG_MPU6050_EN
|