mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-05 13:24:11 +01:00
1769 lines
48 KiB
C
1769 lines
48 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"
|
|
#include "inc/uavobjectprivate.h"
|
|
|
|
// Private functions
|
|
static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId);
|
|
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask, bool fast);
|
|
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb);
|
|
static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId);
|
|
|
|
|
|
int32_t UAVObjPers_stub(__attribute__((unused)) UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId)
|
|
{
|
|
return 0;
|
|
}
|
|
int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId) __attribute__((weak, alias("UAVObjPers_stub")));;
|
|
int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId) __attribute__((weak, alias("UAVObjPers_stub")));
|
|
int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId) __attribute__((weak, alias("UAVObjPers_stub")));
|
|
|
|
|
|
// 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;
|
|
|
|
|
|
static inline bool IsMetaobject(UAVObjHandle obj_handle)
|
|
{
|
|
/* Recover the common object header */
|
|
struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
|
|
|
|
return uavo_base->flags.isMeta;
|
|
}
|
|
|
|
static inline bool IsSingleInstance(UAVObjHandle obj_handle)
|
|
{
|
|
/* Recover the common object header */
|
|
struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
|
|
|
|
return uavo_base->flags.isSingle;
|
|
}
|
|
|
|
static inline bool IsSettings(UAVObjHandle obj_handle)
|
|
{
|
|
/* Recover the common object header */
|
|
struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
|
|
|
|
return uavo_base->flags.isSettings;
|
|
}
|
|
|
|
static inline bool IsPriority(UAVObjHandle obj_handle)
|
|
{
|
|
/* Recover the common object header */
|
|
struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
|
|
|
|
return uavo_base->flags.isPriority;
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
return IsMetaobject(obj_handle);
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
return IsSingleInstance(obj_handle);
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
return IsSettings(obj_handle);
|
|
}
|
|
|
|
|
|
/**
|
|
* Is this a prioritized object?
|
|
* \param[in] obj The object handle
|
|
* \return True (1) if this is a prioritized object
|
|
*/
|
|
bool UAVObjIsPriority(UAVObjHandle obj_handle)
|
|
{
|
|
return IsPriority(obj_handle);
|
|
}
|
|
|
|
|
|
/**
|
|
* Initialize the object manager
|
|
* \return 0 Success
|
|
* \return -1 Failure
|
|
*/
|
|
int32_t UAVObjInitialize()
|
|
{
|
|
// Initialize variables
|
|
memset(&stats, 0, sizeof(UAVObjStats));
|
|
|
|
/* Initialize _uavo_handles start/stop pointers */
|
|
#if (defined(__MACH__) && defined(__APPLE__))
|
|
uint64_t aslr_offset = (uint64_t)&_aslr_offset - getsectbyname("__DATA", "_aslr")->addr;
|
|
__start__uavo_handles = (struct UAVOData * *)(getsectbyname("__DATA", "_uavo_handles")->addr + aslr_offset);
|
|
__stop__uavo_handles = (struct UAVOData * *)((uint64_t)__start__uavo_handles + getsectbyname("__DATA", "_uavo_handles")->size);
|
|
#endif
|
|
|
|
// 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 *)pios_malloc(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 *)pios_malloc(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 multi instance data carried in the UAVO */
|
|
memset(&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + 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] pointer to UAVObjType structure that holds Unique object ID, instance size, initialization function
|
|
* \param[in] isSingleInstance Is this a single instance or multi-instance object
|
|
* \param[in] isSettings Is this a settings object
|
|
* \param[in] isPriority
|
|
* \return Object handle, or NULL if failure.
|
|
* \return
|
|
*/
|
|
UAVObjHandle UAVObjRegister(const UAVObjType *type,
|
|
bool isSingleInstance, bool isSettings, bool isPriority)
|
|
{
|
|
struct UAVOData *uavo_data = NULL;
|
|
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
/* Don't allow duplicate registrations */
|
|
if (UAVObjGetByID(type->id)) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
/* Map the various flags to one of the UAVO types we understand */
|
|
if (isSingleInstance) {
|
|
uavo_data = UAVObjAllocSingle(type->instance_size);
|
|
} else {
|
|
uavo_data = UAVObjAllocMulti(type->instance_size);
|
|
}
|
|
|
|
if (!uavo_data) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
/* Fill in the details about this UAVO */
|
|
uavo_data->type = type;
|
|
if (isSettings) {
|
|
uavo_data->base.flags.isSettings = true;
|
|
// settings defaults to being sent with priority
|
|
uavo_data->base.flags.isPriority = true;
|
|
} else {
|
|
uavo_data->base.flags.isPriority = isPriority;
|
|
}
|
|
/* Initialize the embedded meta UAVO */
|
|
UAVObjInitMetaData(&uavo_data->metaObj);
|
|
|
|
/* Initialize object fields and metadata to default values */
|
|
if (type->init_callback) {
|
|
type->init_callback((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
|
|
instanceAutoUpdated((UAVObjHandle)uavo_data, 0);
|
|
instanceAutoUpdated((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->type->id == id) {
|
|
found_obj = (UAVObjHandle *)tmp_obj;
|
|
goto unlock_exit;
|
|
}
|
|
if (MetaObjectId(tmp_obj->type->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 (IsMetaobject(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->type->id);
|
|
} else {
|
|
/* We have a data object, augment our pointer */
|
|
struct UAVOData *uavo_data = (struct UAVOData *)uavo_base;
|
|
|
|
return uavo_data->type->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->type->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 (IsMetaobject(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 (IsSingleInstance(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)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
if (IsMetaobject(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 (((struct UAVOData *)obj_handle)->type->init_callback) {
|
|
((struct UAVOData *)obj_handle)->type->init_callback(obj_handle, instId);
|
|
}
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
|
|
return instId;
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
* \param[in] create Create the object if it does not already exist.
|
|
* \return 0 if success or -1 if failure
|
|
*/
|
|
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn, bool create)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (IsMetaobject(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) && create) {
|
|
instEntry = createInstance(obj, instId);
|
|
}
|
|
if (instEntry == NULL) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Set the data
|
|
memcpy(InstanceData(instEntry), dataIn, obj->type->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 (IsMetaobject(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->type->instance_size);
|
|
}
|
|
|
|
rc = 0;
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return rc;
|
|
}
|
|
|
|
/**
|
|
* Update a CRC with an object data
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The instance ID
|
|
* \param[in] crc The crc to update
|
|
* \return the updated crc
|
|
*/
|
|
uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
if (IsMetaobject(obj_handle)) {
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
// TODO
|
|
} 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;
|
|
}
|
|
// Update crc
|
|
crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->type->instance_size);
|
|
}
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return crc;
|
|
}
|
|
|
|
/**
|
|
* Actually write the object's data to the logfile
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
*/
|
|
#ifdef PIOS_INCLUDE_DEBUGLOG
|
|
void UAVObjInstanceWriteToLog(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
if (IsMetaobject(obj_handle)) {
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, MetaNumBytes, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle));
|
|
} 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
|
|
PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, obj->type->instance_size, (uint8_t *)InstanceData(instEntry));
|
|
}
|
|
|
|
unlock_exit:
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
#else /* ifdef PIOS_INCLUDE_DEBUGLOG */
|
|
void UAVObjInstanceWriteToLog(__attribute__((unused)) UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) {}
|
|
#endif /* ifdef PIOS_INCLUDE_DEBUGLOG */
|
|
/**
|
|
* 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 (IsSettings(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 (IsSettings(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 (IsSettings(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;
|
|
}
|
|
|
|
int32_t UAVObjSetDefaults(UAVObjHandle obj_handle)
|
|
{
|
|
return UAVObjSetInstanceDefaults(obj_handle, 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_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);
|
|
}
|
|
|
|
int32_t UAVObjSetInstanceDefaults(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (IsMetaobject(obj_handle)) {
|
|
if (instId != 0) {
|
|
goto unlock_exit;
|
|
}
|
|
// How do we set meta data defaults?
|
|
// MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes;
|
|
} else {
|
|
// Check access level
|
|
if (UAVObjReadOnly(obj_handle)) {
|
|
goto unlock_exit;
|
|
}
|
|
|
|
// Set default data
|
|
if (((struct UAVOData *)obj_handle)->type->init_callback) {
|
|
((struct UAVOData *)obj_handle)->type->init_callback(obj_handle, instId);
|
|
}
|
|
}
|
|
|
|
// 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 UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
|
|
const void *dataIn)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
|
|
// Lock
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
int32_t rc = -1;
|
|
|
|
if (IsMetaobject(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->type->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 (IsMetaobject(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->type->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 (IsMetaobject(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->type->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 (IsMetaobject(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->type->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 (IsMetaobject(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 (IsMetaobject(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);
|
|
}
|
|
|
|
/**
|
|
* Get the UAVObject metadata logging update mode
|
|
* \param[in] metadata The metadata object
|
|
* \return the GCS telemetry update mode
|
|
*/
|
|
UAVObjUpdateMode UAVObjGetLoggingUpdateMode(const UAVObjMetadata *metadata)
|
|
{
|
|
PIOS_Assert(metadata);
|
|
return (metadata->flags >> UAVOBJ_LOGGING_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
|
|
}
|
|
|
|
/**
|
|
* Set the UAVObject metadata logging update mode member
|
|
* \param[in] metadata The metadata object
|
|
* \param[in] val The GCS telemetry update mode
|
|
*/
|
|
void UAVObjSetLoggingUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val)
|
|
{
|
|
PIOS_Assert(metadata);
|
|
SET_BITS(metadata->flags, UAVOBJ_LOGGING_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 (!IsMetaobject(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, false);
|
|
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, bool fast)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
int32_t res;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
res = connectObj(obj_handle, 0, cb, eventMask, fast);
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Trigger a EV_UPDATED_MANUAL event for an object.
|
|
* \param[in] obj The object handle
|
|
*/
|
|
void UAVObjUpdated(UAVObjHandle obj_handle)
|
|
{
|
|
UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES);
|
|
}
|
|
|
|
/**
|
|
* Trigger a EV_UPDATED_MANUAL event for an object instance.
|
|
* \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);
|
|
}
|
|
|
|
/**
|
|
* Trigger a EV_UPDATED event for an object instance.
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
*/
|
|
static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
|
|
/*
|
|
* Log the object's data (triggers a EV_LOGGING_MANUAL event on this object).
|
|
* \param[in] obj The object handle
|
|
*/
|
|
void UAVObjLogging(UAVObjHandle obj_handle)
|
|
{
|
|
UAVObjInstanceLogging(obj_handle, UAVOBJ_ALL_INSTANCES);
|
|
}
|
|
|
|
/**
|
|
* Log the object's data (triggers a EV_LOGGING_MANUAL event on this object).
|
|
* \param[in] obj The object handle
|
|
* \param[in] instId The object instance ID
|
|
*/
|
|
void UAVObjInstanceLogging(UAVObjHandle obj_handle, uint16_t instId)
|
|
{
|
|
PIOS_Assert(obj_handle);
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
sendEvent((struct UAVOBase *)obj_handle, instId, EV_LOGGING_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.
|
|
*/
|
|
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,
|
|
.lowPriority = false,
|
|
};
|
|
|
|
// 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.eventQueueErrors;
|
|
stats.lastQueueErrorID = UAVObjGetID(obj);
|
|
}
|
|
}
|
|
|
|
// Invoke callback (from event task) if a valid one is registered
|
|
if (event->cb) {
|
|
if (event->fast) {
|
|
event->cb(&msg);
|
|
} else if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) {
|
|
// invoke callback from the event task, will not block
|
|
++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 (IsSingleInstance(&(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 */
|
|
uint32_t size = sizeof(struct UAVOMultiInst) + obj->type->instance_size;
|
|
instEntry = (struct UAVOMultiInst *)pios_malloc(size);
|
|
if (!instEntry) {
|
|
return NULL;
|
|
}
|
|
memset(instEntry, 0, size);
|
|
LL_APPEND(((struct UAVOMulti *)obj)->instance0.next, instEntry);
|
|
|
|
((struct UAVOMulti *)obj)->num_instances++;
|
|
|
|
// Fire event
|
|
instanceAutoUpdated((UAVObjHandle)obj, instId);
|
|
|
|
// Done
|
|
return InstanceDataOffset(instEntry);
|
|
}
|
|
|
|
/**
|
|
* Get the instance information or NULL if the instance does not exist
|
|
*/
|
|
InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId)
|
|
{
|
|
if (IsMetaobject(&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 (IsSingleInstance(&(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, bool fast)
|
|
{
|
|
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;
|
|
event->fast = fast;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// Add queue to list
|
|
event = (struct ObjectEventEntry *)pios_fastheapmalloc(sizeof(struct ObjectEventEntry));
|
|
if (event == NULL) {
|
|
return -1;
|
|
}
|
|
event->queue = queue;
|
|
event->cb = cb;
|
|
event->eventMask = eventMask;
|
|
event->fast = fast;
|
|
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;
|
|
}
|