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