src/main/drivers/compass/compass_hmc5883l.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 #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
  29
  30 #include "build/debug.h"
  31
  32 #include "common/axis.h"
  33 #include "common/maths.h"
  34
  35 #include "drivers/bus.h"
  36 #include "drivers/bus_i2c.h"
  37 #include "drivers/bus_i2c_busdev.h"
  38 #include "drivers/bus_spi.h"
  39 #include "drivers/exti.h"
  40 #include "drivers/io.h"
  41 #include "drivers/light_led.h"
  42 #include "drivers/nvic.h"
  43 #include "drivers/sensor.h"
  44 #include "drivers/time.h"
  45
  46 #include "compass.h"
  47
  48 #include "compass_hmc5883l.h"
  49
  50 //#define DEBUG_MAG_DATA_READY_INTERRUPT
  51
  52 // 10 MHz max SPI frequency
  53 #define HMC5883_MAX_SPI_CLK_HZ 10000000
  54
  55 // HMC5883L, default address 0x1E
  56 // NAZE Target connections
  57 // PB12 connected to MAG_DRDY on rev4 hardware
  58 // PC14 connected to MAG_DRDY on rev5 hardware
  59
  60 /* CTRL_REGA: Control Register A
  61  * Read Write
  62  * Default value: 0x10
  63  * 7:5  0   These bits must be cleared for correct operation.
  64  * 4:2 DO2-DO0: Data Output Rate Bits
  65  *             DO2 |  DO1 |  DO0 |   Minimum Data Output Rate (Hz)
  66  *            ------------------------------------------------------
  67  *              0  |  0   |  0   |            0.75
  68  *              0  |  0   |  1   |            1.5
  69  *              0  |  1   |  0   |            3
  70  *              0  |  1   |  1   |            7.5
  71  *              1  |  0   |  0   |           15 (default)
  72  *              1  |  0   |  1   |           30
  73  *              1  |  1   |  0   |           75
  74  *              1  |  1   |  1   |           Not Used
  75  * 1:0 MS1-MS0: Measurement Configuration Bits
  76  *             MS1 | MS0 |   MODE
  77  *            ------------------------------
  78  *              0  |  0  |  Normal
  79  *              0  |  1  |  Positive Bias
  80  *              1  |  0  |  Negative Bias
  81  *              1  |  1  |  Not Used
  82  *
  83  * CTRL_REGB: Control RegisterB
  84  * Read Write
  85  * Default value: 0x20
  86  * 7:5 GN2-GN0: Gain Configuration Bits.
  87  *             GN2 |  GN1 |  GN0 |   Mag Input   | Gain       | Output Range
  88  *                 |      |      |  Range[Ga]    | [LSB/mGa]  |
  89  *            ------------------------------------------------------
  90  *              0  |  0   |  0   |  �0.88Ga      |   1370     | 0xF800?0x07FF (-2048:2047)
  91  *              0  |  0   |  1   |  �1.3Ga (def) |   1090     | 0xF800?0x07FF (-2048:2047)
  92  *              0  |  1   |  0   |  �1.9Ga       |   820      | 0xF800?0x07FF (-2048:2047)
  93  *              0  |  1   |  1   |  �2.5Ga       |   660      | 0xF800?0x07FF (-2048:2047)
  94  *              1  |  0   |  0   |  �4.0Ga       |   440      | 0xF800?0x07FF (-2048:2047)
  95  *              1  |  0   |  1   |  �4.7Ga       |   390      | 0xF800?0x07FF (-2048:2047)
  96  *              1  |  1   |  0   |  �5.6Ga       |   330      | 0xF800?0x07FF (-2048:2047)
  97  *              1  |  1   |  1   |  �8.1Ga       |   230      | 0xF800?0x07FF (-2048:2047)
  98  *                               |Not recommended|
  99  *
 100  * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
 101  *
 102  * _MODE_REG: Mode Register
 103  * Read Write
 104  * Default value: 0x02
 105  * 7:2  0   These bits must be cleared for correct operation.
 106  * 1:0 MD1-MD0: Mode Select Bits
 107  *             MS1 | MS0 |   MODE
 108  *            ------------------------------
 109  *              0  |  0  |  Continuous-Conversion Mode.
 110  *              0  |  1  |  Single-Conversion Mode
 111  *              1  |  0  |  Negative Bias
 112  *              1  |  1  |  Sleep Mode
 113  */
 114
 115 #define HMC5883_MAG_I2C_ADDRESS     0x1E
 116 #define HMC5883_DEVICE_ID           0x48
 117
 118 #define HMC58X3_REG_CONFA           0x00
 119 #define HMC58X3_REG_CONFB           0x01
 120 #define HMC58X3_REG_MODE            0x02
 121 #define HMC58X3_REG_DATA            0x03
 122 #define HMC58X3_REG_IDA             0x0A
 123
 124 #define HMC_CONFA_NORMAL            0x00
 125 #define HMC_CONFA_POS_BIAS          0x01
 126 #define HMC_CONFA_NEG_BIAS          0x02
 127 #define HMC_CONFA_DOR_15HZ          0X10
 128 #define HMC_CONFA_8_SAMLES          0X60
 129 #define HMC_CONFB_GAIN_2_5GA        0X60
 130 #define HMC_CONFB_GAIN_1_3GA        0X20
 131 #define HMC_MODE_CONTINOUS          0X00
 132 #define HMC_MODE_SINGLE             0X01
 133
 134 #define HMC58X3_X_SELF_TEST_GAUSS   (+1.16f)            // X axis level when bias current is applied.
 135 #define HMC58X3_Y_SELF_TEST_GAUSS   (+1.16f)            // Y axis level when bias current is applied.
 136 #define HMC58X3_Z_SELF_TEST_GAUSS   (+1.08f)            // Z axis level when bias current is applied.
 137 #define SELF_TEST_LOW_LIMIT         (243.0f / 390.0f)   // Low limit when gain is 5.
 138 #define SELF_TEST_HIGH_LIMIT        (575.0f / 390.0f)   // High limit when gain is 5.
 139
 140 #ifdef USE_MAG_DATA_READY_SIGNAL
 141
 142 static void hmc5883_extiHandler(extiCallbackRec_t* cb)
 143 {
 144     UNUSED(cb);
 145 #ifdef DEBUG_MAG_DATA_READY_INTERRUPT
 146     // Measure the delta between calls to the interrupt handler
 147     // currently should be around 65/66 milli seconds / 15hz output rate
 148     static uint32_t lastCalledAt = 0;
 149     static int32_t callDelta = 0;
 150
 151     uint32_t now = millis();
 152     callDelta = now - lastCalledAt;
 153
 154     //UNUSED(callDelta);
 155     debug[0] = callDelta;
 156
 157     lastCalledAt = now;
 158 #endif
 159 }
 160 #endif
 161
 162 static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
 163 {
 164 #ifdef USE_MAG_DATA_READY_SIGNAL
 165     if (mag->magIntExtiTag == IO_TAG_NONE) {
 166         return;
 167     }
 168
 169     const IO_t magIntIO = IOGetByTag(mag->magIntExtiTag);
 170
 171 #ifdef ENSURE_MAG_DATA_READY_IS_HIGH
 172     uint8_t status = IORead(magIntIO);
 173     if (!status) {
 174         return;
 175     }
 176 #endif
 177
 178     IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
 179     EXTIHandlerInit(&mag->exti, hmc5883_extiHandler);
 180     EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
 181     EXTIEnable(magIntIO, true);
 182     EXTIEnable(magIntIO, true);
 183 #else
 184     UNUSED(mag);
 185 #endif
 186 }
 187
 188 #ifdef USE_MAG_SPI_HMC5883
 189 static void hmc5883SpiInit(const extDevice_t *dev)
 190 {
 191     busDeviceRegister(dev);
 192
 193     IOHi(dev->busType_u.spi.csnPin); // Disable
 194
 195     IOInit(dev->busType_u.spi.csnPin, OWNER_COMPASS_CS, 0);
 196     IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
 197     spiSetClkDivisor(dev, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ));
 198 }
 199 #endif
 200
 201 static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
 202 {
 203     uint8_t buf[6];
 204
 205     extDevice_t *dev = &mag->dev;
 206
 207     bool ack = busReadRegisterBuffer(dev, HMC58X3_REG_DATA, buf, 6);
 208
 209     if (!ack) {
 210         return false;
 211     }
 212
 213     magData[X] = (int16_t)(buf[0] << 8 | buf[1]);
 214     magData[Z] = (int16_t)(buf[2] << 8 | buf[3]);
 215     magData[Y] = (int16_t)(buf[4] << 8 | buf[5]);
 216
 217     return true;
 218 }
 219
 220 static bool hmc5883lInit(magDev_t *mag)
 221 {
 222
 223     extDevice_t *dev = &mag->dev;
 224
 225     // leave test mode
 226     busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL);    // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
 227     busWriteRegister(dev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA);                                          // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
 228     busWriteRegister(dev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS);                                             // Mode register             -- 000000 00    continuous Conversion Mode
 229
 230     delay(100);
 231
 232     hmc5883lConfigureDataReadyInterruptHandling(mag);
 233     return true;
 234 }
 235
 236 bool hmc5883lDetect(magDev_t* mag)
 237 {
 238     extDevice_t *dev = &mag->dev;
 239
 240     uint8_t sig = 0;
 241
 242 #ifdef USE_MAG_SPI_HMC5883
 243     if (dev->bus->busType == BUS_TYPE_SPI) {
 244         hmc5883SpiInit(dev);
 245     }
 246 #endif
 247
 248 #ifdef USE_MAG_HMC5883
 249     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 250         dev->busType_u.i2c.address = HMC5883_MAG_I2C_ADDRESS;
 251     }
 252 #endif
 253
 254     bool ack = busReadRegisterBuffer(&mag->dev, HMC58X3_REG_IDA, &sig, 1);
 255
 256     if (!ack || sig != HMC5883_DEVICE_ID) {
 257         return false;
 258     }
 259
 260     mag->init = hmc5883lInit;
 261     mag->read = hmc5883lRead;
 262
 263     return true;
 264 }
 265 #endif