mpu6050.c

   1 #include <stdio.h>
   2 #include <string.h>
   3
   4 #include <timer.h>
   5
   6 #include <aversive/wait.h>
   7 #include <aversive/pgmspace.h>
   8
   9 #include <uart.h>
  10 #include <i2c.h>
  11
  12 #include "i2c_helper.h"
  13
  14 #include "mpu6050.h"
  15
  16 #define ToRad(x) ((x)*0.01745329252)  // *pi/180
  17
  18 // fs_sel = 1
  19 // 500°/s
  20 /* #define gyro_x_resolution 66.5 */
  21 /* #define gyro_y_resolution 66.5 */
  22 /* #define gyro_z_resolution 66.5 */
  23
  24 // fs_sel = 3
  25 // 2000°/s
  26 #define gyro_x_resolution 16.4
  27 #define gyro_y_resolution 16.4
  28 #define gyro_z_resolution 16.4
  29
  30 #define accel_x_resolution 2048.0
  31 #define accel_y_resolution 2048.0
  32 #define accel_z_resolution 2048.0
  33
  34
  35
  36
  37 #define MPU6050_OFF_MSB 1
  38 #define MPU6050_OFF_LSB 0
  39
  40
  41 #define MPU6050_ADDRESS 0x69
  42 #define MPU6050_MAGNETO_ADDRESS 0x0D
  43
  44 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
  45
  46 double mpu6050_gx;
  47 double mpu6050_gy;
  48 double mpu6050_gz;
  49
  50 double mpu6050_ax;
  51 double mpu6050_ay;
  52 double mpu6050_az;
  53
  54 double mpu6050_mx;
  55 double mpu6050_my;
  56 double mpu6050_mz;
  57
  58 double mpu6050_temp;
  59
  60 int16_t drift_g[3] = {0, 0, 0};
  61
  62
  63 uint8_t mpu6050_read_gyro_raw(int16_t *values)
  64 {
  65         uint8_t err;
  66         uint8_t i;
  67         err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
  68         for (i=0;i<3; i++) {
  69                 values[i] = SWAP_16(values[i]);
  70         }
  71
  72         return err;
  73 }
  74
  75 uint8_t mpu6050_read_all_axes(int16_t *values)
  76 {
  77         uint8_t err;
  78         uint8_t i;
  79         err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10);
  80
  81         for (i=0;i<7; i++) {
  82                 values[i] = SWAP_16(values[i]);
  83         }
  84
  85         mpu6050_ax = 9.81 * (double)values[0] / accel_x_resolution ;
  86         mpu6050_ay = 9.81 * (double)values[1] / accel_y_resolution ;
  87         mpu6050_az = 9.81 * (double)values[2] / accel_z_resolution ;
  88
  89         mpu6050_temp = (double)values[3]/340. + 36.5;
  90
  91         mpu6050_gx = ToRad((double)(values[4] - drift_g[0]) / gyro_x_resolution );
  92         mpu6050_gy = ToRad((double)(values[5] - drift_g[1]) / gyro_y_resolution );
  93         mpu6050_gz = ToRad((double)(values[6] - drift_g[2]) / gyro_z_resolution );
  94
  95         mpu6050_mx = (double) values[7] * 0.3;
  96         mpu6050_my = (double) values[8] * 0.3;
  97         mpu6050_mz = (double) values[9] * 0.3;
  98
  99
 100         return err;
 101 }
 102
 103
 104
 105
 106
 107 uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
 108 {
 109         uint8_t err;
 110         uint8_t buffer[2];
 111
 112         buffer[0] = reg;
 113         buffer[1] = val;
 114
 115         err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
 116         if (err)
 117                 printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
 118
 119         return err;
 120 }
 121
 122 uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
 123 {
 124         uint8_t err;
 125         uint8_t buffer[2];
 126         uint8_t check = 0;
 127
 128         buffer[0] = reg;
 129         buffer[1] = val;
 130
 131         err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
 132         if (err)
 133                 printf("send_check_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
 134
 135         err = read_reg(address, reg, &check);
 136         if (err)
 137                 return err;
 138
 139         if (check != val) {
 140                 printf("reg %x: %x != %x\r\n", reg, check, val);
 141                 return 0xff;
 142         }
 143
 144         return err;
 145 }
 146
 147
 148 void mpu6050_compute_drift(void)
 149 {
 150         uint16_t i;
 151         int32_t s_gx, s_gy, s_gz;
 152         int16_t g_values[3];
 153
 154         drift_g[0] = 0;
 155         drift_g[1] = 0;
 156         drift_g[2] = 0;
 157
 158         s_gx = s_gy = s_gz = 0;
 159
 160         for (i = 0; i < 0x100; i ++) {
 161                 mpu6050_read_gyro_raw(g_values);
 162                 s_gx += g_values[0];
 163                 s_gy += g_values[1];
 164                 s_gz += g_values[2];
 165         }
 166         printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i);
 167         s_gx /= i;
 168         s_gy /= i;
 169         s_gz /= i;
 170
 171         drift_g[0] = s_gx;
 172         drift_g[1] = s_gy;
 173         drift_g[2] = s_gz;
 174         printf("gyro drift:\r\n");
 175         printf("%d %d %d\r\n",
 176                drift_g[0],
 177                drift_g[1],
 178                drift_g[2]);
 179 }
 180
 181
 182 uint8_t setup_mpu9150_magneto(void)
 183 {
 184         //uint8_t i, err;
 185
 186         //MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
 187
 188         /* XXX doesn't work, no answer from magneto */
 189         /* for (i = 0; i < 127; i++) { */
 190         /*      printf("i=%d\r\n", i); */
 191         /*      err = send_check_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
 192
 193         /*      if (err == 0) */
 194         /*              printf("COIN\r\n"); */
 195         /* } */
 196         send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
 197         send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
 198         send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
 199
 200         //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
 201
 202         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
 203         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
 204         //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
 205         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
 206         //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
 207         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
 208         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
 209         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
 210         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
 211         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
 212         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
 213         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
 214
 215         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
 216         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
 217         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
 218         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
 219         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
 220         send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
 221
 222         return 0;
 223 }
 224
 225 uint8_t mpu6050_init(void)
 226 {
 227         uint8_t err;
 228         uint8_t id = 0;
 229         //uint8_t config = 0;
 230
 231         wait_ms(1000);
 232
 233         while(1) {
 234                 err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
 235                 if (err)
 236                         continue;
 237                 if (id == 0x68)
 238                         break;
 239         }
 240
 241         /* use one of the gyro for clock ref: if we don't do that, some i2c commands fail ?? */
 242         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
 243
 244         //Sets sample rate to 1000/1+1 = 500Hz
 245         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
 246         //Disable FSync, 48Hz DLPF
 247         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
 248         //Disable gyro self tests, scale of 500 degrees/s
 249         //send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
 250         //Disable gyro self tests, scale of 2000 degrees/s
 251         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
 252
 253         //Disable accel self tests, scale of +-16g, no DHPF
 254         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
 255
 256         //Freefall threshold of <|0mg|
 257         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
 258         //Freefall duration limit of 0
 259         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
 260         //Motion threshold of >0mg
 261         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
 262         //Motion duration of >0s
 263         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
 264         //Zero motion threshold
 265         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
 266         //Zero motion duration threshold
 267         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
 268         //Disable sensor output to FIFO buffer
 269         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
 270
 271         //AUX I2C setup
 272         //Sets AUX I2C to single master control, plus other config
 273         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
 274         //Setup AUX I2C slaves
 275         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
 276         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
 277         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
 278         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
 279         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
 280
 281         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
 282         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
 283         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
 284         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
 285         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
 286         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
 287         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
 288         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
 289
 290         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
 291         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
 292         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
 293         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
 294
 295
 296         //MPU6050_RA_I2C_MST_STATUS //Read-only
 297         //Setup INT pin and AUX I2C pass through
 298         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
 299         //Enable data ready interrupt
 300         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
 301
 302         //MPU6050_RA_DMP_INT_STATUS                //Read-only
 303         //MPU6050_RA_INT_STATUS 3A                //Read-only
 304         //MPU6050_RA_ACCEL_XOUT_H                 //Read-only
 305         //MPU6050_RA_ACCEL_XOUT_L                 //Read-only
 306         //MPU6050_RA_ACCEL_YOUT_H                 //Read-only
 307         //MPU6050_RA_ACCEL_YOUT_L                 //Read-only
 308         //MPU6050_RA_ACCEL_ZOUT_H                 //Read-only
 309         //MPU6050_RA_ACCEL_ZOUT_L                 //Read-only
 310         //MPU6050_RA_TEMP_OUT_H                 //Read-only
 311         //MPU6050_RA_TEMP_OUT_L                 //Read-only
 312         //MPU6050_RA_GYRO_XOUT_H                 //Read-only
 313         //MPU6050_RA_GYRO_XOUT_L                 //Read-only
 314         //MPU6050_RA_GYRO_YOUT_H                 //Read-only
 315         //MPU6050_RA_GYRO_YOUT_L                 //Read-only
 316         //MPU6050_RA_GYRO_ZOUT_H                 //Read-only
 317         //MPU6050_RA_GYRO_ZOUT_L                 //Read-only
 318         //MPU6050_RA_EXT_SENS_DATA_00         //Read-only
 319         //MPU6050_RA_EXT_SENS_DATA_01         //Read-only
 320         //MPU6050_RA_EXT_SENS_DATA_02         //Read-only
 321         //MPU6050_RA_EXT_SENS_DATA_03         //Read-only
 322         //MPU6050_RA_EXT_SENS_DATA_04         //Read-only
 323         //MPU6050_RA_EXT_SENS_DATA_05         //Read-only
 324         //MPU6050_RA_EXT_SENS_DATA_06         //Read-only
 325         //MPU6050_RA_EXT_SENS_DATA_07         //Read-only
 326         //MPU6050_RA_EXT_SENS_DATA_08         //Read-only
 327         //MPU6050_RA_EXT_SENS_DATA_09         //Read-only
 328         //MPU6050_RA_EXT_SENS_DATA_10         //Read-only
 329         //MPU6050_RA_EXT_SENS_DATA_11         //Read-only
 330         //MPU6050_RA_EXT_SENS_DATA_12         //Read-only
 331         //MPU6050_RA_EXT_SENS_DATA_13         //Read-only
 332         //MPU6050_RA_EXT_SENS_DATA_14         //Read-only
 333         //MPU6050_RA_EXT_SENS_DATA_15         //Read-only
 334         //MPU6050_RA_EXT_SENS_DATA_16         //Read-only
 335         //MPU6050_RA_EXT_SENS_DATA_17         //Read-only
 336         //MPU6050_RA_EXT_SENS_DATA_18         //Read-only
 337         //MPU6050_RA_EXT_SENS_DATA_19         //Read-only
 338         //MPU6050_RA_EXT_SENS_DATA_20         //Read-only
 339         //MPU6050_RA_EXT_SENS_DATA_21         //Read-only
 340         //MPU6050_RA_EXT_SENS_DATA_22         //Read-only
 341         //MPU6050_RA_EXT_SENS_DATA_23         //Read-only
 342         //MPU6050_RA_MOT_DETECT_STATUS         //Read-only
 343
 344         //Slave out, dont care
 345         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
 346         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
 347         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
 348         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
 349
 350         //More slave config
 351         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
 352         //Reset sensor signal paths
 353         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
 354         //Motion detection control
 355         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
 356         //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
 357         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
 358         //Sets clock source to gyro reference w/ PLL
 359         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
 360         //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
 361         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
 362         //MPU6050_RA_BANK_SEL                        //Not in datasheet
 363         //MPU6050_RA_MEM_START_ADDR                //Not in datasheet
 364         //MPU6050_RA_MEM_R_W                        //Not in datasheet
 365         //MPU6050_RA_DMP_CFG_1                        //Not in datasheet
 366         //MPU6050_RA_DMP_CFG_2                        //Not in datasheet
 367         //MPU6050_RA_FIFO_COUNTH                //Read-only
 368         //MPU6050_RA_FIFO_COUNTL                //Read-only
 369         //Data transfer to and from the FIFO buffer
 370         send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
 371         //MPU6050_RA_WHO_AM_I                         //Read-only, I2C address
 372
 373         setup_mpu9150_magneto();
 374
 375
 376         printf("MPU6050 Setup Complete\r\n");
 377         mpu6050_compute_drift();
 378         printf("MPU6050 drift computed\r\n");
 379
 380         return 0;
 381 }