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