src/main/drivers/compass/compass_ist8308.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_IST8308
  26
  27 #include "build/debug.h"
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31
  32 #include "drivers/time.h"
  33 #include "drivers/nvic.h"
  34 #include "drivers/io.h"
  35 #include "drivers/bus.h"
  36 #include "drivers/light_led.h"
  37
  38 #include "drivers/sensor.h"
  39 #include "drivers/compass/compass.h"
  40
  41 #include "drivers/compass/compass_ist8308.h"
  42
  43 #define IST8308_ADDRESS                                 0x0C
  44
  45 #define IST8308_REG_WHOAMI                              0x00
  46 #define     IST8308_CHIP_ID                             0x08
  47
  48 #define IST8308_REG_DATA                                0x11    // XYZ High&Low
  49
  50
  51 // Noise suppression filtering
  52 #define IST8308_REG_CNTRL1                              0x30    // Control setting register 1
  53 //bit6~bit5
  54 #define     IST8308_NSF_DISABLE                         0x00
  55 #define     IST8308_NSF_LOW                             0x20
  56 #define     IST8308_NSF_MIDDLE                          0x40
  57 #define     IST8308_NSF_HIGH                            0x60
  58
  59 #define IST8308_REG_CNTRL2                              0x31    // Control setting register 2
  60 //bit4~bit0  Operation mode
  61 #define     IST8308_OPMODE_STANDBY_MODE                 0x00    // Default
  62 #define     IST8308_OPMODE_SINGLE_MODE                  0x01
  63 #define     IST8308_OPMODE_CONTINUOS_MODE_10HZ          0x02
  64 #define     IST8308_OPMODE_CONTINUOS_MODE_20HZ          0x04
  65 #define     IST8308_OPMODE_CONTINUOS_MODE_50HZ          0x06
  66 #define     IST8308_OPMODE_CONTINUOS_MODE_100HZ         0x08
  67 #define     IST8308_OPMODE_CONTINUOS_MODE_200HZ         0x0A
  68 #define     IST8308_OPMODE_CONTINUOS_MODE_8HZ           0x0B
  69 #define     IST8308_OPMODE_CONTINUOS_MODE_1HZ           0x0C
  70 #define     IST8308_OPMODE_CONTINUOS_MODE_0_5HZ         0x0D
  71 #define     IST8308_OPMODE_SELF_TEST_MODE               0x10
  72
  73 // Sensitivity / measure dynamic range - default 660 for LSB2FSV = 1.5
  74 #define IST8308_REG_CNTRL4                              0x34
  75 #define     IST8308_SENSITIVITY_6_6                     0x00
  76 #define     IST8308_SENSITIVITY_13_2                    0x01
  77
  78 // Over sampling ratio
  79 #define IST8308_REG_OSR_CNTRL                           0x41    //Average control register
  80 //bit2~bit0
  81 #define     IST8308_X_Z_SENSOR_OSR_1                    0x00
  82 #define     IST8308_X_Z_SENSOR_OSR_2                    0x01
  83 #define     IST8308_X_Z_SENSOR_OSR_4                    0x02
  84 #define     IST8308_X_Z_SENSOR_OSR_8                    0x03
  85 #define     IST8308_X_Z_SENSOR_OSR_16                   0x04    //Default (ODRmax=100Hz)
  86 #define     IST8308_X_Z_SENSOR_OSR_32                   0x05
  87 //bit5~bit3
  88 #define     IST8308_Y_SENSOR_OSR_1                      (0x00<<3)
  89 #define     IST8308_Y_SENSOR_OSR_2                      (0x01<<3)
  90 #define     IST8308_Y_SENSOR_OSR_4                      (0x02<<3)
  91 #define     IST8308_Y_SENSOR_OSR_8                      (0x03<<3)
  92 #define     IST8308_Y_SENSOR_OSR_16                     (0x04<<3)   //Default (ODRmax=100Hz)
  93 #define     IST8308_Y_SENSOR_OSR_32                     (0x05<<3)
  94
  95
  96
  97 static bool ist8308Init(magDev_t * mag)
  98 {
  99     bool ack =   busWrite(mag->busDev, IST8308_REG_OSR_CNTRL, IST8308_X_Z_SENSOR_OSR_16 | IST8308_Y_SENSOR_OSR_16);
 100     ack = ack && busWrite(mag->busDev, IST8308_REG_CNTRL1,    IST8308_NSF_LOW);
 101     ack = ack && busWrite(mag->busDev, IST8308_REG_CNTRL2,    IST8308_OPMODE_CONTINUOS_MODE_50HZ);
 102     ack = ack && busWrite(mag->busDev, IST8308_REG_CNTRL4,    IST8308_SENSITIVITY_6_6);
 103     delay(5);
 104
 105     return true;
 106 }
 107
 108 static bool ist8308Read(magDev_t * mag)
 109 {
 110     const float LSB2FSV = 1.5; // 1.5mG
 111
 112     uint8_t buf[6];
 113
 114     bool ack = busReadBuf(mag->busDev, IST8308_REG_DATA, buf, 6);
 115     if (!ack) {
 116         // set magData to zero for case of failed read
 117         mag->magADCRaw[X] = 0;
 118         mag->magADCRaw[Y] = 0;
 119         mag->magADCRaw[Z] = 0;
 120
 121         return false;
 122     }
 123
 124     // Invert Y axis to co convert from left to right coordinate system
 125     mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
 126     mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
 127     mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
 128
 129     return true;
 130 }
 131
 132 #define DETECTION_MAX_RETRY_COUNT   5
 133 static bool deviceDetect(magDev_t * mag)
 134 {
 135     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 136         delay(10);
 137
 138         uint8_t sig = 0;
 139         bool ack = busRead(mag->busDev, IST8308_REG_WHOAMI, &sig);
 140
 141         if (ack && sig == IST8308_CHIP_ID) {
 142             return true;
 143         }
 144     }
 145
 146     return false;
 147 }
 148
 149 bool ist8308Detect(magDev_t * mag)
 150 {
 151     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_IST8308, mag->magSensorToUse, OWNER_COMPASS);
 152     if (mag->busDev == NULL) {
 153         return false;
 154     }
 155
 156     if (!deviceDetect(mag)) {
 157         busDeviceDeInit(mag->busDev);
 158         return false;
 159     }
 160
 161     mag->init = ist8308Init;
 162     mag->read = ist8308Read;
 163
 164     return true;
 165 }
 166
 167 #endif