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