?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Parse GPS DOP values (#11912)
[betaflight.git] / src / main / io / gps.c
blob677bf090f666722741f00c395b661d77bde0840b
1 /*
2 * This file is part of Cleanflight and Betaflight.
3 *
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
9 *
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
17 *
18 * If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <ctype.h>
24 #include <string.h>
25 #include <math.h>
26
27 #include "platform.h"
28
29 #ifdef USE_GPS
30
31 #include "build/build_config.h"
32 #include "build/debug.h"
33
34 #include "common/axis.h"
35 #include "common/gps_conversion.h"
36 #include "common/maths.h"
37 #include "common/utils.h"
38
39 #include "config/feature.h"
40 #include "pg/pg.h"
41 #include "pg/pg_ids.h"
42
43 #include "drivers/light_led.h"
44 #include "drivers/time.h"
45
46 #include "io/beeper.h"
47 #include "io/dashboard.h"
48 #include "io/gps.h"
49 #include "io/serial.h"
50
51 #include "config/config.h"
52 #include "fc/runtime_config.h"
53
54 #include "flight/imu.h"
55 #include "flight/pid.h"
56 #include "flight/gps_rescue.h"
57
58 #include "scheduler/scheduler.h"
59
60 #include "sensors/sensors.h"
61
62 #define LOG_ERROR '?'
63 #define LOG_IGNORED '!'
64 #define LOG_SKIPPED '>'
65 #define LOG_NMEA_GGA 'g'
66 #define LOG_NMEA_GSA 's'
67 #define LOG_NMEA_RMC 'r'
68 #define LOG_UBLOX_DOP 'D'
69 #define LOG_UBLOX_SOL 'O'
70 #define LOG_UBLOX_STATUS 'S'
71 #define LOG_UBLOX_SVINFO 'I'
72 #define LOG_UBLOX_POSLLH 'P'
73 #define LOG_UBLOX_VELNED 'V'
74
75 #define DEBUG_SERIAL_BAUD 0 // set to 1 to debug serial port baud config (/100)
76 #define DEBUG_UBLOX_INIT 0 // set to 1 to debug ublox initialization
77 #define DEBUG_UBLOX_FRAMES 0 // set to 1 to debug ublox received frames
78
79 char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
80 static char *gpsPacketLogChar = gpsPacketLog;
81 // **********************
82 // GPS
83 // **********************
84 int32_t GPS_home[2];
85 uint16_t GPS_distanceToHome; // distance to home point in meters
86 uint32_t GPS_distanceToHomeCm;
87 int16_t GPS_directionToHome; // direction to home or hol point in degrees * 10
88 uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
89 int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s
90 int16_t nav_takeoff_bearing;
91
92 #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 0.54 km/h 0.335 mph
93
94 gpsSolutionData_t gpsSol;
95 uint32_t GPS_packetCount = 0;
96 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
97 uint8_t GPS_update = 0; // toogle to distinct a GPS position update (directly or via MSP)
98
99 uint8_t GPS_numCh; // Details on numCh/svinfo in gps.h
100 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N];
101 uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N];
102 uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N];
103 uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
104
105 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
106 #define GPS_TIMEOUT (2500)
107 // How many entries in gpsInitData array below
108 #define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
109 #define GPS_BAUDRATE_CHANGE_DELAY (200)
110 // Timeout for waiting ACK/NAK in GPS task cycles (0.25s at 100Hz)
111 #define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)
112
113 static serialPort_t *gpsPort;
114
115 typedef struct gpsInitData_s {
116 uint8_t index;
117 uint8_t baudrateIndex; // see baudRate_e
118 const char *ubx;
119 const char *mtk;
120 } gpsInitData_t;
121
122 // NMEA will cycle through these until valid data is received
123 static const gpsInitData_t gpsInitData[] = {
124 { GPS_BAUDRATE_115200, BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
125 { GPS_BAUDRATE_57600, BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
126 { GPS_BAUDRATE_38400, BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
127 { GPS_BAUDRATE_19200, BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
128 // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
129 { GPS_BAUDRATE_9600, BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
130 };
131
132 #define GPS_INIT_DATA_ENTRY_COUNT ARRAYLEN(gpsInitData)
133
134 #define DEFAULT_BAUD_RATE_INDEX 0
135
136 #ifdef USE_GPS_UBLOX
137 enum {
138 PREAMBLE1 = 0xB5,
139 PREAMBLE2 = 0x62,
140 CLASS_NAV = 0x01,
141 CLASS_ACK = 0x05,
142 CLASS_CFG = 0x06,
143 MSG_ACK_NACK = 0x00,
144 MSG_ACK_ACK = 0x01,
145 MSG_POSLLH = 0x2,
146 MSG_STATUS = 0x3,
147 MSG_DOP = 0x4,
148 MSG_SOL = 0x6,
149 MSG_PVT = 0x7,
150 MSG_VELNED = 0x12,
151 MSG_SVINFO = 0x30,
152 MSG_SAT = 0x35,
153 MSG_CFG_MSG = 0x1,
154 MSG_CFG_PRT = 0x00,
155 MSG_CFG_RATE = 0x08,
156 MSG_CFG_SET_RATE = 0x01,
157 MSG_CFG_SBAS = 0x16,
158 MSG_CFG_NAV_SETTINGS = 0x24,
159 MSG_CFG_GNSS = 0x3E
160 } ubx_protocol_bytes;
161
162 #define UBLOX_MODE_ENABLED 0x1
163 #define UBLOX_MODE_TEST 0x2
164
165 #define UBLOX_USAGE_RANGE 0x1
166 #define UBLOX_USAGE_DIFFCORR 0x2
167 #define UBLOX_USAGE_INTEGRITY 0x4
168
169 #define UBLOX_GNSS_ENABLE 0x1
170 #define UBLOX_GNSS_DEFAULT_SIGCFGMASK 0x10000
171
172 #define UBLOX_DYNMODE_PEDESTRIAN 3
173 #define UBLOX_DYNMODE_AIRBORNE_1G 6
174 #define UBLOX_DYNMODE_AIRBORNE_4G 8
175
176 typedef struct {
177 uint8_t preamble1;
178 uint8_t preamble2;
179 uint8_t msg_class;
180 uint8_t msg_id;
181 uint16_t length;
182 } ubx_header;
183
184 typedef struct {
185 uint8_t gnssId;
186 uint8_t resTrkCh;
187 uint8_t maxTrkCh;
188 uint8_t reserved1;
189 uint32_t flags;
190 } ubx_configblock;
191
192 typedef struct {
193 uint8_t msgClass;
194 uint8_t msgID;
195 } ubx_poll_msg;
196
197 typedef struct {
198 uint8_t msgClass;
199 uint8_t msgID;
200 uint8_t rate;
201 } ubx_cfg_msg;
202
203 typedef struct {
204 uint16_t measRate;
205 uint16_t navRate;
206 uint16_t timeRef;
207 } ubx_cfg_rate;
208
209 typedef struct {
210 uint8_t mode;
211 uint8_t usage;
212 uint8_t maxSBAS;
213 uint8_t scanmode2;
214 uint32_t scanmode1;
215 } ubx_cfg_sbas;
216
217 typedef struct {
218 uint8_t msgVer;
219 uint8_t numTrkChHw;
220 uint8_t numTrkChUse;
221 uint8_t numConfigBlocks;
222 ubx_configblock configblocks[7];
223 } ubx_cfg_gnss;
224
225 typedef struct {
226 uint16_t mask;
227 uint8_t dynModel;
228 uint8_t fixMode;
229 int32_t fixedAlt;
230 uint32_t fixedAltVar;
231 int8_t minElev;
232 uint8_t drLimit;
233 uint16_t pDOP;
234 uint16_t tDOP;
235 uint16_t pAcc;
236 uint16_t tAcc;
237 uint8_t staticHoldThresh;
238 uint8_t dgnssTimeout;
239 uint8_t cnoThreshNumSVs;
240 uint8_t cnoThresh;
241 uint8_t reserved0[2];
242 uint16_t staticHoldMaxDist;
243 uint8_t utcStandard;
244 uint8_t reserved1[5];
245 } ubx_cfg_nav5;
246
247 typedef union {
248 ubx_poll_msg poll_msg;
249 ubx_cfg_msg cfg_msg;
250 ubx_cfg_rate cfg_rate;
251 ubx_cfg_nav5 cfg_nav5;
252 ubx_cfg_sbas cfg_sbas;
253 ubx_cfg_gnss cfg_gnss;
254 } ubx_payload;
255
256 typedef struct {
257 ubx_header header;
258 ubx_payload payload;
259 } __attribute__((packed)) ubx_message;
260
261 #endif // USE_GPS_UBLOX
262
263 typedef enum {
264 GPS_STATE_UNKNOWN,
265 GPS_STATE_INITIALIZING,
266 GPS_STATE_INITIALIZED,
267 GPS_STATE_CHANGE_BAUD,
268 GPS_STATE_CONFIGURE,
269 GPS_STATE_RECEIVING_DATA,
270 GPS_STATE_LOST_COMMUNICATION,
271 GPS_STATE_COUNT
272 } gpsState_e;
273
274 // Max time to wait for received data
275 #define GPS_MAX_WAIT_DATA_RX 30
276
277 gpsData_t gpsData;
278
279 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
280
281 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
282 .provider = GPS_UBLOX,
283 .sbasMode = SBAS_NONE,
284 .autoConfig = GPS_AUTOCONFIG_ON,
285 .autoBaud = GPS_AUTOBAUD_OFF,
286 .gps_ublox_use_galileo = false,
287 .gps_ublox_mode = UBLOX_AIRBORNE,
288 .gps_set_home_point_once = false,
289 .gps_use_3d_speed = false,
290 .sbas_integrity = false,
291 );
292
293 static void shiftPacketLog(void)
294 {
295 uint32_t i;
296
297 for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) {
298 gpsPacketLog[i] = gpsPacketLog[i-1];
299 }
300 }
301
302 static bool isConfiguratorConnected(void)
303 {
304 return (getArmingDisableFlags() & ARMING_DISABLED_MSP);
305 }
306
307 static void gpsNewData(uint16_t c);
308 #ifdef USE_GPS_NMEA
309 static bool gpsNewFrameNMEA(char c);
310 #endif
311 #ifdef USE_GPS_UBLOX
312 static bool gpsNewFrameUBLOX(uint8_t data);
313 #endif
314
315 static void gpsSetState(gpsState_e state)
316 {
317 gpsData.lastMessage = millis();
318 sensorsClear(SENSOR_GPS);
319
320 gpsData.state = state;
321 gpsData.state_position = 0;
322 gpsData.state_ts = millis();
323 gpsData.ackState = UBLOX_ACK_IDLE;
324 }
325
326 void gpsInit(void)
327 {
328 gpsData.baudrateIndex = 0;
329 gpsData.errors = 0;
330 gpsData.timeouts = 0;
331
332 memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
333
334 // init gpsData structure. if we're not actually enabled, don't bother doing anything else
335 gpsSetState(GPS_STATE_UNKNOWN);
336
337 gpsData.lastMessage = millis();
338
339 if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
340 gpsSetState(GPS_STATE_INITIALIZED);
341 return;
342 }
343
344 const serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
345 if (!gpsPortConfig) {
346 return;
347 }
348
349 while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) {
350 gpsData.baudrateIndex++;
351 if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
352 gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
353 break;
354 }
355 }
356
357 portMode_e mode = MODE_RXTX;
358 #if defined(GPS_NMEA_TX_ONLY)
359 if (gpsConfig()->provider == GPS_NMEA) {
360 mode &= ~MODE_TX;
361 }
362 #endif
363
364 // no callback - buffer will be consumed in gpsUpdate()
365 gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
366 if (!gpsPort) {
367 return;
368 }
369
370 // signal GPS "thread" to initialize when it gets to it
371 gpsSetState(GPS_STATE_INITIALIZING);
372 }
373
374 #ifdef USE_GPS_NMEA
375 void gpsInitNmea(void)
376 {
377 #if !defined(GPS_NMEA_TX_ONLY)
378 uint32_t now;
379 #endif
380 switch (gpsData.state) {
381 case GPS_STATE_INITIALIZING:
382 #if !defined(GPS_NMEA_TX_ONLY)
383 now = millis();
384 if (now - gpsData.state_ts < 1000) {
385 return;
386 }
387 gpsData.state_ts = now;
388 if (gpsData.state_position < 1) {
389 serialSetBaudRate(gpsPort, 4800);
390 gpsData.state_position++;
391 } else if (gpsData.state_position < 2) {
392 // print our FIXED init string for the baudrate we want to be at
393 serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
394 gpsData.state_position++;
395 } else {
396 // we're now (hopefully) at the correct rate, next state will switch to it
397 gpsSetState(GPS_STATE_CHANGE_BAUD);
398 }
399 break;
400 #endif
401 case GPS_STATE_CHANGE_BAUD:
402 #if !defined(GPS_NMEA_TX_ONLY)
403 now = millis();
404 if (now - gpsData.state_ts < 1000) {
405 return;
406 }
407 gpsData.state_ts = now;
408 if (gpsData.state_position < 1) {
409 serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
410 gpsData.state_position++;
411 } else if (gpsData.state_position < 2) {
412 serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
413 gpsData.state_position++;
414 } else
415 #else
416 {
417 serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
418 }
419 #endif
420 gpsSetState(GPS_STATE_RECEIVING_DATA);
421 break;
422 }
423 }
424 #endif // USE_GPS_NMEA
425
426 #ifdef USE_GPS_UBLOX
427 static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, uint8_t *checksumB)
428 {
429 *checksumA += data;
430 *checksumB += *checksumA;
431 serialWrite(gpsPort, data);
432 }
433
434 static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_t *checksumA, uint8_t *checksumB)
435 {
436 while (len--) {
437 ubloxSendByteUpdateChecksum(*data, checksumA, checksumB);
438 data++;
439 }
440 }
441
442 static void ubloxSendMessage(const uint8_t *data, uint8_t len)
443 {
444 uint8_t checksumA = 0, checksumB = 0;
445 serialWrite(gpsPort, data[0]);
446 serialWrite(gpsPort, data[1]);
447 ubloxSendDataUpdateChecksum(&data[2], len - 2, &checksumA, &checksumB);
448 serialWrite(gpsPort, checksumA);
449 serialWrite(gpsPort, checksumB);
450
451 // Save state for ACK waiting
452 gpsData.ackWaitingMsgId = data[3]; //save message id for ACK
453 gpsData.ackTimeoutCounter = 0;
454 gpsData.ackState = UBLOX_ACK_WAITING;
455 }
456
457 static void ubloxSendConfigMessage(ubx_message *message, uint8_t msg_id, uint8_t length)
458 {
459 message->header.preamble1 = PREAMBLE1;
460 message->header.preamble2 = PREAMBLE2;
461 message->header.msg_class = CLASS_CFG;
462 message->header.msg_id = msg_id;
463 message->header.length = length;
464 ubloxSendMessage((const uint8_t *) message, length + 6);
465 }
466
467 static void ubloxSendPollMessage(uint8_t msg_id)
468 {
469 ubx_message tx_buffer;
470 tx_buffer.header.preamble1 = PREAMBLE1;
471 tx_buffer.header.preamble2 = PREAMBLE2;
472 tx_buffer.header.msg_class = CLASS_CFG;
473 tx_buffer.header.msg_id = msg_id;
474 tx_buffer.header.length = 0;
475 ubloxSendMessage((const uint8_t *) &tx_buffer, 6);
476 }
477
478 static void ubloxSendNAV5Message(bool airborne)
479 {
480 ubx_message tx_buffer;
481 tx_buffer.payload.cfg_nav5.mask = 0xFFFF;
482 if (airborne) {
483 #if defined(GPS_UBLOX_MODE_AIRBORNE_1G)
484 tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_AIRBORNE_1G;
485 #else
486 tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_AIRBORNE_4G;
487 #endif
488 } else {
489 tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_PEDESTRIAN;
490 }
491 tx_buffer.payload.cfg_nav5.fixMode = 3;
492 tx_buffer.payload.cfg_nav5.fixedAlt = 0;
493 tx_buffer.payload.cfg_nav5.fixedAltVar = 10000;
494 tx_buffer.payload.cfg_nav5.minElev = 5;
495 tx_buffer.payload.cfg_nav5.drLimit = 0;
496 tx_buffer.payload.cfg_nav5.pDOP = 250;
497 tx_buffer.payload.cfg_nav5.tDOP = 250;
498 tx_buffer.payload.cfg_nav5.pAcc = 100;
499 tx_buffer.payload.cfg_nav5.tAcc = 300;
500 tx_buffer.payload.cfg_nav5.staticHoldThresh = 0;
501 tx_buffer.payload.cfg_nav5.dgnssTimeout = 60;
502 tx_buffer.payload.cfg_nav5.cnoThreshNumSVs = 0;
503 tx_buffer.payload.cfg_nav5.cnoThresh = 0;
504 tx_buffer.payload.cfg_nav5.reserved0[0] = 0;
505 tx_buffer.payload.cfg_nav5.reserved0[1] = 0;
506 tx_buffer.payload.cfg_nav5.staticHoldMaxDist = 200;
507 tx_buffer.payload.cfg_nav5.utcStandard = 0;
508 tx_buffer.payload.cfg_nav5.reserved1[0] = 0;
509 tx_buffer.payload.cfg_nav5.reserved1[1] = 0;
510 tx_buffer.payload.cfg_nav5.reserved1[2] = 0;
511 tx_buffer.payload.cfg_nav5.reserved1[3] = 0;
512 tx_buffer.payload.cfg_nav5.reserved1[4] = 0;
513
514 ubloxSendConfigMessage(&tx_buffer, MSG_CFG_NAV_SETTINGS, sizeof(ubx_cfg_nav5));
515 }
516
517 static void ubloxSetMessageRate(uint8_t messageClass, uint8_t messageID, uint8_t rate)
518 {
519 ubx_message tx_buffer;
520 tx_buffer.payload.cfg_msg.msgClass = messageClass;
521 tx_buffer.payload.cfg_msg.msgID = messageID;
522 tx_buffer.payload.cfg_msg.rate = rate;
523 ubloxSendConfigMessage(&tx_buffer, MSG_CFG_MSG, sizeof(ubx_cfg_msg));
524 }
525
526 static void ubloxSetNavRate(uint16_t measRate, uint16_t navRate, uint16_t timeRef)
527 {
528 ubx_message tx_buffer;
529 tx_buffer.payload.cfg_rate.measRate = measRate;
530 tx_buffer.payload.cfg_rate.navRate = navRate;
531 tx_buffer.payload.cfg_rate.timeRef = timeRef;
532 ubloxSendConfigMessage(&tx_buffer, MSG_CFG_RATE, sizeof(ubx_cfg_rate));
533 }
534
535 static void ubloxSetSbas(void)
536 {
537 ubx_message tx_buffer;
538
539 //NOTE: default ublox config for sbas mode is: UBLOX_MODE_ENABLED, test is disabled
540 tx_buffer.payload.cfg_sbas.mode = UBLOX_MODE_TEST;
541 if (gpsConfig()->sbasMode != SBAS_NONE) {
542 tx_buffer.payload.cfg_sbas.mode |= UBLOX_MODE_ENABLED;
543 }
544
545 //NOTE: default ublox config for sbas mode is: UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR, integrity is disabled
546 tx_buffer.payload.cfg_sbas.usage = UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR;
547 if (gpsConfig()->sbas_integrity) {
548 tx_buffer.payload.cfg_sbas.usage |= UBLOX_USAGE_INTEGRITY;
549 }
550
551 tx_buffer.payload.cfg_sbas.maxSBAS = 3;
552 tx_buffer.payload.cfg_sbas.scanmode2 = 0;
553 switch (gpsConfig()->sbasMode) {
554 case SBAS_AUTO:
555 tx_buffer.payload.cfg_sbas.scanmode1 = 0;
556 break;
557 case SBAS_EGNOS:
558 tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136
559 break;
560 case SBAS_WAAS:
561 tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138
562 break;
563 case SBAS_MSAS:
564 tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137
565 break;
566 case SBAS_GAGAN:
567 tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132
568 break;
569 default:
570 tx_buffer.payload.cfg_sbas.scanmode1 = 0;
571 break;
572 }
573 ubloxSendConfigMessage(&tx_buffer, MSG_CFG_SBAS, sizeof(ubx_cfg_sbas));
574 }
575
576 void gpsInitUblox(void)
577 {
578 uint32_t now;
579 // UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate
580
581 // Wait until GPS transmit buffer is empty
582 if (!isSerialTransmitBufferEmpty(gpsPort))
583 return;
584
585 switch (gpsData.state) {
586 case GPS_STATE_INITIALIZING:
587 now = millis();
588 if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
589 return;
590
591 if (gpsData.state_position < GPS_INIT_ENTRIES) {
592 // try different speed to INIT
593 baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex;
594
595 gpsData.state_ts = now;
596
597 if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) {
598 // change the rate if needed and wait a little
599 serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]);
600 #if DEBUG_SERIAL_BAUD
601 debug[0] = baudRates[newBaudRateIndex] / 100;
602 #endif
603 return;
604 }
605
606 // print our FIXED init string for the baudrate we want to be at
607 serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
608
609 gpsData.state_position++;
610 } else {
611 // we're now (hopefully) at the correct rate, next state will switch to it
612 gpsSetState(GPS_STATE_CHANGE_BAUD);
613 }
614 break;
615
616 case GPS_STATE_CHANGE_BAUD:
617 serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
618 #if DEBUG_SERIAL_BAUD
619 debug[0] = baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex] / 100;
620 #endif
621 gpsSetState(GPS_STATE_CONFIGURE);
622 break;
623
624 case GPS_STATE_CONFIGURE:
625 // Either use specific config file for GPS or let dynamically upload config
626 if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) {
627 gpsSetState(GPS_STATE_RECEIVING_DATA);
628 break;
629 }
630
631 if (gpsData.ackState == UBLOX_ACK_IDLE) {
632 switch (gpsData.state_position) {
633 case 0:
634 gpsData.ubloxUsePVT = true;
635 gpsData.ubloxUseSAT = true;
636 ubloxSendNAV5Message(gpsConfig()->gps_ublox_mode == UBLOX_AIRBORNE);
637 break;
638 case 1: //Disable NMEA Messages
639 ubloxSetMessageRate(0xF0, 0x05, 0); // VGS: Course over ground and Ground speed
640 break;
641 case 2:
642 ubloxSetMessageRate(0xF0, 0x03, 0); // GSV: GNSS Satellites in View
643 break;
644 case 3:
645 ubloxSetMessageRate(0xF0, 0x01, 0); // GLL: Latitude and longitude, with time of position fix and status
646 break;
647 case 4:
648 ubloxSetMessageRate(0xF0, 0x00, 0); // GGA: Global positioning system fix data
649 break;
650 case 5:
651 ubloxSetMessageRate(0xF0, 0x02, 0); // GSA: GNSS DOP and Active Satellites
652 break;
653 case 6:
654 ubloxSetMessageRate(0xF0, 0x04, 0); // RMC: Recommended Minimum data
655 break;
656 case 7: //Enable UBLOX Messages
657 if (gpsData.ubloxUsePVT) {
658 ubloxSetMessageRate(CLASS_NAV, MSG_PVT, 1); // set PVT MSG rate
659 } else {
660 ubloxSetMessageRate(CLASS_NAV, MSG_SOL, 1); // set SOL MSG rate
661 }
662 break;
663 case 8:
664 if (gpsData.ubloxUsePVT) {
665 gpsData.state_position++;
666 } else {
667 ubloxSetMessageRate(CLASS_NAV, MSG_POSLLH, 1); // set POSLLH MSG rate
668 }
669 break;
670 case 9:
671 if (gpsData.ubloxUsePVT) {
672 gpsData.state_position++;
673 } else {
674 ubloxSetMessageRate(CLASS_NAV, MSG_STATUS, 1); // set STATUS MSG rate
675 }
676 break;
677 case 10:
678 if (gpsData.ubloxUsePVT) {
679 gpsData.state_position++;
680 } else {
681 ubloxSetMessageRate(CLASS_NAV, MSG_VELNED, 1); // set VELNED MSG rate
682 }
683 break;
684 case 11:
685 if (gpsData.ubloxUseSAT) {
686 ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
687 } else {
688 ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
689 }
690 break;
691 case 12:
692 ubloxSetNavRate(0x64, 1, 1); // set rate to 10Hz (measurement period: 100ms, navigation rate: 1 cycle) (for 5Hz use 0xC8)
693 break;
694 case 13:
695 ubloxSetSbas();
696 break;
697 case 14:
698 if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) {
699 ubloxSendPollMessage(MSG_CFG_GNSS);
700 } else {
701 gpsSetState(GPS_STATE_RECEIVING_DATA);
702 }
703 break;
704 default:
705 break;
706 }
707 }
708
709 switch (gpsData.ackState) {
710 case UBLOX_ACK_IDLE:
711 break;
712 case UBLOX_ACK_WAITING:
713 if ((++gpsData.ackTimeoutCounter) == UBLOX_ACK_TIMEOUT_MAX_COUNT) {
714 gpsSetState(GPS_STATE_LOST_COMMUNICATION);
715 }
716 break;
717 case UBLOX_ACK_GOT_ACK:
718 if (gpsData.state_position == 14) {
719 // ublox should be initialised, try receiving
720 gpsSetState(GPS_STATE_RECEIVING_DATA);
721 } else {
722 gpsData.state_position++;
723 gpsData.ackState = UBLOX_ACK_IDLE;
724 }
725 break;
726 case UBLOX_ACK_GOT_NACK:
727 if (gpsData.state_position == 7) { // If we were asking for NAV-PVT...
728 gpsData.ubloxUsePVT = false; // ...retry asking for NAV-SOL
729 gpsData.ackState = UBLOX_ACK_IDLE;
730 } else {
731 if (gpsData.state_position == 11) { // If we were asking for NAV-SAT...
732 gpsData.ubloxUseSAT = false; // ...retry asking for NAV-SVINFO
733 gpsData.ackState = UBLOX_ACK_IDLE;
734 } else {
735 gpsSetState(GPS_STATE_CONFIGURE);
736 }
737 }
738 break;
739 }
740
741 break;
742 }
743 }
744 #endif // USE_GPS_UBLOX
745
746 void gpsInitHardware(void)
747 {
748 switch (gpsConfig()->provider) {
749 case GPS_NMEA:
750 #ifdef USE_GPS_NMEA
751 gpsInitNmea();
752 #endif
753 break;
754
755 case GPS_UBLOX:
756 #ifdef USE_GPS_UBLOX
757 gpsInitUblox();
758 #endif
759 break;
760 default:
761 break;
762 }
763 }
764
765 static void updateGpsIndicator(timeUs_t currentTimeUs)
766 {
767 static uint32_t GPSLEDTime;
768 if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) {
769 GPSLEDTime = currentTimeUs + 150000;
770 LED1_TOGGLE;
771 }
772 }
773
774 void gpsUpdate(timeUs_t currentTimeUs)
775 {
776 static gpsState_e gpsStateDurationUs[GPS_STATE_COUNT];
777 timeUs_t executeTimeUs;
778 gpsState_e gpsCurrentState = gpsData.state;
779
780 // read out available GPS bytes
781 if (gpsPort) {
782 while (serialRxBytesWaiting(gpsPort)) {
783 if (cmpTimeUs(micros(), currentTimeUs) > GPS_MAX_WAIT_DATA_RX) {
784 // Wait 1ms and come back
785 rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE_FAST));
786 return;
787 }
788 gpsNewData(serialRead(gpsPort));
789 }
790 // Restore default task rate
791 rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
792 } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
793 gpsSetState(GPS_STATE_RECEIVING_DATA);
794 onGpsNewData();
795 GPS_update &= ~GPS_MSP_UPDATE;
796 }
797
798 #if DEBUG_UBLOX_INIT
799 debug[0] = gpsData.state;
800 debug[1] = gpsData.state_position;
801 debug[2] = gpsData.ackState;
802 #endif
803
804 switch (gpsData.state) {
805 case GPS_STATE_UNKNOWN:
806 case GPS_STATE_INITIALIZED:
807 break;
808
809 case GPS_STATE_INITIALIZING:
810 case GPS_STATE_CHANGE_BAUD:
811 case GPS_STATE_CONFIGURE:
812 gpsInitHardware();
813 break;
814
815 case GPS_STATE_LOST_COMMUNICATION:
816 gpsData.timeouts++;
817 if (gpsConfig()->autoBaud) {
818 // try another rate
819 gpsData.baudrateIndex++;
820 gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
821 }
822 gpsSol.numSat = 0;
823 DISABLE_STATE(GPS_FIX);
824 gpsSetState(GPS_STATE_INITIALIZING);
825 break;
826
827 case GPS_STATE_RECEIVING_DATA:
828 // check for no data/gps timeout/cable disconnection etc
829 if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
830 gpsSetState(GPS_STATE_LOST_COMMUNICATION);
831 #ifdef USE_GPS_UBLOX
832 } else {
833 if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
834 switch (gpsData.state_position) {
835 case 0:
836 if (!isConfiguratorConnected()) {
837 if (gpsData.ubloxUseSAT) {
838 ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG
839 } else {
840 ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG
841 }
842 gpsData.state_position = 1;
843 }
844 break;
845 case 1:
846 if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) {
847 ubloxSendNAV5Message(true);
848 gpsData.state_position = 2;
849 }
850 if (isConfiguratorConnected()) {
851 gpsData.state_position = 2;
852 }
853 break;
854 case 2:
855 if (isConfiguratorConnected()) {
856 if (gpsData.ubloxUseSAT) {
857 ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
858 } else {
859 ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
860 }
861 gpsData.state_position = 0;
862 }
863 break;
864 }
865 }
866 #endif //USE_GPS_UBLOX
867 }
868 break;
869 }
870
871 if (sensors(SENSOR_GPS)) {
872 updateGpsIndicator(currentTimeUs);
873 }
874
875 if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) {
876 // clear the home fix icon between arms if the user configuration is to reset home point between arms
877 DISABLE_STATE(GPS_FIX_HOME);
878 }
879
880 #if defined(USE_GPS_RESCUE)
881 if (gpsRescueIsConfigured()) {
882 updateGPSRescueState();
883 }
884 #endif
885
886 static bool hasBeeped = false;
887 if (!ARMING_FLAG(ARMED)) {
888 // while disarmed, beep when requirements for a home fix are met
889 // ?? should we also beep if home fix requirements first appear after arming?
890 if (!hasBeeped && STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
891 beeper(BEEPER_READY_BEEP);
892 hasBeeped = true;
893 }
894 }
895
896 DEBUG_SET(DEBUG_GPS_DOP, 0, gpsSol.numSat);
897 DEBUG_SET(DEBUG_GPS_DOP, 1, gpsSol.dop.pdop);
898 DEBUG_SET(DEBUG_GPS_DOP, 2, gpsSol.dop.hdop);
899 DEBUG_SET(DEBUG_GPS_DOP, 3, gpsSol.dop.vdop);
900
901 executeTimeUs = micros() - currentTimeUs;
902 if (executeTimeUs > gpsStateDurationUs[gpsCurrentState]) {
903 gpsStateDurationUs[gpsCurrentState] = executeTimeUs;
904 }
905 schedulerSetNextStateTime(gpsStateDurationUs[gpsData.state]);
906 }
907
908 static void gpsNewData(uint16_t c)
909 {
910 if (!gpsNewFrame(c)) {
911 return;
912 }
913
914 if (gpsData.state == GPS_STATE_RECEIVING_DATA) {
915 // new data received and parsed, we're in business
916 gpsData.lastLastMessage = gpsData.lastMessage;
917 gpsData.lastMessage = millis();
918 sensorsSet(SENSOR_GPS);
919 }
920
921 GPS_update ^= GPS_DIRECT_TICK;
922
923 #if DEBUG_UBLOX_INIT
924 debug[3] = GPS_update;
925 #endif
926
927 onGpsNewData();
928 }
929
930 bool gpsNewFrame(uint8_t c)
931 {
932 switch (gpsConfig()->provider) {
933 case GPS_NMEA: // NMEA
934 #ifdef USE_GPS_NMEA
935 return gpsNewFrameNMEA(c);
936 #endif
937 break;
938 case GPS_UBLOX: // UBX binary
939 #ifdef USE_GPS_UBLOX
940 return gpsNewFrameUBLOX(c);
941 #endif
942 break;
943 default:
944 break;
945 }
946 return false;
947 }
948
949 // Check for healthy communications
950 bool gpsIsHealthy(void)
951 {
952 return (gpsData.state == GPS_STATE_RECEIVING_DATA);
953 }
954
955 /* This is a light implementation of a GPS frame decoding
956 This should work with most of modern GPS devices configured to output 5 frames.
957 It assumes there are some NMEA GGA frames to decode on the serial bus
958 Now verifies checksum correctly before applying data
959
960 Here we use only the following data :
961 - latitude
962 - longitude
963 - GPS fix is/is not ok
964 - GPS num sat (4 is enough to be +/- reliable)
965 // added by Mis
966 - GPS altitude (for OSD displaying)
967 - GPS speed (for OSD displaying)
968 */
969
970 #define NO_FRAME 0
971 #define FRAME_GGA 1
972 #define FRAME_RMC 2
973 #define FRAME_GSV 3
974 #define FRAME_GSA 4
975
976
977 // This code is used for parsing NMEA data
978
979 /* Alex optimization
980 The latitude or longitude is coded this way in NMEA frames
981 dm.f coded as degrees + minutes + minute decimal
982 Where:
983 - d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
984 - m is always 2 char long
985 - f can be 1 or more char long
986 This function converts this format in a unique unsigned long where 1 degree = 10 000 000
987
988 EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
989 with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
990 resolution also increased precision of nav calculations
991 static uint32_t GPS_coord_to_degrees(char *coordinateString)
992 {
993 char *p = s, *d = s;
994 uint8_t min, deg = 0;
995 uint16_t frac = 0, mult = 10000;
996
997 while (*p) { // parse the string until its end
998 if (d != s) {
999 frac += (*p - '0') * mult; // calculate only fractional part on up to 5 digits (d != s condition is true when the . is located)
1000 mult /= 10;
1001 }
1002 if (*p == '.')
1003 d = p; // locate '.' char in the string
1004 p++;
1005 }
1006 if (p == s)
1007 return 0;
1008 while (s < d - 2) {
1009 deg *= 10; // convert degrees : all chars before minutes ; for the first iteration, deg = 0
1010 deg += *(s++) - '0';
1011 }
1012 min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.'
1013 return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
1014 }
1015 */
1016
1017 // helper functions
1018 #ifdef USE_GPS_NMEA
1019 static uint32_t grab_fields(char *src, uint8_t mult)
1020 { // convert string to uint32
1021 uint32_t i;
1022 uint32_t tmp = 0;
1023 int isneg = 0;
1024 for (i = 0; src[i] != 0; i++) {
1025 if ((i == 0) && (src[0] == '-')) { // detect negative sign
1026 isneg = 1;
1027 continue; // jump to next character if the first one was a negative sign
1028 }
1029 if (src[i] == '.') {
1030 i++;
1031 if (mult == 0) {
1032 break;
1033 } else {
1034 src[i + mult] = 0;
1035 }
1036 }
1037 tmp *= 10;
1038 if (src[i] >= '0' && src[i] <= '9') {
1039 tmp += src[i] - '0';
1040 }
1041 if (i >= 15) {
1042 return 0; // out of bounds
1043 }
1044 }
1045 return isneg ? -tmp : tmp; // handle negative altitudes
1046 }
1047
1048 typedef struct gpsDataNmea_s {
1049 int32_t latitude;
1050 int32_t longitude;
1051 uint8_t numSat;
1052 int32_t altitudeCm;
1053 uint16_t speed;
1054 uint16_t pdop;
1055 uint16_t hdop;
1056 uint16_t vdop;
1057 uint16_t ground_course;
1058 uint32_t time;
1059 uint32_t date;
1060 } gpsDataNmea_t;
1061
1062 static bool gpsNewFrameNMEA(char c)
1063 {
1064 static gpsDataNmea_t gps_Msg;
1065
1066 uint8_t frameOK = 0;
1067 static uint8_t param = 0, offset = 0, parity = 0;
1068 static char string[15];
1069 static uint8_t checksum_param, gps_frame = NO_FRAME;
1070 static uint8_t svMessageNum = 0;
1071 uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
1072
1073 switch (c) {
1074 case '$':
1075 param = 0;
1076 offset = 0;
1077 parity = 0;
1078 break;
1079 case ',':
1080 case '*':
1081 string[offset] = 0;
1082 if (param == 0) { //frame identification
1083 gps_frame = NO_FRAME;
1084 if (0 == strcmp(string, "GPGGA") || 0 == strcmp(string, "GNGGA")) {
1085 gps_frame = FRAME_GGA;
1086 } else if (0 == strcmp(string, "GPRMC") || 0 == strcmp(string, "GNRMC")) {
1087 gps_frame = FRAME_RMC;
1088 } else if (0 == strcmp(string, "GPGSV")) {
1089 gps_frame = FRAME_GSV;
1090 } else if (0 == strcmp(string, "GPGSA")) {
1091 gps_frame = FRAME_GSA;
1092 }
1093 }
1094
1095 switch (gps_frame) {
1096 case FRAME_GGA: //************* GPGGA FRAME parsing
1097 switch (param) {
1098 // case 1: // Time information
1099 // break;
1100 case 2:
1101 gps_Msg.latitude = GPS_coord_to_degrees(string);
1102 break;
1103 case 3:
1104 if (string[0] == 'S')
1105 gps_Msg.latitude *= -1;
1106 break;
1107 case 4:
1108 gps_Msg.longitude = GPS_coord_to_degrees(string);
1109 break;
1110 case 5:
1111 if (string[0] == 'W')
1112 gps_Msg.longitude *= -1;
1113 break;
1114 case 6:
1115 gpsSetFixState(string[0] > '0');
1116 break;
1117 case 7:
1118 gps_Msg.numSat = grab_fields(string, 0);
1119 break;
1120 case 9:
1121 gps_Msg.altitudeCm = grab_fields(string, 1) * 10; // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10
1122 break;
1123 }
1124 break;
1125 case FRAME_RMC: //************* GPRMC FRAME parsing
1126 switch (param) {
1127 case 1:
1128 gps_Msg.time = grab_fields(string, 2); // UTC time hhmmss.ss
1129 break;
1130 case 7:
1131 gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
1132 break;
1133 case 8:
1134 gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10
1135 break;
1136 case 9:
1137 gps_Msg.date = grab_fields(string, 0); // date dd/mm/yy
1138 break;
1139 }
1140 break;
1141 case FRAME_GSV:
1142 switch (param) {
1143 /*case 1:
1144 // Total number of messages of this type in this cycle
1145 break; */
1146 case 2:
1147 // Message number
1148 svMessageNum = grab_fields(string, 0);
1149 break;
1150 case 3:
1151 // Total number of SVs visible
1152 GPS_numCh = grab_fields(string, 0);
1153 if (GPS_numCh > GPS_SV_MAXSATS_LEGACY) {
1154 GPS_numCh = GPS_SV_MAXSATS_LEGACY;
1155 }
1156 break;
1157 }
1158 if (param < 4)
1159 break;
1160
1161 svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
1162 svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
1163 svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
1164
1165 if (svSatNum > GPS_SV_MAXSATS_LEGACY)
1166 break;
1167
1168 switch (svSatParam) {
1169 case 1:
1170 // SV PRN number
1171 GPS_svinfo_chn[svSatNum - 1] = svSatNum;
1172 GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
1173 break;
1174 /*case 2:
1175 // Elevation, in degrees, 90 maximum
1176 break;
1177 case 3:
1178 // Azimuth, degrees from True North, 000 through 359
1179 break; */
1180 case 4:
1181 // SNR, 00 through 99 dB (null when not tracking)
1182 GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
1183 GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
1184 break;
1185 }
1186
1187 GPS_svInfoReceivedCount++;
1188
1189 break;
1190
1191 case FRAME_GSA:
1192 switch (param) {
1193 case 15:
1194 gps_Msg.pdop = grab_fields(string, 1) * 100; // pDOP
1195 break;
1196 case 16:
1197 gps_Msg.hdop = grab_fields(string, 1) * 100; // hDOP
1198 break;
1199 case 17:
1200 gps_Msg.vdop = grab_fields(string, 1) * 100; // vDOP
1201 break;
1202 }
1203 break;
1204 }
1205
1206 param++;
1207 offset = 0;
1208 if (c == '*')
1209 checksum_param = 1;
1210 else
1211 parity ^= c;
1212 break;
1213 case '\r':
1214 case '\n':
1215 if (checksum_param) { //parity checksum
1216 shiftPacketLog();
1217 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
1218 if (checksum == parity) {
1219 *gpsPacketLogChar = LOG_IGNORED;
1220 GPS_packetCount++;
1221 switch (gps_frame) {
1222 case FRAME_GGA:
1223 *gpsPacketLogChar = LOG_NMEA_GGA;
1224 frameOK = 1;
1225 if (STATE(GPS_FIX)) {
1226 gpsSol.llh.lat = gps_Msg.latitude;
1227 gpsSol.llh.lon = gps_Msg.longitude;
1228 gpsSol.numSat = gps_Msg.numSat;
1229 gpsSol.llh.altCm = gps_Msg.altitudeCm;
1230 }
1231 break;
1232 case FRAME_GSA:
1233 *gpsPacketLogChar = LOG_NMEA_GSA;
1234 gpsSol.dop.pdop = gps_Msg.pdop;
1235 gpsSol.dop.hdop = gps_Msg.hdop;
1236 gpsSol.dop.vdop = gps_Msg.vdop;
1237 break;
1238 case FRAME_RMC:
1239 *gpsPacketLogChar = LOG_NMEA_RMC;
1240 gpsSol.groundSpeed = gps_Msg.speed;
1241 gpsSol.groundCourse = gps_Msg.ground_course;
1242 #ifdef USE_RTC_TIME
1243 // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
1244 if(!rtcHasTime() && gps_Msg.date != 0 && gps_Msg.time != 0) {
1245 dateTime_t temp_time;
1246 temp_time.year = (gps_Msg.date % 100) + 2000;
1247 temp_time.month = (gps_Msg.date / 100) % 100;
1248 temp_time.day = (gps_Msg.date / 10000) % 100;
1249 temp_time.hours = (gps_Msg.time / 1000000) % 100;
1250 temp_time.minutes = (gps_Msg.time / 10000) % 100;
1251 temp_time.seconds = (gps_Msg.time / 100) % 100;
1252 temp_time.millis = (gps_Msg.time & 100) * 10;
1253 rtcSetDateTime(&temp_time);
1254 }
1255 #endif
1256 break;
1257 } // end switch
1258 } else {
1259 *gpsPacketLogChar = LOG_ERROR;
1260 }
1261 }
1262 checksum_param = 0;
1263 break;
1264 default:
1265 if (offset < 15)
1266 string[offset++] = c;
1267 if (!checksum_param)
1268 parity ^= c;
1269 }
1270 return frameOK;
1271 }
1272 #endif // USE_GPS_NMEA
1273
1274 #ifdef USE_GPS_UBLOX
1275 // UBX support
1276 typedef struct {
1277 uint32_t time; // GPS msToW
1278 int32_t longitude;
1279 int32_t latitude;
1280 int32_t altitude_ellipsoid;
1281 int32_t altitudeMslMm;
1282 uint32_t horizontal_accuracy;
1283 uint32_t vertical_accuracy;
1284 } ubx_nav_posllh;
1285
1286 typedef struct {
1287 uint32_t time; // GPS msToW
1288 uint8_t fix_type;
1289 uint8_t fix_status;
1290 uint8_t differential_status;
1291 uint8_t res;
1292 uint32_t time_to_first_fix;
1293 uint32_t uptime; // milliseconds
1294 } ubx_nav_status;
1295
1296 typedef struct {
1297 uint32_t itow; // GPS Millisecond Time of Week
1298 uint16_t gdop; // Geometric DOP
1299 uint16_t pdop; // Position DOP
1300 uint16_t tdop; // Time DOP
1301 uint16_t vdop; // Vertical DOP
1302 uint16_t hdop; // Horizontal DOP
1303 uint16_t ndop; // Northing DOP
1304 uint16_t edop; // Easting DOP
1305 } ubx_nav_dop;
1306
1307 typedef struct {
1308 uint32_t time;
1309 int32_t time_nsec;
1310 int16_t week;
1311 uint8_t fix_type;
1312 uint8_t fix_status;
1313 int32_t ecef_x;
1314 int32_t ecef_y;
1315 int32_t ecef_z;
1316 uint32_t position_accuracy_3d;
1317 int32_t ecef_x_velocity;
1318 int32_t ecef_y_velocity;
1319 int32_t ecef_z_velocity;
1320 uint32_t speed_accuracy;
1321 uint16_t position_DOP;
1322 uint8_t res;
1323 uint8_t satellites;
1324 uint32_t res2;
1325 } ubx_nav_solution;
1326
1327 typedef struct {
1328 uint32_t time;
1329 uint16_t year;
1330 uint8_t month;
1331 uint8_t day;
1332 uint8_t hour;
1333 uint8_t min;
1334 uint8_t sec;
1335 uint8_t valid;
1336 uint32_t tAcc;
1337 int32_t nano;
1338 uint8_t fixType;
1339 uint8_t flags;
1340 uint8_t flags2;
1341 uint8_t numSV;
1342 int32_t lon;
1343 int32_t lat;
1344 int32_t height;
1345 int32_t hMSL;
1346 uint32_t hAcc;
1347 uint32_t vAcc;
1348 int32_t velN;
1349 int32_t velE;
1350 int32_t velD;
1351 int32_t gSpeed;
1352 int32_t headMot;
1353 uint32_t sAcc;
1354 uint32_t headAcc;
1355 uint16_t pDOP;
1356 uint8_t flags3;
1357 uint8_t reserved0[5];
1358 int32_t headVeh;
1359 int16_t magDec;
1360 uint16_t magAcc;
1361 } ubx_nav_pvt;
1362
1363 typedef struct {
1364 uint32_t time; // GPS msToW
1365 int32_t ned_north;
1366 int32_t ned_east;
1367 int32_t ned_down;
1368 uint32_t speed_3d;
1369 uint32_t speed_2d;
1370 int32_t heading_2d;
1371 uint32_t speed_accuracy;
1372 uint32_t heading_accuracy;
1373 } ubx_nav_velned;
1374
1375 typedef struct {
1376 uint8_t chn; // Channel number, 255 for SVx not assigned to channel
1377 uint8_t svid; // Satellite ID
1378 uint8_t flags; // Bitmask
1379 uint8_t quality; // Bitfield
1380 uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
1381 uint8_t elev; // Elevation in integer degrees
1382 int16_t azim; // Azimuth in integer degrees
1383 int32_t prRes; // Pseudo range residual in centimetres
1384 } ubx_nav_svinfo_channel;
1385
1386 typedef struct {
1387 uint8_t gnssId;
1388 uint8_t svId; // Satellite ID
1389 uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
1390 int8_t elev; // Elevation in integer degrees
1391 int16_t azim; // Azimuth in integer degrees
1392 int16_t prRes; // Pseudo range residual in decimetres
1393 uint32_t flags; // Bitmask
1394 } ubx_nav_sat_sv;
1395
1396 typedef struct {
1397 uint32_t time; // GPS Millisecond time of week
1398 uint8_t numCh; // Number of channels
1399 uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
1400 uint16_t reserved2; // Reserved
1401 ubx_nav_svinfo_channel channel[GPS_SV_MAXSATS_M8N]; // 32 satellites * 12 byte
1402 } ubx_nav_svinfo;
1403
1404 typedef struct {
1405 uint32_t time; // GPS Millisecond time of week
1406 uint8_t version;
1407 uint8_t numSvs;
1408 uint8_t reserved0[2];
1409 ubx_nav_sat_sv svs[GPS_SV_MAXSATS_M9N];
1410 } ubx_nav_sat;
1411
1412 typedef struct {
1413 uint8_t clsId; // Class ID of the acknowledged message
1414 uint8_t msgId; // Message ID of the acknowledged message
1415 } ubx_ack;
1416
1417 enum {
1418 FIX_NONE = 0,
1419 FIX_DEAD_RECKONING = 1,
1420 FIX_2D = 2,
1421 FIX_3D = 3,
1422 FIX_GPS_DEAD_RECKONING = 4,
1423 FIX_TIME = 5
1424 } ubs_nav_fix_type;
1425
1426 enum {
1427 NAV_STATUS_FIX_VALID = 1,
1428 NAV_STATUS_TIME_WEEK_VALID = 4,
1429 NAV_STATUS_TIME_SECOND_VALID = 8
1430 } ubx_nav_status_bits;
1431
1432 enum {
1433 NAV_VALID_DATE = 1,
1434 NAV_VALID_TIME = 2
1435 } ubx_nav_pvt_valid;
1436
1437 // Packet checksum accumulators
1438 static uint8_t _ck_a;
1439 static uint8_t _ck_b;
1440
1441 // State machine state
1442 static bool _skip_packet;
1443 static uint8_t _step;
1444 static uint8_t _msg_id;
1445 static uint16_t _payload_length;
1446 static uint16_t _payload_counter;
1447
1448 static bool next_fix;
1449 static uint8_t _class;
1450
1451 // do we have new position information?
1452 static bool _new_position;
1453
1454 // do we have new speed information?
1455 static bool _new_speed;
1456
1457 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
1458 //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
1459 //15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
1460 //15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
1461 //15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status'
1462 //15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status'
1463 //15:17:55 R -> UBX 03-09, Size 208, 'Unknown'
1464 //15:17:55 R -> UBX 03-10, Size 336, 'Unknown'
1465 //15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution'
1466 //15:17:55 R -> UBX NAV, Size 100, 'Navigation'
1467 //15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information'
1468
1469 // from the UBlox9 document, the largest payout we receive is the NAV-SAT and the payload size
1470 // is calculated as 8 + 12*numCh. numCh in the case of a M9N is 42.
1471 #define UBLOX_PAYLOAD_SIZE (8 + 12 * 42)
1472
1473
1474 // Receive buffer
1475 static union {
1476 ubx_nav_posllh posllh;
1477 ubx_nav_status status;
1478 ubx_nav_dop dop;
1479 ubx_nav_solution solution;
1480 ubx_nav_velned velned;
1481 ubx_nav_pvt pvt;
1482 ubx_nav_svinfo svinfo;
1483 ubx_nav_sat sat;
1484 ubx_cfg_gnss gnss;
1485 ubx_ack ack;
1486 uint8_t bytes[UBLOX_PAYLOAD_SIZE];
1487 } _buffer;
1488
1489 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
1490 {
1491 while (len--) {
1492 *ck_a += *data;
1493 *ck_b += *ck_a;
1494 data++;
1495 }
1496 }
1497
1498 static bool UBLOX_parse_gps(void)
1499 {
1500 uint32_t i;
1501
1502 *gpsPacketLogChar = LOG_IGNORED;
1503
1504 switch (_msg_id) {
1505 case MSG_POSLLH:
1506 *gpsPacketLogChar = LOG_UBLOX_POSLLH;
1507 //i2c_dataset.time = _buffer.posllh.time;
1508 gpsSol.llh.lon = _buffer.posllh.longitude;
1509 gpsSol.llh.lat = _buffer.posllh.latitude;
1510 gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm
1511 gpsSetFixState(next_fix);
1512 _new_position = true;
1513 break;
1514 case MSG_STATUS:
1515 *gpsPacketLogChar = LOG_UBLOX_STATUS;
1516 next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
1517 if (!next_fix)
1518 DISABLE_STATE(GPS_FIX);
1519 break;
1520 case MSG_DOP:
1521 *gpsPacketLogChar = LOG_UBLOX_DOP;
1522 gpsSol.dop.pdop = _buffer.dop.pdop;
1523 gpsSol.dop.hdop = _buffer.dop.hdop;
1524 gpsSol.dop.vdop = _buffer.dop.vdop;
1525 break;
1526 case MSG_SOL:
1527 *gpsPacketLogChar = LOG_UBLOX_SOL;
1528 next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
1529 if (!next_fix)
1530 DISABLE_STATE(GPS_FIX);
1531 gpsSol.numSat = _buffer.solution.satellites;
1532 #ifdef USE_RTC_TIME
1533 //set clock, when gps time is available
1534 if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) {
1535 //calculate rtctime: week number * ms in a week + ms of week + fractions of second + offset to UNIX reference year - 18 leap seconds
1536 rtcTime_t temp_time = (((int64_t) _buffer.solution.week)*7*24*60*60*1000) + _buffer.solution.time + (_buffer.solution.time_nsec/1000000) + 315964800000LL - 18000;
1537 rtcSet(&temp_time);
1538 }
1539 #endif
1540 break;
1541 case MSG_VELNED:
1542 *gpsPacketLogChar = LOG_UBLOX_VELNED;
1543 gpsSol.speed3d = _buffer.velned.speed_3d; // cm/s
1544 gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
1545 gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
1546 _new_speed = true;
1547 break;
1548 case MSG_PVT:
1549 *gpsPacketLogChar = LOG_UBLOX_SOL;
1550 next_fix = (_buffer.pvt.flags & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fixType == FIX_3D);
1551 gpsSol.llh.lon = _buffer.pvt.lon;
1552 gpsSol.llh.lat = _buffer.pvt.lat;
1553 gpsSol.llh.altCm = _buffer.pvt.hMSL / 10; //alt in cm
1554 gpsSetFixState(next_fix);
1555 _new_position = true;
1556 gpsSol.numSat = _buffer.pvt.numSV;
1557 gpsSol.speed3d = (uint16_t) sqrtf(powf(_buffer.pvt.gSpeed / 10, 2.0f) + powf(_buffer.pvt.velD / 10, 2.0f));
1558 gpsSol.groundSpeed = _buffer.pvt.gSpeed / 10; // cm/s
1559 gpsSol.groundCourse = (uint16_t) (_buffer.pvt.headMot / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
1560 _new_speed = true;
1561 #ifdef USE_RTC_TIME
1562 //set clock, when gps time is available
1563 if (!rtcHasTime() && (_buffer.pvt.valid & NAV_VALID_DATE) && (_buffer.pvt.valid & NAV_VALID_TIME)) {
1564 dateTime_t dt;
1565 dt.year = _buffer.pvt.year;
1566 dt.month = _buffer.pvt.month;
1567 dt.day = _buffer.pvt.day;
1568 dt.hours = _buffer.pvt.hour;
1569 dt.minutes = _buffer.pvt.min;
1570 dt.seconds = _buffer.pvt.sec;
1571 dt.millis = (_buffer.pvt.nano > 0) ? _buffer.pvt.nano / 1000 : 0; //up to 5ms of error
1572 rtcSetDateTime(&dt);
1573 }
1574 #endif
1575 break;
1576 case MSG_SVINFO:
1577 *gpsPacketLogChar = LOG_UBLOX_SVINFO;
1578 GPS_numCh = _buffer.svinfo.numCh;
1579 // If we're getting NAV-SVINFO is because we're dealing with an old receiver that does not support NAV-SAT, so we'll only
1580 // save up to GPS_SV_MAXSATS_LEGACY channels so the BF Configurator knows it's receiving the old sat list info format.
1581 if (GPS_numCh > GPS_SV_MAXSATS_LEGACY)
1582 GPS_numCh = GPS_SV_MAXSATS_LEGACY;
1583 for (i = 0; i < GPS_numCh; i++) {
1584 GPS_svinfo_chn[i] = _buffer.svinfo.channel[i].chn;
1585 GPS_svinfo_svid[i] = _buffer.svinfo.channel[i].svid;
1586 GPS_svinfo_quality[i] =_buffer.svinfo.channel[i].quality;
1587 GPS_svinfo_cno[i] = _buffer.svinfo.channel[i].cno;
1588 }
1589 for (i = GPS_numCh; i < GPS_SV_MAXSATS_LEGACY; i++) {
1590 GPS_svinfo_chn[i] = 0;
1591 GPS_svinfo_svid[i] = 0;
1592 GPS_svinfo_quality[i] = 0;
1593 GPS_svinfo_cno[i] = 0;
1594 }
1595 GPS_svInfoReceivedCount++;
1596 break;
1597 case MSG_SAT:
1598 *gpsPacketLogChar = LOG_UBLOX_SVINFO; // The logger won't show this is NAV-SAT instead of NAV-SVINFO
1599 GPS_numCh = _buffer.sat.numSvs;
1600 // We can receive here upto GPS_SV_MAXSATS_M9N channels, but since the majority of receivers currently in use are M8N or older,
1601 // it would be a waste of RAM to size the arrays that big. For now, they're sized GPS_SV_MAXSATS_M8N which means M9N won't show
1602 // all their channel information on BF Configurator. When M9N's are more widespread it would be a good time to increase those arrays.
1603 if (GPS_numCh > GPS_SV_MAXSATS_M8N)
1604 GPS_numCh = GPS_SV_MAXSATS_M8N;
1605 for (i = 0; i < GPS_numCh; i++) {
1606 GPS_svinfo_chn[i] = _buffer.sat.svs[i].gnssId;
1607 GPS_svinfo_svid[i] = _buffer.sat.svs[i].svId;
1608 GPS_svinfo_cno[i] = _buffer.sat.svs[i].cno;
1609 GPS_svinfo_quality[i] =_buffer.sat.svs[i].flags;
1610 }
1611 for (i = GPS_numCh; i < GPS_SV_MAXSATS_M8N; i++) {
1612 GPS_svinfo_chn[i] = 255;
1613 GPS_svinfo_svid[i] = 0;
1614 GPS_svinfo_quality[i] = 0;
1615 GPS_svinfo_cno[i] = 0;
1616 }
1617
1618 // Setting the number of channels higher than GPS_SV_MAXSATS_LEGACY is the only way to tell BF Configurator we're sending the
1619 // enhanced sat list info without changing the MSP protocol. Also, we're sending the complete list each time even if it's empty, so
1620 // BF Conf can erase old entries shown on screen when channels are removed from the list.
1621 GPS_numCh = GPS_SV_MAXSATS_M8N;
1622 GPS_svInfoReceivedCount++;
1623 break;
1624 case MSG_CFG_GNSS:
1625 {
1626 bool isSBASenabled = false;
1627 bool isM8NwithDefaultConfig = false;
1628
1629 if ((_buffer.gnss.numConfigBlocks >= 2) &&
1630 (_buffer.gnss.configblocks[1].gnssId == 1) && //SBAS
1631 (_buffer.gnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled
1632
1633 isSBASenabled = true;
1634 }
1635
1636 if ((_buffer.gnss.numTrkChHw == 32) && //M8N
1637 (_buffer.gnss.numTrkChUse == 32) &&
1638 (_buffer.gnss.numConfigBlocks == 7) &&
1639 (_buffer.gnss.configblocks[2].gnssId == 2) && //Galileo
1640 (_buffer.gnss.configblocks[2].resTrkCh == 4) && //min channels
1641 (_buffer.gnss.configblocks[2].maxTrkCh == 8) && //max channels
1642 !(_buffer.gnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled
1643
1644 isM8NwithDefaultConfig = true;
1645 }
1646
1647 const uint16_t messageSize = 4 + (_buffer.gnss.numConfigBlocks * sizeof(ubx_configblock));
1648
1649 ubx_message tx_buffer;
1650 memcpy(&tx_buffer.payload, &_buffer, messageSize);
1651
1652 if (isSBASenabled && (gpsConfig()->sbasMode == SBAS_NONE)) {
1653 tx_buffer.payload.cfg_gnss.configblocks[1].flags &= ~UBLOX_GNSS_ENABLE; //Disable SBAS
1654 }
1655
1656 if (isM8NwithDefaultConfig && gpsConfig()->gps_ublox_use_galileo) {
1657 tx_buffer.payload.cfg_gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE; //Enable Galileo
1658 }
1659
1660 ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize);
1661 }
1662 break;
1663 case MSG_ACK_ACK:
1664 if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
1665 gpsData.ackState = UBLOX_ACK_GOT_ACK;
1666 }
1667 break;
1668 case MSG_ACK_NACK:
1669 if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
1670 gpsData.ackState = UBLOX_ACK_GOT_NACK;
1671 }
1672 break;
1673 default:
1674 return false;
1675 }
1676
1677 // we only return true when we get new position and speed data
1678 // this ensures we don't use stale data
1679 if (_new_position && _new_speed) {
1680 _new_speed = _new_position = false;
1681 return true;
1682 }
1683 return false;
1684 }
1685
1686 static bool gpsNewFrameUBLOX(uint8_t data)
1687 {
1688 bool parsed = false;
1689
1690 switch (_step) {
1691 case 0: // Sync char 1 (0xB5)
1692 if (PREAMBLE1 == data) {
1693 _skip_packet = false;
1694 _step++;
1695 }
1696 break;
1697 case 1: // Sync char 2 (0x62)
1698 if (PREAMBLE2 != data) {
1699 _step = 0;
1700 break;
1701 }
1702 _step++;
1703 break;
1704 case 2: // Class
1705 _step++;
1706 _class = data;
1707 _ck_b = _ck_a = data; // reset the checksum accumulators
1708 break;
1709 case 3: // Id
1710 _step++;
1711 _ck_b += (_ck_a += data); // checksum byte
1712 _msg_id = data;
1713 #if DEBUG_UBLOX_FRAMES
1714 debug[2] = _msg_id;
1715 #endif
1716 break;
1717 case 4: // Payload length (part 1)
1718 _step++;
1719 _ck_b += (_ck_a += data); // checksum byte
1720 _payload_length = data; // payload length low byte
1721 break;
1722 case 5: // Payload length (part 2)
1723 _step++;
1724 _ck_b += (_ck_a += data); // checksum byte
1725 _payload_length += (uint16_t)(data << 8);
1726 #if DEBUG_UBLOX_FRAMES
1727 debug[3] = _payload_length;
1728 #endif
1729 if (_payload_length > UBLOX_PAYLOAD_SIZE) {
1730 _skip_packet = true;
1731 }
1732 _payload_counter = 0; // prepare to receive payload
1733 if (_payload_length == 0) {
1734 _step = 7;
1735 }
1736 break;
1737 case 6:
1738 _ck_b += (_ck_a += data); // checksum byte
1739 if (_payload_counter < UBLOX_PAYLOAD_SIZE) {
1740 _buffer.bytes[_payload_counter] = data;
1741 }
1742 if (++_payload_counter >= _payload_length) {
1743 _step++;
1744 }
1745 break;
1746 case 7:
1747 _step++;
1748 if (_ck_a != data) {
1749 _skip_packet = true; // bad checksum
1750 gpsData.errors++;
1751 }
1752 break;
1753 case 8:
1754 _step = 0;
1755
1756 shiftPacketLog();
1757
1758 if (_ck_b != data) {
1759 *gpsPacketLogChar = LOG_ERROR;
1760 gpsData.errors++;
1761 break; // bad checksum
1762 }
1763
1764 GPS_packetCount++;
1765
1766 if (_skip_packet) {
1767 *gpsPacketLogChar = LOG_SKIPPED;
1768 break;
1769 }
1770
1771 if (UBLOX_parse_gps()) {
1772 parsed = true;
1773 }
1774 }
1775 return parsed;
1776 }
1777 #endif // USE_GPS_UBLOX
1778
1779 static void gpsHandlePassthrough(uint8_t data)
1780 {
1781 gpsNewData(data);
1782 #ifdef USE_DASHBOARD
1783 if (featureIsEnabled(FEATURE_DASHBOARD)) {
1784 dashboardUpdate(micros());
1785 }
1786 #endif
1787
1788 }
1789
1790 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
1791 {
1792 waitForSerialPortToFinishTransmitting(gpsPort);
1793 waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
1794
1795 if (!(gpsPort->mode & MODE_TX))
1796 serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
1797
1798 #ifdef USE_DASHBOARD
1799 if (featureIsEnabled(FEATURE_DASHBOARD)) {
1800 dashboardShowFixedPage(PAGE_GPS);
1801 }
1802 #endif
1803
1804 serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
1805 }
1806
1807 float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
1808
1809 void GPS_calc_longitude_scaling(int32_t lat)
1810 {
1811 float rads = (fabsf((float)lat) / 10000000.0f) * 0.0174532925f;
1812 GPS_scaleLonDown = cos_approx(rads);
1813 }
1814
1815 ////////////////////////////////////////////////////////////////////////////////////
1816 // Calculate the distance flown and vertical speed from gps position data
1817 //
1818 static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
1819 {
1820 static int32_t lastCoord[2] = { 0, 0 };
1821 static int32_t lastAlt;
1822 static int32_t lastMillis;
1823
1824 int currentMillis = millis();
1825
1826 if (initialize) {
1827 GPS_distanceFlownInCm = 0;
1828 GPS_verticalSpeedInCmS = 0;
1829 } else {
1830 if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
1831 uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
1832 // Only add up movement when speed is faster than minimum threshold
1833 if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
1834 uint32_t dist;
1835 int32_t dir;
1836 GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
1837 if (gpsConfig()->gps_use_3d_speed) {
1838 dist = sqrtf(sq(gpsSol.llh.altCm - lastAlt) + sq(dist));
1839 }
1840 GPS_distanceFlownInCm += dist;
1841 }
1842 }
1843 GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
1844 GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
1845 }
1846 lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
1847 lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
1848 lastAlt = gpsSol.llh.altCm;
1849 lastMillis = currentMillis;
1850 }
1851
1852 void GPS_reset_home_position(void)
1853 // runs, if GPS is defined, on arming via tryArm() in core.c, and on gyro cal via processRcStickPositions() in rc_controls.c
1854 {
1855 if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
1856 if (STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
1857 // those checks are always true for tryArm, but may not be true for gyro cal
1858 GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
1859 GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
1860 GPS_calc_longitude_scaling(gpsSol.llh.lat);
1861 ENABLE_STATE(GPS_FIX_HOME);
1862 // no point beeping success here since:
1863 // when triggered by tryArm, the arming beep is modified to indicate the GPS home fix status on arming, and
1864 // when triggered by gyro cal, the gyro cal beep takes priority over the GPS beep, so we won't hear the GPS beep
1865 // PS: to test for gyro cal, check for !ARMED, since we cannot be here while disarmed other than via gyro cal
1866 }
1867 }
1868 GPS_calculateDistanceFlownVerticalSpeed(true); // Initialize
1869 }
1870
1871 ////////////////////////////////////////////////////////////////////////////////////
1872 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
1873 #define TAN_89_99_DEGREES 5729.57795f
1874 // Get distance between two points in cm
1875 // Get bearing from pos1 to pos2, returns an 1deg = 100 precision
1876 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
1877 {
1878 float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
1879 float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
1880 *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
1881
1882 *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
1883 if (*bearing < 0)
1884 *bearing += 36000;
1885 }
1886
1887 void GPS_calculateDistanceAndDirectionToHome(void)
1888 {
1889 if (STATE(GPS_FIX_HOME)) {
1890 uint32_t dist;
1891 int32_t dir;
1892 GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
1893 GPS_distanceToHome = dist / 100; // m/s
1894 GPS_distanceToHomeCm = dist; // cm/sec
1895 GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees
1896 } else {
1897 // If we don't have home set, do not display anything
1898 GPS_distanceToHome = 0;
1899 GPS_distanceToHomeCm = 0;
1900 GPS_directionToHome = 0;
1901 }
1902 }
1903
1904 void onGpsNewData(void)
1905 {
1906 if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
1907 // if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue
1908 return;
1909 }
1910
1911 GPS_calculateDistanceAndDirectionToHome();
1912 if (ARMING_FLAG(ARMED)) {
1913 GPS_calculateDistanceFlownVerticalSpeed(false);
1914 }
1915
1916 #ifdef USE_GPS_RESCUE
1917 rescueNewGpsData();
1918 #endif
1919 }
1920
1921 void gpsSetFixState(bool state)
1922 {
1923 if (state) {
1924 ENABLE_STATE(GPS_FIX);
1925 ENABLE_STATE(GPS_FIX_EVER);
1926 } else {
1927 DISABLE_STATE(GPS_FIX);
1928 }
1929 }
1930 #endif