mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-30 15:52:12 +01:00
OP-25: preliminary GPS module, needs some cleaning. I'll start testing when I get HW ready.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@351 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
sambas
sambas
parent
2e5b1d02ea
commit
22698313fc
490
flight/OpenPilot/Modules/GPS/GPS.c
Normal file
490
flight/OpenPilot/Modules/GPS/GPS.c
Normal file
@ -0,0 +1,490 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file GPS.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 "gpsinfo.h"
|
||||||
|
#include "buffer.h"
|
||||||
|
|
||||||
|
|
||||||
|
// constants/macros/typdefs
|
||||||
|
#define NMEA_BUFFERSIZE 110
|
||||||
|
|
||||||
|
// Message Codes
|
||||||
|
#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded
|
||||||
|
#define NMEA_GPGGA 1 // Global Positioning System Fix Data
|
||||||
|
#define NMEA_GPVTG 2 // Course over ground and ground speed
|
||||||
|
#define NMEA_GPGLL 3 // Geographic position - latitude/longitude
|
||||||
|
#define NMEA_GPGSV 4 // GPS satellites in view
|
||||||
|
#define NMEA_GPGSA 5 // GPS DOP and active satellites
|
||||||
|
#define NMEA_GPRMC 6 // Recommended minimum specific GPS data
|
||||||
|
#define NMEA_UNKNOWN 0xFF// Packet received but not known
|
||||||
|
|
||||||
|
// Debugging
|
||||||
|
|
||||||
|
//#define GPSDEBUG
|
||||||
|
|
||||||
|
#ifdef GPSDEBUG
|
||||||
|
#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
|
||||||
|
#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
|
||||||
|
#define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// functions
|
||||||
|
void nmeaInit(void);
|
||||||
|
uint8_t* nmeaGetPacketBuffer(void);
|
||||||
|
uint8_t nmeaProcess(cBuffer* rxBuffer);
|
||||||
|
void nmeaProcessGPGGA(uint8_t* packet);
|
||||||
|
void nmeaProcessGPVTG(uint8_t* packet);
|
||||||
|
|
||||||
|
// Private functions
|
||||||
|
static void gpsTask(void* parameters);
|
||||||
|
static void periodicEventHandler(UAVObjEvent* ev);
|
||||||
|
static void registerObject(UAVObjHandle obj);
|
||||||
|
static void updateObject(UAVObjHandle obj);
|
||||||
|
static int32_t addObject(UAVObjHandle obj);
|
||||||
|
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
|
||||||
|
|
||||||
|
cBuffer gpsRxBuffer;
|
||||||
|
static char gpsRxData[512];
|
||||||
|
// Global variables
|
||||||
|
GpsInfoType GpsInfo;
|
||||||
|
uint8_t NmeaPacket[NMEA_BUFFERSIZE];
|
||||||
|
|
||||||
|
// Private constants
|
||||||
|
#define MAX_QUEUE_SIZE 20
|
||||||
|
#define STACK_SIZE 100
|
||||||
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
|
||||||
|
#define REQ_TIMEOUT_MS 500
|
||||||
|
#define MAX_RETRIES 3
|
||||||
|
|
||||||
|
// Private types
|
||||||
|
|
||||||
|
// Private variables
|
||||||
|
static COMPortTypeDef gpsPort;
|
||||||
|
static xQueueHandle queue;
|
||||||
|
static xTaskHandle gpsTaskHandle;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Initialise the gps module
|
||||||
|
* \return -1 if initialisation failed
|
||||||
|
* \return 0 on success
|
||||||
|
*/
|
||||||
|
int32_t GpsInitialize(void)
|
||||||
|
{
|
||||||
|
// Create object queue
|
||||||
|
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||||
|
|
||||||
|
// TODO: Get gps settings object
|
||||||
|
gpsPort = COM_USART2;
|
||||||
|
|
||||||
|
// Init input buffer size 512
|
||||||
|
bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
|
||||||
|
|
||||||
|
// Process all registered objects and connect queue for updates
|
||||||
|
UAVObjIterate(®isterObject);
|
||||||
|
|
||||||
|
// Start gps task
|
||||||
|
xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Register a new object, adds object to local list and connects the queue depending on the object's
|
||||||
|
* telemetry settings.
|
||||||
|
* \param[in] obj Object to connect
|
||||||
|
*/
|
||||||
|
void registerObject(UAVObjHandle obj)
|
||||||
|
{
|
||||||
|
// Setup object for periodic updates
|
||||||
|
addObject(obj);
|
||||||
|
|
||||||
|
// Setup object for telemetry updates
|
||||||
|
updateObject(obj);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Update object's queue connections and timer, depending on object's settings
|
||||||
|
* \param[in] obj Object to updates
|
||||||
|
*/
|
||||||
|
void updateObject(UAVObjHandle obj)
|
||||||
|
{
|
||||||
|
UAVObjMetadata metadata;
|
||||||
|
int32_t eventMask;
|
||||||
|
|
||||||
|
// Get metadata
|
||||||
|
UAVObjGetMetadata(obj, &metadata);
|
||||||
|
|
||||||
|
// Setup object depending on update mode
|
||||||
|
if(metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC)
|
||||||
|
{
|
||||||
|
// Set update period
|
||||||
|
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
|
||||||
|
// Connect queue
|
||||||
|
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||||
|
if(UAVObjIsMetaobject(obj))
|
||||||
|
{
|
||||||
|
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||||
|
}
|
||||||
|
UAVObjConnectQueue(obj, queue, eventMask);
|
||||||
|
}
|
||||||
|
else if(metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE)
|
||||||
|
{
|
||||||
|
// Set update period
|
||||||
|
setUpdatePeriod(obj, 0);
|
||||||
|
// Connect queue
|
||||||
|
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||||
|
if(UAVObjIsMetaobject(obj))
|
||||||
|
{
|
||||||
|
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||||
|
}
|
||||||
|
UAVObjConnectQueue(obj, queue, eventMask);
|
||||||
|
}
|
||||||
|
else if(metadata.telemetryUpdateMode == UPDATEMODE_MANUAL)
|
||||||
|
{
|
||||||
|
// Set update period
|
||||||
|
setUpdatePeriod(obj, 0);
|
||||||
|
// Connect queue
|
||||||
|
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||||
|
if(UAVObjIsMetaobject(obj))
|
||||||
|
{
|
||||||
|
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||||
|
}
|
||||||
|
UAVObjConnectQueue(obj, queue, eventMask);
|
||||||
|
}
|
||||||
|
else if(metadata.telemetryUpdateMode == UPDATEMODE_NEVER)
|
||||||
|
{
|
||||||
|
// Set update period
|
||||||
|
setUpdatePeriod(obj, 0);
|
||||||
|
// Disconnect queue
|
||||||
|
UAVObjDisconnectQueue(obj, queue);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* gps task. Processes input buffer. It does not return.
|
||||||
|
*/
|
||||||
|
static void gpsTask(void* parameters)
|
||||||
|
{
|
||||||
|
int32_t gpsRxOverflow=0;
|
||||||
|
|
||||||
|
// Loop forever
|
||||||
|
while(1)
|
||||||
|
{
|
||||||
|
/* This blocks the task until there is something on the buffer */
|
||||||
|
while(PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
|
||||||
|
{
|
||||||
|
if( !bufferAddToEnd(&gpsRxBuffer, PIOS_COM_ReceiveBuffer(gpsPort)) )
|
||||||
|
{
|
||||||
|
// no space in buffer
|
||||||
|
// count overflow
|
||||||
|
gpsRxOverflow++;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
nmeaProcess(&gpsRxBuffer);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Event handler for periodic object updates (called by the event dispatcher)
|
||||||
|
*/
|
||||||
|
static void periodicEventHandler(UAVObjEvent* ev)
|
||||||
|
{
|
||||||
|
// Push event to the telemetry queue
|
||||||
|
xQueueSend(queue, ev, 0); // do not wait if queue is full
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Setup object for periodic updates.
|
||||||
|
* \param[in] obj The object to update
|
||||||
|
* \return 0 Success
|
||||||
|
* \return -1 Failure
|
||||||
|
*/
|
||||||
|
static int32_t addObject(UAVObjHandle obj)
|
||||||
|
{
|
||||||
|
UAVObjEvent ev;
|
||||||
|
|
||||||
|
// Add object for periodic updates
|
||||||
|
ev.obj = obj;
|
||||||
|
ev.instId = UAVOBJ_ALL_INSTANCES;
|
||||||
|
ev.event = EV_UPDATED_MANUAL;
|
||||||
|
return EventPeriodicCreate(&ev, &periodicEventHandler, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set update period of object (it must be already setup for periodic updates)
|
||||||
|
* \param[in] obj The object to update
|
||||||
|
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
|
||||||
|
* \return 0 Success
|
||||||
|
* \return -1 Failure
|
||||||
|
*/
|
||||||
|
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
|
||||||
|
{
|
||||||
|
UAVObjEvent ev;
|
||||||
|
|
||||||
|
// Add object for periodic updates
|
||||||
|
ev.obj = obj;
|
||||||
|
ev.instId = UAVOBJ_ALL_INSTANCES;
|
||||||
|
ev.event = EV_UPDATED_MANUAL;
|
||||||
|
return EventPeriodicUpdate(&ev, &periodicEventHandler, updatePeriodMs);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t* nmeaGetPacketBuffer(void)
|
||||||
|
{
|
||||||
|
return NmeaPacket;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Prosesses NMEA sentences
|
||||||
|
* \param[in] cBuffer for prosessed nmea sentences
|
||||||
|
* \return Message code for found packet
|
||||||
|
* \return 0xFF NO packet found
|
||||||
|
*/
|
||||||
|
uint8_t nmeaProcess(cBuffer* rxBuffer)
|
||||||
|
{
|
||||||
|
uint8_t foundpacket = NMEA_NODATA;
|
||||||
|
uint8_t startFlag = FALSE;
|
||||||
|
//u08 data;
|
||||||
|
uint16_t i,j;
|
||||||
|
|
||||||
|
// process the receive buffer
|
||||||
|
// go through buffer looking for packets
|
||||||
|
while(rxBuffer->datalength)
|
||||||
|
{
|
||||||
|
// look for a start of NMEA packet
|
||||||
|
if(bufferGetAtIndex(rxBuffer,0) == '$')
|
||||||
|
{
|
||||||
|
// found start
|
||||||
|
startFlag = TRUE;
|
||||||
|
// when start is found, we leave it intact in the receive buffer
|
||||||
|
// in case the full NMEA string is not completely received. The
|
||||||
|
// start will be detected in the next nmeaProcess iteration.
|
||||||
|
|
||||||
|
// done looking for start
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
bufferGetFromFront(rxBuffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
// if we detected a start, look for end of packet
|
||||||
|
if(startFlag)
|
||||||
|
{
|
||||||
|
for(i=1; i<(rxBuffer->datalength)-1; i++)
|
||||||
|
{
|
||||||
|
// check for end of NMEA packet <CR><LF>
|
||||||
|
if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n'))
|
||||||
|
{
|
||||||
|
// have a packet end
|
||||||
|
// dump initial '$'
|
||||||
|
bufferGetFromFront(rxBuffer);
|
||||||
|
// copy packet to NmeaPacket
|
||||||
|
for(j=0; j<(i-1); j++)
|
||||||
|
{
|
||||||
|
// although NMEA strings should be 80 characters or less,
|
||||||
|
// receive buffer errors can generate erroneous packets.
|
||||||
|
// Protect against packet buffer overflow
|
||||||
|
if(j<(NMEA_BUFFERSIZE-1))
|
||||||
|
NmeaPacket[j] = bufferGetFromFront(rxBuffer);
|
||||||
|
else
|
||||||
|
bufferGetFromFront(rxBuffer);
|
||||||
|
}
|
||||||
|
// null terminate it
|
||||||
|
NmeaPacket[j] = 0;
|
||||||
|
// dump <CR><LF> from rxBuffer
|
||||||
|
bufferGetFromFront(rxBuffer);
|
||||||
|
bufferGetFromFront(rxBuffer);
|
||||||
|
//DEBUG
|
||||||
|
//iprintf("$%s\r\n",NmeaPacket);
|
||||||
|
//
|
||||||
|
#ifdef NMEA_DEBUG_PKT
|
||||||
|
printf("$%s\r\n",NmeaPacket);
|
||||||
|
#endif
|
||||||
|
// found a packet
|
||||||
|
// done with this processing session
|
||||||
|
foundpacket = NMEA_UNKNOWN;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(foundpacket)
|
||||||
|
{
|
||||||
|
// check message type and process appropriately
|
||||||
|
if(!strncmp((char *)NmeaPacket, "GPGGA", 5))
|
||||||
|
{
|
||||||
|
// process packet of this type
|
||||||
|
nmeaProcessGPGGA(NmeaPacket);
|
||||||
|
// report packet type
|
||||||
|
foundpacket = NMEA_GPGGA;
|
||||||
|
}
|
||||||
|
else if(!strncmp((char *)NmeaPacket, "GPVTG", 5))
|
||||||
|
{
|
||||||
|
// process packet of this type
|
||||||
|
nmeaProcessGPVTG(NmeaPacket);
|
||||||
|
// report packet type
|
||||||
|
foundpacket = NMEA_GPVTG;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(rxBuffer->datalength >= rxBuffer->size)
|
||||||
|
{
|
||||||
|
// if we found no packet, and the buffer is full
|
||||||
|
// we're logjammed, flush entire buffer
|
||||||
|
bufferFlush(rxBuffer);
|
||||||
|
}
|
||||||
|
return foundpacket;
|
||||||
|
}
|
||||||
|
|
||||||
|
//#define CHAR_GPS
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Prosesses NMEA GPGGA sentences
|
||||||
|
* \param[in] Buffer for parsed nmea GPGGA sentence
|
||||||
|
*/
|
||||||
|
void nmeaProcessGPGGA(uint8_t* packet)
|
||||||
|
{
|
||||||
|
uint8_t i,j=0;
|
||||||
|
char* endptr;
|
||||||
|
double degrees, minutesfrac;
|
||||||
|
|
||||||
|
#ifdef NMEA_DEBUG_GGA
|
||||||
|
printf("NMEA: %s\r\n",packet);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// start parsing just after "GPGGA,"
|
||||||
|
i = 6;
|
||||||
|
// attempt to reject empty packets right away
|
||||||
|
if(packet[i]==',' && packet[i+1]==',')
|
||||||
|
return;
|
||||||
|
|
||||||
|
// get UTC time [hhmmss.sss]
|
||||||
|
GpsInfo.PosLLA.TimeOfFix.f = strtod((char *)&packet[i], &endptr);
|
||||||
|
while(packet[i++] != ','); // next field: latitude
|
||||||
|
|
||||||
|
// get latitude [ddmm.mmmmm]
|
||||||
|
GpsInfo.PosLLA.lat.f = strtod((char *)&packet[i], &endptr);
|
||||||
|
// convert to pure degrees [dd.dddd] format
|
||||||
|
minutesfrac = modf(GpsInfo.PosLLA.lat.f/100, °rees);
|
||||||
|
GpsInfo.PosLLA.lat.f = degrees + (minutesfrac*100)/60;
|
||||||
|
// convert to radians
|
||||||
|
GpsInfo.PosLLA.lat.f *= (M_PI/180);
|
||||||
|
#ifdef CHAR_GPS
|
||||||
|
while(packet[i++] != ',')
|
||||||
|
GpsInfo.PosLLA.lat.c[j++]=packet[i-1]; // next field: N/S indicator
|
||||||
|
GpsInfo.PosLLA.lat.c[j]=0;
|
||||||
|
#else
|
||||||
|
while(packet[i++] != ','); // next field: N/S indicator
|
||||||
|
#endif
|
||||||
|
// correct latitute for N/S
|
||||||
|
if(packet[i] == 'S') GpsInfo.PosLLA.lat.f = -GpsInfo.PosLLA.lat.f;
|
||||||
|
while(packet[i++] != ','); // next field: longitude
|
||||||
|
|
||||||
|
// get longitude [ddmm.mmmmm]
|
||||||
|
GpsInfo.PosLLA.lon.f = strtod((char *)&packet[i], &endptr);
|
||||||
|
// convert to pure degrees [dd.dddd] format
|
||||||
|
minutesfrac = modf(GpsInfo.PosLLA.lon.f/100, °rees);
|
||||||
|
GpsInfo.PosLLA.lon.f = degrees + (minutesfrac*100)/60;
|
||||||
|
// convert to radians
|
||||||
|
GpsInfo.PosLLA.lon.f *= (M_PI/180);
|
||||||
|
#ifdef CHAR_GPS
|
||||||
|
j=0;
|
||||||
|
while(packet[i++] != ',')
|
||||||
|
GpsInfo.PosLLA.lon.c[j++]=packet[i-1]; // next field: E/W indicator
|
||||||
|
GpsInfo.PosLLA.lon.c[j]=0;
|
||||||
|
#else
|
||||||
|
while(packet[i++] != ','); // next field: E/W indicator
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// correct latitute for E/W
|
||||||
|
if(packet[i] == 'W') GpsInfo.PosLLA.lon.f = -GpsInfo.PosLLA.lon.f;
|
||||||
|
while(packet[i++] != ','); // next field: position fix status
|
||||||
|
|
||||||
|
// position fix status
|
||||||
|
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
|
||||||
|
// check for good position fix
|
||||||
|
if( (packet[i] != '0') && (packet[i] != ',') )
|
||||||
|
GpsInfo.PosLLA.updates++;
|
||||||
|
while(packet[i++] != ','); // next field: satellites used
|
||||||
|
|
||||||
|
// get number of satellites used in GPS solution
|
||||||
|
GpsInfo.numSVs = atoi((char *)&packet[i]);
|
||||||
|
while(packet[i++] != ','); // next field: HDOP (horizontal dilution of precision)
|
||||||
|
while(packet[i++] != ','); // next field: altitude
|
||||||
|
|
||||||
|
// get altitude (in meters)
|
||||||
|
GpsInfo.PosLLA.alt.f = strtod((char *)&packet[i], &endptr);
|
||||||
|
|
||||||
|
while(packet[i++] != ','); // next field: altitude units, always 'M'
|
||||||
|
while(packet[i++] != ','); // next field: geoid seperation
|
||||||
|
while(packet[i++] != ','); // next field: seperation units
|
||||||
|
while(packet[i++] != ','); // next field: DGPS age
|
||||||
|
while(packet[i++] != ','); // next field: DGPS station ID
|
||||||
|
while(packet[i++] != '*'); // next field: checksum
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Prosesses NMEA GPVTG sentences
|
||||||
|
* \param[in] Buffer for parsed nmea GPVTG sentence
|
||||||
|
*/
|
||||||
|
void nmeaProcessGPVTG(uint8_t* packet)
|
||||||
|
{
|
||||||
|
uint8_t i;
|
||||||
|
char* endptr;
|
||||||
|
|
||||||
|
#ifdef NMEA_DEBUG_VTG
|
||||||
|
printf("NMEA: %s\r\n",packet);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// start parsing just after "GPVTG,"
|
||||||
|
i = 6;
|
||||||
|
// attempt to reject empty packets right away
|
||||||
|
if(packet[i]==',' && packet[i+1]==',')
|
||||||
|
return;
|
||||||
|
|
||||||
|
// get course (true north ref) in degrees [ddd.dd]
|
||||||
|
GpsInfo.VelHS.heading.f = strtod((char *)&packet[i], &endptr);
|
||||||
|
while(packet[i++] != ','); // next field: 'T'
|
||||||
|
while(packet[i++] != ','); // next field: course (magnetic north)
|
||||||
|
|
||||||
|
// get course (magnetic north ref) in degrees [ddd.dd]
|
||||||
|
//GpsInfo.VelHS.heading.f = strtod(&packet[i], &endptr);
|
||||||
|
while(packet[i++] != ','); // next field: 'M'
|
||||||
|
while(packet[i++] != ','); // next field: speed (knots)
|
||||||
|
|
||||||
|
// get speed in knots
|
||||||
|
//GpsInfo.VelHS.speed.f = strtod(&packet[i], &endptr);
|
||||||
|
while(packet[i++] != ','); // next field: 'N'
|
||||||
|
while(packet[i++] != ','); // next field: speed (km/h)
|
||||||
|
|
||||||
|
// get speed in km/h
|
||||||
|
GpsInfo.VelHS.speed.f = strtod((char *)&packet[i], &endptr);
|
||||||
|
while(packet[i++] != ','); // next field: 'K'
|
||||||
|
while(packet[i++] != '*'); // next field: checksum
|
||||||
|
|
||||||
|
GpsInfo.VelHS.updates++;
|
||||||
|
}
|
||||||
|
|
||||||
139
flight/OpenPilot/Modules/GPS/buffer.c
Normal file
139
flight/OpenPilot/Modules/GPS/buffer.c
Normal file
@ -0,0 +1,139 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file buffer.c
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||||
|
* @brief see below
|
||||||
|
* 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
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*! \file buffer.c \brief Multipurpose byte buffer structure and methods. */
|
||||||
|
//*****************************************************************************
|
||||||
|
//
|
||||||
|
// File Name : 'buffer.c'
|
||||||
|
// Title : Multipurpose byte buffer structure and methods
|
||||||
|
// Author : Pascal Stang - Copyright (C) 2001-2002
|
||||||
|
// Created : 9/23/2001
|
||||||
|
// Revised : 9/23/2001
|
||||||
|
// Version : 1.0
|
||||||
|
// Target MCU : any
|
||||||
|
// Editor Tabs : 4
|
||||||
|
//
|
||||||
|
// This code is distributed under the GNU Public License
|
||||||
|
// which can be found at http://www.gnu.org/licenses/gpl.txt
|
||||||
|
//
|
||||||
|
//*****************************************************************************
|
||||||
|
|
||||||
|
#include "buffer.h"
|
||||||
|
|
||||||
|
// global variables
|
||||||
|
|
||||||
|
// initialization
|
||||||
|
|
||||||
|
void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size)
|
||||||
|
{
|
||||||
|
// set start pointer of the buffer
|
||||||
|
buffer->dataptr = start;
|
||||||
|
buffer->size = size;
|
||||||
|
// initialize index and length
|
||||||
|
buffer->dataindex = 0;
|
||||||
|
buffer->datalength = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// access routines
|
||||||
|
unsigned char bufferGetFromFront(cBuffer* buffer)
|
||||||
|
{
|
||||||
|
unsigned char data = 0;
|
||||||
|
|
||||||
|
// check to see if there's data in the buffer
|
||||||
|
if(buffer->datalength)
|
||||||
|
{
|
||||||
|
// get the first character from buffer
|
||||||
|
data = buffer->dataptr[buffer->dataindex];
|
||||||
|
// move index down and decrement length
|
||||||
|
buffer->dataindex++;
|
||||||
|
if(buffer->dataindex >= buffer->size)
|
||||||
|
{
|
||||||
|
buffer->dataindex %= buffer->size;
|
||||||
|
}
|
||||||
|
buffer->datalength--;
|
||||||
|
}
|
||||||
|
// return
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes)
|
||||||
|
{
|
||||||
|
// dump numbytes from the front of the buffer
|
||||||
|
// are we dumping less than the entire buffer?
|
||||||
|
if(numbytes < buffer->datalength)
|
||||||
|
{
|
||||||
|
// move index down by numbytes and decrement length by numbytes
|
||||||
|
buffer->dataindex += numbytes;
|
||||||
|
if(buffer->dataindex >= buffer->size)
|
||||||
|
{
|
||||||
|
buffer->dataindex %= buffer->size;
|
||||||
|
}
|
||||||
|
buffer->datalength -= numbytes;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// flush the whole buffer
|
||||||
|
buffer->datalength = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index)
|
||||||
|
{
|
||||||
|
// return character at index in buffer
|
||||||
|
return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)];
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data)
|
||||||
|
{
|
||||||
|
// make sure the buffer has room
|
||||||
|
if(buffer->datalength < buffer->size)
|
||||||
|
{
|
||||||
|
// save data byte at end of buffer
|
||||||
|
buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data;
|
||||||
|
// increment the length
|
||||||
|
buffer->datalength++;
|
||||||
|
// return success
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
else return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned char bufferIsNotFull(cBuffer* buffer)
|
||||||
|
{
|
||||||
|
// check to see if the buffer has room
|
||||||
|
// return true if there is room
|
||||||
|
return (buffer->datalength < buffer->size);
|
||||||
|
}
|
||||||
|
|
||||||
|
void bufferFlush(cBuffer* buffer)
|
||||||
|
{
|
||||||
|
// flush contents of the buffer
|
||||||
|
buffer->datalength = 0;
|
||||||
|
}
|
||||||
|
|
||||||
34
flight/OpenPilot/Modules/GPS/inc/GPS.h
Normal file
34
flight/OpenPilot/Modules/GPS/inc/GPS.h
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file GPS.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_H
|
||||||
|
#define GPS_H
|
||||||
|
|
||||||
|
int32_t GpsInitialize(void);
|
||||||
|
|
||||||
|
#endif // GPS_H
|
||||||
87
flight/OpenPilot/Modules/GPS/inc/buffer.h
Normal file
87
flight/OpenPilot/Modules/GPS/inc/buffer.h
Normal file
@ -0,0 +1,87 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file buffer.c
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||||
|
* @brief see below
|
||||||
|
* 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
|
||||||
|
*
|
||||||
|
*****************************************************************************/
|
||||||
|
|
||||||
|
/*! \file buffer.h \brief Multipurpose byte buffer structure and methods. */
|
||||||
|
//*****************************************************************************
|
||||||
|
//
|
||||||
|
// File Name : 'buffer.h'
|
||||||
|
// Title : Multipurpose byte buffer structure and methods
|
||||||
|
// Author : Pascal Stang - Copyright (C) 2001-2002
|
||||||
|
// Created : 9/23/2001
|
||||||
|
// Revised : 11/16/2002
|
||||||
|
// Version : 1.1
|
||||||
|
// Target MCU : any
|
||||||
|
// Editor Tabs : 4
|
||||||
|
//
|
||||||
|
/// \ingroup general
|
||||||
|
/// \defgroup buffer Circular Byte-Buffer Structure and Function Library (buffer.c)
|
||||||
|
/// \code #include "buffer.h" \endcode
|
||||||
|
/// \par Overview
|
||||||
|
/// This byte-buffer structure provides an easy and efficient way to store
|
||||||
|
/// and process a stream of bytes. You can create as many buffers as you
|
||||||
|
/// like (within memory limits), and then use this common set of functions to
|
||||||
|
/// access each buffer. The buffers are designed for FIFO operation (first
|
||||||
|
/// in, first out). This means that the first byte you put in the buffer
|
||||||
|
/// will be the first one you get when you read out the buffer. Supported
|
||||||
|
/// functions include buffer initialize, get byte from front of buffer, add
|
||||||
|
/// byte to end of buffer, check if buffer is full, and flush buffer. The
|
||||||
|
/// buffer uses a circular design so no copying of data is ever necessary.
|
||||||
|
/// This buffer is not dynamically allocated, it has a user-defined fixed
|
||||||
|
/// maximum size. This buffer is used in many places in the avrlib code.
|
||||||
|
//
|
||||||
|
// This code is distributed under the GNU Public License
|
||||||
|
// which can be found at http://www.gnu.org/licenses/gpl.txt
|
||||||
|
//
|
||||||
|
//*****************************************************************************
|
||||||
|
//@{
|
||||||
|
|
||||||
|
#ifndef BUFFER_H
|
||||||
|
#define BUFFER_H
|
||||||
|
|
||||||
|
// structure/typdefs
|
||||||
|
|
||||||
|
//! cBuffer structure
|
||||||
|
typedef struct struct_cBuffer
|
||||||
|
{
|
||||||
|
unsigned char *dataptr; ///< the physical memory address where the buffer is stored
|
||||||
|
unsigned short size; ///< the allocated size of the buffer
|
||||||
|
unsigned short datalength; ///< the length of the data currently in the buffer
|
||||||
|
unsigned short dataindex; ///< the index into the buffer where the data starts
|
||||||
|
} cBuffer;
|
||||||
|
|
||||||
|
// function prototypes
|
||||||
|
|
||||||
|
//! initialize a buffer to start at a given address and have given size
|
||||||
|
void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size);
|
||||||
|
|
||||||
|
//! get the first byte from the front of the buffer
|
||||||
|
unsigned char bufferGetFromFront(cBuffer* buffer);
|
||||||
|
|
||||||
|
//! dump (discard) the first numbytes from the front of the buffer
|
||||||
|
void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes);
|
||||||
|
|
||||||
|
//! get a byte at the specified index in the buffer (kind of like array access)
|
||||||
|
// ** note: this does not remove the byte that was read from the buffer
|
||||||
|
unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index);
|
||||||
|
|
||||||
|
//! add a byte to the end of the buffer
|
||||||
|
unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data);
|
||||||
|
|
||||||
|
//! check if the buffer is full/not full (returns non-zero value if not full)
|
||||||
|
unsigned char bufferIsNotFull(cBuffer* buffer);
|
||||||
|
|
||||||
|
//! flush (clear) the contents of the buffer
|
||||||
|
void bufferFlush(cBuffer* buffer);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
//@}
|
||||||
120
flight/OpenPilot/Modules/GPS/inc/gpsinfo.h
Normal file
120
flight/OpenPilot/Modules/GPS/inc/gpsinfo.h
Normal file
@ -0,0 +1,120 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file gpsinfo.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||||
|
* @brief Include file for 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 GPSINFO_H
|
||||||
|
#define GPSINFO_H
|
||||||
|
|
||||||
|
// constants/macros/typdefs
|
||||||
|
typedef union union_float_u32
|
||||||
|
{
|
||||||
|
float f;
|
||||||
|
unsigned long i;
|
||||||
|
unsigned char b[4];
|
||||||
|
unsigned char c[20];
|
||||||
|
} float_u32;
|
||||||
|
|
||||||
|
typedef union union_double_u64
|
||||||
|
{
|
||||||
|
double f;
|
||||||
|
unsigned long long i;
|
||||||
|
unsigned char b[8];
|
||||||
|
} double_u64;
|
||||||
|
|
||||||
|
struct PositionLLA
|
||||||
|
{
|
||||||
|
float_u32 lat;
|
||||||
|
float_u32 lon;
|
||||||
|
float_u32 alt;
|
||||||
|
float_u32 TimeOfFix;
|
||||||
|
uint16_t updates;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct VelocityENU
|
||||||
|
{
|
||||||
|
float_u32 east;
|
||||||
|
float_u32 north;
|
||||||
|
float_u32 up;
|
||||||
|
float_u32 TimeOfFix;
|
||||||
|
uint16_t updates;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct VelocityHS
|
||||||
|
{
|
||||||
|
float_u32 heading;
|
||||||
|
float_u32 speed;
|
||||||
|
float_u32 TimeOfFix;
|
||||||
|
uint16_t updates;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct PositionECEF
|
||||||
|
{
|
||||||
|
float_u32 x;
|
||||||
|
float_u32 y;
|
||||||
|
float_u32 z;
|
||||||
|
float_u32 TimeOfFix;
|
||||||
|
uint16_t updates;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct VelocityECEF
|
||||||
|
{
|
||||||
|
float_u32 x;
|
||||||
|
float_u32 y;
|
||||||
|
float_u32 z;
|
||||||
|
float_u32 TimeOfFix;
|
||||||
|
uint16_t updates;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct UTCtime
|
||||||
|
{
|
||||||
|
uint16_t year;
|
||||||
|
uint8_t month;
|
||||||
|
uint8_t day;
|
||||||
|
uint8_t hour;
|
||||||
|
uint8_t min;
|
||||||
|
uint8_t sec;
|
||||||
|
uint8_t offset;
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct struct_GpsInfo
|
||||||
|
{
|
||||||
|
float_u32 TimeOfWeek;
|
||||||
|
uint16_t WeekNum;
|
||||||
|
float_u32 UtcOffset;
|
||||||
|
uint8_t numSVs;
|
||||||
|
|
||||||
|
|
||||||
|
struct PositionLLA PosLLA;
|
||||||
|
struct PositionECEF PosECEF;
|
||||||
|
struct VelocityECEF VelECEF;
|
||||||
|
struct VelocityENU VelENU;
|
||||||
|
struct VelocityHS VelHS;
|
||||||
|
struct UTCtime UTC;
|
||||||
|
|
||||||
|
} GpsInfoType;
|
||||||
|
|
||||||
|
#endif // GPSINFO_H
|
||||||
Loading…
x
Reference in New Issue
Block a user