mpu6050.c

   1 /*
   2  * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
   3  * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions are met:
   7  *
   8  *     * Redistributions of source code must retain the above copyright
   9  *       notice, this list of conditions and the following disclaimer.
  10  *     * Redistributions in binary form must reproduce the above copyright
  11  *       notice, this list of conditions and the following disclaimer in the
  12  *       documentation and/or other materials provided with the distribution.
  13  *     * Neither the name of the University of California, Berkeley nor the
  14  *       names of its contributors may be used to endorse or promote products
  15  *       derived from this software without specific prior written permission.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
  18  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  19  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  20  * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
  21  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  22  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  23  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  24  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  25  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  26  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  27  */
  28
  29 #include <stdio.h>
  30 #include <string.h>
  31
  32 #include <aversive/wait.h>
  33
  34 #include "i2cm_sw.h"
  35 #include "mpu6050.h"
  36 #include "mpu6050_regs.h"
  37
  38 #define ToRad(x) ((x) * 0.01745329252)  // *pi/180
  39
  40 // fs_sel = 1
  41 // 500°/s
  42 /* #define gyro_x_resolution 66.5 */
  43 /* #define gyro_y_resolution 66.5 */
  44 /* #define gyro_z_resolution 66.5 */
  45
  46 // fs_sel = 3
  47 // 2000°/s
  48 #define gyro_x_resolution 16.4
  49 #define gyro_y_resolution 16.4
  50 #define gyro_z_resolution 16.4
  51
  52 #define accel_x_resolution 2048.0
  53 #define accel_y_resolution 2048.0
  54 #define accel_z_resolution 2048.0
  55
  56 #define MPU6050_ADDRESS 0x69
  57 #define MPU6050_MAGNETO_ADDRESS 0x0D
  58
  59 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
  60
  61 int16_t drift_g[3] = {0, 0, 0};
  62
  63 /* read "len" bytes of mpu6050 registers starting at specified address */
  64 static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr,
  65         uint8_t *values, uint8_t len)
  66 {
  67         uint8_t err = 0;
  68
  69         err = i2cm_send(i2c_addr, &reg_addr, 1);
  70         if (err) {
  71                 printf("read reg len: i2c error send\r\n");
  72                 return err;
  73         }
  74
  75         err = i2cm_recv(i2c_addr, len);
  76         if (err) {
  77                 printf("read reg len: i2c error recv\r\n");
  78                 return err;
  79         }
  80
  81         err = i2cm_get_recv_buffer(values, len);
  82         if (err != len) {
  83                 printf("read reg len: i2c error get recv\r\n");
  84                 return 0xFF;
  85         }
  86
  87         return 0;
  88 }
  89
  90 /* read one byte of mpu6050 register at specified address */
  91 static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr,
  92         uint8_t *value)
  93 {
  94         return read_reg_len(i2c_addr, reg_addr, value, 1);;
  95 }
  96
  97 /* fill the axes[3] pointer with the 3 axes of gyro (16bits) */
  98 uint8_t mpu6050_read_gyro_raw(int16_t *axes)
  99 {
 100         uint8_t err;
 101         uint8_t i;
 102
 103         err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H,
 104                 (uint8_t *)axes, sizeof(int16_t) * 3);
 105         for (i = 0; i < 3; i++) {
 106                 axes[i] = SWAP_16(axes[i]);
 107         }
 108
 109         return err;
 110 }
 111
 112 /* fill the imu structure with axes comming from mpu6050 */
 113 uint8_t mpu6050_read_all_axes(struct imu_info *imu)
 114 {
 115         int16_t axes[10];
 116         uint8_t err;
 117         uint8_t i;
 118
 119         err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H,
 120                 (uint8_t *)axes, sizeof(axes));
 121
 122         for (i = 0; i < 7; i++) {
 123                 axes[i] = SWAP_16(axes[i]);
 124         }
 125
 126         imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ;
 127         imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ;
 128         imu->az = 9.81 * (double)axes[2] / accel_z_resolution ;
 129
 130         imu->temp = (double)axes[3]/340. + 36.5;
 131
 132         imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution);
 133         imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution);
 134         imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution);
 135
 136         imu->mx = (double) axes[7] * 0.3;
 137         imu->my = (double) axes[8] * 0.3;
 138         imu->mz = (double) axes[9] * 0.3;
 139
 140         return err;
 141 }
 142
 143
 144 /* write a 8bits value at the specified register of the mpu6050 */
 145 static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
 146 {
 147         uint8_t err;
 148         uint8_t buffer[2];
 149         uint8_t check = 0;
 150
 151         buffer[0] = reg;
 152         buffer[1] = val;
 153
 154         err = i2cm_send(address, (unsigned char*)buffer, 2);
 155         if (err)
 156                 printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
 157
 158         err = read_reg(address, reg, &check);
 159         if (err)
 160                 return err;
 161
 162         if (check != val) {
 163                 printf("reg %x: %x != %x\r\n", reg, check, val);
 164                 return 0xff;
 165         }
 166
 167         return err;
 168 }
 169
 170 /* XXX add comment */
 171 static void mpu6050_compute_drift(void)
 172 {
 173         uint16_t i;
 174         int32_t s_gx, s_gy, s_gz;
 175         int16_t g_values[3];
 176
 177         drift_g[0] = 0;
 178         drift_g[1] = 0;
 179         drift_g[2] = 0;
 180
 181         s_gx = s_gy = s_gz = 0;
 182
 183         for (i = 0; i < 0x100; i ++) {
 184                 mpu6050_read_gyro_raw(g_values);
 185                 s_gx += g_values[0];
 186                 s_gy += g_values[1];
 187                 s_gz += g_values[2];
 188         }
 189         printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i);
 190         s_gx /= i;
 191         s_gy /= i;
 192         s_gz /= i;
 193
 194         drift_g[0] = s_gx;
 195         drift_g[1] = s_gy;
 196         drift_g[2] = s_gz;
 197         printf("gyro drift:\r\n");
 198         printf("%d %d %d\r\n",
 199                drift_g[0],
 200                drift_g[1],
 201                drift_g[2]);
 202 }
 203
 204 static uint8_t setup_mpu9150_magneto(void)
 205 {
 206         //uint8_t i, err;
 207
 208         //MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
 209
 210         /* XXX doesn't work, no answer from magneto */
 211         /* for (i = 0; i < 127; i++) { */
 212         /*      printf("i=%d\r\n", i); */
 213         /*      err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
 214
 215         /*      if (err == 0) */
 216         /*              printf("COIN\r\n"); */
 217         /* } */
 218         send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
 219         send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
 220         send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
 221
 222         //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
 223
 224         send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
 225         send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
 226         //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
 227         send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
 228         //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
 229         send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
 230         send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
 231         send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
 232         send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
 233         send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
 234         send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
 235         send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
 236
 237         send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
 238         send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
 239         send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
 240         send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
 241         send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
 242         send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
 243
 244         return 0;
 245 }
 246
 247 int8_t mpu6050_init(void)
 248 {
 249         uint8_t err;
 250         uint8_t id = 0;
 251
 252         wait_ms(1000); /* XXX needed ? */
 253
 254         while (1) { /* XXX timeout */
 255                 err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
 256                 if (err)
 257                         continue;
 258                 if (id == 0x68)
 259                         break;
 260         }
 261
 262         /* XX use one of the gyro for clock ref: if we don't do that, some i2c
 263          * commands fail... why ?? */
 264         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
 265
 266         /* Sets sample rate to 1000/1+1 = 500Hz */
 267         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
 268         /* Disable FSync, 48Hz DLPF */
 269         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
 270         /* Disable gyro self tests, scale of 500 degrees/s */
 271         /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */
 272         /* Disable gyro self tests, scale of 2000 degrees/s */
 273         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
 274
 275         /* Disable accel self tests, scale of +-16g, no DHPF */
 276         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
 277
 278         /* Freefall threshold of <|0mg| */
 279         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
 280         /* Freefall duration limit of 0 */
 281         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
 282         /* Motion threshold of >0mg */
 283         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
 284         /* Motion duration of >0s */
 285         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
 286         /* Zero motion threshold */
 287         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
 288         /* Zero motion duration threshold */
 289         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
 290         /* Disable sensor output to FIFO buffer */
 291         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
 292
 293         /* AUX I2C setup */
 294         /* Sets AUX I2C to single master control, plus other config */
 295         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
 296         /* Setup AUX I2C slaves */
 297         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
 298         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
 299         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
 300         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
 301         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
 302
 303         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
 304         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
 305         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
 306         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
 307         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
 308         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
 309         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
 310         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
 311
 312         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
 313         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
 314         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
 315         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
 316
 317         /* Setup INT pin and AUX I2C pass through */
 318         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
 319         /* Enable data ready interrupt */
 320         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
 321
 322         /* Slave out, dont care */
 323         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
 324         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
 325         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
 326         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
 327
 328         /* More slave config */
 329         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
 330         /* Reset sensor signal paths */
 331         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
 332         /* Motion detection control */
 333         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
 334         /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */
 335         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
 336         /* Sets clock source to gyro reference w/ PLL */
 337         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
 338         /* Controls frequency of wakeups in accel low power mode plus the sensor
 339          * standby modes */
 340         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
 341         /* Data transfer to and from the FIFO buffer */
 342         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
 343
 344         setup_mpu9150_magneto();
 345
 346         printf("MPU6050 Setup Complete\r\n");
 347         mpu6050_compute_drift();
 348         printf("MPU6050 drift computed\r\n");
 349
 350         return 0;
 351 }