From 4547013901a430a03e80a5910425ccc5b0dc0f0c Mon Sep 17 00:00:00 2001 From: pip Date: Tue, 22 Feb 2011 10:16:50 +0000 Subject: [PATCH] Added GTOP BINARY mode to the GPS module. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2837 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Modules/GPS/GPS.c | 315 +++++++++++++++++++--------- flight/Modules/GPS/GTOP_BIN.c | 260 +++++++++++++++++++++++ flight/Modules/GPS/NMEA.c | 338 +++++++++++++++++------------- flight/Modules/GPS/inc/GPS.h | 2 + flight/Modules/GPS/inc/GTOP_BIN.h | 42 ++++ flight/Modules/GPS/inc/NMEA.h | 37 +++- flight/Modules/GPS/inc/gps_mode.h | 60 ++++++ 7 files changed, 804 insertions(+), 250 deletions(-) create mode 100644 flight/Modules/GPS/GTOP_BIN.c create mode 100644 flight/Modules/GPS/inc/GTOP_BIN.h create mode 100644 flight/Modules/GPS/inc/gps_mode.h diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index dd9207362..7e733a4bb 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -28,50 +28,80 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +// **************** + #include "openpilot.h" #include "GPS.h" + #include -#include "NMEA.h" + +#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 "gpsposition.h" #include "homelocation.h" #include "gpstime.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" +//#define FULL_COLD_RESTART // uncomment this to tell the GPS to do a FULL COLD restart +//#define DISABLE_GPS_TRESHOLD // + +// **************** // constants/macros/typdefs -#define NMEA_BUFFERSIZE 128 #define GPS_TIMEOUT_MS 500 +// **************** // Private functions static void gpsTask(void *parameters); static void setHomeLocation(GPSPositionData * gpsData); -// Global variables - +// **************** // Private constants + // Unfortunately need a good size stack for the WMM calculation -#define STACK_SIZE_BYTES 800 +#ifdef ENABLE_GPS_BINARY_GTOP + #define STACK_SIZE_BYTES 800 +#else + #define STACK_SIZE_BYTES 800 +#endif + #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +// **************** // Private types +// **************** // Private variables -static uint32_t gpsPort; -static xTaskHandle gpsTaskHandle; -static char gps_rx_buffer[NMEA_BUFFERSIZE]; +static uint32_t gpsPort; + +static xTaskHandle gpsTaskHandle; + +#ifndef ENABLE_GPS_BINARY_GTOP + static char gps_rx_buffer[128]; +#endif + +static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; static uint32_t numUpdates; static uint32_t numChecksumErrors; static uint32_t numParsingErrors; +// **************** /** * Initialise the gps module * \return -1 if initialisation failed * \return 0 on success */ + int32_t GPSInitialize(void) { signed portBASE_TYPE xReturn; @@ -86,125 +116,206 @@ int32_t GPSInitialize(void) return 0; } +// **************** /** - * gps task. Processes input buffer. It does not return. + * Main gps task. It does not return. */ + static void gpsTask(void *parameters) { - int32_t gpsRxOverflow = 0; - char c; - portTickType xDelay = 100 / portTICK_RATE_MS; - GPSPositionData GpsData; - uint32_t timeNowMs; + portTickType xDelay = 100 / portTICK_RATE_MS; + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; + GPSPositionData GpsData; - bool start_flag = false; - bool found_cr = false; - uint8_t rx_count = 0; - - //#define DISABLE_GPS_TRESHOLD - //#define ENABLE_GPS_ONESENTENCE - //#define ENABLE_DEFAULT_NMEA +#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"); +#endif + #ifdef DISABLE_GPS_TRESHOLD - PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n"); + PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n"); #endif -#ifdef ENABLE_GPS_ONESENTENCE - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n"); -#endif -#ifdef ENABLE_DEFAULT_NMEA - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n"); + +#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 sentance 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; + + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; + // Loop forever - while (1) { - /* This blocks the task until there is something on the buffer */ - while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) { - - c = PIOS_COM_ReceiveBuffer(gpsPort); - - /* 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 >= NMEA_BUFFERSIZE) { - /* - * 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') ) { - - /* 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. */ - ++numChecksumErrors; - } else { - /* Valid checksum, use this packet to update the GPS position */ - if (!NMEA_update_position(&gps_rx_buffer[1])) { - ++numParsingErrors; - } else { - ++numUpdates; - } - timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS; + while (1) + { + #ifdef ENABLE_GPS_BINARY_GTOP + // GTOP BINARY GPS mode + + while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) + { + int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors); + if (res >= 0) + { + numUpdates++; + + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; } } - } + + #else + // NMEA or SINGLE-SENTANCE GPS mode + + // This blocks the task until there is something on the buffer + while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) + { + char c = PIOS_COM_ReceiveBuffer(gpsPort); + + // 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') ) + { + // 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. + ++numChecksumErrors; + } + else + { // Valid checksum, use this packet to update the GPS position + if (!NMEA_update_position(&gps_rx_buffer[1])) + ++numParsingErrors; + else + ++numUpdates; + + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; + } + } + } + #endif // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS) { + if ((timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS) + { GPSPositionGet(&GpsData); GpsData.Status = GPSPOSITION_STATUS_NOGPS; GPSPositionSet(&GpsData); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - } else { + + if ((timeNowMs - timeOfLastCommandMs) > 5000) /// 5000ms + { // resend the command .. just case the gps has only just been plugged in or the gps did not get our last command + + #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 sentance 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 + + timeOfLastCommandMs = timeNowMs; + } + } + else + { // Had an update HomeLocationData home; HomeLocationGet(&home); GPSPositionGet(&GpsData); - if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE)) { + if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE)) setHomeLocation(&GpsData); - } //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))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); + if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7)) + 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); } // Block task until next update @@ -212,6 +323,8 @@ static void gpsTask(void *parameters) } } +// **************** + static void setHomeLocation(GPSPositionData * gpsData) { HomeLocationData home; @@ -219,7 +332,8 @@ static void setHomeLocation(GPSPositionData * gpsData) GPSTimeData gps; GPSTimeGet(&gps); - if (gps.Year >= 2000) { + if (gps.Year >= 2000) + { // Store LLA home.Latitude = gpsData->Latitude; home.Longitude = gpsData->Longitude; @@ -239,13 +353,14 @@ static void setHomeLocation(GPSPositionData * gpsData) // 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) { // calculations appeared to go OK - home.Set = HOMELOCATION_SET_TRUE; HomeLocationSet(&home); } } } +// **************** + /** * @} * @} diff --git a/flight/Modules/GPS/GTOP_BIN.c b/flight/Modules/GPS/GTOP_BIN.c new file mode 100644 index 000000000..1712b4747 --- /dev/null +++ b/flight/Modules/GPS/GTOP_BIN.c @@ -0,0 +1,260 @@ +/** + ****************************************************************************** + * @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 +#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 binary packet +static uint8_t gps_rx_buffer[sizeof(t_gps_bin_packet)] __attribute__ ((aligned(4))); + +static uint8_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 updates the GPSPosition UAVObject + * \param[in] A new byte from the GPS + * \return true if we have found a valid binary packet + * \return false if any errors were encountered with the packet + */ + +int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors) +{ + t_gps_bin_packet *rx_packet = (t_gps_bin_packet *)gps_rx_buffer; + + if (gps_rx_buffer_wr >= sizeof(gps_rx_buffer)) + { // make room for the new byte + 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; + + while (gps_rx_buffer_wr >= sizeof(t_gps_bin_packet)) + { + // scan for the start of a binary packet (the header bytes) + while (gps_rx_buffer_wr >= sizeof(rx_packet->header)) + { + if (rx_packet->header == 0x2404) + break; // found a valid header marker + + // remove oldest byte from buffer + if (gps_rx_buffer_wr > 1) + memmove(gps_rx_buffer, gps_rx_buffer + 1, gps_rx_buffer_wr - 1); + gps_rx_buffer_wr--; + } + + if (gps_rx_buffer_wr < sizeof(t_gps_bin_packet)) + { // not yet enough bytes for a complete binary packet + break; + } + + // we have enough bytes for a complete binary packet + + if (rx_packet->header != 0x2404 || + rx_packet->end_word != 0x0A0D || + rx_packet->asterisk != 0x2A) + { // no valid packet found - yet + memmove(gps_rx_buffer, gps_rx_buffer + 1, gps_rx_buffer_wr - 1); + gps_rx_buffer_wr--; + 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++; + memmove(gps_rx_buffer, gps_rx_buffer + 1, gps_rx_buffer_wr - 1); + gps_rx_buffer_wr--; + continue; + } + } + + // checksum appears correct + + if ( (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) ) + { // found some invalid params - discard the packet + if (parsing_errors) *parsing_errors++; + memmove(gps_rx_buffer, gps_rx_buffer + 1, gps_rx_buffer_wr - 1); + gps_rx_buffer_wr--; + continue; + } + + // we now have all the GPS data in a valid 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; + GpsTime.Minute = (utc_time / 100) % 100; + GpsTime.Hour = utc_time / 10000; + GpsTime.Day = rx_packet->data.day; + GpsTime.Month = rx_packet->data.month; + GpsTime.Year = rx_packet->data.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); // degrees * 1e6 + GpsData.Longitude = rx_packet->data.longitude * (rx_packet->data.ew_indicator == 1 ? +1 : -1); // degrees * 1e6 + 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; // + GpsData.HDOP = (float)rx_packet->data.hdop / 100; // +// GpsData.VDOP; // + GPSPositionSet(&GpsData); + + // remove the spent packet from the buffer + if (gps_rx_buffer_wr > sizeof(t_gps_bin_packet)) + { + memmove(gps_rx_buffer, gps_rx_buffer + sizeof(t_gps_bin_packet), gps_rx_buffer_wr - sizeof(t_gps_bin_packet)); + gps_rx_buffer_wr -= sizeof(t_gps_bin_packet); + } + else + gps_rx_buffer_wr = 0; + + 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 c1a647454..9fee7ae2b 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -1,3 +1,33 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file NMEA.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 "NMEA.h" @@ -5,6 +35,8 @@ #include "gpstime.h" #include "gpssatellites.h" +#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) + // Debugging //#define GPSDEBUG #ifdef GPSDEBUG @@ -18,10 +50,6 @@ #define NMEA_DEBUG_PGTOP ///< define to enable debug of PGTOP messages #endif -/* Utility functions */ -static float NMEA_real_to_float(char *nmea_real); -static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon); - /* NMEA sentence parsers */ struct nmea_parser { @@ -29,39 +57,45 @@ struct nmea_parser { bool(*handler) (GPSPositionData * GpsData, char *sentence); }; -static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence); -static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence); -static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence); -static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence); -static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence); -static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence); -static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence); + #ifdef ENABLE_GPS_NMEA + static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence); + static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence); + static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence); + static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence); + static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence); + static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence); + #endif -static struct nmea_parser nmea_parsers[] = { - { - .prefix = "GPGGA", - .handler = nmeaProcessGPGGA, - }, - { - .prefix = "GPVTG", - .handler = nmeaProcessGPVTG, - }, - { - .prefix = "GPGSA", - .handler = nmeaProcessGPGSA, - }, - { - .prefix = "GPRMC", - .handler = nmeaProcessGPRMC, - }, - { - .prefix = "GPZDA", - .handler = nmeaProcessGPZDA, - }, - { - .prefix = "GPGSV", - .handler = nmeaProcessGPGSV, - }, + static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence); + + static struct nmea_parser nmea_parsers[] = { + + #ifdef ENABLE_GPS_NMEA + { + .prefix = "GPGGA", + .handler = nmeaProcessGPGGA, + }, + { + .prefix = "GPVTG", + .handler = nmeaProcessGPVTG, + }, + { + .prefix = "GPGSA", + .handler = nmeaProcessGPGSA, + }, + { + .prefix = "GPRMC", + .handler = nmeaProcessGPRMC, + }, + { + .prefix = "GPZDA", + .handler = nmeaProcessGPZDA, + }, + { + .prefix = "GPGSV", + .handler = nmeaProcessGPGSV, + }, + #endif { .prefix = "PGTOP", .handler = nmeaProcessPGTOP, @@ -118,6 +152,121 @@ bool NMEA_checksum(char *nmea_sentence) return (checksum_computed == checksum_received); } +/* + * This function only exists to deal with a linking + * failure in the stdlib function strtof(). This + * implementation does not rely on the _sbrk() syscall + * like strtof() does. + */ + +/* Parse a number encoded in a string of the format: + * [-]NN.nnnnn + * into a signed whole part and an unsigned fractional part. + * The fract_units field indicates the units of the fractional part as + * 1 whole = 10^fract_units fract + */ +static bool NMEA_parse_real(int32_t * whole, uint32_t * fract, uint8_t * fract_units, char *field) +{ + char *s = field; + char *field_w; + char *field_f; + + PIOS_DEBUG_Assert(whole); + PIOS_DEBUG_Assert(fract); + PIOS_DEBUG_Assert(fract_units); + + field_w = strsep(&s, "."); + field_f = s; + + *whole = strtol(field_w, NULL, 10); + + if (field_w) { + /* decimal was found so we may have a fractional part */ + *fract = strtoul(field_f, NULL, 10); + *fract_units = strlen(field_f); + } else { + /* no decimal was found, fractional part is zero */ + *fract = 0; + *fract_units = 0; + } + + return true; +} + +static float NMEA_real_to_float(char *nmea_real) +{ + int32_t whole; + uint32_t fract; + uint8_t fract_units; + + /* Sanity checks */ + PIOS_DEBUG_Assert(nmea_real); + + if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) { + return false; + } + + /* Convert to float */ + return (((float)whole) + fract * pow(10, -fract_units)); +} + +#ifdef ENABLE_GPS_NMEA +/* + * Parse a field in the format: + * DD[D]MM.mmmm[mm] + * into a fixed-point representation in units of (degrees * 1e-7) + */ +static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon) +{ + int32_t num_DDDMM; + uint32_t num_m; + uint8_t units; + + /* Sanity checks */ + PIOS_DEBUG_Assert(nmea_latlon); + PIOS_DEBUG_Assert(latlon); + + if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { + return false; + } + + /* scale up the mmmm[mm] field apropriately depending on # of digits */ + switch (units) { + case 0: + /* no digits, value is zero so no scaling */ + break; + case 1: /* m */ + num_m *= 1e6; /* m000000 */ + break; + case 2: /* mm */ + num_m *= 1e5; /* mm00000 */ + break; + case 3: /* mmm */ + num_m *= 1e4; /* mmm0000 */ + break; + case 4: /* mmmm */ + num_m *= 1e3; /* mmmm000 */ + break; + case 5: /* mmmmm */ + num_m *= 1e2; /* mmmmm00 */ + break; + case 6: /* mmmmmm */ + num_m *= 1e1; /* mmmmmm0 */ + break; + default: + /* unhandled format */ + num_m = 0; + break; + } + + *latlon = (num_DDDMM / 100) * 1e7; /* scale the whole degrees */ + *latlon += (num_DDDMM % 100) * 1e7 / 60; /* add in the scaled decimal whole minutes */ + *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */ + + return true; +} +#endif // ENABLE_GPS_NMEA + /** * Parses a complete NMEA sentence and updates the GPSPosition UAVObject * \param[in] An NMEA sentence with a valid checksum @@ -163,6 +312,8 @@ bool NMEA_update_position(char *nmea_sentence) return true; } +#ifdef ENABLE_GPS_NMEA + /** * Parse an NMEA GPGGA sentence and update the given UAVObject * \param[in] A pointer to a GPSPosition UAVObject to be updated. @@ -595,6 +746,8 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence) 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. @@ -710,115 +863,4 @@ static bool nmeaProcessPGTOP(GPSPositionData * GpsData, char *sentence) return true; } -/* Parse a number encoded in a string of the format: - * [-]NN.nnnnn - * into a signed whole part and an unsigned fractional part. - * The fract_units field indicates the units of the fractional part as - * 1 whole = 10^fract_units fract - */ -static bool NMEA_parse_real(int32_t * whole, uint32_t * fract, uint8_t * fract_units, char *field) -{ - char *s = field; - char *field_w; - char *field_f; - - PIOS_DEBUG_Assert(whole); - PIOS_DEBUG_Assert(fract); - PIOS_DEBUG_Assert(fract_units); - - field_w = strsep(&s, "."); - field_f = s; - - *whole = strtol(field_w, NULL, 10); - - if (field_w) { - /* decimal was found so we may have a fractional part */ - *fract = strtoul(field_f, NULL, 10); - *fract_units = strlen(field_f); - } else { - /* no decimal was found, fractional part is zero */ - *fract = 0; - *fract_units = 0; - } - - return true; -} - -/* - * This function only exists to deal with a linking - * failure in the stdlib function strtof(). This - * implementation does not rely on the _sbrk() syscall - * like strtof() does. - */ - -static float NMEA_real_to_float(char *nmea_real) -{ - int32_t whole; - uint32_t fract; - uint8_t fract_units; - - /* Sanity checks */ - PIOS_DEBUG_Assert(nmea_real); - - if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) { - return false; - } - - /* Convert to float */ - return (((float)whole) + fract * pow(10, -fract_units)); -} - -/* - * Parse a field in the format: - * DD[D]MM.mmmm[mm] - * into a fixed-point representation in units of (degrees * 1e-7) - */ -static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon) -{ - int32_t num_DDDMM; - uint32_t num_m; - uint8_t units; - - /* Sanity checks */ - PIOS_DEBUG_Assert(nmea_latlon); - PIOS_DEBUG_Assert(latlon); - - if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { - return false; - } - - /* scale up the mmmm[mm] field apropriately depending on # of digits */ - switch (units) { - case 0: - /* no digits, value is zero so no scaling */ - break; - case 1: /* m */ - num_m *= 1e6; /* m000000 */ - break; - case 2: /* mm */ - num_m *= 1e5; /* mm00000 */ - break; - case 3: /* mmm */ - num_m *= 1e4; /* mmm0000 */ - break; - case 4: /* mmmm */ - num_m *= 1e3; /* mmmm000 */ - break; - case 5: /* mmmmm */ - num_m *= 1e2; /* mmmmm00 */ - break; - case 6: /* mmmmmm */ - num_m *= 1e1; /* mmmmmm0 */ - break; - default: - /* unhandled format */ - num_m = 0; - break; - } - - *latlon = (num_DDDMM / 100) * 1e7; /* scale the whole degrees */ - *latlon += (num_DDDMM % 100) * 1e7 / 60; /* add in the scaled decimal whole minutes */ - *latlon += num_m / 60; /* add in the scaled decimal fractional minutes */ - - 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 86a92d285..1ae683c9a 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -34,6 +34,8 @@ #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 new file mode 100644 index 000000000..6a5234430 --- /dev/null +++ b/flight/Modules/GPS/inc/GTOP_BIN.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @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 792eef2cd..5c1b13f8b 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -1,10 +1,43 @@ +/** + ****************************************************************************** + * @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 +#include "gps_mode.h" -extern bool NMEA_update_position(char *nmea_sentence); -extern bool NMEA_checksum(char *nmea_sentence); +#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 #endif /* NMEA_H */ diff --git a/flight/Modules/GPS/inc/gps_mode.h b/flight/Modules/GPS/inc/gps_mode.h new file mode 100644 index 000000000..d6d29efb1 --- /dev/null +++ b/flight/Modules/GPS/inc/gps_mode.h @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * @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 +// +// note: the NMEA includes the one-sentance code as OP has the memory for it + +//#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 single sentance 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 + +/** + * @} + * @} + */