1 /*
2 * Copyright (C) 2015-2020 Alibaba Group Holding Limited
3 */
4
5 #include "drv_acc_gyro_inv_mpu6050.h"
6 #include "aos/hal/i2c.h"
7 #include "aos/kernel.h"
8 #include "hal_iomux_haas1000.h"
9 #include "ulog/ulog.h"
10 #include <stdio.h>
11 #include <stdlib.h>
12
13 static i2c_dev_t i2c_dev;
14
15 // wtite one byte to register
MPU_Write_Byte(uint8_t reg,uint8_t data)16 uint8_t MPU_Write_Byte(uint8_t reg, uint8_t data)
17 {
18 uint8_t *write_buf[2];
19
20 write_buf[0] = reg;
21 write_buf[1] = data;
22 sensor_i2c_master_send(MPU_I2C_PORT, MPU_ADDR, write_buf, 2, 1000);
23 return 0;
24 }
25
26 // read byte from register
MPU_Read_Byte(uint8_t reg)27 uint8_t MPU_Read_Byte(uint8_t reg)
28 {
29 uint8_t data;
30
31 sensor_i2c_master_send(MPU_I2C_PORT, MPU_ADDR, ®, 1, 1000);
32 aos_msleep(2);
33 sensor_i2c_master_recv(MPU_I2C_PORT, MPU_ADDR, &data, 1, 1000);
34 return data;
35 }
36
37 // write nbytes to register
MPU_Write_Len(uint8_t reg,uint8_t len,uint8_t * buf)38 void MPU_Write_Len(uint8_t reg, uint8_t len, uint8_t *buf)
39 {
40 uint8_t *write_buf[100] = {0};
41
42 if ((len > 100) || (buf == NULL))
43 return;
44
45 write_buf[0] = reg;
46 memcpy(&write_buf[1], buf, len);
47 sensor_i2c_master_send(MPU_I2C_PORT, MPU_ADDR, &write_buf, len + 1, 1000);
48 }
49
50 // read nbytes from register
MPU_Read_Len(uint8_t reg,uint8_t len,uint8_t * buf)51 void MPU_Read_Len(uint8_t reg, uint8_t len, uint8_t *buf)
52 {
53 sensor_i2c_master_send(MPU_I2C_PORT, MPU_ADDR, ®, 1, 1000);
54 aos_msleep(2);
55 sensor_i2c_master_recv(MPU_I2C_PORT, MPU_ADDR, buf, len, 1000);
56 }
57
58 // 设置MPU6050陀螺仪传感器满量程范围
59 // fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
60 // 返回值:0,设置成功
61 // 其他,设置失败
MPU_Set_Gyro_Fsr(uint8_t fsr)62 uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
63 {
64 return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3); // 设置陀螺仪满量程范围
65 }
66 // 设置MPU6050加速度传感器满量程范围
67 // fsr:0,±2g;1,±4g;2,±8g;3,±16g
68 // 返回值:0,设置成功
69 // 其他,设置失败
MPU_Set_Accel_Fsr(uint8_t fsr)70 uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
71 {
72 return MPU_Write_Byte(MPU_ACCEL_CFG_REG,
73 fsr << 3); // 设置加速度传感器满量程范围
74 }
75 // 设置MPU6050的数字低通滤波器
76 // lpf:数字低通滤波频率(Hz)
77 // 返回值:0,设置成功
78 // 其他,设置失败
MPU_Set_LPF(u16 lpf)79 uint8_t MPU_Set_LPF(u16 lpf)
80 {
81 uint8_t data = 0;
82 if (lpf >= 188)
83 data = 1;
84 else if (lpf >= 98)
85 data = 2;
86 else if (lpf >= 42)
87 data = 3;
88 else if (lpf >= 20)
89 data = 4;
90 else if (lpf >= 10)
91 data = 5;
92 else
93 data = 6;
94 return MPU_Write_Byte(MPU_CFG_REG, data); // 设置数字低通滤波器
95 }
96 // 设置MPU6050的采样率(假定Fs=1KHz)
97 // rate:4~1000(Hz)
98 // 返回值:0,设置成功
99 // 其他,设置失败
MPU_Set_Rate(u16 rate)100 uint8_t MPU_Set_Rate(u16 rate)
101 {
102 uint8_t data;
103 if (rate > 1000)
104 rate = 1000;
105 if (rate < 4)
106 rate = 4;
107 data = 1000 / rate - 1;
108 MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data); // 设置数字低通滤波器
109 return MPU_Set_LPF(rate / 2); // 自动设置LPF为采样率的一半
110 }
111
112 // 初始化MPU6050
113 // 返回值:0,成功
114 // 其他,错误代码
MPU_Init(void)115 uint8_t MPU_Init(void)
116 {
117 uint8_t device_id = 0;
118
119 int32_t ret = sensor_i2c_open (MPU_I2C_PORT, MPU_ADDR, I2C_BUS_BIT_RATES_100K, 0);
120 if (ret) {
121 LOGE("SENSOR", "sensor i2c open failed, ret:%d\n", ret);
122 return -EIO;
123 }
124
125 MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80); // 复位MPU6050
126 aos_msleep(100);
127 MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00); // 唤醒MPU6050
128 MPU_Set_Gyro_Fsr(3); // 陀螺仪传感器,±2000dps
129 MPU_Set_Accel_Fsr(0); // 加速度传感器,±2g
130 MPU_Set_Rate(50); // 设置采样率50Hz
131 MPU_Write_Byte(MPU_INT_EN_REG, 0X00); // 关闭所有中断
132 MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00); // I2C主模式关闭
133 MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00); // 关闭FIFO
134 MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80); // INT引脚低电平有效
135 device_id = MPU_Read_Byte(MPU_DEVICE_ID_REG);
136 if (device_id == MPU_DEV_ID) {
137 // 器件ID正确
138 LOGI("SENSOR", "MPU init OK\n");
139 MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01); // 设置CLKSEL,PLL X轴为参考
140 MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00); // 加速度与陀螺仪都工作
141 MPU_Set_Rate(50); // 设置采样率为50Hz
142 } else {
143 LOGE("SENSOR", "MPU init Error -- %x\n", device_id);
144 return 1;
145 }
146 return 0;
147 }
148
149 // 得到温度值
150 // 返回值:温度值(扩大了100倍)
MPU_Get_Temperature(void)151 float MPU_Get_Temperature(void)
152 {
153 uint8_t buf[2];
154 short raw;
155 float temp;
156 MPU_Read_Len(MPU_TEMP_OUTH_REG, 2, buf);
157 raw = ((u16)buf[0] << 8) | buf[1];
158 temp = 36.53 + ((double)raw) / 340;
159 return temp;
160 ;
161 }
162 // 得到陀螺仪值(原始值)
163 // gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
164 // 返回值:0,成功
165 // 其他,错误代码
MPU_Get_Gyroscope(short * gx,short * gy,short * gz)166 void MPU_Get_Gyroscope(short *gx, short *gy, short *gz)
167 {
168 uint8_t buf[6];
169
170 MPU_Read_Len(MPU_GYRO_XOUTH_REG, 6, buf);
171 *gx = ((u16)buf[0] << 8) | buf[1];
172 *gy = ((u16)buf[2] << 8) | buf[3];
173 *gz = ((u16)buf[4] << 8) | buf[5];
174 }
175 // 得到加速度值(原始值)
176 // gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
177 // 返回值:0,成功
178 // 其他,错误代码
MPU_Get_Accelerometer(short * ax,short * ay,short * az)179 void MPU_Get_Accelerometer(short *ax, short *ay, short *az)
180 {
181 uint8_t buf[6];
182
183 MPU_Read_Len(MPU_ACCEL_XOUTH_REG, 6, buf);
184 *ax = ((u16)buf[0] << 8) | buf[1];
185 *ay = ((u16)buf[2] << 8) | buf[3];
186 *az = ((u16)buf[4] << 8) | buf[5];
187 }
188
189
MPU_Deinit(void)190 void MPU_Deinit(void) {
191 int32_t ret = sensor_i2c_close(MPU_I2C_PORT);
192 if (ret) {
193 LOGE("SENSOR", "sensor i2c close failed, ret:%d\n", ret);
194 }
195
196 return;
197 }
198