From ad3ef75fbb2c85b38cfd836a05e83b05ca68ea5f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 24 Mar 2015 16:48:57 +0100 Subject: [PATCH 1/6] [gps] nmea parse GSA --- sw/airborne/subsystems/gps/gps_furuno.c | 4 +-- sw/airborne/subsystems/gps/gps_nmea.c | 47 ++++++++++++++++++++----- sw/airborne/subsystems/gps/gps_nmea.h | 2 ++ 3 files changed, 42 insertions(+), 11 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_furuno.c b/sw/airborne/subsystems/gps/gps_furuno.c index c86275e69b..01e5c05011 100644 --- a/sw/airborne/subsystems/gps/gps_furuno.c +++ b/sw/airborne/subsystems/gps/gps_furuno.c @@ -38,10 +38,10 @@ static const char *gps_furuno_settings[GPS_FURUNO_SETTINGS_NB] = { "PERDCFG,NMEAOUT,GGA,1", // Enable GGA every fix "PERDCFG,NMEAOUT,RMC,1", // Enable RMC every fix "PERDCFG,NMEAOUT,GSA,1", // Enable GSA every fix - "PERDCFG,NMEAOUT,GNS,0", // Disable GSA + "PERDCFG,NMEAOUT,GNS,0", // Disable GNS "PERDCFG,NMEAOUT,ZDA,0", // Disable ZDA "PERDCFG,NMEAOUT,GSV,0", // Disable GSV - "PERDCFG,NMEAOUT,GST,0", // Disable ZDA + "PERDCFG,NMEAOUT,GST,0", // Disable GST "PERDAPI,CROUT,V" // Enable raw velocity }; diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index fa4445538b..7cb4643e0f 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -56,6 +56,7 @@ static void nmea_parse_GGA(void); void gps_impl_init(void) { + gps.nb_channels = GPS_NB_CHANNELS; gps_nmea.msg_available = FALSE; gps_nmea.pos_available = FALSE; gps_nmea.gps_nb_ovrn = 0; @@ -195,9 +196,37 @@ static void nmea_parse_GSA(void) NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix); nmea_read_until(&i); - //int satcount = 0; + // up to 12 PRNs of satellites used for fix + int satcount = 0; + int prn_cnt; + for (prn_cnt = 0; prn_cnt < 12; prn_cnt++) { + if (gps_nmea.msg_buf[i] != ',') { + gps.svinfos[prn_cnt].svid = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GPGSA() - PRN %i=%i\n\r", satcount, gps.svinfos[prn_cnt].svid); + satcount++; + } + else { + gps.svinfos[prn_cnt].svid = 0; + } + nmea_read_until(&i); + } + + // PDOP + float pdop = strtof(&gps_nmea.msg_buf[i], NULL); + gps.pdop = pdop * 100; + NMEA_PRINT("p_GPGSA() - pdop=%f\n\r", pdop); + nmea_read_until(&i); + + // HDOP + float hdop = strtof(&gps_nmea.msg_buf[i], NULL); + NMEA_PRINT("p_GPGSA() - hdop=%f\n\r", hdop); + nmea_read_until(&i); + + // VDOP + float vdop = strtof(&gps_nmea.msg_buf[i], NULL); + NMEA_PRINT("p_GPGSA() - vdop=%f\n\r", vdop); + nmea_read_until(&i); - // TODO: get sateline-numbers for gps_svinfos } /** @@ -234,13 +263,13 @@ static void nmea_parse_RMC(void) nmea_read_until(&i); double speed = strtod(&gps_nmea.msg_buf[i], NULL); gps.gspeed = speed * 1.852 * 100 / (60 * 60); - NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed * 1000), (gps.gspeed * 1000)); + NMEA_PRINT("p_GPRMC() - ground-speed=%f knot = %d cm/s\n\r", (speed * 1000), (gps.gspeed * 1000)); // get course nmea_read_until(&i); double course = strtod(&gps_nmea.msg_buf[i], NULL); gps.course = RadOfDeg(course) * 1e7; - NMEA_PRINT("COURSE: %d \n\r", gps_course); + NMEA_PRINT("COURSE: %d \n\r", gps.course); } @@ -282,7 +311,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 - NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat * 1000), lla_f.lat); + NMEA_PRINT("p_GPGGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat); // get longitude [ddmm.mmmmm] @@ -301,7 +330,7 @@ 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_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow); + NMEA_PRINT("p_GPGGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow); // get position fix status nmea_read_until(&i); @@ -320,10 +349,10 @@ static void nmea_parse_GGA(void) gps.num_sv = atoi(&gps_nmea.msg_buf[i]); NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv); - // we use HDOP here, as the PDOP is not in the message + // 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; + //float hdop = strtof(&gps_nmea.msg_buf[i], NULL); + //gps.pdop = hdop * 100; // get altitude (in meters) above geoid (MSL) nmea_read_until(&i); diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index 6353c00fd5..b710f509ab 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -36,6 +36,8 @@ #define NMEA_PRINT(...) {}; #endif +#define GPS_NB_CHANNELS 12 + #define NMEA_MAXLEN 255 struct GpsNmea { From be177ac6db9f2c3929d41501b0ff18ffe2e90203 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 24 Mar 2015 16:51:45 +0100 Subject: [PATCH 2/6] [gps] NMEA_PRINT for debugging --- sw/airborne/subsystems/gps/gps_nmea.c | 8 ++++++++ sw/airborne/subsystems/gps/gps_nmea.h | 4 ---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 7cb4643e0f..5ac2fd4c80 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -47,6 +47,14 @@ #include #include +#ifndef NMEA_PRINT +#define NMEA_PRINT(...) {}; +#endif + +#if NMEA_PRINT == printf +#include +#endif + struct GpsNmea gps_nmea; static void nmea_parse_GSA(void); diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index b710f509ab..07ae0c3225 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -32,10 +32,6 @@ #include "mcu_periph/uart.h" -#ifndef DEBUG_NMEA -#define NMEA_PRINT(...) {}; -#endif - #define GPS_NB_CHANNELS 12 #define NMEA_MAXLEN 255 From 25b7d167957de3e41939383ef465d061281dc8ff Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 24 Mar 2015 17:38:48 +0100 Subject: [PATCH 3/6] [gps] nmea parse GSV message --- sw/airborne/subsystems/gps/gps_nmea.c | 81 ++++++++++++++++++++++++--- sw/airborne/subsystems/gps/gps_nmea.h | 3 +- 2 files changed, 74 insertions(+), 10 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 5ac2fd4c80..3589a0350b 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -60,6 +60,7 @@ struct GpsNmea gps_nmea; static void nmea_parse_GSA(void); static void nmea_parse_RMC(void); static void nmea_parse_GGA(void); +static void nmea_parse_GSV(void); void gps_impl_init(void) @@ -67,6 +68,7 @@ void gps_impl_init(void) gps.nb_channels = GPS_NB_CHANNELS; gps_nmea.msg_available = FALSE; gps_nmea.pos_available = FALSE; + gps_nmea.have_gsv = FALSE; gps_nmea.gps_nb_ovrn = 0; gps_nmea.msg_len = 0; nmea_parse_prop_init(); @@ -105,22 +107,24 @@ void nmea_parse_msg(void) if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "RMC", 3)) { gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("parsing RMC: \"%s\" \n\r", gps_nmea.msg_buf); - NMEA_PRINT("RMC"); + NMEA_PRINT("RMC: \"%s\" \n\r", gps_nmea.msg_buf); nmea_parse_RMC(); } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GGA", 3)) { gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r", gps_nmea.msg_buf); - NMEA_PRINT("GGA"); + NMEA_PRINT("GGA: \"%s\" \n\r", gps_nmea.msg_buf); nmea_parse_GGA(); } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSA", 3)) { gps_nmea.msg_buf[gps_nmea.msg_len] = 0; NMEA_PRINT("GSA: \"%s\" \n\r", gps_nmea.msg_buf); - NMEA_PRINT("GSA"); nmea_parse_GSA(); + } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSV", 3)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + gps_nmea.have_gsv = TRUE; + NMEA_PRINT("GSV: \"%s\" \n\r", gps_nmea.msg_buf); + nmea_parse_GSV(); } else { gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("Propriarty message: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf); + NMEA_PRINT("Other/propriarty message: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf); nmea_parse_prop_msg(); } @@ -209,12 +213,17 @@ static void nmea_parse_GSA(void) int prn_cnt; for (prn_cnt = 0; prn_cnt < 12; prn_cnt++) { if (gps_nmea.msg_buf[i] != ',') { - gps.svinfos[prn_cnt].svid = atoi(&gps_nmea.msg_buf[i]); - NMEA_PRINT("p_GPGSA() - PRN %i=%i\n\r", satcount, gps.svinfos[prn_cnt].svid); + int prn = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GPGSA() - PRN %i=%i\n\r", satcount, prn); + if (!gps_nmea.have_gsv) { + gps.svinfos[prn_cnt].svid = prn; + } satcount++; } else { - gps.svinfos[prn_cnt].svid = 0; + if (!gps_nmea.have_gsv) { + gps.svinfos[prn_cnt].svid = 0; + } } nmea_read_until(&i); } @@ -401,3 +410,57 @@ static void nmea_parse_GGA(void) gps.ecef_pos.y = ecef_f.y * 100; gps.ecef_pos.z = ecef_f.z * 100; } + +/** + * parse GPGSV-nmea-messages stored in + * nmea_msg_buf . + */ +static void nmea_parse_GSV(void) +{ + int i = 6; // current position in the message, start after: GPGSA, + + // attempt to reject empty packets right away + if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') { + NMEA_PRINT("p_GPGSV() - skipping empty message\n\r"); + return; + } + + // total sentences + int nb_sen = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GPGSV() - %i sentences\n\r", nb_sen); + nmea_read_until(&i); + + // current sentence + int cur_sen = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GPGSV() - sentence=%i\n\r", cur_sen); + nmea_read_until(&i); + + // num satellites in view + int num_sat = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GPGSV() - num_sat=%i\n\r", num_sat); + nmea_read_until(&i); + + // up to 4 sats per sentence + int sat_cnt; + for (sat_cnt = 0; sat_cnt < 4; sat_cnt++) { + if (gps_nmea.msg_buf[i] == ',') break; + // 4 fields per sat: PRN, elevation (deg), azimuth (deg), SNR + int prn = atoi(&gps_nmea.msg_buf[i]); + nmea_read_until(&i); + int elev = atoi(&gps_nmea.msg_buf[i]); + nmea_read_until(&i); + int azim = atoi(&gps_nmea.msg_buf[i]); + nmea_read_until(&i); + int snr = atoi(&gps_nmea.msg_buf[i]); + nmea_read_until(&i); + + int ch_idx = (cur_sen - 1) * 4 + sat_cnt; + if (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; + } + NMEA_PRINT("p_GPGSV() - SAT %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr); + } +} diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index 07ae0c3225..ffbe81f570 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -39,7 +39,8 @@ struct GpsNmea { bool_t msg_available; bool_t pos_available; - uint8_t gps_nb_ovrn; // number if incomplete nmea-messages + bool_t have_gsv; ///< flag set to TRUE if GPGSV message received + uint8_t gps_nb_ovrn; ///< number if incomplete nmea-messages char msg_buf[NMEA_MAXLEN]; ///< buffer for storing one nmea-line int msg_len; }; From 830dd4ac62711c39a7734381bb1e4371a900b8fb Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 24 Mar 2015 18:04:24 +0100 Subject: [PATCH 4/6] [gps] nmea: differentiate between GPS and GLONASS sats --- sw/airborne/subsystems/gps/gps_nmea.c | 32 +++++++++++++++++++-------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 3589a0350b..92f5b1ccdb 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -412,32 +412,40 @@ static void nmea_parse_GGA(void) } /** - * parse GPGSV-nmea-messages stored in - * nmea_msg_buf . + * Parse GSV-nmea-messages. + * Msg stored in gps_nmea.msg_buf. */ static void nmea_parse_GSV(void) { - int i = 6; // current position in the message, start after: GPGSA, + int i = 6; // current position in the message, start after: GxGSA, // attempt to reject empty packets right away if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') { - NMEA_PRINT("p_GPGSV() - skipping empty message\n\r"); + NMEA_PRINT("p_GSV() - skipping empty message\n\r"); return; } + // check what satellites this messages contains + // GPGSV -> GPS + // GLGSV -> GLONASS + bool_t is_glonass = FALSE; + if (!strncmp(&gps_nmea.msg_buf[0] , "GL", 2)) { + is_glonass = TRUE; + } + // total sentences int nb_sen = atoi(&gps_nmea.msg_buf[i]); - NMEA_PRINT("p_GPGSV() - %i sentences\n\r", nb_sen); + NMEA_PRINT("p_GSV() - %i sentences\n\r", nb_sen); nmea_read_until(&i); // current sentence int cur_sen = atoi(&gps_nmea.msg_buf[i]); - NMEA_PRINT("p_GPGSV() - sentence=%i\n\r", cur_sen); + NMEA_PRINT("p_GSV() - sentence=%i\n\r", cur_sen); nmea_read_until(&i); // num satellites in view int num_sat = atoi(&gps_nmea.msg_buf[i]); - NMEA_PRINT("p_GPGSV() - num_sat=%i\n\r", num_sat); + NMEA_PRINT("p_GSV() - num_sat=%i\n\r", num_sat); nmea_read_until(&i); // up to 4 sats per sentence @@ -455,12 +463,18 @@ static void nmea_parse_GSV(void) nmea_read_until(&i); int ch_idx = (cur_sen - 1) * 4 + sat_cnt; - if (ch_idx > 0 && ch_idx < 12) { + // 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; } - NMEA_PRINT("p_GPGSV() - SAT %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr); + 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); + } + else { + NMEA_PRINT("p_GSV() - GPS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr); + } } } From d7568ff2b00abdcd05a201e980940e4a788e91df Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 24 Mar 2015 18:30:07 +0100 Subject: [PATCH 5/6] [gps] nmea: get geoid separation --- sw/airborne/subsystems/gps/gps_nmea.c | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 92f5b1ccdb..5947bb9991 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -373,17 +373,22 @@ static void nmea_parse_GGA(void) // get altitude (in meters) above geoid (MSL) nmea_read_until(&i); - // lla_f.alt should actuall be height above ellipsoid, - // but since we don't get that, use hmsl instead - lla_f.alt = strtof(&gps_nmea.msg_buf[i], NULL); - gps.hmsl = lla_f.alt * 1000; - gps.lla_pos.alt = gps.hmsl; - NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl); + float hmsl = strtof(&gps_nmea.msg_buf[i], NULL); + gps.hmsl = hmsl * 1000; + NMEA_PRINT("p_GPGGA() - gps.hmsl=%i\n\r", gps.hmsl); - // get altitude units (allways M) + // get altitude units (always M) nmea_read_until(&i); + // get geoid seperation nmea_read_until(&i); + float geoid = strtof(&gps_nmea.msg_buf[i], NULL); + NMEA_PRINT("p_GPGGA() - 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_GPGGA() - gps.alt=%i\n\r", gps.lla_pos.alt); + // get seperations units nmea_read_until(&i); // get DGPS age From 5f6649a64992959a8e6604076444915ae70da7de Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 25 Mar 2015 14:49:50 +0100 Subject: [PATCH 6/6] [gps] nmea: suppress some unused variable warnings --- sw/airborne/subsystems/gps/gps_nmea.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 5947bb9991..776cb51fdd 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -235,12 +235,12 @@ static void nmea_parse_GSA(void) nmea_read_until(&i); // HDOP - float hdop = strtof(&gps_nmea.msg_buf[i], NULL); + float hdop __attribute__((unused)) = strtof(&gps_nmea.msg_buf[i], NULL); NMEA_PRINT("p_GPGSA() - hdop=%f\n\r", hdop); nmea_read_until(&i); // VDOP - float vdop = strtof(&gps_nmea.msg_buf[i], NULL); + float vdop __attribute__((unused)) = strtof(&gps_nmea.msg_buf[i], NULL); NMEA_PRINT("p_GPGSA() - vdop=%f\n\r", vdop); nmea_read_until(&i); @@ -439,7 +439,7 @@ static void nmea_parse_GSV(void) } // total sentences - int nb_sen = atoi(&gps_nmea.msg_buf[i]); + int nb_sen __attribute__((unused)) = atoi(&gps_nmea.msg_buf[i]); NMEA_PRINT("p_GSV() - %i sentences\n\r", nb_sen); nmea_read_until(&i); @@ -449,7 +449,7 @@ static void nmea_parse_GSV(void) nmea_read_until(&i); // num satellites in view - int num_sat = atoi(&gps_nmea.msg_buf[i]); + int num_sat __attribute__((unused)) = atoi(&gps_nmea.msg_buf[i]); NMEA_PRINT("p_GSV() - num_sat=%i\n\r", num_sat); nmea_read_until(&i);