7 #include <aversive/wait.h>
11 #include "i2c_helper.h"
15 #define ToRad(x) ((x)*0.01745329252) // *pi/180
19 #define gyro_x_resolution 66.5
20 #define gyro_y_resolution 66.5
21 #define gyro_z_resolution 66.5
25 #define gyro_x_resolution 16.4
26 #define gyro_y_resolution 16.4
27 #define gyro_z_resolution 16.4
29 #define accel_x_resolution 8192.0
30 #define accel_y_resolution 8192.0
31 #define accel_z_resolution 8192.0
36 #define MPU6050_OFF_MSB 1
37 #define MPU6050_OFF_LSB 0
40 #define MPU6050_ADDRESS 0x68
41 #define MPU6050_MAGNETO_ADDRESS 0x0C
43 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
59 int16_t drift_g[3] = {0, 0, 0};
62 uint8_t mpu6050_read_gyro_raw(int16_t *values)
66 err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
68 values[i] = SWAP_16(values[i]);
74 uint8_t mpu6050_read_all_axes(int16_t *values)
78 err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10);
81 values[i] = SWAP_16(values[i]);
84 mpu6050_ax = 9.81 * (float)values[0] / accel_x_resolution ;
85 mpu6050_ay = 9.81 * (float)values[1] / accel_y_resolution ;
86 mpu6050_az = 9.81 * (float)values[2] / accel_z_resolution ;
88 mpu6050_temp = (float)values[3]/340. + 36.5;
90 mpu6050_gx = ToRad((float)(values[4] - drift_g[0]) / gyro_x_resolution );
91 mpu6050_gy = ToRad((float)(values[5] - drift_g[1]) / gyro_y_resolution );
92 mpu6050_gz = ToRad((float)(values[6] - drift_g[2]) / gyro_z_resolution );
94 mpu6050_mx = (float) values[7] * 0.3;
95 mpu6050_my = (float) values[8] * 0.3;
96 mpu6050_mz = (float) values[9] * 0.3;
106 uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
114 err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
116 printf("error %.2X\r\n", err);
121 void mpu6050_compute_drift(void)
124 int32_t s_gx, s_gy, s_gz;
131 s_gx = s_gy = s_gz = 0;
133 for (i = 0; i < 0x100; i ++) {
134 mpu6050_read_gyro_raw(&g_values);
139 printf("%"PRId32" %"PRId32" %"PRId32" (%"PRId32") \r\n", s_gx, s_gy, s_gz, i);
147 printf("gyro drift:\r\n");
148 printf("%d %d %d\r\n",
155 uint8_t setup_mpu9150_magneto()
157 //MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass
159 send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
160 send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
161 send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
163 //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU
165 send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
166 send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
167 //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
168 send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
169 //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
170 send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
171 send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
172 send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
173 send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
174 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
175 send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
176 send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
178 send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
179 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
180 send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
181 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
182 send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
183 send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
187 uint8_t mpu6050_init(void)
195 err = read_reg(MPU6050_ADDRESS, 0x75, &id);
204 //Sets sample rate to 1000/1+1 = 500Hz
205 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
206 //Disable FSync, 48Hz DLPF
207 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
208 //Disable gyro self tests, scale of 500 degrees/s
209 //send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
211 //Disable gyro self tests, scale of 2000 degrees/s
212 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
214 //Disable accel self tests, scale of +-4g, no DHPF
215 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00001000);
216 //Freefall threshold of <|0mg|
217 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
218 //Freefall duration limit of 0
219 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
220 //Motion threshold of >0mg
221 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
222 //Motion duration of >0s
223 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
224 //Zero motion threshold
225 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
226 //Zero motion duration threshold
227 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
228 //Disable sensor output to FIFO buffer
229 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
232 //Sets AUX I2C to single master control, plus other config
233 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
234 //Setup AUX I2C slaves
235 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
236 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
237 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
238 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
239 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
240 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
241 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
242 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
243 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
244 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
245 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
246 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
247 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
248 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
249 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
250 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
251 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
254 //MPU6050_RA_I2C_MST_STATUS //Read-only
255 //Setup INT pin and AUX I2C pass through
256 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
257 //Enable data ready interrupt
258 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
260 //MPU6050_RA_DMP_INT_STATUS //Read-only
261 //MPU6050_RA_INT_STATUS 3A //Read-only
262 //MPU6050_RA_ACCEL_XOUT_H //Read-only
263 //MPU6050_RA_ACCEL_XOUT_L //Read-only
264 //MPU6050_RA_ACCEL_YOUT_H //Read-only
265 //MPU6050_RA_ACCEL_YOUT_L //Read-only
266 //MPU6050_RA_ACCEL_ZOUT_H //Read-only
267 //MPU6050_RA_ACCEL_ZOUT_L //Read-only
268 //MPU6050_RA_TEMP_OUT_H //Read-only
269 //MPU6050_RA_TEMP_OUT_L //Read-only
270 //MPU6050_RA_GYRO_XOUT_H //Read-only
271 //MPU6050_RA_GYRO_XOUT_L //Read-only
272 //MPU6050_RA_GYRO_YOUT_H //Read-only
273 //MPU6050_RA_GYRO_YOUT_L //Read-only
274 //MPU6050_RA_GYRO_ZOUT_H //Read-only
275 //MPU6050_RA_GYRO_ZOUT_L //Read-only
276 //MPU6050_RA_EXT_SENS_DATA_00 //Read-only
277 //MPU6050_RA_EXT_SENS_DATA_01 //Read-only
278 //MPU6050_RA_EXT_SENS_DATA_02 //Read-only
279 //MPU6050_RA_EXT_SENS_DATA_03 //Read-only
280 //MPU6050_RA_EXT_SENS_DATA_04 //Read-only
281 //MPU6050_RA_EXT_SENS_DATA_05 //Read-only
282 //MPU6050_RA_EXT_SENS_DATA_06 //Read-only
283 //MPU6050_RA_EXT_SENS_DATA_07 //Read-only
284 //MPU6050_RA_EXT_SENS_DATA_08 //Read-only
285 //MPU6050_RA_EXT_SENS_DATA_09 //Read-only
286 //MPU6050_RA_EXT_SENS_DATA_10 //Read-only
287 //MPU6050_RA_EXT_SENS_DATA_11 //Read-only
288 //MPU6050_RA_EXT_SENS_DATA_12 //Read-only
289 //MPU6050_RA_EXT_SENS_DATA_13 //Read-only
290 //MPU6050_RA_EXT_SENS_DATA_14 //Read-only
291 //MPU6050_RA_EXT_SENS_DATA_15 //Read-only
292 //MPU6050_RA_EXT_SENS_DATA_16 //Read-only
293 //MPU6050_RA_EXT_SENS_DATA_17 //Read-only
294 //MPU6050_RA_EXT_SENS_DATA_18 //Read-only
295 //MPU6050_RA_EXT_SENS_DATA_19 //Read-only
296 //MPU6050_RA_EXT_SENS_DATA_20 //Read-only
297 //MPU6050_RA_EXT_SENS_DATA_21 //Read-only
298 //MPU6050_RA_EXT_SENS_DATA_22 //Read-only
299 //MPU6050_RA_EXT_SENS_DATA_23 //Read-only
300 //MPU6050_RA_MOT_DETECT_STATUS //Read-only
302 //Slave out, dont care
303 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
304 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
305 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
306 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
308 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
309 //Reset sensor signal paths
310 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
311 //Motion detection control
312 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
313 //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
314 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
315 //Sets clock source to gyro reference w/ PLL
316 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
317 //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
318 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
319 //MPU6050_RA_BANK_SEL //Not in datasheet
320 //MPU6050_RA_MEM_START_ADDR //Not in datasheet
321 //MPU6050_RA_MEM_R_W //Not in datasheet
322 //MPU6050_RA_DMP_CFG_1 //Not in datasheet
323 //MPU6050_RA_DMP_CFG_2 //Not in datasheet
324 //MPU6050_RA_FIFO_COUNTH //Read-only
325 //MPU6050_RA_FIFO_COUNTL //Read-only
326 //Data transfer to and from the FIFO buffer
327 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
328 //MPU6050_RA_WHO_AM_I //Read-only, I2C address
330 setup_mpu9150_magneto();
333 printf("MPU6050 Setup Complete\r\n");
334 mpu6050_compute_drift();
335 printf("MPU6050 drift computed\r\n");