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