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_i2c_busdev.h"
  36 #include "drivers/bus_spi.h"
  37 #include "drivers/compass/compass.h"
  38 #include "drivers/compass/compass_ak8975.h"
  39 #include "drivers/compass/compass_ak8963.h"
  40 #include "drivers/compass/compass_virtual.h"
  41 #include "drivers/compass/compass_hmc5883l.h"
  42 #include "drivers/compass/compass_lis3mdl.h"
  43 #include "drivers/compass/compass_mpu925x_ak8963.h"
  44 #include "drivers/compass/compass_qmc5883l.h"
  45 #include "drivers/compass/compass_ist8310.h"
  46
  47 #include "drivers/io.h"
  48 #include "drivers/light_led.h"
  49 #include "drivers/time.h"
  50
  51 #include "fc/runtime_config.h"
  52
  53 #include "pg/pg.h"
  54 #include "pg/pg_ids.h"
  55
  56 #include "scheduler/scheduler.h"
  57
  58 #include "sensors/boardalignment.h"
  59 #include "sensors/gyro.h"
  60 #include "sensors/gyro_init.h"
  61 #include "sensors/sensors.h"
  62
  63 #include "compass.h"
  64
  65 static timeUs_t tCal = 0;
  66 static flightDynamicsTrims_t magZeroTempMin;
  67 static flightDynamicsTrims_t magZeroTempMax;
  68
  69 magDev_t magDev;
  70 mag_t mag;
  71
  72 PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
  73
  74 #define COMPASS_READ_US 500 // Allow 500us for compass data read
  75
  76 void pgResetFn_compassConfig(compassConfig_t *compassConfig)
  77 {
  78     compassConfig->mag_alignment = ALIGN_DEFAULT;
  79     memset(&compassConfig->mag_customAlignment, 0x00, sizeof(compassConfig->mag_customAlignment));
  80     compassConfig->mag_hardware = MAG_DEFAULT;
  81
  82 #ifndef MAG_I2C_ADDRESS
  83 #define MAG_I2C_ADDRESS 0
  84 #endif
  85
  86 // Generate a reasonable default for backward compatibility
  87 // Strategy is
  88 // 1. If SPI device is defined, it will take precedence, assuming it's onboard.
  89 // 2. I2C devices are will be handled by address = 0 (per device default).
  90 // 3. Slave I2C device on SPI gyro
  91
  92 #if defined(USE_SPI_MAG) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963))
  93     compassConfig->mag_busType = BUS_TYPE_SPI;
  94     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE));
  95     compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN);
  96     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
  97     compassConfig->mag_i2c_address = 0;
  98 #elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || defined(USE_MAG_IST8310) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
  99     compassConfig->mag_busType = BUS_TYPE_I2C;
 100     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE);
 101     compassConfig->mag_i2c_address = MAG_I2C_ADDRESS;
 102     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 103     compassConfig->mag_spi_csn = IO_TAG_NONE;
 104 #elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 105     compassConfig->mag_busType = BUS_TYPE_MPU_SLAVE;
 106     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 107     compassConfig->mag_i2c_address = MAG_I2C_ADDRESS;
 108     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 109     compassConfig->mag_spi_csn = IO_TAG_NONE;
 110 #else
 111     compassConfig->mag_hardware = MAG_NONE;
 112     compassConfig->mag_busType = BUS_TYPE_NONE;
 113     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
 114     compassConfig->mag_i2c_address = 0;
 115     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
 116     compassConfig->mag_spi_csn = IO_TAG_NONE;
 117 #endif
 118     compassConfig->interruptTag = IO_TAG(MAG_INT_EXTI);
 119 }
 120
 121 static int16_t magADCRaw[XYZ_AXIS_COUNT];
 122 static int16_t magADCRawPrevious[XYZ_AXIS_COUNT];
 123 static uint8_t magInit = 0;
 124
 125 void compassPreInit(void)
 126 {
 127 #ifdef USE_SPI
 128     if (compassConfig()->mag_busType == BUS_TYPE_SPI) {
 129         spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1);
 130     }
 131 #endif
 132 }
 133
 134 #if !defined(SIMULATOR_BUILD)
 135 bool compassDetect(magDev_t *magDev, uint8_t *alignment)
 136 {
 137     *alignment = ALIGN_DEFAULT;  // may be overridden if target specifies MAG_*_ALIGN
 138
 139     magSensor_e magHardware = MAG_NONE;
 140
 141     extDevice_t *dev = &magDev->dev;
 142     // Associate magnetometer bus with its device
 143     dev->bus = &magDev->bus;
 144
 145 #ifdef USE_MAG_DATA_READY_SIGNAL
 146     magDev->magIntExtiTag = compassConfig()->interruptTag;
 147 #endif
 148
 149     switch (compassConfig()->mag_busType) {
 150 #ifdef USE_I2C
 151     case BUS_TYPE_I2C:
 152         i2cBusSetInstance(dev, compassConfig()->mag_i2c_device);
 153         dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 154         break;
 155 #endif
 156
 157 #ifdef USE_SPI
 158     case BUS_TYPE_SPI:
 159         {
 160             if (!spiSetBusInstance(dev, compassConfig()->mag_spi_device)) {
 161                 return false;
 162             }
 163
 164             dev->busType_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn);
 165         }
 166         break;
 167 #endif
 168
 169 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 170     case BUS_TYPE_MPU_SLAVE:
 171         {
 172             if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
 173                 extDevice_t *masterDev = &gyroActiveDev()->dev;
 174
 175                 dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 176                 dev->bus->busType = BUS_TYPE_MPU_SLAVE;
 177                 dev->bus->busType_u.mpuSlave.master = masterDev;
 178             } else {
 179                 return false;
 180             }
 181         }
 182         break;
 183 #endif
 184
 185     default:
 186         return false;
 187     }
 188
 189     switch (compassConfig()->mag_hardware) {
 190     case MAG_DEFAULT:
 191         FALLTHROUGH;
 192
 193     case MAG_HMC5883:
 194 #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
 195         if (dev->bus->busType == BUS_TYPE_I2C) {
 196             dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 197         }
 198
 199         if (hmc5883lDetect(magDev)) {
 200 #ifdef MAG_HMC5883_ALIGN
 201             *alignment = MAG_HMC5883_ALIGN;
 202 #endif
 203             magHardware = MAG_HMC5883;
 204             break;
 205         }
 206 #endif
 207         FALLTHROUGH;
 208
 209     case MAG_LIS3MDL:
 210 #if defined(USE_MAG_LIS3MDL)
 211         if (dev->bus->busType == BUS_TYPE_I2C) {
 212             dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 213         }
 214
 215         if (lis3mdlDetect(magDev)) {
 216 #ifdef MAG_LIS3MDL_ALIGN
 217             *alignment = MAG_LIS3MDL_ALIGN;
 218 #endif
 219             magHardware = MAG_LIS3MDL;
 220             break;
 221         }
 222 #endif
 223         FALLTHROUGH;
 224
 225     case MAG_AK8975:
 226 #ifdef USE_MAG_AK8975
 227         if (dev->bus->busType == BUS_TYPE_I2C) {
 228             dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 229         }
 230
 231         if (ak8975Detect(magDev)) {
 232 #ifdef MAG_AK8975_ALIGN
 233             *alignment = MAG_AK8975_ALIGN;
 234 #endif
 235             magHardware = MAG_AK8975;
 236             break;
 237         }
 238 #endif
 239         FALLTHROUGH;
 240
 241     case MAG_AK8963:
 242 #if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963)
 243         if (dev->bus->busType == BUS_TYPE_I2C) {
 244             dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 245         }
 246         if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
 247             dev->bus->busType = BUS_TYPE_MPU_SLAVE;
 248             dev->busType_u.mpuSlave.address = compassConfig()->mag_i2c_address;
 249             dev->bus->busType_u.mpuSlave.master = &gyroActiveDev()->dev;
 250         }
 251
 252         if (ak8963Detect(magDev)) {
 253 #ifdef MAG_AK8963_ALIGN
 254             *alignment = MAG_AK8963_ALIGN;
 255 #endif
 256             magHardware = MAG_AK8963;
 257             break;
 258         }
 259 #endif
 260         FALLTHROUGH;
 261
 262     case MAG_QMC5883:
 263 #ifdef USE_MAG_QMC5883
 264         if (dev->bus->busType == BUS_TYPE_I2C) {
 265             dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 266         }
 267
 268         if (qmc5883lDetect(magDev)) {
 269 #ifdef MAG_QMC5883L_ALIGN
 270             *alignment = MAG_QMC5883L_ALIGN;
 271 #endif
 272             magHardware = MAG_QMC5883;
 273             break;
 274         }
 275 #endif
 276         FALLTHROUGH;
 277
 278     case MAG_IST8310:
 279 #ifdef USE_MAG_IST8310
 280         if (dev->bus->busType == BUS_TYPE_I2C) {
 281             dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
 282         }
 283
 284         if (ist8310Detect(magDev)) {
 285 #ifdef MAG_IST8310_ALIGN
 286             *alignment = MAG_IST8310_ALIGN;
 287 #endif
 288             magHardware = MAG_IST8310;
 289             break;
 290         }
 291 #endif
 292         FALLTHROUGH;
 293
 294     case MAG_NONE:
 295         magHardware = MAG_NONE;
 296         break;
 297     }
 298
 299     // MAG_MPU925X_AK8963 is an MPU925x configured as I2C passthrough to the built-in AK8963 magnetometer
 300     // 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
 301 #ifdef USE_MAG_MPU925X_AK8963
 302     if (compassConfig()->mag_hardware == MAG_MPU925X_AK8963){
 303         if (mpu925Xak8963CompassDetect(magDev)) {
 304             magHardware = MAG_MPU925X_AK8963;
 305         } else {
 306             return false;
 307         }
 308     }
 309 #endif
 310
 311     if (magHardware == MAG_NONE) {
 312         return false;
 313     }
 314
 315     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
 316     sensorsSet(SENSOR_MAG);
 317     return true;
 318 }
 319 #else
 320 bool compassDetect(magDev_t *dev, sensor_align_e *alignment)
 321 {
 322     UNUSED(dev);
 323     UNUSED(alignment);
 324
 325     return false;
 326 }
 327 #endif // !SIMULATOR_BUILD
 328
 329 bool compassInit(void)
 330 {
 331     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
 332
 333     sensor_align_e alignment;
 334
 335     if (!compassDetect(&magDev, &alignment)) {
 336         return false;
 337     }
 338
 339     LED1_ON;
 340     magDev.init(&magDev);
 341     LED1_OFF;
 342     magInit = 1;
 343
 344     magDev.magAlignment = alignment;
 345
 346     if (compassConfig()->mag_alignment != ALIGN_DEFAULT) {
 347         magDev.magAlignment = compassConfig()->mag_alignment;
 348     }
 349
 350     buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);
 351
 352     return true;
 353 }
 354
 355 bool compassIsHealthy(void)
 356 {
 357     return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
 358 }
 359
 360 void compassStartCalibration(void)
 361 {
 362     tCal = micros();
 363     flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
 364     for (int axis = 0; axis < 3; axis++) {
 365         magZero->raw[axis] = 0;
 366         magZeroTempMin.raw[axis] = mag.magADC[axis];
 367         magZeroTempMax.raw[axis] = mag.magADC[axis];
 368     }
 369 }
 370
 371 bool compassIsCalibrationComplete(void)
 372 {
 373     return tCal == 0;
 374 }
 375
 376 uint32_t compassUpdate(timeUs_t currentTimeUs)
 377 {
 378     static uint8_t busyCount = 0;
 379     uint32_t nextPeriod = COMPASS_READ_US;
 380
 381     if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
 382         // No action was taken as the read has not completed
 383         schedulerIgnoreTaskStateTime();
 384
 385         busyCount++;
 386
 387         return nextPeriod; // Wait COMPASS_READ_US between states
 388     }
 389
 390     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 391         if (magADCRaw[axis] != magADCRawPrevious[axis]) {
 392             // this test, and the isNewMagADCFlag itself, is only needed if we calculate magYaw in imu.c
 393             mag.isNewMagADCFlag = true;
 394         }
 395         mag.magADC[axis] = magADCRaw[axis];
 396     }
 397
 398     if (magDev.magAlignment == ALIGN_CUSTOM) {
 399         alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
 400     } else {
 401         alignSensorViaRotation(mag.magADC, magDev.magAlignment);
 402     }
 403
 404     flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
 405     if (magInit) {              // we apply offset only once mag calibration is done
 406         mag.magADC[X] -= magZero->raw[X];
 407         mag.magADC[Y] -= magZero->raw[Y];
 408         mag.magADC[Z] -= magZero->raw[Z];
 409     }
 410
 411     if (tCal != 0) {
 412         if ((currentTimeUs - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
 413             LED0_TOGGLE;
 414             for (int axis = 0; axis < 3; axis++) {
 415                 if (mag.magADC[axis] < magZeroTempMin.raw[axis])
 416                     magZeroTempMin.raw[axis] = mag.magADC[axis];
 417                 if (mag.magADC[axis] > magZeroTempMax.raw[axis])
 418                     magZeroTempMax.raw[axis] = mag.magADC[axis];
 419             }
 420         } else {
 421             tCal = 0;
 422             for (int axis = 0; axis < 3; axis++) {
 423                 magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
 424             }
 425
 426             saveConfigAndNotify();
 427         }
 428     }
 429
 430     nextPeriod = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ) - COMPASS_READ_US * busyCount;
 431
 432     // Reset the busy count
 433     busyCount = 0;
 434
 435     return nextPeriod;
 436 }
 437 #endif // USE_MAG