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
  24 #include "common/axis.h"
  25 #include "common/maths.h"
  26
  27 #include "drivers/sensor.h"
  28 #include "drivers/compass.h"
  29 #include "drivers/compass_hmc5883l.h"
  30 #include "drivers/gpio.h"
  31 #include "drivers/light_led.h"
  32
  33 #include "sensors/boardalignment.h"
  34 #include "config/runtime_config.h"
  35 #include "config/config.h"
  36
  37 #include "sensors/sensors.h"
  38 #include "sensors/compass.h"
  39
  40 #ifdef NAZE
  41 #include "hardware_revision.h"
  42 #endif
  43
  44 mag_t mag;                   // mag access functions
  45
  46 extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
  47
  48 int16_t magADC[XYZ_AXIS_COUNT];
  49 sensor_align_e magAlign = 0;
  50 #ifdef MAG
  51 static uint8_t magInit = 0;
  52 static uint8_t magUpdatedAtLeastOnce = 0;
  53
  54 void compassInit(void)
  55 {
  56     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
  57     LED1_ON;
  58     mag.init();
  59     LED1_OFF;
  60     magInit = 1;
  61 }
  62
  63 bool isCompassReady(void)
  64 {
  65     return magUpdatedAtLeastOnce;
  66 }
  67
  68 static sensorCalibrationState_t calState;
  69
  70 #define COMPASS_UPDATE_FREQUENCY_10HZ   (1000 * 100)
  71
  72 void updateCompass(flightDynamicsTrims_t *magZero)
  73 {
  74     static uint32_t nextUpdateAt, calStartedAt = 0;
  75     static int16_t magPrev[XYZ_AXIS_COUNT];
  76     uint32_t axis;
  77
  78     if ((int32_t)(currentTime - nextUpdateAt) < 0)
  79         return;
  80
  81     nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ;
  82
  83     mag.read(magADC);
  84
  85     if (STATE(CALIBRATE_MAG)) {
  86         calStartedAt = nextUpdateAt;
  87
  88         for (axis = 0; axis < 3; axis++) {
  89             magZero->raw[axis] = 0;
  90             magPrev[axis] = 0;
  91         }
  92
  93         sensorCalibrationResetState(&calState);
  94         DISABLE_STATE(CALIBRATE_MAG);
  95     }
  96
  97     if (magInit) {              // we apply offset only once mag calibration is done
  98         magADC[X] -= magZero->raw[X];
  99         magADC[Y] -= magZero->raw[Y];
 100         magADC[Z] -= magZero->raw[Z];
 101     }
 102
 103     if (calStartedAt != 0) {
 104         if ((nextUpdateAt - calStartedAt) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
 105             LED0_TOGGLE;
 106
 107             float diffMag = 0;
 108             float avgMag = 0;
 109
 110             for (axis = 0; axis < 3; axis++) {
 111                 diffMag += (magADC[axis] - magPrev[axis]) * (magADC[axis] - magPrev[axis]);
 112                 avgMag += (magADC[axis] + magPrev[axis]) * (magADC[axis] + magPrev[axis]) / 4.0f;
 113             }
 114
 115             // sqrtf(diffMag / avgMag) is a rough approximation of tangent of angle between magADC and magPrev. tan(8 deg) = 0.14
 116             if ((avgMag > 0.01f) && ((diffMag / avgMag) > (0.14f * 0.14f))) {
 117                 sensorCalibrationPushSampleForOffsetCalculation(&calState, magADC);
 118
 119                 for (axis = 0; axis < 3; axis++) {
 120                     magPrev[axis] = magADC[axis];
 121                 }
 122             }
 123         } else {
 124             float magZerof[3];
 125             sensorCalibrationSolveForOffset(&calState, magZerof);
 126
 127             for (axis = 0; axis < 3; axis++) {
 128                 magZero->raw[axis] = lrintf(magZerof[axis]);
 129             }
 130
 131             calStartedAt = 0;
 132             persistentFlagSet(FLAG_MAG_CALIBRATION_DONE);
 133             saveConfigAndNotify();
 134         }
 135     }
 136
 137     alignSensors(magADC, magADC, magAlign);
 138
 139     magUpdatedAtLeastOnce = 1;
 140 }
 141 #endif