src/main/drivers/compass/compass_qmc5883l.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 #ifdef USE_MAG_QMC5883
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/bus.h"
  35 #include "drivers/bus_i2c.h"
  36 #include "drivers/bus_i2c_busdev.h"
  37 #include "drivers/sensor.h"
  38 #include "drivers/time.h"
  39
  40 #include "compass.h"
  41 #include "compass_qmc5883l.h"
  42
  43 #define QMC5883L_MAG_I2C_ADDRESS 0x0D
  44
  45 // Registers
  46 #define QMC5883L_REG_CONF1 0x09
  47 #define QMC5883L_REG_CONF2 0x0A
  48
  49 // data output rates for 5883L
  50 #define QMC5883L_ODR_10HZ  (0x00 << 2)
  51 #define QMC5883L_ODR_50HZ  (0x01 << 2)
  52 #define QMC5883L_ODR_100HZ (0x02 << 2)
  53 #define QMC5883L_ODR_200HZ (0x03 << 2)
  54
  55 // Sensor operation modes
  56 #define QMC5883L_MODE_STANDBY    0x00
  57 #define QMC5883L_MODE_CONTINUOUS 0x01
  58
  59 #define QMC5883L_RNG_2G (0x00 << 4)
  60 #define QMC5883L_RNG_8G (0x01 << 4)
  61
  62 #define QMC5883L_OSR_512 (0x00 << 6)
  63 #define QMC5883L_OSR_256 (0x01 << 6)
  64 #define QMC5883L_OSR_128 (0x10 << 6)
  65 #define QMC5883L_OSR_64  (0x11 << 6)
  66
  67 #define QMC5883L_RST 0x80
  68
  69 #define QMC5883L_REG_DATA_OUTPUT_X 0x00
  70 #define QMC5883L_REG_STATUS 0x06
  71
  72 #define QMC5883L_REG_ID 0x0D
  73 #define QMC5883_ID_VAL 0xFF
  74
  75 static bool qmc5883lInit(magDev_t *magDev)
  76 {
  77     bool ack = true;
  78     extDevice_t *dev = &magDev->dev;
  79
  80     busDeviceRegister(dev);
  81
  82     ack = ack && busWriteRegister(dev, 0x0B, 0x01);
  83     ack = ack && busWriteRegister(dev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
  84
  85     if (!ack) {
  86         return false;
  87     }
  88
  89     return true;
  90 }
  91
  92 static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
  93 {
  94     uint8_t status;
  95     uint8_t buf[6];
  96
  97     // set magData to zero for case of failed read
  98     magData[X] = 0;
  99     magData[Y] = 0;
 100     magData[Z] = 0;
 101
 102     extDevice_t *dev = &magDev->dev;
 103
 104     bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_STATUS, &status, 1);
 105
 106     if (!ack || (status & 0x04) == 0) {
 107         return false;
 108     }
 109
 110     ack = busReadRegisterBuffer(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
 111     if (!ack) {
 112         return false;
 113     }
 114
 115     magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
 116     magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
 117     magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);
 118
 119     return true;
 120 }
 121
 122 bool qmc5883lDetect(magDev_t *magDev)
 123 {
 124
 125     extDevice_t *dev = &magDev->dev;
 126
 127     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 128         dev->busType_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
 129     }
 130
 131     // Must write reset first  - don't care about the result
 132     busWriteRegister(dev, QMC5883L_REG_CONF2, QMC5883L_RST);
 133     delay(20);
 134
 135     uint8_t sig = 0;
 136     bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_ID, &sig, 1);
 137     if (ack && sig == QMC5883_ID_VAL) {
 138         // Should be in standby mode after soft reset and sensor is really present
 139         // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present
 140         ack = busReadRegisterBuffer(dev, QMC5883L_REG_CONF1, &sig, 1);
 141         if (ack && sig != QMC5883L_MODE_STANDBY) {
 142             return false;
 143         }
 144         magDev->init = qmc5883lInit;
 145         magDev->read = qmc5883lRead;
 146         return true;
 147     }
 148
 149     return false;
 150 }
 151 #endif