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 GPS
  28
  29 #include "build/debug.h"
  30
  31
  32 #include "common/maths.h"
  33 #include "common/axis.h"
  34 #include "common/utils.h"
  35
  36 #include "drivers/system.h"
  37 #include "drivers/serial.h"
  38 #include "drivers/serial_uart.h"
  39 #include "drivers/gpio.h"
  40 #include "drivers/light_led.h"
  41 #include "drivers/sensor.h"
  42 #include "drivers/compass.h"
  43
  44 #include "sensors/sensors.h"
  45 #include "sensors/compass.h"
  46
  47 #include "io/serial.h"
  48 #include "io/display.h"
  49 #include "io/gps.h"
  50 #include "io/gps_private.h"
  51
  52 #include "flight/navigation_rewrite.h"
  53
  54 #include "config/config.h"
  55 #include "fc/runtime_config.h"
  56
  57 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2000 ms)
  58 #define GPS_TIMEOUT             (2000)
  59 #define GPS_BAUD_CHANGE_DELAY   (200)
  60 #define GPS_INIT_DELAY          (500)
  61 #define GPS_BUS_INIT_DELAY      (500)
  62 #define GPS_BOOT_DELAY          (2000)
  63
  64 typedef enum {
  65     GPS_TYPE_NA,        // Not available
  66     GPS_TYPE_SERIAL,    // Serial connection (UART)
  67     GPS_TYPE_BUS        // Bus connection (I2C/SPI)
  68 } gpsProviderType_e;
  69
  70 typedef struct {
  71     gpsProviderType_e   type;
  72     portMode_t          portMode;      // Port mode RX/TX (only for serial based)
  73     bool                hasCompass;    // Has a compass (NAZA)
  74     bool (*detect)(void);
  75     bool (*read)(void);
  76 } gpsProviderDescriptor_t;
  77
  78 // GPS public data
  79 gpsReceiverData_t gpsState;
  80 gpsStatistics_t   gpsStats;
  81 gpsSolutionData_t gpsSol;
  82
  83 // Map gpsBaudRate_e index to baudRate_e
  84 baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600 };
  85
  86 static gpsProviderDescriptor_t  gpsProviders[GPS_PROVIDER_COUNT] = {
  87     /* NMEA GPS */
  88 #ifdef GPS_PROTO_NMEA
  89     { GPS_TYPE_SERIAL, MODE_RX, false, NULL, &gpsHandleNMEA },
  90 #else
  91     { GPS_TYPE_NA, 0, false,  NULL, NULL },
  92 #endif
  93
  94     /* UBLOX binary */
  95 #ifdef GPS_PROTO_UBLOX
  96     { GPS_TYPE_SERIAL, MODE_RXTX, false,  NULL, &gpsHandleUBLOX },
  97 #else
  98     { GPS_TYPE_NA, 0, false,  NULL, NULL },
  99 #endif
 100
 101     /* MultiWii I2C-NAV module */
 102 #ifdef GPS_PROTO_I2C_NAV
 103     { GPS_TYPE_BUS, 0, false, &gpsDetectI2CNAV, &gpsHandleI2CNAV },
 104 #else
 105     { GPS_TYPE_NA, 0, false,  NULL, NULL },
 106 #endif
 107
 108     /* NAZA GPS module */
 109 #ifdef GPS_PROTO_NAZA
 110     { GPS_TYPE_SERIAL, MODE_RX, true,  NULL, &gpsHandleNAZA },
 111 #else
 112     { GPS_TYPE_NA, 0, false,  NULL, NULL },
 113 #endif
 114 };
 115
 116 void gpsSetState(gpsState_e state)
 117 {
 118     gpsState.state = state;
 119     gpsState.lastStateSwitchMs = millis();
 120 }
 121
 122 static void gpsHandleProtocol(void)
 123 {
 124     bool newDataReceived = false;
 125
 126     // Call protocol-specific code
 127     if (gpsProviders[gpsState.gpsConfig->provider].read) {
 128         newDataReceived = gpsProviders[gpsState.gpsConfig->provider].read();
 129     }
 130
 131     // Received new update for solution data
 132     if (newDataReceived) {
 133         // Set GPS fix flag only if we have 3D fix
 134         if (gpsSol.fixType == GPS_FIX_3D) {
 135             ENABLE_STATE(GPS_FIX);
 136         }
 137         else {
 138             DISABLE_STATE(GPS_FIX);
 139         }
 140
 141         // Update GPS coordinates etc
 142         sensorsSet(SENSOR_GPS);
 143         onNewGPSData();
 144
 145         // Update timeout
 146         gpsState.lastLastMessageMs = gpsState.lastMessageMs;
 147         gpsState.lastMessageMs = millis();
 148
 149         // Update statistics
 150         gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs;
 151     }
 152 }
 153
 154 static void gpsResetSolution(void)
 155 {
 156     gpsSol.eph = 9999;
 157     gpsSol.epv = 9999;
 158
 159     gpsSol.fixType = GPS_NO_FIX;
 160
 161     gpsSol.flags.validVelNE = 0;
 162     gpsSol.flags.validVelD = 0;
 163     gpsSol.flags.validMag = 0;
 164 }
 165
 166 void gpsPreInit(gpsConfig_t *initialGpsConfig)
 167 {
 168     // Make sure gpsProvider is known when gpsMagDetect is called
 169     gpsState.gpsConfig = initialGpsConfig;
 170 }
 171
 172 void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
 173 {
 174     gpsState.serialConfig = initialSerialConfig;
 175     gpsState.gpsConfig = initialGpsConfig;
 176     gpsState.baudrateIndex = 0;
 177
 178     gpsStats.errors = 0;
 179     gpsStats.timeouts = 0;
 180
 181     gpsResetSolution();
 182
 183     // init gpsData structure. if we're not actually enabled, don't bother doing anything else
 184     gpsState.autoConfigStep = 0;
 185     gpsState.lastMessageMs = millis();
 186     gpsSetState(GPS_UNKNOWN);
 187
 188     if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) {
 189         gpsSetState(GPS_INITIALIZING);
 190         return;
 191     }
 192
 193     if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) {
 194         serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 195         if (!gpsPortConfig) {
 196             featureClear(FEATURE_GPS);
 197         }
 198         else {
 199             while (gpsToSerialBaudRate[gpsState.baudrateIndex] != gpsPortConfig->gps_baudrateIndex) {
 200                 gpsState.baudrateIndex++;
 201                 if (gpsState.baudrateIndex >= GPS_BAUDRATE_COUNT) {
 202                     gpsState.baudrateIndex = 0;
 203                     break;
 204                 }
 205             }
 206
 207             portMode_t mode = gpsProviders[gpsState.gpsConfig->provider].portMode;
 208
 209             // no callback - buffer will be consumed in gpsThread()
 210             gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, gpsToSerialBaudRate[gpsState.baudrateIndex], mode, SERIAL_NOT_INVERTED);
 211
 212             if (!gpsState.gpsPort) {
 213                 featureClear(FEATURE_GPS);
 214             }
 215             else {
 216                 gpsSetState(GPS_INITIALIZING);
 217                 return;
 218             }
 219         }
 220     }
 221 }
 222
 223 #ifdef USE_FAKE_GPS
 224 static void gpsFakeGPSUpdate(void)
 225 {
 226     if (millis() - gpsState.lastMessageMs > 100) {
 227         gpsSol.fixType = GPS_FIX_3D;
 228         gpsSol.numSat = 6;
 229         gpsSol.llh.lat = 509102311;
 230         gpsSol.llh.lon = -15349744;
 231         gpsSol.llh.alt = 0;
 232         gpsSol.groundSpeed = 0;
 233         gpsSol.groundCourse = 0;
 234         gpsSol.velNED[X] = 0;
 235         gpsSol.velNED[Y] = 0;
 236         gpsSol.velNED[Z] = 0;
 237         gpsSol.flags.validVelNE = 1;
 238         gpsSol.flags.validVelD = 1;
 239         gpsSol.flags.validEPE = 1;
 240         gpsSol.eph = 100;
 241         gpsSol.epv = 100;
 242
 243         ENABLE_STATE(GPS_FIX);
 244         sensorsSet(SENSOR_GPS);
 245         onNewGPSData();
 246
 247         gpsState.lastLastMessageMs = gpsState.lastMessageMs;
 248         gpsState.lastMessageMs = millis();
 249
 250         gpsSetState(GPS_RECEIVING_DATA);
 251     }
 252 }
 253 #endif
 254
 255 // Finish baud rate change sequence - wait for TX buffer to empty and switch to the desired port speed
 256 void gpsFinalizeChangeBaud(void)
 257 {
 258     if ((gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) && (gpsState.gpsPort != NULL)) {
 259         // Wait for GPS_INIT_DELAY before switching to required baud rate
 260         if ((millis() - gpsState.lastStateSwitchMs) >= GPS_BAUD_CHANGE_DELAY && isSerialTransmitBufferEmpty(gpsState.gpsPort)) {
 261             // Switch to required serial port baud
 262             serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
 263             gpsState.lastMessageMs = millis();
 264             gpsSetState(GPS_CHECK_VERSION);
 265         }
 266     }
 267 }
 268
 269 uint16_t gpsConstrainEPE(uint32_t epe)
 270 {
 271     return (epe > 99999) ? 9999 : epe; // max 99.99m error
 272 }
 273
 274 uint16_t gpsConstrainHDOP(uint32_t hdop)
 275 {
 276     return (hdop > 99999) ? 9999 : hdop; // max 99.99m error
 277 }
 278
 279 void gpsThread(void)
 280 {
 281     /* Extra delay for at least 2 seconds after booting to give GPS time to initialise */
 282     if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) {
 283         sensorsClear(SENSOR_GPS);
 284         DISABLE_STATE(GPS_FIX);
 285         return;
 286     }
 287
 288 #ifdef USE_FAKE_GPS
 289     gpsFakeGPSUpdate();
 290 #else
 291
 292     // Serial-based GPS
 293     if ((gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) && (gpsState.gpsPort != NULL)) {
 294         switch (gpsState.state) {
 295         default:
 296         case GPS_INITIALIZING:
 297             if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
 298                 // Reset internals
 299                 DISABLE_STATE(GPS_FIX);
 300                 gpsSol.fixType = GPS_NO_FIX;
 301
 302                 gpsState.hwVersion = 0;
 303                 gpsState.autoConfigStep = 0;
 304                 gpsState.autoConfigPosition = 0;
 305                 gpsState.autoBaudrateIndex = 0;
 306
 307                 // Reset solution
 308                 gpsResetSolution();
 309
 310                 // Call protocol handler - switch to next state is done there
 311                 gpsHandleProtocol();
 312             }
 313             break;
 314
 315         case GPS_CHANGE_BAUD:
 316             // Call protocol handler - switch to next state is done there
 317             gpsHandleProtocol();
 318             break;
 319
 320         case GPS_CHECK_VERSION:
 321         case GPS_CONFIGURE:
 322         case GPS_RECEIVING_DATA:
 323             gpsHandleProtocol();
 324             if ((millis() - gpsState.lastMessageMs) > GPS_TIMEOUT) {
 325                 // Check for GPS timeout
 326                 sensorsClear(SENSOR_GPS);
 327                 DISABLE_STATE(GPS_FIX);
 328                 gpsSol.fixType = GPS_NO_FIX;
 329
 330                 gpsSetState(GPS_LOST_COMMUNICATION);
 331             }
 332             break;
 333
 334         case GPS_LOST_COMMUNICATION:
 335             gpsStats.timeouts++;
 336             // Handle autobaud - switch to next port baud rate
 337             if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
 338                 gpsState.baudrateIndex++;
 339                 gpsState.baudrateIndex %= GPS_BAUDRATE_COUNT;
 340             }
 341             gpsSetState(GPS_INITIALIZING);
 342             break;
 343         }
 344     }
 345     // Driver-based GPS (I2C)
 346     else if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) {
 347         switch (gpsState.state) {
 348         default:
 349         case GPS_INITIALIZING:
 350             // Detect GPS unit
 351             if ((millis() - gpsState.lastStateSwitchMs) >= GPS_BUS_INIT_DELAY) {
 352                 gpsResetSolution();
 353
 354                 if (gpsProviders[gpsState.gpsConfig->provider].detect && gpsProviders[gpsState.gpsConfig->provider].detect()) {
 355                     gpsState.hwVersion = 0;
 356                     gpsState.autoConfigStep = 0;
 357                     gpsState.autoConfigPosition = 0;
 358                     gpsState.lastMessageMs = millis();
 359                     sensorsSet(SENSOR_GPS);
 360                     gpsSetState(GPS_CHANGE_BAUD);
 361                 }
 362                 else {
 363                     sensorsClear(SENSOR_GPS);
 364                 }
 365             }
 366             break;
 367
 368         case GPS_CHANGE_BAUD:
 369         case GPS_CHECK_VERSION:
 370         case GPS_CONFIGURE:
 371         case GPS_RECEIVING_DATA:
 372             gpsHandleProtocol();
 373             if (millis() - gpsState.lastMessageMs > GPS_TIMEOUT) {
 374                 // remove GPS from capability
 375                 gpsSetState(GPS_LOST_COMMUNICATION);
 376             }
 377             break;
 378
 379         case GPS_LOST_COMMUNICATION:
 380             // No valid data from GPS unit, cause re-init and re-detection
 381             gpsStats.timeouts++;
 382             DISABLE_STATE(GPS_FIX);
 383             gpsSol.fixType = GPS_NO_FIX;
 384
 385             gpsSetState(GPS_INITIALIZING);
 386             break;
 387         }
 388     }
 389     else {
 390         // GPS_TYPE_NA
 391     }
 392 #endif
 393 }
 394
 395 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
 396 {
 397     waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
 398     waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
 399
 400     if(!(gpsState.gpsPort->mode & MODE_TX))
 401     serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);
 402
 403     LED0_OFF;
 404     LED1_OFF;
 405
 406     char c;
 407     while(1) {
 408         if (serialRxBytesWaiting(gpsState.gpsPort)) {
 409             LED0_ON;
 410             c = serialRead(gpsState.gpsPort);
 411             serialWrite(gpsPassthroughPort, c);
 412             LED0_OFF;
 413         }
 414         if (serialRxBytesWaiting(gpsPassthroughPort)) {
 415             LED1_ON;
 416             c = serialRead(gpsPassthroughPort);
 417             serialWrite(gpsState.gpsPort, c);
 418             LED1_OFF;
 419         }
 420     }
 421 }
 422
 423 void updateGpsIndicator(uint32_t currentTime)
 424 {
 425     static uint32_t GPSLEDTime;
 426     if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (gpsSol.numSat>= 5)) {
 427         GPSLEDTime = currentTime + 150000;
 428         LED1_TOGGLE;
 429     }
 430 }
 431
 432 /* Support for built-in magnetometer accessible via the native GPS protocol (i.e. NAZA) */
 433 bool gpsMagInit(void)
 434 {
 435     return true;
 436 }
 437
 438 bool gpsMagRead(int16_t *magData)
 439 {
 440     magData[X] = gpsSol.magData[0];
 441     magData[Y] = gpsSol.magData[1];
 442     magData[Z] = gpsSol.magData[2];
 443     return gpsSol.flags.validMag;
 444 }
 445
 446 bool gpsMagDetect(mag_t *mag)
 447 {
 448     if (!(feature(FEATURE_GPS) && gpsProviders[gpsState.gpsConfig->provider].hasCompass))
 449         return false;
 450
 451     if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL && (!findSerialPortConfig(FUNCTION_GPS))) {
 452         return false;
 453     }
 454
 455     mag->init = gpsMagInit;
 456     mag->read = gpsMagRead;
 457     return true;
 458 }
 459
 460 #endif