rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@199 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-02-27 19:57:45 +00:00 committed by vassilis
parent 163862af54
commit 93901e8754
14 changed files with 1403 additions and 977 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/Makefile
View File

@ -102,8 +102,7 @@ SRC = $(MODTELEMETRY)/telemetry.c
SRC += $(OPSYSTEM)/openpilot.c SRC += $(OPSYSTEM)/openpilot.c
SRC += $(OPSYSTEM)/op_logging.c SRC += $(OPSYSTEM)/op_logging.c
SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectutils.c SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/uavobjectlist.c
SRC += $(OPUAVOBJ)/uavobjectsinit.c SRC += $(OPUAVOBJ)/uavobjectsinit.c
## UAVOBJECTS ## UAVOBJECTS

201
flight/Modules/Telemetry/telemetry.c
View File

@ -23,10 +23,11 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include <stdlib.h> // for malloc
#include "telemetry.h" #include "telemetry.h"
#include "uavtalk.h" #include "uavtalk.h"
#include "uavobject.h" #include "uavobjectmanager.h"
#include "uavobjectlist.h" #include "utlist.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
#include "queue.h" #include "queue.h"
@ -38,17 +39,36 @@
#define TASK_PRIORITY 100 #define TASK_PRIORITY 100
#define REQ_TIMEOUT_MS 500 #define REQ_TIMEOUT_MS 500
#define MAX_RETRIES 2 #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 // Private variables
xQueueHandle queue; xQueueHandle queue;
xTaskHandle telemetryTaskHandle; xTaskHandle telemetryTaskHandle;
ObjectList* objList;
// Private functions // Private functions
void telemetryTask(); void telemetryTask();
void receiveTask(); void receiveTask();
int32_t transmitData(uint8_t* data, int32_t length); int32_t transmitData(uint8_t* data, int32_t length);
void registerObject(UAVObject* obj); void registerObject(UAVObjHandle obj);
void updateObject(UAVObject* 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 * Initialize the telemetry module
@ -57,8 +77,11 @@ void updateObject(UAVObject* obj);
*/ */
int32_t TelemetryInitialize() int32_t TelemetryInitialize()
{ {
// Initialize object list
objList = NULL;
// Create object queue // Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjectQMsg)); queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjQMsg));
// TODO: Get telemetry settings object // TODO: Get telemetry settings object
@ -67,8 +90,8 @@ int32_t TelemetryInitialize()
// Initialize UAVTalk // Initialize UAVTalk
UAVTalkInitialize(&transmitData); UAVTalkInitialize(&transmitData);
// Process all registered objects, register in UAVTalk and connect queue for updates // Process all registered objects and connect queue for updates
UAVObjectListIterate(&registerObject); UAVObjIterate(&registerObject);
// Start tasks // Start tasks
xTaskCreate( telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle ); 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. * telemetry settings.
* \param[in] obj Object to connect * \param[in] obj Object to connect
*/ */
void registerObject(UAVObject* obj) void registerObject(UAVObjHandle obj)
{ {
// Register object in UAVTalk // Add object to list
UAVTalkConnectObject(obj->objectID, obj->pack, obj->unpack, 0); addObject(obj);
// Setup object for telemetry updates // Setup object for telemetry updates
updateObject(obj); updateObject(obj);
@ -95,57 +118,57 @@ void registerObject(UAVObject* obj)
* Update object's queue connections and timer, depending on object's settings * Update object's queue connections and timer, depending on object's settings
* \param[in] obj Object to updates * \param[in] obj Object to updates
*/ */
void updateObject(UAVObject* obj) void updateObject(UAVObjHandle obj)
{ {
UAVObjectMetadata metadata; UAVObjMetadata metadata;
int32_t eventMask; int32_t eventMask;
// Get metadata // Get metadata
obj->getMetadata(&metadata); UAVObjGetMetadata(obj, &metadata);
// Setup object depending on update mode // Setup object depending on update mode
if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC) if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC)
{ {
// Set update period // Set update period
UAVTalkSetUpdatePeriod(obj->objectID, metadata.telemetryUpdatePeriod); setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
// Connect queue // Connect queue
eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ; 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) 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) else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE)
{ {
// Set update period // Set update period
UAVTalkSetUpdatePeriod(obj->objectID, 0); setUpdatePeriod(obj, 0);
// Connect queue // Connect queue
eventMask = QMSG_UPDATED|QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ; 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) 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) else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL)
{ {
// Set update period // Set update period
UAVTalkSetUpdatePeriod(obj->objectID, 0); setUpdatePeriod(obj, 0);
// Connect queue // Connect queue
eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ; 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) 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) else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER)
{ {
// Set update period // Set update period
UAVTalkSetUpdatePeriod(obj->objectID, 0); setUpdatePeriod(obj, 0);
// Disconnect queue // Disconnect queue
obj->disconnect(queue); UAVObjDisconnect(obj, queue);
} }
} }
@ -154,12 +177,12 @@ void updateObject(UAVObject* obj)
*/ */
void telemetryTask() void telemetryTask()
{ {
static int32_t timeToNextUpdateMs; int32_t timeToNextUpdateMs;
static int32_t delayMs; int32_t delayMs;
static UAVObjectQMsg msg; UAVObjQMsg msg;
static UAVObjectMetadata metadata; UAVObjMetadata metadata;
static int32_t retries; int32_t retries;
static int32_t success; int32_t success;
// Initialize time // Initialize time
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS; timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS;
@ -178,7 +201,7 @@ void telemetryTask()
if ( xQueueReceive(queue, &msg, delayMs/portTICK_RATE_MS) == pdTRUE ) if ( xQueueReceive(queue, &msg, delayMs/portTICK_RATE_MS) == pdTRUE )
{ {
// Get object metadata // Get object metadata
msg.obj->getMetadata(&metadata); UAVObjGetMetadata(msg.obj, &metadata);
// Act on event // Act on event
if (msg.event == QMSG_UPDATED || msg.event == QMSG_UPDATED_MANUAL) if (msg.event == QMSG_UPDATED || msg.event == QMSG_UPDATED_MANUAL)
{ {
@ -186,7 +209,7 @@ void telemetryTask()
retries = 0; retries = 0;
while (retries < MAX_RETRIES && success == -1) 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; ++retries;
} }
} }
@ -196,29 +219,28 @@ void telemetryTask()
retries = 0; retries = 0;
while (retries < MAX_RETRIES && success == -1) 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; ++retries;
} }
} }
// If this is a metadata object then make necessary telemetry updates // 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 // Process periodic updates
if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs ) if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs )
{ {
delayMs = UAVTalkProcessPeriodicUpdates(); timeToNextUpdateMs = processPeriodicUpdates();
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS + delayMs;
} }
} }
} }
/** /**
* Receive task. Processes received bytes (from the modem or USB) and passes them to * 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() void receiveTask()
{ {
@ -240,5 +262,106 @@ int32_t transmitData(uint8_t* data, int32_t length)
return 0; 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;
}

0
flight/OpenPilot/Settings/inc/settings.h Normal file
View File

0
flight/OpenPilot/Settings/settings.c Normal file
View File

38
flight/OpenPilot/UAVObjects/inc/uavobjectlist.h
View File

@ -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 <stdint.h>
#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

92
flight/OpenPilot/UAVObjects/inc/uavobject.h → flight/OpenPilot/UAVObjects/inc/uavobjectmanager.h
View File

@ -1,9 +1,9 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file uavobject.h * @file uavobjectlist.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @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 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
@ -23,75 +23,87 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef UAVOBJECT_H #ifndef UAVOBJECTMANAGER_H
#define UAVOBJECT_H #define UAVOBJECTMANAGER_H
#include <stdint.h> #include <stdint.h>
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "queue.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 { typedef enum {
UPDATEMODE_PERIODIC = 0, /** Automatically update object at periodic intervals */ UPDATEMODE_PERIODIC = 0, /** Automatically update object at periodic intervals */
UPDATEMODE_ONCHANGE, /** Only update object when its data changes */ UPDATEMODE_ONCHANGE, /** Only update object when its data changes */
UPDATEMODE_MANUAL, /** Manually update object, by calling the updated() function */ UPDATEMODE_MANUAL, /** Manually update object, by calling the updated() function */
UPDATEMODE_NEVER /** Object is never updated */ UPDATEMODE_NEVER /** Object is never updated */
} UAVObjectUpdateMode; } UAVObjUpdateMode;
/** /**
* Object metadata, each object has a meta object that holds its metadata. The metadata define * 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 { typedef struct {
int8_t ackRequired; /** Defines if an ack is required for the transactions of this object (1:acked, 0:not acked) */ 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) */ 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) */ 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) */ int32_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */
} UAVObjectMetadata; } UAVObjMetadata;
/** /**
* Interface implemented by all UAVObjects. * Event types generated by the objects.
*/
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.
*/ */
typedef enum { typedef enum {
QMSG_UNPACKED = 1, /** Object data updated by unpacking */ QMSG_UNPACKED = 1, /** Object data updated by unpacking */
QMSG_UPDATED = 2, /** Object data updated by changing the data structure */ QMSG_UPDATED = 2, /** Object data updated by changing the data structure */
QMSG_UPDATED_MANUAL = 4, /** Object update event manually generated */ QMSG_UPDATED_MANUAL = 4, /** Object update event manually generated */
QMSG_UPDATE_REQ = 8 /** Request to update object data */ 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 { typedef struct {
UAVObject* obj; UAVObjHandle obj;
UAVObjectQMsgEvent event; int32_t instId;
} UAVObjectQMsg; 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

18
flight/OpenPilot/UAVObjects/inc/uavobjecttemplate.h
View File

@ -32,9 +32,17 @@
#define $(NAMEUC)_H #define $(NAMEUC)_H
#include <stdint.h> #include <stdint.h>
#include "uavobject.h" #include "uavobjectmanager.h"
#include "FreeRTOS.h"
#include "queue.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 // Object data
typedef struct { typedef struct {
@ -43,8 +51,6 @@ typedef struct {
// Generic interface functions // Generic interface functions
int32_t $(NAME)Initialize(); int32_t $(NAME)Initialize();
UAVObject* $(NAME)Get(); UAVObjHandle $(NAME)GetHandle();
void $(NAME)GetData(TestObjectData* dataOut);
void $(NAME)SetData(const TestObjectData* dataIn);
#endif // $(NAME)_H #endif // $(NAME)_H

55
flight/OpenPilot/UAVObjects/inc/uavobjectutils.h
View File

@ -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

176
flight/OpenPilot/UAVObjects/uavobjectlist.c
View File

@ -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 <stdlib.h> // 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);
}

893
flight/OpenPilot/UAVObjects/uavobjectmanager.c Normal file
View File

@ -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 <stdlib.h> // 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;
}
}

232
flight/OpenPilot/UAVObjects/uavobjecttemplate.c
View File

@ -32,38 +32,12 @@
#include <string.h> // for memcpy #include <string.h> // for memcpy
#include "$(NAMELC).h" #include "$(NAMELC).h"
#ifndef METAOBJECT #include "uavobjectmanager.h"
#include "$(NAMELC)meta.h"
#endif //METAOBJECT
#include "uavobjectlist.h"
#include "uavobjectutils.h"
#include "FreeRTOS.h"
#include "semphr.h"
// Private constants
#define OBJECT_ID $(OBJID)
#define METADATA_ID $(METAID)
#define NAME $(NAMESTR)
// Private variables // Private variables
ObjectContext context; UAVObjHandle handle;
$(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);
/** /**
* Initialize object. * Initialize object.
@ -72,197 +46,29 @@ int32_t getFieldIndexByName(char* name);
*/ */
int32_t $(NAME)Initialize() int32_t $(NAME)Initialize()
{ {
// Create mutex UAVObjMetadata metadata;
context.mutex = xSemaphoreCreateRecursiveMutex();
if (context.mutex == NULL)
return -1;
// Setup UAVObject table // Register object with the object manager
context.obj.objectID = OBJECT_ID; handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, 0, $(NAMEUC)_SINGLEINST, $(NAMEUC)_NUMBYTES);
context.obj.metadataID = METADATA_ID; if (handle == 0) return -1;
#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;
// Initialise connected queue list // Initialize meta data
context.queues = NULL; metadata.ackRequired = $(ACK);
objectID = OBJECT_ID; metadata.gcsTelemetryUpdateMode = $(GCSTELEM_UPDATEMODE);
metadata.gcsTelemetryUpdatePeriod = $(GCSTELEM_UPDATEPERIOD);
// Initialise linked object metadata.telemetryUpdateMode = $(TELEM_UPDATEMODE);
#ifndef METAOBJECT metadata.telemetryUpdatePeriod = $(TELEM_UPDATEPERIOD);
$(NAME)MetaInitialize(); metadata.loggingUpdateMode = $(LOGGING_UPDATEMODE);
context.obj.linkedObj = $(NAME)MetaGet(); metadata.loggingUpdatePeriod = $(LOGGING_UPDATEPERIOD);
context.obj.linkedObj->linkedObj = &context.obj; UAVObjSetMetadata(handle, &metadata);
#else
context.obj.linkedObj = NULL;
#endif //METAOBJECT
// Register object with object list
UAVObjectListRegister(&context.obj);
// Done
return 0; return 0;
} }
/** UAVObjHandle $(NAME)GetHandle()
* See UAVObject.h description for more details.
*/
int32_t pack(uint32_t objId, uint8_t* dataOut, int32_t maxLength)
{ {
int32_t res; return handle;
// 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);
} }

133
flight/OpenPilot/UAVObjects/uavobjectutils.c
View File

@ -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 <stdlib.h> // for malloc
#include <string.h> // 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);
}

17
flight/OpenPilot/UAVTalk/inc/uavtalk.h
View File

@ -23,29 +23,24 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef UAVTALK_C_H #ifndef UAVTALK_H
#define UAVTALK_C_H #define UAVTALK_H
#include <stdint.h> #include <stdint.h>
#include "uavobjectmanager.h"
// Public constants // Public constants
#define UAVTALK_WAITFOREVER -1 #define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0 #define UAVTALK_NOWAIT 0
// Public types // 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); typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length);
// Public functions // Public functions
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream); 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 UAVTalkSetOutputStream(UAVTalkOutputStream outputStream);
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);
int32_t UAVTalkSendObjectRequest(uint32_t objectId, int32_t timeoutMs); int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
int32_t UAVTalkProcessInputStream(uint8_t rxbyte); int32_t UAVTalkProcessInputStream(uint8_t rxbyte);
int32_t UAVTalkProcessPeriodicUpdates(void);
#endif // UAVTALK_H
#endif // UAVTALK_C_H

522
flight/OpenPilot/UAVTalk/uavtalk.c
View File

@ -25,19 +25,17 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include <stdlib.h> // for malloc
#include "uavtalk.h" #include "uavtalk.h"
#include "utlist.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "semphr.h" #include "semphr.h"
// Private constants // Private constants
#define TYPE_MASK 0xFC #define TYPE_MASK 0xFC
#define TYPE_BASE 0x50 #define TYPE_VER 0x10
#define TYPE_OBJ (TYPE_BASE | 0x00) #define TYPE_OBJ (TYPE_VER | 0x00)
#define TYPE_OBJ_REQ (TYPE_BASE | 0x01) #define TYPE_OBJ_REQ (TYPE_VER | 0x01)
#define TYPE_OBJ_ACK (TYPE_BASE | 0x02) #define TYPE_OBJ_ACK (TYPE_VER | 0x02)
#define TYPE_ACK (TYPE_BASE | 0x03) #define TYPE_ACK (TYPE_VER | 0x03)
#define HEADER_LENGTH 6 // type (1), object ID (4), length (1) #define HEADER_LENGTH 6 // type (1), object ID (4), length (1)
#define CHECKSUM_LENGTH 2 #define CHECKSUM_LENGTH 2
@ -47,33 +45,23 @@
#define MIN_UPDATE_PERIOD_MS 1 #define MIN_UPDATE_PERIOD_MS 1
// Private types // Private types
struct ObjectHandleStruct { typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState;
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;
// Private variables // Private variables
UAVTalkOutputStream outStream; UAVTalkOutputStream outStream;
ObjectHandle* objects; xSemaphoreHandle lock;
int32_t timeToNextUpdateMs; xSemaphoreHandle respSema;
xSemaphoreHandle mutex; UAVObjHandle respObj;
uint16_t respInstId;
uint8_t rxBuffer[MAX_PACKET_LENGTH]; uint8_t rxBuffer[MAX_PACKET_LENGTH];
uint8_t txBuffer[MAX_PACKET_LENGTH]; uint8_t txBuffer[MAX_PACKET_LENGTH];
// Private functions // Private functions
uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length); uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length);
ObjectHandle* findObject(uint32_t objId); int32_t objectTransaction(uint32_t objectId, uint16_t instId, uint8_t type, int32_t timeout);
int32_t objectTransaction(uint32_t objectId, uint8_t type, int32_t timeout); int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type);
int32_t sendObject(ObjectHandle* obj, uint8_t type); int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type);
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);
/** /**
* Initialize the UAVTalk library * 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 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream) { int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream)
{
outStream = outputStream; outStream = outputStream;
mutex = xSemaphoreCreateRecursiveMutex(); lock = xSemaphoreCreateRecursiveMutex();
timeToNextUpdateMs = 0; vSemaphoreCreateBinary(respSema);
objects = NULL;
return 0; 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 * Request an update for the specified object, on success the object data would have been
* updated by the GCS. * 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 * \param[in] timeout Time to wait for the response, when zero it will return immediately
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkSendObjectRequest(uint32_t objectId, int32_t timeout) { int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeout)
return objectTransaction(objectId, TYPE_OBJ_REQ, timeout); {
return objectTransaction(obj, instId, TYPE_OBJ_REQ, timeout);
} }
/** /**
* Send the specified object through the telemetry link. * 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] 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 * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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) { if (acked == 1) {
return objectTransaction(objectId, TYPE_OBJ_ACK, timeoutMs); return objectTransaction(obj, instId, TYPE_OBJ_ACK, timeoutMs);
} else { } else {
return objectTransaction(objectId, TYPE_OBJ, timeoutMs); return objectTransaction(obj, instId, TYPE_OBJ, timeoutMs);
} }
} }
/** /**
* Execute the requested transaction on an object. * 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 * \param[in] type Transaction type
* TYPE_OBJ: send object, * TYPE_OBJ: send object,
* TYPE_OBJ_REQ: request object update * 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 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t objectTransaction(uint32_t objectId, uint8_t type, int32_t timeoutMs) { int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
ObjectHandle* obj; {
int32_t respReceived;
// Lock // Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY); xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Find object
obj = findObject(objectId);
if (obj == 0) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Send object depending on if a response is needed // Send object depending on if a response is needed
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { 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 sendObject(obj, instId, type);
xSemaphoreTake(obj->sema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema) respObj = obj;
xSemaphoreTake(obj->sema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout) respInstId = instId;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // complete transaction xSemaphoreGiveRecursive(lock); // need to release lock since the next call will block until a response is received
// Check if a response was received xSemaphoreTake(respSema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
if (obj->waitingResp == 1) { respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout)
obj->waitingResp = 0; // Check if a response was received
xSemaphoreGiveRecursive(mutex); if (respReceived == pdFALSE)
return -1; {
} else { return -1;
xSemaphoreGiveRecursive(mutex); }
return 0; else
} {
} else if (type == TYPE_OBJ) { return 0;
sendObject(obj, TYPE_OBJ); }
xSemaphoreGiveRecursive(mutex); }
else if (type == TYPE_OBJ)
{
sendObject(obj, instId, TYPE_OBJ);
xSemaphoreGiveRecursive(lock);
return 0; return 0;
} else { }
xSemaphoreGiveRecursive(mutex); else
{
xSemaphoreGiveRecursive(lock);
return -1; 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. * Process an byte from the telemetry stream.
* \param[in] rxbyte Received byte * \param[in] rxbyte Received byte
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkProcessInputStream(uint8_t rxbyte) { int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
{
static uint8_t tmpBuffer[4]; static uint8_t tmpBuffer[4];
static ObjectHandle* obj; static UAVObjHandle obj;
static uint8_t type; static uint8_t type;
static uint32_t objId; static uint32_t objId;
static uint8_t length; static uint16_t instId;
static uint32_t length;
static uint16_t cs, csRx; static uint16_t cs, csRx;
static int32_t rxCount; static int32_t rxCount;
static RxState state = STATE_SYNC; static RxState state = STATE_SYNC;
@ -294,7 +180,8 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) {
// Receive state machine // Receive state machine
switch (state) { switch (state) {
case STATE_SYNC: case STATE_SYNC:
if ((rxbyte & TYPE_MASK) == TYPE_BASE ) { if ((rxbyte & TYPE_MASK) == TYPE_VER )
{
cs = rxbyte; cs = rxbyte;
type = rxbyte; type = rxbyte;
state = STATE_OBJID; state = STATE_OBJID;
@ -303,39 +190,81 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) {
break; break;
case STATE_OBJID: case STATE_OBJID:
tmpBuffer[rxCount++] = rxbyte; tmpBuffer[rxCount++] = rxbyte;
if (rxCount == 4) { if (rxCount == 4)
{
// Search for object, if not found reset state machine // Search for object, if not found reset state machine
objId = (tmpBuffer[3] << 24) | (tmpBuffer[2] << 16) | (tmpBuffer[1] << 8) | (tmpBuffer[0]); objId = (tmpBuffer[3] << 24) | (tmpBuffer[2] << 16) | (tmpBuffer[1] << 8) | (tmpBuffer[0]);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY); obj = UAVObjGetByID(objId);
obj = findObject(objId); if (obj == 0)
xSemaphoreGiveRecursive(mutex); {
if (obj == 0) {
state = STATE_SYNC; state = STATE_SYNC;
} else { }
else
{
// Update checksum
cs = updateChecksum(cs, tmpBuffer, 4); cs = updateChecksum(cs, tmpBuffer, 4);
state = STATE_LENGTH; // Determine data length
rxCount = 0; 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; break;
case STATE_LENGTH: case STATE_INSTID:
length = (int32_t)rxbyte; tmpBuffer[rxCount++] = rxbyte;
if (length > MAX_PAYLOAD_LENGTH || if (rxCount == 2)
((type == TYPE_OBJ_REQ || type == TYPE_ACK) && length != 0)) { {
state = STATE_SYNC; instId = (tmpBuffer[1] << 8) | (tmpBuffer[0]);
} else { cs = updateChecksum(cs, tmpBuffer, 2);
cs = updateChecksum(cs, &length, 1); rxCount = 0;
rxCount = 0; // If there is a payload get it, otherwise receive checksum
if (length > 0) { if (length > 0)
state = STATE_DATA; {
} else { state = STATE_DATA;
state = STATE_CS; }
} else
{
state = STATE_CS;
}
} }
break; break;
case STATE_DATA: case STATE_DATA:
rxBuffer[rxCount++] = rxbyte; rxBuffer[rxCount++] = rxbyte;
if (rxCount == length) { if (rxCount == length)
{
cs = updateChecksum(cs, rxBuffer, length); cs = updateChecksum(cs, rxBuffer, length);
state = STATE_CS; state = STATE_CS;
rxCount = 0; rxCount = 0;
@ -343,12 +272,14 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) {
break; break;
case STATE_CS: case STATE_CS:
tmpBuffer[rxCount++] = rxbyte; tmpBuffer[rxCount++] = rxbyte;
if (rxCount == 2) { if (rxCount == 2)
{
csRx = (tmpBuffer[1] << 8) | (tmpBuffer[0]); csRx = (tmpBuffer[1] << 8) | (tmpBuffer[0]);
if (csRx == cs) { if (csRx == cs)
xSemaphoreTakeRecursive(mutex, portMAX_DELAY); {
receiveObject(type, obj, rxBuffer, length); xSemaphoreTakeRecursive(lock, portMAX_DELAY);
xSemaphoreGiveRecursive(mutex); receiveObject(type, obj, instId, rxBuffer, length);
xSemaphoreGiveRecursive(lock);
} }
state = STATE_SYNC; 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. * 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] 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] 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] data Data buffer
* \param[in] length Buffer length * \param[in] length Buffer length
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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 // Unpack object if the message is of type OBJ or OBJ_ACK
if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) { if (type == TYPE_OBJ || type == TYPE_OBJ_ACK)
(obj->unpackCb)(obj->objectId, data, length); {
UAVObjUnpack(obj, instId, data);
} }
// Send requested object if message is of type OBJ_REQ // Send requested object if message is of type OBJ_REQ
if (type == TYPE_OBJ_REQ) { if (type == TYPE_OBJ_REQ)
sendObject(obj, TYPE_OBJ); {
sendObject(obj, instId, TYPE_OBJ);
} }
// Send ACK if message is of type OBJ_ACK // Send ACK if message is of type OBJ_ACK
if (type == TYPE_OBJ_ACK) { if (type == TYPE_OBJ_ACK)
sendObject(obj, TYPE_ACK); {
sendObject(obj, instId, TYPE_ACK);
} }
// If a response was pending on the object, unblock any waiting tasks // If a response was pending on the object, unblock any waiting tasks
if (type == TYPE_ACK || type == TYPE_OBJ) { if (type == TYPE_ACK || type == TYPE_OBJ)
if (obj->waitingResp == 1) { {
obj->waitingResp = 0; if (respObj == obj && (respInstId = instId || respInstId == UAVOBJ_ALL_INSTANCES))
xSemaphoreGive(obj->sema); {
} xSemaphoreGive(respSema);
}
} }
// Done // 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. * Send an object through the telemetry link.
* \param[in] obj Object handle to send * \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 * \param[in] type Transaction type
* \return 0 Success * \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 length;
int32_t dataOffset;
uint16_t cs = 0; uint16_t cs = 0;
uint32_t objId;
// Check for valid packet type // 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; 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 // Setup type and object id fields
objId = UAVObjGetID(obj);
txBuffer[0] = type; txBuffer[0] = type;
txBuffer[1] = (uint8_t)(obj->objectId & 0xFF); txBuffer[1] = (uint8_t)(objId & 0xFF);
txBuffer[2] = (uint8_t)((obj->objectId >> 8) & 0xFF); txBuffer[2] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[3] = (uint8_t)((obj->objectId >> 16) & 0xFF); txBuffer[3] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[4] = (uint8_t)((obj->objectId >> 24) & 0xFF); txBuffer[4] = (uint8_t)((objId >> 24) & 0xFF);
// Setup length and data field (if one) // Setup instance ID if one is required
if (type == TYPE_ACK || type == TYPE_OBJ_REQ) { if (UAVObjIsSingleInstance(obj))
length = 0; {
} else { dataOffset = 5;
// Pack object }
length = (obj->packCb)(obj->objectId, &txBuffer[HEADER_LENGTH], MAX_PAYLOAD_LENGTH); else
// Check length {
if (length > MAX_PAYLOAD_LENGTH || length <= 0) { txBuffer[5] = (uint8_t)(instId & 0xFF);
return -1; 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 // Calculate checksum
cs = 0; cs = 0;
cs = updateChecksum(cs, txBuffer, HEADER_LENGTH+length); cs = updateChecksum(cs, txBuffer, dataOffset+length);
txBuffer[HEADER_LENGTH+length] = (uint8_t)(cs & 0xFF); txBuffer[dataOffset+length] = (uint8_t)(cs & 0xFF);
txBuffer[HEADER_LENGTH+length+1] = (uint8_t)((cs >> 8) & 0xFF); txBuffer[dataOffset+length+1] = (uint8_t)((cs >> 8) & 0xFF);
// Send buffer // Send buffer
if (outStream!=NULL) (*outStream)(txBuffer, HEADER_LENGTH+length+CHECKSUM_LENGTH); if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH);
// Done // Done
return 0; return 0;
@ -459,26 +460,19 @@ int32_t sendObject(ObjectHandle* obj, uint8_t type) {
* \param[in] length Length of buffer * \param[in] length Length of buffer
* \return Updated checksum * \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; int32_t n;
for (n = 0; n < length; ++n) { for (n = 0; n < length; ++n)
{
cs += (uint16_t)data[n]; cs += (uint16_t)data[n];
} }
return cs; 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;
}