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