src/main/drivers/compass/compass_qmc5883l.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include <math.h>
  29
  30 #include "platform.h"
  31
  32 #ifdef USE_MAG_QMC5883
  33
  34 #include "build/build_config.h"
  35
  36 #include "common/axis.h"
  37 #include "common/maths.h"
  38 #include "common/utils.h"
  39
  40 #include "drivers/time.h"
  41 #include "drivers/bus_i2c.h"
  42
  43 #include "sensors/boardalignment.h"
  44 #include "sensors/sensors.h"
  45
  46 #include "drivers/sensor.h"
  47 #include "drivers/compass/compass.h"
  48
  49 #include "drivers/compass/compass_qmc5883l.h"
  50
  51 #define QMC5883L_MAG_I2C_ADDRESS     0x0D
  52
  53 // Registers
  54 #define QMC5883L_REG_CONF1 0x09
  55 #define QMC5883L_REG_CONF2 0x0A
  56
  57 // data output rates for 5883L
  58 #define QMC5883L_ODR_10HZ (0x00 << 2)
  59 #define QMC5883L_ODR_50HZ  (0x01 << 2)
  60 #define QMC5883L_ODR_100HZ (0x02 << 2)
  61 #define QMC5883L_ODR_200HZ (0x03 << 2)
  62
  63 // Sensor operation modes
  64 #define QMC5883L_MODE_STANDBY 0x00
  65 #define QMC5883L_MODE_CONTINUOUS 0x01
  66
  67 #define QMC5883L_RNG_2G (0x00 << 4)
  68 #define QMC5883L_RNG_8G (0x01 << 4)
  69
  70 #define QMC5883L_OSR_512 (0x00 << 6)
  71 #define QMC5883L_OSR_256 (0x01 << 6)
  72 #define QMC5883L_OSR_128        (0x10 << 6)
  73 #define QMC5883L_OSR_64 (0x11   << 6)
  74
  75 #define QMC5883L_RST 0x80
  76
  77 #define QMC5883L_REG_DATA_OUTPUT_X 0x00
  78 #define QMC5883L_REG_STATUS 0x06
  79
  80 #define QMC5883L_REG_ID 0x0D
  81 #define QMC5883_ID_VAL 0xFF
  82
  83 static bool qmc5883Init(magDev_t * mag)
  84 {
  85     bool ack = true;
  86
  87     ack = ack && busWrite(mag->busDev, 0x0B, 0x01);
  88     // ack = ack && i2cWrite(busWrite(mag->busDev, 0x20, 0x40);
  89     // ack = ack && i2cWrite(busWrite(mag->busDev, 0x21, 0x01);
  90     ack = ack && busWrite(mag->busDev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
  91
  92     return ack;
  93 }
  94
  95 static bool qmc5883Read(magDev_t * mag)
  96 {
  97     uint8_t status;
  98     uint8_t buf[6];
  99
 100     // set magData to zero for case of failed read
 101     mag->magADCRaw[X] = 0;
 102     mag->magADCRaw[Y] = 0;
 103     mag->magADCRaw[Z] = 0;
 104
 105     bool ack = busRead(mag->busDev, QMC5883L_REG_STATUS, &status);
 106     if (!ack || (status & 0x04) == 0) {
 107         return false;
 108     }
 109
 110     ack = busReadBuf(mag->busDev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
 111     if (!ack) {
 112         return false;
 113     }
 114
 115     mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]);
 116     mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]);
 117     mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]);
 118
 119     return true;
 120 }
 121
 122 #define DETECTION_MAX_RETRY_COUNT   5
 123 static bool deviceDetect(magDev_t * mag)
 124 {
 125     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 126         // Must write reset first  - don't care about the result
 127         busWrite(mag->busDev, QMC5883L_REG_CONF2, QMC5883L_RST);
 128         delay(30);
 129
 130         uint8_t sig = 0;
 131         bool ack = busRead(mag->busDev, QMC5883L_REG_ID, &sig);
 132
 133         if (ack && sig == QMC5883_ID_VAL) {
 134             // Should be in standby mode after soft reset and sensor is really present
 135             // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present
 136
 137             ack = busRead(mag->busDev, QMC5883L_REG_CONF1, &sig);
 138             if (ack && sig == QMC5883L_MODE_STANDBY) {
 139                 return true;
 140             }
 141         }
 142     }
 143
 144     return false;
 145 }
 146
 147 bool qmc5883Detect(magDev_t * mag)
 148 {
 149     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_QMC5883, mag->magSensorToUse, OWNER_COMPASS);
 150     if (mag->busDev == NULL) {
 151         return false;
 152     }
 153
 154     if (!deviceDetect(mag)) {
 155         busDeviceDeInit(mag->busDev);
 156         return false;
 157     }
 158
 159     mag->init = qmc5883Init;
 160     mag->read = qmc5883Read;
 161
 162     return true;
 163 }
 164 #endif