Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
gps_ubx.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
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 
22 
23 #include "subsystems/gps.h"
24 
25 #include "led.h"
26 
27 #if GPS_USE_LATLONG
28 /* currently needed to get nav_utm_zone0 */
31 #endif
32 
33 /* parser status */
34 #define UNINIT 0
35 #define GOT_SYNC1 1
36 #define GOT_SYNC2 2
37 #define GOT_CLASS 3
38 #define GOT_ID 4
39 #define GOT_LEN1 5
40 #define GOT_LEN2 6
41 #define GOT_PAYLOAD 7
42 #define GOT_CHECKSUM1 8
43 
44 /* last error type */
45 #define GPS_UBX_ERR_NONE 0
46 #define GPS_UBX_ERR_OVERRUN 1
47 #define GPS_UBX_ERR_MSG_TOO_LONG 2
48 #define GPS_UBX_ERR_CHECKSUM 3
49 #define GPS_UBX_ERR_UNEXPECTED 4
50 #define GPS_UBX_ERR_OUT_OF_SYNC 5
51 
52 #define UTM_HEM_NORTH 0
53 #define UTM_HEM_SOUTH 1
54 
55 
56 #define GpsUartSend1(c) GpsLink(Transmit(c))
57 #define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a))
58 #define GpsUartRunning GpsLink(TxRunning)
59 #define GpsUartSendMessage GpsLink(SendMessage)
60 
61 #define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; }
62 #define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; }
63 #define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); }
64 
65 #define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); }
66 #define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); }
67 #define UbxSend1ByAddr(x) { UbxSend1(*x); }
68 #define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); }
69 #define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); }
70 
71 #define UbxHeader(nav_id, msg_id, len) { \
72  GpsUartSend1(UBX_SYNC1); \
73  GpsUartSend1(UBX_SYNC2); \
74  UbxInitCheksum(); \
75  UbxSend1(nav_id); \
76  UbxSend1(msg_id); \
77  UbxSend2(len); \
78  }
79 
80 
81 struct GpsUbx gps_ubx;
82 
83 void gps_impl_init(void) {
86  gps_ubx.error_cnt = 0;
88  gps_ubx.have_velned = 0;
89 }
90 
91 
93 
94  if (gps_ubx.msg_class == UBX_NAV_ID) {
95  if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
96 #ifdef GPS_TIMESTAMP
97  /* get hardware clock ticks */
99  gps.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
100  gps.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
101 #endif
102  gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
103  gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
104  gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
105  gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
106  gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
107  gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
108  gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
109  gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
110  gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
111  gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
112  gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
113  gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
114  gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
115 #ifdef GPS_LED
116  if (gps.fix == GPS_FIX_3D) {
117  LED_ON(GPS_LED);
118  }
119  else {
120  LED_TOGGLE(GPS_LED);
121  }
122 #endif
123  } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
124  gps.lla_pos.lat = RadOfDeg(UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf));
125  gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf));
126  gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
127  gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
128 #if GPS_USE_LATLONG
129  /* Computes from (lat, long) in the referenced UTM zone */
130  struct LlaCoor_f lla_f;
131  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
132  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
133  struct UtmCoor_f utm_f;
134  utm_f.zone = nav_utm_zone0;
135  /* convert to utm */
136  utm_of_lla_f(&utm_f, &lla_f);
137  /* copy results of utm conversion */
138  gps.utm_pos.east = utm_f.east*100;
139  gps.utm_pos.north = utm_f.north*100;
142 #else
143  }
144  else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) {
145  gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
146  gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
147  uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf);
148  if (hem == UTM_HEM_SOUTH)
149  gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
150  gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10;
151  gps.hmsl = gps.utm_pos.alt;
152  gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
153  gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
154 #endif
155  }
156  else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) {
157  gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
158  gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
159  gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
160  gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
161  gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
162  // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
163  // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
164  // solution: First to radians, and then scale to 1e-7 radians
165  // First x 10 for loosing less resolution, then to radians, then multiply x 10 again
166  gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10;
167  gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf)*10)) * 10;
168  gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
169  gps_ubx.have_velned = 1;
170  }
171  else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
172  gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
173  uint8_t i;
174  for(i = 0; i < gps.nb_channels; i++) {
175  gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i);
176  gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i);
177  gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i);
178  gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i);
179  gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i);
180  gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
181  }
182  }
183  else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
184  gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
185  gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
186  gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
187  }
188  }
189 }
190 
191 
192 /* UBX parsing */
194  if (gps_ubx.status < GOT_PAYLOAD) {
195  gps_ubx.ck_a += c;
197  }
198  switch (gps_ubx.status) {
199  case UNINIT:
200  if (c == UBX_SYNC1)
201  gps_ubx.status++;
202  break;
203  case GOT_SYNC1:
204  if (c != UBX_SYNC2) {
206  goto error;
207  }
208  gps_ubx.ck_a = 0;
209  gps_ubx.ck_b = 0;
210  gps_ubx.status++;
211  break;
212  case GOT_SYNC2:
213  if (gps_ubx.msg_available) {
214  /* Previous message has not yet been parsed: discard this one */
216  goto error;
217  }
218  gps_ubx.msg_class = c;
219  gps_ubx.status++;
220  break;
221  case GOT_CLASS:
222  gps_ubx.msg_id = c;
223  gps_ubx.status++;
224  break;
225  case GOT_ID:
226  gps_ubx.len = c;
227  gps_ubx.status++;
228  break;
229  case GOT_LEN1:
230  gps_ubx.len |= (c<<8);
233  goto error;
234  }
235  gps_ubx.msg_idx = 0;
236  gps_ubx.status++;
237  break;
238  case GOT_LEN2:
239  gps_ubx.msg_buf[gps_ubx.msg_idx] = c;
240  gps_ubx.msg_idx++;
241  if (gps_ubx.msg_idx >= gps_ubx.len) {
242  gps_ubx.status++;
243  }
244  break;
245  case GOT_PAYLOAD:
246  if (c != gps_ubx.ck_a) {
248  goto error;
249  }
250  gps_ubx.status++;
251  break;
252  case GOT_CHECKSUM1:
253  if (c != gps_ubx.ck_b) {
255  goto error;
256  }
258  goto restart;
259  break;
260  default:
262  goto error;
263  }
264  return;
265  error:
266  gps_ubx.error_cnt++;
267  restart:
269  return;
270 }
271 
272 #ifdef GPS_UBX_UCENTER
273 #include GPS_UBX_UCENTER
274 #endif
275 
276 
277 void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
278 #ifdef GPS_LINK
279  UbxSend_CFG_RST(bbr, reset_mode, 0x00);
280 #endif /* else less harmful for HITL */
281 }
282 
283 
unsigned short uint16_t
Definition: types.h:16
#define GOT_CLASS
Definition: gps_ubx.c:37
#define UTM_HEM_SOUTH
Definition: gps_ubx.c:53
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
int32_t lat
in radians*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:67
#define GOT_CHECKSUM1
Definition: gps_ubx.c:42
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
#define Min(x, y)
#define GOT_ID
Definition: gps_ubx.c:38
Definition: gps_ubx.h:42
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:73
uint8_t zone
UTM zone number.
void gps_ubx_parse(uint8_t c)
Definition: gps_ubx.c:193
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:74
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:75
bool_t msg_available
Definition: gps_ubx.h:43
uint8_t nav_utm_zone0
Definition: common_nav.c:43
#define LED_ON(i)
Definition: led_hw.h:28
uint8_t ck_a
Definition: gps_ubx.h:51
struct GpsUbx gps_ubx
Definition: gps_ubx.c:81
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
float lat
in radians
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:63
int16_t week
GPS week.
Definition: gps.h:78
#define GPS_UBX_ERR_OUT_OF_SYNC
Definition: gps_ubx.c:50
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:55
vector in Latitude, Longitude and Altitude
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define GPS_UBX_MAX_PAYLOAD
Definition: gps_ubx.h:41
#define GOT_SYNC2
Definition: gps_ubx.c:36
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
int16_t azim
azimuth in deg
Definition: gps.h:58
#define GPS_NB_CHANNELS
Definition: gps_nmea.h:34
uint8_t zone
UTM zone number.
#define GPS_UBX_ERR_UNEXPECTED
Definition: gps_ubx.c:49
uint8_t status_flags
Definition: gps_ubx.h:56
uint8_t msg_class
Definition: gps_ubx.h:46
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
float north
in meters
Device independent GPS code (interface)
position in UTM coordinates Units: meters
uint8_t error_cnt
Definition: gps_ubx.h:53
uint8_t msg_idx
Definition: gps_ubx.h:50
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint8_t ck_b
Definition: gps_ubx.h:51
uint8_t error_last
Definition: gps_ubx.h:54
#define GPS_UBX_ERR_MSG_TOO_LONG
Definition: gps_ubx.c:47
int32_t north
in centimeters
int32_t x
in centimeters
#define LED_TOGGLE(i)
Definition: led_hw.h:30
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:82
#define TRUE
Definition: imu_chimu.h:144
void gps_ubx_read_message(void)
Definition: gps_ubx.c:92
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
uint8_t have_velned
Definition: gps_ubx.h:58
int32_t east
in centimeters
uint32_t pacc
position accuracy in cm
Definition: gps.h:72
#define GOT_LEN1
Definition: gps_ubx.c:39
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:54
unsigned char uint8_t
Definition: types.h:14
#define GPS_UBX_ERR_OVERRUN
Definition: gps_ubx.c:46
#define GOT_PAYLOAD
Definition: gps_ubx.c:41
uint8_t sol_flags
Definition: gps_ubx.h:57
int8_t elev
elevation in deg
Definition: gps.h:57
int32_t lon
in radians*1e7
uint8_t svid
Satellite ID.
Definition: gps.h:53
#define GOT_LEN2
Definition: gps_ubx.c:40
arch independent LED (Light Emitting Diodes) API
#define SysTimeTimerStart(_t)
Definition: sys_time_arch.h:70
#define UNINIT
Definition: gps_ubx.c:34
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
uint8_t status
Definition: gps_ubx.h:48
uint16_t len
Definition: gps_ubx.h:49
float lon
in radians
static struct point c
Definition: discsurvey.c:13
void gps_impl_init(void)
Definition: gps_ubx.c:83
void ubxsend_cfg_rst(uint16_t bbr, uint8_t reset_mode)
Definition: gps_ubx.c:277
float east
in meters
#define GOT_SYNC1
Definition: gps_ubx.c:35
#define GPS_UBX_ERR_NONE
Definition: gps_ubx.c:45
struct GpsState gps
global GPS state
Definition: gps.c:31
uint8_t msg_id
Definition: gps_ubx.h:45
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:56
#define GPS_UBX_ERR_CHECKSUM
Definition: gps_ubx.c:48
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:81
uint8_t num_sv
number of sat in fix
Definition: gps.h:76