src/main/drivers/compass/compass_mag3110.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_MAG3110
  26
  27 #include "build/build_config.h"
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/time.h"
  34 #include "drivers/bus.h"
  35
  36 #include "sensors/boardalignment.h"
  37 #include "sensors/sensors.h"
  38
  39 #include "drivers/sensor.h"
  40 #include "drivers/compass/compass.h"
  41
  42 #include "drivers/compass/compass_mag3110.h"
  43
  44
  45 // Registers
  46 #define MAG3110_MAG_I2C_ADDRESS     0x0E
  47 #define MAG3110_MAG_REG_STATUS       0x00
  48 #define MAG3110_MAG_REG_HXL          0x01
  49 #define MAG3110_MAG_REG_HXH          0x02
  50 #define MAG3110_MAG_REG_HYL          0x03
  51 #define MAG3110_MAG_REG_HYH          0x04
  52 #define MAG3110_MAG_REG_HZL          0x05
  53 #define MAG3110_MAG_REG_HZH          0x06
  54 #define MAG3110_MAG_REG_WHO_AM_I     0x07
  55 #define MAG3110_MAG_REG_SYSMODE      0x08
  56 #define MAG3110_MAG_REG_CTRL_REG1    0x10
  57 #define MAG3110_MAG_REG_CTRL_REG2    0x11
  58
  59 static bool mag3110Init(magDev_t * mag)
  60 {
  61     bool ack = busWrite(mag->busDev, MAG3110_MAG_REG_CTRL_REG1, 0x01); //  active mode 80 Hz ODR with OSR = 1
  62     if (!ack) {
  63         return false;
  64     }
  65
  66     delay(20);
  67
  68     ack = busWrite(mag->busDev, MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
  69     if (!ack) {
  70         return false;
  71     }
  72
  73     return true;
  74 }
  75
  76 #define BIT_STATUS_REG_DATA_READY               (1 << 3)
  77
  78 static bool mag3110Read(magDev_t * mag)
  79 {
  80     uint8_t status;
  81     uint8_t buf[6];
  82
  83     // set magData to zero for case of failed read
  84     mag->magADCRaw[X] = 0;
  85     mag->magADCRaw[Y] = 0;
  86     mag->magADCRaw[Z] = 0;
  87
  88     bool ack = busRead(mag->busDev, MAG3110_MAG_REG_STATUS, &status);
  89     if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
  90         return false;
  91     }
  92
  93     ack = busReadBuf(mag->busDev, MAG3110_MAG_REG_HXL, buf, 6);
  94     if (!ack) {
  95         return false;
  96     }
  97
  98     mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]);
  99     mag->magADCRaw[Y] = (int16_t)(buf[2] << 8 | buf[3]);
 100     mag->magADCRaw[Z] = -(int16_t)(buf[4] << 8 | buf[5]);  // Z is down, not up in this sensor
 101
 102     return true;
 103 }
 104
 105 #define DETECTION_MAX_RETRY_COUNT   5
 106 static bool deviceDetect(magDev_t * mag)
 107 {
 108     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 109         delay(10);
 110
 111         uint8_t sig = 0;
 112         bool ack = busRead(mag->busDev, MAG3110_MAG_REG_WHO_AM_I, &sig);
 113
 114         if (ack && sig == 0xC4) {
 115             return true;
 116         }
 117     }
 118
 119     return false;
 120 }
 121
 122 bool mag3110detect(magDev_t * mag)
 123 {
 124     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MAG3110, mag->magSensorToUse, OWNER_COMPASS);
 125     if (mag->busDev == NULL) {
 126         return false;
 127     }
 128
 129     if (!deviceDetect(mag)) {
 130         busDeviceDeInit(mag->busDev);
 131         return false;
 132     }
 133
 134     mag->init = mag3110Init;
 135     mag->read = mag3110Read;
 136
 137     return true;
 138 }
 139 #endif