flight/modules/GPS/GPS.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup GPSModule GPS Module
   6  * @brief Process GPS information
   7  * @{
   8  *
   9  * @file       GPS.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  12  * @brief      GPS module, handles GPS and various streams
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 // ****************
  33
  34 #include <openpilot.h>
  35
  36 #include "gpspositionsensor.h"
  37 #include "homelocation.h"
  38 #include "gpstime.h"
  39 #include "gpssatellites.h"
  40 #include "gpsvelocitysensor.h"
  41 #include "gpssettings.h"
  42 #include "taskinfo.h"
  43 #include "hwsettings.h"
  44 #include "auxmagsensor.h"
  45 #include "WorldMagModel.h"
  46 #include "CoordinateConversions.h"
  47 #include <pios_com.h>
  48
  49 #include "GPS.h"
  50 #include "NMEA.h"
  51 #include "UBX.h"
  52 #include "DJI.h"
  53 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
  54 #include "inc/ubx_autoconfig.h"
  55 #define FULL_UBX_PARSER
  56 #endif
  57 #include <auxmagsupport.h>
  58
  59 #include <pios_instrumentation_helper.h>
  60 PERF_DEFINE_COUNTER(counterBytesIn);
  61 PERF_DEFINE_COUNTER(counterRate);
  62 PERF_DEFINE_COUNTER(counterParse);
  63
  64 #if defined(ANY_GPS_PARSER) || defined(ANY_FULL_GPS_PARSER) || defined(ANY_FULL_MAG_PARSER)
  65 #error ANY_GPS_PARSER ANY_FULL_GPS_PARSER and ANY_FULL_MAG_PARSER should all be undefined at this point.
  66 #endif
  67
  68 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)
  69 #define ANY_GPS_PARSER
  70 #endif
  71 #if defined(ANY_GPS_PARSER) && !defined(PIOS_GPS_MINIMAL)
  72 #define ANY_FULL_GPS_PARSER
  73 #endif
  74 #if (defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
  75 #define ANY_FULL_MAG_PARSER
  76 #endif
  77
  78 // ****************
  79 // Private functions
  80
  81 static void gpsTask(__attribute__((unused)) void *parameters);
  82 static void updateHwSettings(__attribute__((unused)) UAVObjEvent *ev);
  83
  84 #if defined(PIOS_GPS_SETS_HOMELOCATION)
  85 static void setHomeLocation(GPSPositionSensorData *gpsData);
  86 static float GravityAccel(float latitude, float longitude, float altitude);
  87 #endif
  88
  89 #if defined(ANY_FULL_MAG_PARSER)
  90 void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
  91 #endif
  92 #if defined(ANY_FULL_GPS_PARSER)
  93 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
  94 #endif
  95
  96 // ****************
  97 // Private constants
  98
  99 // GPS timeout is greater than 1000ms so that a stock GPS configuration can be used without timeout errors
 100 #define GPS_TIMEOUT_MS             1250
 101
 102 // delay from detecting HomeLocation.Set == False before setting new homelocation
 103 // this prevent that a save with homelocation.Set = false triggered by gps ends saving
 104 // the new location with Set = true.
 105 #define GPS_HOMELOCATION_SET_DELAY 5000
 106
 107 #define GPS_LOOP_DELAY_MS          6
 108
 109 #ifdef PIOS_GPS_SETS_HOMELOCATION
 110 // Unfortunately need a good size stack for the WMM calculation
 111         #define STACK_SIZE_BYTES   1024
 112 #else
 113 #if defined(PIOS_GPS_MINIMAL)
 114         #define GPS_READ_BUFFER    32
 115
 116 #ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
 117         #define STACK_SIZE_BYTES   580 // NMEA
 118 #else
 119         #define STACK_SIZE_BYTES   440 // UBX
 120 #endif // PIOS_INCLUDE_GPS_NMEA_PARSER
 121 #else
 122         #define STACK_SIZE_BYTES   650
 123 #endif // PIOS_GPS_MINIMAL
 124 #endif // PIOS_GPS_SETS_HOMELOCATION
 125
 126 #ifndef GPS_READ_BUFFER
 127 #define GPS_READ_BUFFER            128
 128 #endif
 129
 130 #define TASK_PRIORITY              (tskIDLE_PRIORITY + 1)
 131
 132 // ****************
 133 // Private variables
 134
 135 static GPSSettingsData gpsSettings;
 136
 137 #define gpsPort PIOS_COM_GPS
 138 static bool gpsEnabled = false;
 139
 140 static xTaskHandle gpsTaskHandle;
 141
 142 static char *gps_rx_buffer;
 143
 144 static uint32_t timeOfLastUpdateMs;
 145
 146 #if defined(ANY_GPS_PARSER)
 147 static struct GPS_RX_STATS gpsRxStats;
 148 #endif
 149
 150 // ****************
 151 /**
 152  * Initialise the gps module
 153  * \return -1 if initialisation failed
 154  * \return 0 on success
 155  */
 156
 157 int32_t GPSStart(void)
 158 {
 159     if (gpsEnabled) {
 160         // Start gps task
 161         xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle);
 162         PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
 163         return 0;
 164     }
 165     return -1;
 166 }
 167
 168 /**
 169  * Initialise the gps module
 170  * \return -1 if initialisation failed
 171  * \return 0 on success
 172  */
 173
 174 int32_t GPSInitialize(void)
 175 {
 176     HwSettingsInitialize();
 177 #ifdef MODULE_GPS_BUILTIN
 178     gpsEnabled = true;
 179 #else
 180     HwSettingsOptionalModulesData optionalModules;
 181
 182     HwSettingsOptionalModulesGet(&optionalModules);
 183
 184     if (optionalModules.GPS == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 185         gpsEnabled = true;
 186     } else {
 187         gpsEnabled = false;
 188     }
 189 #endif
 190
 191 #if defined(REVOLUTION)
 192     // These objects MUST be initialized for Revolution
 193     // because the rest of the system expects to just
 194     // attach to their queues
 195     GPSPositionSensorInitialize();
 196     GPSVelocitySensorInitialize();
 197     GPSTimeInitialize();
 198     GPSSatellitesInitialize();
 199     HomeLocationInitialize();
 200 #if defined(ANY_FULL_MAG_PARSER)
 201     AuxMagSensorInitialize();
 202     AuxMagSettingsInitialize();
 203     GPSExtendedStatusInitialize();
 204
 205     // Initialize mag parameters
 206     AuxMagSettingsUpdatedCb(NULL);
 207     AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
 208 #endif
 209     GPSSettingsInitialize();
 210     // updateHwSettings() uses gpsSettings
 211     GPSSettingsGet(&gpsSettings);
 212     // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
 213     updateHwSettings(0);
 214 #else /* if defined(REVOLUTION) */
 215     if (gpsEnabled) {
 216         GPSPositionSensorInitialize();
 217         GPSVelocitySensorInitialize();
 218 #if !defined(PIOS_GPS_MINIMAL)
 219         GPSTimeInitialize();
 220         GPSSatellitesInitialize();
 221 #endif
 222 #if defined(PIOS_GPS_SETS_HOMELOCATION)
 223         HomeLocationInitialize();
 224 #endif
 225         GPSSettingsInitialize();
 226         // updateHwSettings() uses gpsSettings
 227         GPSSettingsGet(&gpsSettings);
 228         // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
 229         updateHwSettings(0);
 230     }
 231 #endif /* if defined(REVOLUTION) */
 232
 233     if (gpsEnabled) {
 234 #if defined(PIOS_GPS_MINIMAL)
 235 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 236         gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
 237 #elif defined(PIOS_INCLUDE_GPS_DJI_PARSER)
 238         gps_rx_buffer = pios_malloc(sizeof(struct DJIPacket));
 239 #else
 240         gps_rx_buffer = NULL;
 241 #endif
 242 #else /* defined(PIOS_GPS_MINIMAL) */
 243 #if defined(ANY_GPS_PARSER)
 244 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
 245         size_t bufSize = NMEA_MAX_PACKET_LENGTH;
 246 #else
 247         size_t bufSize = 0;
 248 #endif
 249 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 250         if (bufSize < sizeof(struct UBXPacket)) {
 251             bufSize = sizeof(struct UBXPacket);
 252         }
 253 #endif
 254 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
 255         if (bufSize < sizeof(struct DJIPacket)) {
 256             bufSize = sizeof(struct DJIPacket);
 257         }
 258 #endif
 259         gps_rx_buffer = pios_malloc(bufSize);
 260 #else /* defined(ANY_GPS_PARSER) */
 261         gps_rx_buffer = NULL;
 262 #endif /* defined(ANY_GPS_PARSER) */
 263 #endif /* defined(PIOS_GPS_MINIMAL) */
 264 #if defined(ANY_GPS_PARSER)
 265         PIOS_Assert(gps_rx_buffer);
 266 #endif
 267 #if defined(ANY_FULL_GPS_PARSER)
 268         HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
 269         GPSSettingsConnectCallback(updateGpsSettings);
 270 #endif
 271         return 0;
 272     }
 273
 274     return -1;
 275 }
 276
 277 MODULE_INITCALL(GPSInitialize, GPSStart);
 278
 279 // ****************
 280 /**
 281  * Main gps task. It does not return.
 282  */
 283
 284 static void gpsTask(__attribute__((unused)) void *parameters)
 285 {
 286     // 57600 baud = 5760 bytes per second
 287     // PIOS serial buffers appear to be 32 bytes long
 288     // that is a maximum of 5760/32 = 180 buffers per second
 289     // that is 1/180 = 0.005555556 seconds per packet
 290     // we must never wait more than 5ms since packet was last drained or it may overflow
 291     // 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
 292     portTickType xDelay = 5 / portTICK_RATE_MS;
 293     uint32_t timeNowMs  = xTaskGetTickCount() * portTICK_RATE_MS;
 294
 295 #ifdef PIOS_GPS_SETS_HOMELOCATION
 296     portTickType homelocationSetDelay = 0;
 297 #endif
 298     GPSPositionSensorData gpspositionsensor;
 299
 300     timeOfLastUpdateMs = timeNowMs;
 301     GPSPositionSensorGet(&gpspositionsensor);
 302 #if defined(ANY_FULL_GPS_PARSER)
 303     // this should be done in the task because it calls out to actually start the ubx GPS serial reads
 304     updateGpsSettings(0);
 305 #endif
 306
 307     TickType_t xLastWakeTime;
 308     xLastWakeTime = xTaskGetTickCount();
 309     PERF_INIT_COUNTER(counterBytesIn, 0x97510001);
 310     PERF_INIT_COUNTER(counterRate, 0x97510002);
 311     PERF_INIT_COUNTER(counterParse, 0x97510003);
 312     uint8_t c[GPS_READ_BUFFER];
 313
 314     // Loop forever
 315     while (1) {
 316         if (gpsPort) {
 317 #if defined(FULL_UBX_PARSER)
 318             // do autoconfig stuff for UBX GPS's
 319             if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
 320                 char *buffer   = 0;
 321                 uint16_t count = 0;
 322
 323                 gps_ubx_autoconfig_run(&buffer, &count);
 324                 // Something to send?
 325                 if (count) {
 326                     // clear ack/nak
 327                     ubxLastAck.clsID = 0x00;
 328                     ubxLastAck.msgID = 0x00;
 329                     ubxLastNak.clsID = 0x00;
 330                     ubxLastNak.msgID = 0x00;
 331
 332                     PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
 333                 }
 334             }
 335
 336             // need to do this whether there is received data or not, or the status (e.g. gcs) will not always be correct
 337             int32_t ac_status = ubx_autoconfig_get_status();
 338             static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
 339                                         + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
 340                                         + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
 341                                         + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
 342             gpspositionsensor.AutoConfigStatus =
 343                 ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
 344                 ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
 345                 ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
 346                 GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
 347             if (gpspositionsensor.AutoConfigStatus != lastStatus) {
 348                 GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
 349                 lastStatus = gpspositionsensor.AutoConfigStatus;
 350             }
 351 #endif /* if defined(FULL_UBX_PARSER) */
 352
 353             uint16_t cnt;
 354             int res;
 355             // This blocks the task until there is something on the buffer (or 100ms? passes)
 356             cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
 357             res = PARSER_INCOMPLETE;
 358             if (cnt > 0) {
 359                 PERF_TIMED_SECTION_START(counterParse);
 360                 PERF_TRACK_VALUE(counterBytesIn, cnt);
 361                 PERF_MEASURE_PERIOD(counterRate);
 362                 switch (gpsSettings.DataProtocol) {
 363 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
 364                 case GPSSETTINGS_DATAPROTOCOL_NMEA:
 365                     res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
 366                     break;
 367 #endif
 368 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 369                 case GPSSETTINGS_DATAPROTOCOL_UBX:
 370                     res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
 371                     break;
 372 #endif
 373 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
 374                 case GPSSETTINGS_DATAPROTOCOL_DJI:
 375                     res = parse_dji_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
 376                     break;
 377 #endif
 378                 default:
 379                     res = NO_PARSER; // this should not happen
 380                     break;
 381                 }
 382                 PERF_TIMED_SECTION_END(counterParse);
 383
 384                 if (res == PARSER_COMPLETE) {
 385                     timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
 386                     timeOfLastUpdateMs = timeNowMs;
 387                 }
 388             }
 389
 390             // if there is a protocol error or communication error, or timeout error,
 391             // generally, if there is an error that is due to configuration or bad hardware, set status to NOGPS
 392             // poor GPS signal gets a different error/alarm (below)
 393             //
 394             // should this be expanded to include aux mag status as well? currently the aux mag
 395             // attached to a GPS protocol (OPV9 and DJI) still says OK after the GPS/mag goes down
 396             // (data cable unplugged or flight battery removed with USB still powering the FC)
 397             timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
 398             if ((res == PARSER_ERROR) ||
 399                 (timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
 400                 (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
 401                 // we have not received any valid GPS sentences for a while.
 402                 // either the GPS is not plugged in or a hardware problem or the GPS has locked up.
 403                 GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
 404                 GPSPositionSensorStatusSet(&status);
 405                 AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
 406             }
 407             // if we parsed at least one packet successfully
 408             // we aren't offline, but we still may not have a good enough fix to fly
 409             // or we might not even be receiving all necessary GPS packets (NMEA)
 410             else if (res == PARSER_COMPLETE) {
 411                 // we appear to be receiving packets OK
 412                 // criteria for GPS-OK taken from this post...
 413                 // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
 414                 // NMEA doesn't verify that all necessary packet types for an update have been received
 415                 //
 416                 // if (the fix is good) {
 417                 if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
 418                     (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
 419                     (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
 420                     AlarmsClear(SYSTEMALARMS_ALARM_GPS);
 421 #ifdef PIOS_GPS_SETS_HOMELOCATION
 422                     HomeLocationData home;
 423                     HomeLocationGet(&home);
 424
 425                     if (home.Set == HOMELOCATION_SET_FALSE) {
 426                         if (homelocationSetDelay == 0) {
 427                             homelocationSetDelay = xTaskGetTickCount();
 428                         }
 429                         if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
 430                             setHomeLocation(&gpspositionsensor);
 431                             homelocationSetDelay = 0;
 432                         }
 433                     } else {
 434                         homelocationSetDelay = 0;
 435                     }
 436 #endif
 437                     // else if (we are at least getting what might be usable GPS data to finish a flight with) {
 438                 } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
 439                            (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
 440                     AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
 441                     // else data is probably not good enough to fly
 442                 } else {
 443                     AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
 444                 }
 445             }
 446         } // if (gpsPort)
 447         vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
 448     } // while (1)
 449 }
 450
 451 #ifdef PIOS_GPS_SETS_HOMELOCATION
 452 /*
 453  * Estimate the acceleration due to gravity for a particular location in LLA
 454  */
 455 static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude)
 456 {
 457     /* WGS84 gravity model.  The effect of gravity over latitude is strong
 458      * enough to change the estimated accelerometer bias in those apps. */
 459     float sinsq = sinf(latitude);
 460
 461     sinsq *= sinsq;
 462     /* Likewise, over the altitude range of a high-altitude balloon, the effect
 463      * due to change in altitude can also affect the model. */
 464     return 9.7803267714f * (1.0f + 0.00193185138639f * sinsq) / sqrtf(1.0f - 0.00669437999013f * sinsq)
 465            - 3.086e-6f * altitude;
 466 }
 467
 468 // ****************
 469
 470 static void setHomeLocation(GPSPositionSensorData *gpsData)
 471 {
 472     HomeLocationData home;
 473
 474     HomeLocationGet(&home);
 475     GPSTimeData gps;
 476     GPSTimeGet(&gps);
 477
 478     if (gps.Year >= 2000) {
 479         /* Store LLA */
 480         home.Latitude  = gpsData->Latitude;
 481         home.Longitude = gpsData->Longitude;
 482         home.Altitude  = gpsData->Altitude + gpsData->GeoidSeparation;
 483
 484         /* Compute home ECEF coordinates and the rotation matrix into NED
 485          * Note that floats are used here - they should give enough precision
 486          * for this application.*/
 487
 488         float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) };
 489
 490         /* Compute magnetic flux direction at home location */
 491         if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) {
 492             /*Compute local acceleration due to gravity.  Vehicles that span a very large
 493              * range of altitude (say, weather balloons) may need to update this during the
 494              * flight. */
 495             home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]);
 496             home.Set = HOMELOCATION_SET_TRUE;
 497             HomeLocationSet(&home);
 498         }
 499     }
 500 }
 501 #endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */
 502
 503
 504 uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud)
 505 {
 506     switch (baud) {
 507     case HWSETTINGS_GPSSPEED_2400:
 508         return 2400;
 509
 510     case HWSETTINGS_GPSSPEED_4800:
 511         return 4800;
 512
 513     default:
 514     case HWSETTINGS_GPSSPEED_9600:
 515         return 9600;
 516
 517     case HWSETTINGS_GPSSPEED_19200:
 518         return 19200;
 519
 520     case HWSETTINGS_GPSSPEED_38400:
 521         return 38400;
 522
 523     case HWSETTINGS_GPSSPEED_57600:
 524         return 57600;
 525
 526     case HWSETTINGS_GPSSPEED_115200:
 527         return 115200;
 528
 529     case HWSETTINGS_GPSSPEED_230400:
 530         return 230400;
 531     }
 532 }
 533
 534
 535 // having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
 536 void gps_set_fc_baud_from_arg(uint8_t baud)
 537 {
 538     static uint8_t previous_baud = 255;
 539     static uint8_t mutex; // = 0
 540
 541     // do we need this?
 542     // can the code stand having two tasks/threads do an XyzSet() call at the same time?
 543     if (__sync_fetch_and_add(&mutex, 1) == 0) {
 544         // don't bother doing the baud change if it is actually the same
 545         // might drop less data
 546         if (previous_baud != baud) {
 547             previous_baud = baud;
 548             // Set Revo port hwsettings_baud
 549             PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud));
 550             GPSPositionSensorBaudRateSet(&baud);
 551         }
 552     }
 553     --mutex;
 554 }
 555
 556
 557 // set the FC port baud rate
 558 // from HwSettings or override with 115200 if DJI
 559 static void gps_set_fc_baud_from_settings()
 560 {
 561     uint8_t speed;
 562
 563     // Retrieve settings
 564 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL)
 565     if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) {
 566         speed = HWSETTINGS_GPSSPEED_115200;
 567     } else {
 568 #endif
 569     HwSettingsGPSSpeedGet(&speed);
 570 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL)
 571 }
 572 #endif
 573     // set fc baud
 574     gps_set_fc_baud_from_arg(speed);
 575 }
 576
 577
 578 /**
 579  * HwSettings callback
 580  * Generally speaking, set the FC baud rate
 581  * and for UBX, if it is safe, start the auto config process
 582  * this allows the user to change the baud rate and see if it works without rebooting
 583  */
 584
 585 // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
 586 static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
 587 {
 588 #if defined(ANY_FULL_GPS_PARSER)
 589     static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport
 590 #endif
 591     // if GPS (UBX or NMEA) is enabled at all
 592     if (gpsPort && gpsEnabled) {
 593 // if we have ubx auto config then sometimes we don't set the baud rate
 594 #if defined(FULL_UBX_PARSER)
 595         // just for UBX, because it has autoconfig
 596         // if in startup, or not configured to do ubx and ubx auto config
 597         //
 598         // on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate
 599         // after startup (previousGpsPort == gpsPort) we must set the Revo port baud rate only if autoconfig is disabled
 600         // always we must set the Revo port baud rate if not using UBX
 601         if (ev == NULL
 602             || previousGpsPort != gpsPort
 603             || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED
 604             || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
 605 #endif /* defined(FULL_UBX_PARSER) */
 606         {
 607             // always set the baud rate
 608             gps_set_fc_baud_from_settings();
 609 #if defined(FULL_UBX_PARSER)
 610             // just for UBX, because it has subtypes UBX(6), UBX7 and UBX8
 611             // changing anything in HwSettings will make it re-verify the sensor type (including auto-baud if not completely disabled)
 612             // for auto baud disabled, the user can just try some baud rates and when the baud rate is correct, the sensor type becomes valid
 613             gps_ubx_reset_sensor_type();
 614 #endif /* defined(FULL_UBX_PARSER) */
 615         }
 616 #if defined(FULL_UBX_PARSER)
 617         else {
 618             // else:
 619             // - we are past startup
 620             // - we are running uxb protocol
 621             // - and some form of ubx auto config is enabled
 622             //
 623             // it will never do this during startup because ev == NULL during startup
 624             //
 625             // this is here so that runtime (not startup) user changes to HwSettings will restart auto-config
 626             gps_ubx_autoconfig_set(NULL);
 627         }
 628 #endif /* defined(FULL_UBX_PARSER) */
 629     }
 630 #if defined(ANY_FULL_GPS_PARSER)
 631     previousGpsPort = gpsPort;
 632 #endif
 633 }
 634
 635
 636 #if defined(ANY_FULL_MAG_PARSER)
 637 void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 638 {
 639     auxmagsupport_reload_settings();
 640 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 641     op_gpsv9_load_mag_settings();
 642 #endif
 643 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
 644     dji_load_mag_settings();
 645 #endif
 646 #if defined(PIOS_INCLUDE_HMC5X83)
 647     aux_hmc5x83_load_mag_settings();
 648 #endif
 649 }
 650 #endif /* defined(ANY_FULL_MAG_PARSER) */
 651
 652
 653 #if defined(ANY_FULL_GPS_PARSER)
 654 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
 655 {
 656     ubx_autoconfig_settings_t newconfig;
 657
 658     GPSSettingsGet(&gpsSettings);
 659
 660 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
 661     // each time there is a protocol change, set the baud rate
 662     // so that DJI can be forced to 115200, but changing to another protocol will change the baud rate to the user specified value
 663     // note that changes to HwSettings GPS baud rate are detected in the HwSettings callback,
 664     // but changing to/from DJI is effectively a baud rate change because DJI is forced to be 115200
 665     gps_set_fc_baud_from_settings(); // knows to force 115200 for DJI
 666 #endif
 667
 668     // it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running another protocol
 669     // because ubx auto config never gets called
 670     // setting it up completely means that if we switch from the other protocol to UBX or disabled to enabled, that it will start normally
 671     newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
 672     newconfig.AssistNowAutonomous = gpsSettings.UbxAssistNowAutonomous;
 673     newconfig.navRate = gpsSettings.UbxRate;
 674     newconfig.dynamicModel  = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
 675                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
 676                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
 677                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
 678                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
 679                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
 680                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
 681                               gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
 682                               UBX_DYNMODEL_AIRBORNE1G;
 683
 684     switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
 685     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
 686     case GPSSETTINGS_UBXSBASMODE_CORRECTION:
 687     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
 688     case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
 689         newconfig.SBASCorrection = true;
 690         break;
 691     default:
 692         newconfig.SBASCorrection = false;
 693     }
 694
 695     switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
 696     case GPSSETTINGS_UBXSBASMODE_RANGING:
 697     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
 698     case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
 699     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
 700         newconfig.SBASRanging = true;
 701         break;
 702     default:
 703         newconfig.SBASRanging = false;
 704     }
 705
 706     switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
 707     case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
 708     case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
 709     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
 710     case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
 711         newconfig.SBASIntegrity = true;
 712         break;
 713     default:
 714         newconfig.SBASIntegrity = false;
 715     }
 716
 717     newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed;
 718
 719     newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
 720                          gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
 721                          gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
 722                          gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
 723                          gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
 724
 725     switch (gpsSettings.UbxGNSSMode) {
 726     case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS:
 727         newconfig.enableGPS     = true;
 728         newconfig.enableGLONASS = true;
 729         newconfig.enableBeiDou  = false;
 730         newconfig.enableGalileo = false;
 731         break;
 732     case GPSSETTINGS_UBXGNSSMODE_GLONASS:
 733         newconfig.enableGPS     = false;
 734         newconfig.enableGLONASS = true;
 735         newconfig.enableBeiDou  = false;
 736         newconfig.enableGalileo = false;
 737         break;
 738     case GPSSETTINGS_UBXGNSSMODE_GPS:
 739         newconfig.enableGPS     = true;
 740         newconfig.enableGLONASS = false;
 741         newconfig.enableBeiDou  = false;
 742         newconfig.enableGalileo = false;
 743         break;
 744     case GPSSETTINGS_UBXGNSSMODE_GPSBEIDOU:
 745         newconfig.enableGPS     = true;
 746         newconfig.enableGLONASS = false;
 747         newconfig.enableBeiDou  = true;
 748         newconfig.enableGalileo = false;
 749         break;
 750     case GPSSETTINGS_UBXGNSSMODE_GLONASSBEIDOU:
 751         newconfig.enableGPS     = false;
 752         newconfig.enableGLONASS = true;
 753         newconfig.enableBeiDou  = true;
 754         newconfig.enableGalileo = false;
 755         break;
 756     case GPSSETTINGS_UBXGNSSMODE_GPSGALILEO:
 757         newconfig.enableGPS     = true;
 758         newconfig.enableGLONASS = false;
 759         newconfig.enableBeiDou  = false;
 760         newconfig.enableGalileo = true;
 761         break;
 762     case GPSSETTINGS_UBXGNSSMODE_GPSGLONASSGALILEO:
 763         newconfig.enableGPS     = true;
 764         newconfig.enableGLONASS = true;
 765         newconfig.enableBeiDou  = false;
 766         newconfig.enableGalileo = true;
 767         break;
 768     default:
 769         newconfig.enableGPS     = false;
 770         newconfig.enableGLONASS = false;
 771         newconfig.enableBeiDou  = false;
 772         newconfig.enableGalileo = false;
 773         break;
 774     }
 775
 776 // handle protocol changes and devices that have just been powered up
 777 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 778     if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
 779         // do whatever the user has configured for power on startup
 780         // even autoconfig disabled still gets the ubx version
 781         gps_ubx_autoconfig_set(&newconfig);
 782     }
 783 #endif
 784 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
 785     if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) {
 786         // clear out satellite data because DJI doesn't provide it
 787         GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0);
 788     }
 789 #endif
 790 }
 791 #endif /* defined(ANY_FULL_GPS_PARSER) */
 792 /**
 793  * @}
 794  * @}
 795  */