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_mtk.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 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 
34 #include "subsystems/gps.h"
35 
36 #include "led.h"
37 
38 /* currently needed to get nav_utm_zone0 */
41 
42 #include "mcu_periph/sys_time.h"
43 
44 #define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ
45 #define OUTPUT_RATE 4
46 
47 /* parser status */
48 #define UNINIT 0
49 #define GOT_SYNC1_14 1
50 #define GOT_SYNC2_14 2
51 #define GOT_CLASS_14 3
52 #define GOT_SYNC1_16 4
53 #define GOT_SYNC2_16 5
54 #define GOT_ID 6
55 #define GOT_PAYLOAD 7
56 #define GOT_CHECKSUM1 8
57 
58 /* last error type */
59 #define GPS_MTK_ERR_NONE 0
60 #define GPS_MTK_ERR_OVERRUN 1
61 #define GPS_MTK_ERR_MSG_TOO_LONG 2
62 #define GPS_MTK_ERR_CHECKSUM 3
63 #define GPS_MTK_ERR_UNEXPECTED 4
64 #define GPS_MTK_ERR_OUT_OF_SYNC 5
65 
66 /* mediatek gps fix mask */
67 #define MTK_DIY_FIX_3D 3
68 #define MTK_DIY_FIX_2D 2
69 #define MTK_DIY_FIX_NONE 1
70 
71 
72 /* defines for UTC-GPS time conversion */
73 #define SECS_MINUTE (60)
74 #define SECS_HOUR (60*60)
75 #define SECS_DAY (60*60*24)
76 #define SECS_WEEK (60*60*24*7)
77 
78 #define isleap(x) ((((x)%400)==0) || (!(((x)%100)==0) && (((x)%4)==0)))
79 
80 const int8_t DAYS_MONTH[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
81 
82 struct GpsMtk gps_mtk;
83 
84 #ifdef GPS_CONFIGURE
85 #define MTK_DIY_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
86 #define MTK_DIY_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n"
87 
88 #define MTK_DIY_OUTPUT_1HZ "$PMTK220,1000*1F\r\n"
89 #define MTK_DIY_OUTPUT_2HZ "$PMTK220,500*2B\r\n"
90 #define MTK_DIY_OUTPUT_4HZ "$PMTK220,250*29\r\n"
91 #define MTK_DIY_OTUPUT_5HZ "$PMTK220,200*2C\r\n"
92 #define MTK_DIY_OUTPUT_10HZ "$PMTK220,100*2F\r\n"
93 
94 #define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
95 
96 #define MTK_DIY_SBAS_ON "$PMTK313,1*2E\r\n"
97 #define MTK_DIY_SBAS_OFF "$PMTK313,0*2F\r\n"
98 
99 #define MTK_DIY_WAAS_ON "$PSRF151,1*3F\r\n"
100 #define MTK_DIY_WAAS_OFF "$PSRF151,0*3E\r\n"
101 
102 bool_t gps_configuring;
103 static uint8_t gps_status_config;
104 #endif
105 
106 void gps_impl_init(void) {
109  gps_mtk.error_cnt = 0;
111 #ifdef GPS_CONFIGURE
112  gps_status_config = 0;
113  gps_configuring = TRUE;
114 #endif
115 }
116 
117 static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time,
118  int16_t* gps_week, uint32_t* gps_itow) {
119  /* convert UTC date/time to GPS week/itow, we have no idea about GPS
120  leap seconds for now */
121  uint16_t gps_msecond = gps_time % 1000;
122  uint8_t gps_second = (gps_time / 1000) % 100;
123  uint8_t gps_minute = (gps_time / 100000) % 100;
124  uint8_t gps_hour = (gps_time / 10000000) % 100;
125  uint16_t gps_year = 2000 + (gps_date % 100);
126  uint8_t gps_month = (gps_date / 100) % 100;
127  uint8_t gps_day = (gps_date / 10000) % 100;
128  int32_t i, days;
129 
130  *gps_week = 0;
131  *gps_itow = 0;
132 
133  /* sanity checks */
134  if (gps_month > 12) return;
135  if (gps_day > (DAYS_MONTH[gps_month] +
136  ((gps_month == 1) ? isleap(gps_year):0))) return;
137  if (gps_hour > 23) return;
138  if (gps_minute > 59) return;
139  if (gps_second > 59) return;
140 
141  /* days since 6-JAN-1980 */
142  days = -6;
143  for (i = 1980; i < gps_year; i++) days += (365 + isleap(i));
144 
145  /* add days in gps_year */
146  for (i = 0; i < gps_month-1; i++) {
147  days += DAYS_MONTH[i] + ((i == 1) ? isleap(gps_year):0);
148  }
149  days += gps_day;
150 
151  /* convert */
152  *gps_week = (uint16_t) (days / 7);
153  *gps_itow = ((days % 7) * SECS_DAY +
154  gps_hour * SECS_HOUR +
155  gps_minute * SECS_MINUTE +
156  gps_second) * 1000 +
157  gps_msecond;
158 }
159 
161  if (gps_mtk.msg_class == MTK_DIY14_ID) {
162  if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
163  /* get hardware clock ticks */
165  gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
167  gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf)*10;
168  gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf)*10;
169  // FIXME: with MTK you do not receive vertical speed
170  if (sys_time.nb_sec - gps.last_3dfix_time < 2) {
171  gps.ned_vel.z = ((gps.hmsl -
172  MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
173  } else gps.ned_vel.z = 0;
174  gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10;
175  // FIXME: with MTK you do not receive ellipsoid altitude
176  gps.lla_pos.alt = gps.hmsl;
177  gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
178  // FIXME: with MTK you do not receive speed 3D
180  gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
181  gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
182  switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
183  case MTK_DIY_FIX_3D:
184  gps.fix = GPS_FIX_3D;
185  break;
186  case MTK_DIY_FIX_2D:
187  gps.fix = GPS_FIX_2D;
188  break;
189  default:
190  gps.fix = GPS_FIX_NONE;
191  }
192  gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
193  // FIXME: with MTK DIY 1.4 you do not receive GPS week
194  gps.week = 0;
195  /* Computes from (lat, long) in the referenced UTM zone */
196  struct LlaCoor_f lla_f;
197  LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
198  struct UtmCoor_f utm_f;
199  utm_f.zone = nav_utm_zone0;
200  /* convert to utm */
201  utm_of_lla_f(&utm_f, &lla_f);
202  /* copy results of utm conversion */
203  gps.utm_pos.east = utm_f.east*100;
204  gps.utm_pos.north = utm_f.north*100;
207 #ifdef GPS_LED
208  if (gps.fix == GPS_FIX_3D) {
209  LED_ON(GPS_LED);
210  }
211  else {
212  LED_TOGGLE(GPS_LED);
213  }
214 #endif
215  }
216  }
217 
218  if (gps_mtk.msg_class == MTK_DIY16_ID) {
219  if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
220  uint32_t gps_date, gps_time;
221  gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
222  gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
223  gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow);
224 #ifdef GPS_TIMESTAMP
225  /* get hardware clock ticks */
227  gps.t0_tow = gps.tow;
228  gps.t0_tow_frac = 0;
229 #endif
230  gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf)*10;
231  gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf)*10;
232  // FIXME: with MTK you do not receive vertical speed
233  if (sys_time.nb_sec - gps.last_3dfix_time < 2) {
234  gps.ned_vel.z = ((gps.hmsl -
235  MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
236  } else gps.ned_vel.z = 0;
237  gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10;
238  // FIXME: with MTK you do not receive ellipsoid altitude
239  gps.lla_pos.alt = gps.hmsl;
240  gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
241  // FIXME: with MTK you do not receive speed 3D
243  gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf)*10000)) * 10;
244  gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
245  switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
246  case MTK_DIY_FIX_3D:
247  gps.fix = GPS_FIX_3D;
248  break;
249  case MTK_DIY_FIX_2D:
250  gps.fix = GPS_FIX_2D;
251  break;
252  default:
253  gps.fix = GPS_FIX_NONE;
254  }
255  /* HDOP? */
256  /* Computes from (lat, long) in the referenced UTM zone */
257  struct LlaCoor_f lla_f;
258  LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
259  struct UtmCoor_f utm_f;
260  utm_f.zone = nav_utm_zone0;
261  /* convert to utm */
262  utm_of_lla_f(&utm_f, &lla_f);
263  /* copy results of utm conversion */
264  gps.utm_pos.east = utm_f.east*100;
265  gps.utm_pos.north = utm_f.north*100;
268 #ifdef GPS_LED
269  if (gps.fix == GPS_FIX_3D) {
270  LED_ON(GPS_LED);
271  }
272  else {
273  LED_TOGGLE(GPS_LED);
274  }
275 #endif
276  }
277  }
278 }
279 
280 /* byte parsing */
282  if (gps_mtk.status < GOT_PAYLOAD) {
283  gps_mtk.ck_a += c;
285  }
286  switch (gps_mtk.status) {
287  case UNINIT:
288  if (c == MTK_DIY14_SYNC1)
290  if (c == MTK_DIY16_ID)
291  gps_mtk.msg_class = c;
293  break;
294  /* MTK_DIY_VER_14 */
295  case GOT_SYNC1_14:
296  if (c != MTK_DIY14_SYNC2) {
298  goto error;
299  }
300  if (gps_mtk.msg_available) {
301  /* Previous message has not yet been parsed: discard this one */
303  goto error;
304  }
305  gps_mtk.ck_a = 0;
306  gps_mtk.ck_b = 0;
307  gps_mtk.status++;
308  gps_mtk.len = MTK_DIY14_NAV_LENGTH;
309  break;
310  case GOT_SYNC2_14:
311  if (c != MTK_DIY14_ID) {
313  goto error;
314  }
315  gps_mtk.msg_class = c;
316  gps_mtk.msg_idx = 0;
317  gps_mtk.status++;
318  break;
319  case GOT_CLASS_14:
320  if (c != MTK_DIY14_NAV_ID) {
322  goto error;
323  }
324  gps_mtk.msg_id = c;
326  break;
327  /* MTK_DIY_VER_16 */
328  case GOT_SYNC1_16:
329  if (c != MTK_DIY16_NAV_ID) {
331  goto error;
332  }
333  if (gps_mtk.msg_available) {
334  /* Previous message has not yet been parsed: discard this one */
336  goto error;
337  }
338  gps_mtk.msg_id = c;
339  gps_mtk.ck_a = 0;
340  gps_mtk.ck_b = 0;
341  gps_mtk.status++;
342  break;
343  case GOT_SYNC2_16:
344  gps_mtk.len = c;
345  gps_mtk.msg_idx = 0;
347  break;
348  case GOT_ID:
350  gps_mtk.msg_idx++;
351  if (gps_mtk.msg_idx >= gps_mtk.len) {
352  gps_mtk.status++;
353  }
354  break;
355  case GOT_PAYLOAD:
356  if (c != gps_mtk.ck_a) {
358  goto error;
359  }
360  gps_mtk.status++;
361  break;
362  case GOT_CHECKSUM1:
363  if (c != gps_mtk.ck_b) {
365  goto error;
366  }
368  goto restart;
369  break;
370  default:
372  goto error;
373  }
374  return;
375  error:
376  gps_mtk.error_cnt++;
377  restart:
379  return;
380 }
381 
382 
383 /*
384  *
385  *
386  * GPS dynamic configuration
387  *
388  *
389  */
390 #ifdef GPS_CONFIGURE
391 
392 static void MtkSend_CFG(char* dat) {
393  while (*dat != 0) GpsLink(Transmit(*dat++));
394 }
395 
396 void gps_configure_uart(void) {
397 }
398 
399 #ifdef USER_GPS_CONFIGURE
400 #include USER_GPS_CONFIGURE
401 #else
402 static bool_t user_gps_configure(bool_t cpt) {
403  switch (cpt) {
404  case 0:
405  MtkSend_CFG(MTK_DIY_SET_BINARY);
406  break;
407  case 1:
408  MtkSend_CFG(MTK_DIY_OUTPUT_RATE);
409  return FALSE;
410  default:
411  break;
412  }
413  return TRUE; /* Continue, except for the last case */
414 }
415 #endif // ! USER_GPS_CONFIGURE
416 
417 void gps_configure( void ) {
418  static uint32_t count=0;
419  /* start configuring after having received 50 bytes */
420  if (count++ > 50)
421  gps_configuring = user_gps_configure(gps_status_config++);
422 }
423 
424 #endif /* GPS_CONFIGURE */
unsigned short uint16_t
Definition: types.h:16
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
uint16_t len
Definition: gps_mtk.h:40
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
Definition: gps_mtk.h:33
int32_t lat
in degrees*1e7
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
#define SECS_MINUTE
Definition: gps_mtk.c:73
uint8_t ck_a
Definition: gps_mtk.h:42
void gps_mtk_read_message(void)
Definition: gps_mtk.c:160
uint8_t zone
UTM zone number.
int32_t t0_tow_frac
fractional ns remainder of tow [ms], range -500000 .. 500000
Definition: gps.h:95
#define GpsLink(_x)
Definition: gps_mtk.h:59
struct GpsMtk gps_mtk
Definition: gps_mtk.c:82
uint8_t nav_utm_zone0
Definition: common_nav.c:44
#define UNINIT
Definition: gps_mtk.c:48
#define LED_ON(i)
Definition: led_hw.h:28
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
#define MTK_DIY_OUTPUT_RATE
Definition: gps_mtk.c:44
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_MTK_ERR_NONE
Definition: gps_mtk.c:59
#define GPS_FIX_3D
Definition: gps.h:43
uint8_t msg_id
Definition: gps_mtk.h:36
#define OUTPUT_RATE
Definition: gps_mtk.c:45
int16_t week
GPS week.
Definition: gps.h:79
vector in Latitude, Longitude and Altitude
uint8_t msg_buf[GPS_MTK_MAX_PAYLOAD]
Definition: gps_mtk.h:35
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
Paparazzi floating point math for geodetic calculations.
static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time, int16_t *gps_week, uint32_t *gps_itow)
Definition: gps_mtk.c:117
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
#define GPS_MTK_ERR_CHECKSUM
Definition: gps_mtk.c:62
int32_t z
Down.
#define MTK_DIY_FIX_3D
Definition: gps_mtk.c:67
uint8_t zone
UTM zone number.
uint32_t t0_ticks
hw clock ticks when GPS message is received
Definition: gps.h:96
uint8_t error_cnt
Definition: gps_mtk.h:44
void gps_mtk_parse(uint8_t c)
Definition: gps_mtk.c:281
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
Architecture independent timing functions.
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:41
#define GPS_FIX_2D
Definition: gps.h:42
#define GOT_SYNC2_16
Definition: gps_mtk.c:53
Device independent GPS code (interface)
#define SECS_DAY
Definition: gps_mtk.c:75
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint8_t status
Definition: gps_mtk.h:39
#define GOT_PAYLOAD
Definition: gps_mtk.c:55
#define GPS_MTK_ERR_UNEXPECTED
Definition: gps_mtk.c:63
int32_t north
in centimeters
#define GOT_SYNC1_16
Definition: gps_mtk.c:52
uint8_t ck_b
Definition: gps_mtk.h:42
#define GOT_CHECKSUM1
Definition: gps_mtk.c:56
#define LED_TOGGLE(i)
Definition: led_hw.h:30
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:71
#define GPS_MTK_ERR_OUT_OF_SYNC
Definition: gps_mtk.c:64
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
bool_t msg_available
Definition: gps_mtk.h:34
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
void gps_impl_init(void)
GPS initialization.
Definition: gps_mtk.c:106
#define GOT_SYNC2_14
Definition: gps_mtk.c:50
int32_t east
in centimeters
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in degrees*1e7
#define LLA_FLOAT_OF_BFP(_o, _i)
#define SECS_HOUR
Definition: gps_mtk.c:74
uint8_t msg_class
Definition: gps_mtk.h:37
#define GOT_CLASS_14
Definition: gps_mtk.c:51
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:69
struct GpsTimeSync gps_time_sync
Definition: gps.c:43
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:86
arch independent LED (Light Emitting Diodes) API
#define GOT_ID
Definition: gps_mtk.c:54
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
uint8_t error_last
Definition: gps_mtk.h:45
#define SysTimeTimerStart(_t)
Definition: sys_time.h:198
signed char int8_t
Definition: types.h:15
#define isleap(x)
Definition: gps_mtk.c:78
const int8_t DAYS_MONTH[12]
Definition: gps_mtk.c:80
uint32_t t0_tow
GPS time of week in ms from last message.
Definition: gps.h:94
#define GOT_SYNC1_14
Definition: gps_mtk.c:49
uint8_t msg_idx
Definition: gps_mtk.h:41
float east
in meters
#define GPS_MTK_ERR_OVERRUN
Definition: gps_mtk.c:60
struct GpsState gps
global GPS state
Definition: gps.c:41
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
#define MTK_DIY_FIX_2D
Definition: gps_mtk.c:68
uint8_t num_sv
number of sat in fix
Definition: gps.h:77