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 "fc/rc_modes.h"
  49
  50 #include "sensors/sensors.h"
  51 #include "sensors/compass.h"
  52 #include "sensors/barometer.h"
  53 #include "sensors/pitotmeter.h"
  54
  55 #include "io/serial.h"
  56 #include "io/gps.h"
  57 #include "io/gps_private.h"
  58 #include "io/gps_ublox.h"
  59
  60 #include "navigation/navigation.h"
  61 #include "navigation/navigation_private.h"
  62
  63 #include "config/feature.h"
  64
  65 #include "fc/config.h"
  66 #include "fc/runtime_config.h"
  67 #include "fc/settings.h"
  68
  69 #include "flight/imu.h"
  70 #include "flight/wind_estimator.h"
  71 #include "flight/pid.h"
  72 #include "flight/mixer.h"
  73
  74 #include "programming/logic_condition.h"
  75
  76 typedef struct {
  77     bool                isDriverBased;
  78     portMode_t          portMode;           // Port mode RX/TX (only for serial based)
  79     void                (*restart)(void);   // Restart protocol driver thread
  80     void                (*protocol)(void);  // Process protocol driver thread
  81 } gpsProviderDescriptor_t;
  82
  83 // GPS public data
  84 gpsReceiverData_t gpsState;
  85 gpsStatistics_t   gpsStats;
  86
  87 //it is not safe to access gpsSolDRV which is filled in gps thread by driver.
  88 //gpsSolDRV can be accessed only after driver signalled that data is ready
  89 //we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features
  90 //and use it in the rest of code.
  91 gpsSolutionData_t gpsSolDRV;  //filled by driver
  92 gpsSolutionData_t gpsSol;     //used in the rest of the code
  93
  94 // Map gpsBaudRate_e index to baudRate_e
  95 baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 };
  96
  97 static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
  98     /* UBLOX binary */
  99 #ifdef USE_GPS_PROTO_UBLOX
 100     { false, MODE_RXTX, &gpsRestartUBLOX, &gpsHandleUBLOX },
 101 #else
 102     { false, 0, NULL, NULL },
 103 #endif
 104
 105     /* MSP GPS */
 106 #ifdef USE_GPS_PROTO_MSP
 107     { true, 0, &gpsRestartMSP, &gpsHandleMSP },
 108 #else
 109     { false, 0, NULL, NULL },
 110 #endif
 111
 112 #ifdef USE_GPS_FAKE
 113     {true, 0, &gpsFakeRestart, &gpsFakeHandle},
 114 #else
 115     { false, 0, NULL, NULL },
 116 #endif
 117
 118 };
 119
 120 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 5);
 121
 122 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
 123     .provider = SETTING_GPS_PROVIDER_DEFAULT,
 124     .sbasMode = SETTING_GPS_SBAS_MODE_DEFAULT,
 125     .autoConfig = SETTING_GPS_AUTO_CONFIG_DEFAULT,
 126     .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT,
 127     .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT,
 128     .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT,
 129     .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT,
 130     .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT,
 131     .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT,
 132     .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT,
 133     .autoBaudMax = SETTING_GPS_AUTO_BAUD_MAX_SUPPORTED_DEFAULT
 134 );
 135
 136 int gpsBaudRateToInt(gpsBaudRate_e baudrate)
 137 {
 138     switch(baudrate)
 139     {
 140         case GPS_BAUDRATE_115200:
 141             return 115200;
 142         case GPS_BAUDRATE_57600:
 143             return 57600;
 144         case GPS_BAUDRATE_38400:
 145             return 38400;
 146         case GPS_BAUDRATE_19200:
 147             return 19200;
 148         case GPS_BAUDRATE_9600:
 149             return 9600;
 150         case GPS_BAUDRATE_230400:
 151             return 230400;
 152         case GPS_BAUDRATE_460800:
 153             return 460800;
 154         case GPS_BAUDRATE_921600:
 155             return 921600;
 156         default:
 157             return 0;
 158     }
 159 }
 160
 161 int getGpsBaudrate(void)
 162 {
 163     return gpsBaudRateToInt(gpsState.baudrateIndex);
 164 }
 165
 166 const char *getGpsHwVersion(void)
 167 {
 168     switch(gpsState.hwVersion)
 169     {
 170         case UBX_HW_VERSION_UBLOX5:
 171             return "UBLOX5";
 172         case UBX_HW_VERSION_UBLOX6:
 173             return "UBLOX6";
 174         case UBX_HW_VERSION_UBLOX7:
 175             return "UBLOX7";
 176         case UBX_HW_VERSION_UBLOX8:
 177             return "UBLOX8";
 178         case UBX_HW_VERSION_UBLOX9:
 179             return "UBLOX9";
 180         case UBX_HW_VERSION_UBLOX10:
 181             return "UBLOX10";
 182         default:
 183             return "Unknown";
 184     }
 185 }
 186
 187 uint8_t getGpsProtoMajorVersion(void)
 188 {
 189     return gpsState.swVersionMajor;
 190 }
 191
 192 uint8_t getGpsProtoMinorVersion(void)
 193 {
 194     return gpsState.swVersionMinor;
 195 }
 196
 197 void gpsSetState(gpsState_e state)
 198 {
 199     gpsState.state = state;
 200     gpsState.lastStateSwitchMs = millis();
 201 }
 202
 203 static void gpsUpdateTime(void)
 204 {
 205     if (!rtcHasTime() && gpsSol.flags.validTime && gpsSol.time.year != 0) {
 206         rtcSetDateTime(&gpsSol.time);
 207     }
 208 }
 209
 210 void gpsSetProtocolTimeout(timeMs_t timeoutMs)
 211 {
 212     gpsState.lastLastMessageMs = gpsState.lastMessageMs;
 213     gpsState.lastMessageMs = millis();
 214     gpsState.timeoutMs = timeoutMs;
 215 }
 216
 217 #ifdef USE_GPS_FIX_ESTIMATION
 218 bool canEstimateGPSFix(void)
 219 {
 220 #if defined(USE_GPS) && defined(USE_BARO)
 221
 222     //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because:
 223     //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once
 224     //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix
 225     return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) &&
 226         sensors(SENSOR_BARO) && baroIsHealthy() &&
 227         ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME);
 228
 229 #else
 230     return false;
 231 #endif
 232 }
 233 #endif
 234
 235 #ifdef USE_GPS_FIX_ESTIMATION
 236 void processDisableGPSFix(void)
 237 {
 238     static int32_t last_lat = 0;
 239     static int32_t last_lon = 0;
 240     static int32_t last_alt = 0;
 241
 242     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) {
 243         gpsSol.fixType = GPS_NO_FIX;
 244         gpsSol.hdop = 9999;
 245         gpsSol.numSat = 0;
 246
 247         gpsSol.flags.validVelNE = false;
 248         gpsSol.flags.validVelD = false;
 249         gpsSol.flags.validEPE = false;
 250         gpsSol.flags.validTime = false;
 251
 252         //freeze coordinates
 253         gpsSol.llh.lat = last_lat;
 254         gpsSol.llh.lon = last_lon;
 255         gpsSol.llh.alt = last_alt;
 256     } else {
 257         last_lat = gpsSol.llh.lat;
 258         last_lon = gpsSol.llh.lon;
 259         last_alt = gpsSol.llh.alt;
 260     }
 261 }
 262 #endif
 263
 264 #ifdef USE_GPS_FIX_ESTIMATION
 265 //called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition"
 266 void updateEstimatedGPSFix(void)
 267 {
 268     static uint32_t lastUpdateMs = 0;
 269     static int32_t estimated_lat = 0;
 270     static int32_t estimated_lon = 0;
 271     static int32_t estimated_alt = 0;
 272
 273     uint32_t t = millis();
 274     int32_t dt = t - lastUpdateMs;
 275     lastUpdateMs = t;
 276
 277     bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats;
 278
 279     if (sensorHasFix || !canEstimateGPSFix()) {
 280         estimated_lat = gpsSol.llh.lat;
 281         estimated_lon = gpsSol.llh.lon;
 282         estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
 283         return;
 284     }
 285
 286     gpsSol.fixType = GPS_FIX_3D;
 287     gpsSol.hdop = 99;
 288     gpsSol.numSat = 99;
 289
 290     gpsSol.eph = 100;
 291     gpsSol.epv = 100;
 292
 293     gpsSol.flags.validVelNE = true;
 294     gpsSol.flags.validVelD = false;  //do not provide velocity.z
 295     gpsSol.flags.validEPE = true;
 296     gpsSol.flags.validTime = false;
 297
 298     float speed = pidProfile()->fixedWingReferenceAirspeed;
 299
 300 #ifdef USE_PITOT
 301     if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
 302         speed = getAirspeedEstimate();
 303     }
 304 #endif
 305
 306     float velX = rMat[0][0] * speed;
 307     float velY = -rMat[1][0] * speed;
 308     // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame
 309
 310     if (isEstimatedWindSpeedValid()) {
 311         velX += getEstimatedWindSpeed(X);
 312         velY += getEstimatedWindSpeed(Y);
 313     }
 314     // here (velX, velY) is estimated horizontal speed with wind influence = ground speed
 315
 316     if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent(false) == 0))) {
 317         velX = 0;
 318         velY = 0;
 319     }
 320
 321     estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) );
 322     estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) );
 323     estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
 324
 325     gpsSol.llh.lat = estimated_lat;
 326     gpsSol.llh.lon = estimated_lon;
 327     gpsSol.llh.alt = estimated_alt;
 328
 329     gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY);
 330
 331     float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction
 332     if (groundCourse < 0) {
 333         groundCourse += 2 * M_PIf;
 334     }
 335     gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse);
 336
 337     gpsSol.velNED[X] = (int16_t)(velX);
 338     gpsSol.velNED[Y] = (int16_t)(velY);
 339     gpsSol.velNED[Z] = 0;
 340 }
 341 #endif
 342
 343
 344 void gpsProcessNewDriverData(void)
 345 {
 346     gpsSol = gpsSolDRV;
 347
 348 #ifdef USE_GPS_FIX_ESTIMATION
 349     processDisableGPSFix();
 350     updateEstimatedGPSFix();
 351 #endif
 352 }
 353
 354 //called after:
 355 //1)driver copies gpsSolDRV to gpsSol
 356 //2)gpsSol is processed by "Disable GPS logical switch"
 357 //3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix()
 358 //On GPS sensor timeout - called after updateEstimatedGPSFix()
 359 void gpsProcessNewSolutionData(bool timeout)
 360 {
 361 #ifdef USE_GPS_FIX_ESTIMATION
 362     if ( gpsSol.numSat == 99 ) {
 363         ENABLE_STATE(GPS_ESTIMATED_FIX);
 364         DISABLE_STATE(GPS_FIX);
 365     } else {
 366         DISABLE_STATE(GPS_ESTIMATED_FIX);
 367 #endif
 368
 369         // Set GPS fix flag only if we have 3D fix
 370         if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
 371             ENABLE_STATE(GPS_FIX);
 372         }
 373         else {
 374             /* When no fix available - reset flags as well */
 375             gpsSol.flags.validVelNE = false;
 376             gpsSol.flags.validVelD = false;
 377             gpsSol.flags.validEPE = false;
 378             DISABLE_STATE(GPS_FIX);
 379         }
 380 #ifdef USE_GPS_FIX_ESTIMATION
 381     }
 382 #endif
 383
 384     if (!timeout) {
 385         // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix)
 386         sensorsSet(SENSOR_GPS);
 387     }
 388
 389     // Pass on GPS update to NAV and IMU
 390     onNewGPSData();
 391
 392     // Update time
 393     gpsUpdateTime();
 394
 395     // Update timeout
 396     gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 397
 398     // Update statistics
 399     gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs;
 400     gpsSol.flags.hasNewData = true;
 401
 402     // Toggle heartbeat
 403     gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 404 }
 405
 406 static void gpsResetSolution(gpsSolutionData_t* gpsSol)
 407 {
 408     gpsSol->eph = 9999;
 409     gpsSol->epv = 9999;
 410     gpsSol->numSat = 0;
 411     gpsSol->hdop = 9999;
 412
 413     gpsSol->fixType = GPS_NO_FIX;
 414
 415     gpsSol->flags.validVelNE = false;
 416     gpsSol->flags.validVelD = false;
 417     gpsSol->flags.validEPE = false;
 418     gpsSol->flags.validTime = false;
 419 }
 420
 421 void gpsTryEstimateOnTimeout(void)
 422 {
 423     gpsResetSolution(&gpsSol);
 424     DISABLE_STATE(GPS_FIX);
 425
 426 #ifdef USE_GPS_FIX_ESTIMATION
 427     if ( canEstimateGPSFix() ) {
 428         updateEstimatedGPSFix();
 429
 430         if (gpsSol.fixType == GPS_FIX_3D) {  //estimation kicked in
 431             gpsProcessNewSolutionData(true);
 432         }
 433     }
 434 #endif
 435 }
 436
 437 void gpsPreInit(void)
 438 {
 439     // Make sure gpsProvider is known when gpsMagDetect is called
 440     gpsState.gpsConfig = gpsConfig();
 441     gpsState.baseTimeoutMs = GPS_TIMEOUT;
 442 }
 443
 444 void gpsInit(void)
 445 {
 446     gpsState.serialConfig = serialConfig();
 447     gpsState.gpsConfig = gpsConfig();
 448
 449     gpsStats.errors = 0;
 450     gpsStats.timeouts = 0;
 451
 452     // Reset solution, timeout and prepare to start
 453     gpsResetSolution(&gpsSolDRV);
 454     gpsResetSolution(&gpsSol);
 455     gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 456     gpsSetState(GPS_UNKNOWN);
 457
 458     // If given GPS provider has protocol() function not defined - we can't use it
 459     if (!gpsProviders[gpsState.gpsConfig->provider].protocol) {
 460         featureClear(FEATURE_GPS);
 461         return;
 462     }
 463
 464     // Shortcut for driver-based GPS (MSP)
 465     if (gpsProviders[gpsState.gpsConfig->provider].isDriverBased) {
 466         gpsSetState(GPS_INITIALIZING);
 467         return;
 468     }
 469
 470     serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 471     if (!gpsPortConfig) {
 472         featureClear(FEATURE_GPS);
 473         return;
 474     }
 475
 476     // Start with baud rate index as configured for serial port
 477     int baudrateIndex;
 478     gpsState.baudrateIndex = GPS_BAUDRATE_115200;
 479     for (baudrateIndex = 0, gpsState.baudrateIndex = 0; baudrateIndex < GPS_BAUDRATE_COUNT; baudrateIndex++) {
 480         if (gpsToSerialBaudRate[baudrateIndex] == gpsPortConfig->gps_baudrateIndex) {
 481             gpsState.baudrateIndex = baudrateIndex;
 482             break;
 483         }
 484     }
 485
 486     // Start with the same baud for autodetection
 487     gpsState.autoBaudrateIndex = gpsState.baudrateIndex;
 488
 489     // Open serial port
 490     portMode_t mode = gpsProviders[gpsState.gpsConfig->provider].portMode;
 491     gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]], mode, SERIAL_NOT_INVERTED);
 492
 493     // Check if we have a serial port opened
 494     if (!gpsState.gpsPort) {
 495         featureClear(FEATURE_GPS);
 496         return;
 497     }
 498
 499     gpsSetState(GPS_INITIALIZING);
 500 }
 501
 502 uint16_t gpsConstrainEPE(uint32_t epe)
 503 {
 504     return (epe > 9999) ? 9999 : epe; // max 99.99m error
 505 }
 506
 507 uint16_t gpsConstrainHDOP(uint32_t hdop)
 508 {
 509     return (hdop > 9999) ? 9999 : hdop; // max 99.99m error
 510 }
 511
 512 bool gpsUpdate(void)
 513 {
 514     // Sanity check
 515     if (!feature(FEATURE_GPS)) {
 516         sensorsClear(SENSOR_GPS);
 517         DISABLE_STATE(GPS_FIX);
 518         return false;
 519     }
 520
 521     /* Extra delay for at least 2 seconds after booting to give GPS time to initialise */
 522     if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) {
 523         sensorsClear(SENSOR_GPS);
 524         DISABLE_STATE(GPS_FIX);
 525         return false;
 526     }
 527
 528 #ifdef USE_SIMULATOR
 529     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 530         if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) {
 531             gpsSetState(GPS_LOST_COMMUNICATION);
 532             sensorsClear(SENSOR_GPS);
 533             gpsStats.timeouts = 5;
 534             gpsTryEstimateOnTimeout();
 535         } else {
 536             gpsSetState(GPS_RUNNING);
 537             sensorsSet(SENSOR_GPS);
 538         }
 539         bool res = gpsSol.flags.hasNewData;
 540         gpsSol.flags.hasNewData = false;
 541         return res;
 542     }
 543 #endif
 544
 545     switch (gpsState.state) {
 546     default:
 547     case GPS_INITIALIZING:
 548         // Wait for GPS_INIT_DELAY before starting the GPS protocol thread
 549         if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
 550             // Reset internals
 551             DISABLE_STATE(GPS_FIX);
 552
 553             // Reset solution
 554             gpsResetSolution(&gpsSolDRV);
 555
 556             // Call GPS protocol reset handler
 557             gpsProviders[gpsState.gpsConfig->provider].restart();
 558
 559             // Switch to GPS_RUNNING state (mind the timeout)
 560             gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 561             gpsSetState(GPS_RUNNING);
 562         }
 563         break;
 564
 565     case GPS_RUNNING:
 566         // Call GPS protocol thread
 567         gpsProviders[gpsState.gpsConfig->provider].protocol();
 568
 569         // Check for GPS timeout
 570         if ((millis() - gpsState.lastMessageMs) > gpsState.baseTimeoutMs) {
 571             sensorsClear(SENSOR_GPS);
 572             DISABLE_STATE(GPS_FIX);
 573             gpsSol.fixType = GPS_NO_FIX;
 574             gpsSetState(GPS_LOST_COMMUNICATION);
 575         }
 576         break;
 577
 578     case GPS_LOST_COMMUNICATION:
 579         gpsStats.timeouts++;
 580         gpsSetState(GPS_INITIALIZING);
 581         break;
 582     }
 583
 584     if ( !sensors(SENSOR_GPS) ) {
 585         gpsTryEstimateOnTimeout();
 586     }
 587
 588     bool res = gpsSol.flags.hasNewData;
 589     gpsSol.flags.hasNewData = false;
 590     return res;
 591 }
 592
 593 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
 594 {
 595     waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
 596     waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
 597
 598     if (!(gpsState.gpsPort->mode & MODE_TX))
 599     serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);
 600
 601     LED0_OFF;
 602     LED1_OFF;
 603
 604     char c;
 605     while (1) {
 606         if (serialRxBytesWaiting(gpsState.gpsPort)) {
 607             LED0_ON;
 608             c = serialRead(gpsState.gpsPort);
 609             serialWrite(gpsPassthroughPort, c);
 610             LED0_OFF;
 611         }
 612         if (serialRxBytesWaiting(gpsPassthroughPort)) {
 613             LED1_ON;
 614             c = serialRead(gpsPassthroughPort);
 615             serialWrite(gpsState.gpsPort, c);
 616             LED1_OFF;
 617         }
 618     }
 619 }
 620
 621 void updateGpsIndicator(timeUs_t currentTimeUs)
 622 {
 623     static timeUs_t GPSLEDTime;
 624     if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat>= 5)) {
 625         GPSLEDTime = currentTimeUs + 150000;
 626         LED1_TOGGLE;
 627     }
 628 }
 629
 630 bool isGPSHealthy(void)
 631 {
 632     return true;
 633 }
 634
 635 bool isGPSHeadingValid(void)
 636 {
 637     return ((STATE(GPS_FIX) && gpsSol.numSat >= 6)
 638 #ifdef USE_GPS_FIX_ESTIMATION
 639         || STATE(GPS_ESTIMATED_FIX)
 640 #endif
 641         ) && gpsSol.groundSpeed >= 300;
 642 }
 643
 644 #endif