Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:05:14 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[mtk] added new gps support

Browse Source
This commit is contained in:
masierra
2016-02-03 17:25:26 -08:00
parent b48824ec68
commit f0266ad5b2
3 changed files with 103 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile
+2 -2
View File
@@ -6,7 +6,7 @@ GPS_LED ?= none
MTK_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) MTK_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE
ap.CFLAGS += -DGPS_LINK=$(MTK_GPS_PORT_LOWER) ap.CFLAGS += -DMTK_GPS_LINK=$(MTK_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -14,7 +14,7 @@ ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED) ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sw/airborne/subsystems/gps/gps_mtk.c
+78 -54
View File
@@ -31,7 +31,6 @@
* *
*/ */
#include "subsystems/gps.h"
#include "subsystems/abi.h" #include "subsystems/abi.h"
#include "led.h" #include "led.h"
@@ -99,7 +98,7 @@ bool_t gps_configuring;
static uint8_t gps_status_config; static uint8_t gps_status_config;
#endif #endif
void gps_impl_init(void) void mtk_gps_impl_init(void)
{ {
gps_mtk.status = UNINIT; gps_mtk.status = UNINIT;
gps_mtk.msg_available = FALSE; gps_mtk.msg_available = FALSE;
@@ -111,29 +110,42 @@ void gps_impl_init(void)
#endif #endif
} }
void mtk_gps_event(void)
{
struct link_device *dev = &((MTK_GPS_LINK).device);
while (dev->char_available(dev->periph)) {
gps_mtk_parse(dev->get_byte(dev->periph));
if (gps_mtk.msg_available) {
gps_mtk_msg();
}
GpsConfigure();
}
}
void gps_mtk_msg(void) void gps_mtk_msg(void)
{ {
// current timestamp // current timestamp
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem; gps_mtk.state.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec; gps_mtk.state.last_msg_time = sys_time.nb_sec;
gps_mtk_read_message(); gps_mtk_read_message();
if (gps_mtk.msg_class == MTK_DIY14_ID && if (gps_mtk.msg_class == MTK_DIY14_ID &&
gps_mtk.msg_id == MTK_DIY14_NAV_ID) { gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
if (gps.fix == GPS_FIX_3D) { if (gps_mtk.state.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem; gps_mtk.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec; gps_mtk.state.last_3dfix_time = sys_time.nb_sec;
} }
AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps); AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state);
} }
if (gps_mtk.msg_class == MTK_DIY16_ID && if (gps_mtk.msg_class == MTK_DIY16_ID &&
gps_mtk.msg_id == MTK_DIY16_NAV_ID) { gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
if (gps.fix == GPS_FIX_3D) { if (gps_mtk.state.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem; gps_mtk.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec; gps_mtk.state.last_3dfix_time = sys_time.nb_sec;
} }
AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps); AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state);
} }
gps_mtk.msg_available = FALSE; gps_mtk.msg_available = FALSE;
} }
@@ -190,39 +202,39 @@ void gps_mtk_read_message(void)
gps_time_sync.t0_ticks = sys_time.nb_tick; gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf); gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
gps_time_sync.t0_tow_frac = 0; gps_time_sync.t0_tow_frac = 0;
gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10; gps_mtk.state.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10;
gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10; gps_mtk.state.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); SetBit(gps_mtk.state.valid_fields, GPS_VALID_POS_LLA_BIT);
// FIXME: with MTK you do not receive vertical speed // FIXME: with MTK you do not receive vertical speed
if (sys_time.nb_sec - gps.last_3dfix_time < 2) { if (sys_time.nb_sec - gps_mtk.state.last_3dfix_time < 2) {
gps.ned_vel.z = ((gps.hmsl - gps_mtk.state.ned_vel.z = ((gps_mtk.state.hmsl -
MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
} else { gps.ned_vel.z = 0; } } else { gps_mtk.state.ned_vel.z = 0; }
gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10; gps_mtk.state.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); SetBit(gps_mtk.state.valid_fields, GPS_VALID_HMSL_BIT);
// FIXME: with MTK you do not receive ellipsoid altitude // FIXME: with MTK you do not receive ellipsoid altitude
gps.lla_pos.alt = gps.hmsl; gps_mtk.state.lla_pos.alt = gps_mtk.state.hmsl;
gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf); gps_mtk.state.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
// FIXME: with MTK you do not receive speed 3D // FIXME: with MTK you do not receive speed 3D
gps.speed_3d = gps.gspeed; gps_mtk.state.speed_3d = gps_mtk.state.gspeed;
gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10; gps_mtk.state.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); SetBit(gps_mtk.state.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf); gps_mtk.state.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) { switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D: case MTK_DIY_FIX_3D:
gps.fix = GPS_FIX_3D; gps_mtk.state.fix = GPS_FIX_3D;
break; break;
case MTK_DIY_FIX_2D: case MTK_DIY_FIX_2D:
gps.fix = GPS_FIX_2D; gps_mtk.state.fix = GPS_FIX_2D;
break; break;
default: default:
gps.fix = GPS_FIX_NONE; gps_mtk.state.fix = GPS_FIX_NONE;
} }
gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);; gps_mtk.state.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
// FIXME: with MTK DIY 1.4 you do not receive GPS week // FIXME: with MTK DIY 1.4 you do not receive GPS week
gps.week = 0; gps_mtk.state.week = 0;
#ifdef GPS_LED #ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) { if (gps_mtk.state.fix == GPS_FIX_3D) {
LED_ON(GPS_LED); LED_ON(GPS_LED);
} else { } else {
LED_TOGGLE(GPS_LED); LED_TOGGLE(GPS_LED);
@@ -236,43 +248,43 @@ void gps_mtk_read_message(void)
uint32_t gps_date, gps_time; uint32_t gps_date, gps_time;
gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf); gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
gps_time = MTK_DIY16_NAV_UTC_TIME(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); gps_mtk_time2itow(gps_date, gps_time, &gps_mtk.state.week, &gps_mtk.state.tow);
#ifdef GPS_TIMESTAMP #ifdef GPS_TIMESTAMP
/* get hardware clock ticks */ /* get hardware clock ticks */
SysTimeTimerStart(gps.t0); SysTimeTimerStart(gps_mtk.state.t0);
gps.t0_tow = gps.tow; gps_mtk.state.t0_tow = gps_mtk.state.tow;
gps.t0_tow_frac = 0; gps_mtk.state.t0_tow_frac = 0;
#endif #endif
gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10; gps_mtk.state.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10;
gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10; gps_mtk.state.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10;
// FIXME: with MTK you do not receive vertical speed // FIXME: with MTK you do not receive vertical speed
if (sys_time.nb_sec - gps.last_3dfix_time < 2) { if (sys_time.nb_sec - gps_mtk.state.last_3dfix_time < 2) {
gps.ned_vel.z = ((gps.hmsl - gps_mtk.state.ned_vel.z = ((gps_mtk.state.hmsl -
MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
} else { gps.ned_vel.z = 0; } } else { gps_mtk.state.ned_vel.z = 0; }
gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10; gps_mtk.state.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); SetBit(gps_mtk.state.valid_fields, GPS_VALID_HMSL_BIT);
// FIXME: with MTK you do not receive ellipsoid altitude // FIXME: with MTK you do not receive ellipsoid altitude
gps.lla_pos.alt = gps.hmsl; gps_mtk.state.lla_pos.alt = gps_mtk.state.hmsl;
gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf); gps_mtk.state.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
// FIXME: with MTK you do not receive speed 3D // FIXME: with MTK you do not receive speed 3D
gps.speed_3d = gps.gspeed; gps_mtk.state.speed_3d = gps_mtk.state.gspeed;
gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10; gps_mtk.state.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); SetBit(gps_mtk.state.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf); gps_mtk.state.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) { switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D: case MTK_DIY_FIX_3D:
gps.fix = GPS_FIX_3D; gps_mtk.state.fix = GPS_FIX_3D;
break; break;
case MTK_DIY_FIX_2D: case MTK_DIY_FIX_2D:
gps.fix = GPS_FIX_2D; gps_mtk.state.fix = GPS_FIX_2D;
break; break;
default: default:
gps.fix = GPS_FIX_NONE; gps_mtk.state.fix = GPS_FIX_NONE;
} }
/* HDOP? */ /* HDOP? */
#ifdef GPS_LED #ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) { if (gps_mtk.state.fix == GPS_FIX_3D) {
LED_ON(GPS_LED); LED_ON(GPS_LED);
} else { } else {
LED_TOGGLE(GPS_LED); LED_TOGGLE(GPS_LED);
@@ -388,6 +400,18 @@ restart:
} }
/*
* register callbacks & structs
*/
void mtk_gps_register(void)
{
#ifdef GPS_SECONDARY_MTK
gps_register_impl(mtk_gps_impl_init, mtk_gps_event, GPS_MTK_ID, 1);
#else
gps_register_impl(mtk_gps_impl_init, mtk_gps_event, GPS_MTK_ID, 0);
#endif
}
/* /*
* *
* *
@@ -401,7 +425,7 @@ restart:
static void MtkSend_CFG(char *dat) static void MtkSend_CFG(char *dat)
{ {
struct link_device *dev = &((GPS_LINK).device); struct link_device *dev = &((MTK_GPS_LINK).device);
while (*dat != 0) { dev->put_byte(dev->periph, *dat++); } while (*dat != 0) { dev->put_byte(dev->periph, *dat++); }
} }
sw/airborne/subsystems/gps/gps_mtk.h
+23 -11
View File
@@ -34,11 +34,28 @@
#ifndef MTK_H #ifndef MTK_H
#define MTK_H #define MTK_H
#include "subsystems/gps.h"
#include "mcu_periph/uart.h" #include "mcu_periph/uart.h"
/** Includes macros generated from mtk.xml */ /** Includes macros generated from mtk.xml */
#include "mtk_protocol.h" #include "mtk_protocol.h"
#if GPS_SECONDARY_MTK
#ifndef MTK_GPS_LINK
#define MTK_GPS_LINK GPS_SECONDARY_PORT
#define SecondaryGpsImpl mtk
#endif
#else
#ifndef PrimaryGpsImpl
#define PrimaryGpsImpl mtk
#endif
#endif
#if GPS_PRIMARY_MTK
#ifndef MTK_GPS_LINK
#define MTK_GPS_LINK GPS_PRIMARY_PORT
#endif
#endif
#define GPS_MTK_MAX_PAYLOAD 255 #define GPS_MTK_MAX_PAYLOAD 255
struct GpsMtk { struct GpsMtk {
@@ -57,6 +74,8 @@ struct GpsMtk {
uint8_t status_flags; uint8_t status_flags;
uint8_t sol_flags; uint8_t sol_flags;
struct GpsState state;
}; };
extern struct GpsMtk gps_mtk; extern struct GpsMtk gps_mtk;
@@ -83,17 +102,10 @@ extern void gps_mtk_read_message(void);
extern void gps_mtk_parse(uint8_t c); extern void gps_mtk_parse(uint8_t c);
extern void gps_mtk_msg(void); extern void gps_mtk_msg(void);
static inline void GpsEvent(void) extern void mtk_gps_event(void);
{ extern void mtk_gps_impl_init(void);
struct link_device *dev = &((GPS_LINK).device); extern void mtk_gps_register(void);
while (dev->char_available(dev->periph)) {
gps_mtk_parse(dev->get_byte(dev->periph));
if (gps_mtk.msg_available) {
gps_mtk_msg();
}
GpsConfigure();
}
}
#endif /* MTK_H */ #endif /* MTK_H */