src/main/drivers/compass/compass_lis3mdl.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 <math.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_MAG_LIS3MDL
  26
  27 #include "common/axis.h"
  28
  29 #include "drivers/time.h"
  30 #include "drivers/sensor.h"
  31
  32 #include "drivers/compass/compass.h"
  33
  34 #include "compass_lis3mdl.h"
  35
  36 #define LIS3MDL_MAG_I2C_ADDRESS     0x1E
  37 #define LIS3MDL_MAG_I2C_ADDRESS2    0x1C // LIS3MDL has two possible addresses, selected by a physical pin
  38 #define LIS3MDL_DEVICE_ID           0x3D
  39
  40 #define LIS3MDL_REG_WHO_AM_I        0x0F
  41
  42 #define LIS3MDL_REG_CTRL_REG1       0x20
  43 #define LIS3MDL_REG_CTRL_REG2       0x21
  44 #define LIS3MDL_REG_CTRL_REG3       0x22
  45 #define LIS3MDL_REG_CTRL_REG4       0x23
  46 #define LIS3MDL_REG_CTRL_REG5       0x24
  47
  48 #define LIS3MDL_REG_STATUS_REG      0x27
  49
  50 #define LIS3MDL_REG_OUT_X_L         0x28
  51 #define LIS3MDL_REG_OUT_X_H         0x29
  52 #define LIS3MDL_REG_OUT_Y_L         0x2A
  53 #define LIS3MDL_REG_OUT_Y_H         0x2B
  54 #define LIS3MDL_REG_OUT_Z_L         0x2C
  55 #define LIS3MDL_REG_OUT_Z_H         0x2D
  56
  57 #define LIS3MDL_TEMP_OUT_L          0x2E
  58 #define LIS3MDL_TEMP_OUT_H          0x2F
  59
  60 #define LIS3MDL_INT_CFG             0x30
  61 #define LIS3MDL_INT_SRC             0x31
  62 #define LIS3MDL_THS_L               0x32
  63 #define LIS3MDL_THS_H               0x33
  64
  65 // CTRL_REG1
  66 #define LIS3MDL_TEMP_EN             0x80  // Default 0
  67 #define LIS3MDL_OM_LOW_POWER        0x00  // Default
  68 #define LIS3MDL_OM_MED_PROF         0x20
  69 #define LIS3MDL_OM_HI_PROF          0x40
  70 #define LIS3MDL_OM_ULTRA_HI_PROF    0x60
  71 #define LIS3MDL_DO_0_625            0x00
  72 #define LIS3MDL_DO_1_25             0x04
  73 #define LIS3MDL_DO_2_5              0x08
  74 #define LIS3MDL_DO_5                0x0C
  75 #define LIS3MDL_DO_10               0x10  // Default
  76 #define LIS3MDL_DO_20               0x14
  77 #define LIS3MDL_DO_40               0x18
  78 #define LIS3MDL_DO_80               0x1C
  79 #define LIS3MDL_FAST_ODR            0x02
  80
  81 // CTRL_REG2
  82 #define LIS3MDL_FS_4GAUSS           0x00  // Default
  83 #define LIS3MDL_FS_8GAUSS           0x20
  84 #define LIS3MDL_FS_12GAUSS          0x40
  85 #define LIS3MDL_FS_16GAUSS          0x60
  86 #define LIS3MDL_REBOOT              0x08
  87 #define LIS3MDL_SOFT_RST            0x04
  88
  89 // CTRL_REG3
  90 #define LIS3MDL_LP                  0x20  // Default 0
  91 #define LIS3MDL_SIM                 0x04  // Default 0
  92 #define LIS3MDL_MD_CONTINUOUS       0x00  // Default
  93 #define LIS3MDL_MD_SINGLE           0x01
  94 #define LIS3MDL_MD_POWERDOWN        0x03
  95
  96 // CTRL_REG4
  97 #define LIS3MDL_ZOM_LP              0x00  // Default
  98 #define LIS3MDL_ZOM_MP              0x04
  99 #define LIS3MDL_ZOM_HP              0x08
 100 #define LIS3MDL_ZOM_UHP             0x0C
 101 #define LIS3MDL_BLE                 0x02  // Default 0
 102
 103 // CTRL_REG5
 104 #define LIS3MDL_FAST_READ           0x80  // Default 0
 105 #define LIS3MDL_BDU                 0x40  // Default 0
 106
 107 static bool lis3mdlRead(magDev_t * mag)
 108 {
 109     bool ack;
 110         uint8_t status = 0;
 111
 112         ack = busRead(mag->busDev, LIS3MDL_REG_STATUS_REG, &status);
 113
 114         if ( !( ack && ( ( status & 0x08 ) >> 3 ) ) ) {
 115                 return false;
 116         }
 117
 118         uint8_t buf[6];
 119
 120     ack = busReadBuf(mag->busDev, LIS3MDL_REG_OUT_X_L, buf, 6);
 121
 122     if (!ack) {
 123         mag->magADCRaw[X] = 0;
 124         mag->magADCRaw[Y] = 0;
 125         mag->magADCRaw[Z] = 0;
 126         return false;
 127     }
 128
 129     mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) / 4;
 130     mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) / 4;
 131     mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) / 4;
 132
 133     return true;
 134 }
 135
 136 #define DETECTION_MAX_RETRY_COUNT   5
 137 static bool deviceDetect(magDev_t * mag)
 138 {
 139     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 140         delay(10);
 141
 142         uint8_t sig = 0;
 143         bool ack = busRead(mag->busDev, LIS3MDL_REG_WHO_AM_I, &sig);
 144
 145         if (ack && sig == LIS3MDL_DEVICE_ID) {
 146             return true;
 147         }
 148     }
 149
 150     return false;
 151 }
 152
 153 static bool lis3mdlInit(magDev_t *mag)
 154 {
 155         busWrite(mag->busDev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS);   // Configuration Register 2
 156     busWrite(mag->busDev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80);   // Configuration Register 1
 157     busWrite(mag->busDev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU);   // Configuration Register 5
 158     busWrite(mag->busDev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP);   // Configuration Register 4
 159     busWrite(mag->busDev, LIS3MDL_REG_CTRL_REG3, 0x00);   // Configuration Register 3
 160
 161     return true;
 162 }
 163
 164 bool lis3mdlDetect(magDev_t * mag)
 165 {
 166     mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LIS3MDL, mag->magSensorToUse, OWNER_COMPASS);
 167     if (mag->busDev == NULL) {
 168         return false;
 169     }
 170
 171 // TODO: Add check for second possible I2C address
 172
 173     if (!deviceDetect(mag)) {
 174         busDeviceDeInit(mag->busDev);
 175         return false;
 176     }
 177
 178     mag->init = lis3mdlInit;
 179     mag->read = lis3mdlRead;
 180
 181     return true;
 182 }
 183
 184 #endif