diff --git a/Makefile b/Makefile
index cb9eec3f45..239dec6985 100644
--- a/Makefile
+++ b/Makefile
@@ -52,11 +52,13 @@ STATICINCLUDE =$(PAPARAZZI_HOME)/var/include
MESSAGES_H=$(STATICINCLUDE)/messages.h
MESSAGES2_H=$(STATICINCLUDE)/messages2.h
UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h
+MTK_PROTOCOL_H=$(STATICINCLUDE)/mtk_protocol.h
XSENS_PROTOCOL_H=$(STATICINCLUDE)/xsens_protocol.h
DL_PROTOCOL_H=$(STATICINCLUDE)/dl_protocol.h
DL_PROTOCOL2_H=$(STATICINCLUDE)/dl_protocol2.h
MESSAGES_XML = $(CONF)/messages.xml
UBX_XML = $(CONF)/ubx.xml
+MTK_XML = $(CONF)/mtk.xml
XSENS_XML = $(CONF)/xsens_MTi-G.xml
TOOLS=$(PAPARAZZI_SRC)/sw/tools
HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc)
@@ -108,7 +110,7 @@ misc:
multimon:
cd $(MULTIMON); $(MAKE)
-static_h: $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(DL_PROTOCOL2_H)
+static_h: $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(DL_PROTOCOL2_H)
usb_lib:
@[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x "$(ARMGCC)" && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing"
@@ -132,6 +134,11 @@ $(UBX_PROTOCOL_H) : $(UBX_XML) tools
$(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(TOOLS)/gen_ubx.out $< > /tmp/ubx.h
$(Q)mv /tmp/ubx.h $@
+$(MTK_PROTOCOL_H) : $(MTK_XML) tools
+ @echo BUILD $@
+ $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(TOOLS)/gen_mtk.out $< > /tmp/mtk.h
+ $(Q)mv /tmp/mtk.h $@
+
$(XSENS_PROTOCOL_H) : $(XSENS_XML) tools
@echo BUILD $@
$(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(TOOLS)/gen_xsens.out $< > /tmp/xsens.h
@@ -215,7 +222,7 @@ fast_deb:
clean:
rm -fr dox build-stamp configure-stamp conf/%gconf.xml debian/files debian/paparazzi-arm7 debian/paparazzi-avr debian/paparazzi-base debian/paparazzi-bin debian/paparazzi-dev
- rm -f $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(DL_PROTOCOL_H)
+ rm -f $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(DL_PROTOCOL_H)
find . -mindepth 2 -name Makefile -exec sh -c '$(MAKE) -C `dirname {}` $@' \;
find . -name '*~' -exec rm -f {} \;
rm -f paparazzi sw/simulator/launchsitl
diff --git a/Makefile.install b/Makefile.install
index 1acf59704f..fd2aa46350 100644
--- a/Makefile.install
+++ b/Makefile.install
@@ -134,6 +134,7 @@ install_tools:
$(INSTALLDATA) sw/tools/gen_settings.ml $(DESTDIR)/sw/tools/
$(INSTALLDATA) sw/tools/gen_tuning.ml $(DESTDIR)/sw/tools/
$(INSTALLDATA) sw/tools/gen_ubx.ml $(DESTDIR)/sw/tools/
+ $(INSTALLDATA) sw/tools/gen_mtk.ml $(DESTDIR)/sw/tools/
$(INSTALLDATA) sw/tools/gen_xsens.ml $(DESTDIR)/sw/tools/
$(INSTALLDATA) sw/tools/gen_modules.ml $(DESTDIR)/sw/tools/
$(INSTALLDATA) sw/tools/gen_common.cmo $(DESTDIR)/sw/tools/
diff --git a/conf/airframes/mm/fixed-wing/funjetmm.xml b/conf/airframes/mm/fixed-wing/funjetmm.xml
index 7dee6dd558..17fd3c558a 100644
--- a/conf/airframes/mm/fixed-wing/funjetmm.xml
+++ b/conf/airframes/mm/fixed-wing/funjetmm.xml
@@ -37,7 +37,7 @@
-
+
diff --git a/conf/autopilot/fixedwing.xml b/conf/autopilot/fixedwing.xml
index f65f6282b1..ad2527f967 100644
--- a/conf/autopilot/fixedwing.xml
+++ b/conf/autopilot/fixedwing.xml
@@ -34,7 +34,7 @@
-
+
diff --git a/conf/autopilot/subsystems/fixedwing/gps_mediatek_diy.makefile b/conf/autopilot/subsystems/fixedwing/gps_mediatek_diy.makefile
new file mode 100644
index 0000000000..5b11c32b8d
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/gps_mediatek_diy.makefile
@@ -0,0 +1,23 @@
+# Mediatek MT3329, DIYDrones V1.4/1.6 protocol
+
+
+ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE -DGPS_USE_LATLONG
+ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
+ap.CFLAGS += -DUSE_$(GPS_PORT)
+ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+
+ifneq ($(GPS_LED),none)
+ ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
diff --git a/conf/mtk.dtd b/conf/mtk.dtd
new file mode 100644
index 0000000000..73fe522085
--- /dev/null
+++ b/conf/mtk.dtd
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/mtk.xml b/conf/mtk.xml
new file mode 100644
index 0000000000..201ba3b60a
--- /dev/null
+++ b/conf/mtk.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index 21a30c6bb1..c8121322b8 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -38,6 +38,7 @@
#endif
#define GPS_FIX_NONE 0x00
+#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GpsFixValid() (gps.fix == GPS_FIX_3D)
diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c
new file mode 100644
index 0000000000..812521670c
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_mtk.c
@@ -0,0 +1,401 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file gps_mtk.h
+ * @brief Mediatek MT3329 specific code
+ *
+ * supports:
+ * DIYDrones V1.4 protocol (AXN1.30_2278)
+ * DIYDrones V1.6 protocol (AXN1.30_2389)
+ *
+ * documentation is partly incorrect, see mtk.xml for what seems
+ * to be "real"
+ *
+ */
+
+#include "subsystems/gps.h"
+
+#include "led.h"
+
+#include "subsystems/nav.h"
+#include "math/pprz_geodetic_float.h"
+#include "sys_time.h"
+
+#define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ
+#define OUTPUT_RATE 4
+
+/* parser status */
+#define UNINIT 0
+#define GOT_SYNC1_14 1
+#define GOT_SYNC2_14 2
+#define GOT_CLASS_14 3
+#define GOT_SYNC1_16 4
+#define GOT_SYNC2_16 5
+#define GOT_ID 6
+#define GOT_PAYLOAD 7
+#define GOT_CHECKSUM1 8
+
+/* last error type */
+#define GPS_MTK_ERR_NONE 0
+#define GPS_MTK_ERR_OVERRUN 1
+#define GPS_MTK_ERR_MSG_TOO_LONG 2
+#define GPS_MTK_ERR_CHECKSUM 3
+#define GPS_MTK_ERR_UNEXPECTED 4
+#define GPS_MTK_ERR_OUT_OF_SYNC 5
+
+/* defines for UTC-GPS time conversion */
+#define SECS_MINUTE (60)
+#define SECS_HOUR (60*60)
+#define SECS_DAY (60*60*24)
+#define SECS_WEEK (60*60*24*7)
+
+#define isleap(x) ((((x)%400)==0) || (!(((x)%100)==0) && (((x)%4)==0)))
+
+const int8_t DAYS_MONTH[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
+
+struct GpsMtk gps_mtk;
+
+#ifdef GPS_CONFIGURE
+bool_t gps_configuring;
+static uint8_t gps_status_config;
+#endif
+
+void gps_impl_init(void) {
+ gps_mtk.status = UNINIT;
+ gps_mtk.msg_available = FALSE;
+ gps_mtk.error_cnt = 0;
+ gps_mtk.error_last = GPS_MTK_ERR_NONE;
+#ifdef GPS_CONFIGURE
+ gps_status_config = 0;
+ gps_configuring = TRUE;
+#endif
+}
+
+static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time,
+ int16_t* gps_week, uint32_t* gps_itow) {
+ /* convert UTC date/time to GPS week/itow, we have no idea about GPS
+ leap seconds for now */
+ uint16_t gps_msecond = gps_time % 1000;
+ uint8_t gps_second = (gps_time / 1000) % 100;
+ uint8_t gps_minute = (gps_time / 100000) % 100;
+ uint8_t gps_hour = (gps_time / 10000000) % 100;
+ uint16_t gps_year = 2000 + (gps_date % 100);
+ uint8_t gps_month = (gps_date / 100) % 100;
+ uint8_t gps_day = (gps_date / 10000) % 100;
+ int32_t i, days;
+
+ *gps_week = 0;
+ *gps_itow = 0;
+
+ /* sanity checks */
+ if (gps_month > 12) return;
+ if (gps_day > (DAYS_MONTH[gps_month] +
+ ((gps_month == 1) ? isleap(gps_year):0))) return;
+ if (gps_hour > 23) return;
+ if (gps_minute > 59) return;
+ if (gps_second > 59) return;
+
+ /* days since 6-JAN-1980 */
+ days = -6;
+ for (i = 1980; i < gps_year; i++) days += (365 + isleap(i));
+
+ /* add days in gps_year */
+ for (i = 0; i < gps_month-1; i++) {
+ days += DAYS_MONTH[i] + ((i == 1) ? isleap(gps_year):0);
+ }
+ days += gps_day;
+
+ /* convert */
+ *gps_week = (uint16_t) (days / 7);
+ *gps_itow = ((days % 7) * SECS_DAY +
+ gps_hour * SECS_HOUR +
+ gps_minute * SECS_MINUTE +
+ gps_second) * 1000 +
+ gps_msecond;
+}
+
+void gps_mtk_read_message(void) {
+ if (gps_mtk.msg_class == MTK_DIY14_ID) {
+ if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
+#ifdef GPS_TIMESTAMP
+ /* get hardware clock ticks */
+ SysTimeTimerStart(gps.t0);
+ gps.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
+ gps.t0_tow_frac = 0;
+#endif
+ gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10;
+ gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10;
+ // FIXME: with MTK you do not receive vertical speed
+ if (cpu_time_sec - gps.last_fix_time < 2) {
+ gps.ned_vel.z = ((gps.hmsl -
+ MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
+ } else gps.ned_vel.z = 0;
+ gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10;
+ // FIXME: with MTK you do not receive ellipsoid altitude
+ gps.lla_pos.alt = gps.hmsl;
+ gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
+ // FIXME: with MTK you do not receive speed 3D
+ gps.speed_3d = gps.gspeed;
+ gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
+ gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
+ switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
+ case MTK_DIY_FIX_3D:
+ gps.fix = GPS_FIX_3D;
+ break;
+ case MTK_DIY_FIX_2D:
+ gps.fix = GPS_FIX_2D;
+ break;
+ default:
+ gps.fix = GPS_FIX_NONE;
+ }
+ gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
+ // FIXME: with MTK DIY 1.4 you do not receive GPS week
+ gps.week = 0;
+ /* Computes from (lat, long) in the referenced UTM zone */
+ struct LlaCoor_f lla_f;
+ lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+ lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ struct UtmCoor_f utm_f;
+ utm_f.zone = nav_utm_zone0;
+ /* convert to utm */
+ utm_of_lla_f(&utm_f, &lla_f);
+ /* copy results of utm conversion */
+ gps.utm_pos.east = utm_f.east*100;
+ gps.utm_pos.north = utm_f.north*100;
+ gps.utm_pos.alt = utm_f.alt*1000;
+ gps.utm_pos.zone = nav_utm_zone0;
+#ifdef GPS_LED
+ if (gps.fix == GPS_FIX_3D) {
+ LED_ON(GPS_LED);
+ }
+ else {
+ LED_TOGGLE(GPS_LED);
+ }
+#endif
+ }
+ }
+
+ if (gps_mtk.msg_class == MTK_DIY16_ID) {
+ if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
+ uint32_t gps_date, gps_time;
+ gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
+ gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
+ gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow);
+#ifdef GPS_TIMESTAMP
+ /* get hardware clock ticks */
+ SysTimeTimerStart(gps.t0);
+ gps.t0_tow = gps.tow;
+ gps.t0_tow_frac = 0;
+#endif
+ gps.lla_pos.lat = RadOfDeg(MTK_DIY16_NAV_LAT(gps_mtk.msg_buf))*10;
+ gps.lla_pos.lon = RadOfDeg(MTK_DIY16_NAV_LON(gps_mtk.msg_buf))*10;
+ // FIXME: with MTK you do not receive vertical speed
+ if (cpu_time_sec - gps.last_fix_time < 2) {
+ gps.ned_vel.z = ((gps.hmsl -
+ MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
+ } else gps.ned_vel.z = 0;
+ gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10;
+ // FIXME: with MTK you do not receive ellipsoid altitude
+ gps.lla_pos.alt = gps.hmsl;
+ gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
+ // FIXME: with MTK you do not receive speed 3D
+ gps.speed_3d = gps.gspeed;
+ gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf)*10000)) * 10;
+ gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
+ switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
+ case MTK_DIY_FIX_3D:
+ gps.fix = GPS_FIX_3D;
+ break;
+ case MTK_DIY_FIX_2D:
+ gps.fix = GPS_FIX_2D;
+ break;
+ default:
+ gps.fix = GPS_FIX_NONE;
+ }
+ /* HDOP? */
+ /* Computes from (lat, long) in the referenced UTM zone */
+ struct LlaCoor_f lla_f;
+ lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+ lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ struct UtmCoor_f utm_f;
+ utm_f.zone = nav_utm_zone0;
+ /* convert to utm */
+ utm_of_lla_f(&utm_f, &lla_f);
+ /* copy results of utm conversion */
+ gps.utm_pos.east = utm_f.east*100;
+ gps.utm_pos.north = utm_f.north*100;
+ gps.utm_pos.alt = utm_f.alt*1000;
+ gps.utm_pos.zone = nav_utm_zone0;
+#ifdef GPS_LED
+ if (gps.fix == GPS_FIX_3D) {
+ LED_ON(GPS_LED);
+ }
+ else {
+ LED_TOGGLE(GPS_LED);
+ }
+#endif
+ }
+ }
+}
+
+/* byte parsing */
+void gps_mtk_parse( uint8_t c ) {
+ if (gps_mtk.status < GOT_PAYLOAD) {
+ gps_mtk.ck_a += c;
+ gps_mtk.ck_b += gps_mtk.ck_a;
+ }
+ switch (gps_mtk.status) {
+ case UNINIT:
+ if (c == MTK_DIY14_SYNC1)
+ gps_mtk.status = GOT_SYNC1_14;
+ if (c == MTK_DIY16_ID)
+ gps_mtk.msg_class = c;
+ gps_mtk.status = GOT_SYNC1_16;
+ break;
+ /* MTK_DIY_VER_14 */
+ case GOT_SYNC1_14:
+ if (c != MTK_DIY14_SYNC2) {
+ gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ if (gps_mtk.msg_available) {
+ /* Previous message has not yet been parsed: discard this one */
+ gps_mtk.error_last = GPS_MTK_ERR_OVERRUN;
+ goto error;
+ }
+ gps_mtk.ck_a = 0;
+ gps_mtk.ck_b = 0;
+ gps_mtk.status++;
+ gps_mtk.len = MTK_DIY14_NAV_LENGTH;
+ break;
+ case GOT_SYNC2_14:
+ if (c != MTK_DIY14_ID) {
+ gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ gps_mtk.msg_class = c;
+ gps_mtk.msg_idx = 0;
+ gps_mtk.status++;
+ break;
+ case GOT_CLASS_14:
+ if (c != MTK_DIY14_NAV_ID) {
+ gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ gps_mtk.msg_id = c;
+ gps_mtk.status = GOT_ID;
+ break;
+ /* MTK_DIY_VER_16 */
+ case GOT_SYNC1_16:
+ if (c != MTK_DIY16_NAV_ID) {
+ gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC;
+ goto error;
+ }
+ if (gps_mtk.msg_available) {
+ /* Previous message has not yet been parsed: discard this one */
+ gps_mtk.error_last = GPS_MTK_ERR_OVERRUN;
+ goto error;
+ }
+ gps_mtk.msg_id = c;
+ gps_mtk.ck_a = 0;
+ gps_mtk.ck_b = 0;
+ gps_mtk.status++;
+ break;
+ case GOT_SYNC2_16:
+ gps_mtk.len = c;
+ gps_mtk.msg_idx = 0;
+ gps_mtk.status = GOT_ID;
+ break;
+ case GOT_ID:
+ gps_mtk.msg_buf[gps_mtk.msg_idx] = c;
+ gps_mtk.msg_idx++;
+ if (gps_mtk.msg_idx >= gps_mtk.len) {
+ gps_mtk.status++;
+ }
+ break;
+ case GOT_PAYLOAD:
+ if (c != gps_mtk.ck_a) {
+ gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM;
+ goto error;
+ }
+ gps_mtk.status++;
+ break;
+ case GOT_CHECKSUM1:
+ if (c != gps_mtk.ck_b) {
+ gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM;
+ goto error;
+ }
+ gps_mtk.msg_available = TRUE;
+ goto restart;
+ break;
+ default:
+ gps_mtk.error_last = GPS_MTK_ERR_UNEXPECTED;
+ goto error;
+ }
+ return;
+ error:
+ gps_mtk.error_cnt++;
+ restart:
+ gps_mtk.status = UNINIT;
+ return;
+}
+
+
+/*
+ *
+ *
+ * GPS dynamic configuration
+ *
+ *
+ */
+#ifdef GPS_CONFIGURE
+
+static void MtkSend_CFG(char* dat) {
+ while (*dat != 0) GpsLink(Transmit(*dat++));
+}
+
+void gps_configure_uart(void) {
+}
+
+#ifdef USER_GPS_CONFIGURE
+#include USER_GPS_CONFIGURE
+#else
+static bool_t user_gps_configure(bool_t cpt) {
+ switch (cpt) {
+ case 0:
+ MtkSend_CFG(MTK_DIY_SET_BINARY);
+ break;
+ case 1:
+ MtkSend_CFG(MTK_DIY_OUTPUT_RATE);
+ return FALSE;
+ }
+ return TRUE; /* Continue, except for the last case */
+}
+#endif // ! USER_GPS_CONFIGURE
+
+void gps_configure( void ) {
+ static uint32_t count=0;
+ /* start configuring after having received 50 bytes */
+ if (count++ > 50)
+ gps_configuring = user_gps_configure(gps_status_config++);
+}
+
+#endif /* GPS_CONFIGURE */
diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h
new file mode 100644
index 0000000000..eeffd03afd
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_mtk.h
@@ -0,0 +1,136 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+
+#ifndef MTK_H
+#define MTK_H
+
+#include "mcu_periph/uart.h"
+
+/** Includes macros generated from mtk.xml */
+#include "mtk_protocol.h"
+
+#define GPS_MTK_MAX_PAYLOAD 255
+
+struct GpsMtk {
+ bool_t msg_available;
+ uint8_t msg_buf[GPS_MTK_MAX_PAYLOAD] __attribute__ ((aligned));
+ uint8_t msg_id;
+ uint8_t msg_class;
+
+ uint8_t status;
+ uint16_t len;
+ uint8_t msg_idx;
+ uint8_t ck_a, ck_b;
+ uint8_t send_ck_a, send_ck_b;
+ uint8_t error_cnt;
+ uint8_t error_last;
+
+ uint8_t status_flags;
+ uint8_t sol_flags;
+};
+
+extern struct GpsMtk gps_mtk;
+
+
+/*
+ * This part is used by the autopilot to read data from a uart
+ */
+#define __GpsLink(dev, _x) dev##_x
+#define _GpsLink(dev, _x) __GpsLink(dev, _x)
+#define GpsLink(_x) _GpsLink(GPS_LINK, _x)
+
+#define GpsBuffer() GpsLink(ChAvailable())
+
+#ifdef GPS_CONFIGURE
+extern bool_t gps_configuring;
+#define GpsConfigure() { \
+ if (gps_configuring) \
+ gps_configure(); \
+ }
+#else
+#define GpsConfigure() {}
+#endif
+
+#define GpsEvent(_sol_available_callback) { \
+ if (GpsBuffer()) { \
+ ReadGpsBuffer(); \
+ GpsConfigure(); \
+ } \
+ if (gps_mtk.msg_available) { \
+ gps_mtk_read_message(); \
+ if (gps_mtk.msg_class == MTK_DIY14_ID && \
+ gps_mtk.msg_id == MTK_DIY14_NAV_ID) { \
+ if (gps.fix == GPS_FIX_3D) { \
+ gps.last_fix_time = cpu_time_sec; \
+ } \
+ _sol_available_callback(); \
+ } \
+ if (gps_mtk.msg_class == MTK_DIY16_ID && \
+ gps_mtk.msg_id == MTK_DIY16_NAV_ID) { \
+ if (gps.fix == GPS_FIX_3D) { \
+ gps.last_fix_time = cpu_time_sec; \
+ } \
+ _sol_available_callback(); \
+ } \
+ gps_mtk.msg_available = FALSE; \
+ } \
+ }
+
+#define ReadGpsBuffer() { \
+ while (GpsLink(ChAvailable())&&!gps_mtk.msg_available) \
+ gps_mtk_parse(GpsLink(Getch())); \
+ }
+
+
+extern void gps_mtk_read_message(void);
+extern void gps_mtk_parse(uint8_t c);
+
+#define MTK_DIY_FIX_3D 3
+#define MTK_DIY_FIX_2D 2
+#define MTK_DIY_FIX_NONE 1
+
+/*
+ * dynamic GPS configuration
+ */
+#ifdef GPS_CONFIGURE
+#define MTK_DIY_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define MTK_DIY_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n"
+
+#define MTK_DIY_OUTPUT_1HZ "$PMTK220,1000*1F\r\n"
+#define MTK_DIY_OUTPUT_2HZ "$PMTK220,500*2B\r\n"
+#define MTK_DIY_OUTPUT_4HZ "$PMTK220,250*29\r\n"
+#define MTK_DIY_OTUPUT_5HZ "$PMTK220,200*2C\r\n"
+#define MTK_DIY_OUTPUT_10HZ "$PMTK220,100*2F\r\n"
+
+#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
+
+#define MTK_DIY_SBAS_ON "$PMTK313,1*2E\r\n"
+#define MTK_DIY_SBAS_OFF "$PMTK313,0*2F\r\n"
+
+#define MTK_DIY_WAAS_ON "$PSRF151,1*3F\r\n"
+#define MTK_DIY_WAAS_OFF "$PSRF151,0*3E\r\n"
+
+extern void gps_configure(void);
+extern void gps_configure_uart(void);
+#endif
+
+#endif /* MTK_H */
diff --git a/sw/tools/Makefile b/sw/tools/Makefile
index e0f0b33ccf..fe5f2a91fd 100644
--- a/sw/tools/Makefile
+++ b/sw/tools/Makefile
@@ -30,7 +30,7 @@ OCAMLNETCMA=$(shell ocamlfind query -r -a-format -predicates byte netstring)
OCAMLLEX=ocamllex
OCAMLYACC=ocamlyacc
-all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_tuning.out gen_xsens.out gen_modules.out find_free_msg_id.out
+all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_tuning.out gen_xsens.out gen_modules.out find_free_msg_id.out
FP_CMO = fp_proc.cmo gen_flight_plan.cmo
ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)
diff --git a/sw/tools/gen_mtk.ml b/sw/tools/gen_mtk.ml
new file mode 100644
index 0000000000..692745152d
--- /dev/null
+++ b/sw/tools/gen_mtk.ml
@@ -0,0 +1,176 @@
+(*
+ * $Id$
+ *
+ * XML preprocessing for Mediatek (DIYDrones 1.4/1.6) protocol
+ *
+ * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ *)
+
+open Printf
+
+let out = stdout
+
+let sizeof = function
+ "U4" | "I4" -> 4
+ | "U2" | "I2" -> 2
+ | "U1" | "I1" -> 1
+ | "U4BE" | "I4BE" -> 4
+ | "U2BE" | "I2BE" -> 2
+ | x -> failwith (sprintf "sizeof: unknown format '%s'" x)
+
+let (+=) = fun r x -> r := !r + x
+
+let c_type = fun format ->
+ match format with
+ "I2" -> "int16_t"
+ | "I4" -> "int32_t"
+ | "U2" -> "uint16_t"
+ | "U4" -> "uint32_t"
+ | "U1" -> "uint8_t"
+ | "I1" -> "int8_t"
+ | "I2BE" -> "int16_t"
+ | "I4BE" -> "int32_t"
+ | "U2BE" -> "uint16_t"
+ | "U4BE" -> "uint32_t"
+ | _ -> failwith (sprintf "Gen_mtk.c_type: unknown format '%s'" format)
+
+let get_at = fun offset format block_size ->
+ let t = c_type format in
+ let block_offset =
+ if block_size = 0 then "" else sprintf "+%d*_mtk_block" block_size in
+ match format with
+ "U4" | "I4" -> sprintf "(%s)(*((uint8_t*)_mtk_payload+%d%s)|*((uint8_t*)_mtk_payload+1+%d%s)<<8|((%s)*((uint8_t*)_mtk_payload+2+%d%s))<<16|((%s)*((uint8_t*)_mtk_payload+3+%d%s))<<24)" t offset block_offset offset block_offset t offset block_offset t offset block_offset
+ | "U2" | "I2" -> sprintf "(%s)(*((uint8_t*)_mtk_payload+%d%s)|*((uint8_t*)_mtk_payload+1+%d%s)<<8)" t offset block_offset offset block_offset
+ | "U1" | "I1" -> sprintf "(%s)(*((uint8_t*)_mtk_payload+%d%s))" t offset block_offset
+ | "U4BE" | "I4BE" -> sprintf "(%s)(*((uint8_t*)_mtk_payload+3+%d%s)|*((uint8_t*)_mtk_payload+2+%d%s)<<8|((%s)*((uint8_t*)_mtk_payload+1+%d%s))<<16|((%s)*((uint8_t*)_mtk_payload+%d%s))<<24)" t offset block_offset offset block_offset t offset block_offset t offset block_offset
+ | "U2BE" | "I2BE" -> sprintf "(%s)(*((uint8_t*)_mtk_payload+1+%d%s)|*((uint8_t*)_mtk_payload+%d%s)<<8)" t offset block_offset offset block_offset
+ | _ -> failwith (sprintf "Gen_mtk.c_type: unknown format '%s'" format)
+
+let define = fun x y ->
+ fprintf out "#define %s %s\n" x y
+
+exception Length_error of Xml.xml*int*int
+
+
+
+
+let parse_message = fun class_name m ->
+ let msg_name = Xml.attrib m "name" in
+
+ fprintf out "\n";
+ let msg_id = sprintf "MTK_%s_%s_ID" class_name msg_name in
+ define msg_id (Xml.attrib m "ID");
+
+ let field_name = fun f -> ExtXml.attrib f "name" in
+ let format = fun f -> Xml.attrib f "format" in
+
+ let offset = ref 0 in
+ let rec gen_access_macro = fun block_size f ->
+ match Xml.tag f with
+ "field" ->
+ let fn = field_name f
+ and fmt = format f in
+ let block_no = if block_size = 0 then "" else ",_mtk_block" in
+ define (sprintf "MTK_%s_%s_%s(_mtk_payload%s)" class_name msg_name fn block_no) (get_at !offset fmt block_size);
+ offset += sizeof fmt
+ | "block" ->
+ let s = int_of_string (Xml.attrib f "length") in
+ let o = !offset in
+ List.iter (gen_access_macro s) (Xml.children f);
+ let s' = !offset - o in
+ if s <> s' then raise (Length_error (f, s, s'))
+ | x -> failwith ("Unexpected field: " ^ x)
+ in
+
+ List.iter (gen_access_macro 0) (Xml.children m);
+ begin
+ try
+ let l = int_of_string (Xml.attrib m "length") in
+ if l <> !offset then raise (Length_error (m, l, !offset))
+ with
+ Xml.No_attribute("length") -> () (** Undefined length authorized *)
+ end;
+
+ (** Generating send function *)
+ let param_name = fun f -> String.lowercase (field_name f) in
+ let rec param_names = fun f r ->
+ if Xml.tag f = "field" then
+ param_name f :: r
+ else
+ List.fold_right param_names (Xml.children f) r in
+ let param_type = fun f -> c_type (format f) in
+ fprintf out "\n#define MtkSend_%s_%s(" class_name msg_name;
+ fprintf out "%s" (String.concat "," (param_names m []));
+ fprintf out ") { \\\n";
+ fprintf out " MtkHeader(MTK_%s_ID, %s, %d);\\\n" class_name msg_id !offset;
+ let rec send_one_field = fun f ->
+ match Xml.tag f with
+ "field" ->
+ let s = sizeof (format f) in
+ let p = param_name f in
+ let t = param_type f in
+ fprintf out " %s _%s = %s; MtkSend%dByAddr((uint8_t*)&_%s);\\\n" t p p s p
+ | "block" ->
+ List.iter send_one_field (Xml.children f)
+ | _ -> assert (false) in
+ List.iter send_one_field (Xml.children m);
+ fprintf out " MtkTrailer();\\\n";
+ fprintf out "}\n\n#define MTK_%s_%s_LENGTH %d\n" class_name msg_name !offset
+
+
+let parse_class = fun c ->
+ let _class_id = int_of_string (Xml.attrib c "id")
+ and class_name = Xml.attrib c "name" in
+
+ fprintf out "\n";
+ define (sprintf "MTK_%s_ID" class_name) (Xml.attrib c "ID");
+
+ List.iter (parse_message class_name) (Xml.children c)
+
+
+let _ =
+ if Array.length Sys.argv <> 2 then begin
+ failwith (sprintf "Usage: %s <.xml mtk protocol file>" Sys.argv.(0))
+ end;
+ let xml_file = Sys.argv.(1) in
+ try
+ let xml = Xml.parse_file xml_file in
+ fprintf out "/* Generated from %s */\n" xml_file;
+ fprintf out "/* Please DO NOT EDIT */\n\n";
+
+ define "MTK_DIY14_SYNC1" "0xB5";
+ define "MTK_DIY14_SYNC2" "0x62";
+
+ List.iter parse_class (Xml.children xml)
+ with
+ Xml.Error (em, ep) ->
+ let l = Xml.line ep
+ and c1, c2 = Xml.range ep in
+ fprintf stderr "File \"%s\", line %d, characters %d-%d:\n" xml_file l c1 c2;
+ fprintf stderr "%s\n" (Xml.error_msg em);
+ exit 1
+ | Length_error (m, l1, l2) ->
+ fprintf stderr "File \"%s\", inconsistent length: %d expected, %d found from fields in message:\n %s\n" xml_file l1 l2 (Xml.to_string_fmt m);
+ exit 1
+ | Dtd.Check_error e ->
+ fprintf stderr "File \"%s\", DTD check error: %s\n" xml_file (Dtd.check_error e)
+ | Dtd.Prove_error e ->
+ fprintf stderr "\nFile \"%s\", DTD check error: %s\n\n" xml_file (Dtd.prove_error e)