mpu6050.c

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