src/main/drivers/compass/compass_ak8975.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 #ifdef USE_MAG_AK8975
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/bus.h"
  35 #include "drivers/bus_i2c.h"
  36 #include "drivers/bus_i2c_busdev.h"
  37 #include "drivers/sensor.h"
  38 #include "drivers/time.h"
  39
  40 #include "compass.h"
  41 #include "compass_ak8975.h"
  42
  43 // This sensor is available in MPU-9150.
  44 // This driver only support I2C mode, direct and in bypass configuration.
  45
  46 // AK8975, mag sensor address
  47 #define AK8975_MAG_I2C_ADDRESS          0x0C
  48 #define AK8975_DEVICE_ID                0x48
  49
  50 // Registers
  51 #define AK8975_MAG_REG_WIA              0x00
  52 #define AK8975_MAG_REG_INFO             0x01
  53 #define AK8975_MAG_REG_ST1              0x02
  54 #define AK8975_MAG_REG_HXL              0x03
  55 #define AK8975_MAG_REG_HXH              0x04
  56 #define AK8975_MAG_REG_HYL              0x05
  57 #define AK8975_MAG_REG_HYH              0x06
  58 #define AK8975_MAG_REG_HZL              0x07
  59 #define AK8975_MAG_REG_HZH              0x08
  60 #define AK8975_MAG_REG_ST2              0x09
  61 #define AK8975_MAG_REG_CNTL             0x0A
  62 #define AK8975_MAG_REG_ASTC             0x0C // self test
  63 #define AK8975_MAG_REG_I2CDIS           0x0F
  64 #define AK8975_MAG_REG_ASAX             0x10 // Fuse ROM x-axis sensitivity adjustment value
  65 #define AK8975_MAG_REG_ASAY             0x11 // Fuse ROM y-axis sensitivity adjustment value
  66 #define AK8975_MAG_REG_ASAZ             0x12 // Fuse ROM z-axis sensitivity adjustment value
  67
  68 #define ST1_REG_DATA_READY              0x01
  69
  70 #define ST2_REG_DATA_ERROR              0x04
  71 #define ST2_REG_MAG_SENSOR_OVERFLOW     0x08
  72
  73 #define CNTL_MODE_POWER_DOWN            0x00
  74 #define CNTL_MODE_ONCE                  0x01
  75 #define CNTL_MODE_CONT1                 0x02
  76 #define CNTL_MODE_FUSE_ROM              0x0F
  77 #define CNTL_BIT_14_BIT                 0x00
  78 #define CNTL_BIT_16_BIT                 0x10
  79
  80 static bool ak8975Init(magDev_t *mag)
  81 {
  82     uint8_t asa[3];
  83     uint8_t status;
  84
  85     extDevice_t *dev = &mag->dev;
  86
  87     busDeviceRegister(dev);
  88
  89     busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
  90     delay(20);
  91
  92     busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
  93     delay(10);
  94
  95     busReadRegisterBuffer(dev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values
  96     delay(10);
  97
  98     mag->magGain[X] = asa[X] + 128;
  99     mag->magGain[Y] = asa[Y] + 128;
 100     mag->magGain[Z] = asa[Z] + 128;
 101
 102     busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
 103     delay(10);
 104
 105     // Clear status registers
 106     busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1);
 107     busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1);
 108
 109     // Trigger first measurement
 110     busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
 111     return true;
 112 }
 113
 114 static int16_t parseMag(uint8_t *raw, int16_t gain)
 115 {
 116   int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
 117   return constrain(ret, INT16_MIN, INT16_MAX);
 118 }
 119
 120 static bool ak8975Read(magDev_t *mag, int16_t *magData)
 121 {
 122     static uint8_t buf[6];
 123     static uint8_t status;
 124     static enum {
 125         STATE_READ_STATUS1,
 126         STATE_WAIT_STATUS1,
 127         STATE_READ_STATUS2,
 128         STATE_WAIT_STATUS2,
 129         STATE_WAIT_START,
 130     } state = STATE_READ_STATUS1;
 131
 132     extDevice_t *dev = &mag->dev;
 133
 134     switch (state) {
 135         default:
 136         case STATE_READ_STATUS1:
 137             busReadRegisterBufferStart(dev, AK8975_MAG_REG_ST1, &status, sizeof(status));
 138             state = STATE_WAIT_STATUS1;
 139             return false;
 140
 141         case STATE_WAIT_STATUS1:
 142             if ((status & ST1_REG_DATA_READY) == 0) {
 143                 state = STATE_READ_STATUS1;
 144                 return false;
 145             }
 146
 147             busReadRegisterBufferStart(dev, AK8975_MAG_REG_HXL, buf, sizeof(buf));
 148
 149             state = STATE_READ_STATUS2;
 150             return false;
 151
 152         case STATE_READ_STATUS2:
 153             busReadRegisterBufferStart(dev, AK8975_MAG_REG_ST2, &status, sizeof(status));
 154             state = STATE_WAIT_STATUS2;
 155             return false;
 156
 157         case STATE_WAIT_STATUS2:
 158             busWriteRegisterStart(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again
 159
 160             if ((status & ST2_REG_DATA_ERROR) || (status & ST2_REG_MAG_SENSOR_OVERFLOW)) {
 161                 state = STATE_READ_STATUS1;
 162                 return false;
 163             }
 164
 165             state = STATE_WAIT_START;
 166             return false;
 167
 168         case STATE_WAIT_START:
 169
 170             magData[X] = -parseMag(buf + 0, mag->magGain[X]);
 171             magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
 172             magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
 173
 174             state = STATE_READ_STATUS1;
 175             return true;
 176     }
 177
 178     return false;
 179 }
 180
 181 bool ak8975Detect(magDev_t *mag)
 182 {
 183     uint8_t sig = 0;
 184
 185     extDevice_t *dev = &mag->dev;
 186
 187     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 188         dev->busType_u.i2c.address = AK8975_MAG_I2C_ADDRESS;
 189     }
 190
 191     bool ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_WIA, &sig, 1);
 192
 193     if (!ack || sig != AK8975_DEVICE_ID) { // 0x48 / 01001000 / 'H'
 194         return false;
 195     }
 196
 197     mag->init = ak8975Init;
 198     mag->read = ak8975Read;
 199
 200     return true;
 201 }
 202 #endif