1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-12 20:08:48 +01:00
LibrePilot/flight/UAVObjects/uavobjectmanager.c

1753 lines
46 KiB
C

/**
******************************************************************************
* @addtogroup UAVObjects OpenPilot UAVObjects
* @{
* @addtogroup UAV Object Manager
* @brief The core UAV Objects functions, most of which are wrappered by
* autogenerated defines
* @{
*
*
* @file uavobjectmanager.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Object manager library. This library holds a collection of all objects.
* It can be used by all modules/libraries to find an object reference.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
// Constants
// Private types
// Macros
#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);
#define OLGetIsMetaobject(olp) ((olp)->flags & OL_IS_METAOBJECT)
#define OLSetIsMetaobject(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_METAOBJECT) : ((olp)->flags | OL_IS_METAOBJECT)))
#define OLGetIsSingleInstance(olp) ((olp)->flags & OL_IS_SINGLE_INSTANCE)
#define OLSetIsSingleInstance(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SINGLE_INSTANCE) : ((olp)->flags | OL_IS_SINGLE_INSTANCE)))
#define OLGetIsSettings(olp) ((olp)->flags & OL_IS_SETTINGS)
#define OLSetIsSettings(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SETTINGS) : ((olp)->flags | OL_IS_SETTINGS)))
/**
* List of event queues and the eventmask associated with the queue.
*/
struct ObjectEventListStruct {
xQueueHandle queue;
UAVObjEventCallback cb;
uint8_t eventMask;
struct ObjectEventListStruct *next;
};
typedef struct ObjectEventListStruct ObjectEventList;
/**
* List of object instances, holds the actual data structure and instance ID
*/
struct ObjectInstListStruct {
void *data;
uint16_t instId;
struct ObjectInstListStruct *next;
};
typedef struct ObjectInstListStruct ObjectInstList;
typedef enum {
OL_IS_METAOBJECT = 0x01, /** Set if this is a metaobject */
OL_IS_SINGLE_INSTANCE = 0x02, /** Set if this object has a single instance */
OL_IS_SETTINGS = 0x04 /** Set if this object is a settings object */
} ObjectListFlags;
/**
* List of objects registered in the object manager
*/
struct ObjectListStruct {
uint32_t id;
/** The object ID */
const char *name;
/** The object name */
ObjectListFlags flags;
/** The object list mode flags */
uint16_t numBytes;
/** Number of data bytes contained in the object (for a single instance) */
uint16_t numInstances;
/** Number of instances */
struct ObjectListStruct *linkedObj;
/** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */
ObjectInstList instances;
/** List of object instances, instance 0 always exists */
ObjectEventList *events;
/** Event queues registered on the object */
struct ObjectListStruct *next;
/** Needed by linked list library (utlist.h) */
};
typedef struct ObjectListStruct ObjectList;
// Private functions
static int32_t sendEvent(ObjectList * obj, uint16_t instId,
UAVObjEventType event);
static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId);
static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId);
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb);
#if defined(PIOS_INCLUDE_SDCARD)
static void objectFilename(ObjectList * obj, uint8_t * filename);
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
#endif
// Private variables
static ObjectList *objList;
static xSemaphoreHandle mutex;
static UAVObjMetadata defMetadata;
static UAVObjStats stats;
/**
* Initialize the object manager
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVObjInitialize()
{
// Initialize variables
objList = NULL;
memset(&stats, 0, sizeof(UAVObjStats));
// Create mutex
mutex = xSemaphoreCreateRecursiveMutex();
if (mutex == NULL)
return -1;
// Initialize default metadata structure (metadata of metaobjects)
UAVObjMetadataInitialize(&defMetadata);
// Done
return 0;
}
/**
* Get the statistics counters
* @param[out] statsOut The statistics counters will be copied there
*/
void UAVObjGetStats(UAVObjStats * statsOut)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memcpy(statsOut, &stats, sizeof(UAVObjStats));
xSemaphoreGiveRecursive(mutex);
}
/**
* Clear the statistics counters
*/
void UAVObjClearStats()
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memset(&stats, 0, sizeof(UAVObjStats));
xSemaphoreGiveRecursive(mutex);
}
/**
* Register and new object in the object manager.
* \param[in] id Unique object ID
* \param[in] name Object name
* \param[in] nameName Metaobject name
* \param[in] isMetaobject Is this a metaobject (1:true, 0:false)
* \param[in] isSingleInstance Is this a single instance or multi-instance object
* \param[in] isSettings Is this a settings object
* \param[in] numBytes Number of bytes of object data (for one instance)
* \param[in] initCb Default field and metadata initialization function
* \return Object handle, or NULL if failure.
* \return
*/
UAVObjHandle UAVObjRegister(uint32_t id, const char *name,
const char *metaName, int32_t isMetaobject,
int32_t isSingleInstance, int32_t isSettings,
uint32_t numBytes,
UAVObjInitializeCallback initCb)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
ObjectList *metaObj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check that the object is not already registered
LL_FOREACH(objList, objEntry) {
if (objEntry->id == id) {
// Already registered, ignore
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Create and append entry
objEntry = (ObjectList *) pvPortMalloc(sizeof(ObjectList));
if (objEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
objEntry->id = id;
objEntry->name = name;
OLSetIsMetaobject(objEntry, isMetaobject);
OLSetIsSingleInstance(objEntry, isSingleInstance);
OLSetIsSettings(objEntry, isSettings);
objEntry->numBytes = numBytes;
objEntry->events = NULL;
objEntry->numInstances = 0;
objEntry->instances.data = NULL;
objEntry->instances.instId = 0xFFFF;
objEntry->instances.next = NULL;
objEntry->linkedObj = NULL; // will be set later
LL_APPEND(objList, objEntry);
// Create instance zero
instEntry = createInstance(objEntry, 0);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Create metaobject and update linkedObj
if (isMetaobject) {
objEntry->linkedObj = NULL; // will be set later
} else {
// Create metaobject
metaObj =
(ObjectList *) UAVObjRegister(id + 1, metaName,
NULL, 1, 1, 0,
sizeof
(UAVObjMetadata),
NULL);
// Link two objects
objEntry->linkedObj = metaObj;
metaObj->linkedObj = objEntry;
}
// Initialize object fields and metadata to default values
if (initCb != NULL) {
initCb((UAVObjHandle) objEntry, 0);
}
// Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object)
if (!OLGetIsMetaobject(objEntry)) {
UAVObjLoad((UAVObjHandle) objEntry->linkedObj, 0);
}
// If this is a settings object, attempt to load from SD card
if (OLGetIsSettings(objEntry)) {
UAVObjLoad((UAVObjHandle) objEntry, 0);
}
// Release lock
xSemaphoreGiveRecursive(mutex);
return (UAVObjHandle) objEntry;
}
/**
* Retrieve an object from the list given its id
* \param[in] The object ID
* \return The object or NULL if not found.
*/
UAVObjHandle UAVObjGetByID(uint32_t id)
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Look for object
LL_FOREACH(objList, objEntry) {
if (objEntry->id == id) {
// Release lock
xSemaphoreGiveRecursive(mutex);
// Done, object found
return (UAVObjHandle) objEntry;
}
}
// Object not found, release lock and return error
xSemaphoreGiveRecursive(mutex);
return NULL;
}
/**
* Retrieve an object from the list given its name
* \param[in] name The name of the object
* \return The object or NULL if not found.
*/
UAVObjHandle UAVObjGetByName(char *name)
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Look for object
LL_FOREACH(objList, objEntry) {
if (objEntry->name != NULL
&& strcmp(objEntry->name, name) == 0) {
// Release lock
xSemaphoreGiveRecursive(mutex);
// Done, object found
return (UAVObjHandle) objEntry;
}
}
// Object not found, release lock and return error
xSemaphoreGiveRecursive(mutex);
return NULL;
}
/**
* Get the object's ID
* \param[in] obj The object handle
* \return The object ID
*/
uint32_t UAVObjGetID(UAVObjHandle obj)
{
return ((ObjectList *) obj)->id;
}
/**
* Get the object's name
* \param[in] obj The object handle
* \return The object's name
*/
const char *UAVObjGetName(UAVObjHandle obj)
{
return ((ObjectList *) obj)->name;
}
/**
* Get the number of bytes of the object's data (for one instance)
* \param[in] obj The object handle
* \return The number of bytes
*/
uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
{
return ((ObjectList *) obj)->numBytes;
}
/**
* Get the object this object is linked to. For regular objects, the linked object
* is the metaobject. For metaobjects the linked object is the parent object.
* This function is normally only needed by the telemetry module.
* \param[in] obj The object handle
* \return The object linked object handle
*/
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
{
return (UAVObjHandle) (((ObjectList *) obj)->linkedObj);
}
/**
* Get the number of instances contained in the object.
* \param[in] obj The object handle
* \return The number of instances
*/
uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
{
uint32_t numInstances;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
numInstances = ((ObjectList *) obj)->numInstances;
xSemaphoreGiveRecursive(mutex);
return numInstances;
}
/**
* Create a new instance in the object.
* \param[in] obj The object handle
* \return The instance ID or 0 if an error
*/
uint16_t UAVObjCreateInstance(UAVObjHandle obj,
UAVObjInitializeCallback initCb)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Create new instance
objEntry = (ObjectList *) obj;
instEntry = createInstance(objEntry, objEntry->numInstances);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Initialize instance data
if (initCb != NULL) {
initCb(obj, instEntry->instId);
}
// Unlock
xSemaphoreGiveRecursive(mutex);
return instEntry->instId;
}
/**
* Does this object contains a single instance or multiple instances?
* \param[in] obj The object handle
* \return True (1) if this is a single instance object
*/
int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
{
return OLGetIsSingleInstance((ObjectList *) obj);
}
/**
* Is this a metaobject?
* \param[in] obj The object handle
* \return True (1) if this is metaobject
*/
int32_t UAVObjIsMetaobject(UAVObjHandle obj)
{
return OLGetIsMetaobject((ObjectList *) obj);
}
/**
* Is this a settings object?
* \param[in] obj The object handle
* \return True (1) if this is a settings object
*/
int32_t UAVObjIsSettings(UAVObjHandle obj)
{
return OLGetIsSettings((ObjectList *) obj);
}
/**
* Unpack an object from a byte array
* \param[in] obj The object handle
* \param[in] instId The instance ID
* \param[in] dataIn The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId,
const uint8_t * dataIn)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast handle to object
objEntry = (ObjectList *) obj;
// Get the instance
instEntry = getInstance(objEntry, instId);
// If the instance does not exist create it and any other instances before it
if (instEntry == NULL) {
instEntry = createInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Set the data
memcpy(instEntry->data, dataIn, objEntry->numBytes);
// Fire event
sendEvent(objEntry, instId, EV_UNPACKED);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Pack an object to a byte array
* \param[in] obj The object handle
* \param[in] instId The instance ID
* \param[out] dataOut The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast handle to object
objEntry = (ObjectList *) obj;
// Get the instance
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Pack data
memcpy(dataOut, instEntry->data, objEntry->numBytes);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Save the data of the specified object instance to the file system (SD card).
* The object will be appended and the file will not be closed.
* The object data can be restored using the UAVObjLoad function.
* @param[in] obj The object handle.
* @param[in] instId The instance ID
* @param[in] file File to append to
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId,
FILEINFO * file)
{
#if defined(PIOS_INCLUDE_SDCARD)
uint32_t bytesWritten;
ObjectList *objEntry;
ObjectInstList *instEntry;
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get the instance information
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Write the object ID
PIOS_FWRITE(file, &objEntry->id, sizeof(objEntry->id),
&bytesWritten);
// Write the instance ID
if (!OLGetIsSingleInstance(objEntry)) {
PIOS_FWRITE(file, &instEntry->instId,
sizeof(instEntry->instId), &bytesWritten);
}
// Write the data and check that the write was successful
PIOS_FWRITE(file, instEntry->data, objEntry->numBytes,
&bytesWritten);
if (bytesWritten != objEntry->numBytes) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
/**
* Save the data of the specified object to the file system (SD card).
* If the object contains multiple instances, all of them will be saved.
* A new file with the name of the object will be created.
* The object data can be restored using the UAVObjLoad function.
* @param[in] obj The object handle.
* @param[in] instId The instance ID
* @param[in] file File to append to
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
ObjectList *objEntry = (ObjectList *) obj;
if (objEntry == NULL)
return -1;
ObjectInstList *instEntry = getInstance(objEntry, instId);
if (instEntry == NULL)
return -1;
if (instEntry->data == NULL)
return -1;
if (PIOS_FLASHFS_ObjSave(obj, instId, instEntry->data) != 0)
return -1;
#endif
#if defined(PIOS_INCLUDE_SDCARD)
FILEINFO file;
ObjectList *objEntry;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(objEntry, filename);
// Open file
if (PIOS_FOPEN_WRITE(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Append object
if (UAVObjSaveToFile(obj, instId, &file) == -1) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
/**
* Load an object from the file system (SD card).
* @param[in] file File to read from
* @return The handle of the object loaded or NULL if a failure
*/
UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
{
#if defined(PIOS_INCLUDE_SDCARD)
uint32_t bytesRead;
ObjectList *objEntry;
ObjectInstList *instEntry;
uint32_t objId;
uint16_t instId;
UAVObjHandle obj;
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return NULL;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Read the object ID
if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Get the object
obj = UAVObjGetByID(objId);
if (obj == 0) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
objEntry = (ObjectList *) obj;
// Get the instance ID
instId = 0;
if (!OLGetIsSingleInstance(objEntry)) {
if (PIOS_FREAD
(file, &instId, sizeof(instId), &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Get the instance information
instEntry = getInstance(objEntry, instId);
// If the instance does not exist create it and any other instances before it
if (instEntry == NULL) {
instEntry = createInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Read the instance data
if (PIOS_FREAD
(file, instEntry->data, objEntry->numBytes, &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Fire event
sendEvent(objEntry, instId, EV_UNPACKED);
// Unlock
xSemaphoreGiveRecursive(mutex);
return obj;
#else /* PIOS_INCLUDE_SDCARD */
return NULL;
#endif
}
/**
* Load an object from the file system (SD card).
* A file with the name of the object will be opened.
* The object data can be saved using the UAVObjSave function.
* @param[in] obj The object handle.
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
ObjectList *objEntry = (ObjectList *) obj;
if (objEntry == NULL)
return -1;
ObjectInstList *instEntry = getInstance(objEntry, instId);
if (instEntry == NULL)
return -1;
if (instEntry->data == NULL)
return -1;
// Fire event on success
if (PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data) == 0)
sendEvent(objEntry, instId, EV_UNPACKED);
else
return -1;
#endif
#if defined(PIOS_INCLUDE_SDCARD)
FILEINFO file;
ObjectList *objEntry;
UAVObjHandle loadedObj;
ObjectList *loadedObjEntry;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(objEntry, filename);
// Open file
if (PIOS_FOPEN_READ(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Load object
loadedObj = UAVObjLoadFromFile(&file);
if (loadedObj == 0) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Check that the IDs match
loadedObjEntry = (ObjectList *) loadedObj;
if (loadedObjEntry->id != objEntry->id) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
/**
* Delete an object from the file system (SD card).
* @param[in] obj The object handle.
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
PIOS_FLASHFS_ObjDelete(obj, instId);
#endif
#if defined(PIOS_INCLUDE_SDCARD)
ObjectList *objEntry;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(objEntry, filename);
// Delete file
PIOS_FUNLINK(filename);
// Done
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
/**
* Save all settings objects to the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSaveSettings()
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsSettings(objEntry)) {
// Save object
if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Load all settings objects from the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjLoadSettings()
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsSettings(objEntry)) {
// Load object
if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Delete all settings objects from the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDeleteSettings()
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsSettings(objEntry)) {
// Save object
if (UAVObjDelete((UAVObjHandle) objEntry, 0)
== -1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Save all metaobjects to the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSaveMetaobjects()
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsMetaobject(objEntry)) {
// Save object
if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Load all metaobjects from the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjLoadMetaobjects()
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsMetaobject(objEntry)) {
// Load object
if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Delete all metaobjects from the SD card.
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDeleteMetaobjects()
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsMetaobject(objEntry)) {
// Load object
if (UAVObjDelete((UAVObjHandle) objEntry, 0)
== -1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Set the object data
* \param[in] obj The object handle
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetData(UAVObjHandle obj, const void *dataIn)
{
return UAVObjSetInstanceData(obj, 0, dataIn);
}
/**
* Set the object data
* \param[in] obj The object handle
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size)
{
return UAVObjSetInstanceDataField(obj, 0, dataIn, offset, size);
}
/**
* Get the object data
* \param[in] obj The object handle
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetData(UAVObjHandle obj, void *dataOut)
{
return UAVObjGetInstanceData(obj, 0, dataOut);
}
/**
* Get the object data
* \param[in] obj The object handle
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size)
{
return UAVObjGetInstanceDataField(obj, 0, dataOut, offset, size);
}
/**
* Set the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId,
const void *dataIn)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
UAVObjMetadata *mdata;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList *) obj;
// Check access level
if (!OLGetIsMetaobject(objEntry)) {
mdata =
(UAVObjMetadata *) (objEntry->linkedObj->instances.
data);
if (UAVObjGetAccess(mdata) == ACCESS_READONLY) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Get instance information
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(instEntry->data, dataIn, objEntry->numBytes);
// Fire event
sendEvent(objEntry, instId, EV_UPDATED);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Set the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
{
ObjectList* objEntry;
ObjectInstList* instEntry;
UAVObjMetadata* mdata;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList*)obj;
// Check access level
if ( !OLGetIsMetaobject(objEntry) )
{
mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data);
if ( UAVObjGetAccess(mdata) == ACCESS_READONLY )
{
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Get instance information
instEntry = getInstance(objEntry, instId);
if ( instEntry == NULL )
{
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// return if we set too much of what we have
if ( (size + offset) > objEntry->numBytes) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(instEntry->data + offset, dataIn, size);
// Fire event
sendEvent(objEntry, instId, EV_UPDATED);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Get the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId,
void *dataOut)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList *) obj;
// Get instance information
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(dataOut, instEntry->data, objEntry->numBytes);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Get the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
{
ObjectList* objEntry;
ObjectInstList* instEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList*)obj;
// Get instance information
instEntry = getInstance(objEntry, instId);
if ( instEntry == NULL )
{
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// return if we request too much of what we can give
if ( (size + offset) > objEntry->numBytes)
{
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(dataOut, instEntry->data + offset, size);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Set the object metadata
* \param[in] obj The object handle
* \param[in] dataIn The object's metadata structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn)
{
ObjectList *objEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Set metadata (metadata of metaobjects can not be modified)
objEntry = (ObjectList *) obj;
if (!OLGetIsMetaobject(objEntry)) {
UAVObjSetData((UAVObjHandle) objEntry->linkedObj,
dataIn);
} else {
return -1;
}
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Get the object metadata
* \param[in] obj The object handle
* \param[out] dataOut The object's metadata structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata * dataOut)
{
ObjectList *objEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get metadata
objEntry = (ObjectList *) obj;
if (OLGetIsMetaobject(objEntry)) {
memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
} else {
UAVObjGetData((UAVObjHandle) objEntry->linkedObj,
dataOut);
}
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Initialize a UAVObjMetadata object.
* \param[in] metadata The metadata object
*/
void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
{
metadata->flags =
ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT;
metadata->telemetryUpdatePeriod = 0;
metadata->gcsTelemetryUpdatePeriod = 0;
metadata->loggingUpdatePeriod = 0;
}
/**
* Get the UAVObject metadata access member
* \param[in] metadata The metadata object
* \return the access type
*/
UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
{
return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1;
}
/**
* Set the UAVObject metadata access member
* \param[in] metadata The metadata object
* \param[in] mode The access mode
*/
void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
{
SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1);
}
/**
* Get the UAVObject metadata GCS access member
* \param[in] metadata The metadata object
* \return the GCS access type
*/
UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
{
return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1;
}
/**
* Set the UAVObject metadata GCS access member
* \param[in] metadata The metadata object
* \param[in] mode The access mode
*/
void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1);
}
/**
* Get the UAVObject metadata telemetry acked member
* \param[in] metadata The metadata object
* \return the telemetry acked boolean
*/
uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1;
}
/**
* Set the UAVObject metadata telemetry acked member
* \param[in] metadata The metadata object
* \param[in] val The telemetry acked boolean
*/
void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1);
}
/**
* Get the UAVObject metadata GCS telemetry acked member
* \param[in] metadata The metadata object
* \return the telemetry acked boolean
*/
uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1;
}
/**
* Set the UAVObject metadata GCS telemetry acked member
* \param[in] metadata The metadata object
* \param[in] val The GCS telemetry acked boolean
*/
void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1);
}
/**
* Get the UAVObject metadata telemetry update mode
* \param[in] metadata The metadata object
* \return the telemetry update mode
*/
UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
}
/**
* Set the UAVObject metadata telemetry update mode member
* \param[in] metadata The metadata object
* \param[in] val The telemetry update mode
*/
void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
}
/**
* Get the UAVObject metadata GCS telemetry update mode
* \param[in] metadata The metadata object
* \return the GCS telemetry update mode
*/
UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) {
return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
}
/**
* Set the UAVObject metadata GCS telemetry update mode member
* \param[in] metadata The metadata object
* \param[in] val The GCS telemetry update mode
*/
void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
}
/**
* Check if an object is read only
* \param[in] obj The object handle
* \return
* \arg 0 if not read only
* \arg 1 if read only
* \arg -1 if unable to get meta data
*/
int8_t UAVObjReadOnly(UAVObjHandle obj)
{
ObjectList *objEntry;
UAVObjMetadata *mdata;
// Cast to object info
objEntry = (ObjectList *) obj;
// Check access level
if (!OLGetIsMetaobject(objEntry)) {
mdata =
(UAVObjMetadata *) (objEntry->linkedObj->instances.
data);
return UAVObjGetAccess(mdata) == ACCESS_READONLY;
}
return -1;
}
/**
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
* All events matching the event mask will be pushed to the event queue.
* \param[in] obj The object handle
* \param[in] queue The event queue
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue,
uint8_t eventMask)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj, queue, 0, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
* Disconnect an event queue from the object.
* \param[in] obj The object handle
* \param[in] queue The event queue
* \return 0 if success or -1 if failure
*/
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj, queue, 0);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
* Connect an event callback to the object, if the callback is already connected then the event mask is only updated.
* The supplied callback will be invoked on all events matching the event mask.
* \param[in] obj The object handle
* \param[in] cb The event callback
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb,
uint8_t eventMask)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj, 0, cb, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
* Disconnect an event callback from the object.
* \param[in] obj The object handle
* \param[in] cb The event callback
* \return 0 if success or -1 if failure
*/
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj, 0, cb);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
* will be generated as soon as the object is updated.
* \param[in] obj The object handle
*/
void UAVObjRequestUpdate(UAVObjHandle obj)
{
UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES);
}
/**
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
* will be generated as soon as the object is updated.
* \param[in] obj The object handle
* \param[in] instId Object instance ID to update
*/
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((ObjectList *) obj, instId, EV_UPDATE_REQ);
xSemaphoreGiveRecursive(mutex);
}
/**
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* \param[in] obj The object handle
*/
void UAVObjUpdated(UAVObjHandle obj)
{
UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES);
}
/**
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* \param[in] obj The object handle
* \param[in] instId The object instance ID
*/
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((ObjectList *) obj, instId, EV_UPDATED_MANUAL);
xSemaphoreGiveRecursive(mutex);
}
/**
* Iterate through all objects in the list.
* \param iterator This function will be called once for each object,
* the object will be passed as a parameter
*/
void UAVObjIterate(void (*iterator) (UAVObjHandle obj))
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Iterate through the list and invoke iterator for each object
LL_FOREACH(objList, objEntry) {
(*iterator) ((UAVObjHandle) objEntry);
}
// Release lock
xSemaphoreGiveRecursive(mutex);
}
/**
* Send an event to all event queues registered on the object.
*/
static int32_t sendEvent(ObjectList * obj, uint16_t instId,
UAVObjEventType event)
{
ObjectEventList *eventEntry;
UAVObjEvent msg;
// Setup event
msg.obj = (UAVObjHandle) obj;
msg.event = event;
msg.instId = instId;
// Go through each object and push the event message in the queue (if event is activated for the queue)
LL_FOREACH(obj->events, eventEntry) {
if (eventEntry->eventMask == 0
|| (eventEntry->eventMask & event) != 0) {
// Send to queue if a valid queue is registered
if (eventEntry->queue != 0) {
if (xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE) // will not block
{
++stats.eventErrors;
}
}
// 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.eventErrors;
}
}
}
}
// Done
return 0;
}
/**
* Create a new object instance, return the instance info or NULL if failure.
*/
static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId)
{
ObjectInstList *instEntry;
int32_t n;
// For single instance objects, only instance zero is allowed
if (OLGetIsSingleInstance(obj) && instId != 0) {
return NULL;
}
// Make sure that the instance ID is within limits
if (instId >= UAVOBJ_MAX_INSTANCES) {
return NULL;
}
// Check if the instance already exists
if (getInstance(obj, instId) != NULL) {
return NULL;
}
// Create any missing instances (all instance IDs must be sequential)
for (n = obj->numInstances; n < instId; ++n) {
if (createInstance(obj, n) == NULL) {
return NULL;
}
}
if (instId == 0) { /* Instance 0 ObjectInstList allocated with ObjectList element */
instEntry = &obj->instances;
instEntry->data = pvPortMalloc(obj->numBytes);
if (instEntry->data == NULL)
return NULL;
memset(instEntry->data, 0, obj->numBytes);
instEntry->instId = instId;
} else {
// Create the actual instance
instEntry =
(ObjectInstList *)
pvPortMalloc(sizeof(ObjectInstList));
if (instEntry == NULL)
return NULL;
instEntry->data = pvPortMalloc(obj->numBytes);
if (instEntry->data == NULL)
return NULL;
memset(instEntry->data, 0, obj->numBytes);
instEntry->instId = instId;
LL_APPEND(obj->instances.next, instEntry);
}
++obj->numInstances;
// Fire event
UAVObjInstanceUpdated((UAVObjHandle) obj, instId);
// Done
return instEntry;
}
/**
* Get the instance information or NULL if the instance does not exist
*/
static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId)
{
ObjectInstList *instEntry;
// Look for specified instance ID
LL_FOREACH(&(obj->instances), instEntry) {
if (instEntry->instId == instId) {
return instEntry;
}
}
// If this point is reached then instance id was not found
return NULL;
}
/**
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
* \param[in] obj The object handle
* \param[in] queue The event queue
* \param[in] cb The event callback
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask)
{
ObjectEventList *eventEntry;
ObjectList *objEntry;
// Check that the queue is not already connected, if it is simply update event mask
objEntry = (ObjectList *) obj;
LL_FOREACH(objEntry->events, eventEntry) {
if (eventEntry->queue == queue && eventEntry->cb == cb) {
// Already connected, update event mask and return
eventEntry->eventMask = eventMask;
return 0;
}
}
// Add queue to list
eventEntry =
(ObjectEventList *) pvPortMalloc(sizeof(ObjectEventList));
if (eventEntry == NULL) {
return -1;
}
eventEntry->queue = queue;
eventEntry->cb = cb;
eventEntry->eventMask = eventMask;
LL_APPEND(objEntry->events, eventEntry);
// Done
return 0;
}
/**
* Disconnect an event queue from the object
* \param[in] obj The object handle
* \param[in] queue The event queue
* \param[in] cb The event callback
* \return 0 if success or -1 if failure
*/
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb)
{
ObjectEventList *eventEntry;
ObjectList *objEntry;
// Find queue and remove it
objEntry = (ObjectList *) obj;
LL_FOREACH(objEntry->events, eventEntry) {
if ((eventEntry->queue == queue
&& eventEntry->cb == cb)) {
LL_DELETE(objEntry->events, eventEntry);
vPortFree(eventEntry);
return 0;
}
}
// If this point is reached the queue was not found
return -1;
}
#if defined(PIOS_INCLUDE_SDCARD)
/**
* Wrapper for the sprintf function
*/
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...)
{
va_list args;
va_start(args, format);
vsprintf((char *)buffer, (char *)format, args);
}
/**
* Get an 8 character (plus extension) filename for the object.
*/
static void objectFilename(ObjectList * obj, uint8_t * filename)
{
customSPrintf(filename, (uint8_t *) "%X.obj", obj->id);
}
#endif /* PIOS_INCLUDE_SDCARD */