src/main/drivers/compass/compass_mlx90393.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV 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  * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "platform.h"
  19
  20 #ifdef USE_MAG_MLX90393
  21
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24
  25 #include <math.h>
  26
  27 #include "build/debug.h"
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/time.h"
  34 #include "drivers/io.h"
  35 #include "drivers/bus.h"
  36
  37 #include "drivers/sensor.h"
  38 #include "drivers/compass/compass.h"
  39
  40 #define MLX90393_MEASURE_3D                         0b00001110
  41
  42 #define MLX90393_NOP                                0b00000000
  43 #define MLX90393_START_BURST_MODE                   0b00010000 //uses with zyxt flags
  44 #define MLX90393_START_WAKE_UP_ON_CHANGE_MODE       0b00100000 //uses with zyxt flags
  45 #define MLX90393_START_SINGLE_MEASUREMENT_MODE      0b00110000 //uses with zyxt flags
  46 #define MLX90393_READ_MEASUREMENT                   0b01000000 //uses with zyxt flags
  47 #define MLX90393_READ_REGISTER                      0b01010000
  48 #define MLX90393_WRITE_REGISTER                     0b01100000
  49 #define MLX90393_EXIT_MODE                          0b10000000
  50 #define MLX90393_MEMORY_RECALL                      0b11010000
  51 #define MLX90393_MEMORY_STORE                       0b11100000
  52 #define MLX90393_RESET                              0b11110000
  53
  54 #define MLX90393_BURST_MODE_FLAG                    0b10000000
  55 #define MLX90393_WAKE_UP_ON_CHANGE_MODE_FLAG        0b01000000
  56 #define MLX90393_SINGLE_MEASUREMENT_MODE_FLAG       0b00100000
  57 #define MLX90393_ERROR_FLAG                         0b00010000
  58 #define MLX90393_SINGLE_ERROR_DETECTION_FLAG        0b00001000
  59 #define MLX90393_RESET_FLAG                         0b00000100
  60 #define MLX90393_D1_FLAG                            0b00000010
  61 #define MLX90393_D0_FLAG                            0b00000001
  62
  63 #define DETECTION_MAX_RETRY_COUNT   5
  64
  65 #define REG_BUF_LEN                 3
  66
  67 // Register 1
  68 #define GAIN_SEL_HALLCONF_REG       0x0 //from datasheet 0x0 << 2 = 0x0
  69 // GAIN - 0b111
  70 // Hall_conf - 0xC
  71 #define GAIN_SEL_HALLCONF_REG_VALUE 0x007C
  72 // Register 2
  73 #define TCMP_EN_REG                 0x4 //from datasheet 0x1 << 2 = 0x4
  74 // Burst Data Rate 0b000100
  75 #define TCMP_EN_REG_VALUE           0x62C4
  76 // Register 3
  77 #define RES_XYZ_OSR_FLT_REG         0x8 //from datasheet 0x2 << 2 = 0x8
  78 // Oversampling 0b01
  79 // Filtering 0b111
  80 #define RES_XYZ_OSR_FLT_REG_VALUE   0x001D
  81
  82
  83 static void mlx90393WriteRegister(magDev_t * mag, uint8_t reg_val, uint16_t value) {
  84
  85     uint8_t buf[REG_BUF_LEN] = {0};
  86
  87     buf[0] = (value >> 8) & 0xFF;
  88     buf[1] = (value >> 0) & 0xFF;
  89     buf[2] = reg_val;
  90
  91     busWriteBuf(mag->busDev, MLX90393_WRITE_REGISTER, buf, REG_BUF_LEN);
  92
  93     // PAUSE
  94     delay(20);
  95     // To skip ACK FLAG of Write
  96     uint8_t sig = 0;
  97     busRead(mag->busDev, MLX90393_NOP, &sig);
  98
  99     return;
 100 }
 101
 102 // =======================================================================================
 103 static bool mlx90393Read(magDev_t * mag)
 104 {
 105
 106     uint8_t buf[7] = {0};
 107
 108     busReadBuf(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, buf, 7);
 109
 110     mag->magADCRaw[X] = ((short)(buf[1] << 8 | buf[2]));
 111     mag->magADCRaw[Y] = ((short)(buf[3] << 8 | buf[4]));
 112     mag->magADCRaw[Z] = ((short)(buf[5] << 8 | buf[6]));
 113
 114     return true;
 115
 116 }
 117
 118 static bool deviceDetect(magDev_t * mag)
 119 {
 120     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 121         uint8_t sig = 0;
 122
 123         bool ack = busRead(mag->busDev, MLX90393_RESET, &sig);
 124         delay(20);
 125
 126         if (ack && ((sig & MLX90393_RESET_FLAG) == MLX90393_RESET_FLAG)) { // Check Reset Flag -> MLX90393
 127             return true;
 128         }
 129     }
 130     return false;
 131 }
 132
 133 //--------------------------------------------------
 134 static bool mlx90393Init(magDev_t * mag)
 135 {
 136     uint8_t sig = 0;
 137
 138     delay(20);
 139     // Remove reset flag
 140     busRead(mag->busDev, MLX90393_NOP, &sig);
 141     // Exit if already in burst mode. (For example when external i2c source power the bus.)
 142     busRead(mag->busDev, MLX90393_EXIT_MODE, &sig);
 143
 144     // Writing Registers
 145     mlx90393WriteRegister(mag, GAIN_SEL_HALLCONF_REG, GAIN_SEL_HALLCONF_REG_VALUE);
 146     mlx90393WriteRegister(mag, TCMP_EN_REG, TCMP_EN_REG_VALUE);
 147     mlx90393WriteRegister(mag, RES_XYZ_OSR_FLT_REG, RES_XYZ_OSR_FLT_REG_VALUE);
 148
 149     // Start burst mode
 150     busRead(mag->busDev, MLX90393_START_BURST_MODE | MLX90393_MEASURE_3D, &sig);
 151
 152     return true;
 153 }
 154
 155 // ==========================================================================
 156
 157 bool mlx90393Detect(magDev_t * mag)
 158 {
 159     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MLX90393, mag->magSensorToUse, OWNER_COMPASS);
 160     if (mag->busDev == NULL) {
 161         return false;
 162     }
 163
 164     if (!deviceDetect(mag)) {
 165         busDeviceDeInit(mag->busDev);
 166         return false;
 167     }
 168
 169     mag->init = mlx90393Init;
 170     mag->read = mlx90393Read;
 171
 172     return true;
 173 }
 174
 175
 176 #endif