src/main/drivers/compass/compass_vcm5883.c

   1 /*
   2  * This file is part of INAV.
   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 "platform.h"
  26
  27 #ifdef USE_MAG_VCM5883
  28
  29 #include <stdbool.h>
  30 #include <stdint.h>
  31 #include <math.h>
  32
  33 #include "build/build_config.h"
  34
  35 #include "common/axis.h"
  36 #include "common/maths.h"
  37 #include "common/utils.h"
  38
  39 #include "drivers/time.h"
  40 #include "drivers/bus_i2c.h"
  41
  42 #include "sensors/boardalignment.h"
  43 #include "sensors/sensors.h"
  44
  45 #include "drivers/sensor.h"
  46 #include "drivers/compass/compass.h"
  47
  48 #include "drivers/compass/compass_vcm5883.h"
  49
  50 #include "build/debug.h"
  51
  52 #define VCM5883_REGISTER_ADDR_CNTL1 0x0B
  53 #define VCM5883_REGISTER_ADDR_CNTL2 0x0A
  54 #define VCM5883_REGISTER_ADDR_CHIPID 0x0C
  55 #define VCM5883_REGISTER_ADDR_OUTPUT_X 0x00
  56 #define VCM5883_INITIAL_CONFIG 0b0100
  57
  58 #define DETECTION_MAX_RETRY_COUNT   5
  59 static bool deviceDetect(magDev_t * mag)
  60 {
  61     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
  62         busWrite(mag->busDev, VCM5883_REGISTER_ADDR_CNTL2, 0b01000001);
  63         delay(30);
  64
  65         uint8_t sig = 0;
  66         bool ack = busRead(mag->busDev, VCM5883_REGISTER_ADDR_CHIPID, &sig);
  67
  68         if (ack && sig == 0x82) {
  69             return true;
  70         }
  71     }
  72
  73     return false;
  74 }
  75
  76 static bool vcm5883Init(magDev_t * mag) {
  77     UNUSED(mag);
  78     return true;
  79 }
  80
  81 static bool vcm5883Read(magDev_t * mag)
  82 {
  83     uint8_t buf[6];
  84
  85     // set magData to zero for case of failed read
  86     mag->magADCRaw[X] = 0;
  87     mag->magADCRaw[Y] = 0;
  88     mag->magADCRaw[Z] = 0;
  89
  90     bool ack = busReadBuf(mag->busDev, VCM5883_REGISTER_ADDR_OUTPUT_X, buf, 6);
  91     if (!ack) {
  92         return false;
  93     }
  94
  95     mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]);
  96     mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]);
  97     mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]);
  98
  99     return true;
 100 }
 101
 102 bool vcm5883Detect(magDev_t * mag)
 103 {
 104     mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_VCM5883, mag->magSensorToUse, OWNER_COMPASS);
 105     if (mag->busDev == NULL) {
 106         return false;
 107     }
 108
 109     if (!deviceDetect(mag)) {
 110         busDeviceDeInit(mag->busDev);
 111         return false;
 112     }
 113
 114     mag->init = vcm5883Init;
 115     mag->read = vcm5883Read;
 116
 117     return true;
 118 }
 119
 120 #endif