diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 318aef868..854e777df 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -195,21 +195,15 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (updateMode == UPDATEMODE_THROTTLED) { - // If we received a periodic update, we can change back to update on change if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { - // Set update period - setUpdatePeriod(obj, 0); - // Connect queue + // If we received a periodic update, we can change back to update on change eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; + // Set update period on initialization and metadata change + if (eventType == EV_NONE) + setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates - // Set update period - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); - // Connect queue eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - if (UAVObjIsMetaobject(obj)) { - eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) - } } if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) @@ -277,7 +271,7 @@ static void processObjEvent(UAVObjEvent * ev) } // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { - updateObject(UAVObjGetLinkedObj(ev->obj), ev->event); // linked object will be the actual object the metadata are for + updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for } } if((updateMode == UPDATEMODE_THROTTLED) && !UAVObjIsMetaobject(ev->obj)) { diff --git a/flight/UAVObjects/eventdispatcher.c b/flight/UAVObjects/eventdispatcher.c index 53a6e039d..f6eed78ff 100644 --- a/flight/UAVObjects/eventdispatcher.c +++ b/flight/UAVObjects/eventdispatcher.c @@ -59,7 +59,7 @@ typedef struct { */ struct PeriodicObjectListStruct { EventCallbackInfo evInfo; /** Event callback information */ - int32_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ + uint16_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */ int32_t timeToNextUpdateMs; /** Time delay to the next update */ struct PeriodicObjectListStruct* next; /** Needed by linked list library (utlist.h) */ }; @@ -75,9 +75,9 @@ static EventStats stats; // Private functions static int32_t processPeriodicUpdates(); static void eventTask(); -static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs); -static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs); -static uint32_t randomizePeriod(uint32_t periodMs); +static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); +static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs); +static uint16_t randomizePeriod(uint16_t periodMs); /** @@ -151,7 +151,7 @@ int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb) * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs) +int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs) { return eventPeriodicCreate(ev, cb, 0, periodMs); } @@ -163,7 +163,7 @@ int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs) +int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs) { return eventPeriodicUpdate(ev, cb, 0, periodMs); } @@ -175,7 +175,7 @@ int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs) +int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs) { return eventPeriodicCreate(ev, 0, queue, periodMs); } @@ -187,7 +187,7 @@ int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t pe * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs) +int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs) { return eventPeriodicUpdate(ev, 0, queue, periodMs); } @@ -200,7 +200,7 @@ int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t pe * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs) +static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { PeriodicObjectList* objEntry; // Get lock @@ -244,7 +244,7 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue * \param[in] periodMs The period the event is generated * \return Success (0), failure (-1) */ -static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs) +static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, uint16_t periodMs) { PeriodicObjectList* objEntry; // Get lock @@ -372,7 +372,7 @@ static int32_t processPeriodicUpdates() * Based on the Park-Miller-Carta Pseudo-Random Number Generator * http://www.firstpr.com.au/dsp/rand31/ */ -static uint32_t randomizePeriod(uint32_t periodMs) +static uint16_t randomizePeriod(uint16_t periodMs) { static uint32_t seed = 1; uint32_t hi, lo; @@ -382,6 +382,6 @@ static uint32_t randomizePeriod(uint32_t periodMs) lo += hi >> 15; if (lo > 0x7FFFFFFF) lo -= 0x7FFFFFFF; seed = lo; - return (uint32_t)( ((float)periodMs * (float)lo) / (float)0x7FFFFFFF ); + return (uint16_t)( ((float)periodMs * (float)lo) / (float)0x7FFFFFFF ); } diff --git a/flight/UAVObjects/inc/eventdispatcher.h b/flight/UAVObjects/inc/eventdispatcher.h index 9e378b65a..bfcb6b85e 100644 --- a/flight/UAVObjects/inc/eventdispatcher.h +++ b/flight/UAVObjects/inc/eventdispatcher.h @@ -39,9 +39,9 @@ int32_t EventDispatcherInitialize(); void EventGetStats(EventStats* statsOut); void EventClearStats(); int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb); -int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs); -int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs); -int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs); -int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs); +int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs); +int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs); +int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs); +int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, uint16_t periodMs); #endif // EVENTDISPATCHER_H diff --git a/flight/UAVObjects/inc/uavobjectmanager.h b/flight/UAVObjects/inc/uavobjectmanager.h index ff152c82d..08078acf5 100644 --- a/flight/UAVObjects/inc/uavobjectmanager.h +++ b/flight/UAVObjects/inc/uavobjectmanager.h @@ -196,9 +196,9 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val) UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* dataOut); void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val); int8_t UAVObjReadOnly(UAVObjHandle obj); -int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask); +int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, uint8_t eventMask); int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue); -int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, int32_t eventMask); +int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, uint8_t eventMask); int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb); void UAVObjRequestUpdate(UAVObjHandle obj); void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId); diff --git a/flight/UAVObjects/uavobjectmanager.c b/flight/UAVObjects/uavobjectmanager.c index 7c4703198..3eea7e450 100644 --- a/flight/UAVObjects/uavobjectmanager.c +++ b/flight/UAVObjects/uavobjectmanager.c @@ -38,7 +38,13 @@ // Private types // Macros -#define SET_BITS(var, shift, value, mask) var = (var & !(mask << shift)) | (value << shift); +#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); +#define OLGetIsMetaobject(olp) ((olp)->flags & OL_IS_METAOBJECT) +#define OLSetIsMetaobject(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_METAOBJECT) : ((olp)->flags | OL_IS_METAOBJECT))) +#define OLGetIsSingleInstance(olp) ((olp)->flags & OL_IS_SINGLE_INSTANCE) +#define OLSetIsSingleInstance(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SINGLE_INSTANCE) : ((olp)->flags | OL_IS_SINGLE_INSTANCE))) +#define OLGetIsSettings(olp) ((olp)->flags & OL_IS_SETTINGS) +#define OLSetIsSettings(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SETTINGS) : ((olp)->flags | OL_IS_SETTINGS))) /** * List of event queues and the eventmask associated with the queue. @@ -46,7 +52,7 @@ struct ObjectEventListStruct { xQueueHandle queue; UAVObjEventCallback cb; - int32_t eventMask; + uint8_t eventMask; struct ObjectEventListStruct *next; }; typedef struct ObjectEventListStruct ObjectEventList; @@ -61,6 +67,12 @@ struct ObjectInstListStruct { }; typedef struct ObjectInstListStruct ObjectInstList; +typedef enum { + OL_IS_METAOBJECT = 0x01, /** Set if this is a metaobject */ + OL_IS_SINGLE_INSTANCE = 0x02, /** Set if this object has a single instance */ + OL_IS_SETTINGS = 0x04 /** Set if this object is a settings object */ +} ObjectListFlags; + /** * List of objects registered in the object manager */ @@ -69,12 +81,8 @@ struct ObjectListStruct { /** The object ID */ const char *name; /** The object name */ - int8_t isMetaobject; - /** Set to 1 if this is a metaobject */ - int8_t isSingleInstance; - /** Set to 1 if this object has a single instance */ - int8_t isSettings; - /** Set to 1 if this object is a settings object */ + ObjectListFlags flags; + /** The object list mode flags */ uint16_t numBytes; /** Number of data bytes contained in the object (for a single instance) */ uint16_t numInstances; @@ -96,7 +104,7 @@ static int32_t sendEvent(ObjectList * obj, uint16_t instId, static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId); static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId); static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, - UAVObjEventCallback cb, int32_t eventMask); + UAVObjEventCallback cb, uint8_t eventMask); static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb); @@ -198,9 +206,9 @@ UAVObjHandle UAVObjRegister(uint32_t id, const char *name, } objEntry->id = id; objEntry->name = name; - objEntry->isMetaobject = (int8_t) isMetaobject; - objEntry->isSingleInstance = (int8_t) isSingleInstance; - objEntry->isSettings = (int8_t) isSettings; + OLSetIsMetaobject(objEntry, isMetaobject); + OLSetIsSingleInstance(objEntry, isSingleInstance); + OLSetIsSettings(objEntry, isSettings); objEntry->numBytes = numBytes; objEntry->events = NULL; objEntry->numInstances = 0; @@ -237,11 +245,11 @@ UAVObjHandle UAVObjRegister(uint32_t id, const char *name, initCb((UAVObjHandle) objEntry, 0); } // Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object) - if (!objEntry->isMetaobject) { + if (!OLGetIsMetaobject(objEntry)) { UAVObjLoad((UAVObjHandle) objEntry->linkedObj, 0); } // If this is a settings object, attempt to load from SD card - if (objEntry->isSettings) { + if (OLGetIsSettings(objEntry)) { UAVObjLoad((UAVObjHandle) objEntry, 0); } // Release lock @@ -397,7 +405,7 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj, */ int32_t UAVObjIsSingleInstance(UAVObjHandle obj) { - return ((ObjectList *) obj)->isSingleInstance; + return OLGetIsSingleInstance((ObjectList *) obj); } /** @@ -407,7 +415,7 @@ int32_t UAVObjIsSingleInstance(UAVObjHandle obj) */ int32_t UAVObjIsMetaobject(UAVObjHandle obj) { - return ((ObjectList *) obj)->isMetaobject; + return OLGetIsMetaobject((ObjectList *) obj); } /** @@ -417,7 +425,7 @@ int32_t UAVObjIsMetaobject(UAVObjHandle obj) */ int32_t UAVObjIsSettings(UAVObjHandle obj) { - return ((ObjectList *) obj)->isSettings; + return OLGetIsSettings((ObjectList *) obj); } /** @@ -533,7 +541,7 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, &bytesWritten); // Write the instance ID - if (!objEntry->isSingleInstance) { + if (!OLGetIsSingleInstance(objEntry)) { PIOS_FWRITE(file, &instEntry->instId, sizeof(instEntry->instId), &bytesWritten); } @@ -652,7 +660,7 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) // Get the instance ID instId = 0; - if (!objEntry->isSingleInstance) { + if (!OLGetIsSingleInstance(objEntry)) { if (PIOS_FREAD (file, &instId, sizeof(instId), &bytesRead)) { xSemaphoreGiveRecursive(mutex); @@ -816,7 +824,7 @@ int32_t UAVObjSaveSettings() // Save all settings objects LL_FOREACH(objList, objEntry) { // Check if this is a settings object - if (objEntry->isSettings) { + if (OLGetIsSettings(objEntry)) { // Save object if (UAVObjSave((UAVObjHandle) objEntry, 0) == -1) { @@ -845,7 +853,7 @@ int32_t UAVObjLoadSettings() // Load all settings objects LL_FOREACH(objList, objEntry) { // Check if this is a settings object - if (objEntry->isSettings) { + if (OLGetIsSettings(objEntry)) { // Load object if (UAVObjLoad((UAVObjHandle) objEntry, 0) == -1) { @@ -874,7 +882,7 @@ int32_t UAVObjDeleteSettings() // Save all settings objects LL_FOREACH(objList, objEntry) { // Check if this is a settings object - if (objEntry->isSettings) { + if (OLGetIsSettings(objEntry)) { // Save object if (UAVObjDelete((UAVObjHandle) objEntry, 0) == -1) { @@ -903,7 +911,7 @@ int32_t UAVObjSaveMetaobjects() // Save all settings objects LL_FOREACH(objList, objEntry) { // Check if this is a settings object - if (objEntry->isMetaobject) { + if (OLGetIsMetaobject(objEntry)) { // Save object if (UAVObjSave((UAVObjHandle) objEntry, 0) == -1) { @@ -932,7 +940,7 @@ int32_t UAVObjLoadMetaobjects() // Load all settings objects LL_FOREACH(objList, objEntry) { // Check if this is a settings object - if (objEntry->isMetaobject) { + if (OLGetIsMetaobject(objEntry)) { // Load object if (UAVObjLoad((UAVObjHandle) objEntry, 0) == -1) { @@ -961,7 +969,7 @@ int32_t UAVObjDeleteMetaobjects() // Load all settings objects LL_FOREACH(objList, objEntry) { // Check if this is a settings object - if (objEntry->isMetaobject) { + if (OLGetIsMetaobject(objEntry)) { // Load object if (UAVObjDelete((UAVObjHandle) objEntry, 0) == -1) { @@ -1041,7 +1049,7 @@ int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, objEntry = (ObjectList *) obj; // Check access level - if (!objEntry->isMetaobject) { + if (!OLGetIsMetaobject(objEntry)) { mdata = (UAVObjMetadata *) (objEntry->linkedObj->instances. data); @@ -1088,7 +1096,7 @@ int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void objEntry = (ObjectList*)obj; // Check access level - if ( !objEntry->isMetaobject ) + if ( !OLGetIsMetaobject(objEntry) ) { mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data); if ( UAVObjGetAccess(mdata) == ACCESS_READONLY ) @@ -1217,7 +1225,7 @@ int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn) // Set metadata (metadata of metaobjects can not be modified) objEntry = (ObjectList *) obj; - if (!objEntry->isMetaobject) { + if (!OLGetIsMetaobject(objEntry)) { UAVObjSetData((UAVObjHandle) objEntry->linkedObj, dataIn); } else { @@ -1244,7 +1252,7 @@ int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata * dataOut) // Get metadata objEntry = (ObjectList *) obj; - if (objEntry->isMetaobject) { + if (OLGetIsMetaobject(objEntry)) { memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); } else { UAVObjGetData((UAVObjHandle) objEntry->linkedObj, @@ -1403,7 +1411,7 @@ int8_t UAVObjReadOnly(UAVObjHandle obj) objEntry = (ObjectList *) obj; // Check access level - if (!objEntry->isMetaobject) { + if (!OLGetIsMetaobject(objEntry)) { mdata = (UAVObjMetadata *) (objEntry->linkedObj->instances. data); @@ -1421,7 +1429,7 @@ int8_t UAVObjReadOnly(UAVObjHandle obj) * \return 0 if success or -1 if failure */ int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, - int32_t eventMask) + uint8_t eventMask) { int32_t res; xSemaphoreTakeRecursive(mutex, portMAX_DELAY); @@ -1454,7 +1462,7 @@ int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue) * \return 0 if success or -1 if failure */ int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, - int32_t eventMask) + uint8_t eventMask) { int32_t res; xSemaphoreTakeRecursive(mutex, portMAX_DELAY); @@ -1591,7 +1599,7 @@ static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId) int32_t n; // For single instance objects, only instance zero is allowed - if (obj->isSingleInstance && instId != 0) { + if (OLGetIsSingleInstance(obj) && instId != 0) { return NULL; } // Make sure that the instance ID is within limits @@ -1665,7 +1673,7 @@ static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId) * \return 0 if success or -1 if failure */ static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, - UAVObjEventCallback cb, int32_t eventMask) + UAVObjEventCallback cb, uint8_t eventMask) { ObjectEventList *eventEntry; ObjectList *objEntry; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 3fd14d776..c1ba625fc 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -39,7 +39,7 @@ #define UAVOBJ_UPDATE_MODE_MASK 0x3 // Macros -#define SET_BITS(var, shift, value, mask) var = (var & !(mask << shift)) | (value << shift); +#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); /** * Constructor