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 8192.0
  30 #define accel_y_resolution 8192.0
  31 #define accel_z_resolution 8192.0
  32
  33
  34
  35
  36 #define MPU6050_OFF_MSB 1
  37 #define MPU6050_OFF_LSB 0
  38
  39
  40 #define MPU6050_ADDRESS 0x68
  41 #define MPU6050_MAGNETO_ADDRESS 0x0C
  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("error %.2X\r\n", err);
 117         return err;
 118 }
 119
 120
 121 void mpu6050_compute_drift(void)
 122 {
 123         uint16_t i;
 124         int32_t s_gx, s_gy, s_gz;
 125         int16_t g_values[3];
 126
 127         drift_g[0] = 0;
 128         drift_g[1] = 0;
 129         drift_g[2] = 0;
 130
 131         s_gx = s_gy = s_gz = 0;
 132
 133         for (i = 0; i < 0x100; i ++) {
 134                 mpu6050_read_gyro_raw(&g_values);
 135                 s_gx += g_values[0];
 136                 s_gy += g_values[1];
 137                 s_gz += g_values[2];
 138         }
 139         printf("%"PRId32" %"PRId32" %"PRId32" (%"PRId32") \r\n", s_gx, s_gy, s_gz, i);
 140         s_gx /= i;
 141         s_gy /= i;
 142         s_gz /= i;
 143
 144         drift_g[0] = s_gx;
 145         drift_g[1] = s_gy;
 146         drift_g[2] = s_gz;
 147         printf("gyro drift:\r\n");
 148         printf("%d %d %d\r\n",
 149                drift_g[0],
 150                drift_g[1],
 151                drift_g[2]);
 152 }
 153
 154
 155 uint8_t setup_mpu9150_magneto()
 156 {
 157         //MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
 158
 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
 162
 163         //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
 164
 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);
 177
 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
 184
 185 }
 186
 187 uint8_t mpu6050_init(void)
 188 {
 189         uint8_t err;
 190         uint8_t id = 0;
 191         uint8_t config = 0;
 192
 193
 194         while(1) {
 195                 err = read_reg(MPU6050_ADDRESS, 0x75, &id);
 196                 if (err)
 197                         continue;
 198                 if (id == 0x68)
 199                         break;
 200         }
 201
 202
 203
 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);
 210
 211         //Disable gyro self tests, scale of 2000 degrees/s
 212         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
 213
 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);
 230
 231         //AUX I2C setup
 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);
 252
 253
 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);
 259
 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
 301
 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);
 307         //More slave config
 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
 329
 330         setup_mpu9150_magneto();
 331
 332
 333         printf("MPU6050 Setup Complete\r\n");
 334         mpu6050_compute_drift();
 335         printf("MPU6050 drift computed\r\n");
 336
 337 }