mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
fa57e42efa
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1086 ebee16cc-31ac-478f-84a7-5cbb03baadba
1476 lines
36 KiB
C
1476 lines
36 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @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
|
|
|
|
/**
|
|
* List of event queues and the eventmask associated with the queue.
|
|
*/
|
|
struct ObjectEventListStruct {
|
|
xQueueHandle queue;
|
|
UAVObjEventCallback cb;
|
|
int32_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;
|
|
|
|
/**
|
|
* List of objects registered in the object manager
|
|
*/
|
|
struct ObjectListStruct {
|
|
uint32_t id; /** The object ID */
|
|
const char* name; /** The object name */
|
|
int8_t isMetaobject; /** Set to 1 if this is a metaobject */
|
|
int8_t isSingleInstance; /** Set to 1 if this object has a single instance */
|
|
int8_t isSettings; /** Set to 1 if this object is a settings object */
|
|
uint16_t numBytes; /** Number of data bytes contained in the object (for a single instance) */
|
|
uint16_t numInstances; /** Number of instances */
|
|
UAVObjInitializeCallback initCb; /** Object field and metadata initialization callback */
|
|
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, int32_t eventMask);
|
|
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb);
|
|
static void objectFilename(ObjectList* obj, uint8_t* filename);
|
|
static void customSPrintf(uint8_t* buffer, uint8_t* format, ...);
|
|
|
|
// 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)
|
|
defMetadata.access = ACCESS_READWRITE;
|
|
defMetadata.gcsAccess = ACCESS_READWRITE;
|
|
defMetadata.telemetryAcked = 1;
|
|
defMetadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
|
defMetadata.telemetryUpdatePeriod = 0;
|
|
defMetadata.gcsTelemetryAcked = 1;
|
|
defMetadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
|
defMetadata.gcsTelemetryUpdatePeriod = 0;
|
|
defMetadata.loggingUpdateMode = UPDATEMODE_ONCHANGE;
|
|
defMetadata.loggingUpdatePeriod = 0;
|
|
|
|
// 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;
|
|
objEntry->isMetaobject = (int8_t)isMetaobject;
|
|
objEntry->isSingleInstance = (int8_t)isSingleInstance;
|
|
objEntry->isSettings = (int8_t)isSettings;
|
|
objEntry->numBytes = numBytes;
|
|
objEntry->events = NULL;
|
|
objEntry->numInstances = 0;
|
|
objEntry->initCb = initCb;
|
|
objEntry->instances = 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 ( objEntry->initCb != NULL )
|
|
{
|
|
objEntry->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 ( !objEntry->isMetaobject )
|
|
{
|
|
UAVObjLoad( (UAVObjHandle)objEntry->linkedObj, 0 );
|
|
}
|
|
|
|
// If this is a settings object, attempt to load from SD card
|
|
if ( objEntry->isSettings )
|
|
{
|
|
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)
|
|
{
|
|
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 ( objEntry->initCb != NULL )
|
|
{
|
|
objEntry->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 ((ObjectList*)obj)->isSingleInstance;
|
|
}
|
|
|
|
/**
|
|
* Is this a metaobject?
|
|
* \param[in] obj The object handle
|
|
* \return True (1) if this is metaobject
|
|
*/
|
|
int32_t UAVObjIsMetaobject(UAVObjHandle obj)
|
|
{
|
|
return ((ObjectList*)obj)->isMetaobject;
|
|
}
|
|
|
|
/**
|
|
* 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 ((ObjectList*)obj)->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, 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)
|
|
{
|
|
uint32_t bytesWritten;
|
|
ObjectList* objEntry;
|
|
ObjectInstList* instEntry;
|
|
|
|
// Check for file system availability
|
|
if ( POIS_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 (!objEntry->isSingleInstance)
|
|
{
|
|
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);
|
|
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)
|
|
{
|
|
FILEINFO file;
|
|
ObjectList* objEntry;
|
|
uint8_t filename[14];
|
|
|
|
// Check for file system availability
|
|
if ( POIS_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);
|
|
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)
|
|
{
|
|
uint32_t bytesRead;
|
|
ObjectList* objEntry;
|
|
ObjectInstList* instEntry;
|
|
uint32_t objId;
|
|
uint16_t instId;
|
|
UAVObjHandle obj;
|
|
|
|
// Check for file system availability
|
|
if ( POIS_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 ( !objEntry->isSingleInstance )
|
|
{
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* 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)
|
|
{
|
|
FILEINFO file;
|
|
ObjectList* objEntry;
|
|
UAVObjHandle loadedObj;
|
|
ObjectList* loadedObjEntry;
|
|
uint8_t filename[14];
|
|
|
|
// Check for file system availability
|
|
if ( POIS_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);
|
|
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)
|
|
{
|
|
ObjectList* objEntry;
|
|
uint8_t filename[14];
|
|
|
|
// Check for file system availability
|
|
if ( POIS_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);
|
|
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 ( objEntry->isSettings )
|
|
{
|
|
// 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 ( objEntry->isSettings )
|
|
{
|
|
// 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 ( objEntry->isSettings )
|
|
{
|
|
// 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 ( objEntry->isMetaobject )
|
|
{
|
|
// 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 ( objEntry->isMetaobject )
|
|
{
|
|
// 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 ( objEntry->isMetaobject )
|
|
{
|
|
// 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);
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
}
|
|
|
|
/**
|
|
* 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 ( !objEntry->isMetaobject )
|
|
{
|
|
mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances->data);
|
|
if ( mdata->access == 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;
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
/**
|
|
* 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 (!objEntry->isMetaobject)
|
|
{
|
|
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 (objEntry->isMetaobject)
|
|
{
|
|
memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
|
|
}
|
|
else
|
|
{
|
|
UAVObjGetData((UAVObjHandle)objEntry->linkedObj, dataOut);
|
|
}
|
|
|
|
// Unlock
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* 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, int32_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, int32_t eventMask)
|
|
{
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = connectObj(obj, 0, cb, eventMask);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* Disconnect an event callback from the object.
|
|
* \param[in] obj The object handle
|
|
* \param[in] cb The event callback
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
|
|
{
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = disconnectObj(obj, 0, cb);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return res;
|
|
}
|
|
|
|
|
|
/**
|
|
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
|
|
* will be generated as soon as the object is updated.
|
|
* \param[in] obj The object handle
|
|
*/
|
|
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 (obj->isSingleInstance && 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;
|
|
}
|
|
}
|
|
|
|
// 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, 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, int32_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;
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|