src/main/drivers/accgyro_legacy/accgyro_lsm303dlhc.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 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #ifdef USE_ACC_LSM303DLHC
  27
  28 #include "build/debug.h"
  29
  30 #include "common/maths.h"
  31 #include "common/axis.h"
  32
  33 #include "drivers/time.h"
  34 #include "drivers/io.h"
  35 #include "drivers/bus_i2c.h"
  36
  37 #include "drivers/sensor.h"
  38 #include "drivers/accgyro/accgyro.h"
  39 #include "accgyro_lsm303dlhc.h"
  40
  41 // Addresses (7 bit address format)
  42
  43 #define LSM303DLHC_ACCEL_ADDRESS 0x19
  44 #define LSM303DLHC_MAG_ADDRESS 0x1E
  45
  46 /**
  47  * Address Auto Increment  - See LSM303DLHC datasheet, Section 5.1.1 I2C operation.
  48  * http://www.st.com/web/en/resource/technical/document/datasheet/DM00027543.pdf
  49  *
  50  * "The I2C embedded inside the LSM303DLHC behaves like a slave device and the following protocol must be adhered to.
  51  * After the START condition (ST) a slave address is sent, once a slave acknowledge (SAK) has been returned, an 8-bit
  52  * sub-address (SUB) is transmitted; the 7 LSBs represent the actual register address while the MSB enables address
  53  * autoincrement.
  54  *
  55  * If the MSB of the SUB field is ‘1’, the SUB (register address) is automatically increased to allow multiple data
  56  * Read/Write.
  57  *
  58  * To minimize the communication between the master and magnetic digital interface of LSM303DLHC, the address pointer
  59  * updates automatically without master intervention.  This automatic address pointer update has two additional
  60  * features. First, when address 12 or higher is accessed, the pointer updates to address 00, and secondly, when
  61  * address 08 is reached, the pointer rolls back to address 03. Logically, the address pointer operation functions
  62  * as shown below.
  63  * 1) If (address pointer = 08) then the address pointer = 03
  64  * Or else, if (address pointer >= 12) then the address pointer = 0
  65  * Or else, (address pointer) = (address pointer) + 1
  66  *
  67  * The address pointer value itself cannot be read via the I2C bus"
  68  */
  69 #define AUTO_INCREMENT_ENABLE 0x80
  70
  71 // Registers
  72
  73 #define CTRL_REG1_A 0x20
  74 #define CTRL_REG4_A 0x23
  75 #define CTRL_REG5_A 0x24
  76 #define OUT_X_L_A 0x28
  77 #define CRA_REG_M 0x00
  78 #define CRB_REG_M 0x01
  79 #define MR_REG_M 0x02
  80 #define OUT_X_H_M 0x03
  81
  82 ///////////////////////////////////////
  83
  84 #define ODR_1344_HZ 0x90
  85 #define AXES_ENABLE 0x07
  86
  87 #define FULLSCALE_2G 0x00
  88 #define FULLSCALE_4G 0x10
  89 #define FULLSCALE_8G 0x20
  90 #define FULLSCALE_16G 0x30
  91
  92 #define BOOT 0x80
  93
  94 ///////////////////////////////////////
  95
  96 #define ODR_75_HZ 0x18
  97 #define ODR_15_HZ 0x10
  98
  99 #define FS_1P3_GA 0x20
 100 #define FS_1P9_GA 0x40
 101 #define FS_2P5_GA 0x60
 102 #define FS_4P0_GA 0x80
 103 #define FS_4P7_GA 0xA0
 104 #define FS_5P6_GA 0xC0
 105 #define FS_8P1_GA 0xE0
 106
 107 #define CONTINUOUS_CONVERSION 0x00
 108
 109 uint8_t accelCalibrating = false;
 110
 111 float accelOneG = 9.8065;
 112
 113 int32_t accelSum100Hz[3] = { 0, 0, 0 };
 114
 115 int32_t accelSum500Hz[3] = { 0, 0, 0 };
 116
 117 int32_t accelSummedSamples100Hz[3];
 118
 119 int32_t accelSummedSamples500Hz[3];
 120
 121 void lsm303dlhcAccInit(accDev_t *acc)
 122 {
 123     i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
 124
 125     delay(100);
 126
 127     i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
 128
 129     delay(10);
 130
 131     i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G);
 132
 133     delay(100);
 134
 135     acc->acc_1G = 512 * 8;
 136 }
 137
 138 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
 139 static bool lsm303dlhcAccRead(accDev_t *acc)
 140 {
 141     uint8_t buf[6];
 142
 143     bool ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf);
 144
 145     if (!ack) {
 146         return false;
 147     }
 148
 149     // the values range from -8192 to +8191
 150     acc->ADCRaw[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
 151     acc->ADCRaw[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
 152     acc->ADCRaw[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
 153
 154     return true;
 155 }
 156
 157 bool lsm303dlhcAccDetect(accDev_t *acc)
 158 {
 159     bool ack;
 160     uint8_t status;
 161
 162     ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_REG_A, 1, &status);
 163     if (!ack) {
 164         return false;
 165     }
 166
 167     acc->initFn = lsm303dlhcAccInit;
 168     acc->readFn = lsm303dlhcAccRead;
 169     return true;
 170 }
 171 #endif