rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Flight/UAVObject: Added support for periodic queue events (in addition to callbacks), removed unused functions in GPS module

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@425 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-04-05 00:59:51 +00:00 committed by vassilis
parent 89749b06e4
commit b00ab0e2ee
6 changed files with 126 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,11 +54,6 @@
// 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);
// functions
char* nmeaGetPacketBuffer(void);
@ -84,7 +79,6 @@ char NmeaPacket[NMEA_BUFFERSIZE];
// Private variables
static COMPortTypeDef gpsPort;
static xQueueHandle queue;
static xTaskHandle gpsTaskHandle;
/**
@ -94,46 +88,18 @@ static xTaskHandle gpsTaskHandle;
*/
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, 1024);
// Process all registered objects and connect queue for updates
//UAVObjIterate(&registerObject);
// Start gps task
xTaskCreate(gpsTask, (signed char*)"GPS", configMINIMAL_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)
{
}
/**
* gps task. Processes input buffer. It does not return.
@ -166,50 +132,6 @@ static void gpsTask(void* parameters)
}
}
/**
* 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);
}
char* nmeaGetPacketBuffer(void)
{
return NmeaPacket;

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

@ -41,7 +41,6 @@ static xTaskHandle telemetryTaskHandle;
// Private functions
static void telemetryTask(void* parameters);
static void periodicEventHandler(UAVObjEvent* ev);
static int32_t transmitData(uint8_t* data, int32_t length);
static void registerObject(UAVObjHandle obj);
static void updateObject(UAVObjHandle obj);
@ -226,15 +225,6 @@ static int32_t transmitData(uint8_t* data, int32_t length)
return PIOS_COM_SendBuffer(OutputPort, data, length);
}
/**
* 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
@ -249,7 +239,7 @@ static int32_t addObject(UAVObjHandle obj)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_MANUAL;
return EventPeriodicCreate(&ev, &periodicEventHandler, 0);
return EventPeriodicQueueCreate(&ev, queue, 0);
}
/**
@ -267,6 +257,6 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_MANUAL;
return EventPeriodicUpdate(&ev, &periodicEventHandler, updatePeriodMs);
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
}

17
flight/OpenPilot/Tests/test_uavobjects.c
View File

@ -128,18 +128,19 @@ static void testTask(void *pvParameters)
ExampleObject1ConnectCallback(eventCallback);
ExampleObject1Set(&data);
// Test periodic callback
UAVObjEvent ev;
ev.event = 0;
ev.instId = 0;
ev.obj = ExampleObject1Handle();
EventPeriodicCreate(&ev, eventCallbackPeriodic, 500);
// Test queue events
xQueueHandle queue;
queue = xQueueCreate(10, sizeof(UAVObjEvent));
ExampleObject1ConnectQueue(queue);
// Test periodic events
UAVObjEvent ev;
ev.event = 0;
ev.instId = 0;
ev.obj = ExampleObject1Handle();
EventPeriodicCallbackCreate(&ev, eventCallbackPeriodic, 500);
EventPeriodicQueueCreate(&ev, queue, 250);
// Done testing
while (1)
{
@ -164,7 +165,7 @@ static void eventCallbackPeriodic(UAVObjEvent* ev)
lastUpdate = xTaskGetTickCount();
const char* name = UAVObjGetName(ev->obj);
PIOS_LED_Toggle(LED1);
ExampleObject1Updated();
//ExampleObject1Updated();
}
void vApplicationIdleHook(void)

123
flight/OpenPilot/UAVObjects/eventdispatcher.c
View File

@ -40,7 +40,8 @@
*/
typedef struct {
UAVObjEvent ev; /** The actual event */
UAVObjEventCallback cb; /** The callback function */
UAVObjEventCallback cb; /** The callback function, or zero if none */
xQueueHandle queue; /** The queue or zero if none */
} EventCallbackInfo;
/**
@ -63,6 +64,9 @@ static xSemaphoreHandle mutex;
// Private functions
static int32_t processPeriodicUpdates();
static void eventTask();
static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs);
static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs);
/**
* Initialize the dispatcher
@ -95,24 +99,74 @@ int32_t EventDispatcherInitialize()
* \param[in] cb The callback function
* \return Success (0), failure (-1)
*/
int32_t EventDispatch(UAVObjEvent* ev, UAVObjEventCallback cb)
int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb)
{
EventCallbackInfo evInfo;
// Initialize event callback information
memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent));
evInfo.cb = cb;
evInfo.queue = 0;
// Push to queue
return xQueueSend(queue, &evInfo, 0); // will not block if queue is full
}
/**
* Dispatch an event through a callback at periodic intervals.
* Dispatch an event at periodic intervals.
* \param[in] ev The event to be dispatched
* \param[in] cb The callback to be invoked
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
{
return eventPeriodicCreate(ev, cb, 0, periodMs);
}
/**
* Update the period of a periodic event.
* \param[in] ev The event to be dispatched
* \param[in] cb The callback to be invoked
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
{
return eventPeriodicUpdate(ev, cb, 0, periodMs);
}
/**
* Dispatch an event at periodic intervals.
* \param[in] ev The event to be dispatched
* \param[in] queue The queue that the event will be pushed in
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs)
{
return eventPeriodicCreate(ev, 0, queue, periodMs);
}
/**
* Update the period of a periodic event.
* \param[in] ev The event to be dispatched
* \param[in] queue The queue
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs)
{
return eventPeriodicUpdate(ev, 0, queue, periodMs);
}
/**
* Dispatch an event through a callback at periodic intervals.
* \param[in] ev The event to be dispatched
* \param[in] cb The callback to be invoked or zero if none
* \param[in] queue The queue or zero if none
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs)
{
PeriodicObjectList* objEntry;
// Get lock
@ -120,7 +174,9 @@ int32_t EventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t per
// Check that the object is not already connected
LL_FOREACH(objList, objEntry)
{
if (objEntry->evInfo.cb == cb && memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
if (objEntry->evInfo.cb == cb &&
objEntry->evInfo.queue == queue &&
memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
{
// Already registered, do nothing
xSemaphoreGiveRecursive(mutex);
@ -132,6 +188,7 @@ int32_t EventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t per
if (objEntry == NULL) return -1;
memcpy(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent));
objEntry->evInfo.cb = cb;
objEntry->evInfo.queue = queue;
objEntry->updatePeriodMs = periodMs;
objEntry->timeToNextUpdateMs = 0;
// Add to list
@ -144,11 +201,12 @@ int32_t EventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t per
/**
* Update the period of a periodic event.
* \param[in] ev The event to be dispatched
* \param[in] cb The callback to be invoked
* \param[in] cb The callback to be invoked or zero if none
* \param[in] queue The queue or zero if none
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs)
{
PeriodicObjectList* objEntry;
// Get lock
@ -156,7 +214,9 @@ int32_t EventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t per
// Find object
LL_FOREACH(objList, objEntry)
{
if (objEntry->evInfo.cb == cb && memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
if (objEntry->evInfo.cb == cb &&
objEntry->evInfo.queue == queue &&
memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
{
// Object found, update period
objEntry->updatePeriodMs = periodMs;
@ -171,34 +231,6 @@ int32_t EventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t per
return -1;
}
/**
* Delete periodic event.
* \param[in] ev The event to be dispatched
* \param[in] cb The callback to be invoked
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicDelete(UAVObjEvent* ev, UAVObjEventCallback cb)
{
PeriodicObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Find object
LL_FOREACH(objList, objEntry)
{
if (objEntry->evInfo.cb == cb && memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
{
// Object found, remove from list
LL_DELETE(objList, objEntry);
vPortFree(objEntry);
xSemaphoreGiveRecursive(mutex);
return 0;
}
}
// If this point is reached the object was not found
xSemaphoreGiveRecursive(mutex);
return -1;
}
/**
* Event task, responsible of invoking callbacks.
*/
@ -224,8 +256,11 @@ static void eventTask()
// Wait for queue message
if ( xQueueReceive(queue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE )
{
// Invoke callback
evInfo.cb(&evInfo.ev); // the function is expected to copy the event information
// Invoke callback, if one
if ( evInfo.cb != 0)
{
evInfo.cb(&evInfo.ev); // the function is expected to copy the event information
}
}
// Process periodic updates
@ -263,8 +298,16 @@ static int32_t processPeriodicUpdates()
{
// Reset timer
objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs;
// Invoke callback
objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information
// Invoke callback, if one
if ( objEntry->evInfo.cb != 0)
{
objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information
}
// Push event to queue, if one
if ( objEntry->evInfo.queue != 0)
{
xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0); // do not block if queue is full
}
}
// Update minimum delay
if (objEntry->timeToNextUpdateMs < timeToNextUpdate)

9
flight/OpenPilot/UAVObjects/inc/eventdispatcher.h
View File

@ -30,9 +30,10 @@
// Public functions
int32_t EventDispatcherInitialize();
int32_t EventDispatch(UAVObjEvent* ev, UAVObjEventCallback cb);
int32_t EventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs);
int32_t EventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs);
int32_t EventPeriodicDelete(UAVObjEvent* ev, UAVObjEventCallback cb);
int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb);
int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs);
int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs);
int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs);
int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs);
#endif // EVENTDISPATCHER_H

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

@ -33,13 +33,13 @@
/**
* List of event queues and the eventmask associated with the queue.
*/
struct ObjectQueueListStruct {
struct ObjectEventListStruct {
xQueueHandle queue;
UAVObjEventCallback cb;
int32_t eventMask;
struct ObjectQueueListStruct* next;
struct ObjectEventListStruct* next;
};
typedef struct ObjectQueueListStruct ObjectQueueList;
typedef struct ObjectEventListStruct ObjectEventList;
/**
* List of object instances, holds the actual data structure and instance ID
@ -64,7 +64,7 @@ struct ObjectListStruct {
uint16_t numInstances; /** Number of instances */
struct ObjectListStruct* linkedObj; /** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */
ObjectInstList* instances; /** List of object instances, instance 0 always exists */
ObjectQueueList* queues; /** Event queues registered on the object */
ObjectEventList* events; /** Event queues registered on the object */
struct ObjectListStruct* next; /** Needed by linked list library (utlist.h) */
};
typedef struct ObjectListStruct ObjectList;
@ -155,7 +155,7 @@ UAVObjHandle UAVObjRegister(uint32_t id, const char* name, int32_t isMetaobject,
objEntry->isSingleInstance = (int8_t)isSingleInstance;
objEntry->isSettings = (int8_t)isSettings;
objEntry->numBytes = numBytes;
objEntry->queues = NULL;
objEntry->events = NULL;
objEntry->numInstances = 0;
objEntry->instances = NULL;
objEntry->linkedObj = NULL; // will be set later
@ -953,7 +953,7 @@ void UAVObjIterate(void (*iterator)(UAVObjHandle obj))
*/
static int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event)
{
ObjectQueueList* queueEntry;
ObjectEventList* eventEntry;
UAVObjEvent msg;
// Setup event
@ -962,19 +962,19 @@ static int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event
msg.instId = instId;
// Go through each object and push the event message in the queue (if event is activated for the queue)
LL_FOREACH(obj->queues, queueEntry)
LL_FOREACH(obj->events, eventEntry)
{
if ( queueEntry->eventMask == 0 || (queueEntry->eventMask & event) != 0 )
if ( eventEntry->eventMask == 0 || (eventEntry->eventMask & event) != 0 )
{
// Send to queue if a valid queue is registered
if (queueEntry->queue != 0)
if (eventEntry->queue != 0)
{
xQueueSend(queueEntry->queue, &msg, 0); // do not wait if queue is full
xQueueSend(eventEntry->queue, &msg, 0); // do not wait if queue is full
}
// Invoke callback (from event task) if a valid one is registered
if (queueEntry->cb != 0)
if (eventEntry->cb != 0)
{
EventDispatch(&msg, queueEntry->cb); // invoke callback from the event task
EventCallbackDispatch(&msg, eventEntry->cb); // invoke callback from the event task
}
}
}
@ -1064,31 +1064,31 @@ static ObjectInstList* getInstance(ObjectList* obj, uint16_t instId)
*/
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask)
{
ObjectQueueList* queueEntry;
ObjectEventList* eventEntry;
ObjectList* objEntry;
// Check that the queue is not already connected, if it is simply update event mask
objEntry = (ObjectList*)obj;
LL_FOREACH(objEntry->queues, queueEntry)
LL_FOREACH(objEntry->events, eventEntry)
{
if ( queueEntry->queue == queue && queueEntry->cb == cb )
if ( eventEntry->queue == queue && eventEntry->cb == cb )
{
// Already connected, update event mask and return
queueEntry->eventMask = eventMask;
eventEntry->eventMask = eventMask;
return 0;
}
}
// Add queue to list
queueEntry = (ObjectQueueList*)pvPortMalloc(sizeof(ObjectQueueList));
if (queueEntry == NULL)
eventEntry = (ObjectEventList*)pvPortMalloc(sizeof(ObjectEventList));
if (eventEntry == NULL)
{
return -1;
}
queueEntry->queue = queue;
queueEntry->cb = cb;
queueEntry->eventMask = eventMask;
LL_APPEND(objEntry->queues, queueEntry);
eventEntry->queue = queue;
eventEntry->cb = cb;
eventEntry->eventMask = eventMask;
LL_APPEND(objEntry->events, eventEntry);
// Done
return 0;
@ -1103,17 +1103,17 @@ static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallb
*/
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb)
{
ObjectQueueList* queueEntry;
ObjectEventList* eventEntry;
ObjectList* objEntry;
// Find queue and remove it
objEntry = (ObjectList*)obj;
LL_FOREACH(objEntry->queues, queueEntry)
LL_FOREACH(objEntry->events, eventEntry)
{
if ( ( queueEntry->queue == queue && queueEntry->cb == cb ) )
if ( ( eventEntry->queue == queue && eventEntry->cb == cb ) )
{
LL_DELETE(objEntry->queues, queueEntry);
vPortFree(queueEntry);
LL_DELETE(objEntry->events, eventEntry);
vPortFree(eventEntry);
return 0;
}
}