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_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 #ifdef GPS_TIMESTAMP
164  /* get hardware clock ticks */
166  gps.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
167  gps.t0_tow_frac = 0;
168 #endif
169  gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10;
170  gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10;
171  // FIXME: with MTK you do not receive vertical speed
172  if (sys_time.nb_sec - gps.last_fix_time < 2) {
173  gps.ned_vel.z = ((gps.hmsl -
174  MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
175  } else gps.ned_vel.z = 0;
176  gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10;
177  // FIXME: with MTK you do not receive ellipsoid altitude
178  gps.lla_pos.alt = gps.hmsl;
179  gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
180  // FIXME: with MTK you do not receive speed 3D
182  gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
183  gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
184  switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
185  case MTK_DIY_FIX_3D:
186  gps.fix = GPS_FIX_3D;
187  break;
188  case MTK_DIY_FIX_2D:
189  gps.fix = GPS_FIX_2D;
190  break;
191  default:
192  gps.fix = GPS_FIX_NONE;
193  }
194  gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
195  // FIXME: with MTK DIY 1.4 you do not receive GPS week
196  gps.week = 0;
197  /* Computes from (lat, long) in the referenced UTM zone */
198  struct LlaCoor_f lla_f;
199  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
200  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
201  struct UtmCoor_f utm_f;
202  utm_f.zone = nav_utm_zone0;
203  /* convert to utm */
204  utm_of_lla_f(&utm_f, &lla_f);
205  /* copy results of utm conversion */
206  gps.utm_pos.east = utm_f.east*100;
207  gps.utm_pos.north = utm_f.north*100;
210 #ifdef GPS_LED
211  if (gps.fix == GPS_FIX_3D) {
212  LED_ON(GPS_LED);
213  }
214  else {
215  LED_TOGGLE(GPS_LED);
216  }
217 #endif
218  }
219  }
220 
221  if (gps_mtk.msg_class == MTK_DIY16_ID) {
222  if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
223  uint32_t gps_date, gps_time;
224  gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
225  gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
226  gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow);
227 #ifdef GPS_TIMESTAMP
228  /* get hardware clock ticks */
230  gps.t0_tow = gps.tow;
231  gps.t0_tow_frac = 0;
232 #endif
233  gps.lla_pos.lat = RadOfDeg(MTK_DIY16_NAV_LAT(gps_mtk.msg_buf))*10;
234  gps.lla_pos.lon = RadOfDeg(MTK_DIY16_NAV_LON(gps_mtk.msg_buf))*10;
235  // FIXME: with MTK you do not receive vertical speed
236  if (sys_time.nb_sec - gps.last_fix_time < 2) {
237  gps.ned_vel.z = ((gps.hmsl -
238  MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
239  } else gps.ned_vel.z = 0;
240  gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10;
241  // FIXME: with MTK you do not receive ellipsoid altitude
242  gps.lla_pos.alt = gps.hmsl;
243  gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
244  // FIXME: with MTK you do not receive speed 3D
246  gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf)*10000)) * 10;
247  gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
248  switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
249  case MTK_DIY_FIX_3D:
250  gps.fix = GPS_FIX_3D;
251  break;
252  case MTK_DIY_FIX_2D:
253  gps.fix = GPS_FIX_2D;
254  break;
255  default:
256  gps.fix = GPS_FIX_NONE;
257  }
258  /* HDOP? */
259  /* Computes from (lat, long) in the referenced UTM zone */
260  struct LlaCoor_f lla_f;
261  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
262  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
263  struct UtmCoor_f utm_f;
264  utm_f.zone = nav_utm_zone0;
265  /* convert to utm */
266  utm_of_lla_f(&utm_f, &lla_f);
267  /* copy results of utm conversion */
268  gps.utm_pos.east = utm_f.east*100;
269  gps.utm_pos.north = utm_f.north*100;
272 #ifdef GPS_LED
273  if (gps.fix == GPS_FIX_3D) {
274  LED_ON(GPS_LED);
275  }
276  else {
277  LED_TOGGLE(GPS_LED);
278  }
279 #endif
280  }
281  }
282 }
283 
284 /* byte parsing */
286  if (gps_mtk.status < GOT_PAYLOAD) {
287  gps_mtk.ck_a += c;
289  }
290  switch (gps_mtk.status) {
291  case UNINIT:
292  if (c == MTK_DIY14_SYNC1)
294  if (c == MTK_DIY16_ID)
295  gps_mtk.msg_class = c;
297  break;
298  /* MTK_DIY_VER_14 */
299  case GOT_SYNC1_14:
300  if (c != MTK_DIY14_SYNC2) {
302  goto error;
303  }
304  if (gps_mtk.msg_available) {
305  /* Previous message has not yet been parsed: discard this one */
307  goto error;
308  }
309  gps_mtk.ck_a = 0;
310  gps_mtk.ck_b = 0;
311  gps_mtk.status++;
312  gps_mtk.len = MTK_DIY14_NAV_LENGTH;
313  break;
314  case GOT_SYNC2_14:
315  if (c != MTK_DIY14_ID) {
317  goto error;
318  }
319  gps_mtk.msg_class = c;
320  gps_mtk.msg_idx = 0;
321  gps_mtk.status++;
322  break;
323  case GOT_CLASS_14:
324  if (c != MTK_DIY14_NAV_ID) {
326  goto error;
327  }
328  gps_mtk.msg_id = c;
330  break;
331  /* MTK_DIY_VER_16 */
332  case GOT_SYNC1_16:
333  if (c != MTK_DIY16_NAV_ID) {
335  goto error;
336  }
337  if (gps_mtk.msg_available) {
338  /* Previous message has not yet been parsed: discard this one */
340  goto error;
341  }
342  gps_mtk.msg_id = c;
343  gps_mtk.ck_a = 0;
344  gps_mtk.ck_b = 0;
345  gps_mtk.status++;
346  break;
347  case GOT_SYNC2_16:
348  gps_mtk.len = c;
349  gps_mtk.msg_idx = 0;
351  break;
352  case GOT_ID:
353  gps_mtk.msg_buf[gps_mtk.msg_idx] = c;
354  gps_mtk.msg_idx++;
355  if (gps_mtk.msg_idx >= gps_mtk.len) {
356  gps_mtk.status++;
357  }
358  break;
359  case GOT_PAYLOAD:
360  if (c != gps_mtk.ck_a) {
362  goto error;
363  }
364  gps_mtk.status++;
365  break;
366  case GOT_CHECKSUM1:
367  if (c != gps_mtk.ck_b) {
369  goto error;
370  }
372  goto restart;
373  break;
374  default:
376  goto error;
377  }
378  return;
379  error:
380  gps_mtk.error_cnt++;
381  restart:
383  return;
384 }
385 
386 
387 /*
388  *
389  *
390  * GPS dynamic configuration
391  *
392  *
393  */
394 #ifdef GPS_CONFIGURE
395 
396 static void MtkSend_CFG(char* dat) {
397  while (*dat != 0) GpsLink(Transmit(*dat++));
398 }
399 
400 void gps_configure_uart(void) {
401 }
402 
403 #ifdef USER_GPS_CONFIGURE
404 #include USER_GPS_CONFIGURE
405 #else
406 static bool_t user_gps_configure(bool_t cpt) {
407  switch (cpt) {
408  case 0:
409  MtkSend_CFG(MTK_DIY_SET_BINARY);
410  break;
411  case 1:
412  MtkSend_CFG(MTK_DIY_OUTPUT_RATE);
413  return FALSE;
414  default:
415  break;
416  }
417  return TRUE; /* Continue, except for the last case */
418 }
419 #endif // ! USER_GPS_CONFIGURE
420 
421 void gps_configure( void ) {
422  static uint32_t count=0;
423  /* start configuring after having received 50 bytes */
424  if (count++ > 50)
425  gps_configuring = user_gps_configure(gps_status_config++);
426 }
427 
428 #endif /* GPS_CONFIGURE */
unsigned short uint16_t
Definition: types.h:16
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
uint16_t len
Definition: gps_mtk.h:40
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
Definition: gps_mtk.h:33
int32_t lat
in radians*1e7
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
#define SECS_MINUTE
Definition: gps_mtk.c:73
uint32_t last_fix_time
cpu time in sec at last valid fix
Definition: gps.h:85
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.
#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:43
#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:66
float lat
in radians
#define MTK_DIY_OUTPUT_RATE
Definition: gps_mtk.c:44
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_MTK_ERR_NONE
Definition: gps_mtk.c:59
#define GPS_FIX_3D
Definition: gps.h:42
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:78
vector in Latitude, Longitude and Altitude
#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:79
#define GPS_MTK_ERR_CHECKSUM
Definition: gps_mtk.c:62
#define MTK_DIY_FIX_3D
Definition: gps_mtk.c:67
uint8_t zone
UTM zone number.
uint8_t error_cnt
Definition: gps_mtk.h:44
void gps_mtk_parse(uint8_t c)
Definition: gps_mtk.c:285
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
Architecture independent timing functions.
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:40
#define GPS_FIX_2D
Definition: gps.h:41
#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
#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:70
void gps_impl_init(void)
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 radians*1e7
#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:55
arch independent LED (Light Emitting Diodes) API
#define SysTimeTimerStart(_t)
Definition: sys_time_arch.h:70
#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:65
uint8_t error_last
Definition: gps_mtk.h:45
float lon
in radians
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
static struct point c
Definition: discsurvey.c:13
#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:31
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:76