OP-1900 added deceleration check to autotakeoff failsafe
[librepilot.git] / flight / modules / GPS / GPS.c
blob74166d6cf63ad2e8540dde39b2202db0496d9dde
1 /**
2 ******************************************************************************
3 * @addtogroup OpenPilotModules OpenPilot Modules
4 * @{
5 * @addtogroup GSPModule GPS Module
6 * @brief Process GPS information
7 * @{
9 * @file GPS.c
10 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
11 * @brief GPS module, handles GPS and NMEA stream
12 * @see The GNU Public License (GPL) Version 3
14 *****************************************************************************/
16 * This program is free software; you can redistribute it and/or modify
17 * it under the terms of the GNU General Public License as published by
18 * the Free Software Foundation; either version 3 of the License, or
19 * (at your option) any later version.
21 * This program is distributed in the hope that it will be useful, but
22 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
23 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
24 * for more details.
26 * You should have received a copy of the GNU General Public License along
27 * with this program; if not, write to the Free Software Foundation, Inc.,
28 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
31 // ****************
33 #include <openpilot.h>
35 #include "gpspositionsensor.h"
36 #include "homelocation.h"
37 #include "gpstime.h"
38 #include "gpssatellites.h"
39 #include "gpsvelocitysensor.h"
40 #include "gpssettings.h"
41 #include "taskinfo.h"
42 #include "hwsettings.h"
43 #include "auxmagsensor.h"
44 #include "WorldMagModel.h"
45 #include "CoordinateConversions.h"
46 #include <pios_com.h>
48 #include "GPS.h"
49 #include "NMEA.h"
50 #include "UBX.h"
51 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
52 #include "inc/ubx_autoconfig.h"
53 #endif
55 #include <pios_instrumentation_helper.h>
56 PERF_DEFINE_COUNTER(counterBytesIn);
57 PERF_DEFINE_COUNTER(counterRate);
58 PERF_DEFINE_COUNTER(counterParse);
59 // ****************
60 // Private functions
62 static void gpsTask(__attribute__((unused)) void *parameters);
63 static void updateHwSettings(__attribute__((unused)) UAVObjEvent *ev);
65 #ifdef PIOS_GPS_SETS_HOMELOCATION
66 static void setHomeLocation(GPSPositionSensorData *gpsData);
67 static float GravityAccel(float latitude, float longitude, float altitude);
68 #endif
70 #ifdef PIOS_INCLUDE_GPS_UBX_PARSER
71 void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
72 #ifndef PIOS_GPS_MINIMAL
73 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
74 #endif
75 #endif
77 // ****************
78 // Private constants
80 #define GPS_TIMEOUT_MS 500
81 // delay from detecting HomeLocation.Set == False before setting new homelocation
82 // this prevent that a save with homelocation.Set = false triggered by gps ends saving
83 // the new location with Set = true.
84 #define GPS_HOMELOCATION_SET_DELAY 5000
86 #define GPS_LOOP_DELAY_MS 6
88 #ifdef PIOS_GPS_SETS_HOMELOCATION
89 // Unfortunately need a good size stack for the WMM calculation
90 #define STACK_SIZE_BYTES 1024
91 #else
92 #if defined(PIOS_GPS_MINIMAL)
93 #define GPS_READ_BUFFER 32
95 #ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
96 #define STACK_SIZE_BYTES 580 // NMEA
97 #else
98 #define STACK_SIZE_BYTES 440 // UBX
99 #endif // PIOS_INCLUDE_GPS_NMEA_PARSER
100 #else
101 #define STACK_SIZE_BYTES 650
102 #endif // PIOS_GPS_MINIMAL
103 #endif // PIOS_GPS_SETS_HOMELOCATION
105 #ifndef GPS_READ_BUFFER
106 #define GPS_READ_BUFFER 128
107 #endif
109 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
111 // ****************
112 // Private variables
114 static GPSSettingsData gpsSettings;
116 #define gpsPort PIOS_COM_GPS
117 static bool gpsEnabled = false;
119 static xTaskHandle gpsTaskHandle;
121 static char *gps_rx_buffer;
123 static uint32_t timeOfLastCommandMs;
124 static uint32_t timeOfLastUpdateMs;
126 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
127 static struct GPS_RX_STATS gpsRxStats;
128 #endif
130 // ****************
132 * Initialise the gps module
133 * \return -1 if initialisation failed
134 * \return 0 on success
137 int32_t GPSStart(void)
139 if (gpsEnabled) {
140 // Start gps task
141 xTaskCreate(gpsTask, "GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle);
142 PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
143 return 0;
145 return -1;
149 * Initialise the gps module
150 * \return -1 if initialisation failed
151 * \return 0 on success
154 int32_t GPSInitialize(void)
156 HwSettingsInitialize();
157 #ifdef MODULE_GPS_BUILTIN
158 gpsEnabled = true;
159 #else
160 HwSettingsOptionalModulesData optionalModules;
162 HwSettingsOptionalModulesGet(&optionalModules);
164 if (optionalModules.GPS == HWSETTINGS_OPTIONALMODULES_ENABLED) {
165 gpsEnabled = true;
166 } else {
167 gpsEnabled = false;
169 #endif
171 #if defined(REVOLUTION)
172 // These objects MUST be initialized for Revolution
173 // because the rest of the system expects to just
174 // attach to their queues
175 GPSPositionSensorInitialize();
176 GPSVelocitySensorInitialize();
177 GPSTimeInitialize();
178 GPSSatellitesInitialize();
179 HomeLocationInitialize();
180 #ifdef PIOS_INCLUDE_GPS_UBX_PARSER
181 AuxMagSensorInitialize();
182 AuxMagSettingsInitialize();
183 GPSExtendedStatusInitialize();
185 // Initialize mag parameters
186 AuxMagSettingsUpdatedCb(NULL);
187 AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
188 #endif
189 GPSSettingsInitialize();
190 // updateHwSettings() uses gpsSettings
191 GPSSettingsGet(&gpsSettings);
192 // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
193 updateHwSettings(0);
194 #else /* if defined(REVOLUTION) */
195 if (gpsEnabled) {
196 GPSPositionSensorInitialize();
197 GPSVelocitySensorInitialize();
198 #if !defined(PIOS_GPS_MINIMAL)
199 GPSTimeInitialize();
200 GPSSatellitesInitialize();
201 #endif
202 #if defined(PIOS_GPS_SETS_HOMELOCATION)
203 HomeLocationInitialize();
204 #endif
205 GPSSettingsInitialize();
206 // updateHwSettings() uses gpsSettings
207 GPSSettingsGet(&gpsSettings);
208 // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
209 updateHwSettings(0);
211 #endif /* if defined(REVOLUTION) */
213 if (gpsEnabled) {
214 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
215 gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
216 #elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
217 gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
218 #elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
219 gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
220 #else
221 gps_rx_buffer = NULL;
222 #endif
223 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
224 PIOS_Assert(gps_rx_buffer);
225 #endif
226 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
227 HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
228 GPSSettingsConnectCallback(updateGpsSettings);
229 #endif
230 return 0;
233 return -1;
236 MODULE_INITCALL(GPSInitialize, GPSStart);
238 // ****************
240 * Main gps task. It does not return.
243 static void gpsTask(__attribute__((unused)) void *parameters)
245 // 57600 baud = 5760 bytes per second
246 // PIOS serial buffers appear to be 32 bytes long
247 // that is a maximum of 5760/32 = 180 buffers per second
248 // that is 1/180 = 0.005555556 seconds per packet
249 // we must never wait more than 5ms since packet was last drained or it may overflow
250 // 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
251 portTickType xDelay = 5 / portTICK_RATE_MS;
252 uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
254 #ifdef PIOS_GPS_SETS_HOMELOCATION
255 portTickType homelocationSetDelay = 0;
256 #endif
257 GPSPositionSensorData gpspositionsensor;
259 timeOfLastUpdateMs = timeNowMs;
260 timeOfLastCommandMs = timeNowMs;
262 GPSPositionSensorGet(&gpspositionsensor);
263 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
264 // this should be done in the task because it calls out to actually start the GPS serial reads
265 updateGpsSettings(0);
266 #endif
268 TickType_t xLastWakeTime;
269 xLastWakeTime = xTaskGetTickCount();
270 PERF_INIT_COUNTER(counterBytesIn, 0x97510001);
271 PERF_INIT_COUNTER(counterRate, 0x97510002);
272 PERF_INIT_COUNTER(counterParse, 0x97510003);
273 uint8_t c[GPS_READ_BUFFER];
275 // Loop forever
276 while (1) {
277 if (gpsPort) {
278 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
279 // do autoconfig stuff for UBX GPS's
280 if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
281 char *buffer = 0;
282 uint16_t count = 0;
284 gps_ubx_autoconfig_run(&buffer, &count);
285 // Something to send?
286 if (count) {
287 // clear ack/nak
288 ubxLastAck.clsID = 0x00;
289 ubxLastAck.msgID = 0x00;
290 ubxLastNak.clsID = 0x00;
291 ubxLastNak.msgID = 0x00;
293 PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
297 // need to do this whether there is received data or not, or the status (e.g. gcs) will not always be correct
298 int32_t ac_status = ubx_autoconfig_get_status();
299 static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
300 + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
301 + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
302 + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
303 gpspositionsensor.AutoConfigStatus =
304 ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
305 ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
306 ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
307 GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
308 if (gpspositionsensor.AutoConfigStatus != lastStatus) {
309 GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
310 lastStatus = gpspositionsensor.AutoConfigStatus;
312 #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
314 uint16_t cnt;
315 // This blocks the task until there is something on the buffer (or 100ms? passes)
316 cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
317 if (cnt > 0) {
318 PERF_TIMED_SECTION_START(counterParse);
319 PERF_TRACK_VALUE(counterBytesIn, cnt);
320 PERF_MEASURE_PERIOD(counterRate);
321 int res;
322 switch (gpsSettings.DataProtocol) {
323 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
324 case GPSSETTINGS_DATAPROTOCOL_NMEA:
325 res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
326 break;
327 #endif
328 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
329 case GPSSETTINGS_DATAPROTOCOL_UBX:
330 res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
331 break;
332 #endif
333 default:
334 res = NO_PARSER; // this should not happen
335 break;
337 PERF_TIMED_SECTION_END(counterParse);
339 if (res == PARSER_COMPLETE) {
340 timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
341 timeOfLastUpdateMs = timeNowMs;
342 timeOfLastCommandMs = timeNowMs;
346 // Check for GPS timeout
347 timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
348 if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
349 (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
350 // we have not received any valid GPS sentences for a while.
351 // either the GPS is not plugged in or a hardware problem or the GPS has locked up.
352 GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
353 GPSPositionSensorStatusSet(&status);
354 AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
355 } else {
356 // we appear to be receiving GPS sentences OK, we've had an update
357 // criteria for GPS-OK taken from this post...
358 // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
359 if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
360 (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
361 (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
362 AlarmsClear(SYSTEMALARMS_ALARM_GPS);
363 #ifdef PIOS_GPS_SETS_HOMELOCATION
364 HomeLocationData home;
365 HomeLocationGet(&home);
367 if (home.Set == HOMELOCATION_SET_FALSE) {
368 if (homelocationSetDelay == 0) {
369 homelocationSetDelay = xTaskGetTickCount();
371 if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
372 setHomeLocation(&gpspositionsensor);
373 homelocationSetDelay = 0;
375 } else {
376 homelocationSetDelay = 0;
378 #endif
379 } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
380 (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
381 AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
382 } else {
383 AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
386 } // if (gpsPort)
387 vTaskDelayUntil(&xLastWakeTime, GPS_LOOP_DELAY_MS / portTICK_RATE_MS);
388 } // while (1)
391 #ifdef PIOS_GPS_SETS_HOMELOCATION
393 * Estimate the acceleration due to gravity for a particular location in LLA
395 static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude)
397 /* WGS84 gravity model. The effect of gravity over latitude is strong
398 * enough to change the estimated accelerometer bias in those apps. */
399 float sinsq = sinf(latitude);
401 sinsq *= sinsq;
402 /* Likewise, over the altitude range of a high-altitude balloon, the effect
403 * due to change in altitude can also affect the model. */
404 return 9.7803267714f * (1.0f + 0.00193185138639f * sinsq) / sqrtf(1.0f - 0.00669437999013f * sinsq)
405 - 3.086e-6f * altitude;
408 // ****************
410 static void setHomeLocation(GPSPositionSensorData *gpsData)
412 HomeLocationData home;
414 HomeLocationGet(&home);
415 GPSTimeData gps;
416 GPSTimeGet(&gps);
418 if (gps.Year >= 2000) {
419 /* Store LLA */
420 home.Latitude = gpsData->Latitude;
421 home.Longitude = gpsData->Longitude;
422 home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation;
424 /* Compute home ECEF coordinates and the rotation matrix into NED
425 * Note that floats are used here - they should give enough precision
426 * for this application.*/
428 float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) };
430 /* Compute magnetic flux direction at home location */
431 if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) {
432 /*Compute local acceleration due to gravity. Vehicles that span a very large
433 * range of altitude (say, weather balloons) may need to update this during the
434 * flight. */
435 home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]);
436 home.Set = HOMELOCATION_SET_TRUE;
437 HomeLocationSet(&home);
441 #endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */
444 uint32_t hwsettings_gpsspeed_enum_to_baud(uint8_t baud)
446 switch (baud) {
447 case HWSETTINGS_GPSSPEED_2400:
448 return 2400;
450 case HWSETTINGS_GPSSPEED_4800:
451 return 4800;
453 default:
454 case HWSETTINGS_GPSSPEED_9600:
455 return 9600;
457 case HWSETTINGS_GPSSPEED_19200:
458 return 19200;
460 case HWSETTINGS_GPSSPEED_38400:
461 return 38400;
463 case HWSETTINGS_GPSSPEED_57600:
464 return 57600;
466 case HWSETTINGS_GPSSPEED_115200:
467 return 115200;
469 case HWSETTINGS_GPSSPEED_230400:
470 return 230400;
475 // having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
476 void gps_set_fc_baud_from_arg(uint8_t baud)
478 static uint8_t previous_baud = 255;
479 static uint8_t mutex; // = 0
481 // do we need this?
482 // can the code stand having two tasks/threads do an XyzSet() call at the same time?
483 if (__sync_fetch_and_add(&mutex, 1) == 0) {
484 // don't bother doing the baud change if it is actually the same
485 // might drop less data
486 if (previous_baud != baud) {
487 previous_baud = baud;
488 // Set Revo port hwsettings_baud
489 PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud));
490 GPSPositionSensorBaudRateSet(&baud);
493 --mutex;
498 * Update the GPS settings, called on startup.
499 * FIXME: This should be in the GPSSettings object. But objects have
500 * too much overhead yet. Also the GPS has no any specific settings
501 * like protocol, etc. Thus the HwSettings object which contains the
502 * GPS port speed is used for now.
505 // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
506 static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
508 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
509 static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport
510 #endif
511 // if GPS (UBX or NMEA) is enabled at all
512 if (gpsPort && gpsEnabled) {
513 // on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate
514 // after startup (previousGpsPort == gpsPort) we must set the Revo port baud rate only if autoconfig is disabled
515 // always we must set the Revo port baud rate if not using UBX
516 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
517 if (ev == NULL
518 || previousGpsPort != gpsPort
519 || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED
520 || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
521 #endif
523 uint8_t speed;
524 // Retrieve settings
525 HwSettingsGPSSpeedGet(&speed);
526 // set fc baud
527 gps_set_fc_baud_from_arg(speed);
528 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
529 // even changing the baud rate will make it re-verify the sensor type
530 // that way the user can just try some baud rates and when the sensor type is valid, the baud rate is correct
531 gps_ubx_reset_sensor_type();
532 #endif
534 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
535 else {
536 // it will never do this during startup because ev == NULL
537 gps_ubx_autoconfig_set(NULL);
539 #endif
541 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
542 previousGpsPort = gpsPort;
543 #endif
547 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
548 void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
550 load_mag_settings();
554 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
556 ubx_autoconfig_settings_t newconfig;
558 GPSSettingsGet(&gpsSettings);
560 // it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NMEA
561 // because ubx auto config never gets called
562 // setting it up completely means that if we switch from initial NMEA to UBX or disabled to enabled, that it will start normally
563 newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
564 newconfig.navRate = gpsSettings.UbxRate;
565 newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
566 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
567 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
568 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
569 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
570 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
571 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
572 gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
573 UBX_DYNMODEL_AIRBORNE1G;
575 switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
576 case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
577 case GPSSETTINGS_UBXSBASMODE_CORRECTION:
578 case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
579 case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
580 newconfig.SBASCorrection = true;
581 break;
582 default:
583 newconfig.SBASCorrection = false;
586 switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
587 case GPSSETTINGS_UBXSBASMODE_RANGING:
588 case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
589 case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
590 case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
591 newconfig.SBASRanging = true;
592 break;
593 default:
594 newconfig.SBASRanging = false;
597 switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
598 case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
599 case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
600 case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
601 case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
602 newconfig.SBASIntegrity = true;
603 break;
604 default:
605 newconfig.SBASIntegrity = false;
608 newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed;
610 newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
611 gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
612 gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
613 gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
614 gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
616 switch (gpsSettings.UbxGNSSMode) {
617 case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS:
618 newconfig.enableGPS = true;
619 newconfig.enableGLONASS = true;
620 newconfig.enableBeiDou = false;
621 break;
622 case GPSSETTINGS_UBXGNSSMODE_GLONASS:
623 newconfig.enableGPS = false;
624 newconfig.enableGLONASS = true;
625 newconfig.enableBeiDou = false;
626 break;
627 case GPSSETTINGS_UBXGNSSMODE_GPS:
628 newconfig.enableGPS = true;
629 newconfig.enableGLONASS = false;
630 newconfig.enableBeiDou = false;
631 break;
632 case GPSSETTINGS_UBXGNSSMODE_GPSBEIDOU:
633 newconfig.enableGPS = true;
634 newconfig.enableGLONASS = false;
635 newconfig.enableBeiDou = true;
636 break;
637 case GPSSETTINGS_UBXGNSSMODE_GLONASSBEIDOU:
638 newconfig.enableGPS = false;
639 newconfig.enableGLONASS = true;
640 newconfig.enableBeiDou = true;
641 break;
642 default:
643 newconfig.enableGPS = false;
644 newconfig.enableGLONASS = false;
645 newconfig.enableBeiDou = false;
646 break;
649 gps_ubx_autoconfig_set(&newconfig);
651 #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
653 * @}
654 * @}