src/main/drivers/compass/compass_msp.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_MAG_MSP)
  31
  32 #include "build/build_config.h"
  33
  34 #include "common/axis.h"
  35 #include "common/utils.h"
  36 #include "common/time.h"
  37
  38 #include "drivers/time.h"
  39 #include "drivers/compass/compass.h"
  40 #include "drivers/compass/compass_msp.h"
  41
  42 #include "sensors/boardalignment.h"
  43
  44 #include "msp/msp_protocol_v2_sensor_msg.h"
  45
  46 #define MSP_MAG_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
  47
  48 static float mspMagData[XYZ_AXIS_COUNT];
  49 static timeMs_t mspMagLastUpdateMs;
  50
  51 static bool mspMagInit(magDev_t *magDev)
  52 {
  53     UNUSED(magDev);
  54     mspMagData[X] = 0;
  55     mspMagData[Y] = 0;
  56     mspMagData[Z] = 0;
  57     mspMagLastUpdateMs = 0;
  58     return true;
  59 }
  60
  61 void mspMagReceiveNewData(uint8_t * bufferPtr)
  62 {
  63     const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr;
  64
  65     mspMagData[X] = (float) pkt->magX;
  66     mspMagData[Y] = (float) pkt->magY;
  67     mspMagData[Z] = (float) pkt->magZ;
  68
  69     applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP);
  70
  71     mspMagLastUpdateMs = millis();
  72 }
  73
  74 static bool mspMagRead(magDev_t *magDev)
  75 {
  76     UNUSED(magDev);
  77
  78     if ((millis() - mspMagLastUpdateMs) > MSP_MAG_TIMEOUT_MS) {
  79         return false;
  80     }
  81
  82     magDev->magADCRaw[X] = mspMagData[X];
  83     magDev->magADCRaw[Y] = mspMagData[Y];
  84     magDev->magADCRaw[Z] = mspMagData[Z];
  85
  86     return true;
  87 }
  88
  89 bool mspMagDetect(magDev_t *mag)
  90 {
  91     mag->init = mspMagInit;
  92     mag->read = mspMagRead;
  93     return true;
  94 }
  95
  96 #endif