pubsub: Send position array

Send a position array instead of multiple variables

include/dronedge.h

@@ -14,10 +14,8 @@ typedef struct {
   struct messageNode *tail;
 } MessageQueue;
 
-UA_Double latitude = 0;
-UA_Double longitude = 0;
-UA_Float altitude_abs = 0;
-UA_Float altitude_rel = 0;
+UA_Double positionArray[POSITION_ARRAY_SIZE] = { 0 };
+UA_UInt32 positionArrayDims[] = {POSITION_ARRAY_SIZE};
 UA_String message = {
     .length = 0,
     .data = NULL,
@@ -25,36 +23,16 @@ UA_String message = {
 
 VariableData droneVariableArray[] = {
     {
-        .name = "latitude",
-        .description = "Latitude",
-        .value = &latitude,
+        .name = "positionArray",
+        .typeName = "Position Array Type",
+        .description = "Position Array",
+        .value = &positionArray,
         .type = UA_TYPES_DOUBLE,
         .builtInType = UA_NS0ID_DOUBLE,
-        .getter.getDouble = mavsdk_getLatitude,
-    },
-    {
-        .name = "longitude",
-        .description = "Longitude",
-        .value = &longitude,
-        .type = UA_TYPES_DOUBLE,
-        .builtInType = UA_NS0ID_DOUBLE,
-        .getter.getDouble = mavsdk_getLongitude,
-    },
-    {
-        .name = "altitude_abs",
-        .description = "Absolute Altitude (AMSL)",
-        .value = &altitude_abs,
-        .type = UA_TYPES_FLOAT,
-        .builtInType = UA_NS0ID_FLOAT,
-        .getter.getFloat = mavsdk_getAltitude,
-    },
-    {
-        .name = "altitude_rel",
-        .description = "Relative Altitude",
-        .value = &altitude_rel,
-        .type = UA_TYPES_FLOAT,
-        .builtInType = UA_NS0ID_FLOAT,
-        .getter.getFloat = mavsdk_getAltitudeRel,
+        .valueRank = UA_VALUERANK_ONE_DIMENSION,
+        .arrayDimensionsSize = 1,
+        .arrayDimensions = positionArrayDims,
+        .getter.getArray = mavsdk_getPositionArray,
     },
     {
         .name = "message",
@@ -62,9 +40,11 @@ VariableData droneVariableArray[] = {
         .value = &message,
         .type = UA_TYPES_STRING,
         .builtInType = UA_NS0ID_STRING,
-        .getter.getString = get_message
+        .valueRank = UA_VALUERANK_SCALAR,
+        .arrayDimensionsSize = 0,
+        .arrayDimensions = NULL,
+        .getter.getString = get_message,
     },
 };
 
-
 #endif /* __DRONEDGE_H__ */

include/mavsdk_wrapper.h

 #ifndef __MAVSDK_H__
 #define __MAVSDK_H__
 
+/*
+ * 0. latitude (double, degrees)
+ * 1. longitude (double, degrees)
+ * 2. absolute altitude (double, meters)
+ * 3. relative altitude (double, meters)
+*/
+#define POSITION_ARRAY_SIZE 4
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -32,6 +40,7 @@ double mavsdk_getInitialLatitude(void);
 double mavsdk_getInitialLongitude(void);
 double mavsdk_getLatitude(void);
 double mavsdk_getLongitude(void);
+double *mavsdk_getPositionArray(void);
 double mavsdk_getTakeOffAltitude(void);
 float mavsdk_getYaw(void);
 int mavsdk_healthAllOk(void);

include/pubsub.h

@@ -17,30 +17,30 @@
 
 typedef struct {
     UA_UInt16 id;
+    UA_UInt32 positionArrayId;
     UA_Double latitude;
-    UA_UInt32 latitudeId;
     UA_Double longitude;
-    UA_UInt32 longitudeId;
-    UA_Float altitudeAbs;
-    UA_UInt32 altitudeAbsId;
-    UA_Float altitudeRel;
-    UA_UInt32 altitudeRelId;
+    UA_Double altitudeAbs;
+    UA_Double altitudeRel;
     char message[MAX_MESSAGE_SIZE];
     UA_UInt32 messageId;
 } JSDroneData;
 
 typedef struct {
     char *name;
+    char *typeName;
+    UA_NodeId typeNodeId;
     char *description;
-    void * UA_RESTRICT value;
+    void *value;
     int type;
     UA_Byte builtInType;
+    UA_Int32 valueRank;
+    size_t arrayDimensionsSize;
+    UA_UInt32 *arrayDimensions;
     union {
-        UA_Double   (*getDouble)(void);
-        UA_Float    (*getFloat)(void);
-        UA_String   (*getString)(void);
+        void*     (*getArray)(void);
+        UA_String (*getString)(void);
     } getter;
-
 } VariableData;
 
 int subscribeOnly(UA_String *transportProfile,

mavsdk_wrapper.cpp

@@ -415,6 +415,16 @@ double mavsdk_getLongitude(void) {
     return telemetry->position().longitude_deg;
 }
 
+double *mavsdk_getPositionArray(void) {
+    Telemetry::Position position = telemetry->position();
+    double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
+    positionArray[0] = position.latitude_deg;
+    positionArray[1] = position.longitude_deg;
+    positionArray[2] = (double) position.absolute_altitude_m;
+    positionArray[3] = (double) position.relative_altitude_m;
+    return positionArray;
+}
+
 double mavsdk_getTakeOffAltitude(void) {
     const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
 

pubsub.c

@@ -66,14 +66,30 @@ addPublishedDataSet(UA_Server *server, UA_UInt32 id) {
 
 static UA_StatusCode
 readVariable(UA_Server *server,
-            const UA_NodeId *sessionId, void *sessionContext,
-            const UA_NodeId *nodeId, void *nodeContext,
-            UA_Boolean sourceTimeStamp, const UA_NumericRange *range,
-            UA_DataValue *dataValue) {
+             const UA_NodeId *sessionId, void *sessionContext,
+             const UA_NodeId *nodeId, void *nodeContext,
+             UA_Boolean sourceTimeStamp, const UA_NumericRange *range,
+             UA_DataValue *dataValue) {
     UA_StatusCode retval;
     VariableData varDetails = pubsubGetValue(nodeId->identifier.string);
-    retval = UA_Variant_setScalarCopy(&dataValue->value, varDetails.value,
-                                      &UA_TYPES[varDetails.type]);
+
+    switch(varDetails.valueRank) {
+        case UA_VALUERANK_SCALAR:
+            retval = UA_Variant_setScalarCopy(&dataValue->value,
+                                              varDetails.value,
+                                              &UA_TYPES[varDetails.type]);
+            break;
+        case UA_VALUERANK_ONE_DIMENSION:
+            retval = UA_Variant_setArrayCopy(&dataValue->value,
+                                             varDetails.value,
+                                             varDetails.arrayDimensions[0],
+                                             &UA_TYPES[varDetails.type]);
+            break;
+        default:
+            retval = UA_STATUSCODE_BAD;
+            break;
+    }
+
     if(retval != UA_STATUSCODE_GOOD)
         UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
     dataValue->hasValue = true;
@@ -96,21 +112,20 @@ addDataSourceVariable(UA_Server *server, VariableData varDetails) {
     attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
     attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.name);
     attr.dataType = UA_TYPES[varDetails.type].typeId;
+    attr.valueRank = varDetails.valueRank;
+    attr.arrayDimensions = varDetails.arrayDimensions;
+    attr.arrayDimensionsSize = varDetails.arrayDimensionsSize;
     attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
 
-    UA_NodeId currentNodeId = UA_NODEID_STRING(1, varDetails.name);
-    UA_QualifiedName currentName = UA_QUALIFIEDNAME(1, varDetails.name);
-    UA_NodeId parentNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER);
-    UA_NodeId parentReferenceNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES);
-    UA_NodeId variableTypeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
-
     UA_DataSource dataSource;
     dataSource.read = readVariable;
     dataSource.write = writeVariable;
-    return UA_Server_addDataSourceVariableNode(server, currentNodeId,
-                                               parentNodeId,
-                                               parentReferenceNodeId,
-                                               currentName, variableTypeNodeId,
+    return UA_Server_addDataSourceVariableNode(server,
+                                               UA_NODEID_STRING(1, varDetails.name),
+                                               UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
+                                               UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
+                                               UA_QUALIFIEDNAME(1, varDetails.name),
+                                               varDetails.typeNodeId,
                                                attr, dataSource, NULL, NULL);
 }
 
@@ -246,7 +261,8 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
  * Add subscribedvariables to the DataSetReader */
 static UA_StatusCode
 addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
-                       UA_UInt32 nb, UA_Duration samplingInterval,
+                       VariableData *variableArray, UA_UInt32 nb,
+                       UA_Duration samplingInterval,
                        void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
     if(server == NULL)
         return UA_STATUSCODE_BADINTERNALERROR;
@@ -287,9 +303,11 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
         UA_VariableAttributes vAttr = UA_VariableAttributes_default;
         UA_LocalizedText_copy(&readerConfig.dataSetMetaData.fields[i].description,
                               &vAttr.description);
-        vAttr.displayName.locale = UA_STRING("en-US");
-        vAttr.displayName.text = readerConfig.dataSetMetaData.fields[i].name;
+        vAttr.displayName = UA_LOCALIZEDTEXT("en-US", variableArray[i].name);
         vAttr.dataType = readerConfig.dataSetMetaData.fields[i].dataType;
+        vAttr.valueRank = readerConfig.dataSetMetaData.fields[i].valueRank;
+        vAttr.arrayDimensions = readerConfig.dataSetMetaData.fields[i].arrayDimensions;
+        vAttr.arrayDimensionsSize = readerConfig.dataSetMetaData.fields[i].arrayDimensionsSize;
 
         UA_NodeId newNode;
         retval |= UA_Server_addVariableNode(server,
@@ -297,7 +315,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
                                             folderId,
                                             UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                                             UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
-                                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
+                                            variableArray[i].typeNodeId,
                                             vAttr, NULL, &newNode);
 
         /*monitor variable*/
@@ -356,8 +374,47 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
         UA_NodeId_copy(&UA_TYPES[variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
         pMetaData->fields[i].builtInType = variableArray[i].builtInType;
         pMetaData->fields[i].name = UA_STRING (variableArray[i].name);
-        pMetaData->fields[i].valueRank = -1; /* scalar */
+        pMetaData->fields[i].valueRank = variableArray[i].valueRank;
+        pMetaData->fields[i].arrayDimensions = variableArray[i].arrayDimensions;
+        pMetaData->fields[i].arrayDimensionsSize = variableArray[i].arrayDimensionsSize;
+    }
+}
+
+static void
+setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariable) {
+    VariableData vDetails;
+    UA_VariableTypeAttributes vtAttr;
+
+    for(UA_UInt32 i = 0; i < nbVariable; i++) {
+        vDetails = variableArray[i];
+
+        switch(vDetails.valueRank) {
+            case UA_VALUERANK_SCALAR:
+                variableArray[i].typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
+                break;
+
+            case UA_VALUERANK_ONE_DIMENSION:
+                vtAttr = UA_VariableTypeAttributes_default;
+                UA_NodeId_copy(&UA_TYPES[vDetails.type].typeId, &vtAttr.dataType);
+                vtAttr.valueRank = vDetails.valueRank;
+                vtAttr.arrayDimensions = vDetails.arrayDimensions;
+                vtAttr.arrayDimensionsSize = vDetails.arrayDimensionsSize;
+                vtAttr.displayName = UA_LOCALIZEDTEXT("en-US", vDetails.typeName);
+                UA_Variant_setArray(&vtAttr.value, vDetails.value,
+                                    vDetails.arrayDimensions[0], &UA_TYPES[vDetails.type]);
+                UA_Server_addVariableTypeNode(server, UA_NODEID_NULL,
+                                              UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
+                                              UA_NODEID_NUMERIC(0, UA_NS0ID_HASSUBTYPE),
+                                              UA_QUALIFIEDNAME(1, vDetails.typeName),
+                                              UA_NODEID_NULL, vtAttr, NULL, &vDetails.typeNodeId);
+                break;
+
+            default:
+                UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Valuerank not handled");
+                break;
+        }
     }
+    return;
 }
 
 static UA_Server*
@@ -407,7 +464,7 @@ subscribe(UA_Server *server,
             return EXIT_FAILURE;
 
         /* Add SubscribedVariables to the created DataSetReader */
-        retval = addSubscribedVariables(server, readerIdent, i, interval, init_node_id);
+        retval = addSubscribedVariables(server, readerIdent, variableArray, i, interval, init_node_id);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
     }
@@ -427,6 +484,7 @@ int subscribeOnly(UA_String *transportProfile,
     UA_StatusCode retval;
 
     server = setServer(transportProfile, networkAddressUrl, id);
+    setVariableType(server, variableArray, nbVariable);
 
     subscribe(server, variableArray, nbVariable, id, nbReader, interval,
               init_node_id, get_reader_id, update);
@@ -449,6 +507,7 @@ int runPubsub(UA_String *transportProfile,
     UA_StatusCode retval;
 
     server = setServer(transportProfile, networkAddressUrl, id);
+    setVariableType(server, variableArray, nbVariable);
 
     /* Publishing */
 

qjs_wrapper.c

@@ -180,54 +180,70 @@ UA_UInt16 get_drone_id(UA_UInt32 nb) {
 }
 
 VariableData pubsub_get_value(UA_String identifier) {
-    VariableData var_details;
-    UA_String name;
+    VariableData varDetails;
+    UA_DataType type;
+    UA_Double *array;
 
     for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) {
-        name = UA_STRING(droneVariableArray[i].name);
+        UA_String name = UA_STRING(droneVariableArray[i].name);
         if(UA_String_equal(&identifier, &name)) {
-            var_details = droneVariableArray[i];
-
-            switch(var_details.type) {
-                case UA_TYPES_DOUBLE:
-                    *(UA_Double*)var_details.value = droneVariableArray[i].getter.getDouble();
-                    break;
-                case UA_TYPES_FLOAT:
-                    *(UA_Float*)var_details.value = droneVariableArray[i].getter.getFloat();
+            varDetails = droneVariableArray[i];
+            switch(varDetails.valueRank) {
+                case UA_VALUERANK_SCALAR:
+                    switch(varDetails.type) {
+                        case UA_TYPES_STRING:
+                            *(UA_String*)varDetails.value = droneVariableArray[i].getter.getString();
+                            break;
+                        default:
+                            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
+                            break;
+                    }
                     break;
-                case UA_TYPES_STRING:
-                    *(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
+
+                case UA_VALUERANK_ONE_DIMENSION:
+                    type = UA_TYPES[varDetails.type];
+                    size_t size = varDetails.arrayDimensions[0];
+                    array = (UA_Double *) droneVariableArray[i].getter.getArray();
+
+                    if(type.pointerFree) {
+                        memcpy(varDetails.value, array, type.memSize * size);
+                    } else {
+                        uintptr_t ptrs = (uintptr_t)array;
+                        uintptr_t ptrd = (uintptr_t)varDetails.value;
+                        UA_StatusCode retval = UA_STATUSCODE_GOOD;
+                        for(size_t j = 0; j < size; ++j) {
+                            retval |= UA_copy((void*)ptrs, (void*)ptrd, &type);
+                            ptrs += type.memSize;
+                            ptrd += type.memSize;
+                        }
+                        if(retval != UA_STATUSCODE_GOOD)
+                            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
+                    }
+
+                    free(array);
                     break;
+
                 default:
-                    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
+                    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
                     break;
             }
-            break;
         }
     }
 
-    return var_details;
+    return varDetails;
 }
 
 void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
     JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
     switch(magic) {
         case 0:
-            s->latitudeId = id;
+            s->positionArrayId = id;
             break;
         case 1:
-            s->longitudeId = id;
-            break;
-        case 2:
-            s->altitudeAbsId = id;
-            break;
-        case 3:
-            s->altitudeRelId = id;
-            break;
-        case 4:
             s->messageId = id;
             break;
         default:
+            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
             break;
     }
 }
@@ -235,20 +251,16 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
 void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
 {
     JSDroneData *s;
-    UA_String uaStr;;
+    UA_String uaStr;
+    UA_Double *positionArray;
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
         s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
-        if (s->latitudeId == id) {
-            s->latitude = *(UA_Double*) var->value.data;
-            return;
-        } else if (s->longitudeId == id) {
-            s->longitude = *(UA_Double*) var->value.data;
-            return;
-        } else if (s->altitudeAbsId == id) {
-            s->altitudeAbs = *(UA_Float*) var->value.data;
-            return;
-        } else if (s->altitudeRelId == id) {
-            s->altitudeRel = *(UA_Float*) var->value.data;
+        if (s->positionArrayId == id) {
+            positionArray = (UA_Double*) var->value.data;
+            s->latitude = positionArray[0];
+            s->longitude = positionArray[1];
+            s->altitudeAbs = positionArray[2];
+            s->altitudeRel = positionArray[3];
             return;
         } else if (s->messageId == id) {
             uaStr = *(UA_String*) var->value.data;
@@ -268,23 +280,20 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
 {
     JSDroneData *s;
     UA_String uaStr;
+    UA_Double *positionArray;
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
         s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
-        if (s->latitudeId == id) {
-            s->latitude = *(UA_Double*) var->value.data;
+        if (s->positionArrayId == id) {
+            positionArray = *(UA_Double**) var->value.data;
+            s->latitude = positionArray[0];
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received latitude of drone %d: %f°", s->id, s->latitude);
-            return;
-        } else if (s->longitudeId == id) {
-            s->longitude = *(UA_Double*) var->value.data;
+            s->longitude = positionArray[1];
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received longitude of drone %d: %f°", s->id, s->longitude);
-            return;
-        } else if (s->altitudeAbsId == id) {
-            s->altitudeAbs = *(UA_Float*) var->value.data;
+            s->altitudeAbs = positionArray[2];
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received absolute altitude of drone %d: %fm", s->id, s->altitudeAbs);
-            return;
-        } else if (s->altitudeRelId == id) {
-            s->altitudeRel = *(UA_Float*) var->value.data;
+            s->altitudeRel = positionArray[3];
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received relative altitude of drone %d: %fm", s->id, s->altitudeRel);
+            s->altitudeAbs = *(UA_Float*) var->value.data;
             return;
         } else if (s->messageId == id) {
             uaStr = *(UA_String*) var->value.data;