Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gps_ubx_ucenter.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  * Initial author: C. De Wagter
22  */
23 
31 #include "subsystems/gps/gps_ubx.h"
32 #include "ubx_protocol.h"
34 #include <stdio.h>
35 
36 #if PRINT_DEBUG_GPS_UBX_UCENTER
37 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
38 #else
39 #define DEBUG_PRINT(...) {}
40 #endif
41 
44 //
45 // UCENTER: init, periodic and event
46 #ifndef GPS_I2C
47 static bool gps_ubx_ucenter_autobaud(uint8_t nr);
48 #endif
49 static bool gps_ubx_ucenter_configure(uint8_t nr);
50 
51 #define GPS_UBX_UCENTER_STATUS_STOPPED 0
52 #define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
53 #define GPS_UBX_UCENTER_STATUS_CONFIG 2
54 #define GPS_UBX_UCENTER_STATUS_WAITING 3
55 
56 #define GPS_UBX_UCENTER_REPLY_NONE 0
57 #define GPS_UBX_UCENTER_REPLY_ACK 1
58 #define GPS_UBX_UCENTER_REPLY_NACK 2
59 #define GPS_UBX_UCENTER_REPLY_VERSION 3
60 #define GPS_UBX_UCENTER_REPLY_CFG_PRT 4
61 
62 // Target baudrate for the module
63 #define UBX_GPS_BAUD (UBX_GPS_LINK).baudrate
64 
65 // All U-Center data
67 
68 
70 // Init Function
71 
73 {
74  // Start UCenter
77  gps_ubx_ucenter.cnt = 0;
78 
82 
87 
88  for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) {
89  gps_ubx_ucenter.replies[i] = 0;
90  }
91 
92  gps_ubx_ucenter.dev = &(UBX_GPS_LINK).device;
93 }
94 
95 
97 // Periodic Function
98 // -time-based configuration
99 
101 {
102  switch (gps_ubx_ucenter.status) {
103  // Save processing time inflight
105  return;
106  break;
107  // Automatically Determine Current Baudrate
109 #ifdef GPS_I2C
110  gps_ubx_ucenter.cnt = 0;
112 #else
115  gps_ubx_ucenter.cnt = 0;
116 #if PRINT_DEBUG_GPS_UBX_UCENTER
117  if (gps_ubx_ucenter.baud_init > 0) {
118  DEBUG_PRINT("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init);
119  } else {
120  DEBUG_PRINT("WARNING: Unable to determine the ublox baudrate. Autoconfiguration is unlikely to work.\n");
121  }
122 #endif
123  } else {
125  }
126 #endif /* GPS_I2C */
127  break;
128  // Send Configuration
132 #ifdef GPS_I2C
133  gps_i2c_begin();
134 #endif
135  gps_ubx_ucenter.cnt = 0;
136  } else {
138 #ifdef GPS_I2C
140  }
141  break;
143  if (gps_i2c_tx_is_ready())
144  {
146 #endif /*GPS_I2C*/
147  }
148  break;
149  default:
150  // stop this module now...
151  // todo
152  break;
153  }
154 }
155 
157 // Event Function
158 // -fetch replies: ACK and VERSION info
159 
161 {
162  // Save processing time inflight
164  return;
165  }
166 
167  // Read Configuration Reply's
168  switch (gps_ubx.msg_class) {
169  case UBX_ACK_ID:
170  if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
172  DEBUG_PRINT("ACK\n");
173  } else if (gps_ubx.msg_id == UBX_ACK_NAK_ID) {
175  DEBUG_PRINT("NACK\n");
176  }
177  break;
178  case UBX_MON_ID:
179  if (gps_ubx.msg_id == UBX_MON_VER_ID) {
181  gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf, 0) - '0';
182  gps_ubx_ucenter.sw_ver_l = 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 2) - '0');
183  gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf, 3) - '0';
184  gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf, 33) - '0';
185  gps_ubx_ucenter.hw_ver_h += 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 32) - '0');
186  gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf, 37) - '0';
187  gps_ubx_ucenter.hw_ver_l += 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 36) - '0');
188 
189  DEBUG_PRINT("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l);
190  DEBUG_PRINT("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l);
191  }
192  break;
193  case UBX_CFG_ID:
194  if (gps_ubx.msg_id == UBX_CFG_PRT_ID) {
196  gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf, 0);
197  gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf, 0);
198 
199  DEBUG_PRINT("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run);
200  DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id);
201  }
202  break;
203  default:
204  break;
205  }
206 }
207 
210 //
211 // UCENTER Configuration Functions
212 
219 static inline void gps_ubx_ucenter_config_port_poll(void)
220 {
221  UbxSend_CFG_PRT_POLL(gps_ubx_ucenter.dev);
222 }
223 
236 static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
237 {
238  UbxSend_CFG_MSG(gps_ubx_ucenter.dev, class, id, rate);
239 }
240 
249 #ifndef GPS_I2C
251 {
252  switch (nr) {
253  case 0:
254  case 1:
255  // Very important for some modules:
256  // Give the GPS some time to boot (up to 0.75 second)
257  break;
258  case 2:
260  uart_periph_set_baudrate(&(UBX_GPS_LINK), B38400); // Try the most common first?
262  break;
263  case 3:
266  return false;
267  }
269  uart_periph_set_baudrate(&(UBX_GPS_LINK), B9600); // Maybe the factory default?
271  break;
272  case 4:
275  return false;
276  }
278  uart_periph_set_baudrate(&(UBX_GPS_LINK), B57600); // The high-rate default?
280  break;
281  case 5:
284  return false;
285  }
287  uart_periph_set_baudrate(&(UBX_GPS_LINK), B4800); // Default NMEA baudrate?
289  break;
290  case 6:
293  return false;
294  }
296  uart_periph_set_baudrate(&(UBX_GPS_LINK), B115200); // Last possible option for ublox
298  break;
299  case 7:
302  return false;
303  }
305  uart_periph_set_baudrate(&(UBX_GPS_LINK), B230400); // Last possible option for ublox
307  break;
308  case 8:
311  return false;
312  }
313 
314  // Autoconfig Failed... let's setup the failsafe baudrate
315  // Should we try even a different baudrate?
316  gps_ubx_ucenter.baud_init = 0; // Set as zero to indicate that we couldn't verify the baudrate
317  uart_periph_set_baudrate(&(UBX_GPS_LINK), B9600);
318  return false;
319  default:
320  break;
321  }
322  return true;
323 }
324 #endif /* GPS_I2C */
325 // UBlox internal Navigation Solution
327 
328 #define NAV_DYN_STATIONARY 1
329 #define NAV_DYN_PEDESTRIAN 2
330 #define NAV_DYN_AUTOMOTIVE 3
331 #define NAV_DYN_SEA 4
332 #define NAV_DYN_AIRBORNE_1G 5
333 #define NAV_DYN_AIRBORNE_2G 6 // paparazzi default
334 #define NAV_DYN_AIRBORNE_4G 7
335 
336 #define NAV5_DYN_PORTABLE 0 // ublox default
337 #define NAV5_DYN_FIXED 1
338 #define NAV5_DYN_STATIONARY 2
339 #define NAV5_DYN_PEDESTRIAN 3
340 #define NAV5_DYN_AUTOMOTIVE 4
341 #define NAV5_DYN_SEA 5
342 #define NAV5_DYN_AIRBORNE_1G 6
343 #define NAV5_DYN_AIRBORNE_2G 7 // paparazzi default
344 #define NAV5_DYN_AIRBORNE_4G 8
345 
346 #ifndef GPS_UBX_NAV5_DYNAMICS
347 #define GPS_UBX_NAV5_DYNAMICS NAV5_DYN_AIRBORNE_2G
348 #endif
349 
350 #define NAV5_MASK 0x05 // Apply dynamic model and position fix mode settings
351 
352 #define NAV5_2D_ONLY 1
353 #define NAV5_3D_ONLY 2 // paparazzi default
354 #define NAV5_AUTO 3 // ublox default
355 
356 #define NAV5_DEFAULT_MIN_ELEV 5 // deg
357 #define NAV5_DEFAULT_PDOP_MASK 25 // no units
358 #define NAV5_DEFAULT_TDOP_MASK 25 // no units
359 #define NAV5_DEFAULT_P_ACC 100 // m
360 #define NAV5_DEFAULT_T_ACC 300 // m
361 #define NAV5_DEFAULT_STATIC_HOLD_THRES 0 // cm/s
362 
363 #define IGNORED 0
364 #define RESERVED 0
365 
366 static inline void gps_ubx_ucenter_config_nav(void)
367 {
368  // New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
369  // If version message couldn't be fetched, default to NAV5
372  UbxSend_CFG_NAV(gps_ubx_ucenter.dev,
373  NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C,
374  0x3C, 0x14, 0x03E8 , 0x0000, 0x0, 0x17, 0x00FA, 0x00FA,
375  0x0064, 0x012C, 0x000F, 0x00, 0x00);
376  } else {
377  UbxSend_CFG_NAV5(gps_ubx_ucenter.dev,
381  }
382 }
383 
384 
385 
387 // UBlox port and protocol GPS configuration
388 
389 #ifdef GPS_PORT_ID
390 #warning "GPS_PORT_ID is no longer needed by the ublox ucenter for automatically configuration. Please remove this defined variable and double check that autoconfig is working as expected."
391 #endif
392 
393 // UART mode: 8N1 with reserved1 set for compatability with A4
394 #define UBX_UART_MODE_MASK 0x000008D0
395 
396 #define UBX_PROTO_MASK 0x0001
397 #define NMEA_PROTO_MASK 0x0002
398 #define RTCM_PROTO_MASK 0x0004
399 
400 #define GPS_PORT_DDC 0x00
401 #define GPS_PORT_UART1 0x01
402 #define GPS_PORT_UART2 0x02
403 #define GPS_PORT_USB 0x03
404 #define GPS_PORT_SPI 0x04
405 #define GPS_PORT_RESERVED 0x05
406 
407 #ifndef GPS_UBX_UCENTER_RATE
408 #define GPS_UBX_UCENTER_RATE 0x00FA // In milliseconds. 0x00FA = 250ms = 4Hz
409 #endif
410 
411 static inline void gps_ubx_ucenter_config_port(void)
412 {
413  switch (gps_ubx_ucenter.port_id) {
414  // I2C Interface
415  case GPS_PORT_DDC:
416 #ifdef GPS_I2C
417  UbxSend_CFG_PRT(gps_ubx_ucenter.dev, gps_ubx_ucenter.port_id, 0x0, 0x0, (0x42<<1), 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
418 #else
419  DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n");
420 #endif
421  break;
422  // UART Interface
423  case GPS_PORT_UART1:
424  case GPS_PORT_UART2:
425  UbxSend_CFG_PRT(gps_ubx_ucenter.dev,
426  gps_ubx_ucenter.port_id, 0x0, 0x0,
428  UBX_PROTO_MASK| NMEA_PROTO_MASK, 0x0, 0x0);
429  break;
430  // USB Interface
431  case GPS_PORT_USB:
432  UbxSend_CFG_PRT(gps_ubx_ucenter.dev,
433  gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0,
435  break;
436  case GPS_PORT_SPI:
437  DEBUG_PRINT("WARNING: ublox SPI port is currently not supported.\n");
438  break;
439  default:
440  DEBUG_PRINT("WARNING: Unknown ublox port id: %u\n", gps_ubx_ucenter.port_id);
441  break;
442  }
443 }
444 
445 #define GPS_SBAS_ENABLED 0x01
446 
447 #define GPS_SBAS_RANGING 0x01
448 #define GPS_SBAS_CORRECTIONS 0x02
449 #define GPS_SBAS_INTEGRITY 0x04
450 
451 #define GPS_SBAS_MAX_SBAS 1 // Default ublox setting uses 3 SBAS channels(?)
452 
453 #define GPS_SBAS_AUTOSCAN 0x00
454 
455 static inline void gps_ubx_ucenter_config_sbas(void)
456 {
457  // Since March 2nd 2011 EGNOS is released for aviation purposes
460  //UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
461 }
462 
463 // Text Telemetry for Debugging
464 #undef GOT_PAYLOAD
465 
467 {
468  DEBUG_PRINT("gps_ubx_ucenter_configure nr: %u\n", nr);
469 
470  // Store the reply of the last configuration step and reset
471  if (nr < GPS_UBX_UCENTER_CONFIG_STEPS) {
473  }
474 
475  switch (nr) {
476  case 0:
477  // Use old baudrate to issue a baudrate change command
479  break;
480  case 1:
481 #ifndef GPS_I2C
482 #if PRINT_DEBUG_GPS_UBX_UCENTER
484  DEBUG_PRINT("ublox did not acknowledge port configuration.\n");
485  } else {
486  DEBUG_PRINT("Changed ublox baudrate to: %u\n", UART_SPEED(gps_ubx_ucenter.baud_target));
487  }
488 #endif
489  // Now the GPS baudrate should have changed
492 #endif /*GPS_I2C*/
493  UbxSend_MON_GET_VER(gps_ubx_ucenter.dev);
494  break;
495  case 2:
496  case 3:
497  case 4:
498  case 5:
499  // UBX_G5010 takes 0.7 seconds to answer a firmware request
500  // Version info is important for proper configuration as different firmwares have different settings
501  break;
502  case 6:
503  // Send some debugging info: detected baudrate, software version etc...
510 #if DEBUG_GPS_UBX_UCENTER
511  DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 6, gps_ubx_ucenter.replies);
512 #endif
513  // Configure CFG-NAV(5) message
515  break;
516  case 7:
517  // Geodetic Position Solution
518  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID, 1);
519  break;
520  case 8:
521  // Velocity Solution in NED
522  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1);
523  break;
524  case 9:
525  // Receiver Navigation Status
526  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1);
527  break;
528  case 10:
529  // Space Vehicle Information
530  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
531  break;
532  case 11:
533  // Navigation Solution Information
534 #if GPS_UBX_UCENTER_SLOW_NAV_SOL
535  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
536 #else
537  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
538 #endif
539  break;
540  case 12:
541  // Disable UTM on old Lea4P
542  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0);
543  break;
544  case 13:
545  // SBAS Configuration
547  break;
548  case 14:
549  // Poll Navigation/Measurement Rate Settings
550  UbxSend_CFG_RATE(gps_ubx_ucenter.dev, GPS_UBX_UCENTER_RATE, 0x0001, 0x0000);
551  break;
552  case 15:
553  // Raw Measurement Data
554 #if USE_GPS_UBX_RXM_RAW
555  gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_RAW_ID, 1);
556 #endif
557  break;
558  case 16:
559  // Subframe Buffer
560 #if USE_GPS_UBX_RXM_SFRB
561  gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_SFRB_ID, 1);
562 #endif
563  break;
564  case 17:
565  // Try to save on non-ROM devices...
566  UbxSend_CFG_CFG(gps_ubx_ucenter.dev, 0x00000000, 0xffffffff, 0x00000000);
567  break;
568  case 18:
569 #if DEBUG_GPS_UBX_UCENTER
570  // Debug Downlink the result of all configuration steps: see messages
571  // To view, enable DEBUG message in your telemetry configuration .xml
573  for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) {
574  DEBUG_PRINT("%u\n", gps_ubx_ucenter.replies[i]);
575  }
576 #endif
577  return false;
578  default:
579  break;
580  }
581 
583  return true; // Continue, except for the last case
584 }
#define NAV5_DEFAULT_MIN_ELEV
#define NAV5_DEFAULT_P_ACC
#define B38400
Definition: uart_arch.h:45
void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud)
Set baudrate.
Definition: uart_arch.c:838
static bool gps_ubx_ucenter_configure(uint8_t nr)
#define GPS_UBX_UCENTER_CONFIG_STEPS
U-Center Variables.
#define NMEA_PROTO_MASK
#define NAV5_DEFAULT_PDOP_MASK
#define GPS_UBX_UCENTER_STATUS_STOPPED
#define GPS_PORT_UART1
#define GPS_SBAS_RANGING
#define GPS_UBX_UCENTER_REPLY_NONE
#define GPS_SBAS_AUTOSCAN
#define GPS_PORT_DDC
#define GPS_SBAS_CORRECTIONS
#define B115200
Definition: uart_arch.h:48
#define NAV5_DEFAULT_STATIC_HOLD_THRES
UBX protocol specific code.
#define IGNORED
#define GPS_UBX_UCENTER_RATE
struct GpsUbx gps_ubx
Definition: gps_ubx.c:52
#define UBX_GPS_BAUD
char replies[GPS_UBX_UCENTER_CONFIG_STEPS]
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD]
Definition: gps_ubx.h:56
#define GPS_SBAS_ENABLED
uint8_t msg_id
Definition: gps_ubx.h:57
static void gps_ubx_ucenter_config_sbas(void)
#define FALSE
Definition: std.h:5
#define B230400
Definition: uart_arch.h:49
#define GPS_UBX_UCENTER_REPLY_NACK
static void gps_ubx_ucenter_config_port(void)
Configure Ublox GPS.
struct gps_ubx_ucenter_struct gps_ubx_ucenter
#define GPS_UBX_UCENTER_STATUS_WAITING
#define GPS_PORT_SPI
void gps_ubx_ucenter_periodic(void)
#define NAV5_DEFAULT_T_ACC
#define GPS_UBX_NAV5_DYNAMICS
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD
#define B4800
Definition: uart_arch.h:42
#define NAV5_DEFAULT_TDOP_MASK
#define GPS_UBX_UCENTER_REPLY_ACK
#define GPS_UBX_UCENTER_STATUS_CONFIG
#define GPS_SBAS_INTEGRITY
#define NAV_DYN_AIRBORNE_2G
#define GPS_UBX_UCENTER_REPLY_VERSION
#define GPS_UBX_UCENTER_REPLY_CFG_PRT
#define B9600
Definition: uart_arch.h:43
#define RESERVED
static void gps_ubx_ucenter_config_port_poll(void)
Polls the u-blox port configuration When the payload is omitted (zero length), the configuration for ...
#define NAV5_MASK
void gps_ubx_ucenter_init(void)
void gps_ubx_ucenter_event(void)
unsigned char uint8_t
Definition: types.h:14
#define UART_SPEED(_def)
Definition: uart_arch.h:53
struct link_device * dev
#define GPS_PORT_USB
#define GPS_SBAS_MAX_SBAS
uint8_t port_id
Port identifier number.
bool gps_i2c_tx_is_ready(void)
is driver ready to send a message
Definition: gps_ubx_i2c.c:154
static bool gps_ubx_ucenter_autobaud(uint8_t nr)
Automatically determine the baudrate of the u-blox module.
#define DEBUG_PRINT(...)
#define NAV5_3D_ONLY
#define UBX_PROTO_MASK
#define GPS_PORT_UART2
#define B57600
Definition: uart_arch.h:46
void gps_i2c_begin(void)
config is done, begin reading messages
Definition: gps_ubx_i2c.c:159
#define UBX_UART_MODE_MASK
uint8_t msg_class
Definition: gps_ubx.h:58
static void gps_ubx_ucenter_config_nav(void)
static void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
Enable u-blox message at desired period.