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
|