flight/uavobjects/eventdispatcher.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       eventdispatcher.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      Event dispatcher, distributes object events as callbacks. Alternative
   7  *             to using tasks and queues. All callbacks are invoked from the event task.
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 #include <openpilot.h>
  28
  29 #include <callbackinfo.h>
  30
  31 // Private constants
  32 #if defined(PIOS_EVENTDISAPTCHER_QUEUE)
  33 #define MAX_QUEUE_SIZE       PIOS_EVENTDISAPTCHER_QUEUE
  34 #else
  35 #define MAX_QUEUE_SIZE       20
  36 #endif
  37
  38 #if defined(PIOS_EVENTDISPATCHER_STACK_SIZE)
  39 #define STACK_SIZE           PIOS_EVENTDISPATCHER_STACK_SIZE
  40 #else
  41 #define STACK_SIZE           configMINIMAL_STACK_SIZE
  42 #endif /* PIOS_EVENTDISPATCHER_STACK_SIZE */
  43
  44 #define CALLBACK_PRIORITY    CALLBACK_PRIORITY_CRITICAL
  45 #define TASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
  46 #define MAX_UPDATE_PERIOD_MS 1000
  47
  48 // Private types
  49
  50
  51 /**
  52  * Event callback information
  53  */
  54 typedef struct {
  55     UAVObjEvent  ev; /** The actual event */
  56     UAVObjEventCallback cb; /** The callback function, or zero if none */
  57     xQueueHandle queue; /** The queue or zero if none */
  58     bool lowpriority; /** set to true for telemetry and other low priority stuffs, prevent raising warning */
  59 } EventCallbackInfo;
  60
  61 /**
  62  * List of object properties that are needed for the periodic updates.
  63  */
  64 struct PeriodicObjectListStruct {
  65     EventCallbackInfo evInfo; /** Event callback information */
  66     uint16_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */
  67     int32_t  timeToNextUpdateMs; /** Time delay to the next update */
  68     struct PeriodicObjectListStruct *next; /** Needed by linked list library (utlist.h) */
  69 };
  70 typedef struct PeriodicObjectListStruct PeriodicObjectList;
  71
  72 // Private variables
  73 static PeriodicObjectList *mObjList;
  74 static xQueueHandle mQueue;
  75 static DelayedCallbackInfo *eventSchedulerCallback;
  76 static xSemaphoreHandle mMutex;
  77 static EventStats mStats;
  78
  79 // Private functions
  80 static int32_t processPeriodicUpdates();
  81 static void eventTask();
  82 static int32_t eventPeriodicCreate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs);
  83 static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs);
  84 static uint16_t randomizePeriod(uint16_t periodMs);
  85
  86
  87 /**
  88  * Initialize the dispatcher
  89  * \return Success (0), failure (-1)
  90  */
  91 int32_t EventDispatcherInitialize()
  92 {
  93     // Initialize variables
  94     mObjList = NULL;
  95     memset(&mStats, 0, sizeof(EventStats));
  96
  97     // Create mMutex
  98     mMutex = xSemaphoreCreateRecursiveMutex();
  99     if (mMutex == NULL) {
 100         return -1;
 101     }
 102
 103     // Create event queue
 104     mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
 105
 106     // Create callback
 107     eventSchedulerCallback = PIOS_CALLBACKSCHEDULER_Create(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_EVENTDISPATCHER, STACK_SIZE * 4);
 108     PIOS_CALLBACKSCHEDULER_Dispatch(eventSchedulerCallback);
 109
 110     // Done
 111     return 0;
 112 }
 113
 114 /**
 115  * Get the statistics counters
 116  * @param[out] statsOut The statistics counters will be copied there
 117  */
 118 void EventGetStats(EventStats *statsOut)
 119 {
 120     xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
 121     memcpy(statsOut, &mStats, sizeof(EventStats));
 122     xSemaphoreGiveRecursive(mMutex);
 123 }
 124
 125 /**
 126  * Clear the statistics counters
 127  */
 128 void EventClearStats()
 129 {
 130     xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
 131     memset(&mStats, 0, sizeof(EventStats));
 132     xSemaphoreGiveRecursive(mMutex);
 133 }
 134
 135 /**
 136  * Dispatch an event by invoking the supplied callback. The function
 137  * returns imidiatelly, the callback is invoked from the event task.
 138  * \param[in] ev The event to be dispatched
 139  * \param[in] cb The callback function
 140  * \return Success (0), failure (-1)
 141  */
 142 int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb)
 143 {
 144     EventCallbackInfo evInfo;
 145
 146     // Initialize event callback information
 147     memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent));
 148     evInfo.cb    = cb;
 149     evInfo.queue = 0;
 150     // Push to queue
 151     int32_t result = xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full
 152     PIOS_CALLBACKSCHEDULER_Dispatch(eventSchedulerCallback);
 153     return result;
 154 }
 155
 156 /**
 157  * Dispatch an event at periodic intervals.
 158  * \param[in] ev The event to be dispatched
 159  * \param[in] cb The callback to be invoked
 160  * \param[in] periodMs The period the event is generated
 161  * \return Success (0), failure (-1)
 162  */
 163 int32_t EventPeriodicCallbackCreate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs)
 164 {
 165     return eventPeriodicCreate(ev, cb, 0, periodMs);
 166 }
 167
 168 /**
 169  * Update the period of a periodic event.
 170  * \param[in] ev The event to be dispatched
 171  * \param[in] cb The callback to be invoked
 172  * \param[in] periodMs The period the event is generated
 173  * \return Success (0), failure (-1)
 174  */
 175 int32_t EventPeriodicCallbackUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs)
 176 {
 177     return eventPeriodicUpdate(ev, cb, 0, periodMs);
 178 }
 179
 180 /**
 181  * Dispatch an event at periodic intervals.
 182  * \param[in] ev The event to be dispatched
 183  * \param[in] queue The queue that the event will be pushed in
 184  * \param[in] periodMs The period the event is generated
 185  * \return Success (0), failure (-1)
 186  */
 187 int32_t EventPeriodicQueueCreate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs)
 188 {
 189     return eventPeriodicCreate(ev, 0, queue, periodMs);
 190 }
 191
 192 /**
 193  * Update the period of a periodic event.
 194  * \param[in] ev The event to be dispatched
 195  * \param[in] queue The queue
 196  * \param[in] periodMs The period the event is generated
 197  * \return Success (0), failure (-1)
 198  */
 199 int32_t EventPeriodicQueueUpdate(UAVObjEvent *ev, xQueueHandle queue, uint16_t periodMs)
 200 {
 201     return eventPeriodicUpdate(ev, 0, queue, periodMs);
 202 }
 203
 204 /**
 205  * Dispatch an event through a callback at periodic intervals.
 206  * \param[in] ev The event to be dispatched
 207  * \param[in] cb The callback to be invoked or zero if none
 208  * \param[in] queue The queue or zero if none
 209  * \param[in] periodMs The period the event is generated
 210  * \return Success (0), failure (-1)
 211  */
 212 static int32_t eventPeriodicCreate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs)
 213 {
 214     PeriodicObjectList *objEntry;
 215
 216     // Get lock
 217     xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
 218     // Check that the object is not already connected
 219     LL_FOREACH(mObjList, objEntry) {
 220         if (objEntry->evInfo.cb == cb &&
 221             objEntry->evInfo.queue == queue &&
 222             objEntry->evInfo.ev.obj == ev->obj &&
 223             objEntry->evInfo.ev.instId == ev->instId &&
 224             objEntry->evInfo.ev.event == ev->event) {
 225             // Already registered, do nothing
 226             xSemaphoreGiveRecursive(mMutex);
 227             return -1;
 228         }
 229     }
 230     // Create handle
 231     objEntry = (PeriodicObjectList *)pios_malloc(sizeof(PeriodicObjectList));
 232     if (objEntry == NULL) {
 233         return -1;
 234     }
 235     objEntry->evInfo.ev.obj      = ev->obj;
 236     objEntry->evInfo.ev.instId   = ev->instId;
 237     objEntry->evInfo.ev.event    = ev->event;
 238     objEntry->evInfo.cb = cb;
 239     objEntry->evInfo.queue       = queue;
 240     objEntry->updatePeriodMs     = periodMs;
 241     objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates
 242     // Add to list
 243     LL_APPEND(mObjList, objEntry);
 244     // Release lock
 245     xSemaphoreGiveRecursive(mMutex);
 246     return 0;
 247 }
 248
 249 /**
 250  * Update the period of a periodic event.
 251  * \param[in] ev The event to be dispatched
 252  * \param[in] cb The callback to be invoked or zero if none
 253  * \param[in] queue The queue or zero if none
 254  * \param[in] periodMs The period the event is generated
 255  * \return Success (0), failure (-1)
 256  */
 257 static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs)
 258 {
 259     PeriodicObjectList *objEntry;
 260
 261     // Get lock
 262     xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
 263     // Find object
 264     LL_FOREACH(mObjList, objEntry) {
 265         if (objEntry->evInfo.cb == cb &&
 266             objEntry->evInfo.queue == queue &&
 267             objEntry->evInfo.ev.obj == ev->obj &&
 268             objEntry->evInfo.ev.instId == ev->instId &&
 269             objEntry->evInfo.ev.event == ev->event) {
 270             // Object found, update period
 271             objEntry->updatePeriodMs     = periodMs;
 272             objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates
 273             // Release lock
 274             xSemaphoreGiveRecursive(mMutex);
 275             return 0;
 276         }
 277     }
 278     // If this point is reached the object was not found
 279     xSemaphoreGiveRecursive(mMutex);
 280     return -1;
 281 }
 282
 283 /**
 284  * Delayed event callback, responsible of invoking (event) callbacks.
 285  */
 286 static void eventTask()
 287 {
 288     static uint32_t timeToNextUpdateMs = 0;
 289     EventCallbackInfo evInfo;
 290
 291     // Wait for queue message
 292     int limit = MAX_QUEUE_SIZE;
 293
 294     while (xQueueReceive(mQueue, &evInfo, 0) == pdTRUE) {
 295         // Invoke callback, if any
 296         if (evInfo.cb != 0) {
 297             evInfo.cb(&evInfo.ev); // the function is expected to copy the event information
 298         }
 299         // limit loop to max queue size to slightly reduce the impact of recursive events
 300         if (!--limit) {
 301             break;
 302         }
 303     }
 304
 305     // Process periodic updates
 306     if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) {
 307         timeToNextUpdateMs = processPeriodicUpdates();
 308     }
 309
 310     PIOS_CALLBACKSCHEDULER_Schedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER);
 311 }
 312
 313 /**
 314  * Handle periodic updates for all objects.
 315  * \return The system time until the next update (in ms) or -1 if failed
 316  */
 317 static int32_t processPeriodicUpdates()
 318 {
 319     PeriodicObjectList *objEntry;
 320     int32_t timeNow;
 321     int32_t timeToNextUpdate;
 322     int32_t offset;
 323
 324     // Get lock
 325     xSemaphoreTakeRecursive(mMutex, portMAX_DELAY);
 326
 327     // Iterate through each object and update its timer, if zero then transmit object.
 328     // Also calculate smallest delay to next update.
 329     timeToNextUpdate = xTaskGetTickCount() * portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS;
 330     LL_FOREACH(mObjList, objEntry) {
 331         // If object is configured for periodic updates
 332         if (objEntry->updatePeriodMs > 0) {
 333             // Check if time for the next update
 334             timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
 335             if (objEntry->timeToNextUpdateMs <= timeNow) {
 336                 // Reset timer
 337                 offset = (timeNow - objEntry->timeToNextUpdateMs) % objEntry->updatePeriodMs;
 338                 objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset;
 339                 // Invoke callback, if one
 340                 if (objEntry->evInfo.cb != 0) {
 341                     objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information
 342                 }
 343                 // Push event to queue, if one
 344                 if (objEntry->evInfo.queue != 0) {
 345                     if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE && !objEntry->evInfo.ev.lowPriority) { // do not block if queue is full
 346                         if (objEntry->evInfo.ev.obj != NULL) {
 347                             mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj);
 348                         }
 349                         ++mStats.eventErrors;
 350                     }
 351                 }
 352             }
 353             // Update minimum delay
 354             if (objEntry->timeToNextUpdateMs < timeToNextUpdate) {
 355                 timeToNextUpdate = objEntry->timeToNextUpdateMs;
 356             }
 357         }
 358     }
 359
 360     // Done
 361     xSemaphoreGiveRecursive(mMutex);
 362     return timeToNextUpdate;
 363 }
 364
 365 /**
 366  * Return a psedorandom integer from 0 to periodMs
 367  * Based on the Park-Miller-Carta Pseudo-Random Number Generator
 368  * http://www.firstpr.com.au/dsp/rand31/
 369  */
 370 static uint16_t randomizePeriod(uint16_t periodMs)
 371 {
 372     static uint32_t seed = 1;
 373     uint32_t hi, lo;
 374
 375     lo  = 16807 * (seed & 0xFFFF);
 376     hi  = 16807 * (seed >> 16);
 377     lo += (hi & 0x7FFF) << 16;
 378     lo += hi >> 15;
 379     if (lo > 0x7FFFFFFF) {
 380         lo -= 0x7FFFFFFF;
 381     }
 382     seed = lo;
 383     return (uint16_t)(((float)periodMs * (float)lo) / (float)0x7FFFFFFF);
 384 }