mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-28 17:54:15 +01:00
599483d5ac
matches. Read the flash first bytewise to compute CRC instead of buffering which is more RAM efficient but very inefficient as it sets up many one byte SPI transfers. Also incremented the filesystem magic flag to trigger an automatic flash wipe on this upgrade.
1499 lines
39 KiB
C
1499 lines
39 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
|
|
|
|
/**
|
|
* 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 */
|
|
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);
|
|
|
|
#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)
|
|
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->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 (!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,
|
|
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 ((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)
|
|
{
|
|
#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 (!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);
|
|
#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 (!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;
|
|
#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 (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;
|
|
}
|
|
|
|
/**
|
|
* 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 (!objEntry->isMetaobject) {
|
|
mdata =
|
|
(UAVObjMetadata *) (objEntry->linkedObj->instances.
|
|
data);
|
|
return mdata->access == 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,
|
|
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;
|
|
}
|
|
}
|
|
|
|
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, 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;
|
|
}
|
|
|
|
#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 */
|