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