src/main/sensors/compass.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23 #include "build/debug.h"
  24
  25 #include "common/axis.h"
  26 #include "common/maths.h"
  27 #include "common/utils.h"
  28
  29 #include "config/parameter_group.h"
  30 #include "config/parameter_group_ids.h"
  31
  32 #include "drivers/compass/compass.h"
  33 #include "drivers/compass/compass_ak8963.h"
  34 #include "drivers/compass/compass_ak8975.h"
  35 #include "drivers/compass/compass_fake.h"
  36 #include "drivers/compass/compass_hmc5883l.h"
  37 #include "drivers/compass/compass_mag3110.h"
  38 #include "drivers/compass/compass_ist8310.h"
  39 #include "drivers/compass/compass_ist8308.h"
  40 #include "drivers/compass/compass_qmc5883l.h"
  41 #include "drivers/compass/compass_mpu9250.h"
  42 #include "drivers/compass/compass_lis3mdl.h"
  43 #include "drivers/compass/compass_rm3100.h"
  44 #include "drivers/compass/compass_vcm5883.h"
  45 #include "drivers/compass/compass_mlx90393.h"
  46 #include "drivers/compass/compass_msp.h"
  47 #include "drivers/io.h"
  48 #include "drivers/light_led.h"
  49 #include "drivers/time.h"
  50
  51 #include "fc/config.h"
  52 #include "fc/runtime_config.h"
  53 #include "fc/settings.h"
  54
  55 #include "io/gps.h"
  56 #include "io/beeper.h"
  57
  58 #include "sensors/boardalignment.h"
  59 #include "sensors/compass.h"
  60 #include "sensors/gyro.h"
  61 #include "sensors/sensors.h"
  62
  63 mag_t mag;                   // mag access functions
  64
  65 #ifdef USE_MAG
  66
  67 PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 6);
  68
  69 PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
  70     .mag_align = SETTING_ALIGN_MAG_DEFAULT,
  71     .mag_hardware = SETTING_MAG_HARDWARE_DEFAULT,
  72     .mag_declination = SETTING_MAG_DECLINATION_DEFAULT,
  73 #ifdef USE_DUAL_MAG
  74     .mag_to_use = SETTING_MAG_TO_USE_DEFAULT,
  75 #endif
  76     .magCalibrationTimeLimit = SETTING_MAG_CALIBRATION_TIME_DEFAULT,
  77     .rollDeciDegrees = SETTING_ALIGN_MAG_ROLL_DEFAULT,
  78     .pitchDeciDegrees = SETTING_ALIGN_MAG_PITCH_DEFAULT,
  79     .yawDeciDegrees = SETTING_ALIGN_MAG_YAW_DEFAULT,
  80     .magGain = {SETTING_MAGGAIN_X_DEFAULT, SETTING_MAGGAIN_Y_DEFAULT, SETTING_MAGGAIN_Z_DEFAULT},
  81 );
  82
  83 static bool magUpdatedAtLeastOnce = false;
  84
  85 bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
  86 {
  87     magSensor_e magHardware = MAG_NONE;
  88     requestedSensors[SENSOR_INDEX_MAG] = magHardwareToUse;
  89
  90     dev->magAlign.useExternal = false;
  91     dev->magAlign.onBoard = ALIGN_DEFAULT;
  92
  93     switch (magHardwareToUse) {
  94     case MAG_AUTODETECT:
  95         FALLTHROUGH;
  96
  97     case MAG_QMC5883:
  98 #ifdef USE_MAG_QMC5883
  99         if (qmc5883Detect(dev)) {
 100             magHardware = MAG_QMC5883;
 101             break;
 102         }
 103 #endif
 104         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 105         if (magHardwareToUse != MAG_AUTODETECT) {
 106             break;
 107         }
 108         FALLTHROUGH;
 109
 110     case MAG_HMC5883:
 111 #ifdef USE_MAG_HMC5883
 112         if (hmc5883lDetect(dev)) {
 113             magHardware = MAG_HMC5883;
 114             break;
 115         }
 116 #endif
 117         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 118         if (magHardwareToUse != MAG_AUTODETECT) {
 119             break;
 120         }
 121         FALLTHROUGH;
 122
 123     case MAG_AK8975:
 124 #ifdef USE_MAG_AK8975
 125         if (ak8975Detect(dev)) {
 126             magHardware = MAG_AK8975;
 127             break;
 128         }
 129 #endif
 130         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 131         if (magHardwareToUse != MAG_AUTODETECT) {
 132             break;
 133         }
 134         FALLTHROUGH;
 135
 136     case MAG_AK8963:
 137 #ifdef USE_MAG_AK8963
 138         if (ak8963Detect(dev)) {
 139             magHardware = MAG_AK8963;
 140             break;
 141         }
 142 #endif
 143         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 144         if (magHardwareToUse != MAG_AUTODETECT) {
 145             break;
 146         }
 147         FALLTHROUGH;
 148
 149     case MAG_MAG3110:
 150 #ifdef USE_MAG_MAG3110
 151         if (mag3110detect(dev)) {
 152             magHardware = MAG_MAG3110;
 153             break;
 154         }
 155 #endif
 156         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 157         if (magHardwareToUse != MAG_AUTODETECT) {
 158             break;
 159         }
 160         FALLTHROUGH;
 161
 162     case MAG_IST8310:
 163 #ifdef USE_MAG_IST8310
 164         if (ist8310Detect(dev)) {
 165             magHardware = MAG_IST8310;
 166             break;
 167         }
 168 #endif
 169         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 170         if (magHardwareToUse != MAG_AUTODETECT) {
 171             break;
 172         }
 173         FALLTHROUGH;
 174
 175     case MAG_IST8308:
 176 #ifdef USE_MAG_IST8308
 177         if (ist8308Detect(dev)) {
 178             magHardware = MAG_IST8308;
 179             break;
 180         }
 181 #endif
 182         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 183         if (magHardwareToUse != MAG_AUTODETECT) {
 184             break;
 185         }
 186         FALLTHROUGH;
 187
 188     case MAG_MPU9250:
 189 #ifdef USE_MAG_MPU9250
 190         if (mpu9250CompassDetect(dev)) {
 191             magHardware = MAG_MPU9250;
 192             break;
 193         }
 194 #endif
 195         FALLTHROUGH;
 196
 197     case MAG_LIS3MDL:
 198 #ifdef USE_MAG_LIS3MDL
 199         if (lis3mdlDetect(dev)) {
 200             magHardware = MAG_LIS3MDL;
 201             break;
 202         }
 203 #endif
 204
 205         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 206         if (magHardwareToUse != MAG_AUTODETECT) {
 207             break;
 208         }
 209         FALLTHROUGH;
 210
 211     case MAG_MSP:
 212 #ifdef USE_MAG_MSP
 213         // Skip autodetection for MSP mag
 214         if (magHardwareToUse != MAG_AUTODETECT && mspMagDetect(dev)) {
 215             magHardware = MAG_MSP;
 216             break;
 217         }
 218 #endif
 219         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 220         if (magHardwareToUse != MAG_AUTODETECT) {
 221             break;
 222         }
 223         FALLTHROUGH;
 224
 225     case MAG_RM3100:
 226 #ifdef USE_MAG_RM3100
 227         if (rm3100MagDetect(dev)) {
 228             magHardware = MAG_RM3100;
 229             break;
 230         }
 231 #endif
 232         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 233         if (magHardwareToUse != MAG_AUTODETECT) {
 234             break;
 235         }
 236         FALLTHROUGH;
 237
 238     case MAG_VCM5883:
 239 #ifdef USE_MAG_VCM5883
 240         if (vcm5883Detect(dev)) {
 241             magHardware = MAG_VCM5883;
 242             break;
 243         }
 244 #endif
 245         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 246         if (magHardwareToUse != MAG_AUTODETECT) {
 247             break;
 248         }
 249         FALLTHROUGH;
 250
 251     case MAG_MLX90393:
 252 #ifdef USE_MAG_MLX90393
 253         if (mlx90393Detect(dev)) {
 254             magHardware = MAG_MLX90393;
 255             break;
 256         }
 257 #endif
 258         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 259         if (magHardwareToUse != MAG_AUTODETECT) {
 260             break;
 261         }
 262         FALLTHROUGH;
 263
 264     case MAG_FAKE:
 265 #ifdef USE_FAKE_MAG
 266         if (fakeMagDetect(dev)) {
 267             magHardware = MAG_FAKE;
 268             break;
 269         }
 270 #endif
 271         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 272         if (magHardwareToUse != MAG_AUTODETECT) {
 273             break;
 274         }
 275         FALLTHROUGH;
 276
 277     case MAG_NONE:
 278         magHardware = MAG_NONE;
 279         break;
 280     }
 281
 282     if (magHardware == MAG_NONE) {
 283         sensorsClear(SENSOR_MAG);
 284         return false;
 285     }
 286
 287     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
 288     sensorsSet(SENSOR_MAG);
 289     return true;
 290 }
 291
 292 bool compassInit(void)
 293 {
 294 #ifdef USE_DUAL_MAG
 295     mag.dev.magSensorToUse = compassConfig()->mag_to_use;
 296 #else
 297     mag.dev.magSensorToUse = 0;
 298 #endif
 299
 300     if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
 301         return false;
 302     }
 303     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
 304     LED1_ON;
 305     const bool ret = mag.dev.init(&mag.dev);
 306     LED1_OFF;
 307
 308     if (!ret) {
 309         sensorsClear(SENSOR_MAG);
 310     }
 311
 312     if (compassConfig()->rollDeciDegrees != 0 ||
 313         compassConfig()->pitchDeciDegrees != 0 ||
 314         compassConfig()->yawDeciDegrees != 0) {
 315
 316         // Externally aligned compass
 317         mag.dev.magAlign.useExternal = true;
 318
 319         fp_angles_t compassAngles = {
 320              .angles.roll = DECIDEGREES_TO_RADIANS(compassConfig()->rollDeciDegrees),
 321              .angles.pitch = DECIDEGREES_TO_RADIANS(compassConfig()->pitchDeciDegrees),
 322              .angles.yaw = DECIDEGREES_TO_RADIANS(compassConfig()->yawDeciDegrees),
 323         };
 324         rotationMatrixFromAngles(&mag.dev.magAlign.externalRotation, &compassAngles);
 325     } else {
 326         mag.dev.magAlign.useExternal = false;
 327         if (compassConfig()->mag_align != ALIGN_DEFAULT) {
 328             mag.dev.magAlign.onBoard = compassConfig()->mag_align;
 329         } else {
 330             mag.dev.magAlign.onBoard = CW270_DEG_FLIP;  // The most popular default is 270FLIP for external mags
 331         }
 332     }
 333
 334     return ret;
 335 }
 336
 337 bool compassIsHealthy(void)
 338 {
 339     return (mag.magADC[X] != 0) || (mag.magADC[Y] != 0) || (mag.magADC[Z] != 0);
 340 }
 341
 342 bool compassIsReady(void)
 343 {
 344     return magUpdatedAtLeastOnce;
 345 }
 346
 347 bool compassIsCalibrationComplete(void)
 348 {
 349     if (STATE(COMPASS_CALIBRATED)) {
 350         return true;
 351     }
 352     else {
 353         return false;
 354     }
 355 }
 356
 357 void compassUpdate(timeUs_t currentTimeUs)
 358 {
 359 #ifdef USE_SIMULATOR
 360         if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 361                 magUpdatedAtLeastOnce = true;
 362                 return;
 363         }
 364 #endif
 365     static sensorCalibrationState_t calState;
 366     static timeUs_t calStartedAt = 0;
 367     static int16_t magPrev[XYZ_AXIS_COUNT];
 368     static int magAxisDeviation[XYZ_AXIS_COUNT];
 369
 370 #if defined(SITL_BUILD)
 371     ENABLE_STATE(COMPASS_CALIBRATED);
 372 #else
 373     // Check magZero
 374     if (
 375         compassConfig()->magZero.raw[X] == 0 && compassConfig()->magZero.raw[Y] == 0 && compassConfig()->magZero.raw[Z] == 0 &&
 376         compassConfig()->magGain[X] == 1024 && compassConfig()->magGain[Y] == 1024 && compassConfig()->magGain[Z] == 1024
 377     ) {
 378         DISABLE_STATE(COMPASS_CALIBRATED);
 379     }
 380     else {
 381         ENABLE_STATE(COMPASS_CALIBRATED);
 382     }
 383 #endif
 384
 385     if (!mag.dev.read(&mag.dev)) {
 386         mag.magADC[X] = 0;
 387         mag.magADC[Y] = 0;
 388         mag.magADC[Z] = 0;
 389         return;
 390     }
 391
 392     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 393         mag.magADC[axis] = mag.dev.magADCRaw[axis];  // int32_t copy to work with
 394     }
 395
 396     if (STATE(CALIBRATE_MAG)) {
 397         calStartedAt = currentTimeUs;
 398
 399         for (int axis = 0; axis < 3; axis++) {
 400             compassConfigMutable()->magZero.raw[axis] = 0;
 401             compassConfigMutable()->magGain[axis] = 1024;
 402             magPrev[axis] = 0;
 403             magAxisDeviation[axis] = 0;  // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0
 404         }
 405
 406         beeper(BEEPER_ACTION_SUCCESS);
 407
 408         sensorCalibrationResetState(&calState);
 409         DISABLE_STATE(CALIBRATE_MAG);
 410     }
 411
 412     if (calStartedAt != 0) {
 413         if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
 414             LED0_TOGGLE;
 415
 416             float diffMag = 0;
 417             float avgMag = 0;
 418
 419             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 420                 diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]);
 421                 avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f;
 422
 423                 // Find the biggest sample deviation together with sample' sign
 424                 if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) {
 425                     magAxisDeviation[axis] = mag.magADC[axis];
 426                 }
 427
 428             }
 429
 430             // sqrtf(diffMag / avgMag) is a rough approximation of tangent of angle between magADC and magPrev. tan(8 deg) = 0.14
 431             if ((avgMag > 0.01f) && ((diffMag / avgMag) > (0.14f * 0.14f))) {
 432                 sensorCalibrationPushSampleForOffsetCalculation(&calState, mag.magADC);
 433
 434                 for (int axis = 0; axis < 3; axis++) {
 435                     magPrev[axis] = mag.magADC[axis];
 436                 }
 437             }
 438         } else {
 439             float magZerof[3];
 440             sensorCalibrationSolveForOffset(&calState, magZerof);
 441
 442             for (int axis = 0; axis < 3; axis++) {
 443                 compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]);
 444             }
 445
 446             /*
 447              * Scale calibration
 448              * We use max absolute value of each axis as scale calibration with constant 1024 as base
 449              * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted
 450              */
 451             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 452                 compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]);
 453             }
 454
 455             calStartedAt = 0;
 456             saveConfigAndNotify();
 457         }
 458     }
 459     else {
 460         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 461             mag.magADC[axis] = (mag.magADC[axis] - compassConfig()->magZero.raw[axis]) * 1024 / compassConfig()->magGain[axis];
 462         }
 463     }
 464
 465     if (mag.dev.magAlign.useExternal) {
 466         const fpVector3_t v = {
 467             .x = mag.magADC[X],
 468             .y = mag.magADC[Y],
 469             .z = mag.magADC[Z],
 470          };
 471
 472         fpVector3_t rotated;
 473
 474         rotationMatrixRotateVector(&rotated, &v, &mag.dev.magAlign.externalRotation);
 475         applyTailSitterAlignment(&rotated);
 476          mag.magADC[X] = rotated.x;
 477          mag.magADC[Y] = rotated.y;
 478          mag.magADC[Z] = rotated.z;
 479
 480     } else {
 481         // On-board compass
 482         applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign.onBoard);
 483         applyBoardAlignment(mag.magADC);
 484     }
 485
 486     magUpdatedAtLeastOnce = true;
 487 }
 488
 489 #endif