Paparazzi UAS  v5.15_devel-187-g731cc33
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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/gps_ubx.h"
24 #include "subsystems/abi.h"
25 #include "led.h"
26 
27 #ifndef USE_GPS_UBX_RTCM
28 #define USE_GPS_UBX_RTCM 0
29 #endif
30 
31 
32 #if USE_GPS_UBX_RTCM
34 #define RTCM3_PREAMBLE 0xD3
35 #define RTCM3_MSG_1005 0x69
36 #define RTCM3_MSG_1077 0xB1
37 #define RTCM3_MSG_4072 0x72
38 #define RTCM3_MSG_1230 0xE6
39 #define RTCM3_MSG_1087 0xBB
40 #endif
41 
42 #if PRINT_DEBUG_GPS_UBX
43 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
44 #else
45 #define DEBUG_PRINT(...) {}
46 #endif
47 
49 #include "ubx_protocol.h"
50 
51 /* parser status */
52 #define UNINIT 0
53 #define GOT_SYNC1 1
54 #define GOT_SYNC2 2
55 #define GOT_CLASS 3
56 #define GOT_ID 4
57 #define GOT_LEN1 5
58 #define GOT_LEN2 6
59 #define GOT_PAYLOAD 7
60 #define GOT_CHECKSUM1 8
61 
62 #define RXM_RTCM_VERSION 0x02
63 #define NAV_RELPOSNED_VERSION 0x00
64 /* last error type */
65 #define GPS_UBX_ERR_NONE 0
66 #define GPS_UBX_ERR_OVERRUN 1
67 #define GPS_UBX_ERR_MSG_TOO_LONG 2
68 #define GPS_UBX_ERR_CHECKSUM 3
69 #define GPS_UBX_ERR_UNEXPECTED 4
70 #define GPS_UBX_ERR_OUT_OF_SYNC 5
71 
72 #define UTM_HEM_NORTH 0
73 #define UTM_HEM_SOUTH 1
74 
75 struct GpsUbx gps_ubx;
76 
77 #if USE_GPS_UBX_RXM_RAW
78 struct GpsUbxRaw gps_ubx_raw;
79 #endif
80 
81 #if USE_GPS_UBX_RTCM
82 extern struct GpsRelposNED gps_relposned;
83 extern struct RtcmMan rtcm_man;
84 
85 #ifndef INJECT_BUFF_SIZE
86 #define INJECT_BUFF_SIZE 512
87 #endif
88 /* RTCM control struct type */
89 struct rtcm_t {
90  uint32_t nbyte;
91  uint32_t len;
92  uint8_t buff[INJECT_BUFF_SIZE + 1];
93 };
94 struct rtcm_t rtcm = { 0 };
95 
96 #endif
97 
98 void gps_ubx_init(void)
99 {
101  gps_ubx.msg_available = false;
102  gps_ubx.error_cnt = 0;
104 
106 }
107 
108 void gps_ubx_event(void)
109 {
110  struct link_device *dev = &((UBX_GPS_LINK).device);
111 
112  while (dev->char_available(dev->periph)) {
113  gps_ubx_parse(dev->get_byte(dev->periph));
114  if (gps_ubx.msg_available) {
115  gps_ubx_msg();
116  }
117  }
118 }
119 
120 static void gps_ubx_parse_nav_pvt(void)
121 {
122  uint8_t flags = UBX_NAV_PVT_flags(gps_ubx.msg_buf);
123 
124  // Copy fix information
125  uint8_t gnssFixOK = bit_is_set(flags, 0);
126  uint8_t diffSoln = bit_is_set(flags, 1);
127  uint8_t carrSoln = (flags & 0xC0) >> 6;
128  if (gnssFixOK > 0) {
129  if (diffSoln > 0) {
130  if (carrSoln == 2) {
131  gps_ubx.state.fix = 5; // rtk
132  } else {
133  gps_ubx.state.fix = 4; // dgnss
134  }
135  } else {
136  gps_ubx.state.fix = 3; // 3D
137  }
138  } else {
139  gps_ubx.state.fix = 0;
140  }
141 
142  // Copy time information
143  gps_ubx.state.tow = UBX_NAV_PVT_iTOW(gps_ubx.msg_buf);
144  uint16_t year = UBX_NAV_PVT_year(gps_ubx.msg_buf);
145  uint8_t month = UBX_NAV_PVT_month(gps_ubx.msg_buf);
146  uint8_t day = UBX_NAV_PVT_day(gps_ubx.msg_buf);
147  gps_ubx.state.week = gps_week_number(year, month, day);
148  gps_ubx.state.num_sv = UBX_NAV_PVT_numSV(gps_ubx.msg_buf);
149 
150  // Copy LLA position
151  gps_ubx.state.lla_pos.lat = UBX_NAV_PVT_lat(gps_ubx.msg_buf);
152  gps_ubx.state.lla_pos.lon = UBX_NAV_PVT_lon(gps_ubx.msg_buf);
153  gps_ubx.state.lla_pos.alt = UBX_NAV_PVT_height(gps_ubx.msg_buf);
155 
156  // Copy heading if valid
157  uint8_t headVehValid = bit_is_set(flags, 5);
158  if (headVehValid) {
159  // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
160  // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
161  // solution: First to radians, and then scale to 1e-7 radians
162  // First x 10 for loosing less resolution, then to radians, then multiply x 10 again
163  gps_ubx.state.course = (RadOfDeg(UBX_NAV_PVT_headMot(gps_ubx.msg_buf) * 10)) * 10;
165  gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_PVT_headAcc(gps_ubx.msg_buf) * 10)) * 10;
166  }
167 
168  // Copy HMSL and ground speed
169  gps_ubx.state.hmsl = UBX_NAV_PVT_hMSL(gps_ubx.msg_buf);
170  gps_ubx.state.gspeed = UBX_NAV_PVT_gSpeed(gps_ubx.msg_buf);
172 
173  // Copy NED velocities
174  gps_ubx.state.ned_vel.x = UBX_NAV_PVT_velN(gps_ubx.msg_buf) / 10;
175  gps_ubx.state.ned_vel.y = UBX_NAV_PVT_velE(gps_ubx.msg_buf) / 10;
176  gps_ubx.state.ned_vel.z = UBX_NAV_PVT_velD(gps_ubx.msg_buf) / 10;
178 
179  // Copy accuracy information
180  gps_ubx.state.pdop = UBX_NAV_PVT_pDOP(gps_ubx.msg_buf);
181  gps_ubx.state.hacc = UBX_NAV_PVT_hAcc(gps_ubx.msg_buf) / 10;
182  gps_ubx.state.vacc = UBX_NAV_PVT_vAcc(gps_ubx.msg_buf) / 10;
183  gps_ubx.state.sacc = UBX_NAV_PVT_sAcc(gps_ubx.msg_buf) / 10;
184 }
185 
186 static void gps_ubx_parse_nav_sol(void)
187 {
188  // Copy time and fix information
189 #if !USE_GPS_UBX_RTCM
190  gps_ubx.state.fix = UBX_NAV_SOL_gpsFix(gps_ubx.msg_buf);
191 #endif
192  gps_ubx.state.tow = UBX_NAV_SOL_iTOW(gps_ubx.msg_buf);
193  gps_ubx.state.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
194  gps_ubx.state.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
195 
196  // Copy ecef position
197  gps_ubx.state.ecef_pos.x = UBX_NAV_SOL_ecefX(gps_ubx.msg_buf);
198  gps_ubx.state.ecef_pos.y = UBX_NAV_SOL_ecefY(gps_ubx.msg_buf);
199  gps_ubx.state.ecef_pos.z = UBX_NAV_SOL_ecefZ(gps_ubx.msg_buf);
201 
202  // Copy ecef velocity
203  gps_ubx.state.ecef_vel.x = UBX_NAV_SOL_ecefVX(gps_ubx.msg_buf);
204  gps_ubx.state.ecef_vel.y = UBX_NAV_SOL_ecefVY(gps_ubx.msg_buf);
205  gps_ubx.state.ecef_vel.z = UBX_NAV_SOL_ecefVZ(gps_ubx.msg_buf);
207 
208  // Copy accuracy measurements
209  gps_ubx.state.pacc = UBX_NAV_SOL_pAcc(gps_ubx.msg_buf);
210  gps_ubx.state.sacc = UBX_NAV_SOL_sAcc(gps_ubx.msg_buf);
211  gps_ubx.state.pdop = UBX_NAV_SOL_pDOP(gps_ubx.msg_buf);
212 }
213 
214 static void gps_ubx_parse_nav_posecef(void)
215 {
216  gps_ubx.state.tow = UBX_NAV_POSECEF_iTOW(gps_ubx.msg_buf);
217 
218  // Copy ecef position
219  gps_ubx.state.ecef_pos.x = UBX_NAV_POSECEF_ecefX(gps_ubx.msg_buf);
220  gps_ubx.state.ecef_pos.y = UBX_NAV_POSECEF_ecefY(gps_ubx.msg_buf);
221  gps_ubx.state.ecef_pos.z = UBX_NAV_POSECEF_ecefZ(gps_ubx.msg_buf);
223 
224  // Copy accuracy information
225  gps_ubx.state.pacc = UBX_NAV_POSECEF_pAcc(gps_ubx.msg_buf);
226 }
227 
228 static void gps_ubx_parse_nav_posllh(void)
229 {
230  // Copy LLA position
231  gps_ubx.state.lla_pos.lat = UBX_NAV_POSLLH_lat(gps_ubx.msg_buf);
232  gps_ubx.state.lla_pos.lon = UBX_NAV_POSLLH_lon(gps_ubx.msg_buf);
233  gps_ubx.state.lla_pos.alt = UBX_NAV_POSLLH_height(gps_ubx.msg_buf);
235 
236  // Copy HMSL
237  gps_ubx.state.hmsl = UBX_NAV_POSLLH_hMSL(gps_ubx.msg_buf);
239 
240  // Copy accuracy information
241  gps_ubx.state.hacc = UBX_NAV_POSLLH_hAcc(gps_ubx.msg_buf) / 10;
242  gps_ubx.state.vacc = UBX_NAV_POSLLH_vAcc(gps_ubx.msg_buf) / 10;
243 }
244 
245 static void gps_ubx_parse_nav_posutm(void)
246 {
247  uint8_t hem = UBX_NAV_POSUTM_hem(gps_ubx.msg_buf);
248 
249  // Copy UTM state
250  gps_ubx.state.utm_pos.east = UBX_NAV_POSUTM_east(gps_ubx.msg_buf);
251  gps_ubx.state.utm_pos.north = UBX_NAV_POSUTM_north(gps_ubx.msg_buf);
252  if (hem == UTM_HEM_SOUTH) {
253  gps_ubx.state.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
254  }
255  gps_ubx.state.utm_pos.alt = UBX_NAV_POSUTM_alt(gps_ubx.msg_buf) * 10;
256  gps_ubx.state.utm_pos.zone = UBX_NAV_POSUTM_zone(gps_ubx.msg_buf);
258 
259  // Copy HMSL
262 }
263 
264 static void gps_ubx_parse_velecef(void)
265 {
266  gps_ubx.state.tow = UBX_NAV_VELECEF_iTOW(gps_ubx.msg_buf);
267 
268  // Copy ecef velocity
269  gps_ubx.state.ecef_vel.x = UBX_NAV_VELECEF_ecefVX(gps_ubx.msg_buf);
270  gps_ubx.state.ecef_vel.y = UBX_NAV_VELECEF_ecefVY(gps_ubx.msg_buf);
271  gps_ubx.state.ecef_vel.z = UBX_NAV_VELECEF_ecefVZ(gps_ubx.msg_buf);
273 
274  // Copy accuracy measurements
275  gps_ubx.state.sacc = UBX_NAV_VELECEF_sAcc(gps_ubx.msg_buf);
276 }
277 
278 static void gps_ubx_parse_nav_velned(void)
279 {
280  // Copy groundspeed and total 3d speed
281  gps_ubx.state.speed_3d = UBX_NAV_VELNED_speed(gps_ubx.msg_buf);
282  gps_ubx.state.gspeed = UBX_NAV_VELNED_gSpeed(gps_ubx.msg_buf);
283 
284  // Copy NED velocities
285  gps_ubx.state.ned_vel.x = UBX_NAV_VELNED_velN(gps_ubx.msg_buf);
286  gps_ubx.state.ned_vel.y = UBX_NAV_VELNED_velE(gps_ubx.msg_buf);
287  gps_ubx.state.ned_vel.z = UBX_NAV_VELNED_velD(gps_ubx.msg_buf);
289 
290  // Copy course
291  gps_ubx.state.course = (RadOfDeg(UBX_NAV_VELNED_heading(gps_ubx.msg_buf) * 10)) * 10;
292  gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_VELNED_cAcc(gps_ubx.msg_buf) * 10)) * 10;
294 
295  // Copy time of week
296  gps_ubx.state.tow = UBX_NAV_VELNED_iTOW(gps_ubx.msg_buf);
297 }
298 
299 static void gps_ubx_parse_nav_svinfo(void)
300 {
301  // Get the number of channels
302  gps_ubx.state.nb_channels = Min(UBX_NAV_SVINFO_numCh(gps_ubx.msg_buf), GPS_NB_CHANNELS);
303 
304  // Go through all the different channels
305  for (uint8_t i = 0; i < gps_ubx.state.nb_channels; i++) {
306  gps_ubx.state.svinfos[i].svid = UBX_NAV_SVINFO_svid(gps_ubx.msg_buf, i);
307  gps_ubx.state.svinfos[i].flags = UBX_NAV_SVINFO_flags(gps_ubx.msg_buf, i);
308  gps_ubx.state.svinfos[i].qi = UBX_NAV_SVINFO_quality(gps_ubx.msg_buf, i);
309  gps_ubx.state.svinfos[i].cno = UBX_NAV_SVINFO_cno(gps_ubx.msg_buf, i);
310  gps_ubx.state.svinfos[i].elev = UBX_NAV_SVINFO_elev(gps_ubx.msg_buf, i);
311  gps_ubx.state.svinfos[i].azim = UBX_NAV_SVINFO_azim(gps_ubx.msg_buf, i);
312  }
313 }
314 
315 static void gps_ubx_parse_nav_sat(void)
316 {
317  // Get the number of channels(sattelites) and time of week
318  gps_ubx.state.tow = UBX_NAV_SAT_iTOW(gps_ubx.msg_buf);
319  gps_ubx.state.nb_channels = Min(UBX_NAV_SAT_numSvs(gps_ubx.msg_buf), GPS_NB_CHANNELS);
320 
321  // Check the version
322  uint8_t version = UBX_NAV_SAT_version(gps_ubx.msg_buf);
323  if (version != 1) {
324  return;
325  }
326 
327  // Go through all the different channels
328  for (uint8_t i = 0; i < gps_ubx.state.nb_channels; i++) {
329  uint32_t flags = UBX_NAV_SVINFO_flags(gps_ubx.msg_buf, i);
330  gps_ubx.state.svinfos[i].svid = UBX_NAV_SAT_svId(gps_ubx.msg_buf, i);
331  gps_ubx.state.svinfos[i].cno = UBX_NAV_SAT_cno(gps_ubx.msg_buf, i);
332  gps_ubx.state.svinfos[i].elev = UBX_NAV_SAT_elev(gps_ubx.msg_buf, i);
333  gps_ubx.state.svinfos[i].azim = UBX_NAV_SAT_azim(gps_ubx.msg_buf, i);
334  gps_ubx.state.svinfos[i].qi = flags & 0x7;
335  gps_ubx.state.svinfos[i].flags = (flags >> 3) & 0x1;
336  }
337 }
338 
339 static void gps_ubx_parse_nav_status(void)
340 {
341 #if !USE_GPS_UBX_RTCM
342  gps_ubx.state.fix = UBX_NAV_STATUS_gpsFix(gps_ubx.msg_buf);
343 #endif
344  gps_ubx.state.tow = UBX_NAV_STATUS_iTOW(gps_ubx.msg_buf);
345  gps_ubx.status_flags = UBX_NAV_STATUS_flags(gps_ubx.msg_buf);
346 }
347 
349 {
350 #if USE_GPS_UBX_RTCM
351  uint8_t version = UBX_NAV_RELPOSNED_VERSION(gps_ubx.msg_buf);
352  if (version == NAV_RELPOSNED_VERSION) {
353  gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gps_ubx.msg_buf);
354  gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf);
355  gps_relposned.relPosN = UBX_NAV_RELPOSNED_relPosN(gps_ubx.msg_buf);
356  gps_relposned.relPosE = UBX_NAV_RELPOSNED_relPosE(gps_ubx.msg_buf);
357  gps_relposned.relPosD = UBX_NAV_RELPOSNED_relPosD(gps_ubx.msg_buf) ;
358  gps_relposned.relPosHPN = UBX_NAV_RELPOSNED_relPosNHP(gps_ubx.msg_buf);
359  gps_relposned.relPosHPE = UBX_NAV_RELPOSNED_relPosEHP(gps_ubx.msg_buf);
360  gps_relposned.relPosHPD = UBX_NAV_RELPOSNED_relPosDHP(gps_ubx.msg_buf);
361  gps_relposned.accN = UBX_NAV_RELPOSNED_accN(gps_ubx.msg_buf);
362  gps_relposned.accE = UBX_NAV_RELPOSNED_accE(gps_ubx.msg_buf);
363  gps_relposned.accD = UBX_NAV_RELPOSNED_accD(gps_ubx.msg_buf);
364  uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
365  gps_relposned.carrSoln = RTCMgetbitu(&flags, 3, 2);
366  gps_relposned.relPosValid = RTCMgetbitu(&flags, 5, 1);
367  gps_relposned.diffSoln = RTCMgetbitu(&flags, 6, 1);
368  gps_relposned.gnssFixOK = RTCMgetbitu(&flags, 7, 1);
369  if (gps_relposned.gnssFixOK > 0) {
370  if (gps_relposned.diffSoln > 0) {
371  if (gps_relposned.carrSoln == 2) {
372  gps_ubx.state.fix = 5; // rtk
373  } else {
374  gps_ubx.state.fix = 4; // dgnss
375  }
376  } else {
377  gps_ubx.state.fix = 3; // 3D
378  }
379  } else {
380  gps_ubx.state.fix = 0;
381  }
382  }
383 #endif
384 }
385 
386 static void gps_ubx_parse_rxm_raw(void)
387 {
388 #if USE_GPS_UBX_RXM_RAW
389  gps_ubx_raw.iTOW = UBX_RXM_RAW_iTOW(gps_ubx.msg_buf);
390  gps_ubx_raw.week = UBX_RXM_RAW_week(gps_ubx.msg_buf);
391  gps_ubx_raw.numSV = UBX_RXM_RAW_numSV(gps_ubx.msg_buf);
392  uint8_t i;
393  uint8_t max_SV = Min(gps_ubx_raw.numSV, GPS_UBX_NB_CHANNELS);
394  for (i = 0; i < max_SV; i++) {
395  gps_ubx_raw.measures[i].cpMes = UBX_RXM_RAW_cpMes(gps_ubx.msg_buf, i);
396  gps_ubx_raw.measures[i].prMes = UBX_RXM_RAW_prMes(gps_ubx.msg_buf, i);
397  gps_ubx_raw.measures[i].doMes = UBX_RXM_RAW_doMes(gps_ubx.msg_buf, i);
398  gps_ubx_raw.measures[i].sv = UBX_RXM_RAW_sv(gps_ubx.msg_buf, i);
399  gps_ubx_raw.measures[i].mesQI = UBX_RXM_RAW_mesQI(gps_ubx.msg_buf, i);
400  gps_ubx_raw.measures[i].cno = UBX_RXM_RAW_cno(gps_ubx.msg_buf, i);
401  gps_ubx_raw.measures[i].lli = UBX_RXM_RAW_lli(gps_ubx.msg_buf, i);
402  }
403 #endif // USE_GPS_UBX_RXM_RAW
404 }
405 
406 static void gps_ubx_parse_rxm_rtcm(void)
407 {
408 #if USE_GPS_UBX_RTCM
409  uint8_t version = UBX_RXM_RTCM_version(gps_ubx.msg_buf);
410  if (version == RXM_RTCM_VERSION) {
411  // uint8_t flags = UBX_RXM_RTCM_flags(gps_ubx.msg_buf);
412  // bool crcFailed = RTCMgetbitu(&flags, 7, 1);
413  // uint16_t refStation = UBX_RXM_RTCM_refStation(gps_ubx.msg_buf);
414  // uint16_t msgType = UBX_RXM_RTCM_msgType(gps_ubx.msg_buf);
415  // DEBUG_PRINT("Message %i from refStation %i processed (CRCfailed: %i)\n", msgType, refStation, crcFailed);
416 
417  rtcm_man.RefStation = UBX_RXM_RTCM_refStation(gps_ubx.msg_buf);
418  rtcm_man.MsgType = UBX_RXM_RTCM_msgType(gps_ubx.msg_buf);
419  uint8_t flags = UBX_RXM_RTCM_flags(gps_ubx.msg_buf);
420  bool crcFailed = RTCMgetbitu(&flags, 7, 1);
421  switch (rtcm_man.MsgType) {
422  case 1005:
423  rtcm_man.Cnt105 += 1;
424  rtcm_man.Crc105 += crcFailed;
425  break;
426  case 1077:
427  rtcm_man.Cnt177 += 1;
428  rtcm_man.Crc177 += crcFailed;;
429  break;
430  case 1087:
431  rtcm_man.Cnt187 += 1;
432  rtcm_man.Crc187 += crcFailed;;
433  break;
434  default:
435  break;
436  }
437  } else {
438  DEBUG_PRINT("Unknown RXM_RTCM version: %i\n", version);
439  }
440 #endif // USE_GPS_UBX_RTCM
441 }
442 
444 {
445 
446  if (gps_ubx.msg_class == UBX_NAV_ID) {
447  switch (gps_ubx.msg_id) {
448  case UBX_NAV_POSECEF_ID:
450  break;
451  case UBX_NAV_POSLLH_ID:
453  break;
454  case UBX_NAV_STATUS_ID:
456  break;
457  case UBX_NAV_SOL_ID:
459  break;
460  case UBX_NAV_PVT_ID:
462  break;
463  case UBX_NAV_POSUTM_ID:
465  break;
466  case UBX_NAV_VELECEF_ID:
468  break;
469  case UBX_NAV_VELNED_ID:
471  break;
472  case UBX_NAV_SVINFO_ID:
474  break;
475  case UBX_NAV_SAT_ID:
477  break;
478  case UBX_NAV_RELPOSNED_ID:
480  break;
481  default:
482  break;
483  }
484  }
485  else if (gps_ubx.msg_class == UBX_RXM_ID) {
486  switch (gps_ubx.msg_id) {
487  case UBX_RXM_RAW_ID:
489  break;
490  case UBX_RXM_RTCM_ID:
492  break;
493  default:
494  break;
495  }
496  }
497 
498 }
499 
500 #if LOG_RAW_GPS
502 #endif
503 
504 /* UBX parsing */
506 {
507 #if LOG_RAW_GPS
508  sdLogWriteByte(pprzLogFile, c);
509 #endif
510  if (gps_ubx.status < GOT_PAYLOAD) {
511  gps_ubx.ck_a += c;
513  }
514  switch (gps_ubx.status) {
515  case UNINIT:
516  if (c == UBX_SYNC1) {
517  gps_ubx.status++;
518  }
519  break;
520  case GOT_SYNC1:
521  if (c != UBX_SYNC2) {
523  goto error;
524  }
525  gps_ubx.ck_a = 0;
526  gps_ubx.ck_b = 0;
527  gps_ubx.status++;
528  break;
529  case GOT_SYNC2:
530  if (gps_ubx.msg_available) {
531  /* Previous message has not yet been parsed: discard this one */
533  goto error;
534  }
535  gps_ubx.msg_class = c;
536  gps_ubx.status++;
537  break;
538  case GOT_CLASS:
539  gps_ubx.msg_id = c;
540  gps_ubx.status++;
541  break;
542  case GOT_ID:
543  gps_ubx.len = c;
544  gps_ubx.status++;
545  break;
546  case GOT_LEN1:
547  gps_ubx.len |= (c << 8);
550  goto error;
551  }
552  gps_ubx.msg_idx = 0;
553  gps_ubx.status++;
554  break;
555  case GOT_LEN2:
557  gps_ubx.msg_idx++;
558  if (gps_ubx.msg_idx >= gps_ubx.len) {
559  gps_ubx.status++;
560  }
561  break;
562  case GOT_PAYLOAD:
563  if (c != gps_ubx.ck_a) {
565  goto error;
566  }
567  gps_ubx.status++;
568  break;
569  case GOT_CHECKSUM1:
570  if (c != gps_ubx.ck_b) {
572  goto error;
573  }
574  gps_ubx.msg_available = true;
575  goto restart;
576  break;
577  default:
579  goto error;
580  }
581  return;
582 error:
583  gps_ubx.error_cnt++;
584 restart:
586  return;
587 }
588 
589 static void ubx_send_1byte(struct link_device *dev, uint8_t byte)
590 {
591  dev->put_byte(dev->periph, 0, byte);
594 }
595 
596 void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len)
597 {
598  dev->put_byte(dev->periph, 0, UBX_SYNC1);
599  dev->put_byte(dev->periph, 0, UBX_SYNC2);
600  gps_ubx.send_ck_a = 0;
601  gps_ubx.send_ck_b = 0;
602  ubx_send_1byte(dev, nav_id);
603  ubx_send_1byte(dev, msg_id);
604  ubx_send_1byte(dev, (uint8_t)(len & 0xFF));
605  ubx_send_1byte(dev, (uint8_t)(len >> 8));
606 }
607 
608 void ubx_trailer(struct link_device *dev)
609 {
610  dev->put_byte(dev->periph, 0, gps_ubx.send_ck_a);
611  dev->put_byte(dev->periph, 0, gps_ubx.send_ck_b);
612  dev->send_message(dev->periph, 0);
613 }
614 
615 void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes)
616 {
617  int i;
618  for (i = 0; i < len; i++) {
619  ubx_send_1byte(dev, bytes[i]);
620  }
621 }
622 
623 void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , uint8_t reset_mode)
624 {
625 #ifdef UBX_GPS_LINK
626  UbxSend_CFG_RST(dev, bbr, reset_mode, 0x00);
627 #endif /* else less harmful for HITL */
628 }
629 
630 #ifndef GPS_UBX_UCENTER
631 #define gps_ubx_ucenter_event() {}
632 #else
634 #endif
635 
636 void gps_ubx_msg(void)
637 {
638  // current timestamp
639  uint32_t now_ts = get_sys_time_usec();
640 
645  if (gps_ubx.msg_class == UBX_NAV_ID &&
646  (gps_ubx.msg_id == UBX_NAV_VELNED_ID ||
647  gps_ubx.msg_id == UBX_NAV_PVT_ID ||
648  (gps_ubx.msg_id == UBX_NAV_SOL_ID &&
650  if (gps_ubx.state.fix >= GPS_FIX_3D) {
653 #ifdef GPS_LED
654  LED_ON(GPS_LED);
655  } else {
656  LED_TOGGLE(GPS_LED);
657  }
658 #else
659  }
660 #endif
661  AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps_ubx.state);
662  }
663  gps_ubx.msg_available = false;
664 }
665 
666 /*
667  * Write bytes to the ublox UART connection
668  * This is a wrapper functions used in the librtcm library
669  */
670 void gps_ublox_write(struct link_device *dev, uint8_t *buff, uint32_t n);
671 void gps_ublox_write(struct link_device *dev, uint8_t *buff, uint32_t n)
672 {
673  uint32_t i = 0;
674  for (i = 0; i < n; i++) {
675  dev->put_byte(dev->periph, 0, buff[i]);
676  }
677  dev->send_message(dev->periph, 0);
678  return;
679 }
680 
684 #if USE_GPS_UBX_RTCM
685 void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
686 {
687  uint8_t i;
688 
689  // nothing to do
690  if (length == 0) {
691  return;
692  }
693 
694 #ifdef GPS_UBX_UCENTER
695  // not ready
696  if (gps_ubx_ucenter_get_status() != 0) {
697  return;
698  }
699 #endif
700 
701  // go through buffer
702  for (i = 0; i < length; i++) {
703  if (rtcm.nbyte == 0) {
704  // wait for frame start byte
705  if (data[i] == RTCM3_PREAMBLE) {
706  rtcm.buff[rtcm.nbyte++] = data[i];
707  }
708  } else {
709  // fill buffer
710  if (rtcm.nbyte < INJECT_BUFF_SIZE) {
711  rtcm.buff[rtcm.nbyte++] = data[i];
712  if (rtcm.nbyte == 3) {
713  // extract length
714  rtcm.len = RTCMgetbitu(rtcm.buff, 14, 10) + 3;
715  } else {
716  // wait complete frame
717  if (rtcm.nbyte == rtcm.len + 3) {
718  // check CRC
719  unsigned int crc1 = crc24q(rtcm.buff, rtcm.len);
720  unsigned int crc2 = RTCMgetbitu(rtcm.buff, rtcm.len * 8, 24);
721 
722  if (crc1 == crc2) {
723  // write to GPS
724  gps_ublox_write(&(UBX_GPS_LINK).device, rtcm.buff, rtcm.len + 3);
725  switch (packet_id) {
726  case RTCM3_MSG_1005 : break;
727  case RTCM3_MSG_1077 : break;
728  case RTCM3_MSG_1087 : break;
729  case RTCM3_MSG_4072 : break;
730  case RTCM3_MSG_1230 : break;
731  default: DEBUG_PRINT("Unknown type: %i", packet_id); break;
732  }
733  } else {
734  DEBUG_PRINT("Skipping message %i (CRC failed) - %d", packet_id, rtcm.buff[0]);
735  unsigned int j;
736  for (j = 1; j < rtcm.len; j++) {
737  DEBUG_PRINT(",%d", rtcm.buff[j]);
738  }
739  DEBUG_PRINT("\n");
740  }
741 
742  // reset index
743  rtcm.nbyte = 0;
744  }
745  }
746  } else {
747  // reset index
748  rtcm.nbyte = 0;
749  }
750  }
751  }
752 }
753 
754 #endif
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:80
unsigned short uint16_t
Definition: types.h:16
#define GOT_CLASS
Definition: gps_ubx.c:55
int32_t z
in centimeters
unsigned int RTCMgetbitu(unsigned char *buff, int pos, int lenb)
Definition: CRC24Q.h:54
#define UTM_HEM_SOUTH
Definition: gps_ubx.c:73
int32_t north
in centimeters
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:112
int16_t azim
azimuth in deg
Definition: gps.h:83
#define Min(x, y)
Definition: esc_dshot.c:85
#define GOT_CHECKSUM1
Definition: gps_ubx.c:60
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
#define GOT_ID
Definition: gps_ubx.c:56
void ubx_trailer(struct link_device *dev)
Definition: gps_ubx.c:608
#define RXM_RTCM_VERSION
Definition: gps_ubx.c:62
static void gps_ubx_parse_nav_svinfo(void)
Definition: gps_ubx.c:299
void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes)
Definition: gps_ubx.c:615
#define gps_ubx_ucenter_event()
Definition: gps_ubx.c:631
void gps_ubx_event(void)
Definition: gps_ubx.c:108
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:111
uint32_t Cnt187
Definition: gps.h:152
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
static void gps_ubx_parse_rxm_raw(void)
Definition: gps_ubx.c:386
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
int32_t y
in centimeters
uint8_t carrSoln
Definition: gps.h:141
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:101
uint16_t week
GPS week.
Definition: gps.h:108
UBX protocol specific code.
Definition: gps_ubx.h:54
#define GPS_NB_CHANNELS
Definition: gps.h:57
#define RTCM3_MSG_1077
Definition: rtcm3.h:22
void gps_ubx_parse(uint8_t c)
Definition: gps_ubx.c:505
int32_t relPosE
Definition: gps.h:133
#define GPS_UBX_ID
Main include for ABI (AirBorneInterface).
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:102
struct RtcmMan rtcm_man
Definition: gps.c:72
static void gps_ubx_parse_nav_velned(void)
Definition: gps_ubx.c:278
void gps_ubx_init(void)
Definition: gps_ubx.c:98
#define RTCM3_PREAMBLE
Definition: rtcm3.h:19
uint16_t len
Definition: gps_ubx.h:61
struct GpsUbx gps_ubx
Definition: gps_ubx.c:75
static void gps_ubx_parse_nav_relposned(void)
Definition: gps_ubx.c:348
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD]
Definition: gps_ubx.h:56
static void gps_ubx_parse_velecef(void)
Definition: gps_ubx.c:264
int32_t east
in centimeters
int8_t relPosHPD
Definition: gps.h:137
static void gps_ubx_parse_nav_pvt(void)
Definition: gps_ubx.c:120
uint8_t svid
Satellite ID.
Definition: gps.h:78
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
int32_t z
Down.
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over MSL)
Definition: gps.h:93
static void ubx_send_1byte(struct link_device *dev, uint8_t byte)
Definition: gps_ubx.c:589
uint8_t diffSoln
Definition: gps.h:143
uint32_t Crc187
Definition: gps.h:155
#define GPS_UBX_ERR_OUT_OF_SYNC
Definition: gps_ubx.c:70
uint8_t msg_id
Definition: gps_ubx.h:57
uint8_t error_cnt
Definition: gps_ubx.h:65
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
int8_t elev
elevation in deg
Definition: gps.h:82
uint16_t msg_idx
Definition: gps_ubx.h:62
int32_t alt
in millimeters above WGS84 reference ellipsoid
static void gps_ubx_parse_nav_sat(void)
Definition: gps_ubx.c:315
#define GPS_UBX_MAX_PAYLOAD
Definition: gps_ubx.h:53
uint32_t accN
Definition: gps.h:138
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
#define NAV_RELPOSNED_VERSION
Definition: gps_ubx.c:63
int32_t y
East.
#define GOT_SYNC2
Definition: gps_ubx.c:54
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
uint8_t zone
UTM zone number.
static void gps_ubx_parse_nav_sol(void)
Definition: gps_ubx.c:186
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
Configure Ublox GPS.
#define GPS_UBX_ERR_UNEXPECTED
Definition: gps_ubx.c:69
int8_t relPosHPE
Definition: gps.h:136
uint32_t Crc105
Definition: gps.h:153
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:81
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
uint32_t Cnt177
Definition: gps.h:151
uint8_t ck_b
Definition: gps_ubx.h:63
unsigned int crc24q(const unsigned char *buff, int len)
Definition: CRC24Q.h:44
void gps_ublox_write(struct link_device *dev, uint8_t *buff, uint32_t n)
Definition: gps_ubx.c:671
static void gps_ubx_parse_nav_status(void)
Definition: gps_ubx.c:339
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
uint8_t ck_a
Definition: gps_ubx.h:63
void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len)
Definition: gps_ubx.c:596
#define RTCM3_MSG_1087
Definition: rtcm3.h:23
int32_t x
North.
Definition: gps.h:147
unsigned long uint32_t
Definition: types.h:18
#define GPS_UBX_NB_CHANNELS
Definition: gps_ubx.h:51
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr, uint8_t reset_mode)
Definition: gps_ubx.c:623
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
int32_t lon
in degrees*1e7
uint16_t gps_week_number(uint16_t year, uint8_t month, uint8_t day)
Number of weeks since navigation epoch (6 January 1980)
Definition: gps.c:462
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
uint32_t iTOW
Definition: gps.h:130
static void gps_ubx_parse_nav_posutm(void)
Definition: gps_ubx.c:245
#define RTCM3_MSG_4072
Definition: rtcm3.h:21
#define GPS_UBX_ERR_MSG_TOO_LONG
Definition: gps_ubx.c:67
int32_t relPosD
Definition: gps.h:134
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
#define LED_TOGGLE(i)
Definition: led_hw.h:53
#define RTCM3_MSG_1230
Definition: rtcm3.h:24
uint8_t send_ck_b
Definition: gps_ubx.h:64
int32_t relPosN
Definition: gps.h:132
bool msg_available
Definition: gps_ubx.h:55
uint8_t relPosValid
Definition: gps.h:142
uint8_t status
Definition: gps_ubx.h:60
struct GpsRelposNED gps_relposned
Definition: gps.c:71
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void gps_ubx_read_message(void)
Definition: gps_ubx.c:443
uint8_t status_flags
Definition: gps_ubx.h:68
int gps_ubx_ucenter_get_status(void)
int8_t relPosHPN
Definition: gps.h:135
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
static void gps_ubx_parse_nav_posllh(void)
Definition: gps_ubx.c:228
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
uint32_t accE
Definition: gps.h:139
#define GOT_LEN1
Definition: gps_ubx.c:57
unsigned char uint8_t
Definition: types.h:14
#define GPS_UBX_ERR_OVERRUN
Definition: gps_ubx.c:66
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
#define GOT_PAYLOAD
Definition: gps_ubx.c:59
uint16_t MsgType
Definition: gps.h:149
uint8_t comp_id
id of current gps
Definition: gps.h:89
#define byte
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
static void gps_ubx_parse_rxm_rtcm(void)
Definition: gps_ubx.c:406
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
data structures for GPS with RTK capabilities
Definition: gps.h:129
uint32_t accD
Definition: gps.h:140
struct GpsState state
Definition: gps_ubx.h:71
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:79
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:116
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
#define GOT_LEN2
Definition: gps_ubx.c:58
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
uint8_t gnssFixOK
Definition: gps.h:144
arch independent LED (Light Emitting Diodes) API
int32_t x
in centimeters
#define UNINIT
Includes macros generated from ubx.xml.
Definition: gps_ubx.c:52
uint32_t Cnt105
Definition: gps.h:150
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
#define LED_ON(i)
Definition: led_hw.h:51
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
uint8_t send_ck_a
Definition: gps_ubx.h:64
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
uint32_t Crc177
Definition: gps.h:154
void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
Override the default GPS packet injector to inject the data trough UART.
Definition: gps_piksi.c:408
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
static void gps_ubx_parse_nav_posecef(void)
Definition: gps_ubx.c:214
#define RTCM3_MSG_1005
Definition: rtcm3.h:20
void gps_ubx_msg(void)
Definition: gps_ubx.c:636
#define DEBUG_PRINT(...)
Definition: gps_ubx.c:45
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:107
uint8_t error_last
Definition: gps_ubx.h:66
#define GPS_VALID_POS_UTM_BIT
Definition: gps.h:50
#define GOT_SYNC1
Definition: gps_ubx.c:53
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
uint16_t RefStation
Definition: gps.h:148
#define GPS_UBX_ERR_NONE
Definition: gps_ubx.c:65
uint8_t msg_class
Definition: gps_ubx.h:58
uint8_t buff[25]
Buffer used for general comunication over SPI (in buffer)
uint16_t refStationId
Definition: gps.h:131
#define GPS_UBX_ERR_CHECKSUM
Definition: gps_ubx.c:68