rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Flight/GPS Renamed GPSObject to PositionActual and implemented field for GPS status (NoGPS, NoFix, Fix2D and Fix3D)

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@734 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-06-10 01:28:06 +00:00 committed by vassilis
parent 6844743050
commit eaa9d97939
6 changed files with 160 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/OpenPilot/Makefile
View File

@ -131,7 +131,7 @@ SRC += $(OPUAVOBJ)/exampleobject1.c
SRC += $(OPUAVOBJ)/exampleobject2.c
SRC += $(OPUAVOBJ)/examplesettings.c
SRC += $(OPUAVOBJ)/objectpersistence.c
SRC += $(OPUAVOBJ)/gpsobject.c
SRC += $(OPUAVOBJ)/positionactual.c
SRC += $(OPUAVOBJ)/gcstelemetrystats.c
SRC += $(OPUAVOBJ)/flighttelemetrystats.c
SRC += $(OPUAVOBJ)/systemstats.c

75
flight/OpenPilot/Modules/GPS/GPS.c
View File

@ -26,7 +26,7 @@
#include "openpilot.h"
#include "buffer.h"
#include "GPS.h"
#include "gpsobject.h"
#include "positionactual.h"
// constants/macros/typdefs
#define NMEA_BUFFERSIZE 128
@ -41,6 +41,8 @@
#define NMEA_GPRMC 6 // Recommended minimum specific GPS data
#define NMEA_UNKNOWN 0xFF// Packet received but not known
#define GPS_TIMEOUT_MS 500
// Debugging
//#define GPSDEBUG
@ -79,6 +81,9 @@ static xTaskHandle gpsTaskHandle;
cBuffer gpsRxBuffer;
static char gpsRxData[512];
char NmeaPacket[NMEA_BUFFERSIZE];
static uint32_t numUpdates;
static uint32_t numErrors;
static uint32_t timeOfLastUpdateMs;
/**
* Initialise the gps module
@ -88,6 +93,12 @@ char NmeaPacket[NMEA_BUFFERSIZE];
int32_t GPSInitialize(void)
{
signed portBASE_TYPE xReturn;
// Init vars
numUpdates = 0;
numErrors = 0;
timeOfLastUpdateMs = 0;
// TODO: Get gps settings object
gpsPort = COM_USART2;
@ -109,6 +120,8 @@ static void gpsTask(void* parameters)
int32_t gpsRxOverflow=0;
char c;
portTickType xDelay = 100 / portTICK_RATE_MS;
PositionActualData GpsData;
uint32_t timeNowMs;
// Loop forever
while(1)
@ -126,6 +139,15 @@ static void gpsTask(void* parameters)
}
nmeaProcess(&gpsRxBuffer);
}
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ( (timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS )
{
PositionActualGet(&GpsData);
GpsData.Status = POSITIONACTUAL_STATUS_NOGPS;
PositionActualSet(&GpsData);
}
// Block task until next update
vTaskDelay(xDelay);
}
}
@ -145,8 +167,8 @@ char nmeaChecksum(char* gps_buffer)
{
char checksum=0;
char checksum_received=0;
GpsObjectData GpsData;
GpsObjectGet(&GpsData);
PositionActualData GpsData;
PositionActualGet(&GpsData);
for(int x=0; x<NMEA_BUFFERSIZE; x++)
{
@ -165,14 +187,13 @@ char nmeaChecksum(char* gps_buffer)
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum);
if(checksum == checksum_received)
{
GpsData.Updates++;
GpsObjectSet(&GpsData);
++numUpdates;
timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
return 1;
}
else
{
GpsData.Failures++;
GpsObjectSet(&GpsData);
++numErrors;
return 0;
}
}
@ -300,7 +321,7 @@ uint8_t nmeaProcess(cBuffer* rxBuffer)
*/
void nmeaProcessGPGGA(char* packet)
{
GpsObjectData GpsData;
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
@ -324,7 +345,7 @@ void nmeaProcessGPGGA(char* packet)
return;
}
GpsObjectGet(&GpsData);
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPGGA header
tokens = strsep(&packet, delimiter);
@ -417,7 +438,7 @@ void nmeaProcessGPGGA(char* packet)
// next field: DGPS age
// next field: DGPS station ID
// next field: checksum
GpsObjectSet(&GpsData);
PositionActualSet(&GpsData);
}
/**
@ -426,7 +447,7 @@ void nmeaProcessGPGGA(char* packet)
*/
void nmeaProcessGPRMC(char* packet)
{
GpsObjectData GpsData;
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
@ -450,7 +471,7 @@ void nmeaProcessGPRMC(char* packet)
return;
}
GpsObjectGet(&GpsData);
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPRMC header
tokens = strsep(&packet, delimiter);
@ -517,7 +538,7 @@ void nmeaProcessGPRMC(char* packet)
tokens = strsep(&packet, delimiter);
deg=strtol (tokens,&pEnd,10);
desim=strtol (pEnd+1,NULL,10);
GpsData.GroundSpeed = (deg+(desim/100.0))*0.51444; //OPGPS style to m/s
GpsData.Groundspeed = (deg+(desim/100.0))*0.51444; //OPGPS style to m/s
// next field: True course
// get True course
@ -533,7 +554,7 @@ void nmeaProcessGPRMC(char* packet)
// next field: Magnetic variation
// next field: E or W
// next field: checksum
GpsObjectSet(&GpsData);
PositionActualSet(&GpsData);
}
@ -543,7 +564,7 @@ void nmeaProcessGPRMC(char* packet)
*/
void nmeaProcessGPVTG(char* packet)
{
GpsObjectData GpsData;
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
@ -562,7 +583,7 @@ void nmeaProcessGPVTG(char* packet)
// checksum not valid
return;
}
GpsObjectGet(&GpsData);
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPVTG header
@ -590,7 +611,7 @@ void nmeaProcessGPVTG(char* packet)
tokens = strsep(&packet, delimiter);
// next field: 'K'
// next field: checksum
GpsObjectSet(&GpsData);
PositionActualSet(&GpsData);
}
@ -600,12 +621,13 @@ void nmeaProcessGPVTG(char* packet)
*/
void nmeaProcessGPGSA(char* packet)
{
GpsObjectData GpsData;
PositionActualData GpsData;
char *tokens;
char *delimiter = ",";
char *pEnd;
long value,desim;
int mode;
#ifdef NMEA_DEBUG_GSA
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
@ -621,7 +643,7 @@ void nmeaProcessGPGSA(char* packet)
// checksum not valid
return;
}
GpsObjectGet(&GpsData);
PositionActualGet(&GpsData);
// tokenizer for nmea sentence
//GPGSA header
@ -633,6 +655,19 @@ void nmeaProcessGPGSA(char* packet)
// next field: Mode
// Mode: 1=Fix not available, 2=2D, 3=3D
tokens = strsep(&packet, delimiter);
mode = atoi(tokens);
if (mode == 1)
{
GpsData.Status = POSITIONACTUAL_STATUS_NOFIX;
}
else if (mode == 2)
{
GpsData.Status = POSITIONACTUAL_STATUS_FIX2D;
}
else if (mode == 3)
{
GpsData.Status = POSITIONACTUAL_STATUS_FIX3D;
}
// next field: 3-14 IDs of SVs used in position fix (null for unused fields)
tokens = strsep(&packet, delimiter);
@ -664,7 +699,7 @@ void nmeaProcessGPGSA(char* packet)
desim=strtol (pEnd+1,NULL,10);
GpsData.VDOP=value+desim/100.0;
// next field: checksum
GpsObjectSet(&GpsData);
PositionActualSet(&GpsData);
}

92
flight/OpenPilot/UAVObjects/inc/gpsobject.h
View File

@ -1,92 +0,0 @@
/**
******************************************************************************
*
* @file gpsobject.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the GpsObject object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: gpsobject.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @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 GPSOBJECT_H
#define GPSOBJECT_H
// Object constants
#define GPSOBJECT_OBJID 3887395742U
#define GPSOBJECT_NAME "GpsObject"
#define GPSOBJECT_METANAME "GpsObjectMeta"
#define GPSOBJECT_ISSINGLEINST 1
#define GPSOBJECT_ISSETTINGS 0
#define GPSOBJECT_NUMBYTES sizeof(GpsObjectData)
// Object access macros
#define GpsObjectGet(dataOut) UAVObjGetData(GpsObjectHandle(), dataOut)
#define GpsObjectSet(dataIn) UAVObjSetData(GpsObjectHandle(), dataIn)
#define GpsObjectInstGet(instId, dataOut) UAVObjGetInstanceData(GpsObjectHandle(), instId, dataOut)
#define GpsObjectInstSet(instId, dataIn) UAVObjSetInstanceData(GpsObjectHandle(), instId, dataIn)
#define GpsObjectConnectQueue(queue) UAVObjConnectQueue(GpsObjectHandle(), queue, EV_MASK_ALL_UPDATES)
#define GpsObjectConnectCallback(cb) UAVObjConnectCallback(GpsObjectHandle(), cb, EV_MASK_ALL_UPDATES)
#define GpsObjectCreateInstance() UAVObjCreateInstance(GpsObjectHandle())
#define GpsObjectRequestUpdate() UAVObjRequestUpdate(GpsObjectHandle())
#define GpsObjectRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(GpsObjectHandle(), instId)
#define GpsObjectUpdated() UAVObjUpdated(GpsObjectHandle())
#define GpsObjectInstUpdated(instId) UAVObjUpdated(GpsObjectHandle(), instId)
#define GpsObjectGetMetadata(dataOut) UAVObjGetMetadata(GpsObjectHandle(), dataOut)
#define GpsObjectSetMetadata(dataIn) UAVObjSetMetadata(GpsObjectHandle(), dataIn)
// Object data
typedef struct {
float Latitude;
float Longitude;
float Altitude;
float Heading;
float GroundSpeed;
int8_t Satellites;
uint16_t Updates;
uint16_t Failures;
float PDOP;
float HDOP;
float VDOP;
} __attribute__((packed)) GpsObjectData;
// Field information
// Field Latitude information
// Field Longitude information
// Field Altitude information
// Field Heading information
// Field GroundSpeed information
// Field Satellites information
// Field Updates information
// Field Failures information
// Field PDOP information
// Field HDOP information
// Field VDOP information
// Generic interface functions
int32_t GpsObjectInitialize();
UAVObjHandle GpsObjectHandle();
#endif // GPSOBJECT_H

92
flight/OpenPilot/UAVObjects/inc/positionactual.h Normal file
View File

@ -0,0 +1,92 @@
/**
******************************************************************************
*
* @file positionactual.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the PositionActual object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: positionactual.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @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 POSITIONACTUAL_H
#define POSITIONACTUAL_H
// Object constants
#define POSITIONACTUAL_OBJID 981132812U
#define POSITIONACTUAL_NAME "PositionActual"
#define POSITIONACTUAL_METANAME "PositionActualMeta"
#define POSITIONACTUAL_ISSINGLEINST 1
#define POSITIONACTUAL_ISSETTINGS 0
#define POSITIONACTUAL_NUMBYTES sizeof(PositionActualData)
// Object access macros
#define PositionActualGet(dataOut) UAVObjGetData(PositionActualHandle(), dataOut)
#define PositionActualSet(dataIn) UAVObjSetData(PositionActualHandle(), dataIn)
#define PositionActualInstGet(instId, dataOut) UAVObjGetInstanceData(PositionActualHandle(), instId, dataOut)
#define PositionActualInstSet(instId, dataIn) UAVObjSetInstanceData(PositionActualHandle(), instId, dataIn)
#define PositionActualConnectQueue(queue) UAVObjConnectQueue(PositionActualHandle(), queue, EV_MASK_ALL_UPDATES)
#define PositionActualConnectCallback(cb) UAVObjConnectCallback(PositionActualHandle(), cb, EV_MASK_ALL_UPDATES)
#define PositionActualCreateInstance() UAVObjCreateInstance(PositionActualHandle())
#define PositionActualRequestUpdate() UAVObjRequestUpdate(PositionActualHandle())
#define PositionActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(PositionActualHandle(), instId)
#define PositionActualUpdated() UAVObjUpdated(PositionActualHandle())
#define PositionActualInstUpdated(instId) UAVObjUpdated(PositionActualHandle(), instId)
#define PositionActualGetMetadata(dataOut) UAVObjGetMetadata(PositionActualHandle(), dataOut)
#define PositionActualSetMetadata(dataIn) UAVObjSetMetadata(PositionActualHandle(), dataIn)
// Object data
typedef struct {
uint8_t Status;
float Latitude;
float Longitude;
float Altitude;
float Heading;
float Groundspeed;
int8_t Satellites;
float PDOP;
float HDOP;
float VDOP;
} __attribute__((packed)) PositionActualData;
// Field information
// Field Status information
/* Enumeration options for field Status */
typedef enum { POSITIONACTUAL_STATUS_NOGPS=0, POSITIONACTUAL_STATUS_NOFIX=1, POSITIONACTUAL_STATUS_FIX2D=2, POSITIONACTUAL_STATUS_FIX3D=3, } PositionActualStatusOptions;
// Field Latitude information
// Field Longitude information
// Field Altitude information
// Field Heading information
// Field Groundspeed information
// Field Satellites information
// Field PDOP information
// Field HDOP information
// Field VDOP information
// Generic interface functions
int32_t PositionActualInitialize();
UAVObjHandle PositionActualHandle();
#endif // POSITIONACTUAL_H

20
flight/OpenPilot/UAVObjects/gpsobject.c → flight/OpenPilot/UAVObjects/positionactual.c
View File

@ -1,12 +1,12 @@
/**
******************************************************************************
*
* @file gpsobject.c
* @file positionactual.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the GpsObject object. This file has been
* @brief Implementation of the PositionActual object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: gpsobject.xml.
* @note Object definition file: positionactual.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
@ -30,7 +30,7 @@
*/
#include "openpilot.h"
#include "gpsobject.h"
#include "positionactual.h"
// Private variables
static UAVObjHandle handle;
@ -43,11 +43,11 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId);
* \return 0 Success
* \return -1 Failure
*/
int32_t GpsObjectInitialize()
int32_t PositionActualInitialize()
{
// Register object with the object manager
handle = UAVObjRegister(GPSOBJECT_OBJID, GPSOBJECT_NAME, GPSOBJECT_METANAME, 0,
GPSOBJECT_ISSINGLEINST, GPSOBJECT_ISSETTINGS, GPSOBJECT_NUMBYTES, &setDefaults);
handle = UAVObjRegister(POSITIONACTUAL_OBJID, POSITIONACTUAL_NAME, POSITIONACTUAL_METANAME, 0,
POSITIONACTUAL_ISSINGLEINST, POSITIONACTUAL_ISSETTINGS, POSITIONACTUAL_NUMBYTES, &setDefaults);
// Done
if (handle != 0)
@ -67,12 +67,12 @@ int32_t GpsObjectInitialize()
*/
static void setDefaults(UAVObjHandle obj, uint16_t instId)
{
GpsObjectData data;
PositionActualData data;
UAVObjMetadata metadata;
// Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(GpsObjectData));
memset(&data, 0, sizeof(PositionActualData));
UAVObjSetInstanceData(obj, instId, &data);
@ -93,7 +93,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
/**
* Get object handle
*/
UAVObjHandle GpsObjectHandle()
UAVObjHandle PositionActualHandle()
{
return handle;
}

4
flight/OpenPilot/UAVObjects/uavobjectsinit.c
View File

@ -40,10 +40,10 @@
#include "examplesettings.h"
#include "flighttelemetrystats.h"
#include "gcstelemetrystats.h"
#include "gpsobject.h"
#include "manualcontrolcommand.h"
#include "manualcontrolsettings.h"
#include "objectpersistence.h"
#include "positionactual.h"
#include "stabilizationsettings.h"
#include "systemalarms.h"
#include "systemsettings.h"
@ -69,10 +69,10 @@ void UAVObjectsInitializeAll()
ExampleSettingsInitialize();
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
GpsObjectInitialize();
ManualControlCommandInitialize();
ManualControlSettingsInitialize();
ObjectPersistenceInitialize();
PositionActualInitialize();
StabilizationSettingsInitialize();
SystemAlarmsInitialize();
SystemSettingsInitialize();