src/main/drivers/compass/compass_ak8963.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 #include <string.h>
  21
  22 #include <math.h>
  23
  24 #include "platform.h"
  25
  26 #ifdef USE_MAG_AK8963
  27
  28 #include "build/debug.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/time.h"
  35 #include "drivers/io.h"
  36 #include "drivers/bus.h"
  37
  38 #include "drivers/sensor.h"
  39 #include "drivers/compass/compass.h"
  40
  41 // AK8963, mag sensor address
  42 #define AK8963_MAG_I2C_ADDRESS          0x0C
  43 #define AK8963_DEVICE_ID                0x48
  44
  45
  46 // Registers
  47 #define AK8963_MAG_REG_WHO_AM_I         0x00
  48 #define AK8963_MAG_REG_INFO             0x01
  49 #define AK8963_MAG_REG_STATUS1          0x02
  50 #define AK8963_MAG_REG_HXL              0x03
  51 #define AK8963_MAG_REG_HXH              0x04
  52 #define AK8963_MAG_REG_HYL              0x05
  53 #define AK8963_MAG_REG_HYH              0x06
  54 #define AK8963_MAG_REG_HZL              0x07
  55 #define AK8963_MAG_REG_HZH              0x08
  56 #define AK8963_MAG_REG_STATUS2          0x09
  57 #define AK8963_MAG_REG_CNTL             0x0a
  58 #define AK8963_MAG_REG_ASCT             0x0c // self test
  59 #define AK8963_MAG_REG_ASAX             0x10 // Fuse ROM x-axis sensitivity adjustment value
  60 #define AK8963_MAG_REG_ASAY             0x11 // Fuse ROM y-axis sensitivity adjustment value
  61 #define AK8963_MAG_REG_ASAZ             0x12 // Fuse ROM z-axis sensitivity adjustment value
  62
  63 #define READ_FLAG                       0x80
  64
  65 #define STATUS1_DATA_READY              0x01
  66 #define STATUS1_DATA_OVERRUN            0x02
  67
  68 #define STATUS2_MAG_SENSOR_OVERFLOW     0x08
  69
  70 #define CNTL_MODE_POWER_DOWN            0x00
  71 #define CNTL_MODE_ONCE                  0x01
  72 #define CNTL_MODE_CONT1                 0x02
  73 #define CNTL_MODE_CONT2                 0x06
  74 #define CNTL_MODE_SELF_TEST             0x08
  75 #define CNTL_MODE_FUSE_ROM              0x0F
  76 #define CNTL_BIT_14_BIT                 0x00
  77 #define CNTL_BIT_16_BIT                 0x10
  78
  79 static int16_t magGain[3];
  80
  81 static bool ak8963Init(magDev_t * mag)
  82 {
  83     bool ack;
  84     UNUSED(ack);
  85     uint8_t calibration[3];
  86     uint8_t status;
  87
  88     ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
  89     delay(20);
  90
  91     ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
  92     delay(10);
  93
  94     ack = busReadBuf(mag->busDev, AK8963_MAG_REG_ASAX, calibration, sizeof(calibration)); // Read the x-, y-, and z-axis calibration values
  95     delay(10);
  96
  97     magGain[X] = calibration[X] + 128;
  98     magGain[Y] = calibration[Y] + 128;
  99     magGain[Z] = calibration[Z] + 128;
 100
 101     ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
 102     delay(10);
 103
 104     // Clear status registers
 105     ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &status);
 106     ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS2, &status);
 107
 108     ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
 109
 110     return true;
 111 }
 112
 113 static int16_t parseMag(uint8_t *raw, int16_t gain) {
 114   int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
 115   return constrain(ret, INT16_MIN, INT16_MAX);
 116 }
 117
 118 static bool ak8963Read(magDev_t * mag)
 119 {
 120     bool ack = false;
 121     uint8_t buf[7];
 122
 123     ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &buf[0]);
 124
 125     if (!ack || (buf[0] & STATUS1_DATA_READY) == 0) {
 126         return false;
 127     }
 128
 129     ack = busReadBuf(mag->busDev, AK8963_MAG_REG_HXL, &buf[0], 7);
 130
 131     if (!ack) {
 132         return false;
 133     }
 134
 135     ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
 136
 137     if (buf[6] & STATUS2_MAG_SENSOR_OVERFLOW) {
 138         return false;
 139     }
 140
 141     mag->magADCRaw[X] = -parseMag(buf + 0, magGain[X]);
 142     mag->magADCRaw[Y] = parseMag(buf + 2, magGain[Y]);
 143     mag->magADCRaw[Z] = -parseMag(buf + 4, magGain[Z]);
 144
 145     return true;
 146 }
 147
 148 #define DETECTION_MAX_RETRY_COUNT   5
 149 static bool deviceDetect(magDev_t * mag)
 150 {
 151     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 152         delay(10);
 153
 154         uint8_t sig = 0;
 155         bool ack = busRead(mag->busDev, AK8963_MAG_REG_WHO_AM_I, &sig);
 156
 157         if (ack && sig == AK8963_DEVICE_ID) {
 158             return true;
 159         }
 160     }
 161
 162     return false;
 163 }
 164
 165 bool ak8963Detect(magDev_t * mag)
 166 {
 167     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_AK8963, mag->magSensorToUse, OWNER_COMPASS);
 168     if (mag->busDev == NULL) {
 169         return false;
 170     }
 171
 172     if (!deviceDetect(mag)) {
 173         busDeviceDeInit(mag->busDev);
 174         return false;
 175     }
 176
 177     mag->init = ak8963Init;
 178     mag->read = ak8963Read;
 179
 180     return true;
 181 }
 182 #endif