mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
f0fb22c090
The uavo_handles section is not part of the .data segment so it doesn't get automatically zeroed during the startup code. This led to random data being stored in the table which resulted in bus errors at runtime. This change ensures that the table is zeroed before we start to use it.
2012 lines
51 KiB
C
2012 lines
51 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"
|
|
#include "pios_struct_helper.h"
|
|
|
|
// Constants
|
|
|
|
// Private types
|
|
|
|
// Macros
|
|
#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);
|
|
|
|
/* Table of UAVO handles registered at compile time */
|
|
extern struct UAVOData * __start__uavo_handles[] __attribute__((weak));
|
|
extern struct UAVOData * __stop__uavo_handles[] __attribute__((weak));
|
|
|
|
#define UAVO_LIST_ITERATE(_item) \
|
|
for (struct UAVOData ** _uavo_slot = __start__uavo_handles; \
|
|
_uavo_slot && _uavo_slot < __stop__uavo_handles; \
|
|
_uavo_slot++) { \
|
|
struct UAVOData * _item = *_uavo_slot; \
|
|
if (_item == NULL) continue;
|
|
|
|
/**
|
|
* List of event queues and the eventmask associated with the queue.
|
|
*/
|
|
|
|
/** opaque type for instances **/
|
|
typedef void* InstanceHandle;
|
|
|
|
struct ObjectEventEntry {
|
|
xQueueHandle queue;
|
|
UAVObjEventCallback cb;
|
|
uint8_t eventMask;
|
|
struct ObjectEventEntry * next;
|
|
};
|
|
|
|
/*
|
|
MetaInstance == [UAVOBase [UAVObjMetadata]]
|
|
SingleInstance == [UAVOBase [UAVOData [InstanceData]]]
|
|
MultiInstance == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]]
|
|
____________________/
|
|
\-->[InstanceData1 [next]]
|
|
_________...________/
|
|
\-->[InstanceDataN [next]]
|
|
*/
|
|
|
|
/*
|
|
* UAVO Base Type
|
|
* - All Types of UAVObjects are of this base type
|
|
* - The flags determine what type(s) this object
|
|
*/
|
|
struct UAVOBase {
|
|
/* Let these objects be added to an event queue */
|
|
struct ObjectEventEntry * next_event;
|
|
|
|
/* Describe the type of object that follows this header */
|
|
struct UAVOInfo {
|
|
bool isMeta : 1;
|
|
bool isSingle : 1;
|
|
bool isSettings : 1;
|
|
} flags;
|
|
|
|
} __attribute__((packed));
|
|
|
|
/* Augmented type for Meta UAVO */
|
|
struct UAVOMeta {
|
|
struct UAVOBase base;
|
|
UAVObjMetadata instance0;
|
|
} __attribute__((packed));
|
|
|
|
/* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */
|
|
struct UAVOData {
|
|
struct UAVOBase base;
|
|
uint32_t id;
|
|
/*
|
|
* Embed the Meta object as another complete UAVO
|
|
* inside the payload for this UAVO.
|
|
*/
|
|
struct UAVOMeta metaObj;
|
|
uint16_t instance_size;
|
|
} __attribute__((packed));
|
|
|
|
/* Augmented type for Single Instance Data UAVO */
|
|
struct UAVOSingle {
|
|
struct UAVOData uavo;
|
|
|
|
uint8_t instance0[];
|
|
/*
|
|
* Additional space will be malloc'd here to hold the
|
|
* the data for this instance.
|
|
*/
|
|
} __attribute__((packed));
|
|
|
|
/* Part of a linked list of instances chained off of a multi instance UAVO. */
|
|
struct UAVOMultiInst {
|
|
struct UAVOMultiInst * next;
|
|
uint8_t instance[];
|
|
/*
|
|
* Additional space will be malloc'd here to hold the
|
|
* the data for this instance.
|
|
*/
|
|
} __attribute__((packed));
|
|
|
|
/* Augmented type for Multi Instance Data UAVO */
|
|
struct UAVOMulti {
|
|
struct UAVOData uavo;
|
|
|
|
uint16_t num_instances;
|
|
struct UAVOMultiInst instance0;
|
|
/*
|
|
* Additional space will be malloc'd here to hold the
|
|
* the data for instance 0.
|
|
*/
|
|
} __attribute__((packed));
|
|
|
|
/** all information about a metaobject are hardcoded constants **/
|
|
#define MetaNumBytes sizeof(UAVObjMetadata)
|
|
#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj)-offsetof(struct UAVOData, metaObj)))
|
|
#define MetaObjectPtr(obj) ((struct UAVODataMeta*) &((obj)->metaObj))
|
|
#define MetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->instance0))
|
|
#define LinkedMetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->metaObj.instance0))
|
|
#define MetaObjectId(id) ((id)+1)
|
|
|
|
/** all information about instances are dependant on object type **/
|
|
#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (struct UAVOSingle*)obj )->instance0)))
|
|
#define InstanceDataOffset(inst) ((void*)&(( (struct UAVOMultiInst*)inst )->instance))
|
|
#define InstanceData(instance) (void*)instance
|
|
|
|
// Private functions
|
|
static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
|
|
UAVObjEventType event);
|
|
static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId);
|
|
static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId);
|
|
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
|
|
UAVObjEventCallback cb, uint8_t eventMask);
|
|
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
|
|
UAVObjEventCallback cb);
|
|
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename);
|
|
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
|
|
#endif
|
|
|
|
// Private variables
|
|
static xSemaphoreHandle mutex;
|
|
static const UAVObjMetadata defMetadata = {
|
|
.flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
|
|
ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
|
|
1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
|
|
1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
|
|
UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
|
|
UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT),
|
|
.telemetryUpdatePeriod = 0,
|
|
.gcsTelemetryUpdatePeriod = 0,
|
|
.loggingUpdatePeriod = 0,
|
|
};
|
|
|
|
static UAVObjStats stats;
|
|
|
|
/**
|
|
* Initialize the object manager
|
|
* \return 0 Success
|
|
* \return -1 Failure
|
|
*/
|
|
int32_t UAVObjInitialize()
|
|
{
|
|
// Initialize variables
|
|
memset(&stats, 0, sizeof(UAVObjStats));
|
|
|
|
// Initialize the uavo handle table
|
|
memset(__start__uavo_handles, 0,
|
|
(uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles);
|
|
|
|
// Create mutex
|
|
mutex = xSemaphoreCreateRecursiveMutex();
|
|
if (mutex == NULL)
|
|
return -1;
|
|
|
|
// Done
|
|
return 0;
|
|
}
|
|
|
|
/*****************
|
|
* Statistics
|
|
****************/
|
|
|
|
/**
|
|
* Get the statistics counters
|
|
* @param[out] statsOut The statistics counters will be copied there
|
|
*/
|
|
void UAVObjGetStats(UAVObjStats * statsOut)
|
|
{
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
memcpy(statsOut, &stats, sizeof(UAVObjStats));
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
|
|
/**
|
|
* Clear the statistics counters
|
|
*/
|
|
void UAVObjClearStats()
|
|
{
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
memset(&stats, 0, sizeof(UAVObjStats));
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
|
|
/************************
|
|
* Object Initialization
|
|
***********************/
|
|
|
|
static void UAVObjInitMetaData (struct UAVOMeta * obj_meta)
|
|
{
|
|
/* Fill in the common part of the UAVO */
|
|
struct UAVOBase * uavo_base = &(obj_meta->base);
|
|
memset(uavo_base, 0, sizeof(*uavo_base));
|
|
uavo_base->flags.isMeta = true;
|
|
uavo_base->flags.isSingle = true;
|
|
uavo_base->next_event = NULL;
|
|
|
|
/* Clear the instance data carried in the UAVO */
|
|
memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0));
|
|
}
|
|
|
|
static struct UAVOData * UAVObjAllocSingle(uint32_t num_bytes)
|
|
{
|
|
/* Compute the complete size of the object, including the data for a single embedded instance */
|
|
uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes;
|
|
|
|
/* Allocate the object from the heap */
|
|
struct UAVOSingle * uavo_single = (struct UAVOSingle *) pvPortMalloc(object_size);
|
|
if (!uavo_single)
|
|
return (NULL);
|
|
|
|
/* Fill in the common part of the UAVO */
|
|
struct UAVOBase * uavo_base = &(uavo_single->uavo.base);
|
|
memset(uavo_base, 0, sizeof(*uavo_base));
|
|
uavo_base->flags.isSingle = true;
|
|
uavo_base->next_event = NULL;
|
|
|
|
/* Clear the instance data carried in the UAVO */
|
|
memset(&(uavo_single->instance0), 0, num_bytes);
|
|
|
|
/* Give back the generic UAVO part */
|
|
return (&(uavo_single->uavo));
|
|
}
|
|
|
|
static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes)
|
|
{
|
|
/* Compute the complete size of the object, including the data for a single embedded instance */
|
|
uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes;
|
|
|
|
/* Allocate the object from the heap */
|
|
struct UAVOMulti * uavo_multi = (struct UAVOMulti *) pvPortMalloc(object_size);
|
|
if (!uavo_multi)
|
|
return (NULL);
|
|
|
|
/* Fill in the common part of the UAVO */
|
|
struct UAVOBase * uavo_base = &(uavo_multi->uavo.base);
|
|
memset(uavo_base, 0, sizeof(*uavo_base));
|
|
uavo_base->flags.isSingle = false;
|
|
uavo_base->next_event = NULL;
|
|
|
|
/* Set up the type-specific part of the UAVO */
|
|
uavo_multi->num_instances = 1;
|
|
|
|
/* Clear the instance data carried in the UAVO */
|
|
memset (&(uavo_multi->instance0), 0, num_bytes);
|
|
|
|
/* Give back the generic UAVO part */
|
|
return (&(uavo_multi->uavo));
|
|
}
|
|
|
|
/**************************
|
|
* UAVObject Database APIs
|
|
*************************/
|
|
|
|
/**
|
|
* Register and new object in the object manager.
|
|
* \param[in] id Unique object ID
|
|
* \param[in] isSingleInstance Is this a single instance or multi-instance object
|
|
* \param[in] isSettings Is this a settings object
|
|
* \param[in] numBytes Number of bytes of object data (for one instance)
|
|
* \param[in] initCb Default field and metadata initialization function
|
|
* \return Object handle, or NULL if failure.
|
|
* \return
|
|
*/
|
|
UAVObjHandle UAVObjRegister(uint32_t id,
|
|
int32_t isSingleInstance, int32_t isSettings,
|
|
uint32_t num_bytes,
|
|
UAVObjInitializeCallback initCb)
|
|
{
|
|
struct UAVOData * uavo_data = NULL;
|
|
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
/* Don't allow duplicate registrations */
|
|
if (UAVObjGetByID(id))
|
|
goto unlock_exit;
|
|
|
|
/* Map the various flags to one of the UAVO types we understand */
|
|
if (isSingleInstance) {
|
|
uavo_data = UAVObjAllocSingle (num_bytes);
|
|
} else {
|
|
uavo_data = UAVObjAllocMulti (num_bytes);
|
|
}
|
|
|
|
if (!uavo_data)
|
|
goto unlock_exit;
|
|
|
|
/* Fill in the details about this UAVO */
|
|
uavo_data->id = id;
|
|
uavo_data->instance_size = num_bytes;
|
|
if (isSettings) {
|
|
uavo_data->base.flags.isSettings = true;
|
|
}
|
|
|
|
/* Initialize the embedded meta UAVO */
|
|
UAVObjInitMetaData (&uavo_data->metaObj);
|
|
|
|
/* Initialize object fields and metadata to default values */
|
|
if (initCb)
|
|
initCb((UAVObjHandle) uavo_data, 0);
|
|
|
|
/* Always try to load the meta object from flash */
|
|
UAVObjLoad((UAVObjHandle) &(uavo_data->metaObj), 0);
|
|
|
|
/* Attempt to load settings object from flash */
|
|
if (uavo_data->base.flags.isSettings)
|
|
UAVObjLoad((UAVObjHandle) uavo_data, 0);
|
|
|
|
// fire events for outer object and its embedded meta object
|
|
UAVObjInstanceUpdated((UAVObjHandle) uavo_data, 0);
|
|
UAVObjInstanceUpdated((UAVObjHandle) &(uavo_data->metaObj), 0);
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return (UAVObjHandle) uavo_data;
|
|
}
|
|
|
|
/**
|
|
* Retrieve an object from the list given its id
|
|
* \param[in] The object ID
|
|
* \return The object or NULL if not found.
|
|
*/
|
|
UAVObjHandle UAVObjGetByID(uint32_t id)
|
|
{
|
|
UAVObjHandle * found_obj = (UAVObjHandle *) NULL;
|
|
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Look for object
|
|
UAVO_LIST_ITERATE(tmp_obj)
|
|
if (tmp_obj->id == id) {
|
|
found_obj = (UAVObjHandle *)tmp_obj;
|
|
goto unlock_exit;
|
|
}
|
|
if (MetaObjectId(tmp_obj->id) == id) {
|
|
found_obj = (UAVObjHandle *)&(tmp_obj->metaObj);
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return found_obj;
|
|
}
|
|
|
|
/**
|
|
* Get the object's ID
|
|
* \param[in] obj The object handle
|
|
* \return The object ID
|
|
*/
|
|
uint32_t UAVObjGetID(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
/* Recover the common object header */
|
|
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
/* We have a meta object, find our containing UAVO */
|
|
struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
|
|
|
|
return MetaObjectId (uavo_data->id);
|
|
} else {
|
|
/* We have a data object, augment our pointer */
|
|
struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;
|
|
|
|
return (uavo_data->id);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the number of bytes of the object's data (for one instance)
|
|
* \param[in] obj The object handle
|
|
* \return The number of bytes
|
|
*/
|
|
uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
|
|
{
|
|
PIOS_Assert(obj);
|
|
|
|
uint32_t instance_size;
|
|
|
|
/* Recover the common object header */
|
|
struct UAVOBase * uavo_base = (struct UAVOBase *) obj;
|
|
|
|
if (uavo_base->flags.isMeta) {
|
|
instance_size = MetaNumBytes;
|
|
} else {
|
|
/* We have a data object, augment our pointer */
|
|
struct UAVOData * uavo = (struct UAVOData *) uavo_base;
|
|
|
|
instance_size = uavo->instance_size;
|
|
}
|
|
|
|
return (instance_size);
|
|
}
|
|
|
|
/**
|
|
* Get the object this object is linked to. For regular objects, the linked object
|
|
* is the metaobject. For metaobjects the linked object is the parent object.
|
|
* This function is normally only needed by the telemetry module.
|
|
* \param[in] obj The object handle
|
|
* \return The object linked object handle
|
|
*/
|
|
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
/* Recover the common object header */
|
|
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
/* We have a meta object, find our containing UAVO. */
|
|
struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
|
|
|
|
return (UAVObjHandle) uavo_data;
|
|
} else {
|
|
/* We have a data object, augment our pointer */
|
|
struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;
|
|
|
|
return (UAVObjHandle) &(uavo_data->metaObj);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the number of instances contained in the object.
|
|
* \param[in] obj The object handle
|
|
* \return The number of instances
|
|
*/
|
|
uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
if (UAVObjIsSingleInstance(obj_handle)) {
|
|
/* Only one instance is allowed */
|
|
return 1;
|
|
} else {
|
|
/* Multi-instance object. Inspect the object */
|
|
/* Augment our pointer to reflect the proper type */
|
|
struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj_handle;
|
|
return uavo_multi->num_instances;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Create a new instance in the object.
|
|
* \param[in] obj The object handle
|
|
* \return The instance ID or 0 if an error
|
|
*/
|
|
uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle,
|
|
UAVObjInitializeCallback initCb)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
return 0;
|
|
}
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
InstanceHandle instEntry;
|
|
uint16_t instId = 0;
|
|
|
|
// Create new instance
|
|
instId = UAVObjGetNumInstances(obj_handle);
|
|
instEntry = createInstance( (struct UAVOData *)obj_handle, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Initialize instance data
|
|
if (initCb) {
|
|
initCb(obj_handle, instId);
|
|
}
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
|
|
return instId;
|
|
}
|
|
|
|
/**
|
|
* Does this object contains a single instance or multiple instances?
|
|
* \param[in] obj The object handle
|
|
* \return True (1) if this is a single instance object
|
|
*/
|
|
bool UAVObjIsSingleInstance(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
/* Recover the common object header */
|
|
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
|
|
|
|
return uavo_base->flags.isSingle;
|
|
}
|
|
|
|
/**
|
|
* Is this a metaobject?
|
|
* \param[in] obj The object handle
|
|
* \return True (1) if this is metaobject
|
|
*/
|
|
bool UAVObjIsMetaobject(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
/* Recover the common object header */
|
|
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
|
|
|
|
return uavo_base->flags.isMeta;
|
|
}
|
|
|
|
/**
|
|
* Is this a settings object?
|
|
* \param[in] obj The object handle
|
|
* \return True (1) if this is a settings object
|
|
*/
|
|
bool UAVObjIsSettings(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
/* Recover the common object header */
|
|
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
|
|
|
|
return uavo_base->flags.isSettings;
|
|
}
|
|
|
|
/**
|
|
* Unpack an object from a byte array
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The instance ID
|
|
* \param[in] dataIn The byte array
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId,
|
|
const uint8_t * dataIn)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
|
|
} else {
|
|
struct UAVOData *obj;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast handle to object
|
|
obj = (struct UAVOData *) obj_handle;
|
|
|
|
// Get the instance
|
|
instEntry = getInstance(obj, instId);
|
|
|
|
// If the instance does not exist create it and any other instances before it
|
|
if (instEntry == NULL) {
|
|
instEntry = createInstance(obj, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
// Set the data
|
|
memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
|
|
}
|
|
|
|
// Fire event
|
|
sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Pack an object to a byte array
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The instance ID
|
|
* \param[out] dataOut The byte array
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t * dataOut)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
|
|
} else {
|
|
struct UAVOData *obj;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast handle to object
|
|
obj = (struct UAVOData *) obj_handle;
|
|
|
|
// Get the instance
|
|
instEntry = getInstance(obj, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
// Pack data
|
|
memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
/**
|
|
* Save the data of the specified object instance to the file system (SD card).
|
|
* The object will be appended and the file will not be closed.
|
|
* The object data can be restored using the UAVObjLoad function.
|
|
* @param[in] obj The object handle.
|
|
* @param[in] instId The instance ID
|
|
* @param[in] file File to append to
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId,
|
|
FILEINFO * file)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
uint32_t bytesWritten;
|
|
// Check for file system availability
|
|
if (PIOS_SDCARD_IsMounted() == 0) {
|
|
return -1;
|
|
}
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
// Get the instance information
|
|
if (instId != 0) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Write the object ID
|
|
uint32_t objId = UAVObjGetID(obj_handle);
|
|
PIOS_FWRITE(file, &objId, sizeof(objId),
|
|
&bytesWritten);
|
|
|
|
// Write the data and check that the write was successful
|
|
PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes,
|
|
&bytesWritten);
|
|
if (bytesWritten != MetaNumBytes) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
} else {
|
|
struct UAVOData * uavo;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast to object
|
|
uavo = (struct UAVOData *) obj_handle;
|
|
|
|
// Get the instance information
|
|
instEntry = getInstance(uavo, instId);
|
|
if (instEntry == NULL) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Write the object ID
|
|
PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id),
|
|
&bytesWritten);
|
|
|
|
// Write the instance ID
|
|
if (!UAVObjIsSingleInstance(obj_handle)) {
|
|
PIOS_FWRITE(file, &instId,
|
|
sizeof(instId), &bytesWritten);
|
|
}
|
|
// Write the data and check that the write was successful
|
|
PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size,
|
|
&bytesWritten);
|
|
if (bytesWritten != uavo->instance_size) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
}
|
|
// Done
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return 0;
|
|
}
|
|
#endif /* PIOS_INCLUDE_SDCARD */
|
|
|
|
/**
|
|
* Save the data of the specified object to the file system (SD card).
|
|
* If the object contains multiple instances, all of them will be saved.
|
|
* A new file with the name of the object will be created.
|
|
* The object data can be restored using the UAVObjLoad function.
|
|
* @param[in] obj The object handle.
|
|
* @param[in] instId The instance ID
|
|
* @param[in] file File to append to
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
if (instId != 0)
|
|
return -1;
|
|
|
|
if (PIOS_FLASHFS_ObjSave(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) != 0)
|
|
return -1;
|
|
} else {
|
|
InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);
|
|
|
|
if (instEntry == NULL)
|
|
return -1;
|
|
|
|
if (InstanceData(instEntry) == NULL)
|
|
return -1;
|
|
|
|
if (PIOS_FLASHFS_ObjSave(obj_handle, instId, InstanceData(instEntry)) != 0)
|
|
return -1;
|
|
}
|
|
#endif
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
FILEINFO file;
|
|
uint8_t filename[14];
|
|
|
|
// Check for file system availability
|
|
if (PIOS_SDCARD_IsMounted() == 0) {
|
|
return -1;
|
|
}
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Get filename
|
|
objectFilename(obj_handle, filename);
|
|
|
|
// Open file
|
|
if (PIOS_FOPEN_WRITE(filename, file)) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Append object
|
|
if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) {
|
|
PIOS_FCLOSE(file);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Done, close file and unlock
|
|
PIOS_FCLOSE(file);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
#endif /* PIOS_INCLUDE_SDCARD */
|
|
return 0;
|
|
}
|
|
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
/**
|
|
* Load an object from the file system (SD card).
|
|
* @param[in] file File to read from
|
|
* @return The handle of the object loaded or NULL if a failure
|
|
*/
|
|
UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
|
|
{
|
|
uint32_t bytesRead;
|
|
struct UAVOBase *objEntry;
|
|
InstanceHandle instEntry;
|
|
uint32_t objId;
|
|
uint16_t instId;
|
|
UAVObjHandle obj_handle;
|
|
|
|
// Check for file system availability
|
|
if (PIOS_SDCARD_IsMounted() == 0) {
|
|
return NULL;
|
|
}
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Read the object ID
|
|
if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
// Get the object
|
|
obj_handle = UAVObjGetByID(objId);
|
|
if (obj_handle == 0) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
objEntry = (struct UAVOBase *) obj_handle;
|
|
|
|
// Get the instance ID
|
|
instId = 0;
|
|
if (!UAVObjIsSingleInstance(obj_handle)) {
|
|
if (PIOS_FREAD
|
|
(file, &instId, sizeof(instId), &bytesRead)) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
// If the instance does not exist create it and any other instances before it
|
|
if (instId != 0) {
|
|
// Error, unlock and return
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
// Read the instance data
|
|
if (PIOS_FREAD
|
|
(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
} else {
|
|
|
|
// Get the instance information
|
|
instEntry = getInstance((struct UAVOData *)objEntry, instId);
|
|
|
|
// If the instance does not exist create it and any other instances before it
|
|
if (instEntry == NULL) {
|
|
instEntry = createInstance((struct UAVOData *)objEntry, instId);
|
|
if (instEntry == NULL) {
|
|
// Error, unlock and return
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
}
|
|
// Read the instance data
|
|
if (PIOS_FREAD
|
|
(file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
|
|
}
|
|
|
|
// Fire event
|
|
sendEvent(objEntry, instId, EV_UNPACKED);
|
|
|
|
// Unlock
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return obj_handle;
|
|
}
|
|
#endif /* PIOS_INCLUDE_SDCARD */
|
|
|
|
/**
|
|
* Load an object from the file system (SD card).
|
|
* A file with the name of the object will be opened.
|
|
* The object data can be saved using the UAVObjSave function.
|
|
* @param[in] obj The object handle.
|
|
* @param[in] instId The object instance
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
if (instId != 0)
|
|
return -1;
|
|
|
|
// Fire event on success
|
|
if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) == 0)
|
|
sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
|
|
else
|
|
return -1;
|
|
} else {
|
|
|
|
InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);
|
|
|
|
if (instEntry == NULL)
|
|
return -1;
|
|
|
|
// Fire event on success
|
|
if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, InstanceData(instEntry)) == 0)
|
|
sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
|
|
else
|
|
return -1;
|
|
}
|
|
|
|
#endif
|
|
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
FILEINFO file;
|
|
UAVObjHandle loadedObj;
|
|
uint8_t filename[14];
|
|
|
|
// Check for file system availability
|
|
if (PIOS_SDCARD_IsMounted() == 0) {
|
|
return -1;
|
|
}
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Get filename
|
|
objectFilename(obj_handle, filename);
|
|
|
|
// Open file
|
|
if (PIOS_FOPEN_READ(filename, file)) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Load object
|
|
loadedObj = UAVObjLoadFromFile(&file);
|
|
if (loadedObj == 0) {
|
|
PIOS_FCLOSE(file);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Check that the IDs match
|
|
if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) {
|
|
PIOS_FCLOSE(file);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return -1;
|
|
}
|
|
// Done, close file and unlock
|
|
PIOS_FCLOSE(file);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
#endif /* PIOS_INCLUDE_SDCARD */
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Delete an object from the file system (SD card).
|
|
* @param[in] obj The object handle.
|
|
* @param[in] instId The object instance
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
|
|
PIOS_FLASHFS_ObjDelete(obj_handle, instId);
|
|
#endif
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
uint8_t filename[14];
|
|
|
|
// Check for file system availability
|
|
if (PIOS_SDCARD_IsMounted() == 0) {
|
|
return -1;
|
|
}
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Get filename
|
|
objectFilename(obj_handle, filename);
|
|
|
|
// Delete file
|
|
PIOS_FUNLINK(filename);
|
|
|
|
// Done
|
|
xSemaphoreGiveRecursive(mutex);
|
|
#endif /* PIOS_INCLUDE_SDCARD */
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Save all settings objects to the SD card.
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSaveSettings()
|
|
{
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
// Save all settings objects
|
|
UAVO_LIST_ITERATE(obj)
|
|
// Check if this is a settings object
|
|
if (UAVObjIsSettings(obj)) {
|
|
// Save object
|
|
if (UAVObjSave((UAVObjHandle) obj, 0) ==
|
|
-1) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Load all settings objects from the SD card.
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjLoadSettings()
|
|
{
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
// Load all settings objects
|
|
UAVO_LIST_ITERATE(obj)
|
|
// Check if this is a settings object
|
|
if (UAVObjIsSettings(obj)) {
|
|
// Load object
|
|
if (UAVObjLoad((UAVObjHandle) obj, 0) ==
|
|
-1) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Delete all settings objects from the SD card.
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjDeleteSettings()
|
|
{
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
// Save all settings objects
|
|
UAVO_LIST_ITERATE(obj)
|
|
// Check if this is a settings object
|
|
if (UAVObjIsSettings(obj)) {
|
|
// Save object
|
|
if (UAVObjDelete((UAVObjHandle) obj, 0)
|
|
== -1) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Save all metaobjects to the SD card.
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSaveMetaobjects()
|
|
{
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
// Save all settings objects
|
|
UAVO_LIST_ITERATE(obj)
|
|
// Save object
|
|
if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) ==
|
|
-1) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Load all metaobjects from the SD card.
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjLoadMetaobjects()
|
|
{
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
// Load all settings objects
|
|
UAVO_LIST_ITERATE(obj)
|
|
// Load object
|
|
if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) ==
|
|
-1) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Delete all metaobjects from the SD card.
|
|
* @return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjDeleteMetaobjects()
|
|
{
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
// Load all settings objects
|
|
UAVO_LIST_ITERATE(obj)
|
|
// Load object
|
|
if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0)
|
|
== -1) {
|
|
goto unlock_exit;
|
|
}
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Set the object data
|
|
* \param[in] obj The object handle
|
|
* \param[in] dataIn The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn)
|
|
{
|
|
return UAVObjSetInstanceData(obj_handle, 0, dataIn);
|
|
}
|
|
|
|
/**
|
|
* Set the object data
|
|
* \param[in] obj The object handle
|
|
* \param[in] dataIn The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size)
|
|
{
|
|
return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size);
|
|
}
|
|
|
|
/**
|
|
* Get the object data
|
|
* \param[in] obj The object handle
|
|
* \param[out] dataOut The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut)
|
|
{
|
|
return UAVObjGetInstanceData(obj_handle, 0, dataOut);
|
|
}
|
|
|
|
/**
|
|
* Get the object data
|
|
* \param[in] obj The object handle
|
|
* \param[out] dataOut The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size)
|
|
{
|
|
return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size);
|
|
}
|
|
|
|
/**
|
|
* Set the data of a specific object instance
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
* \param[in] dataIn The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
|
|
const void *dataIn)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
|
|
} else {
|
|
struct UAVOData *obj;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast to object info
|
|
obj = (struct UAVOData *) obj_handle;
|
|
|
|
// Check access level
|
|
if (UAVObjReadOnly(obj_handle)) {
|
|
goto unlock_exit;
|
|
}
|
|
// Get instance information
|
|
instEntry = getInstance(obj, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
// Set data
|
|
memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
|
|
}
|
|
|
|
// Fire event
|
|
sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Set the data of a specific object instance
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
* \param[in] dataIn The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
// Get instance information
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Check for overrun
|
|
if ((size + offset) > MetaNumBytes) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Set data
|
|
memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size);
|
|
} else {
|
|
struct UAVOData * obj;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast to object info
|
|
obj = (struct UAVOData *)obj_handle;
|
|
|
|
// Check access level
|
|
if (UAVObjReadOnly(obj_handle)) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Get instance information
|
|
instEntry = getInstance(obj, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Check for overrun
|
|
if ((size + offset) > obj->instance_size) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Set data
|
|
memcpy(InstanceData(instEntry) + offset, dataIn, size);
|
|
}
|
|
|
|
|
|
// Fire event
|
|
sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Get the data of a specific object instance
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
* \param[out] dataOut The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
|
|
void *dataOut)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
// Get instance information
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
// Set data
|
|
memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
|
|
} else {
|
|
struct UAVOData *obj;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast to object info
|
|
obj = (struct UAVOData *) obj_handle;
|
|
|
|
// Get instance information
|
|
instEntry = getInstance(obj, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
// Set data
|
|
memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Get the data of a specific object instance
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
* \param[out] dataOut The object's data structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
// Get instance information
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Check for overrun
|
|
if ((size + offset) > MetaNumBytes) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Set data
|
|
memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size);
|
|
} else {
|
|
struct UAVOData * obj;
|
|
InstanceHandle instEntry;
|
|
|
|
// Cast to object info
|
|
obj = (struct UAVOData *)obj_handle;
|
|
|
|
// Get instance information
|
|
instEntry = getInstance(obj, instId);
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Check for overrun
|
|
if ((size + offset) > obj->instance_size) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Set data
|
|
memcpy(dataOut, InstanceData(instEntry) + offset, size);
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Set the object metadata
|
|
* \param[in] obj The object handle
|
|
* \param[in] dataIn The object's metadata structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Set metadata (metadata of metaobjects can not be modified)
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
return -1;
|
|
}
|
|
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
UAVObjSetData((UAVObjHandle) MetaObjectPtr((struct UAVOData *)obj_handle), dataIn);
|
|
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Get the object metadata
|
|
* \param[in] obj The object handle
|
|
* \param[out] dataOut The object's metadata structure
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Get metadata
|
|
if (UAVObjIsMetaobject(obj_handle)) {
|
|
memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
|
|
} else {
|
|
UAVObjGetData((UAVObjHandle) MetaObjectPtr( (struct UAVOData *)obj_handle ),
|
|
dataOut);
|
|
}
|
|
|
|
// Unlock
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return 0;
|
|
}
|
|
|
|
/*******************************
|
|
* Object Metadata Manipulation
|
|
******************************/
|
|
|
|
/**
|
|
* Get the UAVObject metadata access member
|
|
* \param[in] metadata The metadata object
|
|
* \return the access type
|
|
*/
|
|
UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
|
|
{
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata access member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] mode The access mode
|
|
*/
|
|
void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
|
|
{
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1);
|
|
}
|
|
|
|
/**
|
|
* Get the UAVObject metadata GCS access member
|
|
* \param[in] metadata The metadata object
|
|
* \return the GCS access type
|
|
*/
|
|
UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
|
|
{
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata GCS access member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] mode The access mode
|
|
*/
|
|
void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1);
|
|
}
|
|
|
|
/**
|
|
* Get the UAVObject metadata telemetry acked member
|
|
* \param[in] metadata The metadata object
|
|
* \return the telemetry acked boolean
|
|
*/
|
|
uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata telemetry acked member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] val The telemetry acked boolean
|
|
*/
|
|
void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1);
|
|
}
|
|
|
|
/**
|
|
* Get the UAVObject metadata GCS telemetry acked member
|
|
* \param[in] metadata The metadata object
|
|
* \return the telemetry acked boolean
|
|
*/
|
|
uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata GCS telemetry acked member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] val The GCS telemetry acked boolean
|
|
*/
|
|
void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1);
|
|
}
|
|
|
|
/**
|
|
* Get the UAVObject metadata telemetry update mode
|
|
* \param[in] metadata The metadata object
|
|
* \return the telemetry update mode
|
|
*/
|
|
UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata telemetry update mode member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] val The telemetry update mode
|
|
*/
|
|
void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
|
|
}
|
|
|
|
/**
|
|
* Get the UAVObject metadata GCS telemetry update mode
|
|
* \param[in] metadata The metadata object
|
|
* \return the GCS telemetry update mode
|
|
*/
|
|
UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) {
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata GCS telemetry update mode member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] val The GCS telemetry update mode
|
|
*/
|
|
void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
|
|
}
|
|
|
|
|
|
/**
|
|
* Check if an object is read only
|
|
* \param[in] obj The object handle
|
|
* \return
|
|
* \arg 0 if not read only
|
|
* \arg 1 if read only
|
|
* \arg -1 if unable to get meta data
|
|
*/
|
|
int8_t UAVObjReadOnly(UAVObjHandle obj_handle)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
if (!UAVObjIsMetaobject(obj_handle)) {
|
|
return UAVObjGetAccess(LinkedMetaDataPtr( (struct UAVOData *)obj_handle)) == ACCESS_READONLY;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/**
|
|
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
|
|
* All events matching the event mask will be pushed to the event queue.
|
|
* \param[in] obj The object handle
|
|
* \param[in] queue The event queue
|
|
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue,
|
|
uint8_t eventMask)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
PIOS_Assert(queue);
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = connectObj(obj_handle, queue, 0, eventMask);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* Disconnect an event queue from the object.
|
|
* \param[in] obj The object handle
|
|
* \param[in] queue The event queue
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
PIOS_Assert(queue);
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = disconnectObj(obj_handle, queue, 0);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* Connect an event callback to the object, if the callback is already connected then the event mask is only updated.
|
|
* The supplied callback will be invoked on all events matching the event mask.
|
|
* \param[in] obj The object handle
|
|
* \param[in] cb The event callback
|
|
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb,
|
|
uint8_t eventMask)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = connectObj(obj_handle, 0, cb, eventMask);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* Disconnect an event callback from the object.
|
|
* \param[in] obj The object handle
|
|
* \param[in] cb The event callback
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = disconnectObj(obj_handle, 0, cb);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
|
|
* will be generated as soon as the object is updated.
|
|
* \param[in] obj The object handle
|
|
*/
|
|
void UAVObjRequestUpdate(UAVObjHandle obj_handle)
|
|
{
|
|
UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES);
|
|
}
|
|
|
|
/**
|
|
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
|
|
* will be generated as soon as the object is updated.
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId Object instance ID to update
|
|
*/
|
|
void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATE_REQ);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
|
|
/**
|
|
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
|
|
* \param[in] obj The object handle
|
|
*/
|
|
void UAVObjUpdated(UAVObjHandle obj_handle)
|
|
{
|
|
UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES);
|
|
}
|
|
|
|
/**
|
|
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
*/
|
|
void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATED_MANUAL);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
|
|
/**
|
|
* Iterate through all objects in the list.
|
|
* \param iterator This function will be called once for each object,
|
|
* the object will be passed as a parameter
|
|
*/
|
|
void UAVObjIterate(void (*iterator) (UAVObjHandle obj))
|
|
{
|
|
PIOS_Assert(iterator);
|
|
|
|
// Get lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// Iterate through the list and invoke iterator for each object
|
|
UAVO_LIST_ITERATE(obj)
|
|
(*iterator) ((UAVObjHandle) obj);
|
|
(*iterator) ((UAVObjHandle) &obj->metaObj);
|
|
}
|
|
|
|
// Release lock
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
|
|
/**
|
|
* Send a triggered event to all event queues registered on the object.
|
|
*/
|
|
static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
|
|
UAVObjEventType triggered_event)
|
|
{
|
|
/* Set up the message that will be sent to all registered listeners */
|
|
UAVObjEvent msg = {
|
|
.obj = (UAVObjHandle) obj,
|
|
.event = triggered_event,
|
|
.instId = instId,
|
|
};
|
|
|
|
// Go through each object and push the event message in the queue (if event is activated for the queue)
|
|
struct ObjectEventEntry *event;
|
|
LL_FOREACH(obj->next_event, event) {
|
|
if (event->eventMask == 0
|
|
|| (event->eventMask & triggered_event) != 0) {
|
|
// Send to queue if a valid queue is registered
|
|
if (event->queue) {
|
|
// will not block
|
|
if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
|
|
stats.lastQueueErrorID = UAVObjGetID(obj);
|
|
++stats.eventQueueErrors;
|
|
}
|
|
}
|
|
|
|
// Invoke callback (from event task) if a valid one is registered
|
|
if (event->cb) {
|
|
// invoke callback from the event task, will not block
|
|
if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) {
|
|
++stats.eventCallbackErrors;
|
|
stats.lastCallbackErrorID = UAVObjGetID(obj);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Create a new object instance, return the instance info or NULL if failure.
|
|
*/
|
|
static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId)
|
|
{
|
|
struct UAVOMultiInst *instEntry;
|
|
|
|
/* Don't allow more than one instance for single instance objects */
|
|
if (UAVObjIsSingleInstance(&(obj->base))) {
|
|
PIOS_Assert(0);
|
|
return NULL;
|
|
}
|
|
|
|
/* Don't create more than the allowed number of instances */
|
|
if (instId >= UAVOBJ_MAX_INSTANCES) {
|
|
return NULL;
|
|
}
|
|
|
|
/* Don't allow duplicate instances */
|
|
if (instId < UAVObjGetNumInstances(&(obj->base))) {
|
|
return NULL;
|
|
}
|
|
|
|
// Create any missing instances (all instance IDs must be sequential)
|
|
for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) {
|
|
if (createInstance(obj, n) == NULL) {
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
/* Create the actual instance */
|
|
instEntry = (struct UAVOMultiInst *) pvPortMalloc(sizeof(struct UAVOMultiInst)+obj->instance_size);
|
|
if (!instEntry)
|
|
return NULL;
|
|
memset(InstanceDataOffset(instEntry), 0, obj->instance_size);
|
|
LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry);
|
|
|
|
( (struct UAVOMulti*)obj )->num_instances++;
|
|
|
|
// Fire event
|
|
UAVObjInstanceUpdated((UAVObjHandle) obj, instId);
|
|
|
|
// Done
|
|
return InstanceDataOffset(instEntry);
|
|
}
|
|
|
|
/**
|
|
* Get the instance information or NULL if the instance does not exist
|
|
*/
|
|
static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId)
|
|
{
|
|
if (UAVObjIsMetaobject(&obj->base)) {
|
|
/* Metadata Instance */
|
|
|
|
if (instId != 0)
|
|
return NULL;
|
|
|
|
/* Augment our pointer to reflect the proper type */
|
|
struct UAVOMeta * uavo_meta = (struct UAVOMeta *) obj;
|
|
return (&(uavo_meta->instance0));
|
|
|
|
} else if (UAVObjIsSingleInstance(&(obj->base))) {
|
|
/* Single Instance */
|
|
|
|
if (instId != 0)
|
|
return NULL;
|
|
|
|
/* Augment our pointer to reflect the proper type */
|
|
struct UAVOSingle * uavo_single = (struct UAVOSingle *) obj;
|
|
return (&(uavo_single->instance0));
|
|
} else {
|
|
/* Multi Instance */
|
|
/* Augment our pointer to reflect the proper type */
|
|
struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj;
|
|
if (instId >= uavo_multi->num_instances)
|
|
return NULL;
|
|
|
|
// Look for specified instance ID
|
|
uint16_t instance = 0;
|
|
struct UAVOMultiInst *instEntry;
|
|
LL_FOREACH(&(uavo_multi->instance0), instEntry) {
|
|
if (instance++ == instId) {
|
|
/* Found it */
|
|
return &(instEntry->instance);
|
|
}
|
|
}
|
|
/* Instance was not found */
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
|
|
* \param[in] obj The object handle
|
|
* \param[in] queue The event queue
|
|
* \param[in] cb The event callback
|
|
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
|
|
UAVObjEventCallback cb, uint8_t eventMask)
|
|
{
|
|
struct ObjectEventEntry *event;
|
|
struct UAVOBase *obj;
|
|
|
|
// Check that the queue is not already connected, if it is simply update event mask
|
|
obj = (struct UAVOBase *) obj_handle;
|
|
LL_FOREACH(obj->next_event, event) {
|
|
if (event->queue == queue && event->cb == cb) {
|
|
// Already connected, update event mask and return
|
|
event->eventMask = eventMask;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// Add queue to list
|
|
event = (struct ObjectEventEntry *) pvPortMalloc(sizeof(struct ObjectEventEntry));
|
|
if (event == NULL) {
|
|
return -1;
|
|
}
|
|
event->queue = queue;
|
|
event->cb = cb;
|
|
event->eventMask = eventMask;
|
|
LL_APPEND(obj->next_event, event);
|
|
|
|
// Done
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Disconnect an event queue from the object
|
|
* \param[in] obj The object handle
|
|
* \param[in] queue The event queue
|
|
* \param[in] cb The event callback
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
|
|
UAVObjEventCallback cb)
|
|
{
|
|
struct ObjectEventEntry *event;
|
|
struct UAVOBase *obj;
|
|
|
|
// Find queue and remove it
|
|
obj = (struct UAVOBase *) obj_handle;
|
|
LL_FOREACH(obj->next_event, event) {
|
|
if ((event->queue == queue
|
|
&& event->cb == cb)) {
|
|
LL_DELETE(obj->next_event, event);
|
|
vPortFree(event);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// If this point is reached the queue was not found
|
|
return -1;
|
|
}
|
|
|
|
#if defined(PIOS_INCLUDE_SDCARD)
|
|
/**
|
|
* Wrapper for the sprintf function
|
|
*/
|
|
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...)
|
|
{
|
|
va_list args;
|
|
va_start(args, format);
|
|
vsprintf((char *)buffer, (char *)format, args);
|
|
}
|
|
|
|
/**
|
|
* Get an 8 character (plus extension) filename for the object.
|
|
*/
|
|
static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename)
|
|
{
|
|
customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle));
|
|
}
|
|
#endif /* PIOS_INCLUDE_SDCARD */
|