Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[nmea] added new gps support

Browse Source
This commit is contained in:
masierra
2016-02-03 09:45:08 -08:00
parent 93c0f4a970
commit 07efb49367
3 changed files with 97 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/shared/gps_nmea.makefile
+2 -2
View File
@@ -6,7 +6,7 @@ GPS_LED ?= none
NMEA_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(NMEA_GPS_PORT_LOWER)
ap.CFLAGS += -DNMEA_GPS_LINK=$(NMEA_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -14,7 +14,7 @@ ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sw/airborne/subsystems/gps/gps_nmea.c
+69 -41
View File
@@ -30,6 +30,7 @@
* Parsing GGA, RMC, GSA and GSV.
*/
#include "gps_nmea.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "led.h"
@@ -62,10 +63,9 @@ static void nmea_parse_RMC(void);
static void nmea_parse_GGA(void);
static void nmea_parse_GSV(void);
void gps_impl_init(void)
void nmea_gps_impl_init(void)
{
gps.nb_channels = GPS_NB_CHANNELS;
gps_nmea.state.nb_channels = GPS_NMEA_NB_CHANNELS;
gps_nmea.is_configured = FALSE;
gps_nmea.msg_available = FALSE;
gps_nmea.pos_available = FALSE;
@@ -76,18 +76,34 @@ void gps_impl_init(void)
nmea_configure();
}
void gps_nmea_msg(void)
void nmea_gps_event(void)
{
struct link_device *dev = &((NMEA_GPS_LINK).device);
if (!gps_nmea.is_configured) {
nmea_configure();
return;
}
while (dev->char_available(dev->periph)) {
nmea_parse_char(dev->get_byte(dev->periph));
if (gps_nmea.msg_available) {
nmea_gps_msg();
}
}
}
void nmea_gps_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
gps_nmea.state.last_msg_ticks = sys_time.nb_sec_rem;
gps_nmea.state.last_msg_time = sys_time.nb_sec;
nmea_parse_msg();
if (gps_nmea.pos_available) {
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
if (gps_nmea.state.fix == GPS_FIX_3D) {
gps_nmea.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_nmea.state.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_NMEA_ID, now_ts, &gps);
}
@@ -251,11 +267,11 @@ static void nmea_parse_GSA(void)
// get 2D/3D-fix
// set gps_mode=3=3d, 2=2d, 1=no fix or 0
gps.fix = atoi(&gps_nmea.msg_buf[i]);
if (gps.fix == 1) {
gps.fix = 0;
gps_nmea.state.fix = atoi(&gps_nmea.msg_buf[i]);
if (gps_nmea.state.fix == 1) {
gps_nmea.state.fix = 0;
}
NMEA_PRINT("p_GSA() - gps.fix=%i (3=3D)\n\r", gps.fix);
NMEA_PRINT("p_GSA() - gps_nmea.state.fix=%i (3=3D)\n\r", gps_nmea.state.fix);
nmea_read_until(&i);
// up to 12 PRNs of satellites used for fix
@@ -266,13 +282,13 @@ static void nmea_parse_GSA(void)
int prn = atoi(&gps_nmea.msg_buf[i]);
NMEA_PRINT("p_GSA() - PRN %i=%i\n\r", satcount, prn);
if (!gps_nmea.have_gsv) {
gps.svinfos[prn_cnt].svid = prn;
gps_nmea.state.svinfos[prn_cnt].svid = prn;
}
satcount++;
}
else {
if (!gps_nmea.have_gsv) {
gps.svinfos[prn_cnt].svid = 0;
gps_nmea.state.svinfos[prn_cnt].svid = 0;
}
}
nmea_read_until(&i);
@@ -280,7 +296,7 @@ static void nmea_parse_GSA(void)
// PDOP
float pdop = strtof(&gps_nmea.msg_buf[i], NULL);
gps.pdop = pdop * 100;
gps_nmea.state.pdop = pdop * 100;
NMEA_PRINT("p_GSA() - pdop=%f\n\r", pdop);
nmea_read_until(&i);
@@ -330,15 +346,15 @@ static void nmea_parse_RMC(void)
// get speed
nmea_read_until(&i);
double speed = strtod(&gps_nmea.msg_buf[i], NULL);
gps.gspeed = speed * 1.852 * 100 / (60 * 60);
NMEA_PRINT("p_RMC() - ground-speed=%f knot = %d cm/s\n\r", speed, (gps.gspeed * 1000));
gps_nmea.state.gspeed = speed * 1.852 * 100 / (60 * 60);
NMEA_PRINT("p_RMC() - ground-speed=%f knot = %d cm/s\n\r", speed, (gps_nmea.state.gspeed * 1000));
// get course
nmea_read_until(&i);
double course = strtod(&gps_nmea.msg_buf[i], NULL);
gps.course = RadOfDeg(course) * 1e7;
gps_nmea.state.course = RadOfDeg(course) * 1e7;
NMEA_PRINT("p_RMC() - course: %f deg\n\r", course);
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
SetBit(gps_nmea.state.valid_fields, GPS_VALID_COURSE_BIT);
}
@@ -363,7 +379,7 @@ static void nmea_parse_GGA(void)
// ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL);
// FIXME: parse UTC time correctly
double utc_time = strtod(&gps_nmea.msg_buf[i], NULL);
gps.tow = (uint32_t)((utc_time + 1) * 1000);
gps_nmea.state.tow = (uint32_t)((utc_time + 1) * 1000);
// get latitude [ddmm.mmmmm]
nmea_read_until(&i);
@@ -380,7 +396,7 @@ static void nmea_parse_GGA(void)
// convert to radians
lla_f.lat = RadOfDeg(lat);
gps.lla_pos.lat = lat * 1e7; // convert to fixed-point
gps_nmea.state.lla_pos.lat = lat * 1e7; // convert to fixed-point
NMEA_PRINT("p_GGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat);
@@ -399,9 +415,9 @@ static void nmea_parse_GGA(void)
// convert to radians
lla_f.lon = RadOfDeg(lon);
gps.lla_pos.lon = lon * 1e7; // convert to fixed-point
NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps_nmea.state.lla_pos.lon = lon * 1e7; // convert to fixed-point
NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps_nmea.state.tow);
SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_LLA_BIT);
// get position fix status
nmea_read_until(&i);
@@ -417,20 +433,20 @@ static void nmea_parse_GGA(void)
// get number of satellites used in GPS solution
nmea_read_until(&i);
gps.num_sv = atoi(&gps_nmea.msg_buf[i]);
NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv);
gps_nmea.state.num_sv = atoi(&gps_nmea.msg_buf[i]);
NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps_nmea.state.num_sv);
// get HDOP, but we use PDOP from GSA message
nmea_read_until(&i);
//float hdop = strtof(&gps_nmea.msg_buf[i], NULL);
//gps.pdop = hdop * 100;
//gps_nmea.state.pdop = hdop * 100;
// get altitude (in meters) above geoid (MSL)
nmea_read_until(&i);
float hmsl = strtof(&gps_nmea.msg_buf[i], NULL);
gps.hmsl = hmsl * 1000;
NMEA_PRINT("p_GGA() - gps.hmsl=%i\n\r", gps.hmsl);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps_nmea.state.hmsl = hmsl * 1000;
NMEA_PRINT("p_GGA() - gps_nmea.state.hmsl=%i\n\r", gps_nmea.state.hmsl);
SetBit(gps_nmea.state.valid_fields, GPS_VALID_HMSL_BIT);
// get altitude units (always M)
nmea_read_until(&i);
@@ -441,8 +457,8 @@ static void nmea_parse_GGA(void)
NMEA_PRINT("p_GGA() - geoid alt=%f\n\r", geoid);
// height above ellipsoid
lla_f.alt = hmsl + geoid;
gps.lla_pos.alt = lla_f.alt * 1000;
NMEA_PRINT("p_GGA() - gps.alt=%i\n\r", gps.lla_pos.alt);
gps_nmea.state.lla_pos.alt = lla_f.alt * 1000;
NMEA_PRINT("p_GGA() - gps_nmea.state.alt=%i\n\r", gps_nmea.state.lla_pos.alt);
// get seperations units
nmea_read_until(&i);
@@ -453,10 +469,10 @@ static void nmea_parse_GGA(void)
/* convert to ECEF */
struct EcefCoor_f ecef_f;
ecef_of_lla_f(&ecef_f, &lla_f);
gps.ecef_pos.x = ecef_f.x * 100;
gps.ecef_pos.y = ecef_f.y * 100;
gps.ecef_pos.z = ecef_f.z * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps_nmea.state.ecef_pos.x = ecef_f.x * 100;
gps_nmea.state.ecef_pos.y = ecef_f.y * 100;
gps_nmea.state.ecef_pos.z = ecef_f.z * 100;
SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_ECEF_BIT);
}
/**
@@ -513,10 +529,10 @@ static void nmea_parse_GSV(void)
int ch_idx = (cur_sen - 1) * 4 + sat_cnt;
// don't populate svinfos with GLONASS sats for now
if (!is_glonass && ch_idx > 0 && ch_idx < 12) {
gps.svinfos[ch_idx].svid = prn;
gps.svinfos[ch_idx].cno = snr;
gps.svinfos[ch_idx].elev = elev;
gps.svinfos[ch_idx].azim = azim;
gps_nmea.state.svinfos[ch_idx].svid = prn;
gps_nmea.state.svinfos[ch_idx].cno = snr;
gps_nmea.state.svinfos[ch_idx].elev = elev;
gps_nmea.state.svinfos[ch_idx].azim = azim;
}
if (is_glonass) {
NMEA_PRINT("p_GSV() - GLONASS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr);
@@ -525,4 +541,16 @@ static void nmea_parse_GSV(void)
NMEA_PRINT("p_GSV() - GPS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr);
}
}
}
/*
* register callbacks & structs
*/
void nmea_gps_register(void)
{
#ifdef GPS_SECONDARY_NMEA
gps_register_impl(nmea_gps_impl_init, nmea_gps_event, GPS_NMEA_ID, 1);
#else
gps_register_impl(nmea_gps_impl_init, nmea_gps_event, GPS_NMEA_ID, 0);
#endif
}
sw/airborne/subsystems/gps/gps_nmea.h
+25 -17
View File
@@ -31,11 +31,33 @@
#define GPS_NMEA_H
#include "mcu_periph/uart.h"
#include "subsystems/gps.h"
#define GPS_NB_CHANNELS 12
#define GPS_NMEA_NB_CHANNELS 12
#define NMEA_MAXLEN 255
#if GPS_SECONDARY_NMEA
#ifndef NMEA_GPS_LINK
#define NMEA_GPS_LINK GPS_SECONDARY_PORT
#define SecondaryGpsImpl nmea
#endif
#else
#ifndef PrimaryGpsImpl
#define PrimaryGpsImpl nmea
#endif
#endif
#if GPS_PRIMARY_NMEA
#ifndef NMEA_GPS_LINK
#define NMEA_GPS_LINK GPS_PRIMARY_PORT
#endif
#endif
void nmea_gps_impl_init(void);
void nmea_gps_event(void);
extern void nmea_gps_register(void);
void nmea_gps_msg(void);
struct GpsNmea {
bool_t msg_available;
bool_t pos_available;
@@ -45,6 +67,8 @@ struct GpsNmea {
char msg_buf[NMEA_MAXLEN]; ///< buffer for storing one nmea-line
int msg_len;
uint8_t status; ///< line parser status
struct GpsState state;
};
extern struct GpsNmea gps_nmea;
@@ -65,22 +89,6 @@ extern void nmea_parse_prop_init(void);
extern void nmea_parse_prop_msg(void);
extern void gps_nmea_msg(void);
static inline void GpsEvent(void)
{
struct link_device *dev = &((GPS_LINK).device);
if (!gps_nmea.is_configured) {
nmea_configure();
return;
}
while (dev->char_available(dev->periph)) {
nmea_parse_char(dev->get_byte(dev->periph));
if (gps_nmea.msg_available) {
gps_nmea_msg();
}
}
}
/** Read until a certain character, placed here for proprietary includes */
static inline void nmea_read_until(int *i)
{