Use datasources to be single-threaded

include/dronedge.h

@@ -2,47 +2,55 @@
 #define __DRONEDGE_H__
 
 #include <open62541/server.h>
+#include "mavsdk_wrapper.h"
 #include "pubsub.h"
 
-UA_Double defaultDouble = 0;
-UA_Float defaultFloat = 0;
-UA_UInt32 defaultUInt = 0;
+UA_Double latitude = 0;
+UA_Double longitude = 0;
+UA_Float altitude_abs = 0;
+UA_Float altitude_rel = 0;
+UA_UInt32 last_checkpoint = 0;
 
-const VariableData droneVariableArray[] = {
+VariableData droneVariableArray[] = {
     {
         .name = "latitude",
         .description = "Latitude",
-        .pdefaultValue = &defaultDouble,
+        .value = &latitude,
         .type = UA_TYPES_DOUBLE,
         .builtInType = UA_NS0ID_DOUBLE,
+        .getter.getDouble = mavsdk_getLatitude,
     },
     {
         .name = "longitude",
         .description = "Longitude",
-        .pdefaultValue = &defaultDouble,
+        .value = &longitude,
         .type = UA_TYPES_DOUBLE,
         .builtInType = UA_NS0ID_DOUBLE,
+        .getter.getDouble = mavsdk_getLongitude,
     },
     {
         .name = "altitude_abs",
         .description = "Absolute Altitude (AMSL)",
-        .pdefaultValue = &defaultFloat,
+        .value = &altitude_abs,
         .type = UA_TYPES_FLOAT,
         .builtInType = UA_NS0ID_FLOAT,
+        .getter.getFloat = mavsdk_getAltitude,
     },
     {
         .name = "altitude_rel",
         .description = "Relative Altitude",
-        .pdefaultValue = &defaultFloat,
+        .value = &altitude_rel,
         .type = UA_TYPES_FLOAT,
         .builtInType = UA_NS0ID_FLOAT,
+        .getter.getFloat = mavsdk_getAltitudeRel,
     },
     {
         .name = "last_checkpoint",
         .description = "Last checkpoint flown over",
-        .pdefaultValue = &defaultUInt,
+        .value = &last_checkpoint,
         .type = UA_TYPES_UINT32,
         .builtInType = UA_NS0ID_UINT32,
+        .getter.getUint32 = getLastCheckpoint
     },
 };
 

include/mavsdk_wrapper.h

@@ -6,8 +6,7 @@ extern "C" {
 #endif
 #include <stdint.h>
 // Connexion management functions
-int mavsdk_start(const char * url, const char * log_file, int timeout,
-                 void (*publishCoordinates)(double, double, float, float));
+int mavsdk_start(const char * url, const char * log_file, int timeout);
 int mavsdk_stop(void);
 int mavsdk_reboot(void);
 
@@ -31,15 +30,19 @@ int mavsdk_setTargetLatLong(double la, double lo);
 // Information functions
 float mavsdk_getAltitude(void);
 float mavsdk_getAltitudeRel(void);
-double mavsdk_getInitialAltitude(void);
+float mavsdk_getInitialAltitude(void);
 double mavsdk_getInitialLatitude(void);
 double mavsdk_getInitialLongitude(void);
 double mavsdk_getLatitude(void);
 double mavsdk_getLongitude(void);
-double mavsdk_getPitch(void);
-double mavsdk_getRoll(void);
+uint64_t mavsdk_getTimestamp(void);
 double mavsdk_getTakeOffAltitude(void);
-double mavsdk_getYaw(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);
 int mavsdk_landed(void);
 #ifdef __cplusplus

include/pubsub.h

@@ -26,21 +26,32 @@ typedef struct {
 typedef struct {
     char *name;
     char *description;
-    void * UA_RESTRICT pdefaultValue;
+    void * UA_RESTRICT value;
     int type;
     UA_Byte builtInType;
-} VariableData;
+    union {
+        UA_UInt32   (*getUint32)(void);
+        UA_Float    (*getFloat)(void);
+        UA_Double   (*getDouble)(void);
+        UA_DateTime (*getTimestamp)(void);
+    } getter;
 
-UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue,
-                            UA_DataType type);
+} VariableData;
 
 int runPubsub(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
-              VariableData const *variableArray, int nbVariable, 
+              VariableData *variableArray, int nbVariable, 
               int id, int nbReader,
               void (*init_node_id)(UA_UInt32 id, int nb, int magic),
               int (*get_reader_id)(int nb),
+              VariableData (*get_value)(UA_String identifier),
               void (*update)(UA_UInt32 id, const UA_DataValue*),
               UA_Boolean *running);
 
+UA_UInt32 getLastCheckpoint(void);
+
+VariableData pubsub_get_value(UA_String identifier);
+
+void stop_pubsub(void);
+
 #endif /* __PUBSUB_H__ */

mavsdk_wrapper.cpp

@@ -19,7 +19,6 @@ using std::chrono::seconds;
 using std::this_thread::sleep_for;
 
 #define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
-#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue
 #define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour
 
 static std::ofstream log_file_fd;
@@ -33,27 +32,18 @@ static std::shared_ptr<System> msystem;
 static auto prom = std::promise<std::shared_ptr<System>>{};
 static std::future<std::shared_ptr<System>> fut;
 
-static double drone_la;
-static double drone_lo;
-static double drone_roll;
-static double drone_pitch;
-static double drone_yaw;
-static float drone_a;
-static float drone_at;
 static Telemetry::FlightMode flight_mode;
+static Telemetry::EulerAngle angle;
+static Telemetry::FixedwingMetrics metric;
+static Telemetry::Position position;
+static uint64_t timestamp_us;
+static Telemetry::Position origin;
 
-static double initial_drone_la;
-static double initial_drone_lo;
-static double initial_drone_la_rad;
-static double initial_drone_lo_rad;
-static float initial_drone_a;
 static double xy_ratio;
-static bool initial_coords_set = false;
 
 static const double EARTH_RADIUS = 6371000.F;
 
 static int mavsdk_started = 0;
-static void (*publish_fn)(double, double, float, float);
 
 // Logs functions
 
@@ -72,11 +62,6 @@ static void log_error_from_result(std::string message, Enumeration result) {
   log_error(oss.str());
 }
 
-static void log_telemetry(std::string message) {
-          // set to blue                 set to default color again
-    log(TELEMETRY_CONSOLE_TEXT + message + NORMAL_CONSOLE_TEXT);
-}
-
 // Action functions
 
 static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
@@ -104,8 +89,7 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string
 
 // Connexion management functions
 
-int mavsdk_start(const char * url, const char * log_file, int timeout,
-                 void (*publishCoordinates)(double, double, float, float)) {
+int mavsdk_start(const char * url, const char * log_file, int timeout) {
     std::string connection_url(url);
     ConnectionResult connection_result;
     log_file_fd.open(log_file);
@@ -149,40 +133,43 @@ int mavsdk_start(const char * url, const char * log_file, int timeout,
         }
     });
 
-    /*log("Subscribing to Euler angle...");
+    log("Subscribing to Euler angle...");
     telemetry->subscribe_attitude_euler([](Telemetry::EulerAngle euler_angle) {
-        drone_roll = euler_angle.roll_deg;
-        drone_pitch = euler_angle.pitch_deg;
-        drone_yaw = euler_angle.yaw_deg;
-    });*/
+        angle = euler_angle;
+    });
+
+    log("Subscribing to Fixedwing Metrics...");
+    telemetry->subscribe_fixedwing_metrics([](Telemetry::FixedwingMetrics m) {
+        metric = m;
+    });
 
     log("Subscribing to position...");
-    // Set up callback to monitor altitude while the vehicle is in flight
-    publish_fn = publishCoordinates;
-    telemetry->subscribe_position([](Telemetry::Position position) {
+    telemetry->subscribe_position([](Telemetry::Position pos) {
+        timestamp_us = std::chrono::duration_cast<std::chrono::milliseconds>(
+                          std::chrono::system_clock::now().time_since_epoch()
+                       ).count();
+
         std::ostringstream oss;
-	      oss << drone_a << " m " << drone_at << " m "
-            << drone_la << " " << drone_lo << " ";
-        log_telemetry(oss.str());
-
-        drone_la = position.latitude_deg;
-        drone_lo = position.longitude_deg;
-        drone_a = position.absolute_altitude_m;
-        drone_at = position.relative_altitude_m;
-        publish_fn(drone_la, drone_lo, drone_a, drone_at);
-
-        if(!initial_coords_set && drone_la != 0) {
-            initial_drone_la = drone_la;
-            initial_drone_lo = drone_lo;
-            initial_drone_la_rad = (M_PI * drone_la) / 180.F;
-            initial_drone_lo_rad = (M_PI * drone_lo) / 180.F;
-            initial_drone_a = drone_a;
-            xy_ratio = std::cos(initial_drone_la_rad);
-            initial_coords_set = true;
-        }
+	      position = pos;
+
+        oss << mavsdk_getTimestamp() << "; "
+            << mavsdk_getLatitude() << "; " << mavsdk_getLongitude() << "; "
+            << mavsdk_getAltitude() << "; " << mavsdk_getAltitudeRel() << "; "
+            << mavsdk_getPitch() << "; " << mavsdk_getRoll() << "; " << mavsdk_getYaw() << "; "
+            << mavsdk_getAirspeed() << "; " << mavsdk_getThrottle() << "; " << mavsdk_getClimbRate();
+        log(oss.str());
     });
+
+    while(isnan(position.absolute_altitude_m)) {
+        log("Waiting for first telemetry");
+        sleep_for(seconds(1));
+    }
+    origin = position;
+    xy_ratio = std::cos((M_PI * origin.latitude_deg) / 180.F);
+
     log("MAVSDK started...");
     mavsdk_started = 1;
+    log("timestamp; latitude; longitude; AMSL (m); rel altitude (m); pitch (°); roll(°); yaw(°); air speed (m/s); throttle(%); climb rate(m/s)");
     return 0;
 }
 
@@ -264,7 +251,9 @@ int mavsdk_loiter() {
     if(!mavsdk_started)
         return -1;
 
-    return mavsdk_doReposition(drone_la, drone_lo, drone_a, 0);
+    return mavsdk_doReposition((float)position.latitude_deg,
+                               (float)position.longitude_deg,
+                               position.absolute_altitude_m, 0);
 }
 
 int mavsdk_takeOff(void) {
@@ -346,7 +335,9 @@ int mavsdk_setTargetAltitude(double a) {
         return -1;
 
     log("Going to altitude " + std::to_string(a) + "m");
-    const Action::Result result = action->goto_location(drone_la, drone_lo, (float) a, 0);
+    const Action::Result result = action->goto_location(position.latitude_deg,
+                                                        position.longitude_deg,
+                                                        a, 0);
     if (result != Action::Result::Success) {
         log_error_from_result("Goto location failed", result);
         return -1;
@@ -371,52 +362,48 @@ int mavsdk_setTargetCoordinates(double la, double lo, double a, double y) {
 int mavsdk_setTargetCoordinatesXYZ(double x, double y, double z) {
     double la, lo;
 
-    la = ((initial_drone_la_rad + y / EARTH_RADIUS) * 180.F) / M_PI;
-    lo = ((initial_drone_lo_rad + x / (EARTH_RADIUS * xy_ratio)) * 180.F) / M_PI;
+    la = ((origin.latitude_deg + y / EARTH_RADIUS) * 180.F) / M_PI;
+    lo = ((origin.longitude_deg + x / (EARTH_RADIUS * xy_ratio)) * 180.F) / M_PI;
 
     return mavsdk_setTargetCoordinates(la, lo, z, 0);
 }
 
 int mavsdk_setTargetLatLong(double la, double lo) {
-    return mavsdk_setTargetCoordinates(la, lo, drone_a, 0);
+    return mavsdk_setTargetCoordinates(la, lo, position.absolute_altitude_m, 0);
 }
 
 // Information functions
 
 float mavsdk_getAltitude(void) {
-    return drone_a;
+    return position.absolute_altitude_m;
 }
 
 float mavsdk_getAltitudeRel(void) {
-    return drone_at;
+    return position.relative_altitude_m;
 }
 
-double mavsdk_getInitialAltitude(void) {
-    return initial_drone_a;
+float mavsdk_getInitialAltitude(void) {
+    return origin.absolute_altitude_m;
 }
 
 double mavsdk_getInitialLatitude(void) {
-    return initial_drone_la;
+    return origin.latitude_deg;
 }
 
 double mavsdk_getInitialLongitude(void) {
-    return initial_drone_lo;
+    return origin.longitude_deg;
 }
 
 double mavsdk_getLatitude(void) {
-    return drone_la;
+    return position.latitude_deg;
 }
 
 double mavsdk_getLongitude(void) {
-    return drone_lo;
+    return position.longitude_deg;
 }
 
-double mavsdk_getPitch(void) {
-    return drone_pitch;
-}
-
-double mavsdk_getRoll(void) {
-    return drone_roll;
+uint64_t mavsdk_getTimestamp(void) {
+    return timestamp_us;
 }
 
 double mavsdk_getTakeOffAltitude(void) {
@@ -429,8 +416,28 @@ double mavsdk_getTakeOffAltitude(void) {
     return response.second;
 }
 
-double mavsdk_getYaw(void) {
-    return drone_yaw;
+float mavsdk_getPitch(void) {
+    return angle.pitch_deg;
+}
+
+float mavsdk_getRoll(void) {
+    return angle.roll_deg;
+}
+
+float mavsdk_getYaw(void) {
+    return angle.yaw_deg;
+}
+
+float mavsdk_getAirspeed(void) {
+    return metric.airspeed_m_s;
+}
+
+float mavsdk_getClimbRate(void) {
+    return metric.climb_rate_m_s;
+}
+
+float mavsdk_getThrottle(void) {
+    return metric.throttle_percentage;
 }
 
 int mavsdk_healthAllOk(void) {

pubsub.c

@@ -4,7 +4,6 @@
 #include <open62541/plugin/pubsub_udp.h>
 
 #include "ua_pubsub.h"
-
 #include "pubsub.h"
 
 static UA_Server *server;
@@ -13,10 +12,11 @@ UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
 
 UA_DataSetReaderConfig readerConfig;
 
+VariableData (*pubsubGetValue)(UA_String identifier);
 static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*);
 
 static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
-                                VariableData const *variableArray,
+                                VariableData *variableArray,
                                 int nbVariable, int id);
 
 static UA_StatusCode
@@ -66,29 +66,53 @@ addPublishedDataSet(UA_Server *server, int id) {
 }
 
 static UA_StatusCode
-addVariable(UA_Server *server, VariableData varDetails) {
+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) {
+    UA_StatusCode retval;
+    VariableData varDetails = pubsubGetValue(nodeId->identifier.string);
+    retval = UA_Variant_setScalarCopy(&dataValue->value, varDetails.value,
+                                      &UA_TYPES[varDetails.type]);
+    if(retval != UA_STATUSCODE_GOOD)
+        UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
+    dataValue->hasValue = true;
+    return retval;
+}
+
+static UA_StatusCode
+writeVariable(UA_Server *server,
+              const UA_NodeId *sessionId, void *sessionContext,
+              const UA_NodeId *nodeId, void *nodeContext,
+              const UA_NumericRange *range, const UA_DataValue *data) {
+    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
+                "Updating variables manually is not implemented");
+    return UA_STATUSCODE_BADINTERNALERROR;
+}
+
+static UA_StatusCode
+addDataSourceVariable(UA_Server *server, VariableData varDetails) {
     UA_VariableAttributes attr = UA_VariableAttributes_default;
-    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.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.name);
     attr.dataType = UA_TYPES[varDetails.type].typeId;
     attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
 
-    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);
-}
-
-UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue, UA_DataType type)
-{
-    UA_NodeId varNodeId = UA_NODEID_STRING(1, name);
-    UA_Variant var;
-    UA_Variant_init(&var);
-    UA_Variant_setScalar(&var, pvalue, &type);
-    return UA_Server_writeValue(server, varNodeId, var);
+    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,
+                                               attr, dataSource, NULL, NULL);
 }
 
 static void
@@ -97,7 +121,7 @@ addDataSetField(UA_Server *server, VariableData varDetails) {
     UA_DataSetFieldConfig dataSetFieldConfig;
     memset(&dataSetFieldConfig, 0, sizeof(UA_DataSetFieldConfig));
     dataSetFieldConfig.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
-    dataSetFieldConfig.field.variable.fieldNameAlias = UA_STRING(varDetails.description);
+    dataSetFieldConfig.field.variable.fieldNameAlias = UA_STRING(varDetails.name);
     dataSetFieldConfig.field.variable.promotedField = UA_FALSE;
     dataSetFieldConfig.field.variable.publishParameters.publishedVariable =
     UA_NODEID_STRING(1, varDetails.name);
@@ -195,7 +219,7 @@ addReaderGroup(UA_Server *server) {
  * SubscribedDataSet and be contained within a ReaderGroup. */
 /* Add DataSetReader to the ReaderGroup */
 static UA_StatusCode
-addDataSetReader(UA_Server *server, VariableData const *variableArray,
+addDataSetReader(UA_Server *server, VariableData *variableArray,
                  int nbVariable, int id) {
     if(server == NULL) {
         return UA_STATUSCODE_BADINTERNALERROR;
@@ -226,8 +250,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, int nb,
-                        void (*init_node_id)(UA_UInt32 id, int nb, int magic)) {
+addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, int nb,
+                       void (*init_node_id)(UA_UInt32 id, int nb, int magic)) {
     if(server == NULL)
         return UA_STATUSCODE_BADINTERNALERROR;
 
@@ -311,7 +335,7 @@ addSubscribedVariables (UA_Server *server, UA_NodeId dataSetReaderId, int nb,
  * and PublishedDataSetFields of Publisher */
 /* Define MetaData for TargetVariables */
 static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
-                                VariableData const *variableArray,
+                                VariableData *variableArray,
                                 int nbVariable, int id) {
     char name[12];
     UA_snprintf(name, sizeof(name - 1), "DataSet %d", id);
@@ -342,10 +366,11 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
 
 int runPubsub(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
-              VariableData const *variableArray, int nbVariable, 
+              VariableData *variableArray, int nbVariable, 
               int id, int nbReader,
               void (*init_node_id)(UA_UInt32 id, int nb, int magic),
               int (*get_reader_id)(int nb),
+              VariableData (*get_value)(UA_String identifier),
               void (*update)(UA_UInt32 id, const UA_DataValue*),
               UA_Boolean *running) {
     UA_UInt16 publisherIdent;
@@ -357,6 +382,7 @@ int runPubsub(UA_String *transportProfile,
     UA_ServerConfig_setDefault(config);
     UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
 
+    pubsubGetValue = get_value;
     callbackUpdate = update;
 
     retval |= addPubSubConnection(server, transportProfile, networkAddressUrl, id);
@@ -367,7 +393,7 @@ int runPubsub(UA_String *transportProfile,
 
     addPublishedDataSet(server, id);
     for(size_t i = 0; i < nbVariable; i++) {
-        retval |= addVariable(server, variableArray[i]);
+        retval |= addDataSourceVariable(server, variableArray[i]);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
         addDataSetField(server, variableArray[i]);

qjs_wrapper.c

 #include <quickjs/quickjs.h>
-
-#include "mavsdk_wrapper.h"
 #include "dronedge.h"
 
 static JSClassID js_drone_class_id;
@@ -10,6 +8,13 @@ static UA_Boolean pubsub_running = true;
 int nbDrone;
 static JSValueConst *drone_object_id_list;
 
+static UA_UInt32 lastCheckpoint = 0;
+
+static inline int JS_ToUint16(JSContext *ctx, uint16_t *pres, JSValueConst val)
+{
+    return JS_ToInt32(ctx, (int32_t*)pres, val);
+}
+
 static void js_drone_finalizer(JSRuntime *rt, JSValue val)
 {
     JSDroneData *s = JS_GetOpaque(val, js_drone_class_id);
@@ -80,6 +85,21 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
     }
 }
 
+static JSValue js_drone_set_checkpoint(JSContext *ctx, JSValueConst this_val,
+                                       int argc, JSValueConst *argv)
+{
+    UA_UInt16 checkpoint;
+    if (JS_ToUint16(ctx, &checkpoint, argv[0]))
+        return JS_EXCEPTION;
+    lastCheckpoint = checkpoint;
+    return JS_UNDEFINED;
+}
+
+UA_UInt32 getLastCheckpoint(void)
+{
+    return lastCheckpoint;
+}
+
 static JSClassDef js_drone_class = {
     "Drone",
     .finalizer = js_drone_finalizer,
@@ -95,41 +115,47 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
     JS_CFUNC_DEF("init", 1, js_drone_init),
 };
 
-void pubsub_publish_coordinates(double latitude, double longitude, float altitudeAbs, float altitudeRel)
-{
-    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, &altitudeAbs,
-                        UA_TYPES[droneVariableArray[2].type]);
-    res |= writeVariable(droneVariableArray[3].name, &altitudeRel,
-                         UA_TYPES[droneVariableArray[3].type]);
-
-    if (res != UA_STATUSCODE_GOOD)
-        UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
-                     "Writing variable returned value %x", res);
+int get_drone_id(int nb) {
+    JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
+    return s->id;
 }
 
-static JSValue js_publish_checkpoint(JSContext *ctx, JSValueConst this_val,
-                                     int argc, JSValueConst *argv)
-{
-    int checkpoint;
-    int res;
-
-    if (JS_ToInt32(ctx, &checkpoint, argv[0]))
-        return JS_EXCEPTION;
-
-    res = writeVariable(droneVariableArray[4].name, &checkpoint,
-                        UA_TYPES[droneVariableArray[4].type]);
-    return JS_NewInt32(ctx, res);
+void stop_pubsub(void) {
+    pubsub_running = false;
+    free(drone_object_id_list);
 }
 
-int get_drone_id(int nb) {
-    JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
-    return s->id;
+VariableData pubsub_get_value(UA_String identifier) {
+    VariableData var_details;
+    UA_String name;
+
+    for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) {
+        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();
+                    break;
+                case UA_TYPES_UINT32:
+                    *(UA_UInt32*)var_details.value = droneVariableArray[i].getter.getUint32();
+                    break;
+                case UA_TYPES_DATETIME:
+                    *(UA_DateTime*)var_details.value = droneVariableArray[i].getter.getTimestamp();
+                    break;
+                default:
+                    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
+                    break;
+            }
+            break;
+        }
+    }
+
+    return var_details;
 }
 
 void init_node_id(UA_UInt32 id, int nb, int magic) {
@@ -207,7 +233,8 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
 
     res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
                     countof(droneVariableArray), id, nbDrone, init_node_id,
-                    get_drone_id, pubsub_update_coordinates, &pubsub_running);
+                    get_drone_id,  pubsub_get_value, pubsub_update_coordinates,
+                    &pubsub_running);
 
     JS_FreeCString(ctx, ipv6);
     JS_FreeCString(ctx, port);
@@ -246,7 +273,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_publish_coordinates);
+    res = mavsdk_start(url, log_file, timeout);
     JS_FreeCString(ctx, url);
     JS_FreeCString(ctx, log_file);
 
@@ -344,18 +371,6 @@ static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst this_val,
     return JS_NewInt32(ctx, mavsdk_setAirspeed(altitude));
 }
 
-static JSValue js_mavsdk_getRoll(JSContext *ctx, JSValueConst this_val,
-                                 int argc, JSValueConst *argv)
-{
-    return JS_NewFloat64(ctx, mavsdk_getRoll());
-}
-
-static JSValue js_mavsdk_getPitch(JSContext *ctx, JSValueConst this_val,
-                                          int argc, JSValueConst *argv)
-{
-    return JS_NewFloat64(ctx, mavsdk_getPitch());
-}
-
 static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst this_val,
                                 int argc, JSValueConst *argv)
 {
@@ -495,8 +510,6 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("doParachute", 1, js_mavsdk_doParachute ),
     JS_CFUNC_DEF("setTargetCoordinatesXYZ", 3, js_mavsdk_setTargetCoordinatesXYZ ),
     JS_CFUNC_DEF("setTargetAltitude", 1, js_mavsdk_setTargetAltitude ),
-    JS_CFUNC_DEF("getRoll", 0, js_mavsdk_getRoll ),
-    JS_CFUNC_DEF("getPitch", 0, js_mavsdk_getPitch ),
     JS_CFUNC_DEF("getYaw", 0, js_mavsdk_getYaw ),
     JS_CFUNC_DEF("getInitialLatitude", 0, js_mavsdk_getInitialLatitude ),
     JS_CFUNC_DEF("getLatitude", 0, js_mavsdk_getLatitude ),
@@ -511,7 +524,7 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
     JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
     JS_CFUNC_DEF("runPubsub", 4, js_run_pubsub ),
-    JS_CFUNC_DEF("publishCheckpoint", 1, js_publish_checkpoint ),
+    JS_CFUNC_DEF("setCheckpoint", 1, js_drone_set_checkpoint ),
     JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
 };