Comply with simulator API

See merge request !3

Makefile

-CFLAGS=-std=c99 -pipe -Wall -Wextra -Wpedantic -Werror -Wno-overlength-strings -Wno-unused-parameter -Wc++-compat -Wformat -Wformat-security -Wformat-nonliteral -Wmissing-prototypes -Wstrict-prototypes -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wnested-externs -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fPIC -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden -fPIC
+CFLAGS=-std=c99 -D_POSIX_C_SOURCE=200809L -pipe -Wall -Wextra -Wpedantic -Werror -Wno-overlength-strings -Wno-unused-parameter -Wc++-compat -Wformat -Wformat-security -Wformat-nonliteral -Wmissing-prototypes -Wstrict-prototypes -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wnested-externs -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fPIC -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden -fPIC
 CXXFLAGS=-pipe -Wall -Wextra -Wpedantic -Werror -Wno-overlength-strings -Wno-unused-parameter -Wformat -Wformat-security -Wformat-nonliteral -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fPIC -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden -fPIC
-LDFLAGS+= -std=c99 -pipe -Wall -Wextra -Wpedantic -Werror -Wno-static-in-inline -Wno-overlength-strings -Wno-unused-parameter -Wc++-compat -Wformat -Wformat-security -Wformat-nonliteral -Wmissing-prototypes -Wstrict-prototypes -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wnested-externs -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fPIC -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden -fPIC
-LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_mavlink_passthrough -lmavsdk_telemetry -lopen62541
+LDFLAGS+= -std=c99 -D_POSIX_C_SOURCE=200809L -pipe -Wall -Wextra -Wpedantic -Werror -Wno-static-in-inline -Wno-overlength-strings -Wno-unused-parameter -Wc++-compat -Wformat -Wformat-security -Wformat-nonliteral -Wmissing-prototypes -Wstrict-prototypes -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wnested-externs -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fPIC -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden -fPIC
+LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_manual_control -lmavsdk_mavlink_passthrough -lmavsdk_telemetry -lopen62541
 
 LIB_NAME := libqjswrapper.so
 

include/dronedge.h

@@ -5,11 +5,23 @@
 #include "mavsdk_wrapper.h"
 #include "pubsub.h"
 
+struct messageNode {
+  char *message;
+  struct messageNode *next;
+};
+typedef struct {
+  struct messageNode *head;
+  struct messageNode *tail;
+} MessageQueue;
+
 UA_Double latitude = 0;
 UA_Double longitude = 0;
 UA_Float altitude_abs = 0;
 UA_Float altitude_rel = 0;
-UA_UInt32 last_checkpoint = 0;
+UA_String message = {
+    .length = 0,
+    .data = NULL,
+};
 
 VariableData droneVariableArray[] = {
     {
@@ -45,12 +57,12 @@ VariableData droneVariableArray[] = {
         .getter.getFloat = mavsdk_getAltitudeRel,
     },
     {
-        .name = "last_checkpoint",
-        .description = "Last checkpoint flown over",
-        .value = &last_checkpoint,
-        .type = UA_TYPES_UINT32,
-        .builtInType = UA_NS0ID_UINT32,
-        .getter.getUint32 = getLastCheckpoint
+        .name = "message",
+        .description = "Message to send to the other drones",
+        .value = &message,
+        .type = UA_TYPES_STRING,
+        .builtInType = UA_NS0ID_STRING,
+        .getter.getString = get_message
     },
 };
 

include/mavsdk_wrapper.h

@@ -12,20 +12,17 @@ int mavsdk_reboot(void);
 
 // Flight state management functions
 int mavsdk_arm(void);
-int mavsdk_doParachute(float param);
-int mavsdk_loiter();
 int mavsdk_land(void);
 int mavsdk_takeOff(void);
 int mavsdk_takeOffAndWait(void);
+int mavsdk_triggerParachute(void);
 
 // Flight management functions
-int mavsdk_doReposition(float la, float lo, float a, float y);
+int mavsdk_loiter(float radius);
 int mavsdk_setAirspeed(float airspeed);
 int mavsdk_setAltitude(float altitude);
-int mavsdk_setTargetAltitude(float a);
-int mavsdk_setTargetCoordinates(double la, double lo, float a, float y);
-int mavsdk_setTargetCoordinatesXYZ(double x, double y, float z);
-int mavsdk_setTargetLatLong(double la, double lo);
+int mavsdk_setTargetCoordinates(double la, double lo, float a);
+int mavsdk_setManualControlInput(void);
 
 // Information functions
 float mavsdk_getAltitude(void);
@@ -35,15 +32,10 @@ double mavsdk_getInitialLatitude(void);
 double mavsdk_getInitialLongitude(void);
 double mavsdk_getLatitude(void);
 double mavsdk_getLongitude(void);
-uint64_t mavsdk_getTimestamp(void);
 double mavsdk_getTakeOffAltitude(void);
-float mavsdk_getPitch(void);
-float mavsdk_getRoll(void);
 float mavsdk_getYaw(void);
-float mavsdk_getAirspeed(void);
-float mavsdk_getClimbRate(void);
-float mavsdk_getThrottle(void);
 int mavsdk_healthAllOk(void);
+bool mavsdk_isInManualMode(void);
 int mavsdk_landed(void);
 #ifdef __cplusplus
 }

include/pubsub.h

@@ -13,6 +13,8 @@
 #define DATA_SET_WRITER_ID 1
 #define WRITER_GROUP_ID 1
 
+#define MAX_MESSAGE_SIZE 1024
+
 typedef struct {
     UA_UInt16 id;
     UA_Double latitude;
@@ -23,8 +25,8 @@ typedef struct {
     UA_UInt32 altitudeAbsId;
     UA_Float altitudeRel;
     UA_UInt32 altitudeRelId;
-    UA_UInt32 lastCheckpoint;
-    UA_UInt32 lastCheckpointId;
+    char message[MAX_MESSAGE_SIZE];
+    UA_UInt32 messageId;
 } JSDroneData;
 
 typedef struct {
@@ -34,10 +36,11 @@ typedef struct {
     int type;
     UA_Byte builtInType;
     union {
-        UA_UInt32   (*getUint32)(void);
-        UA_Float    (*getFloat)(void);
-        UA_Double   (*getDouble)(void);
         UA_DateTime (*getTimestamp)(void);
+        UA_Double   (*getDouble)(void);
+        UA_Float    (*getFloat)(void);
+        UA_String   (*getString)(void);
+        UA_UInt32   (*getUint32)(void);
     } getter;
 
 } VariableData;
@@ -45,7 +48,7 @@ typedef struct {
 int subscribeOnly(UA_String *transportProfile,
                   UA_NetworkAddressUrlDataType *networkAddressUrl,
                   VariableData *variableArray, size_t nbVariable,
-                  UA_UInt32 id, UA_UInt32 nbReader,
+                  UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
                   void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
                   UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
                   void (*update)(UA_UInt32 id, const UA_DataValue*),
@@ -54,14 +57,14 @@ int subscribeOnly(UA_String *transportProfile,
 int runPubsub(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
               VariableData *variableArray, size_t nbVariable, 
-              UA_UInt32 id, UA_UInt32 nbReader,
+              UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
               void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
               UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
               VariableData (*get_value)(UA_String identifier),
               void (*update)(UA_UInt32 id, const UA_DataValue*),
               UA_Boolean *running);
 
-UA_UInt32 getLastCheckpoint(void);
+UA_String get_message(void);
 
 UA_UInt16 get_drone_id(UA_UInt32 nb);
 
@@ -73,8 +76,6 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var);
 
 VariableData pubsub_get_value(UA_String identifier);
 
-void stop_pubsub(void);
-
 DLL_PUBLIC JSModuleDef *js_init_module(JSContext *ctx, const char *module_name);
 
 #endif /* __PUBSUB_H__ */

pubsub.c

@@ -135,13 +135,13 @@ addDataSetField(UA_Server *server, VariableData varDetails) {
  * The WriterGroup (WG) is part of the connection and contains the primary
  * configuration parameters for the message creation. */
 static void
-addWriterGroup(UA_Server *server) {
+addWriterGroup(UA_Server *server, UA_Duration publishingInterval) {
     /* Now we create a new WriterGroupConfig and add the group to the existing
      * PubSubConnection. */
     UA_WriterGroupConfig writerGroupConfig;
     memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
     writerGroupConfig.name = UA_STRING("Demo WriterGroup");
-    writerGroupConfig.publishingInterval = 100;
+    writerGroupConfig.publishingInterval = publishingInterval;
     writerGroupConfig.enabled = UA_FALSE;
     writerGroupConfig.writerGroupId = WRITER_GROUP_ID;
     writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
@@ -245,7 +245,8 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
  * Set SubscribedDataSet type to TargetVariables data type.
  * Add subscribedvariables to the DataSetReader */
 static UA_StatusCode
-addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, UA_UInt32 nb,
+addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
+                       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;
@@ -302,6 +303,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, UA_UInt32 n
         /*monitor variable*/
         UA_MonitoredItemCreateRequest monRequest = UA_MonitoredItemCreateRequest_default(newNode);
         init_node_id(newNode.identifier.numeric, nb, i);
+        monRequest.requestedParameters.samplingInterval = samplingInterval;
         UA_Server_createDataChangeMonitoredItem(server, UA_TIMESTAMPSTORETURN_SOURCE,
                                                 monRequest, NULL, dataChangeNotificationCallback);
 
@@ -375,7 +377,7 @@ setServer(UA_String *transportProfile,
 static UA_StatusCode
 subscribe(UA_Server *server,
           VariableData *variableArray, size_t nbVariable,
-          UA_UInt32 id, UA_UInt32 nbReader,
+          UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
           void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
           UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
           void (*update)(UA_UInt32 id, const UA_DataValue*)) {
@@ -405,7 +407,7 @@ subscribe(UA_Server *server,
             return EXIT_FAILURE;
 
         /* Add SubscribedVariables to the created DataSetReader */
-        retval = addSubscribedVariables(server, readerIdent, i, init_node_id);
+        retval = addSubscribedVariables(server, readerIdent, i, interval, init_node_id);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
     }
@@ -416,7 +418,7 @@ subscribe(UA_Server *server,
 int subscribeOnly(UA_String *transportProfile,
                   UA_NetworkAddressUrlDataType *networkAddressUrl,
                   VariableData *variableArray, size_t nbVariable,
-                  UA_UInt32 id, UA_UInt32 nbReader,
+                  UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
                   void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
                   UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
                   void (*update)(UA_UInt32 id, const UA_DataValue*),
@@ -426,8 +428,8 @@ int subscribeOnly(UA_String *transportProfile,
 
     server = setServer(transportProfile, networkAddressUrl, id);
 
-    subscribe(server, variableArray, nbVariable, id, nbReader, init_node_id,
-              get_reader_id, update);
+    subscribe(server, variableArray, nbVariable, id, nbReader, interval,
+              init_node_id, get_reader_id, update);
 
     retval = UA_Server_run(server, running);
     UA_Server_delete(server);
@@ -437,7 +439,7 @@ int subscribeOnly(UA_String *transportProfile,
 int runPubsub(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
               VariableData *variableArray, size_t nbVariable,
-              UA_UInt32 id, UA_UInt32 nbReader,
+              UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
               void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
               UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
               VariableData (*get_value)(UA_String identifier),
@@ -460,15 +462,15 @@ int runPubsub(UA_String *transportProfile,
         addDataSetField(server, variableArray[i]);
     }
 
-    addWriterGroup(server);
+    addWriterGroup(server, interval);
     retval = addDataSetWriter(server);
     if (retval != UA_STATUSCODE_GOOD)
         return EXIT_FAILURE;
 
     /* Subscribing */
 
-    subscribe(server, variableArray, nbVariable, id, nbReader, init_node_id,
-              get_reader_id, update);
+    subscribe(server, variableArray, nbVariable, id, nbReader, interval,
+              init_node_id, get_reader_id, update);
 
     retval = UA_Server_run(server, running);
     UA_Server_delete(server);