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 a pointer to the next instance and some UAVObjectData
 */
struct ObjectInstListStruct {
	  struct ObjectInstListStruct *next;
} __attribute__((packed));
typedef struct ObjectInstListStruct ObjectInstList;

/** fake structure for arbitrary sizes **/
struct ObjectInstanceStruct {
	  ObjectInstList header;
	  uint32_t data;
} __attribute__((packed));
typedef struct ObjectInstanceStruct ObjectInstance;
#define ObjectInstanceSize(numBytes) (offsetof(ObjectInstance,data)+(numBytes))

/** anonymous type for instances **/
typedef void* InstanceHandle; 

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 */
} ObjectFlags;

/**
 * List of objects registered in the object manager
 */

struct GenericObjectStruct {
	  ObjectFlags flags;
            /** The object list mode flags */
	  const char *name;
	        /** The object name */
	  ObjectEventList *events;
				 /** Event queues registered on the object */
}  __attribute__((packed));
typedef struct GenericObjectStruct GenericObject;

struct MetaObjectStruct {
	  GenericObject header;
	  UAVObjMetadata data;
	        /** the actual metadata */
} __attribute__((packed));
typedef struct MetaObjectStruct MetaObject;

struct ObjectListStruct {
	  GenericObject header;
	  uint32_t id;
		     /** The object ID */
	  uint16_t numBytes;
			   /** Number of data bytes contained in the object (for a single instance) */
	  MetaObject metaObj;
					    /** Meta object of the UAVObject */
	  struct ObjectListStruct *next;
				       /** Needed by linked list library (utlist.h) */
} __attribute__((packed));
typedef struct ObjectListStruct ObjectList;

/** fake structure for arbitrary sizes **/
struct ObjectListInstanceStruct {
	  ObjectList header;
	  uint32_t data;
} __attribute__((packed));
typedef struct ObjectListInstanceStruct ObjectListInstance;
#define ObjectListInstanceSize(numBytes) (offsetof(ObjectListInstance,data)+(numBytes))

struct ObjectListMultiStruct {
	  ObjectList header;
	  uint16_t numInstances;
			       /** Number of instances */
	  ObjectInstList instances;
				  /** List of object instances, instance 0 always exists */
} __attribute__((packed));
typedef struct ObjectListMultiStruct ObjectListMulti;

/** fake structure for arbitrary sizes **/
struct ObjectListMultiInstanceStruct {
	  ObjectListMulti header;
	  uint32_t data;
} __attribute__((packed));
typedef struct ObjectListMultiInstanceStruct ObjectListMultiInstance;
#define ObjectListMultiInstanceSize(numBytes) (offsetof(ObjectListMultiInstance,data)+(numBytes))

/** all information about a metaobject are hardcoded constants **/
#define MetaNumBytes sizeof(UAVObjMetadata)
#define MetaBaseObjectPtr(obj) ((ObjectList *)((obj)-offsetof(ObjectList, metaObj)))
#define MetaObjectPtr(obj) ((MetaObject*) &((obj)->metaObj))
#define MetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->data))
#define LinkedMetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->metaObj.data))
#define MetaObjectId(id) (id+1)
#define MetaNumInstances 1

/** all information about instances are dependant on object type **/
#define ObjNumInstances(obj) (OLGetIsSingleInstance((GenericObject*)(obj))?1:((ObjectListMulti *)(obj))->numInstances)
#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (ObjectListInstance*)obj )->data)))
#define InstanceDataOffset(inst) ((void*)&(( (ObjectInstance*)inst )->data))
#define InstanceData(instance) (void*)instance

// Private functions
static int32_t sendEvent(GenericObject * obj, uint16_t instId,
			 UAVObjEventType event);
static InstanceHandle createInstance(ObjectList * obj, uint16_t instId);
static InstanceHandle 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(UAVObjHandle 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 isSingleInstance, int32_t isSettings,
			    uint32_t numBytes,
			    UAVObjInitializeCallback initCb)
{
	  ObjectList *objEntry;

	  // 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
	  if (isSingleInstance) {
	  	  objEntry = (ObjectList *) pvPortMalloc( ObjectListInstanceSize(numBytes) );
      } else {
		  objEntry = (ObjectList *) pvPortMalloc( ObjectListMultiInstanceSize(numBytes) );
	  }
	  if (objEntry == NULL) {
		    xSemaphoreGiveRecursive(mutex);
		    return NULL;
	  }
	  ( (GenericObject*)objEntry )->name = name;
	  ( (GenericObject*)objEntry )->events = NULL;
	  OLSetIsMetaobject(     (GenericObject*)objEntry, 0);
	  OLSetIsSingleInstance( (GenericObject*)objEntry, isSingleInstance);
	  OLSetIsSettings(       (GenericObject*)objEntry, isSettings);
	  objEntry->id = id;
	  objEntry->numBytes = numBytes;
	  // Create instance
	  if (isSingleInstance) {
		  memset(ObjSingleInstanceDataOffset(objEntry), 0, numBytes);
	  } else {
		  ( (ObjectListMulti*)objEntry )->numInstances = 1;
		  ( (ObjectListMulti*)objEntry )->instances.next = NULL;
		  memset(InstanceDataOffset(&(( (ObjectListMulti*)objEntry )->instances)), 0, numBytes);
	  }
	  // Create metaobject
	  memset(LinkedMetaDataPtr(objEntry), 0, MetaNumBytes);
	  ( (GenericObject*)MetaObjectPtr(objEntry) )->flags = OL_IS_METAOBJECT | OL_IS_SINGLE_INSTANCE;
	  ( (GenericObject*)MetaObjectPtr(objEntry) )->name = metaName;
	  ( (GenericObject*)MetaObjectPtr(objEntry) )->events = NULL;
	  LL_APPEND(objList, objEntry);

	  // fire events 
	  UAVObjInstanceUpdated((UAVObjHandle) objEntry, 0);

	  UAVObjInstanceUpdated((UAVObjHandle) MetaObjectPtr(objEntry), 0);

	  // 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((GenericObject*)objEntry)) {
		    UAVObjLoad((UAVObjHandle) MetaObjectPtr(objEntry), 0);
	  }
	  // If this is a settings object, attempt to load from SD card
	  if (OLGetIsSettings((GenericObject*)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;
		    }
		    if (MetaObjectId(objEntry->id) == id) {
			      // Release lock
			      xSemaphoreGiveRecursive(mutex);
			      // Done, object found
			      return (UAVObjHandle) MetaObjectPtr(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 (      ( (GenericObject*)objEntry )->name != NULL
			&& strcmp(( (GenericObject*)objEntry )->name, name) == 0) {
			      // Release lock
			      xSemaphoreGiveRecursive(mutex);
			      // Done, object found
			      return (UAVObjHandle) objEntry;
		    }
		    if (      ( (GenericObject*)MetaObjectPtr(objEntry) )->name != NULL
			&& strcmp(( (GenericObject*)MetaObjectPtr(objEntry) )->name, name) == 0) {
			      // Release lock
			      xSemaphoreGiveRecursive(mutex);
			      // Done, object found
			      return (UAVObjHandle) MetaObjectPtr(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)
{
	  PIOS_Assert(obj);
      if (OLGetIsMetaobject( (GenericObject *) obj) ) {
	  	  return MetaObjectId( MetaBaseObjectPtr(obj)->id );
	  } else {
		  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)
{
	  PIOS_Assert(obj);
	  return ((GenericObject *) 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)
{
	  PIOS_Assert(obj);
      if (OLGetIsMetaobject( (GenericObject *) obj) ) {
	      return MetaNumBytes;
	  } else {
		  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)
{
	  PIOS_Assert(obj);
      if (OLGetIsMetaobject( (GenericObject *) obj) ) {
	      return (UAVObjHandle) MetaBaseObjectPtr(obj);
	  } else {
	      return (UAVObjHandle) MetaObjectPtr( (ObjectList*) obj);
	  }
}

/**
 * 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)
{
	  PIOS_Assert(obj);
      if (OLGetIsMetaobject( (GenericObject *) obj) ) {
	      return MetaNumInstances;
	  }
	  uint32_t numInstances;
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  numInstances = ObjNumInstances(obj);
	  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)
{
	  PIOS_Assert(obj);
      if (OLGetIsMetaobject( (GenericObject *) obj) ) {
		    return -1;
	  }

	  InstanceHandle instEntry;
	  uint16_t instId;

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Create new instance
	  instId = ObjNumInstances(obj);
	  instEntry = createInstance( (ObjectList*)obj, instId);
	  if (instEntry == NULL) {
		    xSemaphoreGiveRecursive(mutex);
		    return -1;
	  }
	  // Initialize instance data
	  if (initCb != NULL) {
		    initCb(obj, instId);
	  }
	  // Unlock
	  xSemaphoreGiveRecursive(mutex);
	  return 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)
{
	  PIOS_Assert(obj);
	  return OLGetIsSingleInstance((GenericObject *) obj);
}

/**
 * Is this a metaobject?
 * \param[in] obj The object handle
 * \return True (1) if this is metaobject
 */
int32_t UAVObjIsMetaobject(UAVObjHandle obj)
{
	  PIOS_Assert(obj);
	  return OLGetIsMetaobject((GenericObject *) 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)
{
	  PIOS_Assert(obj);
	  return OLGetIsSettings((GenericObject *) 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)
{
	  PIOS_Assert(obj);
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

      if (OLGetIsMetaobject( (GenericObject *) obj )) {
	      if (instId != 0) {
			  // Error, unlock and return
			  xSemaphoreGiveRecursive(mutex);
			  return -1;
		  }
		  memcpy(MetaDataPtr((MetaObject *)obj), dataIn, MetaNumBytes);
	  } else {
		  ObjectList *objEntry;
		  InstanceHandle instEntry;

		  // 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(InstanceData(instEntry), dataIn, objEntry->numBytes);
	  }

	  // Fire event
	  sendEvent((GenericObject*)obj, 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)
{
	  PIOS_Assert(obj);
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

      if (OLGetIsMetaobject( (GenericObject *) obj )) {
	      if (instId != 0) {
			  // Error, unlock and return
			  xSemaphoreGiveRecursive(mutex);
			  return -1;
		  }
		  memcpy(dataOut, MetaDataPtr((MetaObject *)obj), MetaNumBytes);
	  } else {
		  ObjectList *objEntry;
		  InstanceHandle instEntry;

		  // 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, InstanceData(instEntry), 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)
{
	  PIOS_Assert(obj);
#if defined(PIOS_INCLUDE_SDCARD)
	  uint32_t bytesWritten;
	  // Check for file system availability
	  if (PIOS_SDCARD_IsMounted() == 0) {
		    return -1;
	  }
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

      if (OLGetIsMetaobject( (GenericObject *) obj )) {
		  // Get the instance information
		  if (instId != 0) {
				xSemaphoreGiveRecursive(mutex);
				return -1;
		  }
		  // Write the object ID
		  uint32_t objId = UAVObjGetID(obj);
		  PIOS_FWRITE(file, &objId, sizeof(objId),
				  &bytesWritten);

		  // Write the data and check that the write was successful
		  PIOS_FWRITE(file, MetaDataPtr((MetaObject *)obj), MetaNumBytes,
				  &bytesWritten);
		  if (bytesWritten != MetaNumBytes) {
				xSemaphoreGiveRecursive(mutex);
				return -1;
		  }
	  } else {
		  ObjectList *objEntry;
		  InstanceHandle instEntry;

		  // 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((GenericObject*)obj)) {
				PIOS_FWRITE(file, &instId,
					sizeof(instId), &bytesWritten);
		  }
		  // Write the data and check that the write was successful
		  PIOS_FWRITE(file, InstanceData(instEntry), 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)
{
	  PIOS_Assert(obj);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
      if (OLGetIsMetaobject( (GenericObject *) obj )) {
		  if (instId != 0)
				return -1;

		  if (PIOS_FLASHFS_ObjSave(obj, instId, (uint8_t*) MetaDataPtr((MetaObject *)obj)) != 0)
				return -1;
	  } else {
		  InstanceHandle instEntry = getInstance( (ObjectList*)obj, instId);

		  if (instEntry == NULL)
				return -1;

		  if (InstanceData(instEntry) == NULL)
				return -1;

		  if (PIOS_FLASHFS_ObjSave(obj, instId, InstanceData(instEntry)) != 0)
				return -1;
	  }
#endif
#if defined(PIOS_INCLUDE_SDCARD)
	  FILEINFO file;
	  uint8_t filename[14];

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

	  // Get filename
	  objectFilename(obj, 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;
	  GenericObject *objEntry;
	  InstanceHandle 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 = (GenericObject *) obj;

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

      if (OLGetIsMetaobject( objEntry )) {
		  // If the instance does not exist create it and any other instances before it
		  if (instId != 0) {
			  // Error, unlock and return
			  xSemaphoreGiveRecursive(mutex);
			  return NULL;
		  }
		  // Read the instance data
		  if (PIOS_FREAD
			  (file, MetaDataPtr((MetaObject *)obj), MetaNumBytes, &bytesRead)) {
				xSemaphoreGiveRecursive(mutex);
				return NULL;
		  }
	  } else {

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

		  // If the instance does not exist create it and any other instances before it
		  if (instEntry == NULL) {
				instEntry = createInstance((ObjectList *)objEntry, instId);
				if (instEntry == NULL) {
					  // Error, unlock and return
					  xSemaphoreGiveRecursive(mutex);
					  return NULL;
				}
		  }
		  // Read the instance data
		  if (PIOS_FREAD
			  (file, InstanceData(instEntry), ((ObjectList *)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)
{
	PIOS_Assert(obj);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
    if (OLGetIsMetaobject( (GenericObject*) obj )) {
		if (instId != 0)
			return -1;

		// Fire event on success
		if (PIOS_FLASHFS_ObjLoad(obj, instId, (uint8_t*) MetaDataPtr((MetaObject *)obj)) == 0)
			sendEvent((GenericObject*)obj, instId, EV_UNPACKED);
		else
			return -1;
	} else {

		InstanceHandle instEntry = getInstance( (ObjectList*)obj, instId);

		if (instEntry == NULL)
			return -1;

		// Fire event on success
		if (PIOS_FLASHFS_ObjLoad(obj, instId, InstanceData(instEntry)) == 0)
			sendEvent((GenericObject*)obj, instId, EV_UNPACKED);
		else
			return -1;
	}

#endif

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

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

	  // Get filename
	  objectFilename(obj, 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
	  if (UAVObjGetID(loadedObj) != UAVObjGetID(obj)) {
		    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)
{
	  PIOS_Assert(obj);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	  PIOS_FLASHFS_ObjDelete(obj, instId);
#endif
#if defined(PIOS_INCLUDE_SDCARD)
	  uint8_t filename[14];

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

	  // Get filename
	  objectFilename(obj, 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((GenericObject*)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((GenericObject *)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((GenericObject *)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) {
			// Save object
			if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(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) {
			// Load object
			if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(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) {
			// Load object
			if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(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)
{
	  PIOS_Assert(obj);
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Check access level
	  if (!OLGetIsMetaobject((GenericObject *)obj)) {
		    ObjectList *objEntry;
		    InstanceHandle instEntry;

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

		    if (UAVObjGetAccess( LinkedMetaDataPtr(objEntry) ) == 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(InstanceData(instEntry), dataIn, objEntry->numBytes);
	  } else {
			// Get instance information
			if (instId != 0) {
				// Error, unlock and return
				xSemaphoreGiveRecursive(mutex);
				return -1;
			}
			// Set data
			memcpy(MetaDataPtr((MetaObject *)obj), dataIn, MetaNumBytes);
	  }

	  // Fire event
	  sendEvent((GenericObject *)obj, 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)
{
	PIOS_Assert(obj);
	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	// Check access level
	if ( !OLGetIsMetaobject( (GenericObject*) obj ) )
	{
		ObjectList* objEntry;
		InstanceHandle instEntry;

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

		if ( UAVObjGetAccess( LinkedMetaDataPtr(objEntry) ) == 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(InstanceData(instEntry) + offset, dataIn, size);
	} else {
		// Get instance information
		if ( instId != 0 )
		{
			// Error, unlock and return
			xSemaphoreGiveRecursive(mutex);
			return -1;
		}

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

		// Set data
		memcpy(MetaDataPtr((MetaObject *)obj) + offset, dataIn, size);
	}


	// Fire event
	sendEvent((GenericObject *)obj, 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)
{
	  PIOS_Assert(obj);
	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  if ( !OLGetIsMetaobject( (GenericObject*) obj) )
	  {
		  ObjectList *objEntry;
		  InstanceHandle instEntry;

		  // 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, InstanceData(instEntry), objEntry->numBytes);
	  } else {
		  // Get instance information
		  if (instId != 0) {
				// Error, unlock and return
				xSemaphoreGiveRecursive(mutex);
				return -1;
		  }
		  // Set data
		  memcpy(dataOut, MetaDataPtr((MetaObject *)obj), MetaNumBytes);
	  }

	  // 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)
{
	PIOS_Assert(obj);
	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	if ( !OLGetIsMetaobject( (GenericObject*) obj ) )
	{
		ObjectList* objEntry;
		InstanceHandle instEntry;

		// 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, InstanceData(instEntry) + offset, size);
	} else {
		// Get instance information
		if ( instId != 0)
		{
			// Error, unlock and return
			xSemaphoreGiveRecursive(mutex);
			return -1;
		}

		// return if we request too much of what we can give
		if ( (size + offset) > MetaNumBytes) 
		{
			// Error, unlock and return
			xSemaphoreGiveRecursive(mutex);
			return -1;
		}
		
		// Set data
		memcpy(dataOut, MetaDataPtr((MetaObject *)obj) + 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)
{
	  PIOS_Assert(obj);

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Set metadata (metadata of metaobjects can not be modified)
	  if (!OLGetIsMetaobject((GenericObject*)obj)) {
		    UAVObjSetData((UAVObjHandle) MetaObjectPtr( (ObjectList*)obj ),
				  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)
{
	  PIOS_Assert(obj);

	  // Lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Get metadata
	  if (OLGetIsMetaobject((GenericObject*)obj)) {
		    memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
	  } else {
		    UAVObjGetData((UAVObjHandle) MetaObjectPtr( (ObjectList*)obj ),
				  dataOut);
	  }

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

/**
 * Initialize a UAVObjMetadata object.
 * \param[in] metadata The metadata object
 */
void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
{
	PIOS_Assert(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)
{
	PIOS_Assert(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)
{
	PIOS_Assert(metadata);
	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)
{
	PIOS_Assert(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) {
	PIOS_Assert(metadata);
	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) {
	PIOS_Assert(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) {
	PIOS_Assert(metadata);
	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) {
	PIOS_Assert(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) {
	PIOS_Assert(metadata);
	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) {
	PIOS_Assert(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) {
	PIOS_Assert(metadata);
	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) {
	PIOS_Assert(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) {
	PIOS_Assert(metadata);
	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)
{
	  PIOS_Assert(obj);
	  if (!OLGetIsMetaobject( (GenericObject *)obj)) {
		    return UAVObjGetAccess( LinkedMetaDataPtr( (ObjectList*)obj ) ) == 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)
{
	  PIOS_Assert(obj);
	  PIOS_Assert(queue);
	  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)
{
	  PIOS_Assert(obj);
	  PIOS_Assert(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)
{
	  PIOS_Assert(obj);
	  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)
{
	  PIOS_Assert(obj);
	  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)
{
	  PIOS_Assert(obj);
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  sendEvent((GenericObject *) 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)
{
	  PIOS_Assert(obj);
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	  sendEvent((GenericObject *) 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))
{
	  PIOS_Assert(iterator);
	  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);
		    (*iterator) ((UAVObjHandle) MetaObjectPtr(objEntry));
	  }

	  // Release lock
	  xSemaphoreGiveRecursive(mutex);
}

/**
 * Send an event to all event queues registered on the object.
 */
static int32_t sendEvent(GenericObject * 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 InstanceHandle createInstance(ObjectList * obj, uint16_t instId)
{
	  ObjectInstList *instEntry;
	  int32_t n;

	  // For single instance objects, only instance zero is allowed (and zero gets created in RegisterObject)
	  if (OLGetIsSingleInstance((GenericObject*)obj)) {
	  PIOS_Assert(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 ( instId< ObjNumInstances(obj) ) {
		    return NULL;
	  }
	  // Create any missing instances (all instance IDs must be sequential)
	  for (n = ObjNumInstances(obj); n < instId; ++n) {
		    if (createInstance(obj, n) == NULL) {
			      return NULL;
		    }
	  }

	  // Create the actual instance
	  instEntry =
		  (ObjectInstList *)
		  pvPortMalloc(sizeof(ObjectInstList)+obj->numBytes);
	  if (instEntry == NULL)
		  return NULL;
	  memset(InstanceDataOffset(instEntry), 0, obj->numBytes);
	  LL_APPEND(( (ObjectListMulti*)obj )->instances.next, instEntry);

	  ( (ObjectListMulti*)obj )->numInstances++;

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

	  // Done
	  return InstanceDataOffset(instEntry);
}

/**
 * Get the instance information or NULL if the instance does not exist
 */
static InstanceHandle getInstance(ObjectList * obj, uint16_t instId)
{
	  ObjectInstList *instEntry;
	  // quick solutions
	  if (OLGetIsSingleInstance((GenericObject*)(obj))) {
		  if (instId!=0)
			  return NULL;
		  return ObjSingleInstanceDataOffset(obj);
	  }
	  if (instId>=ObjNumInstances(obj))
		  return NULL;

	  // Look for specified instance ID
	  uint16_t instance=0;
	  LL_FOREACH(&(( (ObjectListMulti*)obj )->instances), instEntry) {
		    if (instance++ == instId) {
			      return InstanceDataOffset(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;
	  GenericObject *objEntry;

	  // Check that the queue is not already connected, if it is simply update event mask
	  objEntry = (GenericObject *) 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;
	  GenericObject *objEntry;

	  // Find queue and remove it
	  objEntry = (GenericObject *) 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(UAVObjHandle obj, uint8_t * filename)
{
	  customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj));
}
#endif /* PIOS_INCLUDE_SDCARD */