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

Added support for event callbacks (as an alternative to task event queues)

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@234 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-03-04 03:04:39 +00:00 committed by vassilis
parent 0310339cb6
commit 94d3c1a3f1
6 changed files with 590 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

222
flight/Modules/Telemetry/telemetry.c
View File

@ -27,7 +27,7 @@
#include "telemetry.h"
#include "uavtalk.h"
#include "uavobjectmanager.h"
#include "utlist.h"
#include "eventdispatcher.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
@ -39,36 +39,22 @@
#define TASK_PRIORITY 100
#define REQ_TIMEOUT_MS 500
#define MAX_RETRIES 2
#define MAX_UPDATE_PERIOD_MS 1000
#define MIN_UPDATE_PERIOD_MS 1
// Private types
/**
* List of object properties that are needed for the periodic updates.
*/
struct ObjectListStruct {
UAVObjHandle obj; /** Object handle */
int32_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */
int32_t timeToNextUpdateMs; /** Time delay to the next update */
struct ObjectListStruct* next; /** Needed by linked list library (utlist.h) */
};
typedef struct ObjectListStruct ObjectList;
// Private variables
xQueueHandle queue;
xTaskHandle telemetryTaskHandle;
ObjectList* objList;
// Private functions
void telemetryTask();
void receiveTask();
void periodicEventHandler(UAVObjEvent* ev);
int32_t transmitData(uint8_t* data, int32_t length);
void registerObject(UAVObjHandle obj);
void updateObject(UAVObjHandle obj);
int32_t addObject(UAVObjHandle obj);
int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
int32_t processPeriodicUpdates();
/**
* Initialize the telemetry module
@ -77,11 +63,8 @@ int32_t processPeriodicUpdates();
*/
int32_t TelemetryInitialize()
{
// Initialize object list
objList = NULL;
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjQMsg));
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// TODO: Get telemetry settings object
@ -93,9 +76,8 @@ int32_t TelemetryInitialize()
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
// Start tasks
// Start telemetry task
xTaskCreate( telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle );
// TODO: Start receive task
return 0;
}
@ -107,7 +89,7 @@ int32_t TelemetryInitialize()
*/
void registerObject(UAVObjHandle obj)
{
// Add object to list
// Setup object for periodic updates
addObject(obj);
// Setup object for telemetry updates
@ -132,43 +114,43 @@ void updateObject(UAVObjHandle obj)
// Set update period
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
// Connect queue
eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ;
eventMask = EV_UPDATED_MANUAL|EV_UPDATE_REQ;
if (UAVObjIsMetaobject(obj))
{
eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnect(obj, queue, eventMask);
UAVObjConnectQueue(obj, queue, eventMask);
}
else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE)
{
// Set update period
setUpdatePeriod(obj, 0);
// Connect queue
eventMask = QMSG_UPDATED|QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ;
eventMask = EV_UPDATED|EV_UPDATED_MANUAL|EV_UPDATE_REQ;
if (UAVObjIsMetaobject(obj))
{
eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnect(obj, queue, eventMask);
UAVObjConnectQueue(obj, queue, eventMask);
}
else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL)
{
// Set update period
setUpdatePeriod(obj, 0);
// Connect queue
eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ;
eventMask = EV_UPDATED_MANUAL|EV_UPDATE_REQ;
if (UAVObjIsMetaobject(obj))
{
eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnect(obj, queue, eventMask);
UAVObjConnectQueue(obj, queue, eventMask);
}
else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER)
{
// Set update period
setUpdatePeriod(obj, 0);
// Disconnect queue
UAVObjDisconnect(obj, queue);
UAVObjDisconnectQueue(obj, queue);
}
}
@ -177,77 +159,50 @@ void updateObject(UAVObjHandle obj)
*/
void telemetryTask()
{
int32_t timeToNextUpdateMs;
int32_t delayMs;
UAVObjQMsg msg;
UAVObjEvent ev;
UAVObjMetadata metadata;
int32_t retries;
int32_t success;
// Initialize time
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS;
// Loop forever
while (1)
{
// Calculate delay time
delayMs = timeToNextUpdateMs-(xTaskGetTickCount()*portTICK_RATE_MS);
if (delayMs < 0)
{
delayMs = 0;
}
// Wait for queue message
if ( xQueueReceive(queue, &msg, delayMs/portTICK_RATE_MS) == pdTRUE )
if ( xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE )
{
// Get object metadata
UAVObjGetMetadata(msg.obj, &metadata);
UAVObjGetMetadata(ev.obj, &metadata);
// Act on event
if (msg.event == QMSG_UPDATED || msg.event == QMSG_UPDATED_MANUAL)
if (ev.event == EV_UPDATED || ev.event == EV_UPDATED_MANUAL)
{
// Send update to GCS (with retries)
retries = 0;
while (retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObject(msg.obj, msg.instId, metadata.ackRequired, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
success = UAVTalkSendObject(ev.obj, ev.instId, metadata.ackRequired, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
}
}
else if (msg.event == QMSG_UPDATE_REQ)
else if (ev.event == EV_UPDATE_REQ)
{
// Request object update from GCS (with retries)
retries = 0;
while (retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObjectRequest(msg.obj, msg.instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
success = UAVTalkSendObjectRequest(ev.obj, ev.instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
}
}
// If this is a metadata object then make necessary telemetry updates
if (UAVObjIsMetaobject(msg.obj))
if (UAVObjIsMetaobject(ev.obj))
{
updateObject(UAVObjGetLinkedObj(msg.obj)); // linked object will be the actual object the metadata are for
updateObject(UAVObjGetLinkedObj(ev.obj)); // linked object will be the actual object the metadata are for
}
}
// Process periodic updates
if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs )
{
timeToNextUpdateMs = processPeriodicUpdates();
}
}
}
/**
* Receive task. Processes received bytes (from the modem or USB) and passes them to
* UAVTalk for decoding. Does not return.
*/
void receiveTask()
{
// Main thread
while (1)
{
// TODO: Wait for bytes and pass to UAVTalk for processing
// TODO: Check for received data (from the modem or USB) and pass them to UAVTalk for decoding
// UAVTalkProcessInputStream(data);
}
}
@ -262,106 +217,47 @@ int32_t transmitData(uint8_t* data, int32_t length)
return 0;
}
/**
* Event handler for periodic object updates (called by the event dispatcher)
*/
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
*/
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
*/
int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
{
ObjectList* objEntry;
// Check that the object is not already connected
LL_FOREACH(objList, objEntry)
{
if (objEntry->obj == obj)
{
objEntry->updatePeriodMs = updatePeriodMs;
objEntry->timeToNextUpdateMs = 0;
return 0;
}
}
// If this point is reached then the object was not found
return -1;
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);
}
/**
* Handle periodic updates for all objects.
* \return The system time until the next update (in ms) or -1 if failed
*/
int32_t processPeriodicUpdates()
{
static int32_t timeOfLastUpdate = 0;
ObjectList* objEntry;
int32_t delaySinceLastUpdateMs;
int32_t minDelay = MAX_UPDATE_PERIOD_MS;
// Iterate through each object and update its timer, if zero then transmit object.
// Also calculate smallest delay to next update.
delaySinceLastUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS - timeOfLastUpdate;
LL_FOREACH(objList, objEntry)
{
// If object is configured for periodic updates
if (objEntry->updatePeriodMs > 0)
{
objEntry->timeToNextUpdateMs -= delaySinceLastUpdateMs;
// Check if time for the next update
if (objEntry->timeToNextUpdateMs <= 0)
{
// Reset timer
objEntry->timeToNextUpdateMs = objEntry->updatePeriodMs;
// Send object (trigger update)
UAVObjUpdated(objEntry->obj);
}
// Update minimum delay
if (objEntry->timeToNextUpdateMs < minDelay)
{
minDelay = objEntry->timeToNextUpdateMs;
}
}
}
// Check if delay for the next update is too short
if (minDelay < MIN_UPDATE_PERIOD_MS)
{
minDelay = MIN_UPDATE_PERIOD_MS;
}
// Done
timeOfLastUpdate = xTaskGetTickCount()*portTICK_RATE_MS;
return timeOfLastUpdate + minDelay;
}
/**
* Add a new object to the object list
*/
int32_t addObject(UAVObjHandle obj)
{
ObjectList* objEntry;
// Check that the object is not already connected
LL_FOREACH(objList, objEntry)
{
if (objEntry->obj == obj)
{
// Already registered, ignore
return -1;
}
}
// Create handle
objEntry = (ObjectList*)malloc(sizeof(ObjectList));
if (objEntry == NULL) return -1;
objEntry->obj = obj;
objEntry->updatePeriodMs = 0;
objEntry->timeToNextUpdateMs = 0;
// Add to list
LL_APPEND(objList, objEntry);
return 0;
}

1
flight/OpenPilot/Makefile
View File

@ -106,6 +106,7 @@ SRC += $(OPSYSTEM)/op_logging.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/uavobjectsinit.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
## UAVOBJECTS

296
flight/OpenPilot/UAVObjects/eventdispatcher.c Normal file
View File

@ -0,0 +1,296 @@
/**
******************************************************************************
*
* @file eventdispatcher.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Event dispatcher, distributes object events as callbacks. Alternative
* to using tasks and queues. All callbacks are invoked from the event task.
* @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 <stdlib.h> // for malloc
#include <string.h> // for memcpy, memcmp
#include "eventdispatcher.h"
#include "utlist.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
// Private constants
#define MAX_QUEUE_SIZE 10
#define STACK_SIZE 100
#define TASK_PRIORITY 100
#define MAX_UPDATE_PERIOD_MS 1000
#define MIN_UPDATE_PERIOD_MS 1
// Private types
/**
* Event callback information
*/
typedef struct {
UAVObjEvent ev; /** The actual event */
UAVObjEventCallback cb; /** The callback function */
} EventCallbackInfo;
/**
* List of object properties that are needed for the periodic updates.
*/
struct PeriodicObjectListStruct {
EventCallbackInfo evInfo; /** Event callback information */
int32_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */
int32_t timeToNextUpdateMs; /** Time delay to the next update */
struct PeriodicObjectListStruct* next; /** Needed by linked list library (utlist.h) */
};
typedef struct PeriodicObjectListStruct PeriodicObjectList;
// Private variables
PeriodicObjectList* objList;
xQueueHandle queue;
xTaskHandle eventTaskHandle;
xSemaphoreHandle mutex;
// Private functions
int32_t processPeriodicUpdates();
void eventTask();
/**
* Initialize the dispatcher
* \return Success (0), failure (-1)
*/
int32_t EventDispatcherInitialize()
{
// Initialize list
objList = NULL;
// Create mutex
mutex = xSemaphoreCreateRecursiveMutex();
if (mutex == NULL)
return -1;
// Create event queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Create task
xTaskCreate( eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &eventTaskHandle );
// Done
return 0;
}
/**
* Dispatch an event by invoking the supplied callback. The function
* returns imidiatelly, the callback is invoked from the event task.
* \param[in] ev The event to be dispatched
* \param[in] cb The callback function
* \return Success (0), failure (-1)
*/
int32_t EventDispatch(UAVObjEvent* ev, UAVObjEventCallback cb)
{
EventCallbackInfo evInfo;
// Initialize event callback information
memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent));
evInfo.cb = cb;
// Push to queue
return xQueueSend(queue, &evInfo, 0); // will not block if queue is full
}
/**
* 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
* \param[in] periodMs The period the event is generated
* \return Success (0), failure (-1)
*/
int32_t EventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
{
PeriodicObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// 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)
{
// Already registered, do nothing
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Create handle
objEntry = (PeriodicObjectList*)malloc(sizeof(PeriodicObjectList));
if (objEntry == NULL) return -1;
memcpy(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent));
objEntry->updatePeriodMs = periodMs;
objEntry->timeToNextUpdateMs = 0;
// Add to list
LL_APPEND(objList, objEntry);
// Release lock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* 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 EventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
{
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, update period
objEntry->updatePeriodMs = periodMs;
objEntry->timeToNextUpdateMs = 0;
// Release lock
xSemaphoreGiveRecursive(mutex);
return 0;
}
}
// If this point is reached the object was not found
xSemaphoreGiveRecursive(mutex);
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);
free(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.
*/
void eventTask()
{
int32_t timeToNextUpdateMs;
int32_t delayMs;
EventCallbackInfo evInfo;
// Initialize time
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS;
// Loop forever
while (1)
{
// Calculate delay time
delayMs = timeToNextUpdateMs-(xTaskGetTickCount()*portTICK_RATE_MS);
if (delayMs < 0)
{
delayMs = 0;
}
// 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
}
// Process periodic updates
if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs )
{
timeToNextUpdateMs = processPeriodicUpdates();
}
}
}
/**
* Handle periodic updates for all objects.
* \return The system time until the next update (in ms) or -1 if failed
*/
int32_t processPeriodicUpdates()
{
static int32_t timeOfLastUpdate = 0;
PeriodicObjectList* objEntry;
int32_t delaySinceLastUpdateMs;
int32_t minDelay = MAX_UPDATE_PERIOD_MS;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Iterate through each object and update its timer, if zero then transmit object.
// Also calculate smallest delay to next update.
delaySinceLastUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS - timeOfLastUpdate;
LL_FOREACH(objList, objEntry)
{
// If object is configured for periodic updates
if (objEntry->updatePeriodMs > 0)
{
objEntry->timeToNextUpdateMs -= delaySinceLastUpdateMs;
// Check if time for the next update
if (objEntry->timeToNextUpdateMs <= 0)
{
// Reset timer
objEntry->timeToNextUpdateMs = objEntry->updatePeriodMs;
// Invoke callback
objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information
}
// Update minimum delay
if (objEntry->timeToNextUpdateMs < minDelay)
{
minDelay = objEntry->timeToNextUpdateMs;
}
}
}
// Check if delay for the next update is too short
if (minDelay < MIN_UPDATE_PERIOD_MS)
{
minDelay = MIN_UPDATE_PERIOD_MS;
}
// Done
timeOfLastUpdate = xTaskGetTickCount()*portTICK_RATE_MS;
xSemaphoreGiveRecursive(mutex);
return timeOfLastUpdate + minDelay;
}

41
flight/OpenPilot/UAVObjects/inc/eventdispatcher.h Normal file
View File

@ -0,0 +1,41 @@
/**
******************************************************************************
*
* @file eventdispatcher.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include files of the uavobjectlist library
* @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 EVENTDISPATCHER_H
#define EVENTDISPATCHER_H
#include <stdint.h>
#include "uavobjectmanager.h"
// Public types
// 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);
#endif // EVENTDISPATCHER_H

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

@ -1,7 +1,7 @@
/**
******************************************************************************
*
* @file uavobjectlist.h
* @file uavobjectmanager.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include files of the uavobjectlist library
* @see The GNU Public License (GPL) Version 3
@ -62,20 +62,33 @@ typedef struct {
* Event types generated by the objects.
*/
typedef enum {
QMSG_UNPACKED = 1, /** Object data updated by unpacking */
QMSG_UPDATED = 2, /** Object data updated by changing the data structure */
QMSG_UPDATED_MANUAL = 4, /** Object update event manually generated */
QMSG_UPDATE_REQ = 8 /** Request to update object data */
} UAVObjQMsgEvent;
EV_UNPACKED = 0x01, /** Object data updated by unpacking */
EV_UPDATED = 0x02, /** Object data updated by changing the data structure */
EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */
EV_UPDATE_REQ = 0x08 /** Request to update object data */
} UAVObjEventType;
/**
* Event message, this structure is send in the event queue each time an event is generated
* Helper macros for event masks
*/
#define EV_MASK_ALL 0
#define EV_MASK_ALL_UPDATES (EV_UNPACKED | EV_UPDATED | EV_UPDATED_MANUAL)
/**
* Event message, this structure is sent in the event queue each time an event is generated
*/
typedef struct {
UAVObjHandle obj;
int32_t instId;
UAVObjQMsgEvent event;
} UAVObjQMsg;
uint16_t instId;
UAVObjEventType event;
} UAVObjEvent;
/**
* Event callback, this function is called when an event is invoked. The function
* will be executed in the event task. The ev parameter should be copied if needed
* after the function returns.
*/
typedef void (*UAVObjEventCallback)(UAVObjEvent* ev);
int32_t UAVObjInitialize();
UAVObjHandle UAVObjRegister(uint32_t id, const char* name, int32_t isMetaobject, int32_t isSingleInstance, uint32_t numBytes);
@ -90,20 +103,22 @@ int32_t UAVObjCreateInstance(UAVObjHandle obj);
int32_t UAVObjIsSingleInstance(UAVObjHandle obj);
int32_t UAVObjIsMetaobject(UAVObjHandle obj);
int32_t UAVObjInitData(UAVObjHandle obj, const char* init);
int32_t UAVObjUnpack(UAVObjHandle obj, uint32_t instId, const uint8_t* dataIn);
int32_t UAVObjPack(UAVObjHandle obj, uint32_t instId, uint8_t* dataOut);
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 UAVObjSetData(UAVObjHandle obj, const void* dataIn);
int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut);
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint32_t instId, const void* dataIn);
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint32_t instId, void* dataOut);
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn);
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut);
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn);
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut);
int32_t UAVObjConnect(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask);
int32_t UAVObjDisconnect(UAVObjHandle obj, xQueueHandle queue);
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask);
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue);
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, int32_t eventMask);
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb);
void UAVObjRequestUpdate(UAVObjHandle obj);
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint32_t instId);
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId);
void UAVObjUpdated(UAVObjHandle obj);
void UAVObjInstanceUpdated(UAVObjHandle obj, uint32_t instId);
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId);
void UAVObjIterate(void (*iterator)(UAVObjHandle obj));
#endif // UAVOBJECTMANAGER_H

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

@ -25,8 +25,9 @@
*/
#include <stdlib.h> // for malloc
#include "string.h" // for strcmp
#include <string.h> // for strcmp
#include "uavobjectmanager.h"
#include "eventdispatcher.h"
#include "utlist.h"
#include "FreeRTOS.h"
#include "queue.h"
@ -41,6 +42,7 @@
*/
struct ObjectQueueListStruct {
xQueueHandle queue;
UAVObjEventCallback cb;
int32_t eventMask;
struct ObjectQueueListStruct* next;
};
@ -51,7 +53,7 @@ typedef struct ObjectQueueListStruct ObjectQueueList;
*/
struct ObjectInstListStruct {
void* data;
uint32_t instId;
uint16_t instId;
struct ObjectInstListStruct* next;
};
typedef struct ObjectInstListStruct ObjectInstList;
@ -62,10 +64,10 @@ typedef struct ObjectInstListStruct ObjectInstList;
struct ObjectListStruct {
uint32_t id; /** The object ID */
const char* name; /** The object name */
int32_t isMetaobject; /** Set to 1 if this is a metaobject */
int32_t isSingleInstance; /** Set to 1 if this object has a single instance */
uint32_t numBytes; /** Number of data bytes contained in the object (for a single instance) */
uint32_t numInstances; /** Number of instances */
int8_t isMetaobject; /** Set to 1 if this is a metaobject */
int8_t isSingleInstance; /** Set to 1 if this object has a single instance */
uint16_t numBytes; /** Number of data bytes contained in the object (for a single instance) */
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 */
union
{
@ -78,11 +80,13 @@ struct ObjectListStruct {
typedef struct ObjectListStruct ObjectList;
// Private functions
int32_t setInstanceData(ObjectList* obj, uint32_t instId, const void* dataIn);
int32_t getInstanceData(ObjectList* obj, uint32_t instId, void* dataOut);
int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event);
int32_t createInstance(ObjectList* obj, uint32_t instId);
int32_t hasInstance(ObjectList* obj, uint32_t instId);
int32_t setInstanceData(ObjectList* obj, uint16_t instId, const void* dataIn);
int32_t getInstanceData(ObjectList* obj, uint16_t instId, void* dataOut);
int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event);
int32_t createInstance(ObjectList* obj, uint16_t instId);
int32_t hasInstance(ObjectList* obj, uint16_t instId);
int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask);
int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb);
// Private variables
ObjectList* objList;
@ -155,8 +159,8 @@ UAVObjHandle UAVObjRegister(uint32_t id, const char* name, int32_t isMetaobject,
}
objEntry->id = id;
objEntry->name = name;
objEntry->isMetaobject = isMetaobject;
objEntry->isSingleInstance = isSingleInstance;
objEntry->isMetaobject = (int8_t)isMetaobject;
objEntry->isSingleInstance = (int8_t)isSingleInstance;
objEntry->numBytes = numBytes;
objEntry->queues = NULL;
if (!isSingleInstance)
@ -370,7 +374,7 @@ int32_t UAVObjInitData(UAVObjHandle obj, const char* init)
* \param[in] dataIn The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjUnpack(UAVObjHandle obj, uint32_t instId, const uint8_t* dataIn)
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn)
{
ObjectList* objEntry;
@ -400,7 +404,7 @@ int32_t UAVObjUnpack(UAVObjHandle obj, uint32_t instId, const uint8_t* dataIn)
}
// Fire event
sendEvent(objEntry, instId, QMSG_UNPACKED);
sendEvent(objEntry, instId, EV_UNPACKED);
// Unlock
xSemaphoreGiveRecursive(mutex);
@ -414,7 +418,7 @@ int32_t UAVObjUnpack(UAVObjHandle obj, uint32_t instId, const uint8_t* dataIn)
* \param[out] dataOut The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjPack(UAVObjHandle obj, uint32_t instId, uint8_t* dataOut)
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut)
{
ObjectList* objEntry;
@ -473,7 +477,7 @@ int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut)
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint32_t instId, const void* dataIn)
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn)
{
ObjectList* objEntry;
@ -497,7 +501,7 @@ int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint32_t instId, const void* dat
}
// Fire event
sendEvent(objEntry, instId, QMSG_UPDATED);
sendEvent(objEntry, instId, EV_UPDATED);
// Unlock
xSemaphoreGiveRecursive(mutex);
@ -511,7 +515,7 @@ int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint32_t instId, const void* dat
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint32_t instId, void* dataOut)
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut)
{
ObjectList* objEntry;
@ -598,82 +602,72 @@ int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut)
}
/**
* Connect an event queue to the object
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
* All events matching the event mask will be pushed to the event queue.
* \param[in] obj The object handle
* \param[in] queue The event queue
* \param[in] eventMask The event mask, if 0 then all events are enabled (e.g. QMSG_UPDATED | QMSG_UPDATED_MANUAL)
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t UAVObjConnect(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask)
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask)
{
ObjectQueueList* queueEntry;
ObjectList* objEntry;
// Lock
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check that the queue is not already connected, if it is simply update event mask
objEntry = (ObjectList*)obj;
LL_FOREACH(objEntry->queues, queueEntry)
{
if (queueEntry->queue == queue)
{
// Already connected, update event mask and return
queueEntry->eventMask = eventMask;
xSemaphoreGiveRecursive(mutex);
return 0;
}
}
// Add queue to list
queueEntry = (ObjectQueueList*)malloc(sizeof(ObjectQueueList));
if (queueEntry == NULL)
{
xSemaphoreGiveRecursive(mutex);
return -1;
}
queueEntry->queue = queue;
queueEntry->eventMask = eventMask;
LL_APPEND(objEntry->queues, queueEntry);
// Done
res = connectObj(obj, queue, 0, eventMask);
xSemaphoreGiveRecursive(mutex);
return 0;
return res;
}
/**
* Disconnect an event queue from the object
* Disconnect an event queue from the object.
* \param[in] obj The object handle
* \param[in] queue The event queue
* \return 0 if success or -1 if failure
*/
int32_t UAVObjDisconnect(UAVObjHandle obj, xQueueHandle queue)
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
{
ObjectQueueList* queueEntry;
ObjectList* objEntry;
// Lock
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Find queue and remove it
objEntry = (ObjectList*)obj;
LL_FOREACH(objEntry->queues, queueEntry)
{
if (queueEntry->queue == queue)
{
LL_DELETE(objEntry->queues, queueEntry);
xSemaphoreGiveRecursive(mutex);
return 0;
}
}
// If this point is reached the queue was not found
res = disconnectObj(obj, queue, 0);
xSemaphoreGiveRecursive(mutex);
return -1;
return res;
}
/**
* Request an update of the object's data from the GCS. The call will not wait for the response, a QMSG_UPDATED event
* Connect an event callback to the object, if the callback is already connected then the event mask is only updated.
* The supplied callback will be invoked on all events matching the event mask.
* \param[in] obj The object handle
* \param[in] cb The event callback
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, int32_t eventMask)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj, 0, cb, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
* Disconnect an event callback from the object.
* \param[in] obj The object handle
* \param[in] cb The event callback
* \return 0 if success or -1 if failure
*/
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj, 0, cb);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
* will be generated as soon as the object is updated.
* \param[in] obj The object handle
*/
@ -683,20 +677,20 @@ void UAVObjRequestUpdate(UAVObjHandle obj)
}
/**
* Request an update of the object's data from the GCS. The call will not wait for the response, a QMSG_UPDATED event
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
* will be generated as soon as the object is updated.
* \param[in] obj The object handle
* \param[in] instId Object instance ID to update
*/
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint32_t instId)
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((ObjectList*)obj, instId, QMSG_UPDATE_REQ);
sendEvent((ObjectList*)obj, instId, EV_UPDATE_REQ);
xSemaphoreGiveRecursive(mutex);
}
/**
* Send the object's data to the GCS (triggers a QMSG_UPDATED_MANUAL event on this object).
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* \param[in] obj The object handle
*/
void UAVObjUpdated(UAVObjHandle obj)
@ -705,14 +699,14 @@ void UAVObjUpdated(UAVObjHandle obj)
}
/**
* Send the object's data to the GCS (triggers a QMSG_UPDATED_MANUAL event on this object).
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* \param[in] obj The object handle
* \param[in] instId The object instance ID
*/
void UAVObjInstanceUpdated(UAVObjHandle obj, uint32_t instId)
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((ObjectList*)obj, instId, QMSG_UPDATED);
sendEvent((ObjectList*)obj, instId, EV_UPDATED);
xSemaphoreGiveRecursive(mutex);
}
@ -741,7 +735,7 @@ void UAVObjIterate(void (*iterator)(UAVObjHandle obj))
/**
* Set the data of a specific object instance.
*/
int32_t setInstanceData(ObjectList* obj, uint32_t instId, const void* dataIn)
int32_t setInstanceData(ObjectList* obj, uint16_t instId, const void* dataIn)
{
ObjectInstList* elemEntry;
@ -770,7 +764,7 @@ int32_t setInstanceData(ObjectList* obj, uint32_t instId, const void* dataIn)
/**
* Get the data of a specific object instance.
*/
int32_t getInstanceData(ObjectList* obj, uint32_t instId, void* dataOut)
int32_t getInstanceData(ObjectList* obj, uint16_t instId, void* dataOut)
{
ObjectInstList* elemEntry;
@ -799,10 +793,10 @@ int32_t getInstanceData(ObjectList* obj, uint32_t instId, void* dataOut)
/**
* Send an event to all event queues registered on the object.
*/
int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event)
int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event)
{
ObjectQueueList* queueEntry;
UAVObjQMsg msg;
UAVObjEvent msg;
// Setup event
msg.obj = (UAVObjHandle)obj;
@ -814,7 +808,16 @@ int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event)
{
if ( queueEntry->eventMask == 0 || (queueEntry->eventMask & event) != 0 )
{
xQueueSend(queueEntry->queue, &msg, 0); // do not wait if queue is full
// Send to queue if a valid queue is registered
if (queueEntry->queue != 0)
{
xQueueSend(queueEntry->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)
{
EventDispatch(&msg, queueEntry->cb); // invoke callback from the event task
}
}
}
@ -825,7 +828,7 @@ int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event)
/**
* Create a new object instance
*/
int32_t createInstance(ObjectList* obj, uint32_t instId)
int32_t createInstance(ObjectList* obj, uint16_t instId)
{
ObjectInstList* elemEntry;
@ -851,7 +854,7 @@ int32_t createInstance(ObjectList* obj, uint32_t instId)
/**
* Check if the object has the instance ID specified.
*/
int32_t hasInstance(ObjectList* obj, uint32_t instId)
int32_t hasInstance(ObjectList* obj, uint16_t instId)
{
ObjectInstList* elemEntry;
@ -875,6 +878,74 @@ int32_t hasInstance(ObjectList* obj, uint32_t instId)
}
}
/**
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
* \param[in] obj The object handle
* \param[in] queue The event queue
* \param[in] cb The event callback
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask)
{
ObjectQueueList* queueEntry;
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)
{
if ( queueEntry->queue == queue && queueEntry->cb == cb )
{
// Already connected, update event mask and return
queueEntry->eventMask = eventMask;
return 0;
}
}
// Add queue to list
queueEntry = (ObjectQueueList*)malloc(sizeof(ObjectQueueList));
if (queueEntry == NULL)
{
return -1;
}
queueEntry->queue = queue;
queueEntry->cb = cb;
queueEntry->eventMask = eventMask;
LL_APPEND(objEntry->queues, queueEntry);
// Done
return 0;
}
/**
* Disconnect an event queue from the object
* \param[in] obj The object handle
* \param[in] queue The event queue
* \param[in] cb The event callback
* \return 0 if success or -1 if failure
*/
int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb)
{
ObjectQueueList* queueEntry;
ObjectList* objEntry;
// Find queue and remove it
objEntry = (ObjectList*)obj;
LL_FOREACH(objEntry->queues, queueEntry)
{
if ( ( queueEntry->queue == queue && queueEntry->cb == cb ) )
{
LL_DELETE(objEntry->queues, queueEntry);
free(queueEntry);
return 0;
}
}
// If this point is reached the queue was not found
return -1;
}