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