39#include "pprzlink/pprzlink_device.h"
42#error "MTK_GPS_LINK not set"
45#define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ
57#define GOT_CHECKSUM1 8
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
68#define MTK_DIY_FIX_3D 3
69#define MTK_DIY_FIX_2D 2
70#define MTK_DIY_FIX_NONE 1
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)
79#define isleap(x) ((((x)%400)==0) || (!(((x)%100)==0) && (((x)%4)==0)))
81const int8_t DAYS_MONTH[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
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"
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"
95#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
97#define MTK_DIY_SBAS_ON "$PMTK313,1*2E\r\n"
98#define MTK_DIY_SBAS_OFF "$PMTK313,0*2F\r\n"
100#define MTK_DIY_WAAS_ON "$PSRF151,1*3F\r\n"
101#define MTK_DIY_WAAS_OFF "$PSRF151,0*3E\r\n"
104static uint8_t gps_status_config;
118 gps_status_config = 0;
119 gps_configuring =
true;
125 struct link_device *
dev = &((MTK_GPS_LINK).device);
127 while (
dev->char_available(
dev->periph)) {
144 if (
gps_mtk.msg_class == MTK_DIY14_ID &&
145 gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
152 if (
gps_mtk.msg_class == MTK_DIY16_ID &&
153 gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
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;
181 if (gps_month > 12) {
return; }
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; }
190 for (i = 1980; i < gps_year; i++) { days += (365 +
isleap(i)); }
193 for (i = 0; i < gps_month - 1; i++) {
200 *gps_itow = ((days % 7) *
SECS_DAY +
209 if (
gps_mtk.msg_class == MTK_DIY14_ID) {
210 if (
gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
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;
219 }
else {
gps_mtk.state.ned_vel.z = 0; }
227 gps_mtk.state.course = (RadOfDeg(MTK_DIY14_NAV_Heading(
gps_mtk.msg_buf))) * 10;
230 switch (MTK_DIY14_NAV_GPSfix(
gps_mtk.msg_buf)) {
253 if (
gps_mtk.msg_class == MTK_DIY16_ID) {
254 if (
gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
256 gps_date = MTK_DIY16_NAV_UTC_DATE(
gps_mtk.msg_buf);
257 gps_time = MTK_DIY16_NAV_UTC_TIME(
gps_mtk.msg_buf);
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;
271 }
else {
gps_mtk.state.ned_vel.z = 0; }
279 gps_mtk.state.course = (RadOfDeg(MTK_DIY16_NAV_Heading(
gps_mtk.msg_buf) * 10000)) * 10;
282 switch (MTK_DIY16_NAV_GPSfix(
gps_mtk.msg_buf)) {
313 if (c == MTK_DIY14_SYNC1) {
316 if (c == MTK_DIY16_ID) {
323 if (c != MTK_DIY14_SYNC2) {
335 gps_mtk.len = MTK_DIY14_NAV_LENGTH;
338 if (c != MTK_DIY14_ID) {
347 if (c != MTK_DIY14_NAV_ID) {
356 if (c != MTK_DIY16_NAV_ID) {
419#include "pprzlink/pprzlink_device.h"
421static void MtkSend_CFG(
char *dat)
423 struct link_device *
dev = &((MTK_GPS_LINK).device);
424 while (*dat != 0) {
dev->put_byte(
dev->periph, 0, *dat++); }
427void gps_configure_uart(
void)
431#ifdef USER_GPS_CONFIGURE
432#include USER_GPS_CONFIGURE
434static bool user_gps_configure(
bool cpt)
438 MtkSend_CFG(MTK_DIY_SET_BINARY);
450void gps_configure(
void)
455 gps_configuring = user_gps_configure(gps_status_config++);
Main include for ABI (AirBorneInterface).
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
#define GPS_FIX_NONE
No GPS fix.
#define GPS_VALID_HMSL_BIT
#define GPS_FIX_2D
2D GPS fix
#define GPS_FIX_3D
3D GPS fix
#define GPS_VALID_COURSE_BIT
void gps_mtk_read_message(void)
#define GPS_MTK_ERR_OUT_OF_SYNC
#define GPS_MTK_ERR_UNEXPECTED
#define GPS_MTK_ERR_CHECKSUM
const int8_t DAYS_MONTH[12]
void gps_mtk_parse(uint8_t c)
#define MTK_DIY_OUTPUT_RATE
#define GPS_MTK_ERR_OVERRUN
static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time, uint16_t *gps_week, uint32_t *gps_itow)
Mediatek MT3329 specific code.
arch independent LED (Light Emitting Diodes) API
#define UNINIT
Receiving pprz messages.
Architecture independent timing functions.
#define SysTimeTimerStart(_t)
volatile uint32_t nb_sec
full seconds since startup
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
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.