diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 82334e56a..7c5498806 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -199,6 +199,8 @@ SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c SRC += $(OPUAVSYNTHDIR)/cameradesired.c SRC += $(OPUAVSYNTHDIR)/gpsposition.c +SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c +SRC += $(OPUAVSYNTHDIR)/gpssettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/receiveractivity.c diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index e4b70e931..bcf0b0b23 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -58,7 +58,8 @@ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_GPS #define PIOS_GPS_MINIMAL - +#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */ +#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */ #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS diff --git a/flight/Libraries/inc/NMEA.h b/flight/Libraries/inc/NMEA.h deleted file mode 100644 index c3f8b66d9..000000000 --- a/flight/Libraries/inc/NMEA.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file NMEA.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief GPS module, handles GPS and NMEA stream - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program 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 3 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef NMEA_H -#define NMEA_H - -#include -#include - -#define NMEA_MAX_PACKET_LENGTH 96 - -extern bool NMEA_update_position(char *nmea_sentence); -extern bool NMEA_checksum(char *nmea_sentence); - -#endif /* NMEA_H */ diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 716e47aa3..5851ac10d 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -33,18 +33,19 @@ #include "openpilot.h" #include "GPS.h" -#include - -#include "NMEA.h" - #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" +#include "gpsvelocity.h" +#include "gpssettings.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" #include "hwsettings.h" +#include "NMEA.h" +#include "UBX.h" + // **************** // Private functions @@ -61,16 +62,18 @@ static float GravityAccel(float latitude, float longitude, float altitude); // Private constants #define GPS_TIMEOUT_MS 500 -#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed -// same as in COM buffer #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 800 + #define STACK_SIZE_BYTES 750 +#else +#if defined(PIOS_GPS_MINIMAL) + #define STACK_SIZE_BYTES 500 #else #define STACK_SIZE_BYTES 650 -#endif +#endif // PIOS_GPS_MINIMAL +#endif // PIOS_GPS_SETS_HOMELOCATION #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) @@ -86,9 +89,8 @@ static char* gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; -static uint32_t numUpdates; -static uint32_t numChecksumErrors; -static uint32_t numParsingErrors; + +static struct GPS_RX_STATS gpsRxStats; // **************** /** @@ -120,6 +122,7 @@ int32_t GPSStart(void) int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; + uint8_t gpsProtocol; #ifdef MODULE_GPS_BUILTIN gpsEnabled = true; @@ -137,6 +140,7 @@ int32_t GPSInitialize(void) if (gpsPort && gpsEnabled) { GPSPositionInitialize(); + GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); @@ -145,8 +149,21 @@ int32_t GPSInitialize(void) HomeLocationInitialize(); #endif updateSettings(); + } - gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + if (gpsPort && gpsEnabled) { + GPSSettingsInitialize(); + GPSSettingsDataProtocolGet(&gpsProtocol); + switch (gpsProtocol) { + case GPSSETTINGS_DATAPROTOCOL_NMEA: + gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + break; + case GPSSETTINGS_DATAPROTOCOL_UBX: + gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); + break; + default: + gps_rx_buffer = NULL; + } PIOS_Assert(gps_rx_buffer); return 0; @@ -165,140 +182,75 @@ MODULE_INITCALL(GPSInitialize, GPSStart) static void gpsTask(void *parameters) { portTickType xDelay = 100 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; - GPSPositionData GpsData; - - uint8_t rx_count = 0; - bool start_flag = false; - bool found_cr = false; - int32_t gpsRxOverflow = 0; - - numUpdates = 0; - numChecksumErrors = 0; - numParsingErrors = 0; + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + + GPSPositionData gpsposition; + uint8_t gpsProtocol; + + GPSSettingsDataProtocolGet(&gpsProtocol); timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; + GPSPositionGet(&gpsposition); // Loop forever while (1) { uint8_t c; - // NMEA or SINGLE-SENTENCE GPS mode // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { - - // detect start while acquiring stream - if (!start_flag && (c == '$')) - { - start_flag = true; - found_cr = false; - rx_count = 0; + int res; + switch (gpsProtocol) { +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + case GPSSETTINGS_DATAPROTOCOL_NMEA: + res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); + break; +#endif +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + case GPSSETTINGS_DATAPROTOCOL_UBX: + res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); + break; +#endif + default: + res = NO_PARSER; // this should not happen + break; } - else - if (!start_flag) - continue; - - if (rx_count >= NMEA_MAX_PACKET_LENGTH) - { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - start_flag = false; - found_cr = false; - rx_count = 0; - } - else - { - gps_rx_buffer[rx_count] = c; - rx_count++; - } - - // look for ending '\r\n' sequence - if (!found_cr && (c == '\r') ) - found_cr = true; - else - if (found_cr && (c != '\n') ) - found_cr = false; // false end flag - else - if (found_cr && (c == '\n') ) - { - // The NMEA functions require a zero-terminated string - // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n - gps_rx_buffer[rx_count-2] = 0; - // prepare to parse next sentence - start_flag = false; - found_cr = false; - rx_count = 0; - // Our rxBuffer must look like this now: - // [0] = '$' - // ... = zero or more bytes of sentence payload - // [end_pos - 1] = '\r' - // [end_pos] = '\n' - // - // Prepare to consume the sentence from the buffer - - // Validate the checksum over the sentence - if (!NMEA_checksum(&gps_rx_buffer[1])) - { // Invalid checksum. May indicate dropped characters on Rx. - //PIOS_DEBUG_PinHigh(2); - ++numChecksumErrors; - //PIOS_DEBUG_PinLow(2); - } - else - { // Valid checksum, use this packet to update the GPS position - if (!NMEA_update_position(&gps_rx_buffer[1])) { - //PIOS_DEBUG_PinHigh(2); - ++numParsingErrors; - //PIOS_DEBUG_PinLow(2); - } - else - ++numUpdates; - - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } + if (res == PARSER_COMPLETE) { + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; } } // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) - { // we have not received any valid GPS sentences for a while. + if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { + // we have not received any valid GPS sentences for a while. // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - - GPSPositionGet(&GpsData); - GpsData.Status = GPSPOSITION_STATUS_NOGPS; - GPSPositionSet(&GpsData); + uint8_t status = GPSPOSITION_STATUS_NOGPS; + GPSPositionStatusSet(&status); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - - } - else - { // we appear to be receiving GPS sentences OK, we've had an update - - GPSPositionGet(&GpsData); - -#ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationData home; - HomeLocationGet(&home); - - if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE)) - setHomeLocation(&GpsData); -#endif - + } else { + // we appear to be receiving GPS sentences OK, we've had an update //criteria for GPS-OK taken from this post... //http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 - if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7)) + if ((gpsposition.PDOP < 3.5) && (gpsposition.Satellites >= 7) && + (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); - else - if (GpsData.Status == GPSPOSITION_STATUS_FIX3D) - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); +#ifdef PIOS_GPS_SETS_HOMELOCATION + HomeLocationData home; + HomeLocationGet(&home); + + if (home.Set == HOMELOCATION_SET_FALSE) + setHomeLocation(&gpsposition); +#endif + } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); + else + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } } @@ -338,14 +290,6 @@ static void setHomeLocation(GPSPositionData * gpsData) // Compute home ECEF coordinates and the rotation matrix into NED double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) }; - double ECEF[3]; - RneFromLLA(LLA, (float (*)[3])home.RNE); - LLA2ECEF(LLA, ECEF); - // TODO: Currently UAVTalk only supports float but these conversions use double - // need to find out if they require that precision and if so extend UAVTAlk - home.ECEF[0] = (int32_t) (ECEF[0] * 100); - home.ECEF[1] = (int32_t) (ECEF[1] * 100); - home.ECEF[2] = (int32_t) (ECEF[2] * 100); // Compute magnetic flux direction at home location if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0) diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 59593b807..150949e8e 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -30,10 +30,14 @@ #include "openpilot.h" #include "pios.h" -#include "NMEA.h" + +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + #include "gpsposition.h" +#include "NMEA.h" #include "gpstime.h" #include "gpssatellites.h" +#include "GPS.h" //#define ENABLE_DEBUG_MSG ///< define to enable debug-messages #define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages @@ -43,7 +47,7 @@ // Debugging #ifdef ENABLE_DEBUG_MSG //#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages -//#define DEBUG_PARS ///< define to display the incoming NMEA messages split into its parameters +//#define DEBUG_PARMS ///< define to display the incoming NMEA messages split into its parameters //#define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages //#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages //#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages @@ -63,7 +67,6 @@ struct nmea_parser { const char *prefix; bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - uint32_t cnt; }; static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); @@ -75,50 +78,125 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); #endif //PIOS_GPS_MINIMAL - -static struct nmea_parser nmea_parsers[] = { +const static struct nmea_parser nmea_parsers[] = { { .prefix = "GPGGA", .handler = nmeaProcessGPGGA, - .cnt = 0, }, { .prefix = "GPVTG", .handler = nmeaProcessGPVTG, - .cnt = 0, }, { .prefix = "GPGSA", .handler = nmeaProcessGPGSA, - .cnt = 0, }, { .prefix = "GPRMC", .handler = nmeaProcessGPRMC, - .cnt = 0, }, #if !defined(PIOS_GPS_MINIMAL) { .prefix = "GPZDA", .handler = nmeaProcessGPZDA, - .cnt = 0, }, { .prefix = "GPGSV", .handler = nmeaProcessGPGSV, - .cnt = 0, }, #endif //PIOS_GPS_MINIMAL }; -static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) +int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +{ + static uint8_t rx_count = 0; + static bool start_flag = false; + static bool found_cr = false; + + // detect start while acquiring stream + if (!start_flag && (c == '$')) // NMEA identifier found + { + start_flag = true; + found_cr = false; + rx_count = 0; + } + else + if (!start_flag) + return PARSER_ERROR; + + if (rx_count >= NMEA_MAX_PACKET_LENGTH) + { + // The buffer is already full and we haven't found a valid NMEA sentence. + // Flush the buffer and note the overflow event. + gpsRxStats->gpsRxOverflow++; + start_flag = false; + found_cr = false; + rx_count = 0; + return PARSER_OVERRUN; + } + else + { + gps_rx_buffer[rx_count] = c; + rx_count++; + } + + // look for ending '\r\n' sequence + if (!found_cr && (c == '\r') ) + found_cr = true; + else + if (found_cr && (c != '\n') ) + found_cr = false; // false end flag + else + if (found_cr && (c == '\n') ) + { + // The NMEA functions require a zero-terminated string + // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n + gps_rx_buffer[rx_count-2] = 0; + + // prepare to parse next sentence + start_flag = false; + found_cr = false; + rx_count = 0; + // Our rxBuffer must look like this now: + // [0] = '$' + // ... = zero or more bytes of sentence payload + // [end_pos - 1] = '\r' + // [end_pos] = '\n' + // + // Prepare to consume the sentence from the buffer + + // Validate the checksum over the sentence + if (!NMEA_checksum(&gps_rx_buffer[1])) + { // Invalid checksum. May indicate dropped characters on Rx. + //PIOS_DEBUG_PinHigh(2); + gpsRxStats->gpsRxChkSumError++; + //PIOS_DEBUG_PinLow(2); + return PARSER_ERROR; + } + else + { // Valid checksum, use this packet to update the GPS position + if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { + //PIOS_DEBUG_PinHigh(2); + gpsRxStats->gpsRxParserError++; + //PIOS_DEBUG_PinLow(2); + } + else + gpsRxStats->gpsRxReceived++;; + + return PARSER_COMPLETE; + } + } + return PARSER_INCOMPLETE; +} + +const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) { if (!prefix) { return (NULL); } for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) { - struct nmea_parser *parser = &nmea_parsers[i]; + const struct nmea_parser *parser = &nmea_parsers[i]; /* Use strcmp to check for exact equality over the entire prefix */ if (!strcmp(prefix, parser->prefix)) { @@ -234,6 +312,10 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool PIOS_DEBUG_Assert(nmea_latlon); PIOS_DEBUG_Assert(latlon); + if (*nmea_latlon == '\0') { /* empty lat/lon field */ + return false; + } + if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { return false; } @@ -285,7 +367,7 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool * \return true if the sentence was successfully parsed * \return false if any errors were encountered with the parsing */ -bool NMEA_update_position(char *nmea_sentence) +bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) { char* p = nmea_sentence; char* params[MAX_NB_PARAMS]; @@ -327,7 +409,7 @@ bool NMEA_update_position(char *nmea_sentence) #endif // The first parameter is the message name, lets see if we find a parser for it - struct nmea_parser *parser; + const struct nmea_parser *parser; parser = NMEA_find_parser_by_prefix(params[0]); if (!parser) { // No parser found @@ -335,18 +417,22 @@ bool NMEA_update_position(char *nmea_sentence) return false; } - parser->cnt++; #ifdef DEBUG_MGSID_IN - DEBUG_MSG("%s %d ", params[0], parser->cnt); + DEBUG_MSG("%s %d ", params[0]); #endif - // Send the message to then parser and get it update the GpsData - GPSPositionData GpsData; - GPSPositionGet(&GpsData); - bool gpsDataUpdated; + // Send the message to the parser and get it update the GpsData + // Information from various different NMEA messages are temporarily + // cumulated in the GpsData structure. An actual GPSPosition update + // is triggered by GGA messages only. This message type sets the + // gpsDataUpdated flag to request this. + bool gpsDataUpdated = false; - if (!parser->handler(&GpsData, &gpsDataUpdated, params, nbParams)) { + if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { // Parse failed DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); + if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { + GPSPositionSet(GpsData); + } return false; } @@ -356,7 +442,7 @@ bool NMEA_update_position(char *nmea_sentence) #ifdef DEBUG_MGSID_IN DEBUG_MSG("U"); #endif - GPSPositionSet(&GpsData); + GPSPositionSet(GpsData); } #ifdef DEBUG_MGSID_IN @@ -390,6 +476,13 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch *gpsDataUpdated = true; + // check for invalid GPS fix + // do this first to make sure we get this information, even if later checks exit + // this function early + if (param[6][0] == '0') { + GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX + } + // get latitude [DDMM.mmmmm] [N|S] if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) { return false; @@ -431,7 +524,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch DEBUG_MSG(" DateOfFix=%s\n\n", param[9]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = false; #if !defined(PIOS_GPS_MINIMAL) GPSTimeData gpst; @@ -444,6 +537,11 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch gpst.Hour = (int)hms / 10000; #endif //PIOS_GPS_MINIMAL + // don't process void sentences + if (param[2][0] == 'V') { + return false; + } + // get latitude [DDMM.mmmmm] [N|S] if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) { return false; @@ -455,7 +553,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch } // get speed in knots - GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444; // to m/s + GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s // get True course GpsData->Heading = NMEA_real_to_float(param[8]); @@ -489,10 +587,10 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = false; GpsData->Heading = NMEA_real_to_float(param[1]); - GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444; // to m/s + GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s return true; } @@ -555,7 +653,7 @@ static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, ch uint8_t nbSentences = atoi(param[1]); uint8_t currSentence = atoi(param[2]); - *gpsDataUpdated = true; + *gpsDataUpdated = false; if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) return false; @@ -639,7 +737,7 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch DEBUG_MSG(" VDOP=%s\n", param[17]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = false; switch (atoi(param[2])) { case 1: @@ -669,3 +767,4 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch return true; } +#endif // PIOS_INCLUDE_GPS_NMEA_PARSER diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c new file mode 100644 index 000000000..1bf9a0979 --- /dev/null +++ b/flight/Modules/GPS/UBX.c @@ -0,0 +1,341 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information (UBX binary format) + * @{ + * + * @file UBX.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief GPS module, handles GPS and NMEA stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "pios.h" + +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + +#include "UBX.h" +#include "gps.h" + +// parse incoming character stream for messages in UBX binary format + +int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) +{ + enum proto_states { + START, + UBX_SY2, + UBX_CLASS, + UBX_ID, + UBX_LEN1, + UBX_LEN2, + UBX_PAYLOAD, + UBX_CHK1, + UBX_CHK2, + FINISHED + }; + + static enum proto_states proto_state = START; + static uint8_t rx_count = 0; + struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; + + switch (proto_state) { + case START: // detect protocol + if (c == UBX_SYNC1) // first UBX sync char found + proto_state = UBX_SY2; + break; + case UBX_SY2: + if (c == UBX_SYNC2) // second UBX sync char found + proto_state = UBX_CLASS; + else + proto_state = START; // reset state + break; + case UBX_CLASS: + ubx->header.class = c; + proto_state = UBX_ID; + break; + case UBX_ID: + ubx->header.id = c; + proto_state = UBX_LEN1; + break; + case UBX_LEN1: + ubx->header.len = c; + proto_state = UBX_LEN2; + break; + case UBX_LEN2: + ubx->header.len += (c << 8); + if (ubx->header.len > sizeof(UBXPayload)) { + gpsRxStats->gpsRxOverflow++; + proto_state = START; + } else { + rx_count = 0; + proto_state = UBX_PAYLOAD; + } + break; + case UBX_PAYLOAD: + if (rx_count < ubx->header.len) { + ubx->payload.payload[rx_count] = c; + if (++rx_count == ubx->header.len) + proto_state = UBX_CHK1; + } else { + gpsRxStats->gpsRxOverflow++; + proto_state = START; + } + break; + case UBX_CHK1: + ubx->header.ck_a = c; + proto_state = UBX_CHK2; + break; + case UBX_CHK2: + ubx->header.ck_b = c; + if (checksum_ubx_message(ubx)) { // message complete and valid + parse_ubx_message(ubx, GpsData); + proto_state = FINISHED; + } else { + gpsRxStats->gpsRxChkSumError++; + proto_state = START; + } + break; + default: break; + } + + if (proto_state == START) + return PARSER_ERROR; // parser couldn't use this byte + else if (proto_state == FINISHED) { + gpsRxStats->gpsRxReceived++; + proto_state = START; + return PARSER_COMPLETE; // message complete & processed + } + + return PARSER_INCOMPLETE; // message not (yet) complete +} + + +// Keep track of various GPS messages needed to make up a single UAVO update +// time-of-week timestamp is used to correlate matching messages + +#define POSLLH_RECEIVED (1 << 0) +#define STATUS_RECEIVED (1 << 1) +#define DOP_RECEIVED (1 << 2) +#define VELNED_RECEIVED (1 << 3) +#define SOL_RECEIVED (1 << 4) +#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED) +#define NONE_RECEIVED 0 + +static struct msgtracker{ + uint32_t currentTOW; // TOW of the message set currently in progress + uint8_t msg_received; // keep track of received message types + } msgtracker; + +// Check if a message belongs to the current data set and register it as 'received' +bool check_msgtracker (uint32_t tow, uint8_t msg_flag) +{ + + if (tow > msgtracker.currentTOW ? true // start of a new message set + : (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured + msgtracker.currentTOW = tow; + msgtracker.msg_received = NONE_RECEIVED; + } else if (tow < msgtracker.currentTOW) // message outdated (don't process) + return false; + + msgtracker.msg_received |= msg_flag; // register reception of this msg type + return true; +} + +bool checksum_ubx_message (struct UBXPacket *ubx) +{ + int i; + uint8_t ck_a, ck_b; + + ck_a = ubx->header.class; + ck_b = ck_a; + + ck_a += ubx->header.id; + ck_b += ck_a; + + ck_a += ubx->header.len & 0xff; + ck_b += ck_a; + + ck_a += ubx->header.len >> 8; + ck_b += ck_a; + + for (i = 0; i < ubx->header.len; i++) { + ck_a += ubx->payload.payload[i]; + ck_b += ck_a; + } + + if (ubx->header.ck_a == ck_a && + ubx->header.ck_b == ck_b) + return true; + else + return false; + +} + +void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) +{ + if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsPosition->Altitude = (float)posllh->hMSL*0.001f; + GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f; + GpsPosition->Latitude = posllh->lat; + GpsPosition->Longitude = posllh->lon; + } + } +} + +void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) +{ + if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { + GpsPosition->Satellites = sol->numSV; + + if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { + switch (sol->gpsFix) { + case STATUS_GPSFIX_2DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; + break; + case STATUS_GPSFIX_3DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } + } + else // fix is not valid so we make sure to treat is as NOFIX + GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } +} + +void parse_ubx_nav_dop (struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) +{ + if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { + GpsPosition->HDOP = (float)dop->hDOP * 0.01f; + GpsPosition->VDOP = (float)dop->vDOP * 0.01f; + GpsPosition->PDOP = (float)dop->pDOP * 0.01f; + } +} + +void parse_ubx_nav_velned (struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) +{ + GPSVelocityData GpsVelocity; + + if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsVelocity.North = (float)velned->velN/100.0f; + GpsVelocity.East = (float)velned->velE/100.0f; + GpsVelocity.Down = (float)velned->velD/100.0f; + GPSVelocitySet(&GpsVelocity); + GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; + GpsPosition->Heading = (float)velned->heading * 1.0e-5f; + } + } +} + +#if !defined(PIOS_GPS_MINIMAL) +void parse_ubx_nav_timeutc (UBX_NAV_TIMEUTC *timeutc) +{ + if (!(timeutc->valid & TIMEUTC_VALIDUTC)) + return; + + GPSTimeData GpsTime; + + GpsTime.Year = timeutc->year; + GpsTime.Month = timeutc->month; + GpsTime.Day = timeutc->day; + GpsTime.Hour = timeutc->hour; + GpsTime.Minute = timeutc->min; + GpsTime.Second = timeutc->sec; + + GPSTimeSet(&GpsTime); +} +#endif + +#if !defined(PIOS_GPS_MINIMAL) +void parse_ubx_nav_svinfo (UBX_NAV_SVINFO *svinfo) +{ + uint8_t chan; + GPSSatellitesData svdata; + + svdata.SatsInView = 0; + for (chan = 0; chan < svinfo->numCh; chan++) { + if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { + svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; + svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; + svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; + svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; + svdata.SatsInView++; + } + + } + // fill remaining slots (if any) + for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { + svdata.Azimuth[chan] = (float)0.0f; + svdata.Elevation[chan] = (float)0.0f; + svdata.PRN[chan] = 0; + svdata.SNR[chan] = 0; + } + + GPSSatellitesSet(&svdata); +} +#endif + +// UBX message parser +// returns UAVObjectID if a UAVObject structure is ready for further processing + +uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition) +{ + uint32_t id = 0; + + switch (ubx->header.class) { + case UBX_CLASS_NAV: + switch (ubx->header.id) { + case UBX_ID_POSLLH: + parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition); + break; + case UBX_ID_DOP: + parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition); + break; + case UBX_ID_SOL: + parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition); + break; + case UBX_ID_VELNED: + parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition); + break; +#if !defined(PIOS_GPS_MINIMAL) + case UBX_ID_TIMEUTC: + parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc); + break; + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo); + break; +#endif + } + break; + } + if (msgtracker.msg_received == ALL_RECEIVED) { + GPSPositionSet(GpsPosition); + msgtracker.msg_received = NONE_RECEIVED; + id = GPSPOSITION_OBJID; + } + return id; +} + +#endif // PIOS_INCLUDE_GPS_UBX_PARSER + diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index 86a92d285..6e243c21f 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -34,6 +34,24 @@ #ifndef GPS_H #define GPS_H +#include "gpsvelocity.h" +#include "gpssatellites.h" +#include "gpsposition.h" +#include "gpstime.h" + +#define NO_PARSER -3 // no parser available +#define PARSER_OVERRUN -2 // message buffer overrun before completing the message +#define PARSER_ERROR -1 // message unparsable by this parser +#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message +#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing + +struct GPS_RX_STATS { + uint16_t gpsRxReceived; + uint16_t gpsRxChkSumError; + uint16_t gpsRxOverflow; + uint16_t gpsRxParserError; +}; + int32_t GPSInitialize(void); #endif // GPS_H diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h index 6ff6b1195..b6c88bd27 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -33,8 +33,12 @@ #include #include +#include "gps.h" -extern bool NMEA_update_position(char *nmea_sentence); +#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed + +extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData); extern bool NMEA_checksum(char *nmea_sentence); +extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); #endif /* NMEA_H */ diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h new file mode 100644 index 000000000..6296a335a --- /dev/null +++ b/flight/Modules/GPS/inc/UBX.h @@ -0,0 +1,224 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file UBX.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GPS module, handles GPS and NMEA stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef UBX_H +#define UBX_H +#include "openpilot.h" +#include "gpsposition.h" +#include "gps.h" + + +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 + +// From u-blox6 receiver protocol specification + +// Messages classes +#define UBX_CLASS_NAV 0x01 + +// Message IDs +#define UBX_ID_POSLLH 0x02 +#define UBX_ID_STATUS 0x03 +#define UBX_ID_DOP 0x04 +#define UBX_ID_SOL 0x06 +#define UBX_ID_VELNED 0x12 +#define UBX_ID_TIMEUTC 0x21 +#define UBX_ID_SVINFO 0x30 + +// private structures + +// Geodetic Position Solution +struct UBX_NAV_POSLLH { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t lon; // Longitude (deg*1e-7) + int32_t lat; // Latitude (deg*1e-7) + int32_t height; // Height above Ellipsoid (mm) + int32_t hMSL; // Height above mean sea level (mm) + uint32_t hAcc; // Horizontal Accuracy Estimate (mm) + uint32_t vAcc; // Vertical Accuracy Estimate (mm) +}; + +// Receiver Navigation Status + +#define STATUS_GPSFIX_NOFIX 0x00 +#define STATUS_GPSFIX_DRONLY 0x01 +#define STATUS_GPSFIX_2DFIX 0x02 +#define STATUS_GPSFIX_3DFIX 0x03 +#define STATUS_GPSFIX_GPSDR 0x04 +#define STATUS_GPSFIX_TIMEONLY 0x05 + +#define STATUS_FLAGS_GPSFIX_OK (1 << 0) +#define STATUS_FLAGS_DIFFSOLN (1 << 1) +#define STATUS_FLAGS_WKNSET (1 << 2) +#define STATUS_FLAGS_TOWSET (1 << 3) + +struct UBX_NAV_STATUS { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Navigation Status Flags + uint8_t fixStat; // Fix Status Information + uint8_t flags2; // Additional navigation output information + uint32_t ttff; // Time to first fix (ms) + uint32_t msss; // Milliseconds since startup/reset (ms) +}; + +// Dilution of precision +struct UBX_NAV_DOP { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint16_t gDOP; // Geometric DOP + uint16_t pDOP; // Position DOP + uint16_t tDOP; // Time DOP + uint16_t vDOP; // Vertical DOP + uint16_t hDOP; // Horizontal DOP + uint16_t nDOP; // Northing DOP + uint16_t eDOP; // Easting DOP +}; + +// Navigation solution + +struct UBX_NAV_SOL { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t fTOW; // fractional nanoseconds (ns) + int16_t week; // GPS week + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Fix status flags + int32_t ecefX; // ECEF X coordinate (cm) + int32_t ecefY; // ECEF Y coordinate (cm) + int32_t ecefZ; // ECEF Z coordinate (cm) + uint32_t pAcc; // 3D Position Accuracy Estimate (cm) + int32_t ecefVX; // ECEF X coordinate (cm/s) + int32_t ecefVY; // ECEF Y coordinate (cm/s) + int32_t ecefVZ; // ECEF Z coordinate (cm/s) + uint32_t sAcc; // Speed Accuracy Estimate + uint16_t pDOP; // Position DOP + uint8_t reserved1; // Reserved + uint8_t numSV; // Number of SVs used in Nav Solution + uint32_t reserved2; // Reserved +}; + +// North/East/Down velocity + +struct UBX_NAV_VELNED { + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate +}; + +// UTC Time Solution + +#define TIMEUTC_VALIDTOW (1 << 0) +#define TIMEUTC_VALIDWKN (1 << 1) +#define TIMEUTC_VALIDUTC (1 << 2) + +struct UBX_NAV_TIMEUTC { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity Flags +}; + +// Space Vehicle (SV) Information + +// Single SV information block + +#define SVUSED (1 << 0) // This SV is used for navigation +#define DIFFCORR (1 << 1) // Differential correction available +#define ORBITAVAIL (1 << 2) // Orbit information available +#define ORBITEPH (1 << 3) // Orbit information is Ephemeris +#define UNHEALTHY (1 << 4) // SV is unhealthy +#define ORBITALM (1 << 5) // Orbit information is Almanac Plus +#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous +#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used + +struct UBX_NAV_SVINFO_SV { + uint8_t chn; // Channel number + uint8_t svid; // Satellite ID + uint8_t flags; // Misc SV information + uint8_t quality; // Misc quality indicators + uint8_t cno; // Carrier to Noise Ratio (dbHz) + int8_t elev; // Elevation (integer degrees) + int16_t azim; // Azimuth (integer degrees) + int32_t prRes; // Pseudo range residual (cm) +}; + +// SV information message +#define MAX_SVS 16 + +struct UBX_NAV_SVINFO { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t numCh; // Number of channels + uint8_t globalFlags; // + uint16_t reserved2; // Reserved + struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times +}; + +typedef union { + uint8_t payload[0]; + struct UBX_NAV_POSLLH nav_posllh; + struct UBX_NAV_STATUS nav_status; + struct UBX_NAV_DOP nav_dop; + struct UBX_NAV_SOL nav_sol; + struct UBX_NAV_VELNED nav_velned; +#if !defined(PIOS_GPS_MINIMAL) + struct UBX_NAV_TIMEUTC nav_timeutc; + struct UBX_NAV_SVINFO nav_svinfo; +#endif +} UBXPayload; + +struct UBXHeader { + uint8_t class; + uint8_t id; + uint16_t len; + uint8_t ck_a; + uint8_t ck_b; +}; + +struct UBXPacket { + struct UBXHeader header; + UBXPayload payload; +}; + +bool checksum_ubx_message(struct UBXPacket *); +uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *); +int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); + +#endif /* UBX_H */ diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 3043ea630..5b249bfe5 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -95,7 +95,9 @@ #define PIOS_INCLUDE_INITCALL /* Include init call structures */ #define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ #define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ -//#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ +#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */ +#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */ /* Alarm Thresholds */ #define HEAP_LIMIT_WARNING 4000 diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index d91eb9c17..462acfc34 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -46,7 +46,9 @@ UAVOBJSRCFILENAMES += flighttelemetrystats UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocity UAVOBJSRCFILENAMES += guidancesettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp index d6bddb291..e70f2ed72 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp @@ -123,7 +123,7 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az satellites[index][2] = azimuth; satellites[index][3] = snr; - if (prn) { + if (prn && elevation >= 0) { QPointF opd = polarToCoord(elevation,azimuth); opd += QPointF(-satIcons[index]->boundingRect().center().x(), -satIcons[index]->boundingRect().center().y()); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index bfe2342b3..720d36ebd 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -52,6 +52,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpsposition.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ + $$UAVOBJECT_SYNTHETICS/gpssettings.h \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ $$UAVOBJECT_SYNTHETICS/positionactual.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ @@ -112,6 +114,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ + $$UAVOBJECT_SYNTHETICS/gpssettings.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml new file mode 100644 index 000000000..5c38f43fe --- /dev/null +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -0,0 +1,10 @@ + + + Settings for the GPS + + + + + + + diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocity.xml new file mode 100644 index 000000000..8673463a8 --- /dev/null +++ b/shared/uavobjectdefinition/gpsvelocity.xml @@ -0,0 +1,12 @@ + + + Raw GPS data from @ref GPSModule. + + + + + + + + +