src/main/drivers/accgyro_mpu6500.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/axis.h"
  25 #include "common/maths.h"
  26
  27 #include "system.h"
  28 #include "exti.h"
  29 #include "gpio.h"
  30
  31 #include "sensor.h"
  32 #include "accgyro.h"
  33 #include "accgyro_mpu.h"
  34 #include "accgyro_mpu6500.h"
  35
  36 extern uint16_t acc_1G;
  37
  38 bool mpu6500AccDetect(acc_t *acc)
  39 {
  40     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
  41         return false;
  42     }
  43
  44     acc->init = mpu6500AccInit;
  45     acc->read = mpuAccRead;
  46
  47     return true;
  48 }
  49
  50 bool mpu6500GyroDetect(gyro_t *gyro)
  51 {
  52     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
  53         return false;
  54     }
  55
  56     gyro->init = mpu6500GyroInit;
  57     gyro->read = mpuGyroRead;
  58
  59     // 16.4 dps/lsb scalefactor
  60     gyro->scale = 1.0f / 16.4f;
  61
  62     return true;
  63 }
  64
  65 void mpu6500AccInit(void)
  66 {
  67     mpuIntExtiInit();
  68
  69     acc_1G = 512 * 8;
  70 }
  71
  72 void mpu6500GyroInit(uint16_t lpf)
  73 {
  74     mpuIntExtiInit();
  75
  76 #ifdef NAZE
  77     // FIXME target specific code in driver code.
  78
  79     gpio_config_t gpio;
  80     // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
  81     if (hse_value == 12000000) {
  82         gpio.pin = Pin_13;
  83         gpio.speed = Speed_2MHz;
  84         gpio.mode = Mode_IN_FLOATING;
  85         gpioInit(GPIOC, &gpio);
  86     }
  87 #endif
  88
  89     uint8_t mpuLowPassFilter = determineMPULPF(lpf);
  90
  91     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
  92     delay(100);
  93     mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
  94     delay(100);
  95     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
  96     delay(100);
  97     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
  98     mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
  99     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 100     mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter);
 101     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0); // 1kHz S/R
 102
 103     // Data ready interrupt configuration
 104     mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
 105 #ifdef USE_MPU_DATA_READY_SIGNAL
 106     mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
 107 #endif
 108
 109 }