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.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 GpsUartRunning GpsLink(TxRunning)
57 
58 struct GpsUbx gps_ubx;
59 
60 #if USE_GPS_UBX_RXM_RAW
61 struct GpsUbxRaw gps_ubx_raw;
62 #endif
63 
64 void gps_impl_init(void) {
67  gps_ubx.error_cnt = 0;
69  gps_ubx.have_velned = 0;
70 }
71 
72 
74 
75  if (gps_ubx.msg_class == UBX_NAV_ID) {
76  if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
77  /* get hardware clock ticks */
79  gps_time_sync.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
80  gps_time_sync.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
81  gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
82  gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
83  gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
84  gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
85  gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
86  gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
87  gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
88  gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
89  gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
90  gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
91  gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
92  gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
93  gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
94 #ifdef GPS_LED
95  if (gps.fix == GPS_FIX_3D) {
96  LED_ON(GPS_LED);
97  }
98  else {
99  LED_TOGGLE(GPS_LED);
100  }
101 #endif
102  } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
103  gps.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf);
104  gps.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf);
105  gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
106  gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
107 #if GPS_USE_LATLONG
108  /* Computes from (lat, long) in the referenced UTM zone */
109  struct LlaCoor_f lla_f;
110  LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
111  struct UtmCoor_f utm_f;
112  utm_f.zone = nav_utm_zone0;
113  /* convert to utm */
114  utm_of_lla_f(&utm_f, &lla_f);
115  /* copy results of utm conversion */
116  gps.utm_pos.east = utm_f.east*100;
117  gps.utm_pos.north = utm_f.north*100;
120 #else
121  }
122  else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) {
123  gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
124  gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
125  uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf);
126  if (hem == UTM_HEM_SOUTH)
127  gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
128  gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10;
129  gps.hmsl = gps.utm_pos.alt;
130  gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
131  gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
132 #endif
133  }
134  else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) {
135  gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
136  gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
137  gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
138  gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
139  gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
140  // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
141  // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
142  // solution: First to radians, and then scale to 1e-7 radians
143  // First x 10 for loosing less resolution, then to radians, then multiply x 10 again
144  gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10;
145  gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf)*10)) * 10;
146  gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
147  gps_ubx.have_velned = 1;
148  }
149  else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
150  gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
151  uint8_t i;
152  for(i = 0; i < gps.nb_channels; i++) {
153  gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i);
154  gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i);
155  gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i);
156  gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i);
157  gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i);
158  gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
159  }
160  }
161  else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
162  gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
163  gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
164  gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
165  }
166  }
167 #if USE_GPS_UBX_RXM_RAW
168  else if (gps_ubx.msg_class == UBX_RXM_ID) {
169  if (gps_ubx.msg_id == UBX_RXM_RAW_ID) {
170  gps_ubx_raw.iTOW = UBX_RXM_RAW_iTOW(gps_ubx.msg_buf);
171  gps_ubx_raw.week = UBX_RXM_RAW_week(gps_ubx.msg_buf);
172  gps_ubx_raw.numSV = UBX_RXM_RAW_numSV(gps_ubx.msg_buf);
173  uint8_t i;
174  for (i = 0; i < gps_ubx_raw.numSV; i++) {
175  gps_ubx_raw.measures[i].cpMes = UBX_RXM_RAW_cpMes(gps_ubx.msg_buf, i);
176  gps_ubx_raw.measures[i].prMes = UBX_RXM_RAW_prMes(gps_ubx.msg_buf, i);
177  gps_ubx_raw.measures[i].doMes = UBX_RXM_RAW_doMes(gps_ubx.msg_buf, i);
178  gps_ubx_raw.measures[i].sv = UBX_RXM_RAW_sv(gps_ubx.msg_buf, i);
179  gps_ubx_raw.measures[i].mesQI = UBX_RXM_RAW_mesQI(gps_ubx.msg_buf, i);
180  gps_ubx_raw.measures[i].cno = UBX_RXM_RAW_cno(gps_ubx.msg_buf, i);
181  gps_ubx_raw.measures[i].lli = UBX_RXM_RAW_lli(gps_ubx.msg_buf, i);
182  }
183  }
184  }
185 #endif
186 }
187 
188 #if LOG_RAW_GPS
189 #include "sdLog.h"
190 #include "subsystems/chibios-libopencm3/chibios_sdlog.h"
191 #endif
192 
193 /* UBX parsing */
195 #if LOG_RAW_GPS
196  sdLogWriteByte(&pprzLogFile, c);
197 #endif
198  if (gps_ubx.status < GOT_PAYLOAD) {
199  gps_ubx.ck_a += c;
201  }
202  switch (gps_ubx.status) {
203  case UNINIT:
204  if (c == UBX_SYNC1)
205  gps_ubx.status++;
206  break;
207  case GOT_SYNC1:
208  if (c != UBX_SYNC2) {
210  goto error;
211  }
212  gps_ubx.ck_a = 0;
213  gps_ubx.ck_b = 0;
214  gps_ubx.status++;
215  break;
216  case GOT_SYNC2:
217  if (gps_ubx.msg_available) {
218  /* Previous message has not yet been parsed: discard this one */
220  goto error;
221  }
222  gps_ubx.msg_class = c;
223  gps_ubx.status++;
224  break;
225  case GOT_CLASS:
226  gps_ubx.msg_id = c;
227  gps_ubx.status++;
228  break;
229  case GOT_ID:
230  gps_ubx.len = c;
231  gps_ubx.status++;
232  break;
233  case GOT_LEN1:
234  gps_ubx.len |= (c<<8);
237  goto error;
238  }
239  gps_ubx.msg_idx = 0;
240  gps_ubx.status++;
241  break;
242  case GOT_LEN2:
244  gps_ubx.msg_idx++;
245  if (gps_ubx.msg_idx >= gps_ubx.len) {
246  gps_ubx.status++;
247  }
248  break;
249  case GOT_PAYLOAD:
250  if (c != gps_ubx.ck_a) {
252  goto error;
253  }
254  gps_ubx.status++;
255  break;
256  case GOT_CHECKSUM1:
257  if (c != gps_ubx.ck_b) {
259  goto error;
260  }
262  goto restart;
263  break;
264  default:
266  goto error;
267  }
268  return;
269  error:
270  gps_ubx.error_cnt++;
271  restart:
273  return;
274 }
275 
276 void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
277 #ifdef GPS_LINK
278  UbxSend_CFG_RST(bbr, reset_mode, 0x00);
279 #endif /* else less harmful for HITL */
280 }
281 
282 
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: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
#define GOT_CHECKSUM1
Definition: gps_ubx.c:42
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
#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:74
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD]
Definition: gps_ubx.h:44
uint8_t zone
UTM zone number.
int32_t t0_tow_frac
fractional ns remainder of tow [ms], range -500000 .. 500000
Definition: gps.h:95
void gps_ubx_parse(uint8_t c)
Definition: gps_ubx.c:194
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:75
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:76
bool_t msg_available
Definition: gps_ubx.h:43
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
#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:58
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
#define GPS_NB_CHANNELS
Definition: gps_ardrone2.h:33
int16_t week
GPS week.
Definition: gps.h:79
#define GPS_UBX_ERR_OUT_OF_SYNC
Definition: gps_ubx.c:50
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:56
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:80
int32_t z
Down.
int16_t azim
azimuth in deg
Definition: gps.h:59
uint8_t zone
UTM zone number.
#define GPS_UBX_ERR_UNEXPECTED
Definition: gps_ubx.c:49
uint32_t t0_ticks
hw clock ticks when GPS message is received
Definition: gps.h:96
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:70
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
int32_t y
East.
#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
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:71
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:83
#define TRUE
Definition: imu_chimu.h:144
void gps_ubx_read_message(void)
Definition: gps_ubx.c:73
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
uint8_t have_velned
Definition: gps_ubx.h:58
int32_t east
in centimeters
uint32_t pacc
position accuracy in cm
Definition: gps.h:73
#define GOT_LEN1
Definition: gps_ubx.c:39
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:55
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:58
int32_t lon
in degrees*1e7
uint8_t svid
Satellite ID.
Definition: gps.h:54
#define LLA_FLOAT_OF_BFP(_o, _i)
struct GpsTimeSync gps_time_sync
Definition: gps.c:43
#define GOT_LEN2
Definition: gps_ubx.c:40
arch independent LED (Light Emitting Diodes) API
#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:66
uint8_t status
Definition: gps_ubx.h:48
uint16_t len
Definition: gps_ubx.h:49
uint32_t t0_tow
GPS time of week in ms from last message.
Definition: gps.h:94
void gps_impl_init(void)
GPS initialization.
Definition: gps_ubx.c:64
void ubxsend_cfg_rst(uint16_t bbr, uint8_t reset_mode)
Definition: gps_ubx.c:276
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:41
uint8_t msg_id
Definition: gps_ubx.h:45
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:57
#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:82
uint8_t num_sv
number of sat in fix
Definition: gps.h:77