-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
gps_ubx_ucenter.c
414 lines (363 loc) · 11.9 KB
/
gps_ubx_ucenter.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
/*
* Copyright (C) 2008-2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
* Initial author: C. De Wagter
*/
#include "gps_ubx_ucenter.h"
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
//
// UCENTER: init, periodic and event
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
static bool_t gps_ubx_ucenter_configure(uint8_t nr);
#define GPS_UBX_UCENTER_STATUS_STOPPED 0
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
#define GPS_UBX_UCENTER_STATUS_CONFIG 2
#define GPS_UBX_UCENTER_REPLY_NONE 0
#define GPS_UBX_UCENTER_REPLY_ACK 1
#define GPS_UBX_UCENTER_REPLY_NACK 2
#define GPS_UBX_UCENTER_REPLY_VERSION 3
// All U-Center data
struct gps_ubx_ucenter_struct gps_ubx_ucenter;
/////////////////////////////
// Init Function
void gps_ubx_ucenter_init(void)
{
// Start UCenter
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_AUTOBAUD;
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
gps_ubx_ucenter.cnt = 0;
gps_ubx_ucenter.baud_init = 0;
gps_ubx_ucenter.baud_run = 0;
gps_ubx_ucenter.sw_ver_h = 0;
gps_ubx_ucenter.sw_ver_l = 0;
gps_ubx_ucenter.hw_ver_h = 0;
gps_ubx_ucenter.hw_ver_l = 0;
for (int i=0; i<GPS_UBX_UCENTER_CONFIG_STEPS; i++)
{
gps_ubx_ucenter.replies[i] = 0;
}
}
/////////////////////////////
// Periodic Function
// -time-based configuration
void gps_ubx_ucenter_periodic(void)
{
switch (gps_ubx_ucenter.status)
{
// Save processing time inflight
case GPS_UBX_UCENTER_STATUS_STOPPED:
return;
// Automatically Determine Current Baudrate
case GPS_UBX_UCENTER_STATUS_AUTOBAUD:
if (gps_ubx_ucenter_autobaud(gps_ubx_ucenter.cnt) == FALSE)
{
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
gps_ubx_ucenter.cnt = 0;
}
else
{
gps_ubx_ucenter.cnt++;
}
break;
// Send Configuration
case GPS_UBX_UCENTER_STATUS_CONFIG:
if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE)
{
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED;
gps_ubx_ucenter.cnt = 0;
}
else
{
gps_ubx_ucenter.cnt++;
}
break;
default:
// stop this module now...
// todo
break;
}
}
/////////////////////////////
// Event Function
// -fetch replies: ACK and VERSION info
void gps_ubx_ucenter_event(void)
{
// Save processing time inflight
if (gps_ubx_ucenter.status == GPS_UBX_UCENTER_STATUS_STOPPED)
return;
// Read Configuration Reply's
if (gps_ubx.msg_class == UBX_ACK_ID) {
if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK;
}
else
{
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
}
}
// Version info
else if (gps_ubx.msg_class == UBX_MON_ID) {
if (gps_ubx.msg_id == UBX_MON_VER_ID ) {
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_VERSION;
gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,0) - '0';
gps_ubx_ucenter.sw_ver_l = 10*(UBX_MON_VER_c(gps_ubx.msg_buf,2) - '0');
gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf,3) - '0';
gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,33) - '0';
gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0');
gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0';
gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0');
}
}
}
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
//
// UCENTER Configuration Functions
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
{
switch (nr)
{
case 0:
case 1:
// Very important for some modules:
// Give the GPS some time to boot (up to 0.75 second)
break;
case 2:
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
GpsUartSetBaudrate(B38400); // Try the most common first?
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
break;
case 3:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
{
gps_ubx_ucenter.baud_init = 38400;
return FALSE;
}
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
GpsUartSetBaudrate(B9600); // Maybe the factory default?
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
break;
case 4:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
{
gps_ubx_ucenter.baud_init = 9600;
return FALSE;
}
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
GpsUartSetBaudrate(B57600); // The high-rate default?
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
break;
case 5:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
{
gps_ubx_ucenter.baud_init = 57600;
return FALSE;
}
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
GpsUartSetBaudrate(B4800); // Default NMEA baudrate finally?
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
break;
case 6:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK)
{
gps_ubx_ucenter.baud_init = 4800;
return FALSE;
}
// Autoconfig Failed... let's setup the failsafe baudrate
// Should we try even a different baudrate?
gps_ubx_ucenter.baud_init = 0;
GpsUartSetBaudrate(B9600);
return FALSE;
default:
break;
}
return TRUE;
}
/////////////////////////////////////
// UBlox internal Navigation Solution
#define NAV_DYN_STATIONARY 1
#define NAV_DYN_PEDESTRIAN 2
#define NAV_DYN_AUTOMOTIVE 3
#define NAV_DYN_SEA 4
#define NAV_DYN_AIRBORNE_1G 5
#define NAV_DYN_AIRBORNE_2G 6
#define NAV_DYN_AIRBORNE_4G 7
#define NAV5_DYN_PORTABLE 0
#define NAV5_DYN_FIXED 1
#define NAV5_DYN_STATIONARY 2
#define NAV5_DYN_PEDESTRIAN 3
#define NAV5_DYN_AUTOMOTIVE 4
#define NAV5_DYN_SEA 5
#define NAV5_DYN_AIRBORNE_1G 6
#define NAV5_DYN_AIRBORNE_2G 7
#define NAV5_DYN_AIRBORNE_4G 8
#define NAV5_2D_ONLY 1
#define NAV5_3D_ONLY 2
#define NAV5_AUTO 3
#define IGNORED 0
#define RESERVED 0
static inline void gps_ubx_ucenter_config_nav(void)
{
//New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
if (gps_ubx_ucenter.sw_ver_h < 5)
{
UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
}
else
{
UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED);
}
}
/////////////////////////////////////
// UBlox port and protocol GPS configuration
#define UBX_PROTO_MASK 0x0001
#define NMEA_PROTO_MASK 0x0002
#define RTCM_PROTO_MASK 0x0004
#define GPS_PORT_DDC 0x00
#define GPS_PORT_UART1 0x01
#define GPS_PORT_UART2 0x02
#define GPS_PORT_USB 0x03
#define GPS_PORT_SPI 0x04
#ifndef GPS_PORT_ID
#define GPS_PORT_ID GPS_PORT_UART1
#endif
#define __UBX_GPS_BAUD(_u) _u##_BAUD
#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
static inline void gps_ubx_ucenter_config_port(void)
{
// I2C Interface
#if GPS_PORT_ID == GPS_PORT_DDC
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
#endif
// UART Interface
#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
#endif
}
#define GPS_SBAS_ENABLED 0x01
#define GPS_SBAS_RANGING 0x01
#define GPS_SBAS_CORRECTIONS 0x02
#define GPS_SBAS_INTEGRITY 0x04
static inline void gps_ubx_ucenter_config_sbas(void)
{
// Since March 2nd 2011 EGNOS is released for aviation purposes
const uint8_t nrofsat = 1;
UbxSend_CFG_SBAS(GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, nrofsat, 0x00, 0x00);
//UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
}
static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
{
#if GPS_PORT_ID == GPS_PORT_UART1
UbxSend_CFG_MSG(class, id, 0, rate, 0, 0);
#endif
#if GPS_PORT_ID == GPS_PORT_UART2
UbxSend_CFG_MSG(class, id, 0, 0, rate, 0);
#endif
#if GPS_PORT_ID == GPS_PORT_DDC
UbxSend_CFG_MSG(class, id, rate, 0, 0, 0);
#endif
}
// Text Telemetry for Debugging
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#undef GOT_PAYLOAD
#include "subsystems/datalink/downlink.h"
static bool_t gps_ubx_ucenter_configure(uint8_t nr)
{
// Store the reply of the last configuration step and reset
if (nr < GPS_UBX_UCENTER_CONFIG_STEPS)
gps_ubx_ucenter.replies[nr] = gps_ubx_ucenter.reply;
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
switch (nr) {
case 0:
UbxSend_MON_GET_VER();
break;
case 1:
case 2:
case 3:
case 4:
// UBX_G5010 takes 0.7 seconds to answer a firmware request
// Version info is important for porper configuration as different firmwares have different settings
break;
case 5:
// Send some debugging info: detected baudrate, software version etc...
gps_ubx_ucenter.replies[0] = (gps_ubx_ucenter.baud_init/1000);
gps_ubx_ucenter.replies[1] = (gps_ubx_ucenter.baud_init - 1000 * gps_ubx_ucenter.replies[0]) / 100;
gps_ubx_ucenter.replies[2] = gps_ubx_ucenter.sw_ver_h;
gps_ubx_ucenter.replies[3] = gps_ubx_ucenter.sw_ver_l;
gps_ubx_ucenter.replies[4] = gps_ubx_ucenter.hw_ver_h;
gps_ubx_ucenter.replies[5] = gps_ubx_ucenter.hw_ver_l;
DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,6,gps_ubx_ucenter.replies);
//////////////////////////////////
// Actual configuration start
// Use old baudrate to issue a baudrate change command
gps_ubx_ucenter_config_port();
break;
case 6:
// Now the GPS baudrate should have changed
GpsUartSetBaudrate(B38400);
gps_ubx_ucenter.baud_run = 38400;
gps_ubx_ucenter_config_nav();
break;
case 7:
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID,1);
break;
case 8:
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1);
break;
case 9:
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1);
break;
case 10:
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
break;
case 11:
#if defined FIRMWARE && FIRMWARE == ROTORCRAFT
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
#else
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
#endif
break;
case 12:
// Disable UTM on old Lea4P
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0);
break;
case 13:
gps_ubx_ucenter_config_sbas();
break;
case 14:
UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
break;
case 15:
// Try to save on non-ROM devices...
UbxSend_CFG_CFG(0x00000000,0xffffffff,0x00000000);
break;
case 16:
// Debug Downlink the result of all configuration steps: see messages
DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies);
return FALSE;
default:
break;
}
return TRUE; // Continue, except for the last case
}