src/main/drivers/compass/compass_lis3mdl.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 <math.h>
  25
  26 #include "platform.h"
  27
  28 #if defined(USE_MAG_LIS3MDL)
  29
  30 #include "compass.h"
  31 #include "drivers/time.h"
  32 #include "common/axis.h"
  33
  34 #define LIS3MDL_MAG_I2C_ADDRESS     0x1E
  35 #define LIS3MDL_DEVICE_ID           0x3D
  36
  37 #define LIS3MDL_REG_WHO_AM_I        0x0F
  38
  39 #define LIS3MDL_REG_CTRL_REG1       0x20
  40 #define LIS3MDL_REG_CTRL_REG2       0x21
  41 #define LIS3MDL_REG_CTRL_REG3       0x22
  42 #define LIS3MDL_REG_CTRL_REG4       0x23
  43 #define LIS3MDL_REG_CTRL_REG5       0x24
  44
  45 #define LIS3MDL_REG_STATUS_REG      0x27
  46
  47 #define LIS3MDL_REG_OUT_X_L         0x28
  48 #define LIS3MDL_REG_OUT_X_H         0x29
  49 #define LIS3MDL_REG_OUT_Y_L         0x2A
  50 #define LIS3MDL_REG_OUT_Y_H         0x2B
  51 #define LIS3MDL_REG_OUT_Z_L         0x2C
  52 #define LIS3MDL_REG_OUT_Z_H         0x2D
  53
  54 #define LIS3MDL_TEMP_OUT_L          0x2E
  55 #define LIS3MDL_TEMP_OUT_H          0x2F
  56
  57 #define LIS3MDL_INT_CFG             0x30
  58 #define LIS3MDL_INT_SRC             0x31
  59 #define LIS3MDL_THS_L               0x32
  60 #define LIS3MDL_THS_H               0x33
  61
  62 // CTRL_REG1
  63 #define LIS3MDL_TEMP_EN             0x80  // Default 0
  64 #define LIS3MDL_OM_LOW_POWER        0x00  // Default
  65 #define LIS3MDL_OM_MED_PROF         0x20
  66 #define LIS3MDL_OM_HI_PROF          0x40
  67 #define LIS3MDL_OM_ULTRA_HI_PROF    0x60
  68 #define LIS3MDL_DO_0_625            0x00
  69 #define LIS3MDL_DO_1_25             0x04
  70 #define LIS3MDL_DO_2_5              0x08
  71 #define LIS3MDL_DO_5                0x0C
  72 #define LIS3MDL_DO_10               0x10  // Default
  73 #define LIS3MDL_DO_20               0x14
  74 #define LIS3MDL_DO_40               0x18
  75 #define LIS3MDL_DO_80               0x1C
  76 #define LIS3MDL_FAST_ODR            0x02
  77
  78 // CTRL_REG2
  79 #define LIS3MDL_FS_4GAUSS           0x00  // Default
  80 #define LIS3MDL_FS_8GAUSS           0x20
  81 #define LIS3MDL_FS_12GAUSS          0x40
  82 #define LIS3MDL_FS_16GAUSS          0x60
  83 #define LIS3MDL_REBOOT              0x08
  84 #define LIS3MDL_SOFT_RST            0x04
  85
  86 // CTRL_REG3
  87 #define LIS3MDL_LP                  0x20  // Default 0
  88 #define LIS3MDL_SIM                 0x04  // Default 0
  89 #define LIS3MDL_MD_CONTINUOUS       0x00  // Default
  90 #define LIS3MDL_MD_SINGLE           0x01
  91 #define LIS3MDL_MD_POWERDOWN        0x03
  92
  93 // CTRL_REG4
  94 #define LIS3MDL_ZOM_LP              0x00  // Default
  95 #define LIS3MDL_ZOM_MP              0x04
  96 #define LIS3MDL_ZOM_HP              0x08
  97 #define LIS3MDL_ZOM_UHP             0x0C
  98 #define LIS3MDL_BLE                 0x02  // Default 0
  99
 100 // CTRL_REG5
 101 #define LIS3MDL_FAST_READ           0x80  // Default 0
 102 #define LIS3MDL_BDU                 0x40  // Default 0
 103
 104 static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
 105 {
 106     uint8_t buf[6];
 107
 108     extDevice_t *dev = &mag->dev;
 109
 110     bool ack = busReadRegisterBuffer(dev, LIS3MDL_REG_OUT_X_L, buf, 6);
 111
 112     if (!ack) {
 113         return false;
 114     }
 115
 116     magData[X] = (int16_t)(buf[1] << 8 | buf[0]) / 4;
 117     magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) / 4;
 118     magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) / 4;
 119
 120     return true;
 121 }
 122
 123 static bool lis3mdlInit(magDev_t *mag)
 124 {
 125     extDevice_t *dev = &mag->dev;
 126
 127     busDeviceRegister(dev);
 128
 129     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS);
 130     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80);
 131     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU);
 132     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP);
 133     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG3, 0x00);
 134
 135     delay(100);
 136
 137     return true;
 138 }
 139
 140 bool lis3mdlDetect(magDev_t * mag)
 141 {
 142     extDevice_t *dev = &mag->dev;
 143
 144     uint8_t sig = 0;
 145
 146     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 147         dev->busType_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS;
 148     }
 149
 150     bool ack = busReadRegisterBuffer(&mag->dev, LIS3MDL_REG_WHO_AM_I, &sig, 1);
 151
 152     if (!ack || sig != LIS3MDL_DEVICE_ID) {
 153         return false;
 154     }
 155
 156     mag->init = lis3mdlInit;
 157     mag->read = lis3mdlRead;
 158
 159     return true;
 160 }
 161 #endif