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   int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
 116   return constrain(ret, INT16_MIN, INT16_MAX);
 117 }
 118
 119 static bool ak8975Read(magDev_t *mag, int16_t *magData)
 120 {
 121     bool ack;
 122     uint8_t status;
 123     uint8_t buf[6];
 124
 125     extDevice_t *dev = &mag->dev;
 126
 127     ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1);
 128     if (!ack || (status & ST1_REG_DATA_READY) == 0) {
 129         return false;
 130     }
 131
 132     busReadRegisterBuffer(dev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
 133
 134     ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1);
 135     if (!ack) {
 136         return false;
 137     }
 138
 139     busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again    uint8_t status2 = buf[6];
 140
 141     if (status & ST2_REG_DATA_ERROR) {
 142         return false;
 143     }
 144
 145     if (status & ST2_REG_MAG_SENSOR_OVERFLOW) {
 146         return false;
 147     }
 148
 149     magData[X] = -parseMag(buf + 0, mag->magGain[X]);
 150     magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
 151     magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
 152
 153     return true;
 154 }
 155
 156 bool ak8975Detect(magDev_t *mag)
 157 {
 158     uint8_t sig = 0;
 159
 160     extDevice_t *dev = &mag->dev;
 161
 162     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 163         dev->busType_u.i2c.address = AK8975_MAG_I2C_ADDRESS;
 164     }
 165
 166     bool ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_WIA, &sig, 1);
 167
 168     if (!ack || sig != AK8975_DEVICE_ID) { // 0x48 / 01001000 / 'H'
 169         return false;
 170     }
 171
 172     mag->init = ak8975Init;
 173     mag->read = ak8975Read;
 174
 175     return true;
 176 }
 177 #endif