mpu60x0.c

Go to the documentation of this file.

/*
 * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
#include "peripherals/mpu60x0.h"
 
const float MPU60X0_GYRO_SENS[4] = {
  MPU60X0_GYRO_SENS_250,
  MPU60X0_GYRO_SENS_500,
  MPU60X0_GYRO_SENS_1000,
  MPU60X0_GYRO_SENS_2000
};
const float MPU60X0_GYRO_SENS[4] = {…};
 
const struct FloatRates MPU60X0_GYRO_SENS_F[4] = {
  {MPU60X0_GYRO_SENS_250, MPU60X0_GYRO_SENS_250, MPU60X0_GYRO_SENS_250},
  {MPU60X0_GYRO_SENS_500, MPU60X0_GYRO_SENS_500, MPU60X0_GYRO_SENS_500},
  {MPU60X0_GYRO_SENS_1000, MPU60X0_GYRO_SENS_1000, MPU60X0_GYRO_SENS_1000},
  {MPU60X0_GYRO_SENS_2000, MPU60X0_GYRO_SENS_2000, MPU60X0_GYRO_SENS_2000}
};
const struct FloatRates MPU60X0_GYRO_SENS_F[4] = {…};
 
 
const float MPU60X0_ACCEL_SENS[4] = {
  MPU60X0_ACCEL_SENS_2G,
  MPU60X0_ACCEL_SENS_4G,
  MPU60X0_ACCEL_SENS_8G,
  MPU60X0_ACCEL_SENS_16G
};
const float MPU60X0_ACCEL_SENS[4] = {…};
 
 
const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4] = {
  {MPU60X0_ACCEL_SENS_2G, MPU60X0_ACCEL_SENS_2G, MPU60X0_ACCEL_SENS_2G},
  {MPU60X0_ACCEL_SENS_4G, MPU60X0_ACCEL_SENS_4G, MPU60X0_ACCEL_SENS_4G},
  {MPU60X0_ACCEL_SENS_8G, MPU60X0_ACCEL_SENS_8G, MPU60X0_ACCEL_SENS_8G},
  {MPU60X0_ACCEL_SENS_16G, MPU60X0_ACCEL_SENS_16G, MPU60X0_ACCEL_SENS_16G}
};
const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4] = {…};
 
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
{
  c->type = MPU60X0;
  c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
  c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
  c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
  c->dlpf_cfg_acc = MPU60X0_DEFAULT_DLPF_CFG_ACC;
  c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
  c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
  c->drdy_int_enable = false;
 
  /* Number of bytes to read starting with MPU60X0_REG_INT_STATUS
   * By default read only gyro and accel data -> 15 bytes.
   * Increase to include slave data.
   */
  c->nb_bytes = 15;
  c->nb_slaves = 0;
  c->nb_slave_init = 0;
 
  c->i2c_bypass = false;
}
void mpu60x0_set_default_config(struct Mpu60x0Config *c) {…}
 
void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Config *config)
{
  switch (config->init_status) {
    case MPU60X0_CONF_RESET:
      /* device reset, set register values to defaults */
      mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, (1 << 6));
      config->init_status++;
      break;
    case MPU60X0_CONF_USER_RESET:
      /* trigger FIFO, I2C_MST and SIG_COND resets */
      mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_FIFO_RESET) |
                                           (1 << MPU60X0_I2C_MST_RESET) |
                                           (1 << MPU60X0_SIG_COND_RESET)));
      config->init_status++;
      break;
    case MPU60X0_CONF_PWR:
      /* switch to gyroX clock by default */
      mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel) | (0 << 6)));
      config->init_status++;
      break;
    case MPU60X0_CONF_SD:
      /* configure sample rate divider */
      mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div);
      config->init_status++;
      break;
    case MPU60X0_CONF_DLPF:
      /* configure digital low pass filter */
      mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg);
      config->init_status++;
      break;
    case MPU60X0_CONF_GYRO:
      /* configure gyro range */
      mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range << 3));
      config->init_status++;
      break;
    case MPU60X0_CONF_ACCEL:
      /* configure accelerometer range */
      mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range << 3));
      config->init_status++;
      break;
    case MPU60X0_CONF_ACCEL2:
      /* configure accelerometer DLPF (for ICM devices) */
      if (config->type != MPU60X0) {
        mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG2, config->dlpf_cfg_acc);
      }
      config->init_status++;
      break;
    case MPU60X0_CONF_I2C_SLAVES:
      /* if any, set MPU for I2C slaves and configure them*/
      if (config->nb_slaves > 0) {
        /* returns TRUE when all slaves are configured */
        if (mpu60x0_configure_i2c_slaves(mpu_set, mpu)) {
          config->init_status++;
        }
      } else {
        config->init_status++;
      }
      break;
    case MPU60X0_CONF_INT_ENABLE:
      /* configure data ready interrupt */
      mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable << 0));
      config->init_status++;
      break;
    case MPU60X0_CONF_UNDOC1:
      /* configure undocumented register (for ICM devices) to remove 2.7m/s^2 y-acc bias */
      if (config->type != MPU60X0) {
        mpu_set(mpu, MPU60X0_REG_UNDOC1, 0xC9);
      }
      config->init_status++;
      break;
    case MPU60X0_CONF_DONE:
      config->initialized = true;
      break;
    default:
      break;
  }
}
void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Config *config) {…}