Skip to content

Commit 4b8f80a

Browse files
DavBfrDavBfr
authored
feat: add U-BLOX GPS support (#4689)
1 parent 9ed9227 commit 4b8f80a

13 files changed

+1531
-257
lines changed

‎radio/src/gps.cpp

+70-244
Original file line numberDiff line numberDiff line change
@@ -38,28 +38,15 @@
3838
*/
3939

4040
#include "opentx.h"
41+
#include "gps.h"
42+
#include "gps_nmea.h"
43+
#include "gps_ubx.h"
4144
#include <ctype.h>
4245

4346
gpsdata_t gpsData;
44-
45-
/* This is a light implementation of a GPS frame decoding
46-
This should work with most of modern GPS devices configured to output 5 frames.
47-
It assumes there are some NMEA GGA frames to decode on the serial bus
48-
Now verifies checksum correctly before applying data
49-
50-
Here we use only the following data :
51-
- latitude
52-
- longitude
53-
- GPS fix is/is not ok
54-
- GPS num sat (4 is enough to be +/- reliable)
55-
// added by Mis
56-
- GPS altitude (for OSD displaying)
57-
- GPS speed (for OSD displaying)
58-
*/
59-
60-
#define NO_FRAME 0
61-
#define FRAME_GGA 1
62-
#define FRAME_RMC 2
47+
static int gpsProtocol = -1;
48+
const etx_serial_driver_t* gpsSerialDrv = nullptr;
49+
void* gpsSerialCtx = nullptr;
6350

6451
#define DIGIT_TO_VAL(_x) (_x - '0')
6552

@@ -104,279 +91,118 @@ uint32_t GPS_coord_to_degrees(const char * coordinateString)
10491
return degrees * 1000000UL + (minutes * 100000UL + fractionalMinutes * 10UL) / 6;
10592
}
10693

107-
// helper functions
108-
uint32_t grab_fields(char * src, uint8_t mult)
94+
static void changeBaudrate()
10995
{
110-
uint32_t i;
111-
uint32_t tmp = 0;
112-
for (i = 0; src[i] != 0; i++) {
113-
if (src[i] == '.') {
114-
i++;
115-
if (mult == 0)
116-
break;
117-
else
118-
src[i + mult] = 0;
119-
}
120-
tmp *= 10;
121-
if (src[i] >= '0' && src[i] <= '9')
122-
tmp += src[i] - '0';
123-
if (i >= 15)
124-
return 0; // out of bounds
125-
}
126-
return tmp;
127-
}
96+
const int baudrates_count = 5;
97+
const uint32_t baudrates[] = {9600, 57600, 115200, 19200, 38400};
98+
static uint8_t current_rate = 0;
12899

129-
typedef struct gpsDataNmea_s
130-
{
131-
uint8_t fix;
132-
int32_t latitude;
133-
int32_t longitude;
134-
uint8_t numSat;
135-
uint16_t altitude;
136-
uint16_t speed;
137-
uint16_t groundCourse;
138-
uint16_t hdop;
139-
uint32_t date;
140-
uint32_t time;
141-
} gpsDataNmea_t;
100+
auto setBaudrate = gpsSerialDrv->setBaudrate;
101+
if (setBaudrate == nullptr) return;
102+
setBaudrate(gpsSerialCtx, baudrates[++current_rate % baudrates_count]);
103+
}
142104

143-
bool gpsNewFrameNMEA(char c)
105+
static void autodetectProtocol(uint8_t c)
144106
{
145-
static gpsDataNmea_t gps_Msg;
146-
147-
uint8_t frameOK = 0;
148-
static uint8_t param = 0, offset = 0, parity = 0;
149-
static char string[15];
150-
static uint8_t checksum_param, gps_frame = NO_FRAME;
151-
152-
switch (c) {
153-
case '$':
154-
param = 0;
155-
offset = 0;
156-
parity = 0;
157-
break;
158-
case ',':
159-
case '*':
160-
string[offset] = 0;
161-
if (param == 0) {
162-
// Frame identification (accept all GPS talkers (GP: GPS, GL:Glonass, GN:combination, etc...))
163-
gps_frame = NO_FRAME;
164-
if (string[0] == 'G' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') {
165-
gps_frame = FRAME_GGA;
166-
}
167-
else if (string[0] == 'G' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') {
168-
gps_frame = FRAME_RMC;
169-
}
170-
else {
171-
// turn off this frame (do this only once a second)
172-
static gtime_t lastGpsCmdSent = 0;
173-
if (string[0] == 'G' && g_rtcTime != lastGpsCmdSent) {
174-
lastGpsCmdSent = g_rtcTime;
175-
char cmd[] = "$PUBX,40,GSV,0,0,0,0";
176-
cmd[9] = string[2];
177-
cmd[10] = string[3];
178-
cmd[11] = string[4];
179-
gpsSendFrame(cmd);
180-
}
181-
}
107+
static tmr10ms_t time;
108+
static uint8_t state = 0;
109+
110+
switch (state) {
111+
case 0: // Init
112+
time = get_tmr10ms();
113+
state = 1;
114+
case 1: // Wait for a valid packet
115+
if (gpsNewFrameNMEA(c)) {
116+
gpsProtocol = GPS_PROTOCOL_NMEA;
117+
state = 0;
118+
return;
182119
}
183120

184-
switch (gps_frame) {
185-
case FRAME_GGA: //************* GPGGA FRAME parsing
186-
switch (param) {
187-
case 2:
188-
gps_Msg.latitude = GPS_coord_to_degrees(string);
189-
break;
190-
case 3:
191-
if (string[0] == 'S')
192-
gps_Msg.latitude *= -1;
193-
break;
194-
case 4:
195-
gps_Msg.longitude = GPS_coord_to_degrees(string);
196-
break;
197-
case 5:
198-
if (string[0] == 'W')
199-
gps_Msg.longitude *= -1;
200-
break;
201-
case 6:
202-
if (string[0] > '0') {
203-
gps_Msg.fix = 1;
204-
}
205-
else {
206-
gps_Msg.fix = 0;
207-
}
208-
break;
209-
case 7:
210-
gps_Msg.numSat = grab_fields(string, 0);
211-
break;
212-
case 8:
213-
gps_Msg.hdop = grab_fields(string, 1) * 10;
214-
break;
215-
case 9:
216-
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
217-
break;
218-
}
219-
break;
220-
case FRAME_RMC: //************* GPRMC FRAME parsing
221-
switch (param) {
222-
case 1:
223-
gps_Msg.time = grab_fields(string, 0);
224-
break;
225-
case 2:
226-
if (string[0] == 'A') {
227-
gps_Msg.fix = 1;
228-
}
229-
else {
230-
gps_Msg.fix = 0;
231-
}
232-
break;
233-
case 7:
234-
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
235-
break;
236-
case 8:
237-
gps_Msg.groundCourse = (grab_fields(string, 1)); // ground course deg * 10
238-
break;
239-
case 9:
240-
gps_Msg.date = grab_fields(string, 0);
241-
break;
242-
}
243-
break;
244-
121+
if (gpsNewFrameUBX(c, true)) {
122+
gpsProtocol = GPS_PROTOCOL_UBX;
123+
state = 0;
124+
return;
245125
}
246126

247-
param++;
248-
offset = 0;
249-
if (c == '*')
250-
checksum_param = 1;
251-
else
252-
parity ^= c;
253-
break;
254-
case '\r':
255-
case '\n':
256-
if (checksum_param) { //parity checksum
257-
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') +
258-
((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
259-
if (checksum == parity) {
260-
gpsData.packetCount++;
261-
switch (gps_frame) {
262-
case FRAME_GGA:
263-
frameOK = 1;
264-
gpsData.fix = gps_Msg.fix;
265-
gpsData.numSat = gps_Msg.numSat;
266-
gpsData.hdop = gps_Msg.hdop;
267-
if (gps_Msg.fix) {
268-
__disable_irq(); // do the atomic update of lat/lon
269-
gpsData.latitude = gps_Msg.latitude;
270-
gpsData.longitude = gps_Msg.longitude;
271-
gpsData.altitude = gps_Msg.altitude;
272-
__enable_irq();
273-
}
274-
break;
275-
case FRAME_RMC:
276-
gpsData.speed = gps_Msg.speed;
277-
gpsData.groundCourse = gps_Msg.groundCourse;
278-
#if defined(RTCLOCK)
279-
// set RTC clock if needed
280-
if (g_eeGeneral.adjustRTC && gps_Msg.fix) {
281-
div_t qr = div(gps_Msg.date, 100);
282-
uint8_t year = qr.rem;
283-
qr = div(qr.quot, 100);
284-
uint8_t mon = qr.rem;
285-
uint8_t day = qr.quot;
286-
qr = div(gps_Msg.time, 100);
287-
uint8_t sec = qr.rem;
288-
qr = div(qr.quot, 100);
289-
uint8_t min = qr.rem;
290-
uint8_t hour = qr.quot;
291-
rtcAdjust(year+2000, mon, day, hour, min, sec);
292-
}
293-
#endif
294-
} // end switch
295-
}
296-
else {
297-
gpsData.errorCount++;
298-
}
127+
uint32_t new_time = get_tmr10ms();
128+
if (new_time - time > 20) {
129+
// No message received
130+
changeBaudrate();
131+
time = new_time;
299132
}
300-
checksum_param = 0;
301-
break;
302-
default:
303-
if (offset < 15)
304-
string[offset++] = c;
305-
if (!checksum_param)
306-
parity ^= c;
307133
}
308-
return frameOK;
309134
}
310135

311-
bool gpsNewFrame(uint8_t c)
136+
static void detectDisconnected(bool has_frame)
312137
{
313-
return gpsNewFrameNMEA(c);
138+
static tmr10ms_t time = 0;
139+
140+
if (has_frame) {
141+
time = get_tmr10ms();
142+
} else if (time > 0 && get_tmr10ms() - time > 500) {
143+
gpsProtocol = GPS_PROTOCOL_AUTO;
144+
time = get_tmr10ms();
145+
}
314146
}
315147

148+
316149
void gpsNewData(uint8_t c)
317150
{
318-
if (!gpsNewFrame(c)) {
319-
return;
151+
switch (gpsProtocol) {
152+
case GPS_PROTOCOL_NMEA:
153+
detectDisconnected(gpsNewFrameNMEA(c));
154+
break;
155+
case GPS_PROTOCOL_UBX:
156+
detectDisconnected(gpsNewFrameUBX(c, false));
157+
break;
158+
case GPS_PROTOCOL_AUTO:
159+
autodetectProtocol(c);
160+
break;
320161
}
321162
}
322163

323-
static const etx_serial_driver_t* gpsSerialDrv = nullptr;
324-
static void* gpsSerialCtx = nullptr;
325164

326165
#if defined(DEBUG)
327166
uint8_t gpsTraceEnabled = false;
328167
#endif
329168

330-
void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv)
169+
void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv, int protocol)
331170
{
171+
gpsProtocol = protocol;
332172
gpsSerialCtx = ctx;
333173
gpsSerialDrv = drv;
334174
}
335175

336176
void gpsWakeup()
337177
{
338178
if (!gpsSerialDrv) return;
339-
179+
340180
auto _getByte = gpsSerialDrv->getByte;
341181
if (!_getByte) return;
342182

183+
static tmr10ms_t time = get_tmr10ms();
343184
uint8_t byte;
344185
while (_getByte(gpsSerialCtx, &byte)) {
345186
#if defined(DEBUG)
346187
if (gpsTraceEnabled) {
347188
dbgSerialPutc(byte);
348189
}
349-
#endif
190+
#endif
350191
gpsNewData(byte);
192+
time = get_tmr10ms();
351193
}
352-
}
353194

354-
char hex(uint8_t b) {
355-
return b > 9 ? b + 'A' - 10 : b + '0';
195+
if (get_tmr10ms() - time > 20) {
196+
changeBaudrate();
197+
time = get_tmr10ms();
198+
}
356199
}
357200

358201
void gpsSendFrame(const char * frame)
359202
{
360-
if (!gpsSerialDrv) return;
361-
362-
auto _sendByte = gpsSerialDrv->sendByte;
363-
if (!_sendByte) return;
364-
365-
// send given frame, add checksum and CRLF
366-
uint8_t parity = 0;
367-
TRACE_NOCRLF("gps> %s", frame);
368-
369-
while (*frame) {
370-
if (*frame != '$') parity ^= *frame;
371-
_sendByte(gpsSerialCtx, *frame);
372-
++frame;
203+
switch (gpsProtocol) {
204+
case GPS_PROTOCOL_NMEA:
205+
gpsSendFrameNMEA(frame);
206+
break;
373207
}
374-
375-
_sendByte(gpsSerialCtx, '*');
376-
_sendByte(gpsSerialCtx, hex(parity >> 4));
377-
_sendByte(gpsSerialCtx, hex(parity & 0x0F));
378-
_sendByte(gpsSerialCtx, '\r');
379-
_sendByte(gpsSerialCtx, '\n');
380-
381-
TRACE("*%02x", parity);
382208
}

0 commit comments

Comments
 (0)