LibrePilot/flight/UAVObjects/uavobjectmanager.c

/**
 ******************************************************************************
 * @addtogroup UAVObjects OpenPilot UAVObjects
 * @{ 
 * @addtogroup UAV Object Manager 
 * @brief The core UAV Objects functions, most of which are wrappered by
 * autogenerated defines
 * @{ 
 *
 *
 * @file       uavobjectmanager.h
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Object manager library. This library holds a collection of all objects.
 *             It can be used by all modules/libraries to find an object reference.
 * @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"

// Constants

// Private types

// Macros
#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);
#define OLGetIsMetaobject(olp)  ((olp)->flags & OL_IS_METAOBJECT)
#define OLSetIsMetaobject(olp, val)  ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_METAOBJECT) : ((olp)->flags | OL_IS_METAOBJECT)))
#define OLGetIsSingleInstance(olp)  ((olp)->flags & OL_IS_SINGLE_INSTANCE)
#define OLSetIsSingleInstance(olp, val)  ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SINGLE_INSTANCE) : ((olp)->flags | OL_IS_SINGLE_INSTANCE)))
#define OLGetIsSettings(olp)  ((olp)->flags & OL_IS_SETTINGS)
#define OLSetIsSettings(olp, val)  ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SETTINGS) : ((olp)->flags | OL_IS_SETTINGS)))

/**
 * List of event queues and the eventmask associated with the queue.
 */
struct ObjectEventListStruct {
	  xQueueHandle queue;
	  UAVObjEventCallback cb;
	  uint8_t eventMask;
	  struct ObjectEventListStruct *next;
};
typedef struct ObjectEventListStruct ObjectEventList;

/**
 * List of object instances, holds the actual data structure and instance ID
 */
struct ObjectInstListStruct {
	  void *data;
	  uint16_t instId;
	  struct ObjectInstListStruct *next;
};
typedef struct ObjectInstListStruct ObjectInstList;

typedef enum {
	OL_IS_METAOBJECT = 0x01, /** Set if this is a metaobject */
	OL_IS_SINGLE_INSTANCE = 0x02, /** Set if this object has a single instance */
	OL_IS_SETTINGS = 0x04 /** Set if this object is a settings object */
} ObjectListFlags;
	
/**
 * List of objects registered in the object manager
 */
struct ObjectListStruct {
	  uint32_t id;
		     /** The object ID */
	  const char *name;
			  /** The object name */
	  ObjectListFlags flags;
                               /** The object list mode flags */
	  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 */
	  ObjectInstList instances;
				  /** List of object instances, instance 0 always exists */
	  ObjectEventList *events;
				 /** Event queues registered on the object */
	  struct ObjectListStruct *next;
				       /** Needed by linked list library (utlist.h) */
};
typedef struct ObjectListStruct ObjectList;

// Private functions
static int32_t sendEvent(ObjectList * obj, uint16_t instId,
			 UAVObjEventType event);
static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId);
static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId);
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
			  UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
			     UAVObjEventCallback cb);

#if defined(PIOS_INCLUDE_SDCARD)
static void objectFilename(ObjectList * obj, uint8_t * filename);
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
#endif

// Private variables
static ObjectList *objList;
static xSemaphoreHandle mutex;
static UAVObjMetadata defMetadata;
static UAVObjStats stats;

/**
 * Initialize the object manager
 * \return 0 Success
 * \return -1 Failure
 */
int32_t UAVObjInitialize()
{
	  // Initialize variables
	  objList = NULL;
	  memset(&stats, 0, sizeof(UAVObjStats));

	  // Create mutex
	  mutex = xSemaphoreCreateRecursiveMutex();
	  if (mutex == NULL)
		    return -1;

	  // Initialize default metadata structure (metadata of metaobjects)
	  UAVObjMetadataInitialize(&defMetadata);

	  // Done
	  return 0;
}

/**
 * Get the statistics counters
 * @param[out] statsOut The statistics counters will be copied there
 */
void UAVObjGetStats(UAVObjStats * statsOut)
{
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  memcpy(statsOut, &stats, sizeof(UAVObjStats));
	  xSemaphoreGiveRecursive(mutex);
}

/**
 * Clear the statistics counters
 */
void UAVObjClearStats()
{
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  memset(&stats, 0, sizeof(UAVObjStats));
	  xSemaphoreGiveRecursive(mutex);
}

/**
 * Register and new object in the object manager.
 * \param[in] id Unique object ID
 * \param[in] name Object name
 * \param[in] nameName Metaobject name
 * \param[in] isMetaobject Is this a metaobject (1:true, 0:false)
 * \param[in] isSingleInstance Is this a single instance or multi-instance object
 * \param[in] isSettings Is this a settings object
 * \param[in] numBytes Number of bytes of object data (for one instance)
 * \param[in] initCb Default field and metadata initialization function
 * \return Object handle, or NULL if failure.
 * \return
 */
UAVObjHandle UAVObjRegister(uint32_t id, const char *name,
			    const char *metaName, int32_t isMetaobject,
			    int32_t isSingleInstance, int32_t isSettings,
			    uint32_t numBytes,
			    UAVObjInitializeCallback initCb)
{
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;
	  ObjectList *metaObj;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Check that the object is not already registered
	  LL_FOREACH(objList, objEntry) {
		    if (objEntry->id == id) {
			      // Already registered, ignore
			      xSemaphoreGiveRecursive(mutex);
			      return NULL;
		    }
	  }

	  // Create and append entry
	  objEntry = (ObjectList *) pvPortMalloc(sizeof(ObjectList));
	  if (objEntry == NULL) {
		    xSemaphoreGiveRecursive(mutex);
		    return NULL;
	  }
	  objEntry->id = id;
	  objEntry->name = name;
	  OLSetIsMetaobject(objEntry, isMetaobject);
	  OLSetIsSingleInstance(objEntry, isSingleInstance);
	  OLSetIsSettings(objEntry, isSettings);
	  objEntry->numBytes = numBytes;
	  objEntry->events = NULL;
	  objEntry->numInstances = 0;
	  objEntry->instances.data = NULL;
	  objEntry->instances.instId = 0xFFFF;
	  objEntry->instances.next = NULL;
	  objEntry->linkedObj = NULL;	// will be set later
	  LL_APPEND(objList, objEntry);

	  // Create instance zero
	  instEntry = createInstance(objEntry, 0);
	  if (instEntry == NULL) {
		    xSemaphoreGiveRecursive(mutex);
		    return NULL;
	  }
	  // Create metaobject and update linkedObj
	  if (isMetaobject) {
		    objEntry->linkedObj = NULL;	// will be set later
	  } else {
		    // Create metaobject
		    metaObj =
			(ObjectList *) UAVObjRegister(id + 1, metaName,
						      NULL, 1, 1, 0,
						      sizeof
						      (UAVObjMetadata),
						      NULL);
		    // Link two objects
		    objEntry->linkedObj = metaObj;
		    metaObj->linkedObj = objEntry;
	  }

	  // Initialize object fields and metadata to default values
	  if (initCb != NULL) {
		    initCb((UAVObjHandle) objEntry, 0);
	  }
	  // Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object)
	  if (!OLGetIsMetaobject(objEntry)) {
		    UAVObjLoad((UAVObjHandle) objEntry->linkedObj, 0);
	  }
	  // If this is a settings object, attempt to load from SD card
	  if (OLGetIsSettings(objEntry)) {
		    UAVObjLoad((UAVObjHandle) objEntry, 0);
	  }
	  // Release lock
	  xSemaphoreGiveRecursive(mutex);
	  return (UAVObjHandle) objEntry;
}

/**
 * Retrieve an object from the list given its id
 * \param[in] The object ID
 * \return The object or NULL if not found.
 */
UAVObjHandle UAVObjGetByID(uint32_t id)
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Look for object
	  LL_FOREACH(objList, objEntry) {
		    if (objEntry->id == id) {
			      // Release lock
			      xSemaphoreGiveRecursive(mutex);
			      // Done, object found
			      return (UAVObjHandle) objEntry;
		    }
	  }

	  // Object not found, release lock and return error
	  xSemaphoreGiveRecursive(mutex);
	  return NULL;
}

/**
 * Retrieve an object from the list given its name
 * \param[in] name The name of the object
 * \return The object or NULL if not found.
 */
UAVObjHandle UAVObjGetByName(char *name)
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Look for object
	  LL_FOREACH(objList, objEntry) {
		    if (objEntry->name != NULL
			&& strcmp(objEntry->name, name) == 0) {
			      // Release lock
			      xSemaphoreGiveRecursive(mutex);
			      // Done, object found
			      return (UAVObjHandle) objEntry;
		    }
	  }

	  // Object not found, release lock and return error
	  xSemaphoreGiveRecursive(mutex);
	  return NULL;
}

/**
 * Get the object's ID
 * \param[in] obj The object handle
 * \return The object ID
 */
uint32_t UAVObjGetID(UAVObjHandle obj)
{
	  return ((ObjectList *) obj)->id;
}

/**
 * Get the object's name
 * \param[in] obj The object handle
 * \return The object's name
 */
const char *UAVObjGetName(UAVObjHandle obj)
{
	  return ((ObjectList *) obj)->name;
}

/**
 * Get the number of bytes of the object's data (for one instance)
 * \param[in] obj The object handle
 * \return The number of bytes
 */
uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
{
	  return ((ObjectList *) obj)->numBytes;
}

/**
 * Get the object this object is linked to. For regular objects, the linked object
 * is the metaobject. For metaobjects the linked object is the parent object.
 * This function is normally only needed by the telemetry module.
 * \param[in] obj The object handle
 * \return The object linked object handle
 */
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
{
	  return (UAVObjHandle) (((ObjectList *) obj)->linkedObj);
}

/**
 * Get the number of instances contained in the object.
 * \param[in] obj The object handle
 * \return The number of instances
 */
uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
{
	  uint32_t numInstances;
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  numInstances = ((ObjectList *) obj)->numInstances;
	  xSemaphoreGiveRecursive(mutex);
	  return numInstances;
}

/**
 * Create a new instance in the object.
 * \param[in] obj The object handle
 * \return The instance ID or 0 if an error
 */
uint16_t UAVObjCreateInstance(UAVObjHandle obj,
			      UAVObjInitializeCallback initCb)
{
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Create new instance
	  objEntry = (ObjectList *) obj;
	  instEntry = createInstance(objEntry, objEntry->numInstances);
	  if (instEntry == NULL) {
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Initialize instance data
	  if (initCb != NULL) {
		    initCb(obj, instEntry->instId);
	  }
	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return instEntry->instId;
}

/**
 * Does this object contains a single instance or multiple instances?
 * \param[in] obj The object handle
 * \return True (1) if this is a single instance object
 */
int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
{
	  return OLGetIsSingleInstance((ObjectList *) obj);
}

/**
 * Is this a metaobject?
 * \param[in] obj The object handle
 * \return True (1) if this is metaobject
 */
int32_t UAVObjIsMetaobject(UAVObjHandle obj)
{
	  return OLGetIsMetaobject((ObjectList *) obj);
}

/**
 * Is this a settings object?
 * \param[in] obj The object handle
 * \return True (1) if this is a settings object
 */
int32_t UAVObjIsSettings(UAVObjHandle obj)
{
	  return OLGetIsSettings((ObjectList *) obj);
}

/**
 * Unpack an object from a byte array
 * \param[in] obj The object handle
 * \param[in] instId The instance ID
 * \param[in] dataIn The byte array
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId,
		     const uint8_t * dataIn)
{
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast handle to object
	  objEntry = (ObjectList *) obj;

	  // Get the instance
	  instEntry = getInstance(objEntry, instId);

	  // If the instance does not exist create it and any other instances before it
	  if (instEntry == NULL) {
		    instEntry = createInstance(objEntry, instId);
		    if (instEntry == NULL) {
			      // Error, unlock and return
			      xSemaphoreGiveRecursive(mutex);
			      return -1;
		    }
	  }
	  // Set the data
	  memcpy(instEntry->data, dataIn, objEntry->numBytes);

	  // Fire event
	  sendEvent(objEntry, instId, EV_UNPACKED);

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Pack an object to a byte array
 * \param[in] obj The object handle
 * \param[in] instId The instance ID
 * \param[out] dataOut The byte array
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut)
{
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast handle to object
	  objEntry = (ObjectList *) obj;

	  // Get the instance
	  instEntry = getInstance(objEntry, instId);
	  if (instEntry == NULL) {
		    // Error, unlock and return
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Pack data
	  memcpy(dataOut, instEntry->data, objEntry->numBytes);

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Save the data of the specified object instance to the file system (SD card).
 * The object will be appended and the file will not be closed.
 * The object data can be restored using the UAVObjLoad function.
 * @param[in] obj The object handle.
 * @param[in] instId The instance ID
 * @param[in] file File to append to
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId,
			 FILEINFO * file)
{
#if defined(PIOS_INCLUDE_SDCARD)
	  uint32_t bytesWritten;
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;

	  // Check for file system availability
	  if (PIOS_SDCARD_IsMounted() == 0) {
		    return -1;
	  }
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast to object
	  objEntry = (ObjectList *) obj;

	  // Get the instance information
	  instEntry = getInstance(objEntry, instId);
	  if (instEntry == NULL) {
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Write the object ID
	  PIOS_FWRITE(file, &objEntry->id, sizeof(objEntry->id),
		      &bytesWritten);

	  // Write the instance ID
	  if (!OLGetIsSingleInstance(objEntry)) {
		    PIOS_FWRITE(file, &instEntry->instId,
				sizeof(instEntry->instId), &bytesWritten);
	  }
	  // Write the data and check that the write was successful
	  PIOS_FWRITE(file, instEntry->data, objEntry->numBytes,
		      &bytesWritten);
	  if (bytesWritten != objEntry->numBytes) {
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Done
	  xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
	  return 0;
}

/**
 * Save the data of the specified object to the file system (SD card).
 * If the object contains multiple instances, all of them will be saved.
 * A new file with the name of the object will be created.
 * The object data can be restored using the UAVObjLoad function.
 * @param[in] obj The object handle.
 * @param[in] instId The instance ID
 * @param[in] file File to append to
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	  ObjectList *objEntry = (ObjectList *) obj;

	  if (objEntry == NULL)
		    return -1;

	  ObjectInstList *instEntry = getInstance(objEntry, instId);

	  if (instEntry == NULL)
		    return -1;

	  if (instEntry->data == NULL)
		    return -1;

	  if (PIOS_FLASHFS_ObjSave(obj, instId, instEntry->data) != 0)
		    return -1;
#endif
#if defined(PIOS_INCLUDE_SDCARD)
	  FILEINFO file;
	  ObjectList *objEntry;
	  uint8_t filename[14];

	  // Check for file system availability
	  if (PIOS_SDCARD_IsMounted() == 0) {
		    return -1;
	  }
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast to object
	  objEntry = (ObjectList *) obj;

	  // Get filename
	  objectFilename(objEntry, filename);

	  // Open file
	  if (PIOS_FOPEN_WRITE(filename, file)) {
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Append object
	  if (UAVObjSaveToFile(obj, instId, &file) == -1) {
		    PIOS_FCLOSE(file);
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Done, close file and unlock
	  PIOS_FCLOSE(file);
	  xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
	  return 0;
}

/**
 * Load an object from the file system (SD card).
 * @param[in] file File to read from
 * @return The handle of the object loaded or NULL if a failure
 */
UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
{
#if defined(PIOS_INCLUDE_SDCARD)
	  uint32_t bytesRead;
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;
	  uint32_t objId;
	  uint16_t instId;
	  UAVObjHandle obj;

	  // Check for file system availability
	  if (PIOS_SDCARD_IsMounted() == 0) {
		    return NULL;
	  }
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Read the object ID
	  if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
		    xSemaphoreGiveRecursive(mutex);
		    return NULL;
	  }
	  // Get the object
	  obj = UAVObjGetByID(objId);
	  if (obj == 0) {
		    xSemaphoreGiveRecursive(mutex);
		    return NULL;
	  }
	  objEntry = (ObjectList *) obj;

	  // Get the instance ID
	  instId = 0;
	  if (!OLGetIsSingleInstance(objEntry)) {
		    if (PIOS_FREAD
			(file, &instId, sizeof(instId), &bytesRead)) {
			      xSemaphoreGiveRecursive(mutex);
			      return NULL;
		    }
	  }
	  // Get the instance information
	  instEntry = getInstance(objEntry, instId);

	  // If the instance does not exist create it and any other instances before it
	  if (instEntry == NULL) {
		    instEntry = createInstance(objEntry, instId);
		    if (instEntry == NULL) {
			      // Error, unlock and return
			      xSemaphoreGiveRecursive(mutex);
			      return NULL;
		    }
	  }
	  // Read the instance data
	  if (PIOS_FREAD
	      (file, instEntry->data, objEntry->numBytes, &bytesRead)) {
		    xSemaphoreGiveRecursive(mutex);
		    return NULL;
	  }
	  // Fire event
	  sendEvent(objEntry, instId, EV_UNPACKED);

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return obj;
#else /* PIOS_INCLUDE_SDCARD */
	  return NULL;
#endif
}

/**
 * Load an object from the file system (SD card).
 * A file with the name of the object will be opened.
 * The object data can be saved using the UAVObjSave function.
 * @param[in] obj The object handle.
 * @param[in] instId The object instance
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	ObjectList *objEntry = (ObjectList *) obj;

	if (objEntry == NULL)
		return -1;

	ObjectInstList *instEntry = getInstance(objEntry, instId);

	if (instEntry == NULL)
		return -1;

	if (instEntry->data == NULL)
		return -1;

	// Fire event on success
	if (PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data) == 0)
		sendEvent(objEntry, instId, EV_UNPACKED);
	else
		return -1;
#endif

#if defined(PIOS_INCLUDE_SDCARD)
	  FILEINFO file;
	  ObjectList *objEntry;
	  UAVObjHandle loadedObj;
	  ObjectList *loadedObjEntry;
	  uint8_t filename[14];

	  // Check for file system availability
	  if (PIOS_SDCARD_IsMounted() == 0) {
		    return -1;
	  }
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast to object
	  objEntry = (ObjectList *) obj;

	  // Get filename
	  objectFilename(objEntry, filename);

	  // Open file
	  if (PIOS_FOPEN_READ(filename, file)) {
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Load object
	  loadedObj = UAVObjLoadFromFile(&file);
	  if (loadedObj == 0) {
		    PIOS_FCLOSE(file);
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Check that the IDs match
	  loadedObjEntry = (ObjectList *) loadedObj;
	  if (loadedObjEntry->id != objEntry->id) {
		    PIOS_FCLOSE(file);
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Done, close file and unlock
	  PIOS_FCLOSE(file);
	  xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
	  return 0;
}

/**
 * Delete an object from the file system (SD card).
 * @param[in] obj The object handle.
 * @param[in] instId The object instance
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	  PIOS_FLASHFS_ObjDelete(obj, instId);
#endif
#if defined(PIOS_INCLUDE_SDCARD)
	  ObjectList *objEntry;
	  uint8_t filename[14];

	  // Check for file system availability
	  if (PIOS_SDCARD_IsMounted() == 0) {
		    return -1;
	  }
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast to object
	  objEntry = (ObjectList *) obj;

	  // Get filename
	  objectFilename(objEntry, filename);

	  // Delete file
	  PIOS_FUNLINK(filename);

	  // Done
	  xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
	  return 0;
}

/**
 * Save all settings objects to the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjSaveSettings()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Save all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (OLGetIsSettings(objEntry)) {
			      // Save object
			      if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
				  -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Load all settings objects from the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjLoadSettings()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Load all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (OLGetIsSettings(objEntry)) {
			      // Load object
			      if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
				  -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Delete all settings objects from the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjDeleteSettings()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Save all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (OLGetIsSettings(objEntry)) {
			      // Save object
			      if (UAVObjDelete((UAVObjHandle) objEntry, 0)
				  == -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Save all metaobjects to the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjSaveMetaobjects()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Save all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (OLGetIsMetaobject(objEntry)) {
			      // Save object
			      if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
				  -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Load all metaobjects from the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjLoadMetaobjects()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Load all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (OLGetIsMetaobject(objEntry)) {
			      // Load object
			      if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
				  -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Delete all metaobjects from the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjDeleteMetaobjects()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Load all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (OLGetIsMetaobject(objEntry)) {
			      // Load object
			      if (UAVObjDelete((UAVObjHandle) objEntry, 0)
				  == -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Set the object data
 * \param[in] obj The object handle
 * \param[in] dataIn The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjSetData(UAVObjHandle obj, const void *dataIn)
{
	  return UAVObjSetInstanceData(obj, 0, dataIn);
}

/**
 * Set the object data
 * \param[in] obj The object handle
 * \param[in] dataIn The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size)
{
	return UAVObjSetInstanceDataField(obj, 0, dataIn, offset, size);
}

/**
 * Get the object data
 * \param[in] obj The object handle
 * \param[out] dataOut The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjGetData(UAVObjHandle obj, void *dataOut)
{
	  return UAVObjGetInstanceData(obj, 0, dataOut);
}

/**
 * Get the object data
 * \param[in] obj The object handle
 * \param[out] dataOut The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size)
{
	return UAVObjGetInstanceDataField(obj, 0, dataOut, offset, size);
}

/**
 * Set the data of a specific object instance
 * \param[in] obj The object handle
 * \param[in] instId The object instance ID
 * \param[in] dataIn The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId,
			      const void *dataIn)
{
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;
	  UAVObjMetadata *mdata;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast to object info
	  objEntry = (ObjectList *) obj;

	  // Check access level
	  if (!OLGetIsMetaobject(objEntry)) {
		    mdata =
			(UAVObjMetadata *) (objEntry->linkedObj->instances.
					    data);
		    if (UAVObjGetAccess(mdata) == ACCESS_READONLY) {
			      xSemaphoreGiveRecursive(mutex);
			      return -1;
		    }
	  }
	  // Get instance information
	  instEntry = getInstance(objEntry, instId);
	  if (instEntry == NULL) {
		    // Error, unlock and return
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Set data
	  memcpy(instEntry->data, dataIn, objEntry->numBytes);

	  // Fire event
	  sendEvent(objEntry, instId, EV_UPDATED);

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Set the data of a specific object instance
 * \param[in] obj The object handle
 * \param[in] instId The object instance ID
 * \param[in] dataIn The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
{
	ObjectList* objEntry;
	ObjectInstList* instEntry;
	UAVObjMetadata* mdata;

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	// Cast to object info
	objEntry = (ObjectList*)obj;

	// Check access level
	if ( !OLGetIsMetaobject(objEntry) )
	{
		mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data);
		if ( UAVObjGetAccess(mdata) == ACCESS_READONLY )
		{
			xSemaphoreGiveRecursive(mutex);
			return -1;
		}
	}

	// Get instance information
	instEntry = getInstance(objEntry, instId);
	if ( instEntry == NULL )
	{
		// Error, unlock and return
		xSemaphoreGiveRecursive(mutex);
		return -1;
	}

	// return if we set too much of what we have
	if ( (size + offset) > objEntry->numBytes) {
		// Error, unlock and return
		xSemaphoreGiveRecursive(mutex);		
		return -1;
	}

	// Set data
	memcpy(instEntry->data + offset, dataIn, size);

	// Fire event
	sendEvent(objEntry, instId, EV_UPDATED);

	// Unlock
	xSemaphoreGiveRecursive(mutex);
	return 0;
}

/**
 * Get the data of a specific object instance
 * \param[in] obj The object handle
 * \param[in] instId The object instance ID
 * \param[out] dataOut The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId,
			      void *dataOut)
{
	  ObjectList *objEntry;
	  ObjectInstList *instEntry;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Cast to object info
	  objEntry = (ObjectList *) obj;

	  // Get instance information
	  instEntry = getInstance(objEntry, instId);
	  if (instEntry == NULL) {
		    // Error, unlock and return
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Set data
	  memcpy(dataOut, instEntry->data, objEntry->numBytes);

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Get the data of a specific object instance
 * \param[in] obj The object handle
 * \param[in] instId The object instance ID
 * \param[out] dataOut The object's data structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
{
	ObjectList* objEntry;
	ObjectInstList* instEntry;

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	// Cast to object info
	objEntry = (ObjectList*)obj;

	// Get instance information
	instEntry = getInstance(objEntry, instId);
	if ( instEntry == NULL )
	{
		// Error, unlock and return
		xSemaphoreGiveRecursive(mutex);
		return -1;
	}

	// return if we request too much of what we can give
	if ( (size + offset) > objEntry->numBytes) 
	{
		// Error, unlock and return
		xSemaphoreGiveRecursive(mutex);
		return -1;
	}
	
	// Set data
	memcpy(dataOut, instEntry->data + offset, size);

	// Unlock
	xSemaphoreGiveRecursive(mutex);
	return 0;
}

/**
 * Set the object metadata
 * \param[in] obj The object handle
 * \param[in] dataIn The object's metadata structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn)
{
	  ObjectList *objEntry;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Set metadata (metadata of metaobjects can not be modified)
	  objEntry = (ObjectList *) obj;
	  if (!OLGetIsMetaobject(objEntry)) {
		    UAVObjSetData((UAVObjHandle) objEntry->linkedObj,
				  dataIn);
	  } else {
		    return -1;
	  }

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Get the object metadata
 * \param[in] obj The object handle
 * \param[out] dataOut The object's metadata structure
 * \return 0 if success or -1 if failure
 */
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata * dataOut)
{
	  ObjectList *objEntry;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Get metadata
	  objEntry = (ObjectList *) obj;
	  if (OLGetIsMetaobject(objEntry)) {
		    memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
	  } else {
		    UAVObjGetData((UAVObjHandle) objEntry->linkedObj,
				  dataOut);
	  }

	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}

/**
 * Initialize a UAVObjMetadata object.
 * \param[in] metadata The metadata object
 */
void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
{
	metadata->flags =
		ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
		ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
		1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
		1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
		UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
		UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT;
	metadata->telemetryUpdatePeriod = 0;
	metadata->gcsTelemetryUpdatePeriod = 0;
	metadata->loggingUpdatePeriod = 0;
}

/**
 * Get the UAVObject metadata access member
 * \param[in] metadata The metadata object
 * \return the access type
 */
UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
{
	return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1;
}

/**
 * Set the UAVObject metadata access member
 * \param[in] metadata The metadata object
 * \param[in] mode The access mode
 */
void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
{
	SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1);
}

/**
 * Get the UAVObject metadata GCS access member
 * \param[in] metadata The metadata object
 * \return the GCS access type
 */
UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
{
	return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1;
}

/**
 * Set the UAVObject metadata GCS access member
 * \param[in] metadata The metadata object
 * \param[in] mode The access mode
 */
void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
	SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1);
}

/**
 * Get the UAVObject metadata telemetry acked member
 * \param[in] metadata The metadata object
 * \return the telemetry acked boolean
 */
uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
	return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1;
}

/**
 * Set the UAVObject metadata telemetry acked member
 * \param[in] metadata The metadata object
 * \param[in] val The telemetry acked boolean
 */
void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
	SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1);
}

/**
 * Get the UAVObject metadata GCS telemetry acked member
 * \param[in] metadata The metadata object
 * \return the telemetry acked boolean
 */
uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
	return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1;
}

/**
 * Set the UAVObject metadata GCS telemetry acked member
 * \param[in] metadata The metadata object
 * \param[in] val The GCS telemetry acked boolean
 */
void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
	SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1);
}

/**
 * Get the UAVObject metadata telemetry update mode
 * \param[in] metadata The metadata object
 * \return the telemetry update mode
 */
UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
	return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
}

/**
 * Set the UAVObject metadata telemetry update mode member
 * \param[in] metadata The metadata object
 * \param[in] val The telemetry update mode
 */
void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
	SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
}

/**
 * Get the UAVObject metadata GCS telemetry update mode
 * \param[in] metadata The metadata object
 * \return the GCS telemetry update mode
 */
UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) {
	return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
}

/**
 * Set the UAVObject metadata GCS telemetry update mode member
 * \param[in] metadata The metadata object
 * \param[in] val The GCS telemetry update mode
 */
void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
	SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
}


/**
 * Check if an object is read only
 * \param[in] obj The object handle
 * \return 
 *   \arg 0 if not read only 
 *   \arg 1 if read only
 *   \arg -1 if unable to get meta data
 */
int8_t UAVObjReadOnly(UAVObjHandle obj)
{
	  ObjectList *objEntry;
	  UAVObjMetadata *mdata;

	  // Cast to object info
	  objEntry = (ObjectList *) obj;

	  // Check access level
	  if (!OLGetIsMetaobject(objEntry)) {
		    mdata =
			(UAVObjMetadata *) (objEntry->linkedObj->instances.
					    data);
		    return UAVObjGetAccess(mdata) == ACCESS_READONLY;
	  }
	  return -1;
}

/**
 * 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 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 UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue,
			   uint8_t eventMask)
{
	  int32_t res;
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  res = connectObj(obj, queue, 0, eventMask);
	  xSemaphoreGiveRecursive(mutex);
	  return res;
}

/**
 * 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 UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
{
	  int32_t res;
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  res = disconnectObj(obj, queue, 0);
	  xSemaphoreGiveRecursive(mutex);
	  return res;
}

/**
 * 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,
			      uint8_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
 */
void UAVObjRequestUpdate(UAVObjHandle obj)
{
	  UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES);
}

/**
 * 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, uint16_t instId)
{
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  sendEvent((ObjectList *) obj, instId, EV_UPDATE_REQ);
	  xSemaphoreGiveRecursive(mutex);
}

/**
 * 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)
{
	  UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES);
}

/**
 * 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, uint16_t instId)
{
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  sendEvent((ObjectList *) obj, instId, EV_UPDATED_MANUAL);
	  xSemaphoreGiveRecursive(mutex);
}

/**
 * Iterate through all objects in the list.
 * \param iterator This function will be called once for each object,
 * the object will be passed as a parameter
 */
void UAVObjIterate(void (*iterator) (UAVObjHandle obj))
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Iterate through the list and invoke iterator for each object
	  LL_FOREACH(objList, objEntry) {
		    (*iterator) ((UAVObjHandle) objEntry);
	  }

	  // Release lock
	  xSemaphoreGiveRecursive(mutex);
}

/**
 * Send an event to all event queues registered on the object.
 */
static int32_t sendEvent(ObjectList * obj, uint16_t instId,
			 UAVObjEventType event)
{
	  ObjectEventList *eventEntry;
	  UAVObjEvent msg;

	  // Setup event
	  msg.obj = (UAVObjHandle) obj;
	  msg.event = 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->events, eventEntry) {
		    if (eventEntry->eventMask == 0
			|| (eventEntry->eventMask & event) != 0) {
			      // Send to queue if a valid queue is registered
			      if (eventEntry->queue != 0) {
					if (xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE)	// will not block
					{
						stats.lastQueueErrorID = UAVObjGetID(obj);
						  ++stats.eventQueueErrors;
					}
			      }
			      // Invoke callback (from event task) if a valid one is registered
			      if (eventEntry->cb != 0) {
					if (EventCallbackDispatch(&msg, eventEntry->cb) != pdTRUE)	// invoke callback from the event task, will not block
					{
						  ++stats.eventCallbackErrors;
						  stats.lastCallbackErrorID = UAVObjGetID(obj);
					}
			      }
		    }
	  }

	  // Done
	  return 0;
}

/**
 * Create a new object instance, return the instance info or NULL if failure.
 */
static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId)
{
	  ObjectInstList *instEntry;
	  int32_t n;

	  // For single instance objects, only instance zero is allowed
	  if (OLGetIsSingleInstance(obj) && instId != 0) {
		    return NULL;
	  }
	  // Make sure that the instance ID is within limits
	  if (instId >= UAVOBJ_MAX_INSTANCES) {
		    return NULL;
	  }
	  // Check if the instance already exists
	  if (getInstance(obj, instId) != NULL) {
		    return NULL;
	  }
	  // Create any missing instances (all instance IDs must be sequential)
	  for (n = obj->numInstances; n < instId; ++n) {
		    if (createInstance(obj, n) == NULL) {
			      return NULL;
		    }
	  }

	  if (instId == 0) {	/* Instance 0 ObjectInstList allocated with ObjectList element */
		    instEntry = &obj->instances;
		    instEntry->data = pvPortMalloc(obj->numBytes);
		    if (instEntry->data == NULL)
			      return NULL;
		    memset(instEntry->data, 0, obj->numBytes);
		    instEntry->instId = instId;
	  } else {
		    // Create the actual instance
		    instEntry =
			(ObjectInstList *)
			pvPortMalloc(sizeof(ObjectInstList));
		    if (instEntry == NULL)
			      return NULL;
		    instEntry->data = pvPortMalloc(obj->numBytes);
		    if (instEntry->data == NULL)
			      return NULL;
		    memset(instEntry->data, 0, obj->numBytes);
		    instEntry->instId = instId;
		    LL_APPEND(obj->instances.next, instEntry);
	  }
	  ++obj->numInstances;

	  // Fire event
	  UAVObjInstanceUpdated((UAVObjHandle) obj, instId);

	  // Done
	  return instEntry;
}

/**
 * Get the instance information or NULL if the instance does not exist
 */
static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId)
{
	  ObjectInstList *instEntry;

	  // Look for specified instance ID
	  LL_FOREACH(&(obj->instances), instEntry) {
		    if (instEntry->instId == instId) {
			      return instEntry;
		    }
	  }
	  // If this point is reached then instance id was not found
	  return NULL;
}

/**
 * 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
 */
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
			  UAVObjEventCallback cb, uint8_t eventMask)
{
	  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->events, eventEntry) {
		    if (eventEntry->queue == queue && eventEntry->cb == cb) {
			      // Already connected, update event mask and return
			      eventEntry->eventMask = eventMask;
			      return 0;
		    }
	  }

	  // Add queue to list
	  eventEntry =
	      (ObjectEventList *) pvPortMalloc(sizeof(ObjectEventList));
	  if (eventEntry == NULL) {
		    return -1;
	  }
	  eventEntry->queue = queue;
	  eventEntry->cb = cb;
	  eventEntry->eventMask = eventMask;
	  LL_APPEND(objEntry->events, eventEntry);

	  // 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
 */
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
			     UAVObjEventCallback cb)
{
	  ObjectEventList *eventEntry;
	  ObjectList *objEntry;

	  // Find queue and remove it
	  objEntry = (ObjectList *) obj;
	  LL_FOREACH(objEntry->events, eventEntry) {
		    if ((eventEntry->queue == queue
			 && eventEntry->cb == cb)) {
			      LL_DELETE(objEntry->events, eventEntry);
			      vPortFree(eventEntry);
			      return 0;
		    }
	  }

	  // If this point is reached the queue was not found
	  return -1;
}

#if defined(PIOS_INCLUDE_SDCARD)
/**
 * Wrapper for the sprintf function
 */
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...)
{
	  va_list args;
	  va_start(args, format);
	  vsprintf((char *)buffer, (char *)format, args);
}

/**
 * Get an 8 character (plus extension) filename for the object.
 */
static void objectFilename(ObjectList * obj, uint8_t * filename)
{
	  customSPrintf(filename, (uint8_t *) "%X.obj", obj->id);
}
#endif /* PIOS_INCLUDE_SDCARD */