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     static uint8_t buf[6];
  95     static uint8_t status;
  96     static enum {
  97         STATE_READ_STATUS,
  98         STATE_WAIT_STATUS,
  99         STATE_WAIT_READ,
 100     } state = STATE_READ_STATUS;
 101
 102     extDevice_t *dev = &magDev->dev;
 103
 104     switch (state) {
 105         default:
 106         case STATE_READ_STATUS:
 107             busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
 108             state = STATE_WAIT_STATUS;
 109             return false;
 110
 111         case STATE_WAIT_STATUS:
 112             if ((status & 0x04) == 0) {
 113                 state = STATE_READ_STATUS;
 114                 return false;
 115             }
 116
 117             busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf));
 118             state = STATE_WAIT_READ;
 119             return false;
 120
 121         case STATE_WAIT_READ:
 122
 123             magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
 124             magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
 125             magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);
 126
 127             state = STATE_READ_STATUS;
 128
 129             return true;
 130     }
 131
 132     return false;
 133 }
 134
 135 bool qmc5883lDetect(magDev_t *magDev)
 136 {
 137
 138     extDevice_t *dev = &magDev->dev;
 139
 140     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 141         dev->busType_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
 142     }
 143
 144     // Must write reset first  - don't care about the result
 145     busWriteRegister(dev, QMC5883L_REG_CONF2, QMC5883L_RST);
 146     delay(20);
 147
 148     uint8_t sig = 0;
 149     bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_ID, &sig, 1);
 150     if (ack && sig == QMC5883_ID_VAL) {
 151         // Should be in standby mode after soft reset and sensor is really present
 152         // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present
 153         ack = busReadRegisterBuffer(dev, QMC5883L_REG_CONF1, &sig, 1);
 154         if (ack && sig != QMC5883L_MODE_STANDBY) {
 155             return false;
 156         }
 157         magDev->init = qmc5883lInit;
 158         magDev->read = qmc5883lRead;
 159         return true;
 160     }
 161
 162     return false;
 163 }
 164 #endif