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