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

Merge remote-tracking branch 'origin/CorvusCorax_unidirectional-GPS-com' into next

Browse Source 
Conflicts:
	flight/OpenPilot/System/pios_board.c
This commit is contained in:
James Cotton 2011-09-04 13:33:52 -05:00
commit b1e0366525
9 changed files with 136 additions and 682 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,13 +35,7 @@
#include <stdbool.h> #include <stdbool.h>
#ifdef ENABLE_GPS_BINARY_GTOP #include "NMEA.h"
#include "GTOP_BIN.h"
#endif
#if defined(ENABLE_GPS_ONESENTENCE_GTOP) || defined(ENABLE_GPS_NMEA)
#include "NMEA.h"
#endif
#include "gpsposition.h" #include "gpsposition.h"
#include "homelocation.h" #include "homelocation.h"
@ -63,25 +57,16 @@ static float GravityAccel(float latitude, float longitude, float altitude);
// **************** // ****************
// Private constants // 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_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 #ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation // Unfortunately need a good size stack for the WMM calculation
#ifdef ENABLE_GPS_BINARY_GTOP #define STACK_SIZE_BYTES 800
#define STACK_SIZE_BYTES 800
#else
#define STACK_SIZE_BYTES 800
#endif
#else #else
#ifdef ENABLE_GPS_BINARY_GTOP #define STACK_SIZE_BYTES 650
#define STACK_SIZE_BYTES 440
#else
#define STACK_SIZE_BYTES 440
#endif
#endif #endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
@ -93,9 +78,7 @@ static uint32_t gpsPort;
static xTaskHandle gpsTaskHandle; static xTaskHandle gpsTaskHandle;
#ifndef ENABLE_GPS_BINARY_GTOP static char* gps_rx_buffer;
static char gps_rx_buffer[128];
#endif
static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastCommandMs;
static uint32_t timeOfLastUpdateMs; static uint32_t timeOfLastUpdateMs;
@ -135,6 +118,9 @@ int32_t GPSInitialize(void)
// TODO: Get gps settings object // TODO: Get gps settings object
gpsPort = PIOS_COM_GPS; gpsPort = PIOS_COM_GPS;
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
PIOS_Assert(gps_rx_buffer);
return 0; return 0;
} }
MODULE_INITCALL(GPSInitialize, GPSStart) MODULE_INITCALL(GPSInitialize, GPSStart)
@ -150,45 +136,11 @@ static void gpsTask(void *parameters)
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
GPSPositionData GpsData; GPSPositionData GpsData;
#ifdef ENABLE_GPS_BINARY_GTOP
GTOP_BIN_init();
#else
uint8_t rx_count = 0; uint8_t rx_count = 0;
bool start_flag = false; bool start_flag = false;
bool found_cr = false; bool found_cr = false;
int32_t gpsRxOverflow = 0; 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; numUpdates = 0;
numChecksumErrors = 0; numChecksumErrors = 0;
numParsingErrors = 0; numParsingErrors = 0;
@ -200,103 +152,86 @@ static void gpsTask(void *parameters)
while (1) while (1)
{ {
uint8_t c; uint8_t c;
#ifdef ENABLE_GPS_BINARY_GTOP // NMEA or SINGLE-SENTENCE GPS mode
// GTOP BINARY 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) start_flag = true;
{ found_cr = false;
numUpdates++; 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; timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs; timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = 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 // Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
@ -309,30 +244,6 @@ static void gpsTask(void *parameters)
GPSPositionSet(&GpsData); GPSPositionSet(&GpsData);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); 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 else
{ // we appear to be receiving GPS sentences OK, we've had an update { // we appear to be receiving GPS sentences OK, we've had an update

262
flight/Modules/GPS/GTOP_BIN.c
View File

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

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

@ -40,8 +40,6 @@
#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP)
// Debugging // Debugging
#ifdef ENABLE_DEBUG_MSG #ifdef ENABLE_DEBUG_MSG
//#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages //#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_GSA ///< define to enable debug of GSA messages
//#define NMEA_DEBUG_GSV ///< define to enable debug of GSV 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_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__) #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
#else #else
#define DEBUG_MSG(format, ...) #define DEBUG_MSG(format, ...)
@ -69,56 +66,45 @@ struct nmea_parser {
uint32_t cnt; uint32_t cnt;
}; };
#ifdef ENABLE_GPS_NMEA static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
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 nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); static bool nmeaProcessGPVTG(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 nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); static bool nmeaProcessGPZDA(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 nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
#endif
static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
static struct nmea_parser nmea_parsers[] = { static struct nmea_parser nmea_parsers[] = {
{
#ifdef ENABLE_GPS_NMEA .prefix = "GPGGA",
{ .handler = nmeaProcessGPGGA,
.prefix = "GPGGA", .cnt = 0,
.handler = nmeaProcessGPGGA, },
.cnt = 0, {
}, .prefix = "GPVTG",
{ .handler = nmeaProcessGPVTG,
.prefix = "GPVTG", .cnt = 0,
.handler = nmeaProcessGPVTG, },
.cnt = 0, {
}, .prefix = "GPGSA",
{ .handler = nmeaProcessGPGSA,
.prefix = "GPGSA", .cnt = 0,
.handler = nmeaProcessGPGSA, },
.cnt = 0, {
}, .prefix = "GPRMC",
{ .handler = nmeaProcessGPRMC,
.prefix = "GPRMC", .cnt = 0,
.handler = nmeaProcessGPRMC, },
.cnt = 0, {
}, .prefix = "GPZDA",
{ .handler = nmeaProcessGPZDA,
.prefix = "GPZDA", .cnt = 0,
.handler = nmeaProcessGPZDA, },
.cnt = 0, {
}, .prefix = "GPGSV",
{ .handler = nmeaProcessGPGSV,
.prefix = "GPGSV", .cnt = 0,
.handler = nmeaProcessGPGSV, },
.cnt = 0,
},
#endif
{
.prefix = "PGTOP",
.handler = nmeaProcessPGTOP,
.cnt = 0,
},
}; };
static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) 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)); return (((float)whole) + fract * pow(10, -fract_units));
} }
#ifdef ENABLE_GPS_NMEA
/* /*
* Parse a field in the format: * Parse a field in the format:
* DD[D]MM.mmmm[mm] * 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; return true;
} }
#endif // ENABLE_GPS_NMEA
/** /**
@ -376,7 +360,6 @@ bool NMEA_update_position(char *nmea_sentence)
return true; return true;
} }
#ifdef ENABLE_GPS_NMEA
/** /**
* Parse an NMEA GPGGA sentence and update the given UAVObject * Parse an NMEA GPGGA sentence and update the given UAVObject
@ -675,83 +658,3 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
return true; 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)

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

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

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

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

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

@ -33,11 +33,8 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#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_update_position(char *nmea_sentence); extern bool NMEA_checksum(char *nmea_sentence);
extern bool NMEA_checksum(char *nmea_sentence);
#endif
#endif /* NMEA_H */ #endif /* NMEA_H */

58
flight/Modules/GPS/inc/gps_mode.h
View File

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

17
flight/OpenPilot/System/pios_board.c
View File

@ -608,7 +608,6 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = {
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 96 #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_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
@ -1174,14 +1173,24 @@ void PIOS_Board_Init(void) {
PIOS_Assert(0); PIOS_Assert(0);
} }
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); 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(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
tx_buffer, PIOS_COM_GPS_TX_BUF_LEN)) { 0, 0)) {
PIOS_Assert(0); PIOS_Assert(0);
} }
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_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,
NULL, 0)) {
PIOS_Assert(0);
>>>>>>> origin/CorvusCorax_unidirectional-GPS-com
} }
#endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_GPS */
break; break;

6
flight/OpenPilot/System/pios_board_posix.c
View File

@ -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_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_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. * Board specific number of devices.
@ -164,12 +164,10 @@ void PIOS_Board_Init(void) {
PIOS_Assert(0); PIOS_Assert(0);
} }
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); 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(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id, if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
tx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) { NULL, 0)) {
PIOS_Assert(0); PIOS_Assert(0);
} }
} }