rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

Added GTOP BINARY mode to the GPS module.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2837 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip 2011-02-22 10:16:50 +00:00 committed by pip
parent 42cf204dcd
commit 4547013901
7 changed files with 804 additions and 250 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

307
flight/Modules/GPS/GPS.c
View File

@ -28,50 +28,80 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
// ****************
#include "openpilot.h"
#include "GPS.h"
#include <stdbool.h>
#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;
#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
//#define DISABLE_GPS_TRESHOLD
//#define ENABLE_GPS_ONESENTENCE
//#define ENABLE_DEFAULT_NMEA
#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");
#ifdef ENABLE_GPS_BINARY_GTOP
// switch to GTOP binary mode
PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n");
#endif
#ifdef ENABLE_DEFAULT_NMEA
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n");
#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) {
while (1)
{
#ifdef ENABLE_GPS_BINARY_GTOP
// GTOP BINARY GPS mode
c = PIOS_COM_ReceiveBuffer(gpsPort);
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors);
if (res >= 0)
{
numUpdates++;
/* 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;
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);
}
}
}
// ****************
/**
* @}
* @}

260
flight/Modules/GPS/GTOP_BIN.c Normal file
View File

@ -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 <stdbool.h>
#include <string.h> // 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

338
flight/Modules/GPS/NMEA.c
View File

@ -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)

2
flight/Modules/GPS/inc/GPS.h
View File

@ -34,6 +34,8 @@
#ifndef GPS_H
#define GPS_H
#include "gps_mode.h"
int32_t GPSInitialize(void);
#endif // GPS_H

42
flight/Modules/GPS/inc/GTOP_BIN.h Normal file
View File

@ -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 <stdint.h>
#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

37
flight/Modules/GPS/inc/NMEA.h
View File

@ -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 <stdbool.h>
#include <stdint.h>
#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 */

60
flight/Modules/GPS/inc/gps_mode.h Normal file
View File

@ -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
/**
* @}
* @}
*/