Add Drone class

include/pubsub_common.h

@@ -2,7 +2,26 @@
 #define __PUBSUB_COMMON_H__
 
 #include <open62541/server.h>
+#include <open62541/plugin/log_stdout.h>
 
 #define countof(x) (sizeof(x) / sizeof((x)[0]))
 
+typedef struct {
+    int id;
+    UA_Double latitude;
+    UA_UInt32 latitudeId;
+    UA_Double longitude;
+    UA_UInt32 longitudeId;
+    UA_Float altitude;
+    UA_UInt32 altitudeId;
+} JSDroneData;
+
+typedef struct {
+    char *name;
+    char *description;
+    void * UA_RESTRICT pdefaultValue;
+    int type;
+    UA_Byte builtInType;
+} VariableData;
+
 #endif /* __PUBSUB_COMMON_H__ */
\ No newline at end of file

include/pubsub_publish.h

@@ -3,10 +3,11 @@
 
 #include "pubsub_common.h"
 
-void writeFloat(char *name, float value);
-void writeDouble(char *name, double value);
+UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue,
+                            UA_DataType type);
 int publish(UA_String *transportProfile,
             UA_NetworkAddressUrlDataType *networkAddressUrl,
-            UA_Boolean *running);
+            VariableData const *variableArray, int nbVariable,
+            int id, UA_Boolean *running);
 
 #endif /* __PUBSUB_PUBLISH_H__ */

include/pubsub_subscribe.h

@@ -5,6 +5,10 @@
 
 int subscribe(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
+              VariableData const *variableArray, int nbVariable, int nbReader,
+              void (*init_node_id)(UA_UInt32 id, int nb, int magic),
+              int (*get_reader_id)(int nb),
+              void (*update)(UA_UInt32 id, const UA_DataValue*),
               UA_Boolean *running);
 
 #endif /* __PUBSUB_SUBSCRIBE_H__ */

pubsub_publish.c

@@ -4,32 +4,24 @@
 
 #include "pubsub_publish.h"
 
-typedef struct PublishedVariable {
-    char *name;
-    char *description;
-    void * UA_RESTRICT pdefaultValue;
-    UA_DataType type;
-} PublishedVariable;
-
 static UA_Server *server;
 UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent;
 
-static void
+static UA_StatusCode
 addPubSubConnection(UA_Server *server, UA_String *transportProfile,
-                    UA_NetworkAddressUrlDataType *networkAddressUrl){
-    /* Details about the connection configuration and handling are located
-     * in the pubsub connection tutorial */
+                    UA_NetworkAddressUrlDataType *networkAddressUrl,
+                    int id){
+    char name[20];
+    UA_snprintf(name, sizeof(name - 1), "UADP Connection %d", id);
     UA_PubSubConnectionConfig connectionConfig;
     memset(&connectionConfig, 0, sizeof(connectionConfig));
-    connectionConfig.name = UA_STRING("UADP Connection 1");
+    connectionConfig.name = UA_STRING(name);
     connectionConfig.transportProfileUri = *transportProfile;
     connectionConfig.enabled = UA_TRUE;
     UA_Variant_setScalar(&connectionConfig.address, networkAddressUrl,
                          &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
-    /* Changed to static publisherId from random generation to identify
-     * the publisher on Subscriber side */
-    connectionConfig.publisherId.numeric = 2234;
-    UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
+    connectionConfig.publisherId.numeric = id;
+    return UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
 }
 
 /**
@@ -40,57 +32,47 @@ addPubSubConnection(UA_Server *server, UA_String *transportProfile,
  * other PubSub elements are directly or indirectly linked with the PDS or
  * connection. */
 static void
-addPublishedDataSet(UA_Server *server) {
+addPublishedDataSet(UA_Server *server, int id) {
     /* The PublishedDataSetConfig contains all necessary public
     * information for the creation of a new PublishedDataSet */
+    char name[8];
+    UA_snprintf(name, sizeof(name - 1), "PDS %d", id);
     UA_PublishedDataSetConfig publishedDataSetConfig;
     memset(&publishedDataSetConfig, 0, sizeof(UA_PublishedDataSetConfig));
     publishedDataSetConfig.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
-    publishedDataSetConfig.name = UA_STRING("Demo PDS");
+    publishedDataSetConfig.name = UA_STRING(name);
     /* Create new PublishedDataSet based on the PublishedDataSetConfig. */
     UA_Server_addPublishedDataSet(server, &publishedDataSetConfig, &publishedDataSetIdent);
 }
 
-static void
-addVariable(UA_Server *server, PublishedVariable varDetails) {
+static UA_StatusCode
+addVariable(UA_Server *server, VariableData varDetails) {
     UA_VariableAttributes attr = UA_VariableAttributes_default;
-    UA_Variant_setScalar(&attr.value, varDetails.pdefaultValue, &varDetails.type);
+    UA_Variant_setScalar(&attr.value, varDetails.pdefaultValue, &UA_TYPES[varDetails.type]);
     attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
     attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.description);
-    attr.dataType = varDetails.type.typeId;
+    attr.dataType = UA_TYPES[varDetails.type].typeId;
     attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
 
-    UA_Server_addVariableNode(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.description),
-                                      UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
-                                      attr, NULL, NULL);
+    return UA_Server_addVariableNode(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.description),
+                                     UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
+                                                       attr, NULL, NULL);
 }
 
-static void
-writeVariable(char *name, void * UA_RESTRICT pvalue,
-              UA_DataType type)
+UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue, UA_DataType type)
 {
     UA_NodeId integerNodeId = UA_NODEID_STRING(1, name);
     UA_Variant var;
     UA_Variant_init(&var);
     UA_Variant_setScalar(&var, pvalue, &type);
-    UA_Server_writeValue(server, integerNodeId, var);
-}
-
-void writeFloat(char *name, float externValue) {
-    float localValue = externValue;
-    writeVariable(name, &localValue, UA_TYPES[UA_TYPES_FLOAT]);
-}
-
-void writeDouble(char *name, double externValue) {
-    double localValue = externValue;
-    writeVariable(name, &localValue, UA_TYPES[UA_TYPES_DOUBLE]);
+    return UA_Server_writeValue(server, integerNodeId, var);
 }
 
 static void
-addDataSetField(UA_Server *server, PublishedVariable varDetails) {
+addDataSetField(UA_Server *server, VariableData varDetails) {
     UA_NodeId dataSetFieldIdent;
     UA_DataSetFieldConfig dataSetFieldConfig;
     memset(&dataSetFieldConfig, 0, sizeof(UA_DataSetFieldConfig));
@@ -101,7 +83,7 @@ addDataSetField(UA_Server *server, PublishedVariable varDetails) {
     UA_NODEID_STRING(1, varDetails.name);
     dataSetFieldConfig.field.variable.publishParameters.attributeId = UA_ATTRIBUTEID_VALUE;
     UA_Server_addDataSetField(server, publishedDataSetIdent,
-                              &dataSetFieldConfig, &dataSetFieldIdent);
+                                     &dataSetFieldConfig, &dataSetFieldIdent);
 }
 
 /**
@@ -146,7 +128,7 @@ addWriterGroup(UA_Server *server) {
  * A DataSetWriter (DSW) is the glue between the WG and the PDS. The DSW is
  * linked to exactly one PDS and contains additional information for the
  * message generation. */
-static void
+static UA_StatusCode
 addDataSetWriter(UA_Server *server) {
     /* We need now a DataSetWriter within the WriterGroup. This means we must
      * create a new DataSetWriterConfig and add call the addWriterGroup function. */
@@ -156,54 +138,40 @@ addDataSetWriter(UA_Server *server) {
     dataSetWriterConfig.name = UA_STRING("Demo DataSetWriter");
     dataSetWriterConfig.dataSetWriterId = 62541;
     dataSetWriterConfig.keyFrameCount = 10;
-    UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
-                               &dataSetWriterConfig, &dataSetWriterIdent);
+    return UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
+                                      &dataSetWriterConfig, &dataSetWriterIdent);
 }
 
 int publish(UA_String *transportProfile,
             UA_NetworkAddressUrlDataType *networkAddressUrl,
-            UA_Boolean *running) {
+            VariableData const *variableArray, int nbVariable,
+            int id, UA_Boolean *running) {
     int i;
-    UA_Float defaultFloat = 0;
-    UA_Double defaultDouble = 0;
-
-    const PublishedVariable publishedVariableArray[] = {
-        {
-            .name = "lattitude",
-            .description = "Lattitude",
-            .pdefaultValue = &defaultDouble,
-            .type = UA_TYPES[UA_TYPES_DOUBLE],
-        },
-        {
-            .name = "longitude",
-            .description = "Longitude",
-            .pdefaultValue = &defaultDouble,
-            .type = UA_TYPES[UA_TYPES_DOUBLE],
-        },
-        {
-            .name = "altitude",
-            .description = "Altitude",
-            .pdefaultValue = &defaultFloat,
-            .type = UA_TYPES[UA_TYPES_FLOAT],
-        },
-    };
+    UA_StatusCode retval = UA_STATUSCODE_GOOD;
 
     server = UA_Server_new();
     UA_ServerConfig *config = UA_Server_getConfig(server);
     UA_ServerConfig_setDefault(config);
     UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
 
-    addPubSubConnection(server, transportProfile, networkAddressUrl);
-    addPublishedDataSet(server);
-    for(i = 0; i < countof(publishedVariableArray); i++) {
-        addVariable(server, publishedVariableArray[i]);
-        addDataSetField(server, publishedVariableArray[i]);
+    retval |= addPubSubConnection(server, transportProfile, networkAddressUrl, id);
+    if (retval != UA_STATUSCODE_GOOD)
+        return EXIT_FAILURE;
+
+    addPublishedDataSet(server, id);
+    for(i = 0; i < nbVariable; i++) {
+        retval |= addVariable(server, variableArray[i]);
+        if (retval != UA_STATUSCODE_GOOD)
+            return EXIT_FAILURE;
+        addDataSetField(server, variableArray[i]);
     }
-    addWriterGroup(server);
-    addDataSetWriter(server);
 
-    UA_StatusCode retval = UA_Server_run(server, running);
+    addWriterGroup(server);
+    retval |= addDataSetWriter(server);
+    if (retval != UA_STATUSCODE_GOOD)
+        return EXIT_FAILURE;
 
+    retval |= UA_Server_run(server, running);
     UA_Server_delete(server);
     return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
 }

qjs_wrapper.c

@@ -4,15 +4,142 @@
 #include "pubsub_publish.h"
 #include "pubsub_subscribe.h"
 
+static JSClassID js_drone_class_id;
+
+UA_Float defaultFloat = 0;
+UA_Double defaultDouble = 0;
+
+const VariableData droneVariableArray[] = {
+    {
+        .name = "latitude",
+        .description = "Latitude",
+        .pdefaultValue = &defaultDouble,
+        .type = UA_TYPES_DOUBLE,
+        .builtInType = UA_NS0ID_DOUBLE,
+    },
+    {
+        .name = "longitude",
+        .description = "Longitude",
+        .pdefaultValue = &defaultDouble,
+        .type = UA_TYPES_DOUBLE,
+        .builtInType = UA_NS0ID_DOUBLE,
+    },
+    {
+        .name = "altitude",
+        .description = "Altitude",
+        .pdefaultValue = &defaultFloat,
+        .type = UA_TYPES_FLOAT,
+        .builtInType = UA_NS0ID_FLOAT,
+    },
+};
+
 static UA_Boolean publishing = true;
 static UA_Boolean subscribing = true;
 
+int nbDrone;
+static JSValueConst *drone_object_id_list;
+
+static void js_drone_finalizer(JSRuntime *rt, JSValue val)
+{
+    JSDroneData *s = JS_GetOpaque(val, js_drone_class_id);
+    js_free_rt(rt, s);
+}
+
+static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target, 
+                             int argc, JSValueConst *argv)
+{
+    JSDroneData *s;
+    JSValue obj = JS_UNDEFINED;
+    JSValue proto;
+    
+    s = js_mallocz(ctx, sizeof(*s));
+    if (!s)
+        return JS_EXCEPTION;
+    if (JS_ToInt32(ctx, &s->id, argv[0]))
+        goto fail;
+
+    proto = JS_GetPropertyStr(ctx, new_target, "prototype");
+    if (JS_IsException(proto))
+        goto fail;
+    obj = JS_NewObjectProtoClass(ctx, proto, js_drone_class_id);
+    JS_FreeValue(ctx, proto);
+    if (JS_IsException(obj))
+        goto fail;
+    JS_SetOpaque(obj, s);
+    return obj;
+ fail:
+    js_free(ctx, s);
+    JS_FreeValue(ctx, obj);
+    return JS_EXCEPTION;
+}
+
+static JSValue js_drone_init(JSContext *ctx, JSValueConst this_val,
+                             int argc, JSValueConst *argv)
+{
+    int nb;
+    JSDroneData *s = JS_GetOpaque2(ctx, this_val, js_drone_class_id);
+    if (!s)
+        return JS_EXCEPTION;
+    if (JS_ToInt32(ctx, &nb, argv[0]))
+        return JS_EXCEPTION;
+    drone_object_id_list[nb] = this_val;
+    return JS_UNDEFINED;
+}
+
+static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
+{
+    JSDroneData *s = JS_GetOpaque2(ctx, this_val, js_drone_class_id);
+    if (!s)
+        return JS_EXCEPTION;
+    switch(magic) {
+        case 0:
+            return JS_NewInt32(ctx, s->id);
+        case 1:
+            return JS_NewFloat64(ctx, s->latitude);
+        case 2:
+            return JS_NewFloat64(ctx, s->longitude);
+        case 3:
+            return JS_NewFloat64(ctx, s->altitude);
+        default:
+            return JS_EXCEPTION;
+    }
+}
+
+static JSClassDef js_drone_class = {
+    "Drone",
+    .finalizer = js_drone_finalizer,
+};
+
+static const JSCFunctionListEntry js_drone_proto_funcs[] = {
+    JS_CGETSET_MAGIC_DEF("id", js_drone_get, NULL, 0),
+    JS_CGETSET_MAGIC_DEF("latitude", js_drone_get, NULL, 1),
+    JS_CGETSET_MAGIC_DEF("longitude", js_drone_get, NULL, 2),
+    JS_CGETSET_MAGIC_DEF("altitude", js_drone_get, NULL, 3),
+    JS_CFUNC_DEF("init", 1, js_drone_init),
+};
+
+void pubsub_publish_coordinates(double latitude, double longitude, float altitude)
+{
+    UA_StatusCode res = UA_STATUSCODE_GOOD;
+
+    res |= writeVariable(droneVariableArray[0].name, &latitude,
+                        UA_TYPES[droneVariableArray[0].type]);
+    res |= writeVariable(droneVariableArray[1].name, &longitude,
+                        UA_TYPES[droneVariableArray[1].type]);
+    res |= writeVariable(droneVariableArray[2].name, &altitude,
+                        UA_TYPES[droneVariableArray[2].type]);
+    if (res != UA_STATUSCODE_GOOD)
+        UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
+                     "Writing variable returned value %x", res);
+}
+
 static JSValue js_pubsub_publish(JSContext *ctx, JSValueConst this_val,
                                  int argc, JSValueConst *argv)
 {
     const char *ipv6;
     const char *port;
     char urlBuffer[44];
+    int id;
     int res;
 
     ipv6 = JS_ToCString(ctx, argv[0]);
@@ -24,13 +151,85 @@ static JSValue js_pubsub_publish(JSContext *ctx, JSValueConst this_val,
     UA_NetworkAddressUrlDataType networkAddressUrl =
         {UA_STRING_NULL , UA_STRING(urlBuffer)};
 
-    res = publish(&transportProfile, &networkAddressUrl, &publishing);
+    if (JS_ToInt32(ctx, &id, argv[2]))
+        return JS_EXCEPTION;
+
+    res = publish(&transportProfile, &networkAddressUrl, droneVariableArray,
+                  countof(droneVariableArray), id, &publishing);
     JS_FreeCString(ctx, ipv6);
     JS_FreeCString(ctx, port);
-
     return JS_NewInt32(ctx, res);
 }
 
+static JSValue js_init_subscription(JSContext *ctx, JSValueConst this_val,
+                                   int argc, JSValueConst *argv)
+{
+    if (JS_ToInt32(ctx, &nbDrone, argv[0]))
+        return JS_EXCEPTION;
+
+    drone_object_id_list = malloc(nbDrone * sizeof(JSValueConst));
+    return JS_NewInt32(ctx, 0);
+}
+
+int get_drone_id(int nb) {
+    JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
+    return s->id;
+}
+
+void init_node_id(UA_UInt32 id, int nb, int magic) {
+    JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
+    switch(magic) {
+        case 0:
+            s->latitudeId = id;
+            break;
+        case 1:
+            s->longitudeId = id;
+            break;
+        case 2:
+            s->altitudeId = id;
+            break;
+        default:
+            break;
+    }
+}
+
+void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
+{
+    JSDroneData *s;
+    for(size_t i = 0; i < nbDrone; i++) {
+        s = JS_GetOpaque(drone_object_id_list[i], js_drone_class_id);
+        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->altitudeId == id) {
+            s->altitude = *(UA_Float*) var->value.data;
+            return;
+        }
+    }
+    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
+}
+
+static JSValue js_pubsub_write(JSContext *ctx, JSValueConst this_val,
+                               int argc, JSValueConst *argv)
+{
+      double latitude;
+      double longitude;
+      double altitude;
+
+      if (JS_ToFloat64(ctx, &latitude, argv[0]))
+          return JS_EXCEPTION;
+      if (JS_ToFloat64(ctx, &longitude, argv[1]))
+          return JS_EXCEPTION;
+      if (JS_ToFloat64(ctx, &altitude, argv[2]))
+          return JS_EXCEPTION;
+
+      pubsub_publish_coordinates(latitude, longitude, altitude);
+      return JS_NewInt32(ctx, 0);
+}
+
 static JSValue js_pubsub_subscribe(JSContext *ctx, JSValueConst this_val,
                                    int argc, JSValueConst *argv)
 {
@@ -48,20 +247,14 @@ static JSValue js_pubsub_subscribe(JSContext *ctx, JSValueConst this_val,
     UA_NetworkAddressUrlDataType networkAddressUrl =
         {UA_STRING_NULL , UA_STRING(urlBuffer)};
 
-    res = subscribe(&transportProfile, &networkAddressUrl, &subscribing);
+    res = subscribe(&transportProfile, &networkAddressUrl, droneVariableArray,
+                    countof(droneVariableArray), nbDrone, init_node_id,
+                    get_drone_id, pubsub_update_coordinates, &subscribing);
     JS_FreeCString(ctx, ipv6);
     JS_FreeCString(ctx, port);
-
     return JS_NewInt32(ctx, res);
 }
 
-void pubsub_set_coordinates(double lattitude, double longitude, float altitude)
-{
-    writeDouble("lattitude", lattitude);
-    writeDouble("longitude", longitude);
-    writeFloat("altitude", altitude);
-}
-
 static JSValue js_stop_publishing(JSContext *ctx, JSValueConst this_val,
                                   int argc, JSValueConst *argv)
 {
@@ -73,6 +266,7 @@ static JSValue js_stop_subscribing(JSContext *ctx, JSValueConst this_val,
                                    int argc, JSValueConst *argv)
 {
     subscribing = false;
+    free(drone_object_id_list);
     return JS_NewInt32(ctx, 0);
 }
 
@@ -89,7 +283,7 @@ static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst this_val,
     if (JS_ToInt32(ctx, &timeout, argv[2]))
         return JS_EXCEPTION;
 
-    res = mavsdk_start(url, log_file, timeout, pubsub_set_coordinates);
+    res = mavsdk_start(url, log_file, timeout, pubsub_publish_coordinates);
     JS_FreeCString(ctx, url);
     JS_FreeCString(ctx, log_file);
 
@@ -361,10 +555,28 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("subscribe", 2, js_pubsub_subscribe ),
     JS_CFUNC_DEF("stopPublishing", 0, js_stop_publishing ),
     JS_CFUNC_DEF("stopSubscribing", 0, js_stop_subscribing ),
+    JS_CFUNC_DEF("initSubscription", 1, js_init_subscription ),
+    JS_CFUNC_DEF("pubsubWrite", 3, js_pubsub_write ),
 };
 
 static int js_mavsdk_init(JSContext *ctx, JSModuleDef *m)
 {
+    JSValue drone_proto, drone_class;
+
+    JS_NewClassID(&js_drone_class_id);
+    JS_NewClass(JS_GetRuntime(ctx), js_drone_class_id, &js_drone_class);
+
+    drone_proto = JS_NewObject(ctx);
+    JS_SetPropertyFunctionList(ctx, drone_proto, js_drone_proto_funcs,
+                               countof(js_drone_proto_funcs));
+
+    drone_class = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
+                                   JS_CFUNC_constructor, 0);
+    JS_SetConstructor(ctx, drone_class, drone_proto);
+    JS_SetClassProto(ctx, js_drone_class_id, drone_proto);
+
+    JS_SetModuleExport(ctx, m, "Drone", drone_class);
+
     return JS_SetModuleExportList(ctx, m, js_mavsdk_funcs,
                                   countof(js_mavsdk_funcs));
 }
@@ -376,5 +588,6 @@ JSModuleDef *js_init_module(JSContext *ctx, const char *module_name)
     if (!m)
         return NULL;
     JS_AddModuleExportList(ctx, m, js_mavsdk_funcs, countof(js_mavsdk_funcs));
+    JS_AddModuleExport(ctx, m, "Drone");
     return m;
 }