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

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@234 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/Modules/Telemetry/telemetry.c

@@ -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;
+	UAVObjEvent ev;
-	int32_t delayMs;
-	UAVObjQMsg msg;
 	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
+		// TODO: Check for received data (from the modem or USB) and pass them to UAVTalk for decoding
-		if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs )
+		// UAVTalkProcessInputStream(data);
-		{
-			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
 	}
 }
 
@@ -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;
+	UAVObjEvent ev;
-	// Check that the object is not already connected
+
-	LL_FOREACH(objList, objEntry)
+	// Add object for periodic updates
-	{
+	ev.obj = obj;
-		if (objEntry->obj == obj)
+	ev.instId = UAVOBJ_ALL_INSTANCES;
-		{
+	ev.event = EV_UPDATED_MANUAL;
-			objEntry->updatePeriodMs = updatePeriodMs;
+	return EventPeriodicUpdate(&ev, &periodicEventHandler, updatePeriodMs);
-			objEntry->timeToNextUpdateMs = 0;
-			return 0;
-		}
-	}
-	// If this point is reached then the object was not found
-	return -1;
 }
 
-/**
- * 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;
-}
-
-
-
-
-
-
-

flight/OpenPilot/Makefile

@@ -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
 

flight/OpenPilot/UAVObjects/eventdispatcher.c

@@ -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;
+}
+

flight/OpenPilot/UAVObjects/inc/eventdispatcher.h

@@ -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

flight/OpenPilot/UAVObjects/inc/uavobjectmanager.h

@@ -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 */
+	EV_UNPACKED = 0x01, /** Object data updated by unpacking */
-    QMSG_UPDATED = 2, /** Object data updated by changing the data structure */
+    EV_UPDATED = 0x02, /** Object data updated by changing the data structure */
-    QMSG_UPDATED_MANUAL = 4, /** Object update event manually generated */
+    EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */
-    QMSG_UPDATE_REQ = 8 /** Request to update object data */
+    EV_UPDATE_REQ = 0x08 /** Request to update object data */
-} UAVObjQMsgEvent;
+} 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;
+	uint16_t instId;
-	UAVObjQMsgEvent event;
+	UAVObjEventType event;
-} UAVObjQMsg;
+} 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 UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn);
-int32_t UAVObjPack(UAVObjHandle obj, uint32_t instId, uint8_t* dataOut);
+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 UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn);
-int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint32_t instId, void* dataOut);
+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 UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask);
-int32_t UAVObjDisconnect(UAVObjHandle obj, xQueueHandle queue);
+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

flight/OpenPilot/UAVObjects/uavobjectmanager.c

@@ -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 */
+	int8_t isMetaobject; /** Set to 1 if this is a metaobject */
-	int32_t isSingleInstance; /** Set to 1 if this object has a single instance */
+	int8_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) */
+	uint16_t numBytes; /** Number of data bytes contained in the object (for a single instance) */
-	uint32_t numInstances; /** Number of instances */
+	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 setInstanceData(ObjectList* obj, uint16_t instId, const void* dataIn);
-int32_t getInstanceData(ObjectList* obj, uint32_t instId, void* dataOut);
+int32_t getInstanceData(ObjectList* obj, uint16_t instId, void* dataOut);
-int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event);
+int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event);
-int32_t createInstance(ObjectList* obj, uint32_t instId);
+int32_t createInstance(ObjectList* obj, uint16_t instId);
-int32_t hasInstance(ObjectList* obj, uint32_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->isMetaobject = (int8_t)isMetaobject;
-	objEntry->isSingleInstance = isSingleInstance;
+	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;
+	int32_t res;
-	ObjectList* objEntry;
-
-	// Lock
 	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-
+	res = connectObj(obj, queue, 0, eventMask);
-	// 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
 	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;
+	int32_t res;
-	ObjectList* objEntry;
-
-	// Lock
 	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-
+	res = disconnectObj(obj, queue, 0);
-	// 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
 	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;
+}
+