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

OP-4 Flight/Telemetry Implemented telemetry connection manager

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@578 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-05-04 01:29:04 +00:00 committed by vassilis
parent 0c65a90a9d
commit e10a16f1c3
11 changed files with 254 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flight/OpenPilot/Modules/System/systemmod.c
View File

@ -150,6 +150,17 @@ static void objectUpdatedCb(UAVObjEvent* ev)
UAVObjSaveMetaobjects();
}
}
else if ( objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE)
{
if ( objper.Objects == OBJECTPERSISTENCE_OBJECTS_SETTINGS || objper.Objects == OBJECTPERSISTENCE_OBJECTS_ALL)
{
UAVObjDeleteSettings();
}
if ( objper.Objects == OBJECTPERSISTENCE_OBJECTS_METAOBJECTS || objper.Objects == OBJECTPERSISTENCE_OBJECTS_ALL)
{
UAVObjDeleteMetaobjects();
}
}
}
}

165
flight/OpenPilot/Modules/Telemetry/telemetry.c
View File

@ -25,6 +25,7 @@
#include "openpilot.h"
#include "flighttelemetrystats.h"
#include "gcstelemetrystats.h"
// Private constants
#define MAX_QUEUE_SIZE 20
@ -58,6 +59,8 @@ static void updateObject(UAVObjHandle obj);
static int32_t addObject(UAVObjHandle obj);
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
static void processObjEvent(UAVObjEvent* ev);
static void updateTelemetryStats();
static void gcsTelemetryStatsUpdated();
/**
* Initialise the telemetry module
@ -85,7 +88,8 @@ int32_t TelemetryInitialize(void)
txErrors = 0;
txRetries = 0;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
EventPeriodicQueueCreate(&ev, queue, STATS_UPDATE_PERIOD_MS);
GCSTelemetryStatsConnectQueue(queue);
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char*)"TelTx", STACK_SIZE, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
@ -175,46 +179,15 @@ static void processObjEvent(UAVObjEvent* ev)
UAVObjMetadata metadata;
int32_t retries;
int32_t success;
UAVTalkStats utalkStats;
FlightTelemetryStatsData stats;
// Check if this is a periodic timer event (used to update stats)
if (ev->obj == 0)
if ( ev->obj == 0 )
{
// Get stats
UAVTalkGetStats(&utalkStats);
UAVTalkResetStats();
// Update stats object
FlightTelemetryStatsGet(&stats);
if (utalkStats.rxBytes > 0)
{
stats.Connected = FLIGHTTELEMETRYSTATS_CONNECTED_TRUE;
}
else
{
stats.Connected = FLIGHTTELEMETRYSTATS_CONNECTED_FALSE;
}
stats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
stats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
stats.RxFailures += utalkStats.rxErrors;
stats.TxFailures += txErrors;
stats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
FlightTelemetryStatsSet(&stats);
// Update the telemetry alarm
if ( stats.Connected == FLIGHTTELEMETRYSTATS_CONNECTED_TRUE )
{
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
}
else
{
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
}
updateTelemetryStats();
}
else if ( ev->obj == GCSTelemetryStatsHandle() )
{
gcsTelemetryStatsUpdated();
}
// This is an object update, handle based on event type
else
{
// Get object metadata
@ -396,3 +369,119 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
}
/**
* Called each time the GCS telemetry stats object is updated.
* Trigger a flight telemetry stats update if a connection is not
* yet established.
*/
static void gcsTelemetryStatsUpdated()
{
FlightTelemetryStatsData flightStats;
GCSTelemetryStatsData gcsStats;
FlightTelemetryStatsGet(&flightStats);
GCSTelemetryStatsGet(&gcsStats);
if ( flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED ||
gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED )
{
updateTelemetryStats();
}
}
/**
* Update telemetry statistics and handle connection handshake
*/
static void updateTelemetryStats()
{
UAVTalkStats utalkStats;
FlightTelemetryStatsData flightStats;
GCSTelemetryStatsData gcsStats;
uint8_t forceUpdate;
// Get stats
UAVTalkGetStats(&utalkStats);
UAVTalkResetStats();
// Get object data
FlightTelemetryStatsGet(&flightStats);
GCSTelemetryStatsGet(&gcsStats);
// Update stats object
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED )
{
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
}
else
{
flightStats.RxDataRate = 0;
flightStats.TxDataRate = 0;
flightStats.RxFailures = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
txErrors = 0;
txRetries = 0;
}
// Update connection state
forceUpdate = 1;
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED )
{
// Wait for connection request
if ( gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ )
{
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
}
}
else if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK )
{
// Wait for connection
if ( gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED )
{
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
}
else if ( gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED )
{
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
}
else if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED )
{
if ( gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || utalkStats.rxBytes == 0 )
{
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
else
{
forceUpdate = 0;
}
}
else
{
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
// Update the telemetry alarm
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED )
{
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
}
else
{
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
}
// Update object
FlightTelemetryStatsSet(&flightStats);
// Force telemetry update if not connected
if ( forceUpdate )
{
FlightTelemetryStatsUpdated();
}
}

2
flight/OpenPilot/UAVObjects/flighttelemetrystats.c
View File

@ -81,7 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = 5000;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_NEVER;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
metadata.loggingUpdatePeriod = 5000;

6
flight/OpenPilot/UAVObjects/gcstelemetrystats.c
View File

@ -78,11 +78,11 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
// Initialize object metadata to their default values
metadata.telemetryAcked = 1;
metadata.telemetryUpdateMode = UPDATEMODE_NEVER;
metadata.telemetryUpdateMode = UPDATEMODE_MANUAL;
metadata.telemetryUpdatePeriod = 0;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_NEVER;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.gcsTelemetryUpdatePeriod = 5000;
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
metadata.loggingUpdatePeriod = 0;
UAVObjSetMetadata(obj, &metadata);

2
flight/OpenPilot/UAVObjects/gpsobject.c
View File

@ -81,7 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = 1000;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_NEVER;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
metadata.loggingUpdatePeriod = 1000;

10
flight/OpenPilot/UAVObjects/inc/flighttelemetrystats.h
View File

@ -33,7 +33,7 @@
#define FLIGHTTELEMETRYSTATS_H
// Object constants
#define FLIGHTTELEMETRYSTATS_OBJID 766280320U
#define FLIGHTTELEMETRYSTATS_OBJID 1712072286U
#define FLIGHTTELEMETRYSTATS_NAME "FlightTelemetryStats"
#define FLIGHTTELEMETRYSTATS_METANAME "FlightTelemetryStatsMeta"
#define FLIGHTTELEMETRYSTATS_ISSINGLEINST 1
@ -57,7 +57,7 @@
// Object data
typedef struct {
uint8_t Connected;
uint8_t Status;
float TxDataRate;
float RxDataRate;
uint32_t TxFailures;
@ -67,9 +67,9 @@ typedef struct {
} __attribute__((packed)) FlightTelemetryStatsData;
// Field information
// Field Connected information
/* Enumeration options for field Connected */
typedef enum { FLIGHTTELEMETRYSTATS_CONNECTED_TRUE=0, FLIGHTTELEMETRYSTATS_CONNECTED_FALSE=1, } FlightTelemetryStatsConnectedOptions;
// Field Status information
/* Enumeration options for field Status */
typedef enum { FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED=0, FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEREQ=1, FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK=2, FLIGHTTELEMETRYSTATS_STATUS_CONNECTED=3, } FlightTelemetryStatsStatusOptions;
// Field TxDataRate information
// Field RxDataRate information
// Field TxFailures information

10
flight/OpenPilot/UAVObjects/inc/gcstelemetrystats.h
View File

@ -33,7 +33,7 @@
#define GCSTELEMETRYSTATS_H
// Object constants
#define GCSTELEMETRYSTATS_OBJID 607270704U
#define GCSTELEMETRYSTATS_OBJID 1998458950U
#define GCSTELEMETRYSTATS_NAME "GCSTelemetryStats"
#define GCSTELEMETRYSTATS_METANAME "GCSTelemetryStatsMeta"
#define GCSTELEMETRYSTATS_ISSINGLEINST 1
@ -57,7 +57,7 @@
// Object data
typedef struct {
uint8_t Connected;
uint8_t Status;
float TxDataRate;
float RxDataRate;
uint32_t TxFailures;
@ -67,9 +67,9 @@ typedef struct {
} __attribute__((packed)) GCSTelemetryStatsData;
// Field information
// Field Connected information
/* Enumeration options for field Connected */
typedef enum { GCSTELEMETRYSTATS_CONNECTED_TRUE=0, GCSTELEMETRYSTATS_CONNECTED_FALSE=1, } GCSTelemetryStatsConnectedOptions;
// Field Status information
/* Enumeration options for field Status */
typedef enum { GCSTELEMETRYSTATS_STATUS_DISCONNECTED=0, GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ=1, GCSTELEMETRYSTATS_STATUS_HANDSHAKEACK=2, GCSTELEMETRYSTATS_STATUS_CONNECTED=3, } GCSTelemetryStatsStatusOptions;
// Field TxDataRate information
// Field RxDataRate information
// Field TxFailures information

2
flight/OpenPilot/UAVObjects/inc/objectpersistence.h
View File

@ -65,7 +65,7 @@ typedef struct {
// Field information
// Field Operation information
/* Enumeration options for field Operation */
typedef enum { OBJECTPERSISTENCE_OPERATION_LOAD=0, OBJECTPERSISTENCE_OPERATION_SAVE=1, } ObjectPersistenceOperationOptions;
typedef enum { OBJECTPERSISTENCE_OPERATION_LOAD=0, OBJECTPERSISTENCE_OPERATION_SAVE=1, OBJECTPERSISTENCE_OPERATION_DELETE=2, } ObjectPersistenceOperationOptions;
// Field Objects information
/* Enumeration options for field Objects */
typedef enum { OBJECTPERSISTENCE_OBJECTS_ALL=0, OBJECTPERSISTENCE_OBJECTS_SETTINGS=1, OBJECTPERSISTENCE_OBJECTS_METAOBJECTS=2, } ObjectPersistenceObjectsOptions;

3
flight/OpenPilot/UAVObjects/inc/uavobjectmanager.h
View File

@ -120,12 +120,15 @@ int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn);
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut);
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId);
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId);
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId);
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, FILEINFO* file);
UAVObjHandle UAVObjLoadFromFile(FILEINFO* file);
int32_t UAVObjSaveSettings();
int32_t UAVObjLoadSettings();
int32_t UAVObjDeleteSettings();
int32_t UAVObjSaveMetaobjects();
int32_t UAVObjLoadMetaobjects();
int32_t UAVObjDeleteMetaobjects();
int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn);
int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut);
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn);

2
flight/OpenPilot/UAVObjects/systemstats.c
View File

@ -81,7 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = 1000;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_NEVER;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
metadata.loggingUpdatePeriod = 1000;

96
flight/OpenPilot/UAVObjects/uavobjectmanager.c
View File

@ -742,6 +742,40 @@ int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
return 0;
}
/**
* Delete an object from the file system (SD card).
* @param[in] obj The object handle.
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
{
ObjectList* objEntry;
uint8_t filename[14];
// Check for file system availability
if ( POIS_SDCARD_IsMounted() == 0 )
{
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList*)obj;
// Get filename
objectFilename(objEntry, filename);
// Delete file
DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector);
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Save all settings objects to the SD card.
* @return 0 if success or -1 if failure
@ -804,6 +838,37 @@ int32_t UAVObjLoadSettings()
return 0;
}
/**
* Delete all settings objects from the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDeleteSettings()
{
ObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry)
{
// Check if this is a settings object
if ( objEntry->isSettings )
{
// Save object
if ( UAVObjDelete( (UAVObjHandle)objEntry, 0 ) == -1 )
{
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Save all metaobjects to the SD card.
* @return 0 if success or -1 if failure
@ -866,6 +931,37 @@ int32_t UAVObjLoadMetaobjects()
return 0;
}
/**
* Delete all metaobjects from the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDeleteMetaobjects()
{
ObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry)
{
// Check if this is a settings object
if ( objEntry->isMetaobject )
{
// Load object
if ( UAVObjDelete( (UAVObjHandle)objEntry, 0 ) == -1 )
{
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Set the object data
* \param[in] obj The object handle