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