src/main/sensors/compass.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #if defined(USE_MAG)
  28
  29 #include "common/axis.h"
  30
  31 #include "config/config.h"
  32
  33 #include "drivers/bus.h"
  34 #include "drivers/bus_i2c.h"
  35 #include "drivers/bus_spi.h"
  36 #include "drivers/compass/compass.h"
  37 #include "drivers/compass/compass_ak8975.h"
  38 #include "drivers/compass/compass_ak8963.h"
  39 #include "drivers/compass/compass_fake.h"
  40 #include "drivers/compass/compass_hmc5883l.h"
  41 #include "drivers/compass/compass_lis3mdl.h"
  42 #include "drivers/compass/compass_mpu925x_ak8963.h"
  43 #include "drivers/compass/compass_qmc5883l.h"
  44
  45 #include "drivers/io.h"
  46 #include "drivers/light_led.h"
  47 #include "drivers/time.h"
  48
  49 #include "fc/runtime_config.h"
  50
  51 #include "pg/pg.h"
  52 #include "pg/pg_ids.h"
  53
  54 #include "sensors/boardalignment.h"
  55 #include "sensors/gyro.h"
  56 #include "sensors/gyro_init.h"
  57 #include "sensors/sensors.h"
  58
  59 #include "compass.h"
  60
  61 static timeUs_t tCal = 0;
  62 static flightDynamicsTrims_t magZeroTempMin;
  63 static flightDynamicsTrims_t magZeroTempMax;
  64
  65 magDev_t magDev;
  66 mag_t mag;                   // mag access functions
  67
  68 PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
  69
  70 void pgResetFn_compassConfig(compassConfig_t *compassConfig)
  71 {
  72     compassConfig->mag_alignment = ALIGN_DEFAULT;
  73     memset(&compassConfig->mag_customAlignment, 0x00, sizeof(compassConfig->mag_customAlignment));
  74     compassConfig->mag_hardware = MAG_DEFAULT;
  75
  76 // Generate a reasonable default for backward compatibility
  77 // Strategy is
  78 // 1. If SPI device is defined, it will take precedence, assuming it's onboard.
  79 // 2. I2C devices are will be handled by address = 0 (per device default).
  80 // 3. Slave I2C device on SPI gyro
  81
  82 #if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963))
  83     compassConfig->mag_bustype = BUSTYPE_SPI;
  84     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE));
  85     compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN);
  86     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
  87     compassConfig->mag_i2c_address = 0;
  88 #elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
  89     compassConfig->mag_bustype = BUSTYPE_I2C;
  90     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE);
  91     compassConfig->mag_i2c_address = 0;
  92     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
  93     compassConfig->mag_spi_csn = IO_TAG_NONE;
  94 #elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
  95     compassConfig->mag_bustype = BUSTYPE_MPU_SLAVE;
  96     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
  97     compassConfig->mag_i2c_address = 0;
  98     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
  99     compassConfig->mag_spi_csn = IO_TAG_NONE;
 100 #else
 101     compassConfig->mag_hardware = MAG_NONE;
 102     compassConfig->mag_bustype = BUSTYPE_NONE;
 103     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 104     compassConfig->mag_i2c_address = 0;
 105     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 106     compassConfig->mag_spi_csn = IO_TAG_NONE;
 107 #endif
 108     compassConfig->interruptTag = IO_TAG(MAG_INT_EXTI);
 109 }
 110
 111 static int16_t magADCRaw[XYZ_AXIS_COUNT];
 112 static uint8_t magInit = 0;
 113
 114 void compassPreInit(void)
 115 {
 116 #ifdef USE_SPI
 117     if (compassConfig()->mag_bustype == BUSTYPE_SPI) {
 118         spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1);
 119     }
 120 #endif
 121 }
 122
 123 #if !defined(SIMULATOR_BUILD)
 124 bool compassDetect(magDev_t *dev, uint8_t *alignment)
 125 {
 126     *alignment = ALIGN_DEFAULT;  // may be overridden if target specifies MAG_*_ALIGN
 127
 128     magSensor_e magHardware = MAG_NONE;
 129
 130     busDevice_t *busdev = &dev->busdev;
 131
 132 #ifdef USE_MAG_DATA_READY_SIGNAL
 133     dev->magIntExtiTag = compassConfig()->interruptTag;
 134 #endif
 135
 136     switch (compassConfig()->mag_bustype) {
 137 #ifdef USE_I2C
 138     case BUSTYPE_I2C:
 139         busdev->bustype = BUSTYPE_I2C;
 140         busdev->busdev_u.i2c.device = I2C_CFG_TO_DEV(compassConfig()->mag_i2c_device);
 141         busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
 142         break;
 143 #endif
 144
 145 #ifdef USE_SPI
 146     case BUSTYPE_SPI:
 147         {
 148             SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(compassConfig()->mag_spi_device));
 149             if (!instance) {
 150                 return false;
 151             }
 152
 153             busdev->bustype = BUSTYPE_SPI;
 154             spiBusSetInstance(busdev, instance);
 155             busdev->busdev_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn);
 156         }
 157         break;
 158 #endif
 159
 160 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 161     case BUSTYPE_MPU_SLAVE:
 162         {
 163             if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
 164                 busdev->bustype = BUSTYPE_MPU_SLAVE;
 165                 busdev->busdev_u.mpuSlave.master = gyroSensorBus();
 166                 busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
 167             } else {
 168                 return false;
 169             }
 170         }
 171         break;
 172 #endif
 173
 174     default:
 175         return false;
 176     }
 177
 178     switch (compassConfig()->mag_hardware) {
 179     case MAG_DEFAULT:
 180         FALLTHROUGH;
 181
 182     case MAG_HMC5883:
 183 #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
 184         if (busdev->bustype == BUSTYPE_I2C) {
 185             busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
 186         }
 187
 188         if (hmc5883lDetect(dev)) {
 189 #ifdef MAG_HMC5883_ALIGN
 190             *alignment = MAG_HMC5883_ALIGN;
 191 #endif
 192             magHardware = MAG_HMC5883;
 193             break;
 194         }
 195 #endif
 196         FALLTHROUGH;
 197
 198     case MAG_LIS3MDL:
 199 #if defined(USE_MAG_LIS3MDL)
 200         if (busdev->bustype == BUSTYPE_I2C) {
 201             busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
 202         }
 203
 204         if (lis3mdlDetect(dev)) {
 205 #ifdef MAG_LIS3MDL_ALIGN
 206             *alignment = MAG_LIS3MDL_ALIGN;
 207 #endif
 208             magHardware = MAG_LIS3MDL;
 209             break;
 210         }
 211 #endif
 212         FALLTHROUGH;
 213
 214     case MAG_AK8975:
 215 #ifdef USE_MAG_AK8975
 216         if (busdev->bustype == BUSTYPE_I2C) {
 217             busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
 218         }
 219
 220         if (ak8975Detect(dev)) {
 221 #ifdef MAG_AK8975_ALIGN
 222             *alignment = MAG_AK8975_ALIGN;
 223 #endif
 224             magHardware = MAG_AK8975;
 225             break;
 226         }
 227 #endif
 228         FALLTHROUGH;
 229
 230     case MAG_AK8963:
 231 #if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963)
 232         if (busdev->bustype == BUSTYPE_I2C) {
 233             busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
 234         }
 235         if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
 236             dev->busdev.bustype = BUSTYPE_MPU_SLAVE;
 237             busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
 238             dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus();
 239         }
 240
 241         if (ak8963Detect(dev)) {
 242 #ifdef MAG_AK8963_ALIGN
 243             *alignment = MAG_AK8963_ALIGN;
 244 #endif
 245             magHardware = MAG_AK8963;
 246             break;
 247         }
 248 #endif
 249         FALLTHROUGH;
 250
 251     case MAG_QMC5883:
 252 #ifdef USE_MAG_QMC5883
 253         if (busdev->bustype == BUSTYPE_I2C) {
 254             busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
 255         }
 256
 257         if (qmc5883lDetect(dev)) {
 258 #ifdef MAG_QMC5883L_ALIGN
 259             *alignment = MAG_QMC5883L_ALIGN;
 260 #endif
 261             magHardware = MAG_QMC5883;
 262             break;
 263         }
 264 #endif
 265         FALLTHROUGH;
 266
 267     case MAG_NONE:
 268         magHardware = MAG_NONE;
 269         break;
 270     }
 271
 272     // MAG_MPU925X_AK8963 is an MPU925x configured as I2C passthrough to the built-in AK8963 magnetometer
 273     // Passthrough mode disables the gyro/acc part of the MPU, so we only want to detect this sensor if mag_hardware was explicitly set to MAG_MPU925X_AK8963
 274 #ifdef USE_MAG_MPU925X_AK8963
 275     if(compassConfig()->mag_hardware == MAG_MPU925X_AK8963){
 276         if (mpu925Xak8963CompassDetect(dev)) {
 277             magHardware = MAG_MPU925X_AK8963;
 278         } else {
 279             return false;
 280         }
 281     }
 282 #endif
 283
 284     if (magHardware == MAG_NONE) {
 285         return false;
 286     }
 287
 288     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
 289     sensorsSet(SENSOR_MAG);
 290     return true;
 291 }
 292 #else
 293 bool compassDetect(magDev_t *dev, sensor_align_e *alignment)
 294 {
 295     UNUSED(dev);
 296     UNUSED(alignment);
 297
 298     return false;
 299 }
 300 #endif // !SIMULATOR_BUILD
 301
 302 bool compassInit(void)
 303 {
 304     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
 305
 306     sensor_align_e alignment;
 307
 308     if (!compassDetect(&magDev, &alignment)) {
 309         return false;
 310     }
 311
 312     LED1_ON;
 313     magDev.init(&magDev);
 314     LED1_OFF;
 315     magInit = 1;
 316
 317     magDev.magAlignment = alignment;
 318
 319     if (compassConfig()->mag_alignment != ALIGN_DEFAULT) {
 320         magDev.magAlignment = compassConfig()->mag_alignment;
 321     }
 322
 323     buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);
 324
 325     return true;
 326 }
 327
 328 bool compassIsHealthy(void)
 329 {
 330     return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
 331 }
 332
 333 void compassStartCalibration(void)
 334 {
 335     tCal = micros();
 336     flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
 337     for (int axis = 0; axis < 3; axis++) {
 338         magZero->raw[axis] = 0;
 339         magZeroTempMin.raw[axis] = mag.magADC[axis];
 340         magZeroTempMax.raw[axis] = mag.magADC[axis];
 341     }
 342 }
 343
 344 bool compassIsCalibrationComplete(void)
 345 {
 346     return tCal == 0;
 347 }
 348
 349 void compassUpdate(timeUs_t currentTimeUs)
 350 {
 351
 352     magDev.read(&magDev, magADCRaw);
 353     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 354         mag.magADC[axis] = magADCRaw[axis];
 355     }
 356     if (magDev.magAlignment == ALIGN_CUSTOM) {
 357         alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
 358     } else {
 359         alignSensorViaRotation(mag.magADC, magDev.magAlignment);
 360     }
 361
 362     flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
 363     if (magInit) {              // we apply offset only once mag calibration is done
 364         mag.magADC[X] -= magZero->raw[X];
 365         mag.magADC[Y] -= magZero->raw[Y];
 366         mag.magADC[Z] -= magZero->raw[Z];
 367     }
 368
 369     if (tCal != 0) {
 370         if ((currentTimeUs - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
 371             LED0_TOGGLE;
 372             for (int axis = 0; axis < 3; axis++) {
 373                 if (mag.magADC[axis] < magZeroTempMin.raw[axis])
 374                     magZeroTempMin.raw[axis] = mag.magADC[axis];
 375                 if (mag.magADC[axis] > magZeroTempMax.raw[axis])
 376                     magZeroTempMax.raw[axis] = mag.magADC[axis];
 377             }
 378         } else {
 379             tCal = 0;
 380             for (int axis = 0; axis < 3; axis++) {
 381                 magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
 382             }
 383
 384             saveConfigAndNotify();
 385         }
 386     }
 387 }
 388 #endif // USE_MAG