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