src/main/drivers/compass/compass_ak8975.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_AK8975
  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 "drivers/sensor.h"
  37 #include "drivers/compass/compass.h"
  38
  39 #include "drivers/compass/compass_ak8975.h"
  40
  41 // This sensor is available in MPU-9150.
  42
  43 // AK8975, mag sensor address
  44 #define AK8975_MAG_I2C_ADDRESS      0x0C
  45
  46 // Registers
  47 #define AK8975_MAG_REG_WHO_AM_I     0x00
  48 #define AK8975_MAG_REG_INFO         0x01
  49 #define AK8975_MAG_REG_STATUS1      0x02
  50 #define AK8975_MAG_REG_HXL          0x03
  51 #define AK8975_MAG_REG_HXH          0x04
  52 #define AK8975_MAG_REG_HYL          0x05
  53 #define AK8975_MAG_REG_HYH          0x06
  54 #define AK8975_MAG_REG_HZL          0x07
  55 #define AK8975_MAG_REG_HZH          0x08
  56 #define AK8975_MAG_REG_STATUS2      0x09
  57 #define AK8975_MAG_REG_CNTL         0x0a
  58 #define AK8975_MAG_REG_ASCT         0x0c // self test
  59
  60 #define AK8975A_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
  61 #define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
  62 #define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
  63
  64 static bool ak8975Init(magDev_t * mag)
  65 {
  66     bool ack;
  67     uint8_t buffer[3];
  68     uint8_t status;
  69
  70     UNUSED(ack);
  71
  72     ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
  73     delay(20);
  74
  75     ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
  76     delay(10);
  77
  78     ack = busReadBuf(mag->busDev, AK8975A_ASAX, &buffer[0], 3); // Read the x-, y-, and z-axis calibration values
  79     delay(10);
  80
  81     ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
  82     delay(10);
  83
  84     // Clear status registers
  85     ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS1, &status);
  86     ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS2, &status);
  87
  88     // Trigger first measurement
  89     ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x01);
  90     return true;
  91 }
  92
  93 #define BIT_STATUS1_REG_DATA_READY              (1 << 0)
  94
  95 #define BIT_STATUS2_REG_DATA_ERROR              (1 << 2)
  96 #define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW     (1 << 3)
  97
  98 static bool ak8975Read(magDev_t * mag)
  99 {
 100     uint8_t status;
 101     uint8_t buf[6];
 102
 103     // set magData to zero for case of failed read
 104     mag->magADCRaw[X] = 0;
 105     mag->magADCRaw[Y] = 0;
 106     mag->magADCRaw[Z] = 0;
 107
 108     bool ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS1, &status);
 109     if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
 110         return false;
 111     }
 112
 113     ack = busReadBuf(mag->busDev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
 114     ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS2, &status);
 115
 116     if (!ack || (status & BIT_STATUS2_REG_DATA_ERROR) || (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW)) {
 117         return false;
 118     }
 119
 120     mag->magADCRaw[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
 121     mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * 4;
 122     mag->magADCRaw[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
 123
 124     ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x01); // start reading again
 125     return true;
 126 }
 127
 128 #define DETECTION_MAX_RETRY_COUNT   5
 129 static bool deviceDetect(magDev_t * mag)
 130 {
 131     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 132         delay(10);
 133
 134         uint8_t sig = 0;
 135         bool ack = busRead(mag->busDev, AK8975_MAG_REG_WHO_AM_I, &sig);
 136
 137         if (ack && sig == 0x48) {
 138             return true;
 139         }
 140     }
 141
 142     return false;
 143 }
 144
 145 bool ak8975Detect(magDev_t * mag)
 146 {
 147     mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_AK8975, mag->magSensorToUse, OWNER_COMPASS);
 148     if (mag->busDev == NULL) {
 149         return false;
 150     }
 151
 152     if (!deviceDetect(mag)) {
 153         busDeviceDeInit(mag->busDev);
 154         return false;
 155     }
 156
 157     mag->init = ak8975Init;
 158     mag->read = ak8975Read;
 159
 160     return true;
 161 }
 162
 163 #endif