src/main/io/gps_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 #include <ctype.h>
  28 #include <string.h>
  29 #include <math.h>
  30
  31 #include "platform.h"
  32 #include "build/build_config.h"
  33
  34
  35 #if defined(USE_GPS_PROTO_MSP)
  36
  37 #include "build/debug.h"
  38
  39 #include "common/axis.h"
  40 #include "common/gps_conversion.h"
  41 #include "common/maths.h"
  42 #include "common/utils.h"
  43
  44 #include "drivers/serial.h"
  45 #include "drivers/time.h"
  46
  47 #include "fc/config.h"
  48 #include "fc/runtime_config.h"
  49
  50 #include "io/gps.h"
  51 #include "io/gps_private.h"
  52 #include "io/serial.h"
  53
  54 #include "msp/msp_protocol_v2_sensor_msg.h"
  55
  56 static bool newDataReady;
  57
  58 void gpsRestartMSP(void)
  59 {
  60     // NOP
  61 }
  62
  63 void gpsHandleMSP(void)
  64 {
  65     if (newDataReady) {
  66         gpsProcessNewSolutionData();
  67         newDataReady = false;
  68     }
  69 }
  70
  71 static uint8_t gpsMapFixType(uint8_t mspFixType)
  72 {
  73     if (mspFixType == 2)
  74         return GPS_FIX_2D;
  75     if (mspFixType >= 3)
  76         return GPS_FIX_3D;
  77     return GPS_NO_FIX;
  78 }
  79
  80 void mspGPSReceiveNewData(const uint8_t * bufferPtr)
  81 {
  82     const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
  83
  84     gpsSol.fixType   = gpsMapFixType(pkt->fixType);
  85     gpsSol.numSat    = pkt->satellitesInView;
  86     gpsSol.llh.lon   = pkt->longitude;
  87     gpsSol.llh.lat   = pkt->latitude;
  88     gpsSol.llh.alt   = pkt->mslAltitude;
  89     gpsSol.velNED[X] = pkt->nedVelNorth;
  90     gpsSol.velNED[Y] = pkt->nedVelEast;
  91     gpsSol.velNED[Z] = pkt->nedVelDown;
  92     gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
  93     gpsSol.groundCourse = pkt->groundCourse / 10;   // in deg * 10
  94     gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
  95     gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
  96     gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
  97     gpsSol.flags.validVelNE = true;
  98     gpsSol.flags.validVelD = true;
  99     gpsSol.flags.validEPE = true;
 100
 101     gpsSol.time.year   = pkt->year;
 102     gpsSol.time.month  = pkt->month;
 103     gpsSol.time.day    = pkt->day;
 104     gpsSol.time.hours  = pkt->hour;
 105     gpsSol.time.minutes = pkt->min;
 106     gpsSol.time.seconds = pkt->sec;
 107     gpsSol.time.millis  = 0;
 108
 109     gpsSol.flags.validTime = (pkt->fixType >= 3);
 110
 111     newDataReady = true;
 112 }
 113 #endif