From dfdea1606576e7261ca4d8aca0a0b7d6d3d96281 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 25 Aug 2011 14:46:30 +0200 Subject: [PATCH 1/3] Modules/GPS: Make GPS COM unidirectional (rx only) and use smaller, runtime allocated heap buffers --- flight/Modules/GPS/GPS.c | 252 +++++++------------- flight/Modules/GPS/GTOP_BIN.c | 262 --------------------- flight/Modules/GPS/NMEA.c | 171 +++----------- flight/Modules/GPS/inc/GPS.h | 2 - flight/Modules/GPS/inc/GTOP_BIN.h | 42 ---- flight/Modules/GPS/inc/NMEA.h | 7 +- flight/Modules/GPS/inc/gps_mode.h | 58 ----- flight/OpenPilot/System/pios_board.c | 5 +- flight/OpenPilot/System/pios_board_posix.c | 6 +- 9 files changed, 123 insertions(+), 682 deletions(-) delete mode 100644 flight/Modules/GPS/GTOP_BIN.c delete mode 100644 flight/Modules/GPS/inc/GTOP_BIN.h delete mode 100644 flight/Modules/GPS/inc/gps_mode.h diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index d2ab2b3c5..44b039eb1 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -35,13 +35,7 @@ #include -#ifdef ENABLE_GPS_BINARY_GTOP - #include "GTOP_BIN.h" -#endif - -#if defined(ENABLE_GPS_ONESENTENCE_GTOP) || defined(ENABLE_GPS_NMEA) - #include "NMEA.h" -#endif +#include "NMEA.h" #include "gpsposition.h" #include "homelocation.h" @@ -63,25 +57,16 @@ static float GravityAccel(float latitude, float longitude, float altitude); // **************** // Private constants -//#define FULL_COLD_RESTART // uncomment this to tell the GPS to do a FULL COLD restart -//#define DISABLE_GPS_THRESHOLD // - #define GPS_TIMEOUT_MS 500 -#define GPS_COMMAND_RESEND_TIMEOUT_MS 2000 +#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 - #ifdef ENABLE_GPS_BINARY_GTOP - #define STACK_SIZE_BYTES 800 - #else - #define STACK_SIZE_BYTES 800 - #endif + #define STACK_SIZE_BYTES 800 #else - #ifdef ENABLE_GPS_BINARY_GTOP - #define STACK_SIZE_BYTES 440 - #else - #define STACK_SIZE_BYTES 440 - #endif + #define STACK_SIZE_BYTES 650 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) @@ -93,9 +78,7 @@ static uint32_t gpsPort; static xTaskHandle gpsTaskHandle; -#ifndef ENABLE_GPS_BINARY_GTOP - static char gps_rx_buffer[128]; -#endif +static char* gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; @@ -135,6 +118,8 @@ int32_t GPSInitialize(void) // TODO: Get gps settings object gpsPort = PIOS_COM_GPS; + gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + return 0; } MODULE_INITCALL(GPSInitialize, GPSStart) @@ -150,45 +135,11 @@ static void gpsTask(void *parameters) uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; GPSPositionData GpsData; -#ifdef ENABLE_GPS_BINARY_GTOP - GTOP_BIN_init(); -#else uint8_t rx_count = 0; bool start_flag = false; bool found_cr = false; int32_t gpsRxOverflow = 0; -#endif -#ifdef FULL_COLD_RESTART - // tell the GPS to do a FULL COLD restart - PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n"); - timeOfLastCommandMs = timeNowMs; - while (timeNowMs - timeOfLastCommandMs < 300) // delay for 300ms to let the GPS sort itself out - { - vTaskDelay(xDelay); // Block task until next update - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; - } -#endif - -#ifdef DISABLE_GPS_THRESHOLD - PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n"); -#endif - -#ifdef ENABLE_GPS_BINARY_GTOP - // switch to GTOP binary mode - PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n"); -#endif - -#ifdef ENABLE_GPS_ONESENTENCE_GTOP - // switch to single sentence mode - PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n"); -#endif - -#ifdef ENABLE_GPS_NMEA - // switch to NMEA mode - PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n"); -#endif - numUpdates = 0; numChecksumErrors = 0; numParsingErrors = 0; @@ -200,103 +151,86 @@ static void gpsTask(void *parameters) while (1) { uint8_t c; - #ifdef ENABLE_GPS_BINARY_GTOP - // GTOP BINARY GPS mode + // NMEA or SINGLE-SENTENCE GPS mode - while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) + // 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 == '$')) { - if (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0) - { - numUpdates++; + start_flag = true; + found_cr = false; + rx_count = 0; + } + 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; } } - - #else - // 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; - } - else - if (!start_flag) - continue; - - if (rx_count >= sizeof(gps_rx_buffer)) - { - // 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; - } - } - } - #endif + } // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; @@ -309,30 +243,6 @@ static void gpsTask(void *parameters) GPSPositionSet(&GpsData); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS) - { // resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command - timeOfLastCommandMs = timeNowMs; - - #ifdef ENABLE_GPS_BINARY_GTOP - GTOP_BIN_init(); - // switch to binary mode - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n"); - #endif - - #ifdef ENABLE_GPS_ONESENTENCE_GTOP - // switch to single sentence mode - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n"); - #endif - - #ifdef ENABLE_GPS_NMEA - // switch to NMEA mode - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n"); - #endif - - #ifdef DISABLE_GPS_TRESHOLD - PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n"); - #endif - } } else { // we appear to be receiving GPS sentences OK, we've had an update diff --git a/flight/Modules/GPS/GTOP_BIN.c b/flight/Modules/GPS/GTOP_BIN.c deleted file mode 100644 index 7bd2964d9..000000000 --- a/flight/Modules/GPS/GTOP_BIN.c +++ /dev/null @@ -1,262 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file GTOP_BIN.c - * @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 - */ - -#include "openpilot.h" -#include "pios.h" -#include "GTOP_BIN.h" -#include "gpsposition.h" -#include "gpstime.h" -#include "gpssatellites.h" - -#include // memmove - -#ifdef ENABLE_GPS_BINARY_GTOP - -// ************ -// the structure of the binary packet - -typedef struct -{ - uint32_t utc_time; - - int32_t latitude; - uint8_t ns_indicator; - - int32_t longitude; - uint8_t ew_indicator; - - uint8_t fix_quality; - - uint8_t satellites_used; - - uint16_t hdop; - - int32_t msl_altitude; - - int32_t geoidal_seperation; - - uint8_t fix_type; - - int32_t course_over_ground; - - int32_t speed_over_ground; - - uint8_t day; - uint8_t month; - uint16_t year; -} __attribute__((__packed__)) t_gps_bin_packet_data; - -typedef struct -{ - uint16_t header; - t_gps_bin_packet_data data; - uint8_t asterisk; - uint8_t checksum; - uint16_t end_word; -} __attribute__((__packed__)) t_gps_bin_packet; - -// ************ - -// buffer that holds the incoming binary packet -static uint8_t gps_rx_buffer[sizeof(t_gps_bin_packet)] __attribute__ ((aligned(4))); - -// number of bytes currently in the rx buffer -static int16_t gps_rx_buffer_wr = 0; - -// ************ -// endian swapping functions - -static uint16_t swap2Bytes(uint16_t data) -{ - return (((data >> 8) & 0x00ff) | - ((data << 8) & 0xff00)); -} - -static uint32_t swap4Bytes(uint32_t data) -{ - return (((data >> 24) & 0x000000ff) | - ((data >> 8) & 0x0000ff00) | - ((data << 8) & 0x00ff0000) | - ((data << 24) & 0xff000000)); -} - -// ************ -/** - * Parses a complete binary packet and update the GPSPosition and GPSTime UAVObjects - * - * param[in] .. b = a new received byte from the GPS - * - * return '0' if we have found a valid binary packet - * return <0 if any errors were encountered with the packet or no packet found - */ - -int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors) -{ - if (gps_rx_buffer_wr >= sizeof(gps_rx_buffer)) - { // make room for the new byte .. this will actually never get executed, just here as a safe guard really - memmove(gps_rx_buffer, gps_rx_buffer + 1, sizeof(gps_rx_buffer) - 1); - gps_rx_buffer_wr = sizeof(gps_rx_buffer) - 1; - } - - // add the new byte into the buffer - gps_rx_buffer[gps_rx_buffer_wr++] = b; - - int16_t i = 0; - - while (gps_rx_buffer_wr > 0) - { - t_gps_bin_packet *rx_packet = (t_gps_bin_packet *)(gps_rx_buffer + i); - - // scan for the start of a binary packet (the header bytes) - while (gps_rx_buffer_wr - i >= sizeof(rx_packet->header)) - { - if (rx_packet->header == 0x2404) - break; // found a valid header marker - rx_packet = (t_gps_bin_packet *)(gps_rx_buffer + ++i); - } - - // remove unwanted bytes before the start of the packet header - if (i > 0) - { - gps_rx_buffer_wr -= i; - if (gps_rx_buffer_wr > 0) - memmove(gps_rx_buffer, gps_rx_buffer + i, gps_rx_buffer_wr); - i = 0; - } - - if (gps_rx_buffer_wr < sizeof(t_gps_bin_packet)) - break; // not yet enough bytes for a complete binary packet - - // we have enough bytes for a complete binary packet - - // check to see if certain parameters in the binary packet are valid - if (rx_packet->header != 0x2404 || - rx_packet->end_word != 0x0A0D || - rx_packet->asterisk != 0x2A || - (rx_packet->data.ns_indicator != 1 && rx_packet->data.ns_indicator != 2) || - (rx_packet->data.ew_indicator != 1 && rx_packet->data.ew_indicator != 2) || - (rx_packet->data.fix_quality > 2) || - (rx_packet->data.fix_type < 1 || rx_packet->data.fix_type > 3) ) - { // invalid packet - if (parsing_errors) *parsing_errors++; - i++; - continue; - } - - { // check the checksum is valid - uint8_t *p = (uint8_t *)&rx_packet->data; - uint8_t checksum = 0; - for (int i = 0; i < sizeof(t_gps_bin_packet_data); i++) - checksum ^= *p++; - - if (checksum != rx_packet->checksum) - { // checksum error - if (chksum_errors) *chksum_errors++; - i++; - continue; - } - } - - // we now have a valid complete binary packet, update the GpsData and GpsTime objects - - // correct the endian order of the parameters - rx_packet->data.utc_time = swap4Bytes(rx_packet->data.utc_time); - rx_packet->data.latitude = swap4Bytes(rx_packet->data.latitude); - rx_packet->data.longitude = swap4Bytes(rx_packet->data.longitude); - rx_packet->data.hdop = swap2Bytes(rx_packet->data.hdop); - rx_packet->data.msl_altitude = swap4Bytes(rx_packet->data.msl_altitude); - rx_packet->data.geoidal_seperation = swap4Bytes(rx_packet->data.geoidal_seperation); - rx_packet->data.course_over_ground = swap4Bytes(rx_packet->data.course_over_ground); - rx_packet->data.speed_over_ground = swap4Bytes(rx_packet->data.speed_over_ground); - rx_packet->data.year = swap2Bytes(rx_packet->data.year); - - // set the gps time object - GPSTimeData GpsTime; -// GPSTimeGet(&GpsTime); - uint32_t utc_time = rx_packet->data.utc_time / 1000; - GpsTime.Second = utc_time % 100; // seconds - GpsTime.Minute = (utc_time / 100) % 100; // minutes - GpsTime.Hour = utc_time / 10000; // hours - GpsTime.Day = rx_packet->data.day; // day - GpsTime.Month = rx_packet->data.month; // month - GpsTime.Year = rx_packet->data.year; // year - GPSTimeSet(&GpsTime); - - // set the gps position object - GPSPositionData GpsData; -// GPSPositionGet(&GpsData); - switch (rx_packet->data.fix_type) - { - case 1: GpsData.Status = GPSPOSITION_STATUS_NOFIX; break; - case 2: GpsData.Status = GPSPOSITION_STATUS_FIX2D; break; - case 3: GpsData.Status = GPSPOSITION_STATUS_FIX3D; break; - default: GpsData.Status = GPSPOSITION_STATUS_NOGPS; break; - } - GpsData.Latitude = rx_packet->data.latitude * (rx_packet->data.ns_indicator == 1 ? +1 : -1) * 10; // degrees * 10e6 - GpsData.Longitude = rx_packet->data.longitude * (rx_packet->data.ew_indicator == 1 ? +1 : -1) * 10; // degrees * 10e6 - GpsData.Altitude = (float)rx_packet->data.msl_altitude / 1000; // meters - GpsData.GeoidSeparation = (float)rx_packet->data.geoidal_seperation / 1000; // meters - GpsData.Heading = (float)rx_packet->data.course_over_ground / 1000; // degrees - GpsData.Groundspeed = (float)rx_packet->data.speed_over_ground / 3600; // m/s - GpsData.Satellites = rx_packet->data.satellites_used; // - GpsData.PDOP = 99.99; // not available in binary mode - GpsData.HDOP = (float)rx_packet->data.hdop / 100; // - GpsData.VDOP = 99.99; // not available in binary mode - GPSPositionSet(&GpsData); - - // set the number of satellites -// GPSSatellitesData SattelliteData; -//// GPSSatellitesGet(&SattelliteData); -// memset(&SattelliteData, 0, sizeof(SattelliteData)); -// SattelliteData.SatsInView = rx_packet->data.satellites_used; // -// GPSSatellitesSet(&SattelliteData); - - // remove the spent binary packet from the buffer - gps_rx_buffer_wr -= sizeof(t_gps_bin_packet); - if (gps_rx_buffer_wr > 0) - memmove(gps_rx_buffer, gps_rx_buffer + sizeof(t_gps_bin_packet), gps_rx_buffer_wr); - - return 0; // found a valid packet - } - - return -1; // no valid packet found -} - -// ************ - -void GTOP_BIN_init(void) -{ - gps_rx_buffer_wr = 0; -} - -// ************ - -#endif // ENABLE_GPS_BINARY_GTOP - diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 3dfd98d40..adadc588a 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -40,8 +40,6 @@ -#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) - // Debugging #ifdef ENABLE_DEBUG_MSG //#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages @@ -54,7 +52,6 @@ //#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages //#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages //#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages -//#define NMEA_DEBUG_PGTOP ///< define to enable debug of PGTOP messages #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) #else #define DEBUG_MSG(format, ...) @@ -69,56 +66,45 @@ struct nmea_parser { uint32_t cnt; }; - #ifdef ENABLE_GPS_NMEA - static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - #endif +static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static struct nmea_parser nmea_parsers[] = { - - #ifdef ENABLE_GPS_NMEA - { - .prefix = "GPGGA", - .handler = nmeaProcessGPGGA, - .cnt = 0, - }, - { - .prefix = "GPVTG", - .handler = nmeaProcessGPVTG, - .cnt = 0, - }, - { - .prefix = "GPGSA", - .handler = nmeaProcessGPGSA, - .cnt = 0, - }, - { - .prefix = "GPRMC", - .handler = nmeaProcessGPRMC, - .cnt = 0, - }, - { - .prefix = "GPZDA", - .handler = nmeaProcessGPZDA, - .cnt = 0, - }, - { - .prefix = "GPGSV", - .handler = nmeaProcessGPGSV, - .cnt = 0, - }, - #endif - { - .prefix = "PGTOP", - .handler = nmeaProcessPGTOP, - .cnt = 0, - }, +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, + }, + { + .prefix = "GPZDA", + .handler = nmeaProcessGPZDA, + .cnt = 0, + }, + { + .prefix = "GPGSV", + .handler = nmeaProcessGPGSV, + .cnt = 0, + }, }; static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) @@ -229,7 +215,6 @@ static float NMEA_real_to_float(char *nmea_real) return (((float)whole) + fract * pow(10, -fract_units)); } -#ifdef ENABLE_GPS_NMEA /* * Parse a field in the format: * DD[D]MM.mmmm[mm] @@ -287,7 +272,6 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool return true; } -#endif // ENABLE_GPS_NMEA /** @@ -376,7 +360,6 @@ bool NMEA_update_position(char *nmea_sentence) return true; } -#ifdef ENABLE_GPS_NMEA /** * Parse an NMEA GPGGA sentence and update the given UAVObject @@ -675,83 +658,3 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch return true; } -#endif // ENABLE_GPS_NMEA - -/** - * Parse an NMEA PGTOP sentence and update the given UAVObject - * \param[in] A pointer to a GPSPosition UAVObject to be updated. - * \param[in] An NMEA sentence with a valid checksum - */ -static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) -{ - if (nbParam != 17) - return false; - - GPSTimeData gpst; - GPSTimeGet(&gpst); - - *gpsDataUpdated = true; - - // get UTC time [hhmmss.sss] - float hms = NMEA_real_to_float(param[1]); - gpst.Second = (int)hms % 100; - gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; - gpst.Hour = (int)hms / 10000; - - // get latitude decimal degrees - GpsData->Latitude = NMEA_real_to_float(param[2])*1e7; - if (param[3][0] == 'S') - GpsData->Latitude = -GpsData->Latitude; - - - // get longitude decimal degrees - GpsData->Longitude = NMEA_real_to_float(param[4])*1e7; - if (param[5][0] == 'W') - GpsData->Longitude = -GpsData->Longitude; - - // get number of satellites used in GPS solution - GpsData->Satellites = atoi(param[7]); - - // next field: HDOP - GpsData->HDOP = NMEA_real_to_float(param[8]); - - // get altitude (in meters mm.m) - GpsData->Altitude = NMEA_real_to_float(param[9]); - - // next field: geoid separation - GpsData->GeoidSeparation = NMEA_real_to_float(param[10]); - - // Mode: 1=Fix not available, 2=2D, 3=3D - switch (atoi(param[11])) { - case 1: - GpsData->Status = GPSPOSITION_STATUS_NOFIX; - break; - case 2: - GpsData->Status = GPSPOSITION_STATUS_FIX2D; - break; - case 3: - GpsData->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: - /* Unhandled */ - return false; - break; - } - - // get course over ground in degrees [ddd.dd] - GpsData->Heading = NMEA_real_to_float(param[12]); - - // get speed in km/h - GpsData->Groundspeed = NMEA_real_to_float(param[13]); - // to m/s - GpsData->Groundspeed /= 3.6; - - gpst.Day = atoi(param[14]); - gpst.Month = atoi(param[15]); - gpst.Year = atoi(param[16]); - GPSTimeSet(&gpst); - - return true; -} - -#endif // #if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index 1ae683c9a..86a92d285 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -34,8 +34,6 @@ #ifndef GPS_H #define GPS_H -#include "gps_mode.h" - int32_t GPSInitialize(void); #endif // GPS_H diff --git a/flight/Modules/GPS/inc/GTOP_BIN.h b/flight/Modules/GPS/inc/GTOP_BIN.h deleted file mode 100644 index 6a5234430..000000000 --- a/flight/Modules/GPS/inc/GTOP_BIN.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file GTOP_BIN.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 GTOP_BIN_H -#define GTOP_BIN_H - -#include -#include "gps_mode.h" - -#ifdef ENABLE_GPS_BINARY_GTOP - extern int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors); - extern void GTOP_BIN_init(void); -#endif - -#endif diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h index 5c1b13f8b..6ff6b1195 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -33,11 +33,8 @@ #include #include -#include "gps_mode.h" -#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) - extern bool NMEA_update_position(char *nmea_sentence); - extern bool NMEA_checksum(char *nmea_sentence); -#endif +extern bool NMEA_update_position(char *nmea_sentence); +extern bool NMEA_checksum(char *nmea_sentence); #endif /* NMEA_H */ diff --git a/flight/Modules/GPS/inc/gps_mode.h b/flight/Modules/GPS/inc/gps_mode.h deleted file mode 100644 index 43e5b0398..000000000 --- a/flight/Modules/GPS/inc/gps_mode.h +++ /dev/null @@ -1,58 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file gps_mode.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and - * objects. - * @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 GPS_MODE_H -#define GPS_MODE_H - -// **************** -// you MUST have one of these uncommented - and ONLY one - -//#define ENABLE_GPS_BINARY_GTOP // uncomment this if we are using GTOP BINARY mode -//#define ENABLE_GPS_ONESENTENCE_GTOP // uncomment this if we are using GTOP SINGLE SENTENCE mode -#define ENABLE_GPS_NMEA // uncomment this if we are using NMEA mode - -// **************** -// make sure they have defined a protocol to use - -#if !defined(ENABLE_GPS_BINARY_GTOP) && !defined(ENABLE_GPS_ONESENTENCE_GTOP) && !defined(ENABLE_GPS_NMEA) - #error YOU MUST SELECT THE DESIRED GPS PROTOCOL IN gps_mode.h! -#endif - -// **************** - -#endif - -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 12d365b67..150bfb938 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -525,7 +525,6 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = { #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 #define PIOS_COM_GPS_RX_BUF_LEN 96 -#define PIOS_COM_GPS_TX_BUF_LEN 96 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 @@ -1075,12 +1074,10 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_TX_BUF_LEN); PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - tx_buffer, PIOS_COM_GPS_TX_BUF_LEN)) { + NULL, 0)) { PIOS_Assert(0); } } diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index e1e8dcc54..37fadd760 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -71,7 +71,7 @@ const struct pios_udp_cfg pios_udp_aux_cfg = { #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 -#define PIOS_COM_GPS_RX_BUF_LEN 192 +#define PIOS_COM_GPS_RX_BUF_LEN 96 /* * Board specific number of devices. @@ -164,12 +164,10 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - tx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) { + NULL, 0)) { PIOS_Assert(0); } } From 7e396efbbf90a133d50791427c04bd996e7d7cb2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 25 Aug 2011 15:11:29 +0200 Subject: [PATCH 2/3] Modules/GPS: Assert buffer creation --- flight/Modules/GPS/GPS.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 44b039eb1..3d9cd1b20 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -119,6 +119,7 @@ int32_t GPSInitialize(void) gpsPort = PIOS_COM_GPS; gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + PIOS_ASSERT(gps_rx_buffer); return 0; } From 54ecf233f31fc8cb812e0a7b9fde8ef6f344cf39 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 25 Aug 2011 15:13:50 +0200 Subject: [PATCH 3/3] Modules/PS: typo fix --- flight/Modules/GPS/GPS.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 3d9cd1b20..2e6b0901b 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -119,7 +119,7 @@ int32_t GPSInitialize(void) gpsPort = PIOS_COM_GPS; gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); - PIOS_ASSERT(gps_rx_buffer); + PIOS_Assert(gps_rx_buffer); return 0; }