Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 
31 #include "gps_ubx_ucenter.h"
32 
35 //
36 // UCENTER: init, periodic and event
37 
38 static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
39 static bool_t gps_ubx_ucenter_configure(uint8_t nr);
40 
41 #define GPS_UBX_UCENTER_STATUS_STOPPED 0
42 #define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
43 #define GPS_UBX_UCENTER_STATUS_CONFIG 2
44 
45 #define GPS_UBX_UCENTER_REPLY_NONE 0
46 #define GPS_UBX_UCENTER_REPLY_ACK 1
47 #define GPS_UBX_UCENTER_REPLY_NACK 2
48 #define GPS_UBX_UCENTER_REPLY_VERSION 3
49 
50 // All U-Center data
52 
53 
55 // Init Function
56 
58 {
59  // Start UCenter
62  gps_ubx_ucenter.cnt = 0;
63 
66 
71 
72  for (int i=0; i<GPS_UBX_UCENTER_CONFIG_STEPS; i++)
73  {
74  gps_ubx_ucenter.replies[i] = 0;
75  }
76 }
77 
78 
80 // Periodic Function
81 // -time-based configuration
82 
84 {
85  switch (gps_ubx_ucenter.status)
86  {
87  // Save processing time inflight
89  return;
90  // Automatically Determine Current Baudrate
93  {
95  gps_ubx_ucenter.cnt = 0;
96  }
97  else
98  {
100  }
101  break;
102  // Send Configuration
105  {
107  gps_ubx_ucenter.cnt = 0;
108  }
109  else
110  {
112  }
113  break;
114  default:
115  // stop this module now...
116  // todo
117  break;
118  }
119 }
120 
122 // Event Function
123 // -fetch replies: ACK and VERSION info
124 
126 {
127  // Save processing time inflight
129  return;
130 
131  // Read Configuration Reply's
132  if (gps_ubx.msg_class == UBX_ACK_ID) {
133  if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
135  }
136  else
137  {
139  }
140  }
141  // Version info
142  else if (gps_ubx.msg_class == UBX_MON_ID) {
143  if (gps_ubx.msg_id == UBX_MON_VER_ID ) {
145  gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,0) - '0';
146  gps_ubx_ucenter.sw_ver_l = 10*(UBX_MON_VER_c(gps_ubx.msg_buf,2) - '0');
147  gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf,3) - '0';
148  gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,33) - '0';
149  gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0');
150  gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0';
151  gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0');
152  }
153  }
154 }
155 
158 //
159 // UCENTER Configuration Functions
160 
161 
163 {
164  switch (nr)
165  {
166  case 0:
167  case 1:
168  // Very important for some modules:
169  // Give the GPS some time to boot (up to 0.75 second)
170  break;
171  case 2:
173  GpsUartSetBaudrate(B38400); // Try the most common first?
174  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
175  break;
176  case 3:
178  {
179  gps_ubx_ucenter.baud_init = 38400;
180  return FALSE;
181  }
183  GpsUartSetBaudrate(B9600); // Maybe the factory default?
184  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
185  break;
186  case 4:
188  {
189  gps_ubx_ucenter.baud_init = 9600;
190  return FALSE;
191  }
193  GpsUartSetBaudrate(B57600); // The high-rate default?
194  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
195  break;
196  case 5:
198  {
199  gps_ubx_ucenter.baud_init = 57600;
200  return FALSE;
201  }
203  GpsUartSetBaudrate(B4800); // Default NMEA baudrate finally?
204  UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
205  break;
206  case 6:
208  {
209  gps_ubx_ucenter.baud_init = 4800;
210  return FALSE;
211  }
212 
213  // Autoconfig Failed... let's setup the failsafe baudrate
214  // Should we try even a different baudrate?
217  return FALSE;
218  default:
219  break;
220  }
221  return TRUE;
222 }
223 
225 // UBlox internal Navigation Solution
226 
227 #define NAV_DYN_STATIONARY 1
228 #define NAV_DYN_PEDESTRIAN 2
229 #define NAV_DYN_AUTOMOTIVE 3
230 #define NAV_DYN_SEA 4
231 #define NAV_DYN_AIRBORNE_1G 5
232 #define NAV_DYN_AIRBORNE_2G 6
233 #define NAV_DYN_AIRBORNE_4G 7
234 
235 #define NAV5_DYN_PORTABLE 0
236 #define NAV5_DYN_FIXED 1
237 #define NAV5_DYN_STATIONARY 2
238 #define NAV5_DYN_PEDESTRIAN 3
239 #define NAV5_DYN_AUTOMOTIVE 4
240 #define NAV5_DYN_SEA 5
241 #define NAV5_DYN_AIRBORNE_1G 6
242 #define NAV5_DYN_AIRBORNE_2G 7
243 #define NAV5_DYN_AIRBORNE_4G 8
244 
245 #define NAV5_2D_ONLY 1
246 #define NAV5_3D_ONLY 2
247 #define NAV5_AUTO 3
248 
249 #define IGNORED 0
250 #define RESERVED 0
251 
252 static inline void gps_ubx_ucenter_config_nav(void)
253 {
254  //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
255  if (gps_ubx_ucenter.sw_ver_h < 5)
256  {
257  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);
258  }
259  else
260  {
262  }
263 }
264 
265 
266 
268 // UBlox port and protocol GPS configuration
269 
270 #define UBX_PROTO_MASK 0x0001
271 #define NMEA_PROTO_MASK 0x0002
272 #define RTCM_PROTO_MASK 0x0004
273 
274 #define GPS_PORT_DDC 0x00
275 #define GPS_PORT_UART1 0x01
276 #define GPS_PORT_UART2 0x02
277 #define GPS_PORT_USB 0x03
278 #define GPS_PORT_SPI 0x04
279 
280 #ifndef GPS_PORT_ID
281 #define GPS_PORT_ID GPS_PORT_UART1
282 #endif
283 
284 #define __UBX_GPS_BAUD(_u) _u##_BAUD
285 #define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
286 #define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
287 
288 static inline void gps_ubx_ucenter_config_port(void)
289 {
290  // I2C Interface
291  #if GPS_PORT_ID == GPS_PORT_DDC
292  UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
293  #endif
294  // UART Interface
295  #if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
296  UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
297  #endif
298 }
299 
300 #define GPS_SBAS_ENABLED 0x01
301 
302 #define GPS_SBAS_RANGING 0x01
303 #define GPS_SBAS_CORRECTIONS 0x02
304 #define GPS_SBAS_INTEGRITY 0x04
305 
306 static inline void gps_ubx_ucenter_config_sbas(void)
307 {
308  // Since March 2nd 2011 EGNOS is released for aviation purposes
309  const uint8_t nrofsat = 1;
310  UbxSend_CFG_SBAS(GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, nrofsat, 0x00, 0x00);
311  //UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
312 }
313 
314 static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)
315 {
316  #if GPS_PORT_ID == GPS_PORT_UART1
317  UbxSend_CFG_MSG(class, id, 0, rate, 0, 0);
318  #endif
319  #if GPS_PORT_ID == GPS_PORT_UART2
320  UbxSend_CFG_MSG(class, id, 0, 0, rate, 0);
321  #endif
322  #if GPS_PORT_ID == GPS_PORT_DDC
323  UbxSend_CFG_MSG(class, id, rate, 0, 0, 0);
324  #endif
325 }
326 
327 
328 // Text Telemetry for Debugging
329 #ifndef DOWNLINK_DEVICE
330 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
331 #endif
332 #undef GOT_PAYLOAD
334 
336 {
337  // Store the reply of the last configuration step and reset
340 
342 
343  switch (nr) {
344  case 0:
345  UbxSend_MON_GET_VER();
346  break;
347  case 1:
348  case 2:
349  case 3:
350  case 4:
351  // UBX_G5010 takes 0.7 seconds to answer a firmware request
352  // Version info is important for porper configuration as different firmwares have different settings
353  break;
354  case 5:
355  // Send some debugging info: detected baudrate, software version etc...
362 #if DEBUG_GPS_UBX_UCENTER
363  DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,6,gps_ubx_ucenter.replies);
364 #endif
365 
367  // Actual configuration start
368 
369  // Use old baudrate to issue a baudrate change command
371  break;
372  case 6:
373  // Now the GPS baudrate should have changed
375  gps_ubx_ucenter.baud_run = 38400;
377  break;
378  case 7:
379  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID,1);
380  break;
381  case 8:
382  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1);
383  break;
384  case 9:
385  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1);
386  break;
387  case 10:
388  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
389  break;
390  case 11:
391 #if defined FIRMWARE && FIRMWARE == ROTORCRAFT
392  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
393 #else
394  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
395 #endif
396  break;
397  case 12:
398  // Disable UTM on old Lea4P
399  gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0);
400  break;
401  case 13:
403  break;
404  case 14:
405  UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
406  break;
407  case 15:
408  // Try to save on non-ROM devices...
409  UbxSend_CFG_CFG(0x00000000,0xffffffff,0x00000000);
410  break;
411  case 16:
412 #if DEBUG_GPS_UBX_UCENTER
413  // Debug Downlink the result of all configuration steps: see messages
415 #endif
416  return FALSE;
417  default:
418  break;
419  }
420  return TRUE; // Continue, except for the last case
421 }
422 
423 
424 
425 
#define B9600
Definition: uart.h:45
#define GPS_UBX_UCENTER_CONFIG_STEPS
U-Center Variables.
#define GPS_UBX_UCENTER_STATUS_STOPPED
#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_CORRECTIONS
#define IGNORED
#define GpsUartSetBaudrate(_a)
Definition: gps_ubx.c:57
struct GpsUbx gps_ubx
Definition: gps_ubx.c:81
#define B57600
Definition: uart.h:48
#define GPS_PORT_ID
#define GPS_SBAS_ENABLED
#define NAV5_DYN_AIRBORNE_2G
static void gps_ubx_ucenter_config_sbas(void)
#define FALSE
Definition: imu_chimu.h:141
#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
void gps_ubx_ucenter_periodic(void)
uint8_t msg_class
Definition: gps_ubx.h:46
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD
#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 RESERVED
#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
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
#define NAV5_3D_ONLY
#define UBX_PROTO_MASK
#define B38400
Definition: uart.h:47
#define GPS_I2C_SLAVE_ADDR
Definition: gps_i2c.h:28
static void gps_ubx_ucenter_config_nav(void)
uint8_t msg_id
Definition: gps_ubx.h:45
#define B4800
Definition: uart.h:44
static void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate)