1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/OpenPilot/UAVObjects/eventdispatcher.c
dankers e66c491942 Update to headers in the flight portion of the code. Just formatting.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@276 ebee16cc-31ac-478f-84a7-5cbb03baadba
2010-03-10 09:12:31 +00:00

290 lines
8.5 KiB
C

/**
******************************************************************************
*
* @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 "openpilot.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;
}