src/main/drivers/compass/compass_ist8310.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24
  25 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #ifdef USE_MAG_IST8310
  30
  31 #include "build/debug.h"
  32
  33 #include "common/axis.h"
  34 #include "common/maths.h"
  35
  36 #include "drivers/bus.h"
  37 #include "drivers/bus_i2c.h"
  38 #include "drivers/bus_i2c_busdev.h"
  39 #include "drivers/sensor.h"
  40 #include "drivers/time.h"
  41
  42 #include "compass.h"
  43 #include "compass_ist8310.h"
  44
  45 //#define DEBUG_MAG_DATA_READY_INTERRUPT
  46
  47 #define IST8310_MAG_I2C_ADDRESS 0x0C
  48
  49 /* ist8310 Slave Address Select : default address 0x0C
  50  *        CAD1  |  CAD0   |  Address
  51  *    ------------------------------
  52  *         VSS   |   VSS  |  0CH
  53  *         VSS   |   VDD  |  0DH
  54  *         VDD   |   VSS  |  0EH
  55  *         VDD   |   VDD  |  0FH
  56  * if CAD1 and CAD0 are floating, I2C address will be 0EH
  57  *
  58  *
  59  * CTRL_REGA: Control Register 1
  60  * Read Write
  61  * Default value: 0x0A
  62  * 7:4  0   Reserved.
  63  * 3:0  DO2-DO0: Operating mode setting
  64  *        DO3  |  DO2 |  DO1 |  DO0 |   mode
  65  *    ------------------------------------------------------
  66  *         0   |   0  |  0   |  0   |   Stand-By mode
  67  *         0   |   0  |  0   |  1   |   Single measurement mode
  68  *                                       Others: Reserved
  69  *
  70  * CTRL_REGB: Control Register 2
  71  * Read Write
  72  * Default value: 0x0B
  73  * 7:4  0   Reserved.
  74  * 3    DREN : Data ready enable control:
  75  *      0: disable
  76  *      1: enable
  77  * 2    DRP: DRDY pin polarity control
  78  *      0: active low
  79  *      1: active high
  80  * 1    0   Reserved.
  81  * 0    SRST: Soft reset, perform Power On Reset (POR) routine
  82  *      0: no action
  83  *      1: start immediately POR routine
  84  *      This bit will be set to zero after POR routine
  85  */
  86
  87 #define IST8310_REG_DATA 0x03
  88 #define IST8310_REG_WAI 0x00
  89 #define IST8310_REG_WAI_VALID 0x10
  90
  91 #define IST8310_REG_STAT1 0x02
  92 #define IST8310_REG_STAT2 0x09
  93
  94 #define IST8310_DRDY_MASK 0x01
  95
  96 // I2C Control Register
  97 #define IST8310_REG_CNTRL1 0x0A
  98 #define IST8310_REG_CNTRL2 0x0B
  99 #define IST8310_REG_AVERAGE 0x41
 100 #define IST8310_REG_PDCNTL 0x42
 101
 102 // Parameter
 103 // ODR = Output Data Rate, we use single measure mode for getting more data.
 104 #define IST8310_ODR_SINGLE 0x01
 105 #define IST8310_ODR_10_HZ 0x03
 106 #define IST8310_ODR_20_HZ 0x05
 107 #define IST8310_ODR_50_HZ 0x07
 108 #define IST8310_ODR_100_HZ 0x06
 109
 110 #define IST8310_AVG_16  0x24
 111 #define IST8310_PULSE_DURATION_NORMAL 0xC0
 112
 113 #define IST8310_CNTRL2_RESET 0x01
 114 #define IST8310_CNTRL2_DRPOL 0x04
 115 #define IST8310_CNTRL2_DRENA 0x08
 116
 117 static bool ist8310Init(magDev_t *magDev)
 118 {
 119     extDevice_t *dev = &magDev->dev;
 120
 121     busDeviceRegister(dev);
 122
 123     // Init setting
 124     bool ack = busWriteRegister(dev, IST8310_REG_AVERAGE, IST8310_AVG_16);
 125     delay(6);
 126     ack = ack && busWriteRegister(dev, IST8310_REG_PDCNTL, IST8310_PULSE_DURATION_NORMAL);
 127     delay(6);
 128     ack = ack && busWriteRegister(dev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE);
 129
 130     return ack;
 131 }
 132
 133 static bool ist8310Read(magDev_t * magDev, int16_t *magData)
 134 {
 135     extDevice_t *dev = &magDev->dev;
 136
 137     static uint8_t buf[6];
 138     const int LSB2FSV = 3; // 3mG - 14 bit
 139
 140     static enum {
 141         STATE_REQUEST_DATA,
 142         STATE_FETCH_DATA,
 143     } state = STATE_REQUEST_DATA;
 144
 145     switch (state) {
 146         default:
 147         case STATE_REQUEST_DATA:
 148             busReadRegisterBufferStart(dev, IST8310_REG_DATA, buf, sizeof(buf));
 149
 150             state = STATE_FETCH_DATA;
 151
 152             return false;
 153         case STATE_FETCH_DATA:
 154             // Looks like datasheet is incorrect and we need to invert Y axis to conform to right hand rule
 155             magData[X] =  (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
 156             magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
 157             magData[Z] =  (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
 158
 159             // Force single measurement mode for next read
 160             busWriteRegisterStart(dev, IST8310_REG_CNTRL1, IST8310_ODR_SINGLE);
 161
 162             state = STATE_REQUEST_DATA;
 163
 164             return true;
 165     }
 166
 167     // TODO: do cross axis compensation
 168
 169     return false;
 170 }
 171
 172 static bool deviceDetect(magDev_t * magDev)
 173 {
 174     uint8_t result = 0;
 175     bool ack = busReadRegisterBuffer(&magDev->dev, IST8310_REG_WAI, &result, 1);
 176
 177     return ack && result == IST8310_REG_WAI_VALID;
 178 }
 179
 180 bool ist8310Detect(magDev_t * magDev)
 181 {
 182     extDevice_t *dev = &magDev->dev;
 183
 184     if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
 185         dev->busType_u.i2c.address = IST8310_MAG_I2C_ADDRESS;
 186     }
 187
 188     if (deviceDetect(magDev)) {
 189         magDev->init = ist8310Init;
 190         magDev->read = ist8310Read;
 191
 192         return true;
 193     }
 194
 195     return false;
 196 }
 197
 198 #endif