src/main/drivers/accgyro_legacy/accgyro_l3g4200d.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 // NOTE: This gyro is considered obsolete and may be removed in the future.
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_GYRO_L3G4200D
  29
  30 #include "drivers/accgyro/accgyro.h"
  31 #include "accgyro_l3g4200d.h"
  32
  33 #include "common/maths.h"
  34 #include "common/axis.h"
  35
  36 #include "drivers/bus_i2c.h"
  37 #include "drivers/time.h"
  38 #include "drivers/sensor.h"
  39 #include "drivers/system.h"
  40
  41
  42 // L3G4200D, Standard address 0x68
  43 #define L3G4200D_ADDRESS         0x68
  44 #define L3G4200D_ID              0xD3
  45 #define L3G4200D_AUTOINCR        0x80
  46
  47 // Registers
  48 #define L3G4200D_WHO_AM_I        0x0F
  49 #define L3G4200D_CTRL_REG1       0x20
  50 #define L3G4200D_CTRL_REG2       0x21
  51 #define L3G4200D_CTRL_REG3       0x22
  52 #define L3G4200D_CTRL_REG4       0x23
  53 #define L3G4200D_CTRL_REG5       0x24
  54 #define L3G4200D_REFERENCE       0x25
  55 #define L3G4200D_STATUS_REG      0x27
  56 #define L3G4200D_GYRO_OUT        0x28
  57
  58 // Bits
  59 #define L3G4200D_POWER_ON        0x0F
  60 #define L3G4200D_FS_SEL_2000DPS  0xF0
  61 #define L3G4200D_DLPF_32HZ       0x00
  62 #define L3G4200D_DLPF_54HZ       0x40
  63 #define L3G4200D_DLPF_78HZ       0x80
  64 #define L3G4200D_DLPF_93HZ       0xC0
  65
  66 static void l3g4200dInit(gyroDev_t *gyro)
  67 {
  68     bool ack;
  69
  70     // Removed lowpass filter selection and just default to 32Hz regardless of gyro->hardware_lpf
  71     // The previous selection was broken anyway as the old gyro->lpf values ranged from 0-7 and
  72     // the switch statement would have always taken the default and used L3G4200D_DLPF_32HZ
  73
  74     delay(100);
  75
  76     ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
  77     if (!ack)
  78         failureMode(FAILURE_ACC_INIT);
  79
  80     delay(5);
  81     i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | L3G4200D_DLPF_32HZ);
  82
  83     UNUSED(gyro);
  84 }
  85
  86 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
  87 static bool l3g4200dRead(gyroDev_t *gyro)
  88 {
  89     uint8_t buf[6];
  90
  91     if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
  92         return false;
  93     }
  94
  95     gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
  96     gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
  97     gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
  98
  99     return true;
 100 }
 101
 102 bool l3g4200dDetect(gyroDev_t *gyro)
 103 {
 104     uint8_t deviceid;
 105
 106     delay(25);
 107
 108     i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
 109     if (deviceid != L3G4200D_ID)
 110         return false;
 111
 112     gyro->initFn = l3g4200dInit;
 113     gyro->readFn = l3g4200dRead;
 114
 115     // 14.2857dps/lsb scalefactor
 116     gyro->scale = 1.0f / 14.2857f;
 117
 118     return true;
 119 }
 120 #endif