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     static uint8_t buf[6];
 107     static bool pendingRead = true;
 108
 109     extDevice_t *dev = &mag->dev;
 110
 111     if (pendingRead) {
 112         if (busReadRegisterBufferStart(dev, LIS3MDL_REG_OUT_X_L, buf, sizeof(buf))) {
 113             pendingRead = false;
 114         }
 115         return false;
 116     }
 117
 118     magData[X] = (int16_t)(buf[1] << 8 | buf[0]) / 4;
 119     magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) / 4;
 120     magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) / 4;
 121
 122     pendingRead = true;
 123
 124     return true;
 125 }
 126
 127 static bool lis3mdlInit(magDev_t *mag)
 128 {
 129     extDevice_t *dev = &mag->dev;
 130
 131     busDeviceRegister(dev);
 132
 133     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS);
 134     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80);
 135     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU);
 136     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP);
 137     busWriteRegister(dev, LIS3MDL_REG_CTRL_REG3, 0x00);
 138
 139     delay(100);
 140     mag->magOdrHz = 80; // LIS3MDL_DO_80
 141     return true;
 142 }
 143
 144 bool lis3mdlDetect(magDev_t * mag)
 145 {
 146     extDevice_t *dev = &mag->dev;
 147
 148     uint8_t sig = 0;
 149
 150     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 151         dev->busType_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS;
 152     }
 153
 154     bool ack = busReadRegisterBuffer(&mag->dev, LIS3MDL_REG_WHO_AM_I, &sig, 1);
 155
 156     if (!ack || sig != LIS3MDL_DEVICE_ID) {
 157         return false;
 158     }
 159
 160     mag->init = lis3mdlInit;
 161     mag->read = lis3mdlRead;
 162
 163     return true;
 164 }
 165 #endif