Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
30 #include "gps_ubx_ucenter.h"
31 #include "subsystems/gps/gps_ubx.h"
33 #include <stdio.h>
34 
35 #if PRINT_DEBUG_GPS_UBX_UCENTER
36 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
37 #else
38 #define DEBUG_PRINT(...) {}
39 #endif
40 
43 //
44 // UCENTER: init, periodic and event
45 
46 static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
47 static bool_t gps_ubx_ucenter_configure(uint8_t nr);
48 
49 #define GPS_UBX_UCENTER_STATUS_STOPPED 0
50 #define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
51 #define GPS_UBX_UCENTER_STATUS_CONFIG 2
52 
53 #define GPS_UBX_UCENTER_REPLY_NONE 0
54 #define GPS_UBX_UCENTER_REPLY_ACK 1
55 #define GPS_UBX_UCENTER_REPLY_NACK 2
56 #define GPS_UBX_UCENTER_REPLY_VERSION 3
57 #define GPS_UBX_UCENTER_REPLY_CFG_PRT 4
58 
59 // All U-Center data
61 
62 
64 // Init Function
65 
67 {
68  // Start UCenter
71  gps_ubx_ucenter.cnt = 0;
72 
75 
80 
81  for (int i=0; i<GPS_UBX_UCENTER_CONFIG_STEPS; i++)
82  {
83  gps_ubx_ucenter.replies[i] = 0;
84  }
85 }
86 
87 
89 // Periodic Function
90 // -time-based configuration
91 
93 {
94  switch (gps_ubx_ucenter.status)
95  {
96  // Save processing time inflight
98  return;
99  // Automatically Determine Current Baudrate
102  {
104  gps_ubx_ucenter.cnt = 0;
105 #if PRINT_DEBUG_GPS_UBX_UCENTER
106  if (gps_ubx_ucenter.baud_init > 0) {
107  DEBUG_PRINT("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init);
108  }
109  else {
110  DEBUG_PRINT("WARNING: Unable to determine the ublox baudrate. Autoconfiguration is unlikely to work.\n");
111  }
112 #endif
113  }
114  else {
116  }
117  break;
118  // Send Configuration
122  gps_ubx_ucenter.cnt = 0;
123  }
124  else {
126  }
127  break;
128  default:
129  // stop this module now...
130  // todo
131  break;
132  }
133 }
134 
136 // Event Function
137 // -fetch replies: ACK and VERSION info
138 
140 {
141  // Save processing time inflight
143  return;
144 
145  // Read Configuration Reply's
146  switch (gps_ubx.msg_class)
147  {
148  case UBX_ACK_ID:
149  if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
151  DEBUG_PRINT("ACK\n");
152  }
153  else {
155  DEBUG_PRINT("NACK\n");
156  }
157  break;
158  case UBX_MON_ID:
159  if (gps_ubx.msg_id == UBX_MON_VER_ID ) {
161  gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,0) - '0';
162  gps_ubx_ucenter.sw_ver_l = 10*(UBX_MON_VER_c(gps_ubx.msg_buf,2) - '0');
163  gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf,3) - '0';
164  gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,33) - '0';
165  gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0');
166  gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0';
167  gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0');
168 
169  DEBUG_PRINT("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l);
170  DEBUG_PRINT("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l);
171  }
172  break;
173  case UBX_CFG_ID:
174  if (gps_ubx.msg_id == UBX_CFG_PRT_ID) {
176  gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf,0);
177  gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf,0);
178 
179  DEBUG_PRINT("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run);
180  DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id);
181  }
182  break;
183  default:
184  break;
185  }
186 }
187 
190 //
191 // UCENTER Configuration Functions
192 
199 static inline void gps_ubx_ucenter_config_port_poll(void)
200 {
201  UbxSend_CFG_PRT_POLL();
202 }
203 
216 static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
217 {
218  UbxSend_CFG_MSG(class, id, rate);
219 }
220 
230 {
231  switch (nr)
232  {
233  case 0:
234  case 1:
235  // Very important for some modules:
236  // Give the GPS some time to boot (up to 0.75 second)
237  break;
238  case 2:
240  GpsUartSetBaudrate(B38400); // Try the most common first?
242  break;
243  case 3:
246  return FALSE;
247  }
249  GpsUartSetBaudrate(B9600); // Maybe the factory default?
251  break;
252  case 4:
255  return FALSE;
256  }
258  GpsUartSetBaudrate(B57600); // The high-rate default?
260  break;
261  case 5:
264  return FALSE;
265  }
267  GpsUartSetBaudrate(B4800); // Default NMEA baudrate?
269  break;
270  case 6:
273  return FALSE;
274  }
276  GpsUartSetBaudrate(B115200); // Last possible option for ublox
278  break;
279  case 7:
282  return FALSE;
283  }
284 
285  // Autoconfig Failed... let's setup the failsafe baudrate
286  // Should we try even a different baudrate?
287  gps_ubx_ucenter.baud_init = 0; // Set as zero to indicate that we couldn't verify the baudrate
289  return FALSE;
290  default:
291  break;
292  }
293  return TRUE;
294 }
295 
297 // UBlox internal Navigation Solution
298 
299 #define NAV_DYN_STATIONARY 1
300 #define NAV_DYN_PEDESTRIAN 2
301 #define NAV_DYN_AUTOMOTIVE 3
302 #define NAV_DYN_SEA 4
303 #define NAV_DYN_AIRBORNE_1G 5
304 #define NAV_DYN_AIRBORNE_2G 6 // paparazzi default
305 #define NAV_DYN_AIRBORNE_4G 7
306 
307 #define NAV5_DYN_PORTABLE 0 // ublox default
308 #define NAV5_DYN_FIXED 1
309 #define NAV5_DYN_STATIONARY 2
310 #define NAV5_DYN_PEDESTRIAN 3
311 #define NAV5_DYN_AUTOMOTIVE 4
312 #define NAV5_DYN_SEA 5
313 #define NAV5_DYN_AIRBORNE_1G 6
314 #define NAV5_DYN_AIRBORNE_2G 7 // paparazzi default
315 #define NAV5_DYN_AIRBORNE_4G 8
316 
317 #ifndef GPS_UBX_NAV5_DYNAMICS
318 #define GPS_UBX_NAV5_DYNAMICS NAV5_DYN_AIRBORNE_2G
319 #endif
320 
321 #define NAV5_MASK 0x05 // Apply dynamic model and position fix mode settings
322 
323 #define NAV5_2D_ONLY 1
324 #define NAV5_3D_ONLY 2 // paparazzi default
325 #define NAV5_AUTO 3 // ublox default
326 
327 #define NAV5_DEFAULT_MIN_ELEV 5 // deg
328 #define NAV5_DEFAULT_PDOP_MASK 25 // no units
329 #define NAV5_DEFAULT_TDOP_MASK 25 // no units
330 #define NAV5_DEFAULT_P_ACC 100 // m
331 #define NAV5_DEFAULT_T_ACC 300 // m
332 #define NAV5_DEFAULT_STATIC_HOLD_THRES 0 // cm/s
333 
334 #define IGNORED 0
335 #define RESERVED 0
336 
337 static inline void gps_ubx_ucenter_config_nav(void)
338 {
339  //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
341  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);
342  }
343  else {
347  }
348 }
349 
350 
351 
353 // UBlox port and protocol GPS configuration
354 
355 #ifdef GPS_PORT_ID
356 #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."
357 #endif
358 
359 // UART mode: 8N1 with reserved1 set for compatability with A4
360 #define UBX_UART_MODE_MASK 0x000008D0
361 
362 #define UBX_PROTO_MASK 0x0001
363 #define NMEA_PROTO_MASK 0x0002
364 #define RTCM_PROTO_MASK 0x0004
365 
366 #define GPS_PORT_DDC 0x00
367 #define GPS_PORT_UART1 0x01
368 #define GPS_PORT_UART2 0x02
369 #define GPS_PORT_USB 0x03
370 #define GPS_PORT_SPI 0x04
371 #define GPS_PORT_RESERVED 0x05
372 
373 #define __UBX_GPS_BAUD(_u) _u##_BAUD
374 #define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
375 #define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
376 
377 #ifndef GPS_UBX_UCENTER_RATE
378 #define GPS_UBX_UCENTER_RATE 0x00FA // In milliseconds. 0x00FA = 250ms = 4Hz
379 #endif
380 
381 static inline void gps_ubx_ucenter_config_port(void)
382 {
383  switch(gps_ubx_ucenter.port_id)
384  {
385  // I2C Interface
386  case GPS_PORT_DDC:
387 #ifdef GPS_I2C
388  UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
389 #else
390  DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n");
391 #endif
392  break;
393  // UART Interface
394  case GPS_PORT_UART1:
395  case GPS_PORT_UART2:
397  break;
398  // USB Interface
399  case GPS_PORT_USB:
400  UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
401  break;
402  case GPS_PORT_SPI:
403  DEBUG_PRINT("WARNING: ublox SPI port is currently not supported.\n");
404  break;
405  default:
406  DEBUG_PRINT("WARNING: Unknown ublox port id: %u\n", gps_ubx_ucenter.port_id);
407  break;
408  }
409 }
410 
411 #define GPS_SBAS_ENABLED 0x01
412 
413 #define GPS_SBAS_RANGING 0x01
414 #define GPS_SBAS_CORRECTIONS 0x02
415 #define GPS_SBAS_INTEGRITY 0x04
416 
417 #define GPS_SBAS_MAX_SBAS 1 // Default ublox setting uses 3 SBAS channels(?)
418 
419 #define GPS_SBAS_AUTOSCAN 0x00
420 
421 static inline void gps_ubx_ucenter_config_sbas(void)
422 {
423  // Since March 2nd 2011 EGNOS is released for aviation purposes
425  //UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
426 }
427 
428 // Text Telemetry for Debugging
429 #undef GOT_PAYLOAD
430 
432 {
433  DEBUG_PRINT("gps_ubx_ucenter_configure nr: %u\n",nr);
434 
435  // Store the reply of the last configuration step and reset
438 
439  switch (nr) {
440  case 0:
441  // Use old baudrate to issue a baudrate change command
443  break;
444  case 1:
445 #if PRINT_DEBUG_GPS_UBX_UCENTER
447  DEBUG_PRINT("ublox did not acknowledge port configuration.\n");
448  }
449  else {
450  DEBUG_PRINT("Changed ublox baudrate to: %u\n", UART_SPEED(UBX_GPS_BAUD));
451  }
452 #endif
453  // Now the GPS baudrate should have changed
456  UbxSend_MON_GET_VER();
457  break;
458  case 2:
459  case 3:
460  case 4:
461  case 5:
462  // UBX_G5010 takes 0.7 seconds to answer a firmware request
463  // Version info is important for proper configuration as different firmwares have different settings
464  break;
465  case 6:
466  // Send some debugging info: detected baudrate, software version etc...
473 #if DEBUG_GPS_UBX_UCENTER
474  DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,6,gps_ubx_ucenter.replies);
475 #endif
476  // Configure CFG-NAV(5) message
478  break;
479  case 7:
480  // Geodetic Position Solution
481  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID,1);
482  break;
483  case 8:
484  // Velocity Solution in NED
485  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1);
486  break;
487  case 9:
488  // Receiver Navigation Status
489  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1);
490  break;
491  case 10:
492  // Space Vehicle Information
493  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
494  break;
495  case 11:
496  // Navigation Solution Information
497 #if GPS_UBX_UCENTER_SLOW_NAV_SOL
498  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
499 #else
500  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
501 #endif
502  break;
503  case 12:
504  // Disable UTM on old Lea4P
505  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0);
506  break;
507  case 13:
508  // SBAS Configuration
510  break;
511  case 14:
512  // Poll Navigation/Measurement Rate Settings
513  UbxSend_CFG_RATE(GPS_UBX_UCENTER_RATE, 0x0001, 0x0000);
514  break;
515  case 15:
516  // Raw Measurement Data
517 #if USE_GPS_UBX_RXM_RAW
518  gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_RAW_ID, 1);
519 #endif
520  break;
521  case 16:
522  // Subframe Buffer
523 #if USE_GPS_UBX_RXM_SFRB
524  gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_SFRB_ID, 1);
525 #endif
526  break;
527  case 17:
528  // Try to save on non-ROM devices...
529  UbxSend_CFG_CFG(0x00000000,0xffffffff,0x00000000);
530  break;
531  case 18:
532 #if DEBUG_GPS_UBX_UCENTER
533  // Debug Downlink the result of all configuration steps: see messages
534  // To view, enable DEBUG message in your telemetry configuration .xml
536  for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) {
537  DEBUG_PRINT("%u\n", gps_ubx_ucenter.replies[i]);
538  }
539 #endif
540  return FALSE;
541  default:
542  break;
543  }
544 
546  return TRUE; // Continue, except for the last case
547 }
#define NAV5_DEFAULT_MIN_ELEV
#define NAV5_DEFAULT_P_ACC
#define GPS_UBX_UCENTER_CONFIG_STEPS
U-Center Variables.
#define NAV5_DEFAULT_PDOP_MASK
#define GPS_UBX_UCENTER_STATUS_STOPPED
#define GPS_PORT_UART1
#define GPS_SBAS_RANGING
char replies[GPS_UBX_UCENTER_CONFIG_STEPS]
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD]
Definition: gps_ubx.h:44
#define GPS_UBX_UCENTER_REPLY_NONE
#define GPS_SBAS_AUTOSCAN
#define GPS_PORT_DDC
#define GPS_SBAS_CORRECTIONS
#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:58
#define UBX_GPS_BAUD
#define GPS_SBAS_ENABLED
#define B9600
Definition: uart_arch.h:40
static void gps_ubx_ucenter_config_sbas(void)
#define FALSE
Definition: imu_chimu.h:141
#define B115200
Definition: uart_arch.h:45
#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_PORT_SPI
void gps_ubx_ucenter_periodic(void)
uint8_t msg_class
Definition: gps_ubx.h:46
#define NAV5_DEFAULT_T_ACC
#define GPS_UBX_NAV5_DYNAMICS
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD
#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 B38400
Definition: uart_arch.h:42
#define GPS_UBX_UCENTER_REPLY_VERSION
#define GPS_UBX_UCENTER_REPLY_CFG_PRT
#define B57600
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
#define TRUE
Definition: imu_chimu.h:144
void gps_ubx_ucenter_init(void)
void gps_ubx_ucenter_event(void)
static bool_t gps_ubx_ucenter_configure(uint8_t nr)
unsigned char uint8_t
Definition: types.h:14
#define GPS_PORT_USB
#define GPS_SBAS_MAX_SBAS
#define DEBUG_PRINT(...)
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
Automatically determine the baudrate of the u-blox module.
#define NAV5_3D_ONLY
#define UBX_PROTO_MASK
#define UART_SPEED(_def)
Definition: uart_arch.h:47
uint8_t port_id
Port identifier number.
#define GPS_PORT_UART2
#define B4800
Definition: uart_arch.h:39
#define UBX_UART_MODE_MASK
#define GPS_I2C_SLAVE_ADDR
Default address for u-blox (and others?)
Definition: gps_i2c.h:29
static void gps_ubx_ucenter_config_nav(void)
uint8_t msg_id
Definition: gps_ubx.h:45
#define GpsUartSetBaudrate(_a)
Definition: gps_ubx.h:170
static void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
Enable u-blox message at desired period.