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"
#include "pios_struct_helper.h"

// Constants

// Private types

// Macros
#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);

/**
 * List of event queues and the eventmask associated with the queue.
 */

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

struct ObjectEventEntry {
	xQueueHandle              queue;
	UAVObjEventCallback       cb;
	uint8_t                   eventMask;
	struct ObjectEventEntry * next;
};

/*
  MetaInstance   == [UAVOBase [UAVObjMetadata]]
  SingleInstance == [UAVOBase [UAVOData [InstanceData]]]
  MultiInstance  == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]]
                                                  ____________________/
                                                  \-->[InstanceData1 [next]]
                                                  _________...________/
                                                  \-->[InstanceDataN [next]]
 */

/*
 * UAVO Base Type
 *   - All Types of UAVObjects are of this base type
 *   - The flags determine what type(s) this object
 */
struct UAVOBase {
	/* Let these objects be added to an event queue */
	struct ObjectEventEntry * next_event;

	/* Describe the type of object that follows this header */
	struct UAVOInfo {
		bool isMeta        : 1;
		bool isSingle      : 1;
		bool isSettings    : 1;
	} flags;

} __attribute__((packed));

/* Augmented type for Meta UAVO */
struct UAVOMeta {
	struct UAVOBase   base;
	UAVObjMetadata    instance0;
} __attribute__((packed));

/* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */
struct UAVOData {
	struct UAVOBase   base;
	uint32_t          id;
	/*
	 * Embed the Meta object as another complete UAVO
	 * inside the payload for this UAVO.
	 */
	struct UAVOMeta   metaObj;
	struct UAVOData * next;
	uint16_t          instance_size;
} __attribute__((packed));

/* Augmented type for Single Instance Data UAVO */
struct UAVOSingle {
	struct UAVOData   uavo;

	uint8_t           instance0[];
	/* 
	 * Additional space will be malloc'd here to hold the
	 * the data for this instance.
	 */
} __attribute__((packed));

/* Part of a linked list of instances chained off of a multi instance UAVO. */
struct UAVOMultiInst {
	struct UAVOMultiInst * next;
	uint8_t                instance[];
	/* 
	 * Additional space will be malloc'd here to hold the
	 * the data for this instance.
	 */
} __attribute__((packed));

/* Augmented type for Multi Instance Data UAVO */
struct UAVOMulti {
	struct UAVOData        uavo;

	uint16_t               num_instances;
	struct UAVOMultiInst   instance0;
	/*
	 * Additional space will be malloc'd here to hold the
	 * the data for instance 0.
	 */
} __attribute__((packed));

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

/** all information about instances are dependant on object type **/
#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (struct UAVOSingle*)obj )->instance0)))
#define InstanceDataOffset(inst) ((void*)&(( (struct UAVOMultiInst*)inst )->instance))
#define InstanceData(instance) (void*)instance

// Private functions
static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
			UAVObjEventType event);
static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId);
static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId);
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
			UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
			UAVObjEventCallback cb);

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

// Private variables
static struct UAVOData * uavo_list;
static xSemaphoreHandle mutex;
static const UAVObjMetadata defMetadata = {
	.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),
	.telemetryUpdatePeriod    = 0,
	.gcsTelemetryUpdatePeriod = 0,
	.loggingUpdatePeriod      = 0,
};

static UAVObjStats stats;

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

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

	// Done
	return 0;
}

/*****************
 * Statistics
 ****************/

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

/************************
 * Object Initialization
 ***********************/

static void UAVObjInitMetaData (struct UAVOMeta * obj_meta)
{
	/* Fill in the common part of the UAVO */
	struct UAVOBase * uavo_base = &(obj_meta->base);
	memset(uavo_base, 0, sizeof(*uavo_base));
	uavo_base->flags.isMeta   = true;
	uavo_base->flags.isSingle = true;
	uavo_base->next_event     = NULL;

	/* Clear the instance data carried in the UAVO */
	memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0));
}

static struct UAVOData * UAVObjAllocSingle(uint32_t num_bytes)
{
	/* Compute the complete size of the object, including the data for a single embedded instance */
	uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes;

	/* Allocate the object from the heap */
	struct UAVOSingle * uavo_single = (struct UAVOSingle *) pvPortMalloc(object_size);
	if (!uavo_single)
		return (NULL);

	/* Fill in the common part of the UAVO */
	struct UAVOBase * uavo_base = &(uavo_single->uavo.base);
	memset(uavo_base, 0, sizeof(*uavo_base));
	uavo_base->flags.isSingle = true;
	uavo_base->next_event     = NULL;

	/* Clear the instance data carried in the UAVO */
	memset(&(uavo_single->instance0), 0, num_bytes);

	/* Give back the generic UAVO part */
	return (&(uavo_single->uavo));
}

static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes)
{
	/* Compute the complete size of the object, including the data for a single embedded instance */
	uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes;

	/* Allocate the object from the heap */
	struct UAVOMulti * uavo_multi = (struct UAVOMulti *) pvPortMalloc(object_size);
	if (!uavo_multi)
		return (NULL);

	/* Fill in the common part of the UAVO */
	struct UAVOBase * uavo_base = &(uavo_multi->uavo.base);
	memset(uavo_base, 0, sizeof(*uavo_base));
	uavo_base->flags.isSingle = false;
	uavo_base->next_event     = NULL;

	/* Set up the type-specific part of the UAVO */
	uavo_multi->num_instances = 1;

	/* Clear the instance data carried in the UAVO */
	memset (&(uavo_multi->instance0), 0, num_bytes);

	/* Give back the generic UAVO part */
	return (&(uavo_multi->uavo));
}

/**************************
 * UAVObject Database APIs
 *************************/

/**
 * Register and new object in the object manager.
 * \param[in] id Unique object ID
 * \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, 
			int32_t isSingleInstance, int32_t isSettings,
			uint32_t num_bytes,
			UAVObjInitializeCallback initCb)
{
	struct UAVOData * uavo_data = NULL;

	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	/* Don't allow duplicate registrations */
	if (UAVObjGetByID(id))
		goto unlock_exit;

	/* Map the various flags to one of the UAVO types we understand */
	if (isSingleInstance) {
		uavo_data = UAVObjAllocSingle (num_bytes);
	} else {
		uavo_data = UAVObjAllocMulti (num_bytes);
	}

	if (!uavo_data)
		goto unlock_exit;

	/* Fill in the details about this UAVO */
	uavo_data->id            = id;
	uavo_data->instance_size = num_bytes;
	if (isSettings) {
		uavo_data->base.flags.isSettings = true;
	}

	/* Initialize the embedded meta UAVO */
	UAVObjInitMetaData (&uavo_data->metaObj);

	/* Add the newly created object to the global list of objects */
	LL_APPEND(uavo_list, uavo_data);

	/* Initialize object fields and metadata to default values */
	if (initCb)
		initCb((UAVObjHandle) uavo_data, 0);

	/* Always try to load the meta object from flash */
	UAVObjLoad((UAVObjHandle) &(uavo_data->metaObj), 0);

	/* Attempt to load settings object from flash */
	if (uavo_data->base.flags.isSettings)
		UAVObjLoad((UAVObjHandle) uavo_data, 0);

	// fire events for outer object and its embedded meta object
	UAVObjInstanceUpdated((UAVObjHandle) uavo_data, 0);
	UAVObjInstanceUpdated((UAVObjHandle) &(uavo_data->metaObj), 0);

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return (UAVObjHandle) uavo_data;
}

/**
 * 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)
{
	UAVObjHandle * found_obj = (UAVObjHandle *) NULL;

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	// Look for object
	struct UAVOData * tmp_obj;
	LL_FOREACH(uavo_list, tmp_obj) {
		if (tmp_obj->id == id) {
			found_obj = (UAVObjHandle *)tmp_obj;
			goto unlock_exit;
		}
		if (MetaObjectId(tmp_obj->id) == id) {
			found_obj = (UAVObjHandle *)&(tmp_obj->metaObj);
			goto unlock_exit;
		}
	}

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return found_obj;
}

/**
 * Get the object's ID
 * \param[in] obj The object handle
 * \return The object ID
 */
uint32_t UAVObjGetID(UAVObjHandle obj_handle)
{
	PIOS_Assert(obj_handle);

	/* Recover the common object header */
	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;

	if (UAVObjIsMetaobject(obj_handle)) {
		/* We have a meta object, find our containing UAVO */
		struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);

		return MetaObjectId (uavo_data->id);
	} else {
		/* We have a data object, augment our pointer */
		struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;

		return (uavo_data->id);
	}
}

/**
 * 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);

	uint32_t instance_size;

	/* Recover the common object header */
	struct UAVOBase * uavo_base = (struct UAVOBase *) obj;

	if (uavo_base->flags.isMeta) {
		instance_size = MetaNumBytes;
	} else {
		/* We have a data object, augment our pointer */
		struct UAVOData * uavo = (struct UAVOData *) uavo_base;

		instance_size = uavo->instance_size;
	}

	return (instance_size);
}

/**
 * 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_handle)
{
	PIOS_Assert(obj_handle);

	/* Recover the common object header */
	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;

	if (UAVObjIsMetaobject(obj_handle)) {
		/* We have a meta object, find our containing UAVO. */
		struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);

		return (UAVObjHandle) uavo_data;
	} else {
		/* We have a data object, augment our pointer */
		struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;

		return (UAVObjHandle) &(uavo_data->metaObj);
	}
}

/**
 * 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_handle)
{
	PIOS_Assert(obj_handle);

	if (UAVObjIsSingleInstance(obj_handle)) {
		/* Only one instance is allowed */
		return 1;
	} else {
		/* Multi-instance object.  Inspect the object */
		/* Augment our pointer to reflect the proper type */
		struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj_handle;
		return uavo_multi->num_instances;
	}
}

/**
 * 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_handle,
			UAVObjInitializeCallback initCb)
{
	PIOS_Assert(obj_handle);

	if (UAVObjIsMetaobject(obj_handle)) {
		return 0;
	}

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	InstanceHandle instEntry;
	uint16_t instId = 0;

	// Create new instance
	instId = UAVObjGetNumInstances(obj_handle);
	instEntry = createInstance( (struct UAVOData *)obj_handle, instId);
	if (instEntry == NULL) {
		goto unlock_exit;
	}

	// Initialize instance data
	if (initCb) {
		initCb(obj_handle, instId);
	}

unlock_exit:
	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
 */
bool UAVObjIsSingleInstance(UAVObjHandle obj_handle)
{
	PIOS_Assert(obj_handle);

	/* Recover the common object header */
	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;

	return uavo_base->flags.isSingle;
}

/**
 * Is this a metaobject?
 * \param[in] obj The object handle
 * \return True (1) if this is metaobject
 */
bool UAVObjIsMetaobject(UAVObjHandle obj_handle)
{
	PIOS_Assert(obj_handle);

	/* Recover the common object header */
	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;

	return uavo_base->flags.isMeta;
}

/**
 * Is this a settings object?
 * \param[in] obj The object handle
 * \return True (1) if this is a settings object
 */
bool UAVObjIsSettings(UAVObjHandle obj_handle)
{
	PIOS_Assert(obj_handle);

	/* Recover the common object header */
	struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;

	return uavo_base->flags.isSettings;
}

/**
 * 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_handle, uint16_t instId,
		const uint8_t * dataIn)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	if (UAVObjIsMetaobject(obj_handle)) {
		if (instId != 0) {
			goto unlock_exit;
		}
		memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
	} else {
		struct UAVOData *obj;
		InstanceHandle instEntry;

		// Cast handle to object
		obj = (struct UAVOData *) obj_handle;

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

		// If the instance does not exist create it and any other instances before it
		if (instEntry == NULL) {
			instEntry = createInstance(obj, instId);
			if (instEntry == NULL) {
				goto unlock_exit;
			}
		}
		// Set the data
		memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
	}

	// Fire event
	sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

/**
 * 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_handle, uint16_t instId, uint8_t * dataOut)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	if (UAVObjIsMetaobject(obj_handle)) {
		if (instId != 0) {
			goto unlock_exit;
		}
		memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
	} else {
		struct UAVOData *obj;
		InstanceHandle instEntry;

		// Cast handle to object
		obj = (struct UAVOData *) obj_handle;

		// Get the instance
		instEntry = getInstance(obj, instId);
		if (instEntry == NULL) {
			goto unlock_exit;
		}
		// Pack data
		memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

#if defined(PIOS_INCLUDE_SDCARD)
/**
 * 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_handle, uint16_t instId,
			FILEINFO * file)
{
	PIOS_Assert(obj_handle);

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

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

		// Write the data and check that the write was successful
		PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes,
			&bytesWritten);
		if (bytesWritten != MetaNumBytes) {
			xSemaphoreGiveRecursive(mutex);
			return -1;
		}
	} else {
		struct UAVOData * uavo;
		InstanceHandle instEntry;

		// Cast to object
		uavo = (struct UAVOData *) obj_handle;

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

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

/**
 * 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_handle, uint16_t instId)
{
	PIOS_Assert(obj_handle);

#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	if (UAVObjIsMetaobject(obj_handle)) {
		if (instId != 0)
			return -1;

		if (PIOS_FLASHFS_ObjSave(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) != 0)
			return -1;
	} else {
		InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);

		if (instEntry == NULL)
			return -1;

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

		if (PIOS_FLASHFS_ObjSave(obj_handle, 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_handle, filename);

	// Open file
	if (PIOS_FOPEN_WRITE(filename, file)) {
		xSemaphoreGiveRecursive(mutex);
		return -1;
	}
	// Append object
	if (UAVObjSaveToFile(obj_handle, 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;
}

#if defined(PIOS_INCLUDE_SDCARD)
/**
 * 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)
{
	uint32_t bytesRead;
	struct UAVOBase *objEntry;
	InstanceHandle instEntry;
	uint32_t objId;
	uint16_t instId;
	UAVObjHandle obj_handle;

	// 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_handle = UAVObjGetByID(objId);
	if (obj_handle == 0) {
		xSemaphoreGiveRecursive(mutex);
		return NULL;
	}
	objEntry = (struct UAVOBase *) obj_handle;

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

	if (UAVObjIsMetaobject(obj_handle)) {
		// 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((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) {
			xSemaphoreGiveRecursive(mutex);
			return NULL;
		}
	} else {

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

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

	}

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

	// Unlock
	xSemaphoreGiveRecursive(mutex);
	return obj_handle;
}
#endif /* PIOS_INCLUDE_SDCARD */

/**
 * 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_handle, uint16_t instId)
{
	PIOS_Assert(obj_handle);

#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	if (UAVObjIsMetaobject(obj_handle)) {
		if (instId != 0)
			return -1;

		// Fire event on success
		if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) == 0)
			sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
		else
			return -1;
	} else {

		InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);

		if (instEntry == NULL)
			return -1;

		// Fire event on success
		if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, InstanceData(instEntry)) == 0)
			sendEvent((struct UAVOBase*)obj_handle, 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_handle, 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_handle)) {
		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_handle, uint16_t instId)
{
	PIOS_Assert(obj_handle);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
	PIOS_FLASHFS_ObjDelete(obj_handle, 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_handle, 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()
{
	struct UAVOData *obj;

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	// Save all settings objects
	LL_FOREACH(uavo_list, obj) {
		// Check if this is a settings object
		if (UAVObjIsSettings(obj)) {
			// Save object
			if (UAVObjSave((UAVObjHandle) obj, 0) ==
				-1) {
				goto unlock_exit;
			}
		}
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

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

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	// Load all settings objects
	LL_FOREACH(uavo_list, obj) {
		// Check if this is a settings object
		if (UAVObjIsSettings(obj)) {
			// Load object
			if (UAVObjLoad((UAVObjHandle) obj, 0) ==
				-1) {
				goto unlock_exit;
			}
		}
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

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

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	// Save all settings objects
	LL_FOREACH(uavo_list, obj) {
		// Check if this is a settings object
		if (UAVObjIsSettings(obj)) {
			// Save object
			if (UAVObjDelete((UAVObjHandle) obj, 0)
				== -1) {
				goto unlock_exit;
			}
		}
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

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

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	// Save all settings objects
	LL_FOREACH(uavo_list, obj) {
		// Save object
		if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) ==
			-1) {
			goto unlock_exit;
		}
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

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

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	// Load all settings objects
	LL_FOREACH(uavo_list, obj) {
		// Load object
		if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) ==
			-1) {
			goto unlock_exit;
		}
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

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

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	// Load all settings objects
	LL_FOREACH(uavo_list, obj) {
		// Load object
		if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0)
			== -1) {
			goto unlock_exit;
		}
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

/**
 * 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_handle, const void *dataIn)
{
	return UAVObjSetInstanceData(obj_handle, 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_handle, const void* dataIn, uint32_t offset, uint32_t size)
{
	return UAVObjSetInstanceDataField(obj_handle, 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_handle, void *dataOut)
{
	return UAVObjGetInstanceData(obj_handle, 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_handle, void* dataOut, uint32_t offset, uint32_t size)
{
	return UAVObjGetInstanceDataField(obj_handle, 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_handle, uint16_t instId,
			const void *dataIn)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	if (UAVObjIsMetaobject(obj_handle)) {
		if (instId != 0) {
			goto unlock_exit;
		}
		memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
	} else {
		struct UAVOData *obj;
		InstanceHandle instEntry;

		// Cast to object info
		obj = (struct UAVOData *) obj_handle;

		// Check access level
		if (UAVObjReadOnly(obj_handle)) {
			goto unlock_exit;
		}
		// Get instance information
		instEntry = getInstance(obj, instId);
		if (instEntry == NULL) {
			goto unlock_exit;
		}
		// Set data
		memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
	}

	// Fire event
	sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

/**
 * 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_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	if (UAVObjIsMetaobject(obj_handle)) {
		// Get instance information
		if (instId != 0) {
			goto unlock_exit;
		}

		// Check for overrun
		if ((size + offset) > MetaNumBytes) {
			goto unlock_exit;
		}

		// Set data
		memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size);
	} else {
		struct UAVOData * obj;
		InstanceHandle instEntry;

		// Cast to object info
		obj = (struct UAVOData *)obj_handle;

		// Check access level
		if (UAVObjReadOnly(obj_handle)) {
			goto unlock_exit;
		}

		// Get instance information
		instEntry = getInstance(obj, instId);
		if (instEntry == NULL) {
			goto unlock_exit;
		}

		// Check for overrun
		if ((size + offset) > obj->instance_size) {
			goto unlock_exit;
		}

		// Set data
		memcpy(InstanceData(instEntry) + offset, dataIn, size);
	}


	// Fire event
	sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

/**
 * 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_handle, uint16_t instId,
			void *dataOut)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	if (UAVObjIsMetaobject(obj_handle)) {
		// Get instance information
		if (instId != 0) {
			goto unlock_exit;
		}
		// Set data
		memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
	} else {
		struct UAVOData *obj;
		InstanceHandle instEntry;

		// Cast to object info
		obj = (struct UAVOData *) obj_handle;

		// Get instance information
		instEntry = getInstance(obj, instId);
		if (instEntry == NULL) {
			goto unlock_exit;
		}
		// Set data
		memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

/**
 * 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_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	int32_t rc = -1;

	if (UAVObjIsMetaobject(obj_handle)) {
		// Get instance information
		if (instId != 0) {
			goto unlock_exit;
		}

		// Check for overrun
		if ((size + offset) > MetaNumBytes) {
			goto unlock_exit;
		}

		// Set data
		memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size);
	} else {
		struct UAVOData * obj;
		InstanceHandle instEntry;

		// Cast to object info
		obj = (struct UAVOData *)obj_handle;

		// Get instance information
		instEntry = getInstance(obj, instId);
		if (instEntry == NULL) {
			goto unlock_exit;
		}

		// Check for overrun
		if ((size + offset) > obj->instance_size) {
			goto unlock_exit;
		}
		
		// Set data
		memcpy(dataOut, InstanceData(instEntry) + offset, size);
	}

	rc = 0;

unlock_exit:
	xSemaphoreGiveRecursive(mutex);
	return rc;
}

/**
 * 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_handle, const UAVObjMetadata * dataIn)
{
	PIOS_Assert(obj_handle);

	// Set metadata (metadata of metaobjects can not be modified)
	if (UAVObjIsMetaobject(obj_handle)) {
		return -1;
	}

	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	UAVObjSetData((UAVObjHandle) MetaObjectPtr((struct UAVOData *)obj_handle), dataIn);

	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_handle, UAVObjMetadata * dataOut)
{
	PIOS_Assert(obj_handle);

	// Lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	// Get metadata
	if (UAVObjIsMetaobject(obj_handle)) {
		memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
	} else {
		UAVObjGetData((UAVObjHandle) MetaObjectPtr( (struct UAVOData *)obj_handle ),
			dataOut);
	}

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

/*******************************
 * Object Metadata Manipulation
 ******************************/

/**
 * 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_handle)
{
	PIOS_Assert(obj_handle);
	if (!UAVObjIsMetaobject(obj_handle)) {
		return UAVObjGetAccess(LinkedMetaDataPtr( (struct UAVOData *)obj_handle)) == 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_handle, xQueueHandle queue,
			uint8_t eventMask)
{
	PIOS_Assert(obj_handle);
	PIOS_Assert(queue);
	int32_t res;
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	res = connectObj(obj_handle, 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_handle, xQueueHandle queue)
{
	PIOS_Assert(obj_handle);
	PIOS_Assert(queue);
	int32_t res;
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	res = disconnectObj(obj_handle, 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_handle, UAVObjEventCallback cb,
			uint8_t eventMask)
{
	PIOS_Assert(obj_handle);
	int32_t res;
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	res = connectObj(obj_handle, 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_handle, UAVObjEventCallback cb)
{
	PIOS_Assert(obj_handle);
	int32_t res;
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	res = disconnectObj(obj_handle, 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_handle)
{
	UAVObjRequestInstanceUpdate(obj_handle, 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_handle, uint16_t instId)
{
	PIOS_Assert(obj_handle);
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	sendEvent((struct UAVOBase *) obj_handle, 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_handle)
{
	UAVObjInstanceUpdated(obj_handle, 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_handle, uint16_t instId)
{
	PIOS_Assert(obj_handle);
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
	sendEvent((struct UAVOBase *) obj_handle, 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);

	// Get lock
	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	// Iterate through the list and invoke iterator for each object
	struct UAVOData *obj;
	LL_FOREACH(uavo_list, obj) {
		(*iterator) ((UAVObjHandle) obj);
		(*iterator) ((UAVObjHandle) &obj->metaObj);
	}

	// Release lock
	xSemaphoreGiveRecursive(mutex);
}

/**
 * Send a triggered event to all event queues registered on the object.
 */
static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
			UAVObjEventType triggered_event)
{
	/* Set up the message that will be sent to all registered listeners */
	UAVObjEvent msg = {
		.obj    = (UAVObjHandle) obj,
		.event  = triggered_event,
		.instId = instId,
	};

	// Go through each object and push the event message in the queue (if event is activated for the queue)
	struct ObjectEventEntry *event;
	LL_FOREACH(obj->next_event, event) {
		if (event->eventMask == 0
			|| (event->eventMask & triggered_event) != 0) {
			// Send to queue if a valid queue is registered
			if (event->queue) {
				// will not block
				if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
					stats.lastQueueErrorID = UAVObjGetID(obj);
					++stats.eventQueueErrors;
				}
			}

			// Invoke callback (from event task) if a valid one is registered
			if (event->cb) {
				// invoke callback from the event task, will not block
				if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) {
					++stats.eventCallbackErrors;
					stats.lastCallbackErrorID = UAVObjGetID(obj);
				}
			}
		}
	}

	return 0;
}

/**
 * Create a new object instance, return the instance info or NULL if failure.
 */
static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId)
{
	struct UAVOMultiInst *instEntry;

	/* Don't allow more than one instance for single instance objects */
	if (UAVObjIsSingleInstance(&(obj->base))) {
		PIOS_Assert(0);
		return NULL;
	}

	/* Don't create more than the allowed number of instances */
	if (instId >= UAVOBJ_MAX_INSTANCES) {
		return NULL;
	}

	/* Don't allow duplicate instances */
	if (instId < UAVObjGetNumInstances(&(obj->base))) {
		return NULL;
	}

	// Create any missing instances (all instance IDs must be sequential)
	for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) {
		if (createInstance(obj, n) == NULL) {
			return NULL;
		}
	}

	/* Create the actual instance */
	instEntry = (struct UAVOMultiInst *) pvPortMalloc(sizeof(struct UAVOMultiInst)+obj->instance_size);
	if (!instEntry)
		return NULL;
	memset(InstanceDataOffset(instEntry), 0, obj->instance_size);
	LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry);

	( (struct UAVOMulti*)obj )->num_instances++;

	// 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(struct UAVOData * obj, uint16_t instId)
{
	if (UAVObjIsMetaobject(&obj->base)) {
		/* Metadata Instance */

		if (instId != 0)
			return NULL;

		/* Augment our pointer to reflect the proper type */
		struct UAVOMeta * uavo_meta = (struct UAVOMeta *) obj;
		return (&(uavo_meta->instance0));

	} else if (UAVObjIsSingleInstance(&(obj->base))) {
		/* Single Instance */

		if (instId != 0)
			return NULL;

		/* Augment our pointer to reflect the proper type */
		struct UAVOSingle * uavo_single = (struct UAVOSingle *) obj;
		return (&(uavo_single->instance0));
	} else {
		/* Multi Instance */
		/* Augment our pointer to reflect the proper type */
		struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj;
		if (instId >= uavo_multi->num_instances)
			return NULL;

		// Look for specified instance ID
		uint16_t instance = 0;
		struct UAVOMultiInst *instEntry;
		LL_FOREACH(&(uavo_multi->instance0), instEntry) {
			if (instance++ == instId) {
				/* Found it */
				return &(instEntry->instance);
			}
		}
		/* Instance 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_handle, xQueueHandle queue,
			UAVObjEventCallback cb, uint8_t eventMask)
{
	struct ObjectEventEntry *event;
	struct UAVOBase *obj;

	// Check that the queue is not already connected, if it is simply update event mask
	obj = (struct UAVOBase *) obj_handle;
	LL_FOREACH(obj->next_event, event) {
		if (event->queue == queue && event->cb == cb) {
			// Already connected, update event mask and return
			event->eventMask = eventMask;
			return 0;
		}
	}

	// Add queue to list
	event =	(struct ObjectEventEntry *) pvPortMalloc(sizeof(struct ObjectEventEntry));
	if (event == NULL) {
		return -1;
	}
	event->queue = queue;
	event->cb = cb;
	event->eventMask = eventMask;
	LL_APPEND(obj->next_event, event);

	// 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_handle, xQueueHandle queue,
			UAVObjEventCallback cb)
{
	struct ObjectEventEntry *event;
	struct UAVOBase *obj;

	// Find queue and remove it
	obj = (struct UAVOBase *) obj_handle;
	LL_FOREACH(obj->next_event, event) {
		if ((event->queue == queue
				&& event->cb == cb)) {
			LL_DELETE(obj->next_event, event);
			vPortFree(event);
			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_handle, uint8_t * filename)
{
	customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle));
}
#endif /* PIOS_INCLUDE_SDCARD */