From 93901e87549e2920a44e3faa5f813f4948136aa9 Mon Sep 17 00:00:00 2001 From: vassilis Date: Sat, 27 Feb 2010 19:57:45 +0000 Subject: [PATCH] Refactored UAVObjects and UAVTalk for reduced memory footprint, added support for multiple instances within objects (useful for array objects, e.g. waypoints). Still not fully tested. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@199 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Makefile | 3 +- flight/Modules/Telemetry/telemetry.c | 201 +++- flight/OpenPilot/Settings/inc/settings.h | 0 flight/OpenPilot/Settings/settings.c | 0 .../OpenPilot/UAVObjects/inc/uavobjectlist.h | 38 - .../inc/{uavobject.h => uavobjectmanager.h} | 92 +- .../UAVObjects/inc/uavobjecttemplate.h | 18 +- .../OpenPilot/UAVObjects/inc/uavobjectutils.h | 55 -- flight/OpenPilot/UAVObjects/uavobjectlist.c | 176 ---- .../OpenPilot/UAVObjects/uavobjectmanager.c | 893 ++++++++++++++++++ .../OpenPilot/UAVObjects/uavobjecttemplate.c | 232 +---- flight/OpenPilot/UAVObjects/uavobjectutils.c | 133 --- flight/OpenPilot/UAVTalk/inc/uavtalk.h | 17 +- flight/OpenPilot/UAVTalk/uavtalk.c | 522 +++++----- 14 files changed, 1403 insertions(+), 977 deletions(-) create mode 100644 flight/OpenPilot/Settings/inc/settings.h create mode 100644 flight/OpenPilot/Settings/settings.c delete mode 100644 flight/OpenPilot/UAVObjects/inc/uavobjectlist.h rename flight/OpenPilot/UAVObjects/inc/{uavobject.h => uavobjectmanager.h} (52%) delete mode 100644 flight/OpenPilot/UAVObjects/inc/uavobjectutils.h delete mode 100644 flight/OpenPilot/UAVObjects/uavobjectlist.c create mode 100644 flight/OpenPilot/UAVObjects/uavobjectmanager.c delete mode 100644 flight/OpenPilot/UAVObjects/uavobjectutils.c diff --git a/flight/Makefile b/flight/Makefile index bf5f3021d..5e7acd492 100644 --- a/flight/Makefile +++ b/flight/Makefile @@ -102,8 +102,7 @@ SRC = $(MODTELEMETRY)/telemetry.c SRC += $(OPSYSTEM)/openpilot.c SRC += $(OPSYSTEM)/op_logging.c SRC += $(OPUAVTALK)/uavtalk.c -SRC += $(OPUAVOBJ)/uavobjectutils.c -SRC += $(OPUAVOBJ)/uavobjectlist.c +SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/uavobjectsinit.c ## UAVOBJECTS diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 013a78445..1e8196356 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -23,10 +23,11 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include // for malloc #include "telemetry.h" #include "uavtalk.h" -#include "uavobject.h" -#include "uavobjectlist.h" +#include "uavobjectmanager.h" +#include "utlist.h" #include "FreeRTOS.h" #include "task.h" #include "queue.h" @@ -38,17 +39,36 @@ #define TASK_PRIORITY 100 #define REQ_TIMEOUT_MS 500 #define MAX_RETRIES 2 +#define MAX_UPDATE_PERIOD_MS 1000 +#define MIN_UPDATE_PERIOD_MS 1 + +// Private types + +/** + * List of object properties that are needed for the periodic updates. + */ +struct ObjectListStruct { + UAVObjHandle obj; /** Object handle */ + int32_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ + int32_t timeToNextUpdateMs; /** Time delay to the next update */ + struct ObjectListStruct* next; /** Needed by linked list library (utlist.h) */ +}; +typedef struct ObjectListStruct ObjectList; // Private variables xQueueHandle queue; xTaskHandle telemetryTaskHandle; +ObjectList* objList; // Private functions void telemetryTask(); void receiveTask(); int32_t transmitData(uint8_t* data, int32_t length); -void registerObject(UAVObject* obj); -void updateObject(UAVObject* obj); +void registerObject(UAVObjHandle obj); +void updateObject(UAVObjHandle obj); +int32_t addObject(UAVObjHandle obj); +int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs); +int32_t processPeriodicUpdates(); /** * Initialize the telemetry module @@ -57,8 +77,11 @@ void updateObject(UAVObject* obj); */ int32_t TelemetryInitialize() { + // Initialize object list + objList = NULL; + // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjectQMsg)); + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjQMsg)); // TODO: Get telemetry settings object @@ -67,8 +90,8 @@ int32_t TelemetryInitialize() // Initialize UAVTalk UAVTalkInitialize(&transmitData); - // Process all registered objects, register in UAVTalk and connect queue for updates - UAVObjectListIterate(®isterObject); + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); // Start tasks xTaskCreate( telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle ); @@ -78,14 +101,14 @@ int32_t TelemetryInitialize() } /** -* Register a new object, connects object to UAVTalk and connects the queue depending on the object's +* Register a new object, adds object to local list and connects the queue depending on the object's * telemetry settings. * \param[in] obj Object to connect */ -void registerObject(UAVObject* obj) +void registerObject(UAVObjHandle obj) { - // Register object in UAVTalk - UAVTalkConnectObject(obj->objectID, obj->pack, obj->unpack, 0); + // Add object to list + addObject(obj); // Setup object for telemetry updates updateObject(obj); @@ -95,57 +118,57 @@ void registerObject(UAVObject* obj) * Update object's queue connections and timer, depending on object's settings * \param[in] obj Object to updates */ -void updateObject(UAVObject* obj) +void updateObject(UAVObjHandle obj) { - UAVObjectMetadata metadata; + UAVObjMetadata metadata; int32_t eventMask; // Get metadata - obj->getMetadata(&metadata); + UAVObjGetMetadata(obj, &metadata); // Setup object depending on update mode if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC) { // Set update period - UAVTalkSetUpdatePeriod(obj->objectID, metadata.telemetryUpdatePeriod); + setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); // Connect queue eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ; - if (obj->isMetadata) + if (UAVObjIsMetaobject(obj)) { eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events) } - obj->connect(queue, eventMask); + UAVObjConnect(obj, queue, eventMask); } else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE) { // Set update period - UAVTalkSetUpdatePeriod(obj->objectID, 0); + setUpdatePeriod(obj, 0); // Connect queue eventMask = QMSG_UPDATED|QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ; - if (obj->isMetadata) + if (UAVObjIsMetaobject(obj)) { eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events) } - obj->connect(queue, eventMask); + UAVObjConnect(obj, queue, eventMask); } else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL) { // Set update period - UAVTalkSetUpdatePeriod(obj->objectID, 0); + setUpdatePeriod(obj, 0); // Connect queue eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ; - if (obj->isMetadata) + if (UAVObjIsMetaobject(obj)) { eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events) } - obj->connect(queue, eventMask); + UAVObjConnect(obj, queue, eventMask); } else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER) { // Set update period - UAVTalkSetUpdatePeriod(obj->objectID, 0); + setUpdatePeriod(obj, 0); // Disconnect queue - obj->disconnect(queue); + UAVObjDisconnect(obj, queue); } } @@ -154,12 +177,12 @@ void updateObject(UAVObject* obj) */ void telemetryTask() { - static int32_t timeToNextUpdateMs; - static int32_t delayMs; - static UAVObjectQMsg msg; - static UAVObjectMetadata metadata; - static int32_t retries; - static int32_t success; + int32_t timeToNextUpdateMs; + int32_t delayMs; + UAVObjQMsg msg; + UAVObjMetadata metadata; + int32_t retries; + int32_t success; // Initialize time timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS; @@ -178,7 +201,7 @@ void telemetryTask() if ( xQueueReceive(queue, &msg, delayMs/portTICK_RATE_MS) == pdTRUE ) { // Get object metadata - msg.obj->getMetadata(&metadata); + UAVObjGetMetadata(msg.obj, &metadata); // Act on event if (msg.event == QMSG_UPDATED || msg.event == QMSG_UPDATED_MANUAL) { @@ -186,7 +209,7 @@ void telemetryTask() retries = 0; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(msg.obj->objectID, metadata.ackRequired, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout + success = UAVTalkSendObject(msg.obj, msg.instId, metadata.ackRequired, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout ++retries; } } @@ -196,29 +219,28 @@ void telemetryTask() retries = 0; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObjectRequest(msg.obj->objectID, REQ_TIMEOUT_MS); // call blocks until update is received or timeout + success = UAVTalkSendObjectRequest(msg.obj, msg.instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout ++retries; } } // If this is a metadata object then make necessary telemetry updates - if (msg.obj->isMetadata) + if (UAVObjIsMetaobject(msg.obj)) { - updateObject(msg.obj->linkedObj); // linked object will be the actual object the metadata are for + updateObject(UAVObjGetLinkedObj(msg.obj)); // linked object will be the actual object the metadata are for } } // Process periodic updates if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs ) { - delayMs = UAVTalkProcessPeriodicUpdates(); - timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS + delayMs; + timeToNextUpdateMs = processPeriodicUpdates(); } } } /** * Receive task. Processes received bytes (from the modem or USB) and passes them to - * UAVTalk for decodings. Does not return. + * UAVTalk for decoding. Does not return. */ void receiveTask() { @@ -240,5 +262,106 @@ int32_t transmitData(uint8_t* data, int32_t length) return 0; } +/** + * Setup object for periodic updates. + * \param[in] obj The object to update + * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled + * \return 0 Success + * \return -1 Failure + */ +int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) +{ + ObjectList* objEntry; + // Check that the object is not already connected + LL_FOREACH(objList, objEntry) + { + if (objEntry->obj == obj) + { + objEntry->updatePeriodMs = updatePeriodMs; + objEntry->timeToNextUpdateMs = 0; + return 0; + } + } + // If this point is reached then the object was not found + return -1; +} + +/** + * Handle periodic updates for all objects. + * \return The system time until the next update (in ms) or -1 if failed + */ +int32_t processPeriodicUpdates() +{ + static int32_t timeOfLastUpdate = 0; + ObjectList* objEntry; + int32_t delaySinceLastUpdateMs; + int32_t minDelay = MAX_UPDATE_PERIOD_MS; + + // Iterate through each object and update its timer, if zero then transmit object. + // Also calculate smallest delay to next update. + delaySinceLastUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS - timeOfLastUpdate; + LL_FOREACH(objList, objEntry) + { + // If object is configured for periodic updates + if (objEntry->updatePeriodMs > 0) + { + objEntry->timeToNextUpdateMs -= delaySinceLastUpdateMs; + // Check if time for the next update + if (objEntry->timeToNextUpdateMs <= 0) + { + // Reset timer + objEntry->timeToNextUpdateMs = objEntry->updatePeriodMs; + // Send object (trigger update) + UAVObjUpdated(objEntry->obj); + } + // Update minimum delay + if (objEntry->timeToNextUpdateMs < minDelay) + { + minDelay = objEntry->timeToNextUpdateMs; + } + } + } + + // Check if delay for the next update is too short + if (minDelay < MIN_UPDATE_PERIOD_MS) + { + minDelay = MIN_UPDATE_PERIOD_MS; + } + + // Done + timeOfLastUpdate = xTaskGetTickCount()*portTICK_RATE_MS; + return timeOfLastUpdate + minDelay; +} + +/** + * Add a new object to the object list + */ +int32_t addObject(UAVObjHandle obj) +{ + ObjectList* objEntry; + // Check that the object is not already connected + LL_FOREACH(objList, objEntry) + { + if (objEntry->obj == obj) + { + // Already registered, ignore + return -1; + } + } + // Create handle + objEntry = (ObjectList*)malloc(sizeof(ObjectList)); + if (objEntry == NULL) return -1; + objEntry->obj = obj; + objEntry->updatePeriodMs = 0; + objEntry->timeToNextUpdateMs = 0; + // Add to list + LL_APPEND(objList, objEntry); + return 0; +} + + + + + diff --git a/flight/OpenPilot/Settings/inc/settings.h b/flight/OpenPilot/Settings/inc/settings.h new file mode 100644 index 000000000..e69de29bb diff --git a/flight/OpenPilot/Settings/settings.c b/flight/OpenPilot/Settings/settings.c new file mode 100644 index 000000000..e69de29bb diff --git a/flight/OpenPilot/UAVObjects/inc/uavobjectlist.h b/flight/OpenPilot/UAVObjects/inc/uavobjectlist.h deleted file mode 100644 index 630e27e10..000000000 --- a/flight/OpenPilot/UAVObjects/inc/uavobjectlist.h +++ /dev/null @@ -1,38 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavobjectlist.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include files of the uavobjectlist library - * @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 - */ - -#ifndef UAVOBJECTLIST_H -#define UAVOBJECTLIST_H - -#include -#include "uavobject.h" - -int32_t UAVObjectListInitialize(); -int32_t UAVObjectListRegister(UAVObject* obj); -UAVObject* UAVObjectListGetByID(int32_t id); -UAVObject* UAVObjectListGetByName(char* name); -void UAVObjectListIterate(void (*iterator)(UAVObject* obj)); - -#endif // UAVOBJECTLIST_H diff --git a/flight/OpenPilot/UAVObjects/inc/uavobject.h b/flight/OpenPilot/UAVObjects/inc/uavobjectmanager.h similarity index 52% rename from flight/OpenPilot/UAVObjects/inc/uavobject.h rename to flight/OpenPilot/UAVObjects/inc/uavobjectmanager.h index d33645ab8..9cec5959d 100644 --- a/flight/OpenPilot/UAVObjects/inc/uavobject.h +++ b/flight/OpenPilot/UAVObjects/inc/uavobjectmanager.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * - * @file uavobject.h + * @file uavobjectlist.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Interface implemented by all UAVObjects. + * @brief Include files of the uavobjectlist library * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -23,75 +23,87 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef UAVOBJECT_H -#define UAVOBJECT_H +#ifndef UAVOBJECTMANAGER_H +#define UAVOBJECTMANAGER_H #include #include "FreeRTOS.h" #include "queue.h" +#define UAVOBJ_ALL_INSTANCES 0xFFFF + +typedef uint32_t UAVObjHandle; + /** - * Object update mode, used by multiple modules (e.g. telemetry and logger) + * Object update mode, used by multiple modules (e.g. telemetry and logger) */ typedef enum { UPDATEMODE_PERIODIC = 0, /** Automatically update object at periodic intervals */ UPDATEMODE_ONCHANGE, /** Only update object when its data changes */ UPDATEMODE_MANUAL, /** Manually update object, by calling the updated() function */ UPDATEMODE_NEVER /** Object is never updated */ -} UAVObjectUpdateMode; +} UAVObjUpdateMode; /** * Object metadata, each object has a meta object that holds its metadata. The metadata define - * properties for each object and can be used by multiple modules (e.g. telemetry and logger) + * properties for each object and can be used by multiple modules (e.g. telemetry and logger) */ typedef struct { int8_t ackRequired; /** Defines if an ack is required for the transactions of this object (1:acked, 0:not acked) */ - UAVObjectUpdateMode telemetryUpdateMode; /** Update mode used by the telemetry module */ + UAVObjUpdateMode telemetryUpdateMode; /** Update mode used by the telemetry module */ int32_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ - UAVObjectUpdateMode gcsTelemetryUpdateMode; /** Update mode used by the GCS */ + UAVObjUpdateMode gcsTelemetryUpdateMode; /** Update mode used by the GCS */ int32_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ - UAVObjectUpdateMode loggingUpdateMode; /** Update mode used by the logging module */ + UAVObjUpdateMode loggingUpdateMode; /** Update mode used by the logging module */ int32_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ -} UAVObjectMetadata; +} UAVObjMetadata; /** - * Interface implemented by all UAVObjects. - */ -struct UAVObjectStruct { - uint32_t objectID; /** Unique object ID */ - uint32_t metadataID; /** ID of the metadata object */ - int32_t isMetadata; /** Defines if this object is a meta object (1:meta, 0:regular object) */ - int32_t numBytes; /** Number of bytes of object's data */ - const char* name; /** Object name */ - struct UAVObjectStruct* linkedObj; /** Link between regular objects and metaobject, for regular objects this is the metaobject, for metadata objects this is the parent object */ - int32_t (*pack)(uint32_t objId, uint8_t* data, int32_t maxLength); /** Pack object data in a byte buffer */ - int32_t (*unpack)(uint32_t objId, uint8_t* data, int32_t length); /** Unpack object data from a byte buffer */ - int32_t (*initializeData)(const char* init); /** Initialize object data from a string, this is used by settings objects, the settings string is parsed and fields initialized */ - void (*getMetadata)(UAVObjectMetadata* dataOut); /** Get the object's metadata */ - void (*setMetadata)(const UAVObjectMetadata* dataIn); /** Set the object's metadata */ - void (*requestUpdate)(); /** Request that this object is updated with the latest value from the GCS */ - void (*updated)(); /** Trigger an update event, used when the update mode is set to UPDATEMODE_MANUAL */ - int32_t (*connect)(xQueueHandle queue, int32_t eventMask); /** Connect an event queue (from a module) to this object, the eventMask defines which events are enabled (or'ed UAVObjectQMsgEvent or 0 for all events) */ - int32_t (*disconnect)(xQueueHandle queue); /** Disconnect an event queue from this object */ -}; -typedef struct UAVObjectStruct UAVObject; - -/** - * Event types generated by the objects. + * Event types generated by the objects. */ typedef enum { QMSG_UNPACKED = 1, /** Object data updated by unpacking */ QMSG_UPDATED = 2, /** Object data updated by changing the data structure */ QMSG_UPDATED_MANUAL = 4, /** Object update event manually generated */ QMSG_UPDATE_REQ = 8 /** Request to update object data */ -} UAVObjectQMsgEvent; +} UAVObjQMsgEvent; /** - * Event message, this structure is send in the event queue each time an event is generated + * Event message, this structure is send in the event queue each time an event is generated */ typedef struct { - UAVObject* obj; - UAVObjectQMsgEvent event; -} UAVObjectQMsg; + UAVObjHandle obj; + int32_t instId; + UAVObjQMsgEvent event; +} UAVObjQMsg; -#endif // UAVOBJECT_H +int32_t UAVObjInitialize(); +UAVObjHandle UAVObjRegister(uint32_t id, const char* name, int32_t isMetaobject, int32_t isSingleInstance, uint32_t numBytes); +UAVObjHandle UAVObjGetByID(uint32_t id); +UAVObjHandle UAVObjGetByName(char* name); +uint32_t UAVObjGetID(UAVObjHandle obj); +const char* UAVObjGetName(UAVObjHandle obj); +uint32_t UAVObjGetNumBytes(UAVObjHandle obj); +uint32_t UAVObjGetNumInstances(UAVObjHandle obj); +UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj); +int32_t UAVObjCreateInstance(UAVObjHandle obj); +int32_t UAVObjIsSingleInstance(UAVObjHandle obj); +int32_t UAVObjIsMetaobject(UAVObjHandle obj); +int32_t UAVObjInitData(UAVObjHandle obj, const char* init); +int32_t UAVObjUnpack(UAVObjHandle obj, uint32_t instId, const uint8_t* dataIn); +int32_t UAVObjPack(UAVObjHandle obj, uint32_t instId, uint8_t* dataOut); +int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn); +int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut); +int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint32_t instId, const void* dataIn); +int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint32_t instId, void* dataOut); +int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn); +int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut); +int32_t UAVObjConnect(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask); +int32_t UAVObjDisconnect(UAVObjHandle obj, xQueueHandle queue); +void UAVObjRequestUpdate(UAVObjHandle obj); +void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint32_t instId); +void UAVObjUpdated(UAVObjHandle obj); +void UAVObjInstanceUpdated(UAVObjHandle obj, uint32_t instId); +void UAVObjIterate(void (*iterator)(UAVObjHandle obj)); + +#endif // UAVOBJECTMANAGER_H diff --git a/flight/OpenPilot/UAVObjects/inc/uavobjecttemplate.h b/flight/OpenPilot/UAVObjects/inc/uavobjecttemplate.h index a04e8fc00..114ea9bf8 100644 --- a/flight/OpenPilot/UAVObjects/inc/uavobjecttemplate.h +++ b/flight/OpenPilot/UAVObjects/inc/uavobjecttemplate.h @@ -32,9 +32,17 @@ #define $(NAMEUC)_H #include -#include "uavobject.h" -#include "FreeRTOS.h" -#include "queue.h" +#include "uavobjectmanager.h" + +// Object constants +#define $(NAMEUC)_OBJID $(OBJID) +#define $(NAMEUC)_NAME "$(NAME)" +#define $(NAMEUC)_SINGLEINST $(SINGLEINST) +#define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data) + +// Data access macros +#define $(NAMEUC)_GET(dataOut) UAVObjGetData($(NAME)GetHandle(), dataOut) +#define $(NAMEUC)_SET(dataIn) UAVObjGetData($(NAME)GetHandle(), dataIn) // Object data typedef struct { @@ -43,8 +51,6 @@ typedef struct { // Generic interface functions int32_t $(NAME)Initialize(); -UAVObject* $(NAME)Get(); -void $(NAME)GetData(TestObjectData* dataOut); -void $(NAME)SetData(const TestObjectData* dataIn); +UAVObjHandle $(NAME)GetHandle(); #endif // $(NAME)_H diff --git a/flight/OpenPilot/UAVObjects/inc/uavobjectutils.h b/flight/OpenPilot/UAVObjects/inc/uavobjectutils.h deleted file mode 100644 index c613fed68..000000000 --- a/flight/OpenPilot/UAVObjects/inc/uavobjectutils.h +++ /dev/null @@ -1,55 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavobjectutils.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Functions common to all objects, they are included in this file - * to avoid duplication on each generated object. The functions - * in this file should be called only by UAVObjects. - * @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 - */ - -#ifndef UAVOBJECTUTILS_H -#define UAVOBJECTUTILS_H - -#include "uavobject.h" -#include "FreeRTOS.h" -#include "queue.h" -#include "semphr.h" - -struct ObjectQueueListStruct { - xQueueHandle queue; - int32_t eventMask; - struct ObjectQueueListStruct* next; -}; -typedef struct ObjectQueueListStruct ObjectQueueList; - -typedef struct { - UAVObject obj; - xSemaphoreHandle mutex; - ObjectQueueList* queues; -} ObjectContext; - -int32_t UAVObjUtilsConnect(ObjectContext* context, xQueueHandle queue, int32_t eventMask); -int32_t UAVObjUtilsDisconnect(ObjectContext* context, xQueueHandle queue); -void UAVObjUtilsSendEvent(ObjectContext* context, UAVObjectQMsgEvent event); -void UAVObjUtilsRequestUpdate(ObjectContext* context); -void UAVObjUtilsUpdated(ObjectContext* context); - -#endif //UAVOBJECTUTILS_H diff --git a/flight/OpenPilot/UAVObjects/uavobjectlist.c b/flight/OpenPilot/UAVObjects/uavobjectlist.c deleted file mode 100644 index 118b0d3d2..000000000 --- a/flight/OpenPilot/UAVObjects/uavobjectlist.c +++ /dev/null @@ -1,176 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavobjectlist.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Object list 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 // for malloc -#include "string.h" // for strcmp -#include "uavobjectlist.h" -#include "utlist.h" -#include "FreeRTOS.h" -#include "queue.h" -#include "semphr.h" - -// Private types -struct ObjectListStruct { - UAVObject* obj; - struct ObjectListStruct* next; -}; -typedef struct ObjectListStruct ObjectList; - -// Private variables -ObjectList* objList; -xSemaphoreHandle mutex; - -/** - * Initialize the object list - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVObjectListInitialize() -{ - // Initialize object list - objList = NULL; - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) - return -1; - // Done - return 0; -} - -/** - * Register an object to the list - * \param[in] obj Object to register - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVObjectListRegister(UAVObject* obj) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Check that the object is not already registred - LL_FOREACH(objList, objEntry) - { - if (objList->obj == obj) - { - // Already registered, ignore - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - - // Create and append entry - objEntry = (ObjectList*)malloc(sizeof(ObjectList)); - objEntry->obj = obj; - LL_APPEND(objList, objEntry); - - // Release lock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Retrieve an object from the list given its id - * \param[in] The object ID - * \return The object or NULL if not found. - */ -UAVObject* UAVObjectListGetByID(int32_t id) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Look for object - LL_FOREACH(objList, objEntry) - { - if (objEntry->obj->objectID == id) - { - // Release lock - xSemaphoreGiveRecursive(mutex); - // Done, object found - return objEntry->obj; - } - } - - // Object not found, release lock and return error - xSemaphoreGiveRecursive(mutex); - return NULL; -} - -/** - * Retrieve an object from the list given its name - * \param[in] name The name of the object - * \return 0 Success - * \return -1 Failure - */ -UAVObject* UAVObjectListGetByName(char* name) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Look for object - LL_FOREACH(objList, objEntry) - { - if (strcmp(objEntry->obj->name, name) == 0) - { - // Release lock - xSemaphoreGiveRecursive(mutex); - // Done, object found - return objEntry->obj; - } - } - - // Object not found, release lock and return error - xSemaphoreGiveRecursive(mutex); - return NULL; -} - -/** - * 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 UAVObjectsIterate(void (*iterator)(UAVObject* obj)) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Iterate through the list and invoke iterator for each object - LL_FOREACH(objList, objEntry) - { - (*iterator)(objEntry->obj); - } - - // Release lock - xSemaphoreGiveRecursive(mutex); -} diff --git a/flight/OpenPilot/UAVObjects/uavobjectmanager.c b/flight/OpenPilot/UAVObjects/uavobjectmanager.c new file mode 100644 index 000000000..f2b879ce3 --- /dev/null +++ b/flight/OpenPilot/UAVObjects/uavobjectmanager.c @@ -0,0 +1,893 @@ +/** + ****************************************************************************** + * + * @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 // for malloc +#include "string.h" // for strcmp +#include "uavobjectmanager.h" +#include "utlist.h" +#include "FreeRTOS.h" +#include "queue.h" +#include "semphr.h" + +// Constants + +// Private types + +/** + * List of event queues and the eventmask associated with the queue. + */ +struct ObjectQueueListStruct { + xQueueHandle queue; + int32_t eventMask; + struct ObjectQueueListStruct* next; +}; +typedef struct ObjectQueueListStruct ObjectQueueList; + +/** + * List of object instances, holds the actual data structure and instance ID + */ +struct ObjectInstListStruct { + void* data; + uint32_t instId; + struct ObjectInstListStruct* next; +}; +typedef struct ObjectInstListStruct ObjectInstList; + +/** + * List of objects registered in the object manager + */ +struct ObjectListStruct { + uint32_t id; /** The object ID */ + const char* name; /** The object name */ + int32_t isMetaobject; /** Set to 1 if this is a metaobject */ + int32_t isSingleInstance; /** Set to 1 if this object has a single instance */ + uint32_t numBytes; /** Number of data bytes contained in the object (for a single instance) */ + uint32_t numInstances; /** Number of instances */ + struct ObjectListStruct* linkedObj; /** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */ + union + { + void* instance; + ObjectInstList* instances; + } data; /** Actual object data, for single instances it is the object data structure, for multiple instances it is a ObjectInstList */ + ObjectQueueList* queues; /** Event queues registered on the object */ + struct ObjectListStruct* next; /** Needed by linked list library (utlist.h) */ +}; +typedef struct ObjectListStruct ObjectList; + +// Private functions +int32_t setInstanceData(ObjectList* obj, uint32_t instId, const void* dataIn); +int32_t getInstanceData(ObjectList* obj, uint32_t instId, void* dataOut); +int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event); +int32_t createInstance(ObjectList* obj, uint32_t instId); +int32_t hasInstance(ObjectList* obj, uint32_t instId); + +// Private variables +ObjectList* objList; +xSemaphoreHandle mutex; +UAVObjMetadata defMetadata; + +/** + * Initialize the object manager + * \return 0 Success + * \return -1 Failure + */ +int32_t UAVObjInitialize() +{ + // Initialize object list + objList = NULL; + + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) + return -1; + + // Initialize default metadata structure (metadata of metaobjects) + defMetadata.ackRequired = 1; + defMetadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE; + defMetadata.telemetryUpdatePeriod = 0; + defMetadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; + defMetadata.gcsTelemetryUpdatePeriod = 0; + defMetadata.loggingUpdateMode = UPDATEMODE_ONCHANGE; + defMetadata.loggingUpdatePeriod = 0; + + // Done + return 0; +} + +/** + * Register and new object in the object manager. + * \param[in] id Unique object ID + * \param[in] name Object name + * \param[in] isMetaobject Is this a metaobject (1:true, 0:false) + * \param[in] isSingleInstance Is this a single instance or multi-instance object + * \param[in] numBytes Number of bytes of object data (for one instance) + * \return Object handle, or 0 if failure. + * \return + */ +UAVObjHandle UAVObjRegister(uint32_t id, const char* name, int32_t isMetaobject, int32_t isSingleInstance, uint32_t numBytes) +{ + ObjectList* objEntry; + ObjectList* metaObj; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Check that the object is not already registered + LL_FOREACH(objList, objEntry) + { + if (objEntry->id == id) + { + // Already registered, ignore + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + + // Create and append entry + objEntry = (ObjectList*)malloc(sizeof(ObjectList)); + if (objEntry == NULL) + { + xSemaphoreGiveRecursive(mutex); + return 0; + } + objEntry->id = id; + objEntry->name = name; + objEntry->isMetaobject = isMetaobject; + objEntry->isSingleInstance = isSingleInstance; + objEntry->numBytes = numBytes; + objEntry->queues = NULL; + if (!isSingleInstance) + { + objEntry->numInstances = 0; + objEntry->data.instances = NULL; + } + else + { + objEntry->numInstances = 1; + objEntry->data.instance = malloc(numBytes); + if (objEntry->data.instance == NULL) + { + xSemaphoreGiveRecursive(mutex); + return 0; + } + memset(objEntry->data.instance, 0, numBytes); + } + if (isMetaobject) + { + objEntry->linkedObj = NULL; // will be set later + } + else + { + // Create metaobject + metaObj = (ObjectList*)UAVObjRegister(id+1, NULL, 1, 1, sizeof(UAVObjMetadata)); + // Link two objects + objEntry->linkedObj = metaObj; + metaObj->linkedObj = objEntry; + } + LL_APPEND(objList, objEntry); + + // Release lock + xSemaphoreGiveRecursive(mutex); + return (UAVObjHandle)objEntry; +} + +/** + * Retrieve an object from the list given its id + * \param[in] The object ID + * \return The object or 0 if not found. + */ +UAVObjHandle UAVObjGetByID(uint32_t id) +{ + ObjectList* objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Look for object + LL_FOREACH(objList, objEntry) + { + if (objEntry->id == id) + { + // Release lock + xSemaphoreGiveRecursive(mutex); + // Done, object found + return (UAVObjHandle)objEntry; + } + } + + // Object not found, release lock and return error + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Retrieve an object from the list given its name + * \param[in] name The name of the object + * \return The object or 0 if not found. + */ +UAVObjHandle UAVObjGetByName(char* name) +{ + ObjectList* objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Look for object + LL_FOREACH(objList, objEntry) + { + if (objEntry->name != NULL && strcmp(objEntry->name, name) == 0) + { + // Release lock + xSemaphoreGiveRecursive(mutex); + // Done, object found + return (UAVObjHandle)objEntry; + } + } + + // Object not found, release lock and return error + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Get the object's ID + * \param[in] obj The object handle + * \return The object ID + */ +uint32_t UAVObjGetID(UAVObjHandle obj) +{ + return ((ObjectList*)obj)->id; +} + +/** + * Get the object's name + * \param[in] obj The object handle + * \return The object's name + */ +const char* UAVObjGetName(UAVObjHandle obj) +{ + return ((ObjectList*)obj)->name; +} + +/** + * Get the number of bytes of the object's data (for one instance) + * \param[in] obj The object handle + * \return The number of bytes + */ +uint32_t UAVObjGetNumBytes(UAVObjHandle obj) +{ + return ((ObjectList*)obj)->numBytes; +} + +/** + * Get the object this object is linked to. For regular objects, the linked object + * is the metaobject. For metaobjects the linked object is the parent object. + * This function is normally only needed by the telemetry module. + * \param[in] obj The object handle + * \return The object linked object handle + */ +UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj) +{ + return (UAVObjHandle)(((ObjectList*)obj)->linkedObj); +} + +/** + * Get the number of instances contained in the object. + * \param[in] obj The object handle + * \return The number of instances + */ +uint32_t UAVObjGetNumInstances(UAVObjHandle obj) +{ + uint32_t numInstances; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + numInstances = ((ObjectList*)obj)->numInstances; + xSemaphoreGiveRecursive(mutex); + return numInstances; +} + +/** + * Create a new instance in the object. + * \param[in] obj The object handle + * \return The instance ID + */ +int32_t UAVObjCreateInstance(UAVObjHandle obj) +{ + ObjectList* objEntry; + int32_t res; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Create new instance + objEntry = (ObjectList*)obj; + res = createInstance(objEntry, objEntry->numInstances); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return res; + +} + +/** + * Does this object contains a single instance or multiple instances? + * \param[in] obj The object handle + * \return True (1) if this is a single instance object + */ +int32_t UAVObjIsSingleInstance(UAVObjHandle obj) +{ + return ((ObjectList*)obj)->isSingleInstance; +} + +/** + * Is this a metaobject? + * \param[in] obj The object handle + * \return True (1) if this is metaobject + */ +int32_t UAVObjIsMetaobject(UAVObjHandle obj) +{ + return ((ObjectList*)obj)->isMetaobject; +} + +/** + * Initialize object data from a string (usually stored as a settings file) + * \param[in] obj The object handle + * \param[in] init Text with initialization information (settings file) + * \return 0 if success or -1 if failure + */ +int32_t UAVObjInitData(UAVObjHandle obj, const char* init) +{ + // TODO: Implement object data initialization from string (settings) + return -1; +} + +/** + * 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, uint32_t instId, const uint8_t* dataIn) +{ + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Unpack + objEntry = (ObjectList*)obj; + if (!objEntry->isSingleInstance) + { + // If instance does not exist, create it + if (!hasInstance(objEntry, instId)) + { + createInstance(objEntry, instId); + } + // Set data + if (setInstanceData(objEntry, instId, dataIn) < 0) + { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + else + { + memcpy(objEntry->data.instance, dataIn, objEntry->numBytes); + } + + // Fire event + sendEvent(objEntry, instId, QMSG_UNPACKED); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Pack an object to a byte array + * \param[in] obj The object handle + * \param[in] instId The instance ID + * \param[out] dataOut The byte array + * \return 0 if success or -1 if failure + */ +int32_t UAVObjPack(UAVObjHandle obj, uint32_t instId, uint8_t* dataOut) +{ + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Pack + objEntry = (ObjectList*)obj; + if (!objEntry->isSingleInstance) + { + dataOut[0] = (uint8_t)(instId && 0xFF); + dataOut[1] = (uint8_t)((instId >> 8) && 0xFF); + if (getInstanceData(objEntry, instId, &dataOut[2]) < 0) + { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + else + { + memcpy(dataOut, objEntry->data.instance, objEntry->numBytes); + } + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Set the object data + * \param[in] obj The object handle + * \param[in] dataIn The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn) +{ + return UAVObjSetInstanceData(obj, 0, dataIn); +} + +/** + * Get the object data + * \param[in] obj The object handle + * \param[out] dataOut The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut) +{ + return UAVObjGetInstanceData(obj, 0, dataOut); +} + +/** + * Set the data of a specific object instance + * \param[in] obj The object handle + * \param[in] instId The object instance ID + * \param[in] dataIn The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint32_t instId, const void* dataIn) +{ + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Set data + objEntry = (ObjectList*)obj; + if (!objEntry->isSingleInstance) + { + if ( setInstanceData(objEntry, instId, dataIn) < 0 ) + { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + else + { + memcpy(objEntry->data.instance, dataIn, objEntry->numBytes); + } + + // Fire event + sendEvent(objEntry, instId, QMSG_UPDATED); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Get the data of a specific object instance + * \param[in] obj The object handle + * \param[in] instId The object instance ID + * \param[out] dataOut The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint32_t instId, void* dataOut) +{ + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Get data + objEntry = (ObjectList*)obj; + if (!objEntry->isSingleInstance) + { + if ( getInstanceData(objEntry, 0, dataOut) < 0 ) + { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + else + { + memcpy(dataOut, objEntry->data.instance, objEntry->numBytes); + } + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Set the object metadata + * \param[in] obj The object handle + * \param[in] dataIn The object's metadata structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn) +{ + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Set metadata + objEntry = (ObjectList*)obj; + if (objEntry->isMetaobject) + { + memcpy(&defMetadata, dataIn, sizeof(UAVObjMetadata)); + } + else + { + UAVObjSetData((UAVObjHandle)objEntry->linkedObj, dataIn); + } + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Get the object metadata + * \param[in] obj The object handle + * \param[out] dataOut The object's metadata structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut) +{ + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Get metadata + objEntry = (ObjectList*)obj; + if (objEntry->isMetaobject) + { + memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); + } + else + { + UAVObjGetData((UAVObjHandle)objEntry->linkedObj, dataOut); + } + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Connect an event queue to the object + * \param[in] obj The object handle + * \param[in] queue The event queue + * \param[in] eventMask The event mask, if 0 then all events are enabled (e.g. QMSG_UPDATED | QMSG_UPDATED_MANUAL) + * \return 0 if success or -1 if failure + */ +int32_t UAVObjConnect(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask) +{ + ObjectQueueList* queueEntry; + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Check that the queue is not already connected, if it is simply update event mask + objEntry = (ObjectList*)obj; + LL_FOREACH(objEntry->queues, queueEntry) + { + if (queueEntry->queue == queue) + { + // Already connected, update event mask and return + queueEntry->eventMask = eventMask; + xSemaphoreGiveRecursive(mutex); + return 0; + } + } + + // Add queue to list + queueEntry = (ObjectQueueList*)malloc(sizeof(ObjectQueueList)); + if (queueEntry == NULL) + { + xSemaphoreGiveRecursive(mutex); + return -1; + } + queueEntry->queue = queue; + queueEntry->eventMask = eventMask; + LL_APPEND(objEntry->queues, queueEntry); + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * 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 UAVObjDisconnect(UAVObjHandle obj, xQueueHandle queue) +{ + ObjectQueueList* queueEntry; + ObjectList* objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Find queue and remove it + objEntry = (ObjectList*)obj; + LL_FOREACH(objEntry->queues, queueEntry) + { + if (queueEntry->queue == queue) + { + LL_DELETE(objEntry->queues, queueEntry); + xSemaphoreGiveRecursive(mutex); + return 0; + } + } + + // If this point is reached the queue was not found + xSemaphoreGiveRecursive(mutex); + return -1; +} + +/** + * Request an update of the object's data from the GCS. The call will not wait for the response, a QMSG_UPDATED event + * will be generated as soon as the object is updated. + * \param[in] obj The object handle + */ +void UAVObjRequestUpdate(UAVObjHandle obj) +{ + UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES); +} + +/** + * Request an update of the object's data from the GCS. The call will not wait for the response, a QMSG_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, uint32_t instId) +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((ObjectList*)obj, instId, QMSG_UPDATE_REQ); + xSemaphoreGiveRecursive(mutex); +} + +/** + * Send the object's data to the GCS (triggers a QMSG_UPDATED_MANUAL event on this object). + * \param[in] obj The object handle + */ +void UAVObjUpdated(UAVObjHandle obj) +{ + UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES); +} + +/** + * Send the object's data to the GCS (triggers a QMSG_UPDATED_MANUAL event on this object). + * \param[in] obj The object handle + * \param[in] instId The object instance ID + */ +void UAVObjInstanceUpdated(UAVObjHandle obj, uint32_t instId) +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((ObjectList*)obj, instId, QMSG_UPDATED); + xSemaphoreGiveRecursive(mutex); +} + +/** + * Iterate through all objects in the list. + * \param iterator This function will be called once for each object, + * the object will be passed as a parameter + */ +void UAVObjIterate(void (*iterator)(UAVObjHandle obj)) +{ + ObjectList* objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Iterate through the list and invoke iterator for each object + LL_FOREACH(objList, objEntry) + { + (*iterator)((UAVObjHandle)objEntry); + } + + // Release lock + xSemaphoreGiveRecursive(mutex); +} + +/** + * Set the data of a specific object instance. + */ +int32_t setInstanceData(ObjectList* obj, uint32_t instId, const void* dataIn) +{ + ObjectInstList* elemEntry; + + // Set data depending on data format + if (!obj->isSingleInstance) + { + // Look for specified instance ID + LL_FOREACH(obj->data.instances, elemEntry) + { + if (elemEntry->instId == instId) + { + memcpy(elemEntry->data, dataIn, obj->numBytes); + return 0; + } + } + // If this point is reached then instance id was not found + return -1; + } + else + { + memcpy(obj->data.instance, dataIn, obj->numBytes); + return 0; + } +} + +/** + * Get the data of a specific object instance. + */ +int32_t getInstanceData(ObjectList* obj, uint32_t instId, void* dataOut) +{ + ObjectInstList* elemEntry; + + // Get data depending on data format + if (!obj->isSingleInstance) + { + // Look for specified instance ID + LL_FOREACH(obj->data.instances, elemEntry) + { + if (elemEntry->instId == instId) + { + memcpy(dataOut, elemEntry->data, obj->numBytes); + return 0; + } + } + // If this point is reached then instance id was not found + return -1; + } + else + { + memcpy(dataOut, obj->data.instance, obj->numBytes); + return 0; + } +} + +/** + * Send an event to all event queues registered on the object. + */ +int32_t sendEvent(ObjectList* obj, uint32_t instId, UAVObjQMsgEvent event) +{ + ObjectQueueList* queueEntry; + UAVObjQMsg msg; + + // Setup event + msg.obj = (UAVObjHandle)obj; + msg.event = event; + msg.instId = instId; + + // Go through each object and push the event message in the queue (if event is activated for the queue) + LL_FOREACH(obj->queues, queueEntry) + { + if ( queueEntry->eventMask == 0 || (queueEntry->eventMask & event) != 0 ) + { + xQueueSend(queueEntry->queue, &msg, 0); // do not wait if queue is full + } + } + + // Done + return 0; +} + +/** + * Create a new object instance + */ +int32_t createInstance(ObjectList* obj, uint32_t instId) +{ + ObjectInstList* elemEntry; + + // Create new instance + if (!obj->isSingleInstance) + { + elemEntry = (ObjectInstList*)malloc(sizeof(ObjectInstList)); + if (elemEntry == NULL) return -1; + elemEntry->data = malloc(obj->numBytes); + if (elemEntry->data == NULL) return -1; + memset(elemEntry->data, 0, obj->numBytes); + elemEntry->instId = instId; + LL_APPEND(obj->data.instances, elemEntry); + ++obj->numInstances; + return instId; + } + else + { + return -1; + } +} + +/** + * Check if the object has the instance ID specified. + */ +int32_t hasInstance(ObjectList* obj, uint32_t instId) +{ + ObjectInstList* elemEntry; + + // Get data depending on data format + if (!obj->isSingleInstance) + { + // Look for specified instance ID + LL_FOREACH(obj->data.instances, elemEntry) + { + if (elemEntry->instId == instId) + { + return 0; + } + } + // If this point is reached then instance id was not found + return -1; + } + else + { + return -1; + } +} + + + + + + + + + + + + + + + + + diff --git a/flight/OpenPilot/UAVObjects/uavobjecttemplate.c b/flight/OpenPilot/UAVObjects/uavobjecttemplate.c index 495e5a1ba..bfe180529 100644 --- a/flight/OpenPilot/UAVObjects/uavobjecttemplate.c +++ b/flight/OpenPilot/UAVObjects/uavobjecttemplate.c @@ -32,38 +32,12 @@ #include // for memcpy #include "$(NAMELC).h" -#ifndef METAOBJECT -#include "$(NAMELC)meta.h" -#endif //METAOBJECT -#include "uavobjectlist.h" -#include "uavobjectutils.h" -#include "FreeRTOS.h" -#include "semphr.h" +#include "uavobjectmanager.h" + -// Private constants -#define OBJECT_ID $(OBJID) -#define METADATA_ID $(METAID) -#define NAME $(NAMESTR) // Private variables -ObjectContext context; -$(NAME)Data data; -uint32_t objectID; -#ifdef METAOBJECT -UAVObjectMetadata metaData; -#endif //METAOBJECT - -// Private functions -int32_t pack(uint32_t objId, uint8_t* data, int32_t maxLength); -int32_t unpack(uint32_t objId, uint8_t* data, int32_t length); -int32_t initializeData(const char* init); -void getMetadata(UAVObjectMetadata* dataOut); -void setMetadata(const UAVObjectMetadata* dataIn); -void requestUpdate(); -void updated(); -int32_t connect(xQueueHandle queue, int32_t ignoreUnpack); -int32_t disconnect(xQueueHandle queue); -int32_t getFieldIndexByName(char* name); +UAVObjHandle handle; /** * Initialize object. @@ -72,197 +46,29 @@ int32_t getFieldIndexByName(char* name); */ int32_t $(NAME)Initialize() { - // Create mutex - context.mutex = xSemaphoreCreateRecursiveMutex(); - if (context.mutex == NULL) - return -1; + UAVObjMetadata metadata; - // Setup UAVObject table - context.obj.objectID = OBJECT_ID; - context.obj.metadataID = METADATA_ID; -#ifndef METAOBJECT - context.obj.isMetadata = 0; -#else - context.obj.isMetadata = 1; -#endif //METAOBJECT - context.obj.name = NAME; - context.obj.numBytes = sizeof($(NAME)Data); - context.obj.pack = &pack; - context.obj.unpack = &unpack; - context.obj.initializeData = initializeData; - context.obj.getMetadata = &getMetadata; - context.obj.setMetadata = &setMetadata; - context.obj.requestUpdate = &requestUpdate; - context.obj.updated = &updated; - context.obj.connect = &connect; - context.obj.disconnect = &disconnect; + // Register object with the object manager + handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, 0, $(NAMEUC)_SINGLEINST, $(NAMEUC)_NUMBYTES); + if (handle == 0) return -1; - // Initialise connected queue list - context.queues = NULL; - objectID = OBJECT_ID; - - // Initialise linked object -#ifndef METAOBJECT - $(NAME)MetaInitialize(); - context.obj.linkedObj = $(NAME)MetaGet(); - context.obj.linkedObj->linkedObj = &context.obj; -#else - context.obj.linkedObj = NULL; -#endif //METAOBJECT - - // Register object with object list - UAVObjectListRegister(&context.obj); + // Initialize meta data + metadata.ackRequired = $(ACK); + metadata.gcsTelemetryUpdateMode = $(GCSTELEM_UPDATEMODE); + metadata.gcsTelemetryUpdatePeriod = $(GCSTELEM_UPDATEPERIOD); + metadata.telemetryUpdateMode = $(TELEM_UPDATEMODE); + metadata.telemetryUpdatePeriod = $(TELEM_UPDATEPERIOD); + metadata.loggingUpdateMode = $(LOGGING_UPDATEMODE); + metadata.loggingUpdatePeriod = $(LOGGING_UPDATEPERIOD); + UAVObjSetMetadata(handle, &metadata); + // Done return 0; } -/** - * See UAVObject.h description for more details. - */ -int32_t pack(uint32_t objId, uint8_t* dataOut, int32_t maxLength) +UAVObjHandle $(NAME)GetHandle() { - int32_t res; - // Lock - xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY); - // Pack data - if (sizeof($(NAME)Data) <= maxLength) - { - memcpy(dataOut, &data, sizeof($(NAME)Data)); - res = sizeof($(NAME)Data); - } - else - { - res = -1; - } - // Unlock and return - xSemaphoreGiveRecursive(context.mutex); - return sizeof($(NAME)Data); -} - -/** - * See UAVObject.h description for more details. - */ -int32_t unpack(uint32_t objId, uint8_t* dataIn, int32_t length) -{ - // Lock - xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY); - // Unpack data - memcpy(&data, dataIn, sizeof($(NAME)Data)); - // Trigger event - UAVObjUtilsSendEvent(&context, QMSG_UNPACKED); - // Unlock and return - xSemaphoreGiveRecursive(context.mutex); - return 0; -} - -/** - * See UAVObject.h description for more details. - */ -int32_t initializeData(const char* init) -{ - return -1; -} - -/** - * See UAVObject.h description for more details. - */ -void getMetadata(UAVObjectMetadata* dataOut) -{ - // Get lock - xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY); - - // Get data -#ifndef METAOBJECT - $(NAME)MetaGetData(dataOut); -#else - memcpy(dataOut, &metaData, sizeof(UAVObjectMetadata)); -#endif //METAOBJECT - - // Release lock - xSemaphoreGiveRecursive(context.mutex); -} - -/** - * See UAVObject.h description for more details. - */ -void setMetadata(const UAVObjectMetadata* dataIn) -{ - // Get lock - xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY); - - // Get data -#ifndef METAOBJECT - $(NAME)MetaSetData(dataIn); -#else - memcpy(&metaData, dataIn, sizeof(UAVObjectMetadata)); -#endif //METAOBJECT - - // Release lock - xSemaphoreGiveRecursive(context.mutex); -} - -/** - * See UAVObject.h description for more details. - */ -void requestUpdate() -{ - UAVObjUtilsRequestUpdate(&context); -} - -/** - * See UAVObject.h description for more details. - */ -void updated() -{ - UAVObjUtilsUpdated(&context); -} - -/** - * See UAVObject.h description for more details. - */ -int32_t connect(xQueueHandle queue, int32_t eventMask) -{ - return UAVObjUtilsConnect(&context, queue, eventMask); -} - -/** - * See UAVObject.h description for more details. - */ -int32_t disconnect(xQueueHandle queue) -{ - return UAVObjUtilsDisconnect(&context, queue); -} - -/** - * Get object's data structure. - * \param[out] dataOut Data structure to copy data into - */ -void $(NAME)GetData($(NAME)Data* dataOut) -{ - // Get lock - xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY); - // Copy data - memcpy(dataOut, &data, sizeof($(NAME)Data)); - // Release lock - xSemaphoreGiveRecursive(context.mutex); -} - -/** - * Set object's data structure. - * \param[in] dataIn Data structure to copy data from. - */ -void $(NAME)SetData(const $(NAME)Data* dataIn) -{ - // Get lock - xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY); - // Copy data - memcpy(&data, dataIn, sizeof($(NAME)Data)); - // Send notification for updates - UAVObjUtilsSendEvent(&context, QMSG_UPDATED); - // Release lock - xSemaphoreGiveRecursive(context.mutex); + return handle; } - - diff --git a/flight/OpenPilot/UAVObjects/uavobjectutils.c b/flight/OpenPilot/UAVObjects/uavobjectutils.c deleted file mode 100644 index 761fa77f3..000000000 --- a/flight/OpenPilot/UAVObjects/uavobjectutils.c +++ /dev/null @@ -1,133 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavobjectutils.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Functions common to all objects, they are included in this file - * to avoid duplication on each generated object. The functions - * in this file should be called only by UAVObjects. - * @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 // for malloc -#include // for memcpy -#include "uavobjectutils.h" -#include "uavobject.h" -#include "utlist.h" - -/** - * See UAVObject.h description for more details. - */ -int32_t UAVObjUtilsConnect(ObjectContext* context, xQueueHandle queue, int32_t eventMask) -{ - ObjectQueueList* queueHandle; - - // Lock - xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY); - - // Check that the queue is not already connected, if it is simply update event mask - LL_FOREACH(context->queues, queueHandle) - { - if (queueHandle->queue == queue) - { - // Already connected, update event mask and return - queueHandle->eventMask = eventMask; - xSemaphoreGiveRecursive(context->mutex); - return 0; - } - } - - // Add queue to list - queueHandle = (ObjectQueueList*)malloc(sizeof(ObjectQueueList)); - queueHandle->queue = queue; - queueHandle->eventMask = eventMask; - LL_APPEND(context->queues, queueHandle); - - // Done - xSemaphoreGiveRecursive(context->mutex); - return 0; -} - -/** - * See UAVObject.h description for more details. - */ -int32_t UAVObjUtilsDisconnect(ObjectContext* context, xQueueHandle queue) -{ - ObjectQueueList* queueHandle; - - // Lock - xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY); - - // Find queue and remove it - LL_FOREACH(context->queues, queueHandle) - { - if (queueHandle->queue == queue) - { - LL_DELETE(context->queues, queueHandle); - xSemaphoreGiveRecursive(context->mutex); - return 0; - } - } - - // If this point is reached the queue was not found - xSemaphoreGiveRecursive(context->mutex); - return -1; -} - -/** - * See UAVObject.h description for more details. - */ -void UAVObjUtilsSendEvent(ObjectContext* context, UAVObjectQMsgEvent event) -{ - ObjectQueueList* queueHandle; - UAVObjectQMsg msg; - - // Setup event - msg.obj = &context->obj; - msg.event = event; - - // Go through each object and push the object ID in the queue (if event is activated for the queue) - LL_FOREACH(context->queues, queueHandle) - { - if ( queueHandle->eventMask == 0 || (queueHandle->eventMask & event) != 0 ) - { - xQueueSend(queueHandle->queue, &msg, 0); // do not wait if queue is full - } - } -} - -/** - * See UAVObject.h description for more details. - */ -void UAVObjUtilsRequestUpdate(ObjectContext* context) -{ - xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY); - UAVObjUtilsSendEvent(context, QMSG_UPDATE_REQ); - xSemaphoreGiveRecursive(context->mutex); -} - -/** - * See UAVObject.h description for more details. - */ -void UAVObjUtilsUpdated(ObjectContext* context) -{ - xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY); - UAVObjUtilsSendEvent(context, QMSG_UPDATED_MANUAL); - xSemaphoreGiveRecursive(context->mutex); -} diff --git a/flight/OpenPilot/UAVTalk/inc/uavtalk.h b/flight/OpenPilot/UAVTalk/inc/uavtalk.h index 8525c5216..78da81de2 100644 --- a/flight/OpenPilot/UAVTalk/inc/uavtalk.h +++ b/flight/OpenPilot/UAVTalk/inc/uavtalk.h @@ -23,29 +23,24 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef UAVTALK_C_H -#define UAVTALK_C_H +#ifndef UAVTALK_H +#define UAVTALK_H #include +#include "uavobjectmanager.h" // Public constants #define UAVTALK_WAITFOREVER -1 #define UAVTALK_NOWAIT 0 // Public types -typedef int32_t (*UAVTalkUnpackCb)(uint32_t objId, uint8_t* data, int32_t length); -typedef int32_t (*UAVTalkPackCb)(uint32_t objId, uint8_t* data, int32_t maxLength); typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); // Public functions int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream); -int32_t UAVTalkConnectObject(uint32_t objectId, UAVTalkPackCb packCb, UAVTalkUnpackCb unpackCb, int32_t updatePeriodMs); -int32_t UAVTalkSetUpdatePeriod(uint32_t objectId, int32_t updatePeriodMs); int32_t UAVTalkSetOutputStream(UAVTalkOutputStream outputStream); -int32_t UAVTalkSendObject(uint32_t objectId, uint8_t acked, int32_t timeoutMs); -int32_t UAVTalkSendObjectRequest(uint32_t objectId, int32_t timeoutMs); +int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); +int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); int32_t UAVTalkProcessInputStream(uint8_t rxbyte); -int32_t UAVTalkProcessPeriodicUpdates(void); - -#endif // UAVTALK_C_H +#endif // UAVTALK_H diff --git a/flight/OpenPilot/UAVTalk/uavtalk.c b/flight/OpenPilot/UAVTalk/uavtalk.c index 21ae8b615..18ed5c895 100644 --- a/flight/OpenPilot/UAVTalk/uavtalk.c +++ b/flight/OpenPilot/UAVTalk/uavtalk.c @@ -25,19 +25,17 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include // for malloc #include "uavtalk.h" -#include "utlist.h" #include "FreeRTOS.h" #include "semphr.h" // Private constants #define TYPE_MASK 0xFC -#define TYPE_BASE 0x50 -#define TYPE_OBJ (TYPE_BASE | 0x00) -#define TYPE_OBJ_REQ (TYPE_BASE | 0x01) -#define TYPE_OBJ_ACK (TYPE_BASE | 0x02) -#define TYPE_ACK (TYPE_BASE | 0x03) +#define TYPE_VER 0x10 +#define TYPE_OBJ (TYPE_VER | 0x00) +#define TYPE_OBJ_REQ (TYPE_VER | 0x01) +#define TYPE_OBJ_ACK (TYPE_VER | 0x02) +#define TYPE_ACK (TYPE_VER | 0x03) #define HEADER_LENGTH 6 // type (1), object ID (4), length (1) #define CHECKSUM_LENGTH 2 @@ -47,33 +45,23 @@ #define MIN_UPDATE_PERIOD_MS 1 // Private types -struct ObjectHandleStruct { - uint32_t objectId; - UAVTalkUnpackCb packCb; - UAVTalkUnpackCb unpackCb; - xSemaphoreHandle sema; - uint32_t waitingResp; - int32_t updatePeriodMs; - int32_t timeToNextUpdateMs; - struct ObjectHandleStruct* next; -}; -typedef struct ObjectHandleStruct ObjectHandle; -typedef enum {STATE_SYNC, STATE_OBJID, STATE_LENGTH, STATE_DATA, STATE_CS} RxState; +typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState; // Private variables UAVTalkOutputStream outStream; -ObjectHandle* objects; -int32_t timeToNextUpdateMs; -xSemaphoreHandle mutex; +xSemaphoreHandle lock; +xSemaphoreHandle respSema; +UAVObjHandle respObj; +uint16_t respInstId; uint8_t rxBuffer[MAX_PACKET_LENGTH]; uint8_t txBuffer[MAX_PACKET_LENGTH]; // Private functions uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length); -ObjectHandle* findObject(uint32_t objId); -int32_t objectTransaction(uint32_t objectId, uint8_t type, int32_t timeout); -int32_t sendObject(ObjectHandle* obj, uint8_t type); -int32_t receiveObject(uint8_t type, ObjectHandle* obj, uint8_t* data, int32_t length); +int32_t objectTransaction(uint32_t objectId, uint16_t instId, uint8_t type, int32_t timeout); +int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type); +int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type); +int32_t receiveObject(uint8_t type, UAVObjHandle obj, uint16_t instId, uint8_t* data, int32_t length); /** * Initialize the UAVTalk library @@ -81,110 +69,50 @@ int32_t receiveObject(uint8_t type, ObjectHandle* obj, uint8_t* data, int32_t le * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream) { +int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream) +{ outStream = outputStream; - mutex = xSemaphoreCreateRecursiveMutex(); - timeToNextUpdateMs = 0; - objects = NULL; - + lock = xSemaphoreCreateRecursiveMutex(); + vSemaphoreCreateBinary(respSema); return 0; } -/** - * Connect an object to the UAVTalk library. All objects needs to be registered, this is needed - * so that the library knows how to call the pack and unpack functions of the object. - * \param[in] objectId ID of the object - * \param[in] packCb Callback function that is used to pack the object, called each time the object needs to be sent. - * \param[in] unpackCb Callback function that is used to unpack the object, called each time the object is received. - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVTalkConnectObject(uint32_t objectId, UAVTalkPackCb packCb, UAVTalkUnpackCb unpackCb, int32_t updatePeriodMs) { - ObjectHandle* obj; - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Check that the object is not already connected - LL_FOREACH(objects, obj) - { - if (obj->objectId == objectId) - { - // Already registered, ignore - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - // Create handle - obj = (ObjectHandle*)malloc(sizeof(ObjectHandle)); - obj->objectId = objectId; - obj->packCb = packCb; - obj->unpackCb = unpackCb; - vSemaphoreCreateBinary(obj->sema); - obj->waitingResp = 0; - obj->updatePeriodMs = updatePeriodMs; - obj->timeToNextUpdateMs = 0; - // Add to list - LL_APPEND(objects, obj); - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Setup object for periodic updates. - * \param[in] objectId ID of the object to update - * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVTalkSetUpdatePeriod(uint32_t objectId, int32_t updatePeriodMs) { - ObjectHandle* obj; - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - // Get and update object - obj = findObject(objectId); - if (obj != 0) { - obj->updatePeriodMs = updatePeriodMs; - obj->timeToNextUpdateMs = 0; - xSemaphoreGiveRecursive(mutex); - return 0; - } - else { - xSemaphoreGiveRecursive(mutex); - return -1; - } -} - /** * Request an update for the specified object, on success the object data would have been * updated by the GCS. - * \param[in] objectId ID of the object to update + * \param[in] obj Object to update + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] timeout Time to wait for the response, when zero it will return immediately * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSendObjectRequest(uint32_t objectId, int32_t timeout) { - return objectTransaction(objectId, TYPE_OBJ_REQ, timeout); +int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeout) +{ + return objectTransaction(obj, instId, TYPE_OBJ_REQ, timeout); } /** * Send the specified object through the telemetry link. - * \param[in] objectId ID of the object to send + * \param[in] obj Object to send + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required) * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSendObject(uint32_t objectId, uint8_t acked, int32_t timeoutMs) { +int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) +{ if (acked == 1) { - return objectTransaction(objectId, TYPE_OBJ_ACK, timeoutMs); + return objectTransaction(obj, instId, TYPE_OBJ_ACK, timeoutMs); } else { - return objectTransaction(objectId, TYPE_OBJ, timeoutMs); + return objectTransaction(obj, instId, TYPE_OBJ, timeoutMs); } } /** * Execute the requested transaction on an object. - * \param[in] objectId ID of object + * \param[in] obj Object + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type * TYPE_OBJ: send object, * TYPE_OBJ_REQ: request object update @@ -192,101 +120,59 @@ int32_t UAVTalkSendObject(uint32_t objectId, uint8_t acked, int32_t timeoutMs) { * \return 0 Success * \return -1 Failure */ -int32_t objectTransaction(uint32_t objectId, uint8_t type, int32_t timeoutMs) { - ObjectHandle* obj; +int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) +{ + int32_t respReceived; - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Find object - obj = findObject(objectId); - if (obj == 0) { - xSemaphoreGiveRecursive(mutex); - return -1; - } + // Lock + xSemaphoreTakeRecursive(lock, portMAX_DELAY); // Send object depending on if a response is needed - if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { - sendObject(obj, type); - xSemaphoreGiveRecursive(mutex); // need to release lock since the next call will block until a response is received - xSemaphoreTake(obj->sema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema) - xSemaphoreTake(obj->sema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout) - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // complete transaction - // Check if a response was received - if (obj->waitingResp == 1) { - obj->waitingResp = 0; - xSemaphoreGiveRecursive(mutex); - return -1; - } else { - xSemaphoreGiveRecursive(mutex); - return 0; - } - } else if (type == TYPE_OBJ) { - sendObject(obj, TYPE_OBJ); - xSemaphoreGiveRecursive(mutex); + if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) + { + sendObject(obj, instId, type); + respObj = obj; + respInstId = instId; + xSemaphoreGiveRecursive(lock); // need to release lock since the next call will block until a response is received + xSemaphoreTake(respSema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema) + respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout) + // Check if a response was received + if (respReceived == pdFALSE) + { + return -1; + } + else + { + return 0; + } + } + else if (type == TYPE_OBJ) + { + sendObject(obj, instId, TYPE_OBJ); + xSemaphoreGiveRecursive(lock); return 0; - } else { - xSemaphoreGiveRecursive(mutex); + } + else + { + xSemaphoreGiveRecursive(lock); return -1; } } -/** - * Handle periodic updates for all objects. - * \return The time to wait until the next update (in ms) - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVTalkProcessPeriodicUpdates(void) { - ObjectHandle* obj; - int32_t minDelay = MAX_UPDATE_PERIOD_MS; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Iterate through each object and update its timer, if zero then transmit object. - // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs) - LL_FOREACH(objects, obj) { - // If object is configured for periodic updates - if (obj->updatePeriodMs > 0) { - obj->timeToNextUpdateMs -= timeToNextUpdateMs; - // Check if time for the next update - if (obj->timeToNextUpdateMs <= 0) { - // Reset timer - obj->timeToNextUpdateMs = obj->updatePeriodMs; - // Send object - sendObject(obj, TYPE_OBJ); - } - // Update minimum delay - if (obj->timeToNextUpdateMs < minDelay) { - minDelay = obj->timeToNextUpdateMs; - } - } - } - - // Check if delay for the next update is too short - if (minDelay < MIN_UPDATE_PERIOD_MS) { - minDelay = MIN_UPDATE_PERIOD_MS; - } - - // Done - timeToNextUpdateMs = minDelay; - xSemaphoreGiveRecursive(mutex); - return timeToNextUpdateMs; -} - /** * Process an byte from the telemetry stream. * \param[in] rxbyte Received byte * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkProcessInputStream(uint8_t rxbyte) { +int32_t UAVTalkProcessInputStream(uint8_t rxbyte) +{ static uint8_t tmpBuffer[4]; - static ObjectHandle* obj; + static UAVObjHandle obj; static uint8_t type; static uint32_t objId; - static uint8_t length; + static uint16_t instId; + static uint32_t length; static uint16_t cs, csRx; static int32_t rxCount; static RxState state = STATE_SYNC; @@ -294,7 +180,8 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) { // Receive state machine switch (state) { case STATE_SYNC: - if ((rxbyte & TYPE_MASK) == TYPE_BASE ) { + if ((rxbyte & TYPE_MASK) == TYPE_VER ) + { cs = rxbyte; type = rxbyte; state = STATE_OBJID; @@ -303,39 +190,81 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) { break; case STATE_OBJID: tmpBuffer[rxCount++] = rxbyte; - if (rxCount == 4) { + if (rxCount == 4) + { // Search for object, if not found reset state machine objId = (tmpBuffer[3] << 24) | (tmpBuffer[2] << 16) | (tmpBuffer[1] << 8) | (tmpBuffer[0]); - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - obj = findObject(objId); - xSemaphoreGiveRecursive(mutex); - if (obj == 0) { + obj = UAVObjGetByID(objId); + if (obj == 0) + { state = STATE_SYNC; - } else { + } + else + { + // Update checksum cs = updateChecksum(cs, tmpBuffer, 4); - state = STATE_LENGTH; - rxCount = 0; + // Determine data length + if (type == TYPE_OBJ_REQ || type == TYPE_ACK) + { + length = 0; + } + else + { + length = UAVObjGetNumBytes(obj); + } + // Check length and determine next state + if (length >= MAX_PAYLOAD_LENGTH) + { + state = STATE_SYNC; + } + else + { + // Check if this is a single instance object (i.e. if the instance ID field is coming next) + if ( UAVObjIsSingleInstance(obj) ) + { + // If there is a payload get it, otherwise receive checksum + if (length > 0) + { + state = STATE_DATA; + } + else + { + state = STATE_CS; + } + instId = 0; + rxCount = 0; + } + else + { + state = STATE_INSTID; + rxCount = 0; + } + } } } break; - case STATE_LENGTH: - length = (int32_t)rxbyte; - if (length > MAX_PAYLOAD_LENGTH || - ((type == TYPE_OBJ_REQ || type == TYPE_ACK) && length != 0)) { - state = STATE_SYNC; - } else { - cs = updateChecksum(cs, &length, 1); - rxCount = 0; - if (length > 0) { - state = STATE_DATA; - } else { - state = STATE_CS; - } + case STATE_INSTID: + tmpBuffer[rxCount++] = rxbyte; + if (rxCount == 2) + { + instId = (tmpBuffer[1] << 8) | (tmpBuffer[0]); + cs = updateChecksum(cs, tmpBuffer, 2); + rxCount = 0; + // If there is a payload get it, otherwise receive checksum + if (length > 0) + { + state = STATE_DATA; + } + else + { + state = STATE_CS; + } } break; case STATE_DATA: rxBuffer[rxCount++] = rxbyte; - if (rxCount == length) { + if (rxCount == length) + { cs = updateChecksum(cs, rxBuffer, length); state = STATE_CS; rxCount = 0; @@ -343,12 +272,14 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) { break; case STATE_CS: tmpBuffer[rxCount++] = rxbyte; - if (rxCount == 2) { + if (rxCount == 2) + { csRx = (tmpBuffer[1] << 8) | (tmpBuffer[0]); - if (csRx == cs) { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - receiveObject(type, obj, rxBuffer, length); - xSemaphoreGiveRecursive(mutex); + if (csRx == cs) + { + xSemaphoreTakeRecursive(lock, portMAX_DELAY); + receiveObject(type, obj, instId, rxBuffer, length); + xSemaphoreGiveRecursive(lock); } state = STATE_SYNC; } @@ -365,33 +296,39 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) { * Receive an object. This function process objects received through the telemetry stream. * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK) * \param[in] obj Handle of the received object + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] data Data buffer * \param[in] length Buffer length * \return 0 Success * \return -1 Failure */ -int32_t receiveObject(uint8_t type, ObjectHandle* obj, uint8_t* data, int32_t length) { +int32_t receiveObject(uint8_t type, UAVObjHandle obj, uint16_t instId, uint8_t* data, int32_t length) +{ // Unpack object if the message is of type OBJ or OBJ_ACK - if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { - (obj->unpackCb)(obj->objectId, data, length); + if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) + { + UAVObjUnpack(obj, instId, data); } // Send requested object if message is of type OBJ_REQ - if (type == TYPE_OBJ_REQ) { - sendObject(obj, TYPE_OBJ); + if (type == TYPE_OBJ_REQ) + { + sendObject(obj, instId, TYPE_OBJ); } // Send ACK if message is of type OBJ_ACK - if (type == TYPE_OBJ_ACK) { - sendObject(obj, TYPE_ACK); + if (type == TYPE_OBJ_ACK) + { + sendObject(obj, instId, TYPE_ACK); } // If a response was pending on the object, unblock any waiting tasks - if (type == TYPE_ACK || type == TYPE_OBJ) { - if (obj->waitingResp == 1) { - obj->waitingResp = 0; - xSemaphoreGive(obj->sema); - } + if (type == TYPE_ACK || type == TYPE_OBJ) + { + if (respObj == obj && (respInstId = instId || respInstId == UAVOBJ_ALL_INSTANCES)) + { + xSemaphoreGive(respSema); + } } // Done @@ -401,52 +338,116 @@ int32_t receiveObject(uint8_t type, ObjectHandle* obj, uint8_t* data, int32_t le /** * Send an object through the telemetry link. * \param[in] obj Object handle to send + * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances * \param[in] type Transaction type * \return 0 Success - * \return -1 Failure + * \return -1 Failure */ -int32_t sendObject(ObjectHandle* obj, uint8_t type) { +int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type) +{ + uint32_t numInst; + uint32_t n; + + // Check if this operation is for a single instance or all + if (instId == UAVOBJ_ALL_INSTANCES && !UAVObjIsSingleInstance(obj)) + { + if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) + { + // Get number of instances + numInst = UAVObjGetNumInstances(obj); + // Send all instances + for (n = 0; n < numInst; ++n) + { + sendSingleObject(obj, n, type); + } + return 0; + } + else + { + return -1; + } + } + else + { + return sendSingleObject(obj, instId, type); + } +} + + +/** + * Send an object through the telemetry link. + * \param[in] obj Object handle to send + * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead) + * \param[in] type Transaction type + * \return 0 Success + * \return -1 Failure + */ +int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) +{ int32_t length; + int32_t dataOffset; uint16_t cs = 0; + uint32_t objId; // Check for valid packet type - if (type != TYPE_OBJ && type != TYPE_OBJ_ACK && type != TYPE_OBJ_REQ && type != TYPE_ACK) { + if (type != TYPE_OBJ && type != TYPE_OBJ_ACK && type != TYPE_OBJ_REQ && type != TYPE_ACK) + { return -1; } - // If a response is expected, set the flag - if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { - obj->waitingResp = 1; - } - // Setup type and object id fields + objId = UAVObjGetID(obj); txBuffer[0] = type; - txBuffer[1] = (uint8_t)(obj->objectId & 0xFF); - txBuffer[2] = (uint8_t)((obj->objectId >> 8) & 0xFF); - txBuffer[3] = (uint8_t)((obj->objectId >> 16) & 0xFF); - txBuffer[4] = (uint8_t)((obj->objectId >> 24) & 0xFF); + txBuffer[1] = (uint8_t)(objId & 0xFF); + txBuffer[2] = (uint8_t)((objId >> 8) & 0xFF); + txBuffer[3] = (uint8_t)((objId >> 16) & 0xFF); + txBuffer[4] = (uint8_t)((objId >> 24) & 0xFF); - // Setup length and data field (if one) - if (type == TYPE_ACK || type == TYPE_OBJ_REQ) { - length = 0; - } else { - // Pack object - length = (obj->packCb)(obj->objectId, &txBuffer[HEADER_LENGTH], MAX_PAYLOAD_LENGTH); - // Check length - if (length > MAX_PAYLOAD_LENGTH || length <= 0) { - return -1; - } + // Setup instance ID if one is required + if (UAVObjIsSingleInstance(obj)) + { + dataOffset = 5; + } + else + { + txBuffer[5] = (uint8_t)(instId & 0xFF); + txBuffer[6] = (uint8_t)((instId >> 8) & 0xFF); + dataOffset = 7; + } + + // Determine data length + if (type == TYPE_OBJ_REQ || type == TYPE_ACK) + { + length = 0; + } + else + { + length = UAVObjGetNumBytes(obj); + } + + // Check length + if (length >= MAX_PAYLOAD_LENGTH) + { + return -1; + } + + // Copy data (if any) + if (length > 0) + { + if ( UAVObjPack(obj, instId, &txBuffer[dataOffset]) < 0 ) + { + return -1; + } } - txBuffer[5] = (uint8_t)length; // Calculate checksum cs = 0; - cs = updateChecksum(cs, txBuffer, HEADER_LENGTH+length); - txBuffer[HEADER_LENGTH+length] = (uint8_t)(cs & 0xFF); - txBuffer[HEADER_LENGTH+length+1] = (uint8_t)((cs >> 8) & 0xFF); + cs = updateChecksum(cs, txBuffer, dataOffset+length); + txBuffer[dataOffset+length] = (uint8_t)(cs & 0xFF); + txBuffer[dataOffset+length+1] = (uint8_t)((cs >> 8) & 0xFF); // Send buffer - if (outStream!=NULL) (*outStream)(txBuffer, HEADER_LENGTH+length+CHECKSUM_LENGTH); + if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH); // Done return 0; @@ -459,26 +460,19 @@ int32_t sendObject(ObjectHandle* obj, uint8_t type) { * \param[in] length Length of buffer * \return Updated checksum */ -uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length) { +uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length) +{ int32_t n; - for (n = 0; n < length; ++n) { + for (n = 0; n < length; ++n) + { cs += (uint16_t)data[n]; } return cs; } -/** - * Find an object handle given the object ID - * \param[in] objId Object ID - * \return The object handle or NULL if not found - */ -ObjectHandle* findObject(uint32_t objId) { - ObjectHandle* obj; - LL_FOREACH(objects, obj) { - if (obj->objectId == objId) { - return obj; - } - } - return NULL; -} + + + + +