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