diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 4b90cbf5a..ea98d32bb 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -268,12 +268,11 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) eventMask |= EV_LOGGING_MANUAL; break; } - if(UAVObjIsSettings(obj)){ + if (UAVObjIsPriority(obj)) { UAVObjConnectQueue(obj, priorityQueue, eventMask); } else { UAVObjConnectQueue(obj, queue, eventMask); } - } /** @@ -376,10 +375,10 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) */ #if defined(PIOS_TELEM_PRIORITY_QUEUE) // Loop forever - while (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) { - // Process event - processObjEvent(&ev); - } + while (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) { + // Process event + processObjEvent(&ev); + } #endif // Wait for queue message if (xQueueReceive(queue, &ev, 1) == pdTRUE) { diff --git a/flight/uavobjects/inc/uavobject.h.template b/flight/uavobjects/inc/uavobject.h.template index 599ba4fb8..a17a657a6 100644 --- a/flight/uavobjects/inc/uavobject.h.template +++ b/flight/uavobjects/inc/uavobject.h.template @@ -44,6 +44,7 @@ #define $(NAMEUC)_OBJID $(OBJIDHEX) #define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST) #define $(NAMEUC)_ISSETTINGS $(ISSETTINGS) +#define $(NAMEUC)_ISPRIORITY $(ISPRIORITY) #define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data) /* Generic interface functions */ diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index c3813ef74..c50c14d76 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/flight/uavobjects/inc/uavobjectmanager.h @@ -147,8 +147,7 @@ typedef struct { int32_t UAVObjInitialize(); void UAVObjGetStats(UAVObjStats *statsOut); void UAVObjClearStats(); -UAVObjHandle UAVObjRegister(uint32_t id, - int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb); +UAVObjHandle UAVObjRegister(uint32_t id, bool isSingleInstance, bool isSettings, bool isPriority, uint32_t num_bytes, UAVObjInitializeCallback initCb); UAVObjHandle UAVObjGetByID(uint32_t id); uint32_t UAVObjGetID(UAVObjHandle obj); uint32_t UAVObjGetNumBytes(UAVObjHandle obj); @@ -158,6 +157,7 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback bool UAVObjIsSingleInstance(UAVObjHandle obj); bool UAVObjIsMetaobject(UAVObjHandle obj); bool UAVObjIsSettings(UAVObjHandle obj); +bool UAVObjIsPriority(UAVObjHandle obj); int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn); int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut); uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc); diff --git a/flight/uavobjects/uavobject.c.template b/flight/uavobjects/uavobject.c.template index b9f45b5ae..395279bd1 100644 --- a/flight/uavobjects/uavobject.c.template +++ b/flight/uavobjects/uavobject.c.template @@ -65,7 +65,7 @@ int32_t $(NAME)Initialize(void) // Register object with the object manager handle = UAVObjRegister($(NAMEUC)_OBJID, - $(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults); + $(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_ISPRIORITY, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults); // Done return handle ? 0 : -1; diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index f5077c982..02441a44e 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -108,6 +108,7 @@ struct UAVOBase { bool isMeta : 1; bool isSingle : 1; bool isSettings : 1; + bool isPriority : 1; } flags; } __attribute__((packed)); @@ -339,7 +340,7 @@ static struct UAVOData *UAVObjAllocMulti(uint32_t num_bytes) * \return */ UAVObjHandle UAVObjRegister(uint32_t id, - int32_t isSingleInstance, int32_t isSettings, + bool isSingleInstance, bool isSettings, bool isPriority, uint32_t num_bytes, UAVObjInitializeCallback initCb) { @@ -368,8 +369,11 @@ UAVObjHandle UAVObjRegister(uint32_t id, uavo_data->instance_size = num_bytes; if (isSettings) { uavo_data->base.flags.isSettings = true; + // settings defaults to being sent with priority + uavo_data->base.flags.isPriority = true; + } else { + uavo_data->base.flags.isPriority = isPriority; } - /* Initialize the embedded meta UAVO */ UAVObjInitMetaData(&uavo_data->metaObj); @@ -605,6 +609,22 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle) return uavo_base->flags.isSettings; } +/** + * Is this a prioritized object? + * \param[in] obj The object handle + * \return True (1) if this is a prioritized object + */ +bool UAVObjIsPriority(UAVObjHandle obj_handle) +{ + PIOS_Assert(obj_handle); + + /* Recover the common object header */ + struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle; + + return uavo_base->flags.isPriority; +} + + /** * Unpack an object from a byte array * \param[in] obj The object handle diff --git a/ground/uavobjgenerator/generators/generator_common.cpp b/ground/uavobjgenerator/generators/generator_common.cpp index 3a96ca982..83ce1dd13 100644 --- a/ground/uavobjgenerator/generators/generator_common.cpp +++ b/ground/uavobjgenerator/generators/generator_common.cpp @@ -72,6 +72,9 @@ void replaceCommonTags(QString & out, ObjectInfo *info) // Replace $(ISSETTINGS) tag out.replace(QString("$(ISSETTINGS)"), boolTo01String(info->isSettings)); out.replace(QString("$(ISSETTINGSTF)"), boolToTRUEFALSEString(info->isSettings)); + // Replace $(ISPRIORITY) tag + out.replace(QString("$(ISPRIORITY)"), boolTo01String(info->isPriority)); + out.replace(QString("$(ISPRIORITYTF)"), boolToTRUEFALSEString(info->isPriority)); // Replace $(GCSACCESS) tag value = accessModeStr[info->gcsAccess]; out.replace(QString("$(GCSACCESS)"), value); diff --git a/ground/uavobjgenerator/uavobjectparser.cpp b/ground/uavobjgenerator/uavobjectparser.cpp index d0cfb0449..a8534088c 100644 --- a/ground/uavobjgenerator/uavobjectparser.cpp +++ b/ground/uavobjgenerator/uavobjectparser.cpp @@ -623,9 +623,19 @@ QString UAVObjectParser::processObjectAttributes(QDomNode & node, ObjectInfo *in } else if (attr.nodeValue().compare(QString("false")) == 0) { info->isSettings = false; } else { - return QString("Object:settings attribute value is invalid"); + return QString("Object:settings attribute value is invalid (true|false)"); } + // Get priority attribute + attr = attributes.namedItem("priority"); + info->isPriority = false; + if (!attr.isNull()) { + if (attr.nodeValue().compare(QString("true")) == 0) { + info->isPriority = true; + } else if (attr.nodeValue().compare(QString("false")) != 0) { + return QString("Object:priority attribute value is invalid (true|false)"); + } + } // Settings objects can only have a single instance if (info->isSettings && !info->isSingleInst) { diff --git a/ground/uavobjgenerator/uavobjectparser.h b/ground/uavobjgenerator/uavobjectparser.h index d6ae4b60c..9028af325 100644 --- a/ground/uavobjgenerator/uavobjectparser.h +++ b/ground/uavobjgenerator/uavobjectparser.h @@ -84,6 +84,7 @@ typedef struct { quint32 id; bool isSingleInst; bool isSettings; + bool isPriority; AccessMode gcsAccess; AccessMode flightAccess; bool flightTelemetryAcked;