Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "gps_mtk.h"
35 #include "modules/core/abi.h"
36 #include "led.h"
37 
38 #include "mcu_periph/sys_time.h"
39 #include "pprzlink/pprzlink_device.h"
40 
41 #ifndef MTK_GPS_LINK
42 #error "MTK_GPS_LINK not set"
43 #endif
44 
45 #define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ
46 #define OUTPUT_RATE 4
47 
48 /* parser status */
49 #define UNINIT 0
50 #define GOT_SYNC1_14 1
51 #define GOT_SYNC2_14 2
52 #define GOT_CLASS_14 3
53 #define GOT_SYNC1_16 4
54 #define GOT_SYNC2_16 5
55 #define GOT_ID 6
56 #define GOT_PAYLOAD 7
57 #define GOT_CHECKSUM1 8
58 
59 /* last error type */
60 #define GPS_MTK_ERR_NONE 0
61 #define GPS_MTK_ERR_OVERRUN 1
62 #define GPS_MTK_ERR_MSG_TOO_LONG 2
63 #define GPS_MTK_ERR_CHECKSUM 3
64 #define GPS_MTK_ERR_UNEXPECTED 4
65 #define GPS_MTK_ERR_OUT_OF_SYNC 5
66 
67 /* mediatek gps fix mask */
68 #define MTK_DIY_FIX_3D 3
69 #define MTK_DIY_FIX_2D 2
70 #define MTK_DIY_FIX_NONE 1
71 
72 
73 /* defines for UTC-GPS time conversion */
74 #define SECS_MINUTE (60)
75 #define SECS_HOUR (60*60)
76 #define SECS_DAY (60*60*24)
77 #define SECS_WEEK (60*60*24*7)
78 
79 #define isleap(x) ((((x)%400)==0) || (!(((x)%100)==0) && (((x)%4)==0)))
80 
81 const int8_t DAYS_MONTH[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
82 
83 struct GpsMtk gps_mtk;
84 
85 #ifdef GPS_CONFIGURE
86 #define MTK_DIY_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
87 #define MTK_DIY_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n"
88 
89 #define MTK_DIY_OUTPUT_1HZ "$PMTK220,1000*1F\r\n"
90 #define MTK_DIY_OUTPUT_2HZ "$PMTK220,500*2B\r\n"
91 #define MTK_DIY_OUTPUT_4HZ "$PMTK220,250*29\r\n"
92 #define MTK_DIY_OTUPUT_5HZ "$PMTK220,200*2C\r\n"
93 #define MTK_DIY_OUTPUT_10HZ "$PMTK220,100*2F\r\n"
94 
95 #define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
96 
97 #define MTK_DIY_SBAS_ON "$PMTK313,1*2E\r\n"
98 #define MTK_DIY_SBAS_OFF "$PMTK313,0*2F\r\n"
99 
100 #define MTK_DIY_WAAS_ON "$PSRF151,1*3F\r\n"
101 #define MTK_DIY_WAAS_OFF "$PSRF151,0*3E\r\n"
102 
103 bool gps_configuring;
104 static uint8_t gps_status_config;
105 #endif
106 
107 void gps_mtk_read_message(void);
108 void gps_mtk_parse(uint8_t c);
109 void gps_mtk_msg(void);
110 
111 void gps_mtk_init(void)
112 {
114  gps_mtk.msg_available = false;
115  gps_mtk.error_cnt = 0;
117 #ifdef GPS_CONFIGURE
118  gps_status_config = 0;
119  gps_configuring = true;
120 #endif
121 }
122 
123 void gps_mtk_event(void)
124 {
125  struct link_device *dev = &((MTK_GPS_LINK).device);
126 
127  while (dev->char_available(dev->periph)) {
128  gps_mtk_parse(dev->get_byte(dev->periph));
129  if (gps_mtk.msg_available) {
130  gps_mtk_msg();
131  }
132  GpsConfigure();
133  }
134 }
135 
136 void gps_mtk_msg(void)
137 {
138  // current timestamp
139  uint32_t now_ts = get_sys_time_usec();
140 
144  if (gps_mtk.msg_class == MTK_DIY14_ID &&
145  gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
146  if (gps_mtk.state.fix == GPS_FIX_3D) {
149  }
150  AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state);
151  }
152  if (gps_mtk.msg_class == MTK_DIY16_ID &&
153  gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
154  if (gps_mtk.state.fix == GPS_FIX_3D) {
157  }
158  AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state);
159  }
160  gps_mtk.msg_available = false;
161 }
162 
163 static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time,
164  uint16_t *gps_week, uint32_t *gps_itow)
165 {
166  /* convert UTC date/time to GPS week/itow, we have no idea about GPS
167  leap seconds for now */
168  uint16_t gps_msecond = gps_time % 1000;
169  uint8_t gps_second = (gps_time / 1000) % 100;
170  uint8_t gps_minute = (gps_time / 100000) % 100;
171  uint8_t gps_hour = (gps_time / 10000000) % 100;
172  uint16_t gps_year = 2000 + (gps_date % 100);
173  uint8_t gps_month = (gps_date / 100) % 100;
174  uint8_t gps_day = (gps_date / 10000) % 100;
175  int32_t i, days;
176 
177  *gps_week = 0;
178  *gps_itow = 0;
179 
180  /* sanity checks */
181  if (gps_month > 12) { return; }
182  if (gps_day > (DAYS_MONTH[gps_month] +
183  ((gps_month == 1) ? isleap(gps_year) : 0))) { return; }
184  if (gps_hour > 23) { return; }
185  if (gps_minute > 59) { return; }
186  if (gps_second > 59) { return; }
187 
188  /* days since 6-JAN-1980 */
189  days = -6;
190  for (i = 1980; i < gps_year; i++) { days += (365 + isleap(i)); }
191 
192  /* add days in gps_year */
193  for (i = 0; i < gps_month - 1; i++) {
194  days += DAYS_MONTH[i] + ((i == 1) ? isleap(gps_year) : 0);
195  }
196  days += gps_day;
197 
198  /* convert */
199  *gps_week = (uint16_t)(days / 7);
200  *gps_itow = ((days % 7) * SECS_DAY +
201  gps_hour * SECS_HOUR +
202  gps_minute * SECS_MINUTE +
203  gps_second) * 1000 +
204  gps_msecond;
205 }
206 
208 {
209  if (gps_mtk.msg_class == MTK_DIY14_ID) {
210  if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
211  /* get hardware clock ticks */
212  gps_mtk.state.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10;
213  gps_mtk.state.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10;
215  // FIXME: with MTK you do not receive vertical speed
218  MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
219  } else { gps_mtk.state.ned_vel.z = 0; }
220  gps_mtk.state.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
222  // FIXME: with MTK you do not receive ellipsoid altitude
224  gps_mtk.state.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
225  // FIXME: with MTK you do not receive speed 3D
227  gps_mtk.state.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10;
229  gps_mtk.state.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
230  switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
231  case MTK_DIY_FIX_3D:
233  break;
234  case MTK_DIY_FIX_2D:
236  break;
237  default:
239  }
240  gps_mtk.state.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
241  // FIXME: with MTK DIY 1.4 you do not receive GPS week
242  gps_mtk.state.week = 0;
243 #ifdef GPS_LED
244  if (gps_mtk.state.fix == GPS_FIX_3D) {
245  LED_ON(GPS_LED);
246  } else {
247  LED_TOGGLE(GPS_LED);
248  }
249 #endif
250  }
251  }
252 
253  if (gps_mtk.msg_class == MTK_DIY16_ID) {
254  if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
255  uint32_t gps_date, gps_time;
256  gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
257  gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
258  gps_mtk_time2itow(gps_date, gps_time, &gps_mtk.state.week, &gps_mtk.state.tow);
259 #ifdef GPS_TIMESTAMP
260  /* get hardware clock ticks */
262  gps_mtk.state.t0_tow = gps_mtk.state.tow;
263  gps_mtk.state.t0_tow_frac = 0;
264 #endif
265  gps_mtk.state.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10;
266  gps_mtk.state.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10;
267  // FIXME: with MTK you do not receive vertical speed
270  MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
271  } else { gps_mtk.state.ned_vel.z = 0; }
272  gps_mtk.state.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
274  // FIXME: with MTK you do not receive ellipsoid altitude
276  gps_mtk.state.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
277  // FIXME: with MTK you do not receive speed 3D
279  gps_mtk.state.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10;
281  gps_mtk.state.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
282  switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
283  case MTK_DIY_FIX_3D:
285  break;
286  case MTK_DIY_FIX_2D:
288  break;
289  default:
291  }
292  /* HDOP? */
293 #ifdef GPS_LED
294  if (gps_mtk.state.fix == GPS_FIX_3D) {
295  LED_ON(GPS_LED);
296  } else {
297  LED_TOGGLE(GPS_LED);
298  }
299 #endif
300  }
301  }
302 }
303 
304 /* byte parsing */
306 {
307  if (gps_mtk.status < GOT_PAYLOAD) {
308  gps_mtk.ck_a += c;
310  }
311  switch (gps_mtk.status) {
312  case UNINIT:
313  if (c == MTK_DIY14_SYNC1) {
315  }
316  if (c == MTK_DIY16_ID) {
317  gps_mtk.msg_class = c;
318  }
320  break;
321  /* MTK_DIY_VER_14 */
322  case GOT_SYNC1_14:
323  if (c != MTK_DIY14_SYNC2) {
325  goto error;
326  }
327  if (gps_mtk.msg_available) {
328  /* Previous message has not yet been parsed: discard this one */
330  goto error;
331  }
332  gps_mtk.ck_a = 0;
333  gps_mtk.ck_b = 0;
334  gps_mtk.status++;
335  gps_mtk.len = MTK_DIY14_NAV_LENGTH;
336  break;
337  case GOT_SYNC2_14:
338  if (c != MTK_DIY14_ID) {
340  goto error;
341  }
342  gps_mtk.msg_class = c;
343  gps_mtk.msg_idx = 0;
344  gps_mtk.status++;
345  break;
346  case GOT_CLASS_14:
347  if (c != MTK_DIY14_NAV_ID) {
349  goto error;
350  }
351  gps_mtk.msg_id = c;
353  break;
354  /* MTK_DIY_VER_16 */
355  case GOT_SYNC1_16:
356  if (c != MTK_DIY16_NAV_ID) {
358  goto error;
359  }
360  if (gps_mtk.msg_available) {
361  /* Previous message has not yet been parsed: discard this one */
363  goto error;
364  }
365  gps_mtk.msg_id = c;
366  gps_mtk.ck_a = 0;
367  gps_mtk.ck_b = 0;
368  gps_mtk.status++;
369  break;
370  case GOT_SYNC2_16:
371  gps_mtk.len = c;
372  gps_mtk.msg_idx = 0;
374  break;
375  case GOT_ID:
377  gps_mtk.msg_idx++;
378  if (gps_mtk.msg_idx >= gps_mtk.len) {
379  gps_mtk.status++;
380  }
381  break;
382  case GOT_PAYLOAD:
383  if (c != gps_mtk.ck_a) {
385  goto error;
386  }
387  gps_mtk.status++;
388  break;
389  case GOT_CHECKSUM1:
390  if (c != gps_mtk.ck_b) {
392  goto error;
393  }
394  gps_mtk.msg_available = true;
395  goto restart;
396  break;
397  default:
399  goto error;
400  }
401  return;
402 error:
403  gps_mtk.error_cnt++;
404 restart:
406  return;
407 }
408 
409 
410 /*
411  *
412  *
413  * GPS dynamic configuration
414  *
415  *
416  */
417 #ifdef GPS_CONFIGURE
418 
419 #include "pprzlink/pprzlink_device.h"
420 
421 static void MtkSend_CFG(char *dat)
422 {
423  struct link_device *dev = &((MTK_GPS_LINK).device);
424  while (*dat != 0) { dev->put_byte(dev->periph, 0, *dat++); }
425 }
426 
427 void gps_configure_uart(void)
428 {
429 }
430 
431 #ifdef USER_GPS_CONFIGURE
432 #include USER_GPS_CONFIGURE
433 #else
434 static bool user_gps_configure(bool cpt)
435 {
436  switch (cpt) {
437  case 0:
438  MtkSend_CFG(MTK_DIY_SET_BINARY);
439  break;
440  case 1:
441  MtkSend_CFG(MTK_DIY_OUTPUT_RATE);
442  return false;
443  default:
444  break;
445  }
446  return true; /* Continue, except for the last case */
447 }
448 #endif // ! USER_GPS_CONFIGURE
449 
450 void gps_configure(void)
451 {
452  static uint32_t count = 0;
453  /* start configuring after having received 50 bytes */
454  if (count++ > 50) {
455  gps_configuring = user_gps_configure(gps_status_config++);
456  }
457 }
458 
459 #endif /* GPS_CONFIGURE */
Main include for ABI (AirBorneInterface).
#define GPS_MTK_ID
#define LED_ON(i)
Definition: led_hw.h:51
#define LED_TOGGLE(i)
Definition: led_hw.h:53
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
uint32_t tow
GPS time of week in ms.
Definition: gps.h:107
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:90
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:97
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:47
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:112
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:40
#define GPS_VALID_HMSL_BIT
Definition: gps.h:51
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:94
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:115
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:113
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:86
#define GPS_FIX_2D
2D GPS fix
Definition: gps.h:41
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:96
#define GPS_VALID_COURSE_BIT
Definition: gps.h:52
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:114
uint8_t num_sv
number of sat in fix
Definition: gps.h:104
uint16_t week
GPS week.
Definition: gps.h:106
uint8_t fix
status of fix
Definition: gps.h:105
void gps_mtk_msg(void)
Definition: gps_mtk.c:136
#define GOT_SYNC1_16
Definition: gps_mtk.c:53
#define GOT_SYNC2_14
Definition: gps_mtk.c:51
void gps_mtk_read_message(void)
Definition: gps_mtk.c:207
#define isleap(x)
Definition: gps_mtk.c:79
#define MTK_DIY_FIX_2D
Definition: gps_mtk.c:69
#define SECS_HOUR
Definition: gps_mtk.c:75
void gps_mtk_event(void)
Definition: gps_mtk.c:123
#define GPS_MTK_ERR_OUT_OF_SYNC
Definition: gps_mtk.c:65
#define GOT_CLASS_14
Definition: gps_mtk.c:52
#define SECS_DAY
Definition: gps_mtk.c:76
#define GOT_PAYLOAD
Definition: gps_mtk.c:56
#define GPS_MTK_ERR_UNEXPECTED
Definition: gps_mtk.c:64
#define MTK_DIY_FIX_3D
Definition: gps_mtk.c:68
#define UNINIT
Definition: gps_mtk.c:49
#define GOT_CHECKSUM1
Definition: gps_mtk.c:57
#define GOT_ID
Definition: gps_mtk.c:55
#define SECS_MINUTE
Definition: gps_mtk.c:74
#define GPS_MTK_ERR_CHECKSUM
Definition: gps_mtk.c:63
void gps_mtk_init(void)
Definition: gps_mtk.c:111
const int8_t DAYS_MONTH[12]
Definition: gps_mtk.c:81
#define OUTPUT_RATE
Definition: gps_mtk.c:46
void gps_mtk_parse(uint8_t c)
Definition: gps_mtk.c:305
struct GpsMtk gps_mtk
Definition: gps_mtk.c:83
#define GOT_SYNC2_16
Definition: gps_mtk.c:54
#define MTK_DIY_OUTPUT_RATE
Definition: gps_mtk.c:45
#define GPS_MTK_ERR_OVERRUN
Definition: gps_mtk.c:61
static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time, uint16_t *gps_week, uint32_t *gps_itow)
Definition: gps_mtk.c:163
#define GPS_MTK_ERR_NONE
Definition: gps_mtk.c:60
#define GOT_SYNC1_14
Definition: gps_mtk.c:50
Mediatek MT3329 specific code.
uint8_t msg_class
Definition: gps_mtk.h:53
uint8_t error_last
Definition: gps_mtk.h:61
uint8_t msg_idx
Definition: gps_mtk.h:57
uint8_t msg_id
Definition: gps_mtk.h:52
bool msg_available
Definition: gps_mtk.h:50
uint8_t error_cnt
Definition: gps_mtk.h:60
uint8_t ck_a
Definition: gps_mtk.h:58
uint8_t status
Definition: gps_mtk.h:55
uint16_t len
Definition: gps_mtk.h:56
struct GpsState state
Definition: gps_mtk.h:66
uint8_t ck_b
Definition: gps_mtk.h:58
#define GpsConfigure()
Definition: gps_mtk.h:85
uint8_t msg_buf[GPS_MTK_MAX_PAYLOAD]
Definition: gps_mtk.h:51
Definition: gps_mtk.h:49
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t lon
in degrees*1e7
arch independent LED (Light Emitting Diodes) API
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
#define SysTimeTimerStart(_t)
Definition: sys_time.h:227
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103