src/main/drivers/accgyro_legacy/accgyro_adxl345.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_ADXL345
  27
  28 #include "drivers/bus_i2c.h"
  29
  30 #include "drivers/sensor.h"
  31 #include "drivers/accgyro/accgyro.h"
  32 #include "accgyro_adxl345.h"
  33
  34 // ADXL345, Alternative address mode 0x53
  35 #define ADXL345_ADDRESS     0x53
  36
  37 // Registers
  38 #define ADXL345_BW_RATE     0x2C
  39 #define ADXL345_POWER_CTL   0x2D
  40 #define ADXL345_INT_ENABLE  0x2E
  41 #define ADXL345_DATA_FORMAT 0x31
  42 #define ADXL345_DATA_OUT    0x32
  43 #define ADXL345_FIFO_CTL    0x38
  44
  45 // BW_RATE values
  46 #define ADXL345_RATE_50     0x09
  47 #define ADXL345_RATE_100    0x0A
  48 #define ADXL345_RATE_200    0x0B
  49 #define ADXL345_RATE_400    0x0C
  50 #define ADXL345_RATE_800    0x0D
  51 #define ADXL345_RATE_1600   0x0E
  52 #define ADXL345_RATE_3200   0x0F
  53
  54 // various register values
  55 #define ADXL345_POWER_MEAS  0x08
  56 #define ADXL345_FULL_RANGE  0x08
  57 #define ADXL345_RANGE_2G    0x00
  58 #define ADXL345_RANGE_4G    0x01
  59 #define ADXL345_RANGE_8G    0x02
  60 #define ADXL345_RANGE_16G   0x03
  61 #define ADXL345_FIFO_STREAM 0x80
  62
  63 static bool useFifo = false;
  64
  65 static void adxl345Init(accDev_t *acc)
  66 {
  67     if (useFifo) {
  68         uint8_t fifoDepth = 16;
  69         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
  70         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
  71         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_400);
  72         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_FIFO_CTL, (fifoDepth & 0x1F) | ADXL345_FIFO_STREAM);
  73     } else {
  74         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
  75         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
  76         i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100);
  77     }
  78     acc->acc_1G = 256; // 3.3V operation
  79 }
  80
  81 uint8_t acc_samples = 0;
  82
  83 static bool adxl345Read(accDev_t *acc)
  84 {
  85     uint8_t buf[8];
  86
  87     if (useFifo) {
  88         int32_t x = 0;
  89         int32_t y = 0;
  90         int32_t z = 0;
  91         uint8_t i = 0;
  92         uint8_t samples_remaining;
  93
  94         do {
  95             i++;
  96
  97             if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
  98                 return false;
  99             }
 100
 101             x += (int16_t)(buf[0] + (buf[1] << 8));
 102             y += (int16_t)(buf[2] + (buf[3] << 8));
 103             z += (int16_t)(buf[4] + (buf[5] << 8));
 104             samples_remaining = buf[7] & 0x7F;
 105         } while ((i < 32) && (samples_remaining > 0));
 106         acc->ADCRaw[0] = x / i;
 107         acc->ADCRaw[1] = y / i;
 108         acc->ADCRaw[2] = z / i;
 109         acc_samples = i;
 110     } else {
 111
 112         if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) {
 113             return false;
 114         }
 115
 116         acc->ADCRaw[0] = buf[0] + (buf[1] << 8);
 117         acc->ADCRaw[1] = buf[2] + (buf[3] << 8);
 118         acc->ADCRaw[2] = buf[4] + (buf[5] << 8);
 119     }
 120
 121     return true;
 122 }
 123
 124 bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc)
 125 {
 126     uint8_t sig = 0;
 127     bool ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig);
 128
 129     if (!ack || sig != 0xE5)
 130         return false;
 131
 132     // use ADXL345's fifo to filter data or not
 133     useFifo = init->useFifo;
 134
 135     acc->initFn = adxl345Init;
 136     acc->readFn = adxl345Read;
 137     return true;
 138 }
 139 #endif