src/main/io/gps.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 <ctype.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24 #include "build/build_config.h"
  25
  26
  27 #ifdef USE_GPS
  28
  29 #include "build/debug.h"
  30
  31 #include "common/maths.h"
  32 #include "common/axis.h"
  33 #include "common/utils.h"
  34
  35 #include "config/parameter_group.h"
  36 #include "config/parameter_group_ids.h"
  37
  38 #include "drivers/compass/compass.h"
  39 #include "drivers/light_led.h"
  40 #include "drivers/serial.h"
  41 #include "drivers/system.h"
  42 #include "drivers/time.h"
  43
  44 #if defined(USE_FAKE_GPS)
  45 #include "fc/runtime_config.h"
  46 #endif
  47
  48 #include "sensors/sensors.h"
  49 #include "sensors/compass.h"
  50
  51 #include "io/serial.h"
  52 #include "io/gps.h"
  53 #include "io/gps_private.h"
  54
  55 #include "navigation/navigation.h"
  56
  57 #include "config/feature.h"
  58
  59 #include "fc/config.h"
  60 #include "fc/runtime_config.h"
  61
  62 typedef struct {
  63     portMode_t          portMode;           // Port mode RX/TX (only for serial based)
  64     bool                hasCompass;         // Has a compass (NAZA)
  65     void                (*restart)(void);   // Restart protocol driver thread
  66     void                (*protocol)(void);  // Process protocol driver thread
  67 } gpsProviderDescriptor_t;
  68
  69 // GPS public data
  70 gpsReceiverData_t gpsState;
  71 gpsStatistics_t   gpsStats;
  72 gpsSolutionData_t gpsSol;
  73
  74 // Map gpsBaudRate_e index to baudRate_e
  75 baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600 };
  76
  77 static gpsProviderDescriptor_t  gpsProviders[GPS_PROVIDER_COUNT] = {
  78     /* NMEA GPS */
  79 #ifdef USE_GPS_PROTO_NMEA
  80     { MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
  81 #else
  82     { 0, false,  NULL, NULL },
  83 #endif
  84
  85     /* UBLOX binary */
  86 #ifdef USE_GPS_PROTO_UBLOX
  87     { MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
  88 #else
  89     { 0, false,  NULL, NULL },
  90 #endif
  91
  92     /* Stub */
  93     { 0, false,  NULL, NULL },
  94
  95     /* NAZA GPS module */
  96 #ifdef USE_GPS_PROTO_NAZA
  97     { MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
  98 #else
  99     { 0, false,  NULL, NULL },
 100 #endif
 101
 102     /* UBLOX7PLUS binary */
 103 #ifdef USE_GPS_PROTO_UBLOX
 104     { MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
 105 #else
 106     { 0, false,  NULL, NULL },
 107 #endif
 108
 109     /* MTK GPS */
 110 #ifdef USE_GPS_PROTO_MTK
 111     { MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
 112 #else
 113     { 0, false,  NULL, NULL },
 114 #endif
 115
 116 };
 117
 118 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
 119
 120 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
 121     .provider = GPS_UBLOX,
 122     .sbasMode = SBAS_NONE,
 123     .autoConfig = GPS_AUTOCONFIG_ON,
 124     .autoBaud = GPS_AUTOBAUD_ON,
 125     .dynModel = GPS_DYNMODEL_AIR_1G,
 126     .gpsMinSats = 6,
 127     .ubloxUseGalileo = false
 128 );
 129
 130 void gpsSetState(gpsState_e state)
 131 {
 132     gpsState.state = state;
 133     gpsState.lastStateSwitchMs = millis();
 134 }
 135
 136 static void gpsUpdateTime(void)
 137 {
 138     if (!rtcHasTime() && gpsSol.flags.validTime) {
 139         rtcSetDateTime(&gpsSol.time);
 140     }
 141 }
 142
 143 void gpsSetProtocolTimeout(timeMs_t timeoutMs)
 144 {
 145     gpsState.lastLastMessageMs = gpsState.lastMessageMs;
 146     gpsState.lastMessageMs = millis();
 147     gpsState.timeoutMs = timeoutMs;
 148 }
 149
 150 void gpsProcessNewSolutionData(void)
 151 {
 152     // Set GPS fix flag only if we have 3D fix
 153     if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
 154         ENABLE_STATE(GPS_FIX);
 155     }
 156     else {
 157         /* When no fix available - reset flags as well */
 158         gpsSol.flags.validVelNE = 0;
 159         gpsSol.flags.validVelD = 0;
 160         gpsSol.flags.validEPE = 0;
 161         DISABLE_STATE(GPS_FIX);
 162     }
 163
 164     // Set sensor as ready and available
 165     sensorsSet(SENSOR_GPS);
 166
 167     // Pass on GPS update to NAV and IMU
 168     onNewGPSData();
 169
 170     // Update time
 171     gpsUpdateTime();
 172
 173     // Update timeout
 174     gpsSetProtocolTimeout(GPS_TIMEOUT);
 175
 176     // Update statistics
 177     gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs;
 178     gpsSol.flags.hasNewData = true;
 179
 180     // Toggle heartbeat
 181     gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 182 }
 183
 184 static void gpsResetSolution(void)
 185 {
 186     gpsSol.eph = 9999;
 187     gpsSol.epv = 9999;
 188
 189     gpsSol.fixType = GPS_NO_FIX;
 190
 191     gpsSol.flags.validVelNE = 0;
 192     gpsSol.flags.validVelD = 0;
 193     gpsSol.flags.validMag = 0;
 194     gpsSol.flags.validEPE = 0;
 195     gpsSol.flags.validTime = 0;
 196 }
 197
 198 void gpsPreInit(void)
 199 {
 200     // Make sure gpsProvider is known when gpsMagDetect is called
 201     gpsState.gpsConfig = gpsConfig();
 202 }
 203
 204 void gpsInit(void)
 205 {
 206     gpsState.serialConfig = serialConfig();
 207     gpsState.gpsConfig = gpsConfig();
 208
 209     gpsStats.errors = 0;
 210     gpsStats.timeouts = 0;
 211
 212     // Reset solution, timeout and prepare to start
 213     gpsResetSolution();
 214     gpsSetProtocolTimeout(GPS_TIMEOUT);
 215     gpsSetState(GPS_UNKNOWN);
 216
 217     // If given GPS provider has protocol() function not defined - we can't use it
 218     if (!gpsProviders[gpsState.gpsConfig->provider].protocol) {
 219         featureClear(FEATURE_GPS);
 220         return;
 221     }
 222
 223     serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 224     if (!gpsPortConfig) {
 225         featureClear(FEATURE_GPS);
 226         return;
 227     }
 228
 229     // Start with baud rate index as configured for serial port
 230     int baudrateIndex;
 231     for (gpsState.baudrateIndex = 0, baudrateIndex = 0; baudrateIndex < GPS_BAUDRATE_COUNT; baudrateIndex++) {
 232         if (gpsToSerialBaudRate[baudrateIndex] == gpsPortConfig->gps_baudrateIndex) {
 233             gpsState.baudrateIndex = baudrateIndex;
 234             break;
 235         }
 236     }
 237
 238     // Start with the same baud for autodetection
 239     gpsState.autoBaudrateIndex = gpsState.baudrateIndex;
 240
 241     // Open serial port
 242     portMode_t mode = gpsProviders[gpsState.gpsConfig->provider].portMode;
 243     gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]], mode, SERIAL_NOT_INVERTED);
 244
 245     // Check if we have a serial port opened
 246     if (!gpsState.gpsPort) {
 247         featureClear(FEATURE_GPS);
 248         return;
 249     }
 250
 251     gpsSetState(GPS_INITIALIZING);
 252 }
 253
 254 #ifdef USE_FAKE_GPS
 255 static bool gpsFakeGPSUpdate(void)
 256 {
 257 #define FAKE_GPS_INITIAL_LAT 509102311
 258 #define FAKE_GPS_INITIAL_LON -15349744
 259 #define FAKE_GPS_GROUND_ARMED_SPEED 350 // In cm/s
 260 #define FAKE_GPS_GROUND_UNARMED_SPEED 0
 261 #define FAKE_GPS_GROUND_COURSE_DECIDEGREES 300 //30deg
 262
 263     // Each degree in latitude corresponds to 111km.
 264     // Each degree in longitude at the equator is 111km,
 265     // going down to zero as latitude gets close to 90º.
 266     // We approximate it linearly.
 267
 268     static int32_t lat = FAKE_GPS_INITIAL_LAT;
 269     static int32_t lon = FAKE_GPS_INITIAL_LON;
 270
 271     timeMs_t now = millis();
 272     uint32_t delta = now - gpsState.lastMessageMs;
 273     if (delta > 100) {
 274         int32_t speed = ARMING_FLAG(ARMED) ? FAKE_GPS_GROUND_ARMED_SPEED : FAKE_GPS_GROUND_UNARMED_SPEED;
 275         int32_t cmDelta = speed * (delta / 1000.0f);
 276         int32_t latCmDelta = cmDelta * cos_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 277         int32_t lonCmDelta = cmDelta * sin_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 278         int32_t latDelta = ceilf((float)latCmDelta / (111 * 1000 * 100 / 1e7));
 279         int32_t lonDelta = ceilf((float)lonCmDelta / (111 * 1000 * 100 / 1e7));
 280         if (speed > 0 && latDelta == 0 && lonDelta == 0) {
 281             return false;
 282         }
 283         lat += latDelta;
 284         lon += lonDelta;
 285         gpsSol.fixType = GPS_FIX_3D;
 286         gpsSol.numSat = 6;
 287         gpsSol.llh.lat = lat;
 288         gpsSol.llh.lon = lon;
 289         gpsSol.llh.alt = 0;
 290         gpsSol.groundSpeed = speed;
 291         gpsSol.groundCourse = FAKE_GPS_GROUND_COURSE_DECIDEGREES;
 292         gpsSol.velNED[X] = speed * cos_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 293         gpsSol.velNED[Y] = speed * sin_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 294         gpsSol.velNED[Z] = 0;
 295         gpsSol.flags.validVelNE = 1;
 296         gpsSol.flags.validVelD = 1;
 297         gpsSol.flags.validEPE = 1;
 298         gpsSol.flags.validTime = 1;
 299         gpsSol.eph = 100;
 300         gpsSol.epv = 100;
 301         gpsSol.time.year = 1983;
 302         gpsSol.time.month = 1;
 303         gpsSol.time.day = 1;
 304         gpsSol.time.hours = 3;
 305         gpsSol.time.minutes = 15;
 306         gpsSol.time.seconds = 42;
 307
 308         ENABLE_STATE(GPS_FIX);
 309         sensorsSet(SENSOR_GPS);
 310         gpsUpdateTime();
 311         onNewGPSData();
 312
 313         gpsSetProtocolTimeout(GPS_TIMEOUT);
 314
 315         gpsSetState(GPS_RUNNING);
 316         return true;
 317     }
 318     return false;
 319 }
 320 #endif
 321
 322 uint16_t gpsConstrainEPE(uint32_t epe)
 323 {
 324     return (epe > 9999) ? 9999 : epe; // max 99.99m error
 325 }
 326
 327 uint16_t gpsConstrainHDOP(uint32_t hdop)
 328 {
 329     return (hdop > 9999) ? 9999 : hdop; // max 99.99m error
 330 }
 331
 332 bool gpsUpdate(void)
 333 {
 334     // Sanity check
 335     if (!feature(FEATURE_GPS)) {
 336         sensorsClear(SENSOR_GPS);
 337         DISABLE_STATE(GPS_FIX);
 338         return false;
 339     }
 340
 341     /* Extra delay for at least 2 seconds after booting to give GPS time to initialise */
 342     if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) {
 343         sensorsClear(SENSOR_GPS);
 344         DISABLE_STATE(GPS_FIX);
 345         return false;
 346     }
 347
 348 #ifdef USE_FAKE_GPS
 349     return gpsFakeGPSUpdate();
 350 #else
 351
 352     // Assume that we don't have new data this run
 353     gpsSol.flags.hasNewData = false;
 354
 355     switch (gpsState.state) {
 356     default:
 357     case GPS_INITIALIZING:
 358         // Wait for GPS_INIT_DELAY before starting the GPS protocol thread
 359         if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
 360             // Reset internals
 361             DISABLE_STATE(GPS_FIX);
 362             gpsSol.fixType = GPS_NO_FIX;
 363
 364             // Reset solution
 365             gpsResetSolution();
 366
 367             // Call GPS protocol reset handler
 368             gpsProviders[gpsState.gpsConfig->provider].restart();
 369
 370             // Switch to GPS_RUNNING state (mind the timeout)
 371             gpsSetProtocolTimeout(GPS_TIMEOUT);
 372             gpsSetState(GPS_RUNNING);
 373         }
 374         break;
 375
 376     case GPS_RUNNING:
 377         // Call GPS protocol thread
 378         gpsProviders[gpsState.gpsConfig->provider].protocol();
 379
 380         // Check for GPS timeout
 381         if ((millis() - gpsState.lastMessageMs) > GPS_TIMEOUT) {
 382             sensorsClear(SENSOR_GPS);
 383             DISABLE_STATE(GPS_FIX);
 384             gpsSol.fixType = GPS_NO_FIX;
 385             gpsSetState(GPS_LOST_COMMUNICATION);
 386         }
 387         break;
 388
 389     case GPS_LOST_COMMUNICATION:
 390         gpsStats.timeouts++;
 391         gpsSetState(GPS_INITIALIZING);
 392         break;
 393     }
 394
 395     return gpsSol.flags.hasNewData;
 396 #endif
 397 }
 398
 399 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
 400 {
 401     waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
 402     waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
 403
 404     if (!(gpsState.gpsPort->mode & MODE_TX))
 405     serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);
 406
 407     LED0_OFF;
 408     LED1_OFF;
 409
 410     char c;
 411     while (1) {
 412         if (serialRxBytesWaiting(gpsState.gpsPort)) {
 413             LED0_ON;
 414             c = serialRead(gpsState.gpsPort);
 415             serialWrite(gpsPassthroughPort, c);
 416             LED0_OFF;
 417         }
 418         if (serialRxBytesWaiting(gpsPassthroughPort)) {
 419             LED1_ON;
 420             c = serialRead(gpsPassthroughPort);
 421             serialWrite(gpsState.gpsPort, c);
 422             LED1_OFF;
 423         }
 424     }
 425 }
 426
 427 void updateGpsIndicator(timeUs_t currentTimeUs)
 428 {
 429     static timeUs_t GPSLEDTime;
 430     if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat>= 5)) {
 431         GPSLEDTime = currentTimeUs + 150000;
 432         LED1_TOGGLE;
 433     }
 434 }
 435
 436 /* Support for built-in magnetometer accessible via the native GPS protocol (i.e. NAZA) */
 437 bool gpsMagInit(magDev_t *magDev)
 438 {
 439     UNUSED(magDev);
 440     return true;
 441 }
 442
 443 bool gpsMagRead(magDev_t *magDev)
 444 {
 445     magDev->magADCRaw[X] = gpsSol.magData[0];
 446     magDev->magADCRaw[Y] = gpsSol.magData[1];
 447     magDev->magADCRaw[Z] = gpsSol.magData[2];
 448     return gpsSol.flags.validMag;
 449 }
 450
 451 bool gpsMagDetect(magDev_t *mag)
 452 {
 453     if (!(feature(FEATURE_GPS) && gpsProviders[gpsState.gpsConfig->provider].hasCompass)) {
 454         return false;
 455     }
 456
 457     if (!gpsProviders[gpsState.gpsConfig->provider].protocol || !findSerialPortConfig(FUNCTION_GPS)) {
 458         return false;
 459     }
 460
 461     mag->init = gpsMagInit;
 462     mag->read = gpsMagRead;
 463     return true;
 464 }
 465
 466 bool isGPSHealthy(void)
 467 {
 468     return true;
 469 }
 470
 471 bool isGPSHeadingValid(void)
 472 {
 473     return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
 474 }
 475
 476 #endif