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     void                (*restart)(void);   // Restart protocol driver thread
  67     void                (*protocol)(void);  // Process protocol driver thread
  68 } gpsProviderDescriptor_t;
  69
  70 // GPS public data
  71 gpsReceiverData_t gpsState;
  72 gpsStatistics_t   gpsStats;
  73 gpsSolutionData_t gpsSol;
  74
  75 // Map gpsBaudRate_e index to baudRate_e
  76 baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 };
  77
  78 static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
  79     /* NMEA GPS */
  80 #ifdef USE_GPS_PROTO_NMEA
  81     { false, MODE_RX, gpsRestartNMEA, &gpsHandleNMEA },
  82 #else
  83     { false, 0, NULL, NULL },
  84 #endif
  85
  86     /* UBLOX binary */
  87 #ifdef USE_GPS_PROTO_UBLOX
  88     { false, MODE_RXTX, &gpsRestartUBLOX, &gpsHandleUBLOX },
  89 #else
  90     { false, 0, NULL, NULL },
  91 #endif
  92
  93     /* UBLOX7PLUS binary */
  94 #ifdef USE_GPS_PROTO_UBLOX
  95     { false, MODE_RXTX, &gpsRestartUBLOX, &gpsHandleUBLOX },
  96 #else
  97     { false, 0,  NULL, NULL },
  98 #endif
  99
 100     /* MSP GPS */
 101 #ifdef USE_GPS_PROTO_MSP
 102     { true, 0, &gpsRestartMSP, &gpsHandleMSP },
 103 #else
 104     { false, 0, NULL, NULL },
 105 #endif
 106 };
 107
 108 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 2);
 109
 110 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
 111     .provider = SETTING_GPS_PROVIDER_DEFAULT,
 112     .sbasMode = SETTING_GPS_SBAS_MODE_DEFAULT,
 113     .autoConfig = SETTING_GPS_AUTO_CONFIG_DEFAULT,
 114     .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT,
 115     .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT,
 116     .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT,
 117     .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT
 118 );
 119
 120 void gpsSetState(gpsState_e state)
 121 {
 122     gpsState.state = state;
 123     gpsState.lastStateSwitchMs = millis();
 124 }
 125
 126 static void gpsUpdateTime(void)
 127 {
 128     if (!rtcHasTime() && gpsSol.flags.validTime && gpsSol.time.year != 0) {
 129         rtcSetDateTime(&gpsSol.time);
 130     }
 131 }
 132
 133 void gpsSetProtocolTimeout(timeMs_t timeoutMs)
 134 {
 135     gpsState.lastLastMessageMs = gpsState.lastMessageMs;
 136     gpsState.lastMessageMs = millis();
 137     gpsState.timeoutMs = timeoutMs;
 138 }
 139
 140 void gpsProcessNewSolutionData(void)
 141 {
 142     // Set GPS fix flag only if we have 3D fix
 143     if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
 144         ENABLE_STATE(GPS_FIX);
 145     }
 146     else {
 147         /* When no fix available - reset flags as well */
 148         gpsSol.flags.validVelNE = false;
 149         gpsSol.flags.validVelD = false;
 150         gpsSol.flags.validEPE = false;
 151         DISABLE_STATE(GPS_FIX);
 152     }
 153
 154     // Set sensor as ready and available
 155     sensorsSet(SENSOR_GPS);
 156
 157     // Pass on GPS update to NAV and IMU
 158     onNewGPSData();
 159
 160     // Update time
 161     gpsUpdateTime();
 162
 163     // Update timeout
 164     gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 165
 166     // Update statistics
 167     gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs;
 168     gpsSol.flags.hasNewData = true;
 169
 170     // Toggle heartbeat
 171     gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 172 }
 173
 174 static void gpsResetSolution(void)
 175 {
 176     gpsSol.eph = 9999;
 177     gpsSol.epv = 9999;
 178     gpsSol.numSat = 0;
 179     gpsSol.hdop = 9999;
 180
 181     gpsSol.fixType = GPS_NO_FIX;
 182
 183     gpsSol.flags.validVelNE = false;
 184     gpsSol.flags.validVelD = false;
 185     gpsSol.flags.validMag = false;
 186     gpsSol.flags.validEPE = false;
 187     gpsSol.flags.validTime = false;
 188 }
 189
 190 void gpsPreInit(void)
 191 {
 192     // Make sure gpsProvider is known when gpsMagDetect is called
 193     gpsState.gpsConfig = gpsConfig();
 194     gpsState.baseTimeoutMs = (gpsState.gpsConfig->provider == GPS_NMEA) ? GPS_TIMEOUT*2 : GPS_TIMEOUT;
 195 }
 196
 197 void gpsInit(void)
 198 {
 199     gpsState.serialConfig = serialConfig();
 200     gpsState.gpsConfig = gpsConfig();
 201
 202     gpsStats.errors = 0;
 203     gpsStats.timeouts = 0;
 204
 205     // Reset solution, timeout and prepare to start
 206     gpsResetSolution();
 207     gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 208     gpsSetState(GPS_UNKNOWN);
 209
 210     // If given GPS provider has protocol() function not defined - we can't use it
 211     if (!gpsProviders[gpsState.gpsConfig->provider].protocol) {
 212         featureClear(FEATURE_GPS);
 213         return;
 214     }
 215
 216     // Shortcut for driver-based GPS (MSP)
 217     if (gpsProviders[gpsState.gpsConfig->provider].isDriverBased) {
 218         gpsSetState(GPS_INITIALIZING);
 219         return;
 220     }
 221
 222     serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 223     if (!gpsPortConfig) {
 224         featureClear(FEATURE_GPS);
 225         return;
 226     }
 227
 228     // Start with baud rate index as configured for serial port
 229     int baudrateIndex;
 230     gpsState.baudrateIndex = GPS_BAUDRATE_115200;
 231     for (baudrateIndex = 0, gpsState.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         speed = speed * sin_approx((now % 1000) / 1000.f * M_PIf) * +speed;
 276         int32_t cmDelta = speed * (delta / 1000.0f);
 277         int32_t latCmDelta = cmDelta * cos_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 278         int32_t lonCmDelta = cmDelta * sin_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 279         int32_t latDelta = ceilf((float)latCmDelta / (111 * 1000 * 100 / 1e7));
 280         int32_t lonDelta = ceilf((float)lonCmDelta / (111 * 1000 * 100 / 1e7));
 281         if (speed > 0 && latDelta == 0 && lonDelta == 0) {
 282             return false;
 283         }
 284         lat += latDelta;
 285         lon += lonDelta;
 286         gpsSol.fixType = GPS_FIX_3D;
 287         gpsSol.numSat = 6;
 288         gpsSol.llh.lat = lat;
 289         gpsSol.llh.lon = lon;
 290         gpsSol.llh.alt = 0;
 291         gpsSol.groundSpeed = speed;
 292         gpsSol.groundCourse = FAKE_GPS_GROUND_COURSE_DECIDEGREES;
 293         gpsSol.velNED[X] = speed * cos_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 294         gpsSol.velNED[Y] = speed * sin_approx(DECIDEGREES_TO_RADIANS(FAKE_GPS_GROUND_COURSE_DECIDEGREES));
 295         gpsSol.velNED[Z] = 0;
 296         gpsSol.flags.validVelNE = true;
 297         gpsSol.flags.validVelD = true;
 298         gpsSol.flags.validEPE = true;
 299         gpsSol.flags.validTime = true;
 300         gpsSol.eph = 100;
 301         gpsSol.epv = 100;
 302         gpsSol.time.year = 1983;
 303         gpsSol.time.month = 1;
 304         gpsSol.time.day = 1;
 305         gpsSol.time.hours = 3;
 306         gpsSol.time.minutes = 15;
 307         gpsSol.time.seconds = 42;
 308
 309         ENABLE_STATE(GPS_FIX);
 310         sensorsSet(SENSOR_GPS);
 311         gpsUpdateTime();
 312         onNewGPSData();
 313
 314         gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 315
 316         gpsSetState(GPS_RUNNING);
 317         gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 318         return true;
 319     }
 320     return false;
 321 }
 322 #endif
 323
 324 uint16_t gpsConstrainEPE(uint32_t epe)
 325 {
 326     return (epe > 9999) ? 9999 : epe; // max 99.99m error
 327 }
 328
 329 uint16_t gpsConstrainHDOP(uint32_t hdop)
 330 {
 331     return (hdop > 9999) ? 9999 : hdop; // max 99.99m error
 332 }
 333
 334 bool gpsUpdate(void)
 335 {
 336     // Sanity check
 337     if (!feature(FEATURE_GPS)) {
 338         sensorsClear(SENSOR_GPS);
 339         DISABLE_STATE(GPS_FIX);
 340         return false;
 341     }
 342
 343     /* Extra delay for at least 2 seconds after booting to give GPS time to initialise */
 344     if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) {
 345         sensorsClear(SENSOR_GPS);
 346         DISABLE_STATE(GPS_FIX);
 347         return false;
 348     }
 349
 350 #ifdef USE_SIMULATOR
 351     if (ARMING_FLAG(SIMULATOR_MODE)) {
 352         gpsUpdateTime();
 353         gpsSetState(GPS_RUNNING);
 354         sensorsSet(SENSOR_GPS);
 355         return gpsSol.flags.hasNewData;
 356     }
 357 #endif
 358 #ifdef USE_FAKE_GPS
 359     return gpsFakeGPSUpdate();
 360 #else
 361
 362     // Assume that we don't have new data this run
 363     gpsSol.flags.hasNewData = false;
 364
 365     switch (gpsState.state) {
 366     default:
 367     case GPS_INITIALIZING:
 368         // Wait for GPS_INIT_DELAY before starting the GPS protocol thread
 369         if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
 370             // Reset internals
 371             DISABLE_STATE(GPS_FIX);
 372             gpsSol.fixType = GPS_NO_FIX;
 373
 374             // Reset solution
 375             gpsResetSolution();
 376
 377             // Call GPS protocol reset handler
 378             gpsProviders[gpsState.gpsConfig->provider].restart();
 379
 380             // Switch to GPS_RUNNING state (mind the timeout)
 381             gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
 382             gpsSetState(GPS_RUNNING);
 383         }
 384         break;
 385
 386     case GPS_RUNNING:
 387         // Call GPS protocol thread
 388         gpsProviders[gpsState.gpsConfig->provider].protocol();
 389
 390         // Check for GPS timeout
 391         if ((millis() - gpsState.lastMessageMs) > gpsState.baseTimeoutMs) {
 392             sensorsClear(SENSOR_GPS);
 393             DISABLE_STATE(GPS_FIX);
 394             gpsSol.fixType = GPS_NO_FIX;
 395             gpsSetState(GPS_LOST_COMMUNICATION);
 396         }
 397         break;
 398
 399     case GPS_LOST_COMMUNICATION:
 400         gpsStats.timeouts++;
 401         gpsSetState(GPS_INITIALIZING);
 402         break;
 403     }
 404
 405     return gpsSol.flags.hasNewData;
 406 #endif
 407 }
 408
 409 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
 410 {
 411     waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
 412     waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
 413
 414     if (!(gpsState.gpsPort->mode & MODE_TX))
 415     serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);
 416
 417     LED0_OFF;
 418     LED1_OFF;
 419
 420     char c;
 421     while (1) {
 422         if (serialRxBytesWaiting(gpsState.gpsPort)) {
 423             LED0_ON;
 424             c = serialRead(gpsState.gpsPort);
 425             serialWrite(gpsPassthroughPort, c);
 426             LED0_OFF;
 427         }
 428         if (serialRxBytesWaiting(gpsPassthroughPort)) {
 429             LED1_ON;
 430             c = serialRead(gpsPassthroughPort);
 431             serialWrite(gpsState.gpsPort, c);
 432             LED1_OFF;
 433         }
 434     }
 435 }
 436
 437 void updateGpsIndicator(timeUs_t currentTimeUs)
 438 {
 439     static timeUs_t GPSLEDTime;
 440     if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat>= 5)) {
 441         GPSLEDTime = currentTimeUs + 150000;
 442         LED1_TOGGLE;
 443     }
 444 }
 445
 446 bool isGPSHealthy(void)
 447 {
 448     return true;
 449 }
 450
 451 bool isGPSHeadingValid(void)
 452 {
 453     return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
 454 }
 455
 456 #endif