src/main/drivers/compass/compass_hmc5883l.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_HMC5883
  26
  27 #include "build/debug.h"
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31
  32 #include "drivers/time.h"
  33 #include "drivers/nvic.h"
  34 #include "drivers/io.h"
  35 #include "drivers/bus.h"
  36 #include "drivers/light_led.h"
  37
  38 #include "drivers/sensor.h"
  39 #include "drivers/compass/compass.h"
  40
  41 #include "drivers/compass/compass_hmc5883l.h"
  42
  43 // HMC5883L, default address 0x1E
  44
  45 /* CTRL_REGA: Control Register A
  46  * Read Write
  47  * Default value: 0x10
  48  * 7:5  0   These bits must be cleared for correct operation.
  49  * 4:2 DO2-DO0: Data Output Rate Bits
  50  *             DO2 |  DO1 |  DO0 |   Minimum Data Output Rate (Hz)
  51  *            ------------------------------------------------------
  52  *              0  |  0   |  0   |            0.75
  53  *              0  |  0   |  1   |            1.5
  54  *              0  |  1   |  0   |            3
  55  *              0  |  1   |  1   |            7.5
  56  *              1  |  0   |  0   |           15 (default)
  57  *              1  |  0   |  1   |           30
  58  *              1  |  1   |  0   |           75
  59  *              1  |  1   |  1   |           Not Used
  60  * 1:0 MS1-MS0: Measurement Configuration Bits
  61  *             MS1 | MS0 |   MODE
  62  *            ------------------------------
  63  *              0  |  0   |  Normal
  64  *              0  |  1   |  Positive Bias
  65  *              1  |  0   |  Negative Bias
  66  *              1  |  1   |  Not Used
  67  *
  68  * CTRL_REGB: Control RegisterB
  69  * Read Write
  70  * Default value: 0x20
  71  * 7:5 GN2-GN0: Gain Configuration Bits.
  72  *             GN2 |  GN1 |  GN0 |   Mag Input   | Gain       | Output Range
  73  *                 |      |      |  Range[Ga]    | [LSB/mGa]  |
  74  *            ------------------------------------------------------
  75  *              0  |  0   |  0   |  �0.88Ga      |   1370     | 0xF800?0x07FF (-2048:2047)
  76  *              0  |  0   |  1   |  �1.3Ga (def) |   1090     | 0xF800?0x07FF (-2048:2047)
  77  *              0  |  1   |  0   |  �1.9Ga       |   820      | 0xF800?0x07FF (-2048:2047)
  78  *              0  |  1   |  1   |  �2.5Ga       |   660      | 0xF800?0x07FF (-2048:2047)
  79  *              1  |  0   |  0   |  �4.0Ga       |   440      | 0xF800?0x07FF (-2048:2047)
  80  *              1  |  0   |  1   |  �4.7Ga       |   390      | 0xF800?0x07FF (-2048:2047)
  81  *              1  |  1   |  0   |  �5.6Ga       |   330      | 0xF800?0x07FF (-2048:2047)
  82  *              1  |  1   |  1   |  �8.1Ga       |   230      | 0xF800?0x07FF (-2048:2047)
  83  *                               |Not recommended|
  84  *
  85  * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
  86  *
  87  * _MODE_REG: Mode Register
  88  * Read Write
  89  * Default value: 0x02
  90  * 7:2  0   These bits must be cleared for correct operation.
  91  * 1:0 MD1-MD0: Mode Select Bits
  92  *             MS1 | MS0 |   MODE
  93  *            ------------------------------
  94  *              0  |  0   |  Continuous-Conversion Mode.
  95  *              0  |  1   |  Single-Conversion Mode
  96  *              1  |  0   |  Negative Bias
  97  *              1  |  1   |  Sleep Mode
  98  */
  99
 100 #define MAG_ADDRESS             0x1E
 101 #define MAG_DATA_REGISTER       0x03
 102 #define MAG_DATA_REGISTER_SPI   (0x03 | 0x40)
 103 #define HMC58X3_R_CONFA         0x00
 104 #define HMC58X3_R_CONFB         0x01
 105 #define HMC58X3_R_MODE          0x02
 106 #define HMC58X3_X_SELF_TEST_GAUSS (+1.16f)       // X axis level when bias current is applied.
 107 #define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f)       // Y axis level when bias current is applied.
 108 #define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f)       // Z axis level when bias current is applied.
 109 #define SELF_TEST_LOW_LIMIT  (243.0f / 390.0f)    // Low limit when gain is 5.
 110 #define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f)    // High limit when gain is 5.
 111 #define HMC_POS_BIAS 1
 112 #define HMC_NEG_BIAS 2
 113
 114 static bool hmc5883lRead(magDev_t * mag)
 115 {
 116     uint8_t buf[6];
 117     bool ack;
 118
 119     if (mag->busDev->busType == BUSTYPE_SPI) {
 120         ack = busReadBuf(mag->busDev, MAG_DATA_REGISTER_SPI, buf, 6);
 121     }
 122     else {
 123         ack = busReadBuf(mag->busDev, MAG_DATA_REGISTER, buf, 6);
 124     }
 125
 126     if (!ack) {
 127         mag->magADCRaw[X] = 0;
 128         mag->magADCRaw[Y] = 0;
 129         mag->magADCRaw[Z] = 0;
 130         return false;
 131     }
 132
 133     mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]);
 134     mag->magADCRaw[Z] = (int16_t)(buf[2] << 8 | buf[3]);
 135     mag->magADCRaw[Y] = (int16_t)(buf[4] << 8 | buf[5]);
 136
 137     return true;
 138 }
 139
 140 #define INITIALISATION_MAX_READ_FAILURES 5
 141 static bool hmc5883lInit(magDev_t * mag)
 142 {
 143     busWrite(mag->busDev, HMC58X3_R_CONFA, 0x78);   // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 75Hz ; normal measurement mode
 144     delay(5);
 145
 146     busWrite(mag->busDev, HMC58X3_R_CONFB, 0x20);   // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
 147     delay(5);
 148
 149     busWrite(mag->busDev, HMC58X3_R_MODE, 0x00);    // Mode register             -- 000000 00    continuous Conversion Mode
 150     delay(100);
 151
 152     return true;
 153 }
 154
 155 #define DETECTION_MAX_RETRY_COUNT   200
 156 static bool deviceDetect(magDev_t * mag)
 157 {
 158     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 159         delay(10);
 160
 161         uint8_t sig = 0;
 162         bool ack = busRead(mag->busDev, 0x0A, &sig);
 163
 164         if (ack && sig == 'H') {
 165             return true;
 166         }
 167     }
 168
 169     return false;
 170 }
 171
 172 bool hmc5883lDetect(magDev_t * mag)
 173 {
 174     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_HMC5883, mag->magSensorToUse, OWNER_COMPASS);
 175     if (mag->busDev == NULL) {
 176         return false;
 177     }
 178
 179     if (!deviceDetect(mag)) {
 180         busDeviceDeInit(mag->busDev);
 181         return false;
 182     }
 183
 184     mag->init = hmc5883lInit;
 185     mag->read = hmc5883lRead;
 186
 187     return true;
 188 }
 189 #endif