?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
New SPI API supporting DMA
[betaflight.git] / src / main / io / gps.c
blob76263d0973a2347e322d5d032d832252509b973d
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/dashboard.h"
47 #include "io/gps.h"
48 #include "io/serial.h"
49
50 #include "config/config.h"
51 #include "fc/runtime_config.h"
52
53 #include "flight/imu.h"
54 #include "flight/pid.h"
55 #include "flight/gps_rescue.h"
56
57 #include "scheduler/scheduler.h"
58
59 #include "sensors/sensors.h"
60
61 #define LOG_ERROR '?'
62 #define LOG_IGNORED '!'
63 #define LOG_SKIPPED '>'
64 #define LOG_NMEA_GGA 'g'
65 #define LOG_NMEA_RMC 'r'
66 #define LOG_UBLOX_SOL 'O'
67 #define LOG_UBLOX_STATUS 'S'
68 #define LOG_UBLOX_SVINFO 'I'
69 #define LOG_UBLOX_POSLLH 'P'
70 #define LOG_UBLOX_VELNED 'V'
71
72 #define GPS_SV_MAXSATS 16
73
74 char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
75 static char *gpsPacketLogChar = gpsPacketLog;
76 // **********************
77 // GPS
78 // **********************
79 int32_t GPS_home[2];
80 uint16_t GPS_distanceToHome; // distance to home point in meters
81 int16_t GPS_directionToHome; // direction to home or hol point in degrees
82 uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
83 int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s
84 float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
85 int16_t nav_takeoff_bearing;
86
87 #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph
88
89 gpsSolutionData_t gpsSol;
90 uint32_t GPS_packetCount = 0;
91 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
92 uint8_t GPS_update = 0; // toogle to distinct a GPS position update (directly or via MSP)
93
94 uint8_t GPS_numCh; // Number of channels
95 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
96 uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
97 uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
98 uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength)
99
100 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
101 #define GPS_TIMEOUT (2500)
102 // How many entries in gpsInitData array below
103 #define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
104 #define GPS_BAUDRATE_CHANGE_DELAY (200)
105 // Timeout for waiting ACK/NAK in GPS task cycles (0.1s at 100Hz)
106 #define UBLOX_ACK_TIMEOUT_MAX_COUNT (10)
107
108 static serialPort_t *gpsPort;
109
110 typedef struct gpsInitData_s {
111 uint8_t index;
112 uint8_t baudrateIndex; // see baudRate_e
113 const char *ubx;
114 const char *mtk;
115 } gpsInitData_t;
116
117 // NMEA will cycle through these until valid data is received
118 static const gpsInitData_t gpsInitData[] = {
119 { GPS_BAUDRATE_115200, BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
120 { GPS_BAUDRATE_57600, BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
121 { GPS_BAUDRATE_38400, BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
122 { GPS_BAUDRATE_19200, BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
123 // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
124 { GPS_BAUDRATE_9600, BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
125 };
126
127 #define GPS_INIT_DATA_ENTRY_COUNT (sizeof(gpsInitData) / sizeof(gpsInitData[0]))
128
129 #define DEFAULT_BAUD_RATE_INDEX 0
130
131 #ifdef USE_GPS_UBLOX
132 static const uint8_t ubloxInit[] = {
133 //Preprocessor Pedestrian Dynamic Platform Model Option
134 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings
135 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedestrian and
136 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console.
137 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xC2,
138
139 // DISABLE NMEA messages
140 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed
141 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View
142 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11, // GLL: Latitude and longitude, with time of position fix and status
143 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F, // GGA: Global positioning system fix data
144 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13, // GSA: GNSS DOP and Active Satellites
145 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17, // RMC: Recommended Minimum data
146
147 // Enable UBLOX messages
148 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47, // set POSLLH MSG rate
149 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49, // set STATUS MSG rate
150 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate
151 //0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x01, 0x3C, 0xA3, // set SVINFO MSG rate (every cycle - high bandwidth)
152 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x05, 0x40, 0xA7, // set SVINFO MSG rate (evey 5 cycles - low bandwidth)
153 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate
154
155 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
156 };
157
158 static const uint8_t ubloxAirborne[] = {
159 //Preprocessor Airborne_1g Dynamic Platform Model Option
160 #if defined(GPS_UBLOX_MODE_AIRBORNE_1G)
161 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, // CFG-NAV5 - Set engine settings
162 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with
163 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <1g acceleration and capturing the data from the U-Center binary console.
164 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1A, 0x28,
165
166 //Default Airborne_4g Dynamic Platform Model
167 #else
168 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, // CFG-NAV5 - Set engine settings
169 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with
170 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // <4g acceleration and capturing the data from the U-Center binary console.
171 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1C, 0x6C,
172 #endif
173 };
174
175 typedef struct {
176 uint8_t preamble1;
177 uint8_t preamble2;
178 uint8_t msg_class;
179 uint8_t msg_id;
180 uint16_t length;
181 } ubx_header;
182
183 typedef struct {
184 uint8_t mode;
185 uint8_t usage;
186 uint8_t maxSBAS;
187 uint8_t scanmode2;
188 uint32_t scanmode1;
189 } ubx_sbas;
190
191 typedef struct {
192 uint8_t gnssId;
193 uint8_t resTrkCh;
194 uint8_t maxTrkCh;
195 uint8_t reserved1;
196 uint32_t flags;
197 } ubx_configblock;
198
199 typedef struct {
200 uint8_t msgVer;
201 uint8_t numTrkChHw;
202 uint8_t numTrkChUse;
203 uint8_t numConfigBlocks;
204 ubx_configblock configblocks[7];
205 } ubx_gnss;
206
207 typedef union {
208 ubx_sbas sbas;
209 ubx_gnss gnss;
210 } ubx_payload;
211
212 typedef struct {
213 ubx_header header;
214 ubx_payload payload;
215 } __attribute__((packed)) ubx_message;
216
217 #define UBLOX_MODE_ENABLED 0x1
218 #define UBLOX_MODE_TEST 0x2
219
220 #define UBLOX_USAGE_RANGE 0x1
221 #define UBLOX_USAGE_DIFFCORR 0x2
222 #define UBLOX_USAGE_INTEGRITY 0x4
223
224 #define UBLOX_GNSS_ENABLE 0x1
225 #define UBLOX_GNSS_DEFAULT_SIGCFGMASK 0x10000
226
227 #define UBLOX_SBAS_MESSAGE_LENGTH 14
228 #define UBLOX_GNSS_MESSAGE_LENGTH 66
229
230 #endif // USE_GPS_UBLOX
231
232 typedef enum {
233 GPS_UNKNOWN,
234 GPS_INITIALIZING,
235 GPS_INITIALIZED,
236 GPS_CHANGE_BAUD,
237 GPS_CONFIGURE,
238 GPS_RECEIVING_DATA,
239 GPS_LOST_COMMUNICATION
240 } gpsState_e;
241
242 gpsData_t gpsData;
243
244
245 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
246
247 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
248 .provider = GPS_NMEA,
249 .sbasMode = SBAS_NONE,
250 .autoConfig = GPS_AUTOCONFIG_ON,
251 .autoBaud = GPS_AUTOBAUD_OFF,
252 .gps_ublox_use_galileo = false,
253 .gps_ublox_mode = UBLOX_AIRBORNE,
254 .gps_set_home_point_once = false,
255 .gps_use_3d_speed = false,
256 .sbas_integrity = false
257 );
258
259 static void shiftPacketLog(void)
260 {
261 uint32_t i;
262
263 for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) {
264 gpsPacketLog[i] = gpsPacketLog[i-1];
265 }
266 }
267
268 static void gpsNewData(uint16_t c);
269 #ifdef USE_GPS_NMEA
270 static bool gpsNewFrameNMEA(char c);
271 #endif
272 #ifdef USE_GPS_UBLOX
273 static bool gpsNewFrameUBLOX(uint8_t data);
274 #endif
275
276 static void gpsSetState(gpsState_e state)
277 {
278 gpsData.state = state;
279 gpsData.state_position = 0;
280 gpsData.state_ts = millis();
281 gpsData.messageState = GPS_MESSAGE_STATE_IDLE;
282 }
283
284 void gpsInit(void)
285 {
286 gpsData.baudrateIndex = 0;
287 gpsData.errors = 0;
288 gpsData.timeouts = 0;
289
290 memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
291
292 // init gpsData structure. if we're not actually enabled, don't bother doing anything else
293 gpsSetState(GPS_UNKNOWN);
294
295 gpsData.lastMessage = millis();
296
297 if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
298 gpsSetState(GPS_INITIALIZED);
299 return;
300 }
301
302 const serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
303 if (!gpsPortConfig) {
304 return;
305 }
306
307 while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) {
308 gpsData.baudrateIndex++;
309 if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
310 gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
311 break;
312 }
313 }
314
315 portMode_e mode = MODE_RXTX;
316 #if defined(GPS_NMEA_TX_ONLY)
317 if (gpsConfig()->provider == GPS_NMEA) {
318 mode &= ~MODE_TX;
319 }
320 #endif
321
322 // no callback - buffer will be consumed in gpsUpdate()
323 gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
324 if (!gpsPort) {
325 return;
326 }
327
328 // signal GPS "thread" to initialize when it gets to it
329 gpsSetState(GPS_INITIALIZING);
330 }
331
332 #ifdef USE_GPS_NMEA
333 void gpsInitNmea(void)
334 {
335 #if !defined(GPS_NMEA_TX_ONLY)
336 uint32_t now;
337 #endif
338 switch (gpsData.state) {
339 case GPS_INITIALIZING:
340 #if !defined(GPS_NMEA_TX_ONLY)
341 now = millis();
342 if (now - gpsData.state_ts < 1000) {
343 return;
344 }
345 gpsData.state_ts = now;
346 if (gpsData.state_position < 1) {
347 serialSetBaudRate(gpsPort, 4800);
348 gpsData.state_position++;
349 } else if (gpsData.state_position < 2) {
350 // print our FIXED init string for the baudrate we want to be at
351 serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
352 gpsData.state_position++;
353 } else {
354 // we're now (hopefully) at the correct rate, next state will switch to it
355 gpsSetState(GPS_CHANGE_BAUD);
356 }
357 break;
358 #endif
359 case GPS_CHANGE_BAUD:
360 #if !defined(GPS_NMEA_TX_ONLY)
361 now = millis();
362 if (now - gpsData.state_ts < 1000) {
363 return;
364 }
365 gpsData.state_ts = now;
366 if (gpsData.state_position < 1) {
367 serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
368 gpsData.state_position++;
369 } else if (gpsData.state_position < 2) {
370 serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
371 gpsData.state_position++;
372 } else
373 #else
374 {
375 serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
376 }
377 #endif
378 gpsSetState(GPS_RECEIVING_DATA);
379 break;
380 }
381 }
382 #endif // USE_GPS_NMEA
383
384 #ifdef USE_GPS_UBLOX
385 static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, uint8_t *checksumB)
386 {
387 *checksumA += data;
388 *checksumB += *checksumA;
389 serialWrite(gpsPort, data);
390 }
391
392 static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_t *checksumA, uint8_t *checksumB)
393 {
394 while (len--) {
395 ubloxSendByteUpdateChecksum(*data, checksumA, checksumB);
396 data++;
397 }
398 }
399
400 static void ubloxSendConfigMessage(const uint8_t *data, uint8_t len)
401 {
402 uint8_t checksumA = 0, checksumB = 0;
403 serialWrite(gpsPort, data[0]);
404 serialWrite(gpsPort, data[1]);
405 ubloxSendDataUpdateChecksum(&data[2], len-2, &checksumA, &checksumB);
406 serialWrite(gpsPort, checksumA);
407 serialWrite(gpsPort, checksumB);
408
409 // Save state for ACK waiting
410 gpsData.ackWaitingMsgId = data[3]; //save message id for ACK
411 gpsData.ackTimeoutCounter = 0;
412 gpsData.ackState = UBLOX_ACK_WAITING;
413 }
414
415 void gpsInitUblox(void)
416 {
417 uint32_t now;
418 // UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate
419
420 // Wait until GPS transmit buffer is empty
421 if (!isSerialTransmitBufferEmpty(gpsPort))
422 return;
423
424
425 switch (gpsData.state) {
426 case GPS_INITIALIZING:
427 now = millis();
428 if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
429 return;
430
431 if (gpsData.state_position < GPS_INIT_ENTRIES) {
432 // try different speed to INIT
433 baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex;
434
435 gpsData.state_ts = now;
436
437 if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) {
438 // change the rate if needed and wait a little
439 serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]);
440 return;
441 }
442
443 // print our FIXED init string for the baudrate we want to be at
444 serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
445
446 gpsData.state_position++;
447 } else {
448 // we're now (hopefully) at the correct rate, next state will switch to it
449 gpsSetState(GPS_CHANGE_BAUD);
450 }
451 break;
452 case GPS_CHANGE_BAUD:
453 serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
454 gpsSetState(GPS_CONFIGURE);
455 break;
456 case GPS_CONFIGURE:
457
458 // Either use specific config file for GPS or let dynamically upload config
459 if ( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
460 gpsSetState(GPS_RECEIVING_DATA);
461 break;
462 }
463
464 if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
465 gpsData.messageState++;
466 }
467
468 if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
469 if (gpsData.state_position < sizeof(ubloxInit)) {
470 if (gpsData.state_position < sizeof(ubloxAirborne)) {
471 if (gpsConfig()->gps_ublox_mode == UBLOX_AIRBORNE) {
472 serialWrite(gpsPort, ubloxAirborne[gpsData.state_position]);
473 } else {
474 serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
475 }
476 } else {
477 serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
478 }
479 gpsData.state_position++;
480 } else {
481 gpsData.state_position = 0;
482 gpsData.messageState++;
483 gpsData.ackState = UBLOX_ACK_IDLE;
484 }
485 }
486
487 if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
488 switch (gpsData.ackState) {
489 case UBLOX_ACK_IDLE:
490 {
491 ubx_message tx_buffer;
492 tx_buffer.header.preamble1 = 0xB5;
493 tx_buffer.header.preamble2 = 0x62;
494 tx_buffer.header.msg_class = 0x06;
495 tx_buffer.header.msg_id = 0x16;
496 tx_buffer.header.length = 8;
497
498 //NOTE: default ublox config for sbas mode is: UBLOX_MODE_ENABLED, test is disabled
499 tx_buffer.payload.sbas.mode = UBLOX_MODE_TEST;
500 if (gpsConfig()->sbasMode != SBAS_NONE) {
501 tx_buffer.payload.sbas.mode |= UBLOX_MODE_ENABLED;
502 }
503
504 //NOTE: default ublox config for sbas mode is: UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR, integrity is disabled
505 tx_buffer.payload.sbas.usage = UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR;
506 if (gpsConfig()->sbas_integrity) {
507 tx_buffer.payload.sbas.usage |= UBLOX_USAGE_INTEGRITY;
508 }
509
510 tx_buffer.payload.sbas.maxSBAS = 3;
511 tx_buffer.payload.sbas.scanmode2 = 0;
512 switch (gpsConfig()->sbasMode) {
513 case SBAS_AUTO:
514 tx_buffer.payload.sbas.scanmode1 = 0;
515 break;
516 case SBAS_EGNOS:
517 tx_buffer.payload.sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136
518 break;
519 case SBAS_WAAS:
520 tx_buffer.payload.sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138
521 break;
522 case SBAS_MSAS:
523 tx_buffer.payload.sbas.scanmode1 = 0x00020200; //PRN129, PRN137
524 break;
525 case SBAS_GAGAN:
526 tx_buffer.payload.sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132
527 break;
528 default:
529 tx_buffer.payload.sbas.scanmode1 = 0;
530 break;
531 }
532 ubloxSendConfigMessage((const uint8_t *) &tx_buffer, UBLOX_SBAS_MESSAGE_LENGTH);
533 }
534 break;
535 case UBLOX_ACK_WAITING:
536 if ((++gpsData.ackTimeoutCounter) == UBLOX_ACK_TIMEOUT_MAX_COUNT) {
537 gpsData.ackState = UBLOX_ACK_GOT_TIMEOUT;
538 }
539 break;
540 case UBLOX_ACK_GOT_TIMEOUT:
541 case UBLOX_ACK_GOT_NACK:
542 case UBLOX_ACK_GOT_ACK:
543 gpsData.state_position = 0;
544 gpsData.ackState = UBLOX_ACK_IDLE;
545 gpsData.messageState++;
546 break;
547 default:
548 break;
549 }
550 }
551
552 if (gpsData.messageState == GPS_MESSAGE_STATE_GNSS) {
553 switch (gpsData.ackState) {
554 case UBLOX_ACK_IDLE:
555 {
556 ubx_message tx_buffer;
557 tx_buffer.header.preamble1 = 0xB5;
558 tx_buffer.header.preamble2 = 0x62;
559 tx_buffer.header.msg_class = 0x06;
560 tx_buffer.header.msg_id = 0x3E;
561 tx_buffer.header.length = 60;
562
563 tx_buffer.payload.gnss.msgVer = 0;
564 tx_buffer.payload.gnss.numTrkChHw = 32;
565 tx_buffer.payload.gnss.numTrkChUse = 32;
566 tx_buffer.payload.gnss.numConfigBlocks = 7;
567
568 tx_buffer.payload.gnss.configblocks[0].gnssId = 0; //GPS
569 tx_buffer.payload.gnss.configblocks[0].resTrkCh = 8; //min channels
570 tx_buffer.payload.gnss.configblocks[0].maxTrkCh = 16; //max channels
571 tx_buffer.payload.gnss.configblocks[0].reserved1 = 0;
572 tx_buffer.payload.gnss.configblocks[0].flags = UBLOX_GNSS_ENABLE | UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x01000000; //last number is undocumented and was captured from uCenter
573
574 tx_buffer.payload.gnss.configblocks[1].gnssId = 1; //SBAS
575 tx_buffer.payload.gnss.configblocks[1].resTrkCh = 1; //min channels
576 tx_buffer.payload.gnss.configblocks[1].maxTrkCh = 3; //max channels
577 tx_buffer.payload.gnss.configblocks[1].reserved1 = 0;
578 tx_buffer.payload.gnss.configblocks[1].flags = UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x01000000; //last number is undocumented and was captured from uCenter
579 if (gpsConfig()->sbasMode != SBAS_NONE) {
580 tx_buffer.payload.gnss.configblocks[1].flags |= UBLOX_GNSS_ENABLE;
581 }
582
583 tx_buffer.payload.gnss.configblocks[2].gnssId = 2; //Galileo
584 tx_buffer.payload.gnss.configblocks[2].resTrkCh = 4; //min channels
585 tx_buffer.payload.gnss.configblocks[2].maxTrkCh = 8; //max channels
586 tx_buffer.payload.gnss.configblocks[2].reserved1 = 0;
587 tx_buffer.payload.gnss.configblocks[2].flags = UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x01000000; //last number is undocumented and was captured from uCenter
588 if (gpsConfig()->gps_ublox_use_galileo) {
589 tx_buffer.payload.gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE;
590 }
591
592 tx_buffer.payload.gnss.configblocks[3].gnssId = 3; //BeiDou
593 tx_buffer.payload.gnss.configblocks[3].resTrkCh = 8; //min channels
594 tx_buffer.payload.gnss.configblocks[3].maxTrkCh = 16; //max channels
595 tx_buffer.payload.gnss.configblocks[3].reserved1 = 0;
596 tx_buffer.payload.gnss.configblocks[3].flags = UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x01000000; //last number is undocumented and was captured from uCenter
597
598 tx_buffer.payload.gnss.configblocks[4].gnssId = 4; //IMES
599 tx_buffer.payload.gnss.configblocks[4].resTrkCh = 0; //min channels
600 tx_buffer.payload.gnss.configblocks[4].maxTrkCh = 8; //max channels
601 tx_buffer.payload.gnss.configblocks[4].reserved1 = 0;
602 tx_buffer.payload.gnss.configblocks[4].flags = UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x03000000; //last number is undocumented and was captured from uCenter
603
604 tx_buffer.payload.gnss.configblocks[5].gnssId = 5; //QZSS
605 tx_buffer.payload.gnss.configblocks[5].resTrkCh = 0; //min channels
606 tx_buffer.payload.gnss.configblocks[5].maxTrkCh = 3; //max channels
607 tx_buffer.payload.gnss.configblocks[5].reserved1 = 0;
608 tx_buffer.payload.gnss.configblocks[5].flags = UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x05000000; //last number is undocumented and was captured from uCenter
609 if (!gpsConfig()->gps_ublox_use_galileo) {
610 tx_buffer.payload.gnss.configblocks[5].flags |= UBLOX_GNSS_ENABLE;
611 }
612
613 tx_buffer.payload.gnss.configblocks[6].gnssId = 6; //GLONASS
614 tx_buffer.payload.gnss.configblocks[6].resTrkCh = 8; //min channels
615 tx_buffer.payload.gnss.configblocks[6].maxTrkCh = 14; //max channels
616 tx_buffer.payload.gnss.configblocks[6].reserved1 = 0;
617 tx_buffer.payload.gnss.configblocks[6].flags = UBLOX_GNSS_ENABLE | UBLOX_GNSS_DEFAULT_SIGCFGMASK | 0x01000000; //last number is undocumented and was captured from uCenter
618
619 ubloxSendConfigMessage((const uint8_t *) &tx_buffer, UBLOX_GNSS_MESSAGE_LENGTH);
620 }
621 break;
622 case UBLOX_ACK_WAITING:
623 if ((++gpsData.ackTimeoutCounter) == UBLOX_ACK_TIMEOUT_MAX_COUNT) {
624 gpsData.ackState = UBLOX_ACK_GOT_TIMEOUT;
625 }
626 break;
627 case UBLOX_ACK_GOT_TIMEOUT:
628 case UBLOX_ACK_GOT_NACK:
629 case UBLOX_ACK_GOT_ACK:
630 gpsData.state_position = 0;
631 gpsData.ackState = UBLOX_ACK_IDLE;
632 gpsData.messageState++;
633 break;
634 default:
635 break;
636 }
637 }
638
639 if (gpsData.messageState >= GPS_MESSAGE_STATE_INITIALIZED) {
640 // ublox should be initialised, try receiving
641 gpsSetState(GPS_RECEIVING_DATA);
642 }
643 break;
644 }
645 }
646 #endif // USE_GPS_UBLOX
647
648 void gpsInitHardware(void)
649 {
650 switch (gpsConfig()->provider) {
651 case GPS_NMEA:
652 #ifdef USE_GPS_NMEA
653 gpsInitNmea();
654 #endif
655 break;
656
657 case GPS_UBLOX:
658 #ifdef USE_GPS_UBLOX
659 gpsInitUblox();
660 #endif
661 break;
662 default:
663 break;
664 }
665 }
666
667 static void updateGpsIndicator(timeUs_t currentTimeUs)
668 {
669 static uint32_t GPSLEDTime;
670 if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
671 GPSLEDTime = currentTimeUs + 150000;
672 LED1_TOGGLE;
673 }
674 }
675
676 void gpsUpdate(timeUs_t currentTimeUs)
677 {
678 static timeUs_t maxTimeUs = 0;
679 timeUs_t endTimeUs;
680
681 // read out available GPS bytes
682 if (gpsPort) {
683 while (serialRxBytesWaiting(gpsPort)) {
684 gpsNewData(serialRead(gpsPort));
685 }
686 } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
687 gpsSetState(GPS_RECEIVING_DATA);
688 gpsData.lastMessage = millis();
689 sensorsSet(SENSOR_GPS);
690 onGpsNewData();
691 GPS_update &= ~GPS_MSP_UPDATE;
692 }
693
694 switch (gpsData.state) {
695 case GPS_UNKNOWN:
696 case GPS_INITIALIZED:
697 break;
698
699 case GPS_INITIALIZING:
700 case GPS_CHANGE_BAUD:
701 case GPS_CONFIGURE:
702 gpsInitHardware();
703 break;
704
705 case GPS_LOST_COMMUNICATION:
706 gpsData.timeouts++;
707 if (gpsConfig()->autoBaud) {
708 // try another rate
709 gpsData.baudrateIndex++;
710 gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
711 }
712 gpsData.lastMessage = millis();
713 gpsSol.numSat = 0;
714 DISABLE_STATE(GPS_FIX);
715 gpsSetState(GPS_INITIALIZING);
716 break;
717
718 case GPS_RECEIVING_DATA:
719 // check for no data/gps timeout/cable disconnection etc
720 if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
721 // remove GPS from capability
722 sensorsClear(SENSOR_GPS);
723 gpsSetState(GPS_LOST_COMMUNICATION);
724 #ifdef USE_GPS_UBLOX
725 } else {
726 if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
727 if ((gpsData.messageState == GPS_MESSAGE_STATE_INITIALIZED) && STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) {
728 gpsData.messageState = GPS_MESSAGE_STATE_PEDESTRIAN_TO_AIRBORNE;
729 gpsData.state_position = 0;
730 }
731 if (gpsData.messageState == GPS_MESSAGE_STATE_PEDESTRIAN_TO_AIRBORNE) {
732 if (gpsData.state_position < sizeof(ubloxAirborne)) {
733 if (isSerialTransmitBufferEmpty(gpsPort)) {
734 serialWrite(gpsPort, ubloxAirborne[gpsData.state_position]);
735 gpsData.state_position++;
736 }
737 } else {
738 gpsData.messageState = GPS_MESSAGE_STATE_ENTRY_COUNT;
739 }
740 }
741 }
742 #endif //USE_GPS_UBLOX
743 }
744 break;
745 }
746
747 if (sensors(SENSOR_GPS)) {
748 updateGpsIndicator(currentTimeUs);
749 }
750 if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) {
751 DISABLE_STATE(GPS_FIX_HOME);
752 }
753 #if defined(USE_GPS_RESCUE)
754 if (gpsRescueIsConfigured()) {
755 updateGPSRescueState();
756 }
757 #endif
758 // Call ignoreTaskTime() unless this took appreciable time
759 // Note that this will mess up the rate/Hz display under tasks, but the code
760 // takes widely varying time to complete
761 endTimeUs = micros();
762 if ((endTimeUs - currentTimeUs) > maxTimeUs) {
763 maxTimeUs = endTimeUs - currentTimeUs;
764 } else {
765 ignoreTaskTime();
766 // Decay max time
767 maxTimeUs--;
768 }
769 }
770
771 static void gpsNewData(uint16_t c)
772 {
773 if (!gpsNewFrame(c)) {
774 return;
775 }
776
777 // new data received and parsed, we're in business
778 gpsData.lastLastMessage = gpsData.lastMessage;
779 gpsData.lastMessage = millis();
780 sensorsSet(SENSOR_GPS);
781
782 GPS_update ^= GPS_DIRECT_TICK;
783
784 #if 0
785 debug[3] = GPS_update;
786 #endif
787
788 onGpsNewData();
789 }
790
791 bool gpsNewFrame(uint8_t c)
792 {
793 switch (gpsConfig()->provider) {
794 case GPS_NMEA: // NMEA
795 #ifdef USE_GPS_NMEA
796 return gpsNewFrameNMEA(c);
797 #endif
798 break;
799 case GPS_UBLOX: // UBX binary
800 #ifdef USE_GPS_UBLOX
801 return gpsNewFrameUBLOX(c);
802 #endif
803 break;
804 default:
805 break;
806 }
807 return false;
808 }
809
810 // Check for healthy communications
811 bool gpsIsHealthy()
812 {
813 return (gpsData.state == GPS_RECEIVING_DATA);
814 }
815
816 /* This is a light implementation of a GPS frame decoding
817 This should work with most of modern GPS devices configured to output 5 frames.
818 It assumes there are some NMEA GGA frames to decode on the serial bus
819 Now verifies checksum correctly before applying data
820
821 Here we use only the following data :
822 - latitude
823 - longitude
824 - GPS fix is/is not ok
825 - GPS num sat (4 is enough to be +/- reliable)
826 // added by Mis
827 - GPS altitude (for OSD displaying)
828 - GPS speed (for OSD displaying)
829 */
830
831 #define NO_FRAME 0
832 #define FRAME_GGA 1
833 #define FRAME_RMC 2
834 #define FRAME_GSV 3
835
836
837 // This code is used for parsing NMEA data
838
839 /* Alex optimization
840 The latitude or longitude is coded this way in NMEA frames
841 dm.f coded as degrees + minutes + minute decimal
842 Where:
843 - d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
844 - m is always 2 char long
845 - f can be 1 or more char long
846 This function converts this format in a unique unsigned long where 1 degree = 10 000 000
847
848 EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
849 with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
850 resolution also increased precision of nav calculations
851 static uint32_t GPS_coord_to_degrees(char *coordinateString)
852 {
853 char *p = s, *d = s;
854 uint8_t min, deg = 0;
855 uint16_t frac = 0, mult = 10000;
856
857 while (*p) { // parse the string until its end
858 if (d != s) {
859 frac += (*p - '0') * mult; // calculate only fractional part on up to 5 digits (d != s condition is true when the . is located)
860 mult /= 10;
861 }
862 if (*p == '.')
863 d = p; // locate '.' char in the string
864 p++;
865 }
866 if (p == s)
867 return 0;
868 while (s < d - 2) {
869 deg *= 10; // convert degrees : all chars before minutes ; for the first iteration, deg = 0
870 deg += *(s++) - '0';
871 }
872 min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.'
873 return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
874 }
875 */
876
877 // helper functions
878 #ifdef USE_GPS_NMEA
879 static uint32_t grab_fields(char *src, uint8_t mult)
880 { // convert string to uint32
881 uint32_t i;
882 uint32_t tmp = 0;
883 int isneg = 0;
884 for (i = 0; src[i] != 0; i++) {
885 if ((i == 0) && (src[0] == '-')) { // detect negative sign
886 isneg = 1;
887 continue; // jump to next character if the first one was a negative sign
888 }
889 if (src[i] == '.') {
890 i++;
891 if (mult == 0) {
892 break;
893 } else {
894 src[i + mult] = 0;
895 }
896 }
897 tmp *= 10;
898 if (src[i] >= '0' && src[i] <= '9') {
899 tmp += src[i] - '0';
900 }
901 if (i >= 15) {
902 return 0; // out of bounds
903 }
904 }
905 return isneg ? -tmp : tmp; // handle negative altitudes
906 }
907
908 typedef struct gpsDataNmea_s {
909 int32_t latitude;
910 int32_t longitude;
911 uint8_t numSat;
912 int32_t altitudeCm;
913 uint16_t speed;
914 uint16_t hdop;
915 uint16_t ground_course;
916 uint32_t time;
917 uint32_t date;
918 } gpsDataNmea_t;
919
920 static bool gpsNewFrameNMEA(char c)
921 {
922 static gpsDataNmea_t gps_Msg;
923
924 uint8_t frameOK = 0;
925 static uint8_t param = 0, offset = 0, parity = 0;
926 static char string[15];
927 static uint8_t checksum_param, gps_frame = NO_FRAME;
928 static uint8_t svMessageNum = 0;
929 uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
930
931 switch (c) {
932 case '$':
933 param = 0;
934 offset = 0;
935 parity = 0;
936 break;
937 case ',':
938 case '*':
939 string[offset] = 0;
940 if (param == 0) { //frame identification
941 gps_frame = NO_FRAME;
942 if (0 == strcmp(string, "GPGGA") || 0 == strcmp(string, "GNGGA")) {
943 gps_frame = FRAME_GGA;
944 } else if (0 == strcmp(string, "GPRMC") || 0 == strcmp(string, "GNRMC")) {
945 gps_frame = FRAME_RMC;
946 } else if (0 == strcmp(string, "GPGSV")) {
947 gps_frame = FRAME_GSV;
948 }
949 }
950
951 switch (gps_frame) {
952 case FRAME_GGA: //************* GPGGA FRAME parsing
953 switch (param) {
954 // case 1: // Time information
955 // break;
956 case 2:
957 gps_Msg.latitude = GPS_coord_to_degrees(string);
958 break;
959 case 3:
960 if (string[0] == 'S')
961 gps_Msg.latitude *= -1;
962 break;
963 case 4:
964 gps_Msg.longitude = GPS_coord_to_degrees(string);
965 break;
966 case 5:
967 if (string[0] == 'W')
968 gps_Msg.longitude *= -1;
969 break;
970 case 6:
971 gpsSetFixState(string[0] > '0');
972 break;
973 case 7:
974 gps_Msg.numSat = grab_fields(string, 0);
975 break;
976 case 8:
977 gps_Msg.hdop = grab_fields(string, 1) * 100; // hdop
978 break;
979 case 9:
980 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
981 break;
982 }
983 break;
984 case FRAME_RMC: //************* GPRMC FRAME parsing
985 switch (param) {
986 case 1:
987 gps_Msg.time = grab_fields(string, 2); // UTC time hhmmss.ss
988 break;
989 case 7:
990 gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
991 break;
992 case 8:
993 gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10
994 break;
995 case 9:
996 gps_Msg.date = grab_fields(string, 0); // date dd/mm/yy
997 break;
998 }
999 break;
1000 case FRAME_GSV:
1001 switch (param) {
1002 /*case 1:
1003 // Total number of messages of this type in this cycle
1004 break; */
1005 case 2:
1006 // Message number
1007 svMessageNum = grab_fields(string, 0);
1008 break;
1009 case 3:
1010 // Total number of SVs visible
1011 GPS_numCh = grab_fields(string, 0);
1012 break;
1013 }
1014 if (param < 4)
1015 break;
1016
1017 svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
1018 svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
1019 svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
1020
1021 if (svSatNum > GPS_SV_MAXSATS)
1022 break;
1023
1024 switch (svSatParam) {
1025 case 1:
1026 // SV PRN number
1027 GPS_svinfo_chn[svSatNum - 1] = svSatNum;
1028 GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
1029 break;
1030 /*case 2:
1031 // Elevation, in degrees, 90 maximum
1032 break;
1033 case 3:
1034 // Azimuth, degrees from True North, 000 through 359
1035 break; */
1036 case 4:
1037 // SNR, 00 through 99 dB (null when not tracking)
1038 GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
1039 GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
1040 break;
1041 }
1042
1043 GPS_svInfoReceivedCount++;
1044
1045 break;
1046 }
1047
1048 param++;
1049 offset = 0;
1050 if (c == '*')
1051 checksum_param = 1;
1052 else
1053 parity ^= c;
1054 break;
1055 case '\r':
1056 case '\n':
1057 if (checksum_param) { //parity checksum
1058 shiftPacketLog();
1059 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
1060 if (checksum == parity) {
1061 *gpsPacketLogChar = LOG_IGNORED;
1062 GPS_packetCount++;
1063 switch (gps_frame) {
1064 case FRAME_GGA:
1065 *gpsPacketLogChar = LOG_NMEA_GGA;
1066 frameOK = 1;
1067 if (STATE(GPS_FIX)) {
1068 gpsSol.llh.lat = gps_Msg.latitude;
1069 gpsSol.llh.lon = gps_Msg.longitude;
1070 gpsSol.numSat = gps_Msg.numSat;
1071 gpsSol.llh.altCm = gps_Msg.altitudeCm;
1072 gpsSol.hdop = gps_Msg.hdop;
1073 }
1074 break;
1075 case FRAME_RMC:
1076 *gpsPacketLogChar = LOG_NMEA_RMC;
1077 gpsSol.groundSpeed = gps_Msg.speed;
1078 gpsSol.groundCourse = gps_Msg.ground_course;
1079 #ifdef USE_RTC_TIME
1080 // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
1081 if(!rtcHasTime() && gps_Msg.date != 0 && gps_Msg.time != 0) {
1082 dateTime_t temp_time;
1083 temp_time.year = (gps_Msg.date % 100) + 2000;
1084 temp_time.month = (gps_Msg.date / 100) % 100;
1085 temp_time.day = (gps_Msg.date / 10000) % 100;
1086 temp_time.hours = (gps_Msg.time / 1000000) % 100;
1087 temp_time.minutes = (gps_Msg.time / 10000) % 100;
1088 temp_time.seconds = (gps_Msg.time / 100) % 100;
1089 temp_time.millis = (gps_Msg.time & 100) * 10;
1090 rtcSetDateTime(&temp_time);
1091 }
1092 #endif
1093 break;
1094 } // end switch
1095 } else {
1096 *gpsPacketLogChar = LOG_ERROR;
1097 }
1098 }
1099 checksum_param = 0;
1100 break;
1101 default:
1102 if (offset < 15)
1103 string[offset++] = c;
1104 if (!checksum_param)
1105 parity ^= c;
1106 }
1107 return frameOK;
1108 }
1109 #endif // USE_GPS_NMEA
1110
1111 #ifdef USE_GPS_UBLOX
1112 // UBX support
1113 typedef struct {
1114 uint32_t time; // GPS msToW
1115 int32_t longitude;
1116 int32_t latitude;
1117 int32_t altitude_ellipsoid;
1118 int32_t altitudeMslMm;
1119 uint32_t horizontal_accuracy;
1120 uint32_t vertical_accuracy;
1121 } ubx_nav_posllh;
1122
1123 typedef struct {
1124 uint32_t time; // GPS msToW
1125 uint8_t fix_type;
1126 uint8_t fix_status;
1127 uint8_t differential_status;
1128 uint8_t res;
1129 uint32_t time_to_first_fix;
1130 uint32_t uptime; // milliseconds
1131 } ubx_nav_status;
1132
1133 typedef struct {
1134 uint32_t time;
1135 int32_t time_nsec;
1136 int16_t week;
1137 uint8_t fix_type;
1138 uint8_t fix_status;
1139 int32_t ecef_x;
1140 int32_t ecef_y;
1141 int32_t ecef_z;
1142 uint32_t position_accuracy_3d;
1143 int32_t ecef_x_velocity;
1144 int32_t ecef_y_velocity;
1145 int32_t ecef_z_velocity;
1146 uint32_t speed_accuracy;
1147 uint16_t position_DOP;
1148 uint8_t res;
1149 uint8_t satellites;
1150 uint32_t res2;
1151 } ubx_nav_solution;
1152
1153 typedef struct {
1154 uint32_t time; // GPS msToW
1155 int32_t ned_north;
1156 int32_t ned_east;
1157 int32_t ned_down;
1158 uint32_t speed_3d;
1159 uint32_t speed_2d;
1160 int32_t heading_2d;
1161 uint32_t speed_accuracy;
1162 uint32_t heading_accuracy;
1163 } ubx_nav_velned;
1164
1165 typedef struct {
1166 uint8_t chn; // Channel number, 255 for SVx not assigned to channel
1167 uint8_t svid; // Satellite ID
1168 uint8_t flags; // Bitmask
1169 uint8_t quality; // Bitfield
1170 uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
1171 uint8_t elev; // Elevation in integer degrees
1172 int16_t azim; // Azimuth in integer degrees
1173 int32_t prRes; // Pseudo range residual in centimetres
1174 } ubx_nav_svinfo_channel;
1175
1176 typedef struct {
1177 uint32_t time; // GPS Millisecond time of week
1178 uint8_t numCh; // Number of channels
1179 uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
1180 uint16_t reserved2; // Reserved
1181 ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
1182 } ubx_nav_svinfo;
1183
1184 typedef struct {
1185 uint8_t clsId; // Class ID of the acknowledged message
1186 uint8_t msgId; // Message ID of the acknowledged message
1187 } ubx_ack;
1188
1189 enum {
1190 PREAMBLE1 = 0xb5,
1191 PREAMBLE2 = 0x62,
1192 CLASS_NAV = 0x01,
1193 CLASS_ACK = 0x05,
1194 CLASS_CFG = 0x06,
1195 MSG_ACK_NACK = 0x00,
1196 MSG_ACK_ACK = 0x01,
1197 MSG_POSLLH = 0x2,
1198 MSG_STATUS = 0x3,
1199 MSG_SOL = 0x6,
1200 MSG_VELNED = 0x12,
1201 MSG_SVINFO = 0x30,
1202 MSG_CFG_PRT = 0x00,
1203 MSG_CFG_RATE = 0x08,
1204 MSG_CFG_SET_RATE = 0x01,
1205 MSG_CFG_NAV_SETTINGS = 0x24
1206 } ubx_protocol_bytes;
1207
1208 enum {
1209 FIX_NONE = 0,
1210 FIX_DEAD_RECKONING = 1,
1211 FIX_2D = 2,
1212 FIX_3D = 3,
1213 FIX_GPS_DEAD_RECKONING = 4,
1214 FIX_TIME = 5
1215 } ubs_nav_fix_type;
1216
1217 enum {
1218 NAV_STATUS_FIX_VALID = 1,
1219 NAV_STATUS_TIME_WEEK_VALID = 4,
1220 NAV_STATUS_TIME_SECOND_VALID = 8
1221 } ubx_nav_status_bits;
1222
1223 // Packet checksum accumulators
1224 static uint8_t _ck_a;
1225 static uint8_t _ck_b;
1226
1227 // State machine state
1228 static bool _skip_packet;
1229 static uint8_t _step;
1230 static uint8_t _msg_id;
1231 static uint16_t _payload_length;
1232 static uint16_t _payload_counter;
1233
1234 static bool next_fix;
1235 static uint8_t _class;
1236
1237 // do we have new position information?
1238 static bool _new_position;
1239
1240 // do we have new speed information?
1241 static bool _new_speed;
1242
1243 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
1244 //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
1245 //15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
1246 //15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
1247 //15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status'
1248 //15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status'
1249 //15:17:55 R -> UBX 03-09, Size 208, 'Unknown'
1250 //15:17:55 R -> UBX 03-10, Size 336, 'Unknown'
1251 //15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution'
1252 //15:17:55 R -> UBX NAV, Size 100, 'Navigation'
1253 //15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information'
1254
1255 // from the UBlox6 document, the largest payout we receive i the NAV-SVINFO and the payload size
1256 // is calculated as 8 + 12*numCh. numCh in the case of a Glonass receiver is 28.
1257 #define UBLOX_PAYLOAD_SIZE 344
1258
1259
1260 // Receive buffer
1261 static union {
1262 ubx_nav_posllh posllh;
1263 ubx_nav_status status;
1264 ubx_nav_solution solution;
1265 ubx_nav_velned velned;
1266 ubx_nav_svinfo svinfo;
1267 ubx_ack ack;
1268 uint8_t bytes[UBLOX_PAYLOAD_SIZE];
1269 } _buffer;
1270
1271 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
1272 {
1273 while (len--) {
1274 *ck_a += *data;
1275 *ck_b += *ck_a;
1276 data++;
1277 }
1278 }
1279
1280
1281 static bool UBLOX_parse_gps(void)
1282 {
1283 uint32_t i;
1284
1285 *gpsPacketLogChar = LOG_IGNORED;
1286
1287 switch (_msg_id) {
1288 case MSG_POSLLH:
1289 *gpsPacketLogChar = LOG_UBLOX_POSLLH;
1290 //i2c_dataset.time = _buffer.posllh.time;
1291 gpsSol.llh.lon = _buffer.posllh.longitude;
1292 gpsSol.llh.lat = _buffer.posllh.latitude;
1293 gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm
1294 gpsSetFixState(next_fix);
1295 _new_position = true;
1296 break;
1297 case MSG_STATUS:
1298 *gpsPacketLogChar = LOG_UBLOX_STATUS;
1299 next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
1300 if (!next_fix)
1301 DISABLE_STATE(GPS_FIX);
1302 break;
1303 case MSG_SOL:
1304 *gpsPacketLogChar = LOG_UBLOX_SOL;
1305 next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
1306 if (!next_fix)
1307 DISABLE_STATE(GPS_FIX);
1308 gpsSol.numSat = _buffer.solution.satellites;
1309 gpsSol.hdop = _buffer.solution.position_DOP;
1310 #ifdef USE_RTC_TIME
1311 //set clock, when gps time is available
1312 if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) {
1313 //calculate rtctime: week number * ms in a week + ms of week + fractions of second + offset to UNIX reference year - 18 leap seconds
1314 rtcTime_t temp_time = (((int64_t) _buffer.solution.week)*7*24*60*60*1000) + _buffer.solution.time + (_buffer.solution.time_nsec/1000000) + 315964800000LL - 18000;
1315 rtcSet(&temp_time);
1316 }
1317 #endif
1318 break;
1319 case MSG_VELNED:
1320 *gpsPacketLogChar = LOG_UBLOX_VELNED;
1321 gpsSol.speed3d = _buffer.velned.speed_3d; // cm/s
1322 gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
1323 gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
1324 _new_speed = true;
1325 break;
1326 case MSG_SVINFO:
1327 *gpsPacketLogChar = LOG_UBLOX_SVINFO;
1328 GPS_numCh = _buffer.svinfo.numCh;
1329 if (GPS_numCh > 16)
1330 GPS_numCh = 16;
1331 for (i = 0; i < GPS_numCh; i++) {
1332 GPS_svinfo_chn[i] = _buffer.svinfo.channel[i].chn;
1333 GPS_svinfo_svid[i] = _buffer.svinfo.channel[i].svid;
1334 GPS_svinfo_quality[i] =_buffer.svinfo.channel[i].quality;
1335 GPS_svinfo_cno[i] = _buffer.svinfo.channel[i].cno;
1336 }
1337 for (i = GPS_numCh; i < 16; i++) {
1338 GPS_svinfo_chn[i] = 0;
1339 GPS_svinfo_svid[i] = 0;
1340 GPS_svinfo_quality[i] = 0;
1341 GPS_svinfo_cno[i] = 0;
1342 }
1343 GPS_svInfoReceivedCount++;
1344 break;
1345 case MSG_ACK_ACK:
1346 if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
1347 gpsData.ackState = UBLOX_ACK_GOT_ACK;
1348 }
1349 break;
1350 case MSG_ACK_NACK:
1351 if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
1352 gpsData.ackState = UBLOX_ACK_GOT_NACK;
1353 }
1354 break;
1355 default:
1356 return false;
1357 }
1358
1359 // we only return true when we get new position and speed data
1360 // this ensures we don't use stale data
1361 if (_new_position && _new_speed) {
1362 _new_speed = _new_position = false;
1363 return true;
1364 }
1365 return false;
1366 }
1367
1368 static bool gpsNewFrameUBLOX(uint8_t data)
1369 {
1370 bool parsed = false;
1371
1372 switch (_step) {
1373 case 0: // Sync char 1 (0xB5)
1374 if (PREAMBLE1 == data) {
1375 _skip_packet = false;
1376 _step++;
1377 }
1378 break;
1379 case 1: // Sync char 2 (0x62)
1380 if (PREAMBLE2 != data) {
1381 _step = 0;
1382 break;
1383 }
1384 _step++;
1385 break;
1386 case 2: // Class
1387 _step++;
1388 _class = data;
1389 _ck_b = _ck_a = data; // reset the checksum accumulators
1390 break;
1391 case 3: // Id
1392 _step++;
1393 _ck_b += (_ck_a += data); // checksum byte
1394 _msg_id = data;
1395 break;
1396 case 4: // Payload length (part 1)
1397 _step++;
1398 _ck_b += (_ck_a += data); // checksum byte
1399 _payload_length = data; // payload length low byte
1400 break;
1401 case 5: // Payload length (part 2)
1402 _step++;
1403 _ck_b += (_ck_a += data); // checksum byte
1404 _payload_length += (uint16_t)(data << 8);
1405 if (_payload_length > UBLOX_PAYLOAD_SIZE) {
1406 _skip_packet = true;
1407 }
1408 _payload_counter = 0; // prepare to receive payload
1409 if (_payload_length == 0) {
1410 _step = 7;
1411 }
1412 break;
1413 case 6:
1414 _ck_b += (_ck_a += data); // checksum byte
1415 if (_payload_counter < UBLOX_PAYLOAD_SIZE) {
1416 _buffer.bytes[_payload_counter] = data;
1417 }
1418 if (++_payload_counter >= _payload_length) {
1419 _step++;
1420 }
1421 break;
1422 case 7:
1423 _step++;
1424 if (_ck_a != data) {
1425 _skip_packet = true; // bad checksum
1426 gpsData.errors++;
1427 }
1428 break;
1429 case 8:
1430 _step = 0;
1431
1432 shiftPacketLog();
1433
1434 if (_ck_b != data) {
1435 *gpsPacketLogChar = LOG_ERROR;
1436 gpsData.errors++;
1437 break; // bad checksum
1438 }
1439
1440 GPS_packetCount++;
1441
1442 if (_skip_packet) {
1443 *gpsPacketLogChar = LOG_SKIPPED;
1444 break;
1445 }
1446
1447 if (UBLOX_parse_gps()) {
1448 parsed = true;
1449 }
1450 }
1451 return parsed;
1452 }
1453 #endif // USE_GPS_UBLOX
1454
1455 static void gpsHandlePassthrough(uint8_t data)
1456 {
1457 gpsNewData(data);
1458 #ifdef USE_DASHBOARD
1459 if (featureIsEnabled(FEATURE_DASHBOARD)) {
1460 dashboardUpdate(micros());
1461 }
1462 #endif
1463
1464 }
1465
1466 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
1467 {
1468 waitForSerialPortToFinishTransmitting(gpsPort);
1469 waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
1470
1471 if (!(gpsPort->mode & MODE_TX))
1472 serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
1473
1474 #ifdef USE_DASHBOARD
1475 if (featureIsEnabled(FEATURE_DASHBOARD)) {
1476 dashboardShowFixedPage(PAGE_GPS);
1477 }
1478 #endif
1479
1480 serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
1481 }
1482
1483 float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
1484
1485 void GPS_calc_longitude_scaling(int32_t lat)
1486 {
1487 float rads = (fabsf((float)lat) / 10000000.0f) * 0.0174532925f;
1488 GPS_scaleLonDown = cos_approx(rads);
1489 }
1490
1491 ////////////////////////////////////////////////////////////////////////////////////
1492 // Calculate the distance flown and vertical speed from gps position data
1493 //
1494 static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
1495 {
1496 static int32_t lastCoord[2] = { 0, 0 };
1497 static int32_t lastAlt;
1498 static int32_t lastMillis;
1499
1500 int currentMillis = millis();
1501
1502 if (initialize) {
1503 GPS_distanceFlownInCm = 0;
1504 GPS_verticalSpeedInCmS = 0;
1505 } else {
1506 if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
1507 uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
1508 // Only add up movement when speed is faster than minimum threshold
1509 if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
1510 uint32_t dist;
1511 int32_t dir;
1512 GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
1513 if (gpsConfig()->gps_use_3d_speed) {
1514 dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
1515 }
1516 GPS_distanceFlownInCm += dist;
1517 }
1518 }
1519 GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
1520 GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
1521 }
1522 lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
1523 lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
1524 lastAlt = gpsSol.llh.altCm;
1525 lastMillis = currentMillis;
1526 }
1527
1528 void GPS_reset_home_position(void)
1529 {
1530 if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
1531 if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
1532 GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
1533 GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
1534 GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
1535 // Set ground altitude
1536 ENABLE_STATE(GPS_FIX_HOME);
1537 }
1538 }
1539 GPS_calculateDistanceFlownVerticalSpeed(true); //Initialize
1540 }
1541
1542 ////////////////////////////////////////////////////////////////////////////////////
1543 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
1544 #define TAN_89_99_DEGREES 5729.57795f
1545 // Get distance between two points in cm
1546 // Get bearing from pos1 to pos2, returns an 1deg = 100 precision
1547 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
1548 {
1549 float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
1550 float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
1551 *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
1552
1553 *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
1554 if (*bearing < 0)
1555 *bearing += 36000;
1556 }
1557
1558 void GPS_calculateDistanceAndDirectionToHome(void)
1559 {
1560 if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
1561 uint32_t dist;
1562 int32_t dir;
1563 GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
1564 GPS_distanceToHome = dist / 100;
1565 GPS_directionToHome = dir / 100;
1566 } else {
1567 GPS_distanceToHome = 0;
1568 GPS_directionToHome = 0;
1569 }
1570 }
1571
1572 void onGpsNewData(void)
1573 {
1574 if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
1575 return;
1576 }
1577
1578 //
1579 // Calculate time delta for navigation loop, range 0-1.0f, in seconds
1580 //
1581 // Time for calculating x,y speed and navigation pids
1582 static uint32_t nav_loopTimer;
1583 dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
1584 nav_loopTimer = millis();
1585 // prevent runup from bad GPS
1586 dTnav = MIN(dTnav, 1.0f);
1587
1588 GPS_calculateDistanceAndDirectionToHome();
1589 if (ARMING_FLAG(ARMED)) {
1590 GPS_calculateDistanceFlownVerticalSpeed(false);
1591 }
1592
1593 #ifdef USE_GPS_RESCUE
1594 rescueNewGpsData();
1595 #endif
1596 }
1597
1598 void gpsSetFixState(bool state)
1599 {
1600 if (state) {
1601 ENABLE_STATE(GPS_FIX);
1602 ENABLE_STATE(GPS_FIX_EVER);
1603 } else {
1604 DISABLE_STATE(GPS_FIX);
1605 }
1606 }
1607 #endif