Make the qjs wrapper a binary to display the positions of the drones

pubsub.c

@@ -6,8 +6,9 @@
 #include "ua_pubsub.h"
 
 #include "pubsub.h"
+#include "dronedge.h"
 
-static UA_Server *server;
+static UA_Server *global_server;
 UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
           readerGroupIdent, readerIdent;
 
@@ -87,7 +88,7 @@ UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue, UA_DataType t
     UA_Variant var;
     UA_Variant_init(&var);
     UA_Variant_setScalar(&var, pvalue, &type);
-    return UA_Server_writeValue(server, varNodeId, var);
+    return UA_Server_writeValue(global_server, varNodeId, var);
 }
 
 static void
@@ -271,6 +272,7 @@ addSubscribedVariables (UA_Server *server, UA_NodeId dataSetReaderId, int nb,
         vAttr.dataType = readerConfig.dataSetMetaData.fields[i].dataType;
 
         UA_NodeId newNode;
+        printf("Subscribing to node is %d which is %s\n", (UA_UInt32) readerConfig.dataSetMetaData.fieldsSize*nb + i + 50000, readerConfig.dataSetMetaData.fields[i].name.data);
         retval |= UA_Server_addVariableNode(server,
                                             UA_NODEID_NUMERIC(1, (UA_UInt32) readerConfig.dataSetMetaData.fieldsSize*nb + i + 50000),
                                             folderId,
@@ -351,35 +353,35 @@ int runPubsub(UA_String *transportProfile,
     char readerName[19];
 
     UA_StatusCode retval = UA_STATUSCODE_GOOD;
-    server = UA_Server_new();
-    UA_ServerConfig *config = UA_Server_getConfig(server);
+    global_server = UA_Server_new();
+    UA_ServerConfig *config = UA_Server_getConfig(global_server);
     UA_ServerConfig_setDefault(config);
     UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
 
     callbackUpdate = update;
 
-    retval |= addPubSubConnection(server, transportProfile, networkAddressUrl, id);
+    retval |= addPubSubConnection(global_server, transportProfile, networkAddressUrl, id);
     if (retval != UA_STATUSCODE_GOOD)
         return EXIT_FAILURE;
 
     /* Publishing */
 
-    addPublishedDataSet(server, id);
+    addPublishedDataSet(global_server, id);
     for(size_t i = 0; i < nbVariable; i++) {
-        retval |= addVariable(server, variableArray[i]);
+        retval |= addVariable(global_server, variableArray[i]);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
-        addDataSetField(server, variableArray[i]);
+        addDataSetField(global_server, variableArray[i]);
     }
 
-    addWriterGroup(server);
-    retval |= addDataSetWriter(server);
+    addWriterGroup(global_server);
+    retval |= addDataSetWriter(global_server);
     if (retval != UA_STATUSCODE_GOOD)
         return EXIT_FAILURE;
 
     /* Subscribing */
 
-    retval |= addReaderGroup(server);
+    retval |= addReaderGroup(global_server);
     if (retval != UA_STATUSCODE_GOOD)
         return EXIT_FAILURE;
 
@@ -391,20 +393,104 @@ int runPubsub(UA_String *transportProfile,
     for (size_t i = 0; i < nbReader; i++) {
         publisherIdent = get_reader_id(i);
         UA_snprintf(readerName, sizeof(readerName - 1), "DataSet Reader %d", publisherIdent);
+        printf("DataSet Reader %d", publisherIdent);
         readerConfig.name = UA_STRING(readerName);
         readerConfig.publisherId.data = &publisherIdent;
 
-        retval |= addDataSetReader(server, variableArray, nbVariable, publisherIdent);
+        retval |= addDataSetReader(global_server, variableArray, nbVariable, publisherIdent);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
 
         /* Add SubscribedVariables to the created DataSetReader */
-        retval |= addSubscribedVariables(server, readerIdent, i, init_node_id);
+        retval |= addSubscribedVariables(global_server, readerIdent, i, init_node_id);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
     }
 
-    retval |= UA_Server_run(server, running);
-    UA_Server_delete(server);
+    retval |= UA_Server_run(global_server, running);
+    UA_Server_delete(global_server);
     return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
 }
+
+#define MAX_DRONE_ID 10
+int drone_ids[MAX_DRONE_ID];
+void init_node_id(UA_UInt32 id, int nb, int magic) {
+  return;
+}
+int get_reader_id(int nb) {
+  return drone_ids[nb];
+}
+UA_Float altitude_rel[MAX_DRONE_ID];
+UA_Float altitude_abs[MAX_DRONE_ID];
+UA_Double longitude[MAX_DRONE_ID];
+UA_Double latitude[MAX_DRONE_ID];
+void update(UA_UInt32 id, const UA_DataValue* value) {
+  UA_UInt16 index = id - 50000;
+  UA_UInt16 drone_id = index / 4;
+  if ((index % 4) == 3) {
+    // this is relative altitude in float
+    altitude_rel[drone_id] = *(UA_Float *)value->value.data;
+  } else if ((index % 4) == 2) {
+    // this is absolute altitude in float
+    altitude_abs[drone_id] = *(UA_Float *)value->value.data;
+    printf("DRONE %d: (%010f, %010f, %010f, %010f)\n", drone_id, latitude[drone_id], longitude[drone_id], altitude_abs[drone_id], altitude_rel[drone_id]);
+  } else if ((index % 4) == 1) {
+    // this is longitude in double
+    longitude[drone_id] = *(UA_Double *)value->value.data;
+  } else if ((index % 4) == 0) {
+    // this is latitude in double
+    latitude[drone_id] = *(UA_Double *)value->value.data;
+  }
+  return;
+}
+
+static void
+usage(char *progname) {
+    printf("usage: %s <uri> [device] drone_id1 drone_id2 ...\n", progname);
+}
+
+int main(int argc, char **argv) {
+    UA_String transportProfile = UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-udp-uadp");
+    UA_NetworkAddressUrlDataType networkAddressUrl = {UA_STRING_NULL , UA_STRING("opc.udp://224.0.0.22:4840/")};
+    UA_UInt16 drone_id_index, i;
+    UA_Boolean running = true;
+    if(argc < 2) {
+        usage(argv[0]);
+        return EXIT_FAILURE;
+    }
+
+    if(strcmp(argv[1], "-h") == 0) {
+        usage(argv[0]);
+        return EXIT_SUCCESS;
+    } else if(strncmp(argv[1], "opc.udp://", 10) == 0) {
+        networkAddressUrl.url = UA_STRING(argv[1]);
+        drone_id_index = 1;
+    } else if(strncmp(argv[1], "opc.eth://", 10) == 0) {
+        transportProfile =
+            UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-eth-uadp");
+        if(argc < 3) {
+            printf("Error: UADP/ETH needs an interface name\n");
+            return EXIT_FAILURE;
+        }
+
+        networkAddressUrl.networkInterface = UA_STRING(argv[2]);
+        networkAddressUrl.url = UA_STRING(argv[1]);
+        drone_id_index = 1;
+    } else {
+        printf ("Error: unknown URI\n");
+        return EXIT_FAILURE;
+    }
+    if (argc - drone_id_index < 1) {
+        usage(argv[0]);
+        return EXIT_FAILURE;
+    }
+    if (argc - drone_id_index > 10) {
+        printf("Cannot support more than MAX_DRONE_ID drone ids");
+        return EXIT_FAILURE;
+    }
+    for (i = drone_id_index; i < argc; i++)
+        drone_ids[i-drone_id_index] = (int)strtol(argv[i], NULL, 10);
+
+    return runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray, 4, 0 /* this reader id */, argc-drone_id_index, init_node_id, get_reader_id, update, &running);
+}
+