Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
33
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
81const int8_t DAYS_MONTH[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
82
83struct GpsMtk gps_mtk = {0};
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
103bool gps_configuring;
104static uint8_t gps_status_config;
105#endif
106
107void gps_mtk_read_message(void);
108void gps_mtk_parse(uint8_t c);
109void gps_mtk_msg(void);
110
111void gps_mtk_init(void)
112{
113 gps_mtk.status = UNINIT;
114 gps_mtk.msg_available = false;
115 gps_mtk.error_cnt = 0;
116 gps_mtk.error_last = GPS_MTK_ERR_NONE;
117#ifdef GPS_CONFIGURE
118 gps_status_config = 0;
119 gps_configuring = true;
120#endif
121}
122
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
136void gps_mtk_msg(void)
137{
138 // current timestamp
139 uint32_t now_ts = get_sys_time_usec();
140
141 gps_mtk.state.last_msg_ticks = sys_time.nb_sec_rem;
142 gps_mtk.state.last_msg_time = sys_time.nb_sec;
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) {
147 gps_mtk.state.last_3dfix_ticks = sys_time.nb_sec_rem;
148 gps_mtk.state.last_3dfix_time = sys_time.nb_sec;
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) {
155 gps_mtk.state.last_3dfix_ticks = sys_time.nb_sec_rem;
156 gps_mtk.state.last_3dfix_time = sys_time.nb_sec;
157 }
158 AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state);
159 }
160 gps_mtk.msg_available = false;
161}
162
163static 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;
214 SetBit(gps_mtk.state.valid_fields, GPS_VALID_POS_LLA_BIT);
215 // FIXME: with MTK you do not receive vertical speed
216 if (sys_time.nb_sec - gps_mtk.state.last_3dfix_time < 2) {
217 gps_mtk.state.ned_vel.z = ((gps_mtk.state.hmsl -
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;
221 SetBit(gps_mtk.state.valid_fields, GPS_VALID_HMSL_BIT);
222 // FIXME: with MTK you do not receive ellipsoid altitude
223 gps_mtk.state.lla_pos.alt = gps_mtk.state.hmsl;
224 gps_mtk.state.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
225 // FIXME: with MTK you do not receive speed 3D
226 gps_mtk.state.speed_3d = gps_mtk.state.gspeed;
227 gps_mtk.state.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10;
228 SetBit(gps_mtk.state.valid_fields, GPS_VALID_COURSE_BIT);
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:
232 gps_mtk.state.fix = GPS_FIX_3D;
233 break;
234 case MTK_DIY_FIX_2D:
235 gps_mtk.state.fix = GPS_FIX_2D;
236 break;
237 default:
238 gps_mtk.state.fix = GPS_FIX_NONE;
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 */
261 SysTimeTimerStart(gps_mtk.state.t0);
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
268 if (sys_time.nb_sec - gps_mtk.state.last_3dfix_time < 2) {
269 gps_mtk.state.ned_vel.z = ((gps_mtk.state.hmsl -
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;
273 SetBit(gps_mtk.state.valid_fields, GPS_VALID_HMSL_BIT);
274 // FIXME: with MTK you do not receive ellipsoid altitude
275 gps_mtk.state.lla_pos.alt = gps_mtk.state.hmsl;
276 gps_mtk.state.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
277 // FIXME: with MTK you do not receive speed 3D
278 gps_mtk.state.speed_3d = gps_mtk.state.gspeed;
279 gps_mtk.state.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10;
280 SetBit(gps_mtk.state.valid_fields, GPS_VALID_COURSE_BIT);
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:
284 gps_mtk.state.fix = GPS_FIX_3D;
285 break;
286 case MTK_DIY_FIX_2D:
287 gps_mtk.state.fix = GPS_FIX_2D;
288 break;
289 default:
290 gps_mtk.state.fix = GPS_FIX_NONE;
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;
309 gps_mtk.ck_b += gps_mtk.ck_a;
310 }
311 switch (gps_mtk.status) {
312 case UNINIT:
313 if (c == MTK_DIY14_SYNC1) {
314 gps_mtk.status = GOT_SYNC1_14;
315 }
316 if (c == MTK_DIY16_ID) {
317 gps_mtk.msg_class = c;
318 }
319 gps_mtk.status = GOT_SYNC1_16;
320 break;
321 /* MTK_DIY_VER_14 */
322 case GOT_SYNC1_14:
323 if (c != MTK_DIY14_SYNC2) {
324 gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
325 goto error;
326 }
327 if (gps_mtk.msg_available) {
328 /* Previous message has not yet been parsed: discard this one */
329 gps_mtk.error_last = GPS_MTK_ERR_OVERRUN;
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) {
339 gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
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) {
348 gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
349 goto error;
350 }
351 gps_mtk.msg_id = c;
352 gps_mtk.status = GOT_ID;
353 break;
354 /* MTK_DIY_VER_16 */
355 case GOT_SYNC1_16:
356 if (c != MTK_DIY16_NAV_ID) {
357 gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
358 goto error;
359 }
360 if (gps_mtk.msg_available) {
361 /* Previous message has not yet been parsed: discard this one */
362 gps_mtk.error_last = GPS_MTK_ERR_OVERRUN;
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;
373 gps_mtk.status = GOT_ID;
374 break;
375 case GOT_ID:
376 gps_mtk.msg_buf[gps_mtk.msg_idx] = c;
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) {
384 gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM;
385 goto error;
386 }
387 gps_mtk.status++;
388 break;
389 case GOT_CHECKSUM1:
390 if (c != gps_mtk.ck_b) {
391 gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM;
392 goto error;
393 }
394 gps_mtk.msg_available = true;
395 goto restart;
396 break;
397 default:
398 gps_mtk.error_last = GPS_MTK_ERR_UNEXPECTED;
399 goto error;
400 }
401 return;
402error:
403 gps_mtk.error_cnt++;
404restart:
405 gps_mtk.status = UNINIT;
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
421static 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
427void gps_configure_uart(void)
428{
429}
430
431#ifdef USER_GPS_CONFIGURE
432#include USER_GPS_CONFIGURE
433#else
434static 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
450void 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.
static struct uart_periph * dev
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_2D
2D GPS fix
Definition gps.h:43
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
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 GPS_MTK_ERR_UNEXPECTED
Definition gps_mtk.c:64
#define MTK_DIY_FIX_3D
Definition gps_mtk.c:68
#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.
#define GpsConfigure()
Definition gps_mtk.h:85
arch independent LED (Light Emitting Diodes) API
#define GOT_PAYLOAD
Definition protocol.c:15
#define UNINIT
Receiving pprz messages.
Definition protocol.c:11
Architecture independent timing functions.
#define SysTimeTimerStart(_t)
Definition sys_time.h:329
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.