diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c index 0bf8e9474..f2a29c23f 100644 --- a/flight/OpenPilot/Modules/GPS/GPS.c +++ b/flight/OpenPilot/Modules/GPS/GPS.c @@ -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(®isterObject); - // 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; diff --git a/flight/OpenPilot/Modules/Telemetry/telemetry.c b/flight/OpenPilot/Modules/Telemetry/telemetry.c index 3036676fd..9b106d056 100644 --- a/flight/OpenPilot/Modules/Telemetry/telemetry.c +++ b/flight/OpenPilot/Modules/Telemetry/telemetry.c @@ -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); } diff --git a/flight/OpenPilot/Tests/test_uavobjects.c b/flight/OpenPilot/Tests/test_uavobjects.c index 35f390396..fffbc3f25 100644 --- a/flight/OpenPilot/Tests/test_uavobjects.c +++ b/flight/OpenPilot/Tests/test_uavobjects.c @@ -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) diff --git a/flight/OpenPilot/UAVObjects/eventdispatcher.c b/flight/OpenPilot/UAVObjects/eventdispatcher.c index 00df283a1..cff9e8bde 100644 --- a/flight/OpenPilot/UAVObjects/eventdispatcher.c +++ b/flight/OpenPilot/UAVObjects/eventdispatcher.c @@ -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) diff --git a/flight/OpenPilot/UAVObjects/inc/eventdispatcher.h b/flight/OpenPilot/UAVObjects/inc/eventdispatcher.h index 7de4173f0..af3f384ed 100644 --- a/flight/OpenPilot/UAVObjects/inc/eventdispatcher.h +++ b/flight/OpenPilot/UAVObjects/inc/eventdispatcher.h @@ -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 diff --git a/flight/OpenPilot/UAVObjects/uavobjectmanager.c b/flight/OpenPilot/UAVObjects/uavobjectmanager.c index 2b8153770..b7eb08aec 100644 --- a/flight/OpenPilot/UAVObjects/uavobjectmanager.c +++ b/flight/OpenPilot/UAVObjects/uavobjectmanager.c @@ -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; } }