src/main/drivers/compass/compass_rm3100.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include <math.h>
  29
  30 #include "platform.h"
  31
  32 #ifdef USE_MAG_RM3100
  33
  34 #include "build/build_config.h"
  35 #include "build/debug.h"
  36
  37 #include "common/axis.h"
  38 #include "common/maths.h"
  39 #include "common/utils.h"
  40
  41 #include "drivers/time.h"
  42 #include "drivers/bus_i2c.h"
  43
  44 #include "sensors/boardalignment.h"
  45 #include "sensors/sensors.h"
  46
  47 #include "drivers/sensor.h"
  48 #include "drivers/compass/compass.h"
  49
  50 #include "drivers/compass/compass_rm3100.h"
  51
  52 #define RM3100_REG_POLL        0x00
  53 #define RM3100_REG_CMM         0x01
  54 #define RM3100_REG_CCX1        0x04
  55 #define RM3100_REG_CCX0        0x05
  56 #define RM3100_REG_CCY1        0x06
  57 #define RM3100_REG_CCY0        0x07
  58 #define RM3100_REG_CCZ1        0x08
  59 #define RM3100_REG_CCZ0        0x09
  60 #define RM3100_REG_TMRC        0x0B
  61 #define RM3100_REG_MX          0x24
  62 #define RM3100_REG_MY          0x27
  63 #define RM3100_REG_MZ          0x2A
  64 #define RM3100_REG_BIST        0x33
  65 #define RM3100_REG_STATUS      0x34
  66 #define RM3100_REG_HSHAKE      0x35
  67 #define RM3100_REG_REVID       0x36
  68
  69 #define RM3100_REVID           0x22
  70
  71 #define CCX_DEFAULT_MSB        0x00
  72 #define CCX_DEFAULT_LSB        0xC8
  73 #define CCY_DEFAULT_MSB        CCX_DEFAULT_MSB
  74 #define CCY_DEFAULT_LSB        CCX_DEFAULT_LSB
  75 #define CCZ_DEFAULT_MSB        CCX_DEFAULT_MSB
  76 #define CCZ_DEFAULT_LSB        CCX_DEFAULT_LSB
  77 #define CMM_DEFAULT            0x71    // Continuous mode
  78 #define TMRC_DEFAULT           0x94
  79
  80
  81 static bool deviceInit(magDev_t * mag)
  82 {
  83     busWrite(mag->busDev, RM3100_REG_TMRC, TMRC_DEFAULT);
  84
  85     busWrite(mag->busDev, RM3100_REG_CMM, CMM_DEFAULT);
  86
  87     busWrite(mag->busDev, RM3100_REG_CCX1, CCX_DEFAULT_MSB);
  88     busWrite(mag->busDev, RM3100_REG_CCX0, CCX_DEFAULT_LSB);
  89
  90     busWrite(mag->busDev, RM3100_REG_CCY1, CCY_DEFAULT_MSB);
  91     busWrite(mag->busDev, RM3100_REG_CCY0, CCY_DEFAULT_LSB);
  92
  93     busWrite(mag->busDev, RM3100_REG_CCZ1, CCZ_DEFAULT_MSB);
  94     busWrite(mag->busDev, RM3100_REG_CCZ0, CCZ_DEFAULT_LSB);
  95
  96     return true;
  97 }
  98
  99 static bool deviceRead(magDev_t * mag)
 100 {
 101     uint8_t status;
 102
 103 #pragma pack(push, 1)
 104     struct {
 105         uint8_t x[3];
 106         uint8_t y[3];
 107         uint8_t z[3];
 108     } rm_report;
 109 #pragma pack(pop)
 110
 111     mag->magADCRaw[X] = 0;
 112     mag->magADCRaw[Y] = 0;
 113     mag->magADCRaw[Z] = 0;
 114
 115     /* Check if new measurement is ready */
 116     bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status);
 117
 118     if (!ack || (status & 0x80) == 0) {
 119         return false;
 120     }
 121
 122     ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report));
 123     if (!ack) {
 124         return false;
 125     }
 126
 127     int32_t xraw;
 128     int32_t yraw;
 129     int32_t zraw;
 130
 131     /* Rearrange mag data */
 132     xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8);
 133     yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8);
 134     zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8);
 135
 136     /* Truncate to 16-bit integers and pass along */
 137     mag->magADCRaw[X] = (int16_t)(xraw >> 16);
 138     mag->magADCRaw[Y] = (int16_t)(yraw >> 16);
 139     mag->magADCRaw[Z] = (int16_t)(zraw >> 16);
 140
 141     return true;
 142 }
 143
 144 #define DETECTION_MAX_RETRY_COUNT   5
 145 static bool deviceDetect(magDev_t * mag)
 146 {
 147     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 148         uint8_t revid = 0;
 149         bool ack = busRead(mag->busDev, RM3100_REG_REVID, &revid);
 150
 151         if (ack && revid == RM3100_REVID) {
 152             return true;
 153         }
 154     }
 155
 156     return false;
 157 }
 158
 159 bool rm3100MagDetect(magDev_t * mag)
 160 {
 161     busSetSpeed(mag->busDev, BUS_SPEED_STANDARD);
 162
 163     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_RM3100, mag->magSensorToUse, OWNER_COMPASS);
 164     if (mag->busDev == NULL) {
 165         return false;
 166     }
 167
 168     if (!deviceDetect(mag)) {
 169         busDeviceDeInit(mag->busDev);
 170         return false;
 171     }
 172
 173     mag->init = deviceInit;
 174     mag->read = deviceRead;
 175
 176     return true;
 177 }
 178
 179 #endif