Remove mavsdk dependence

See merge request !8

Makefile

-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 -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_mavlink_passthrough -lmavsdk_telemetry -lopen62541
+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 -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
+LIBS=-lautopilotwrapper -lopen62541
 
 LIB_NAME := libqjswrapper.so
 
-SRCS := mavsdk_wrapper.cpp pubsub.c qjs_wrapper.c
-OBJS := mavsdk_wrapper.o pubsub.o qjs_wrapper.o
+SRCS := pubsub.c qjs_wrapper.c
+OBJS := pubsub.o qjs_wrapper.o
 
 all: $(LIB_NAME)
 

README

-This project is holding the source code for QuickJS component that will wrap the functions from MAVSDK and open62541.
+# qjs-wrapper
+
+This project is holding the source code for QuickJS component that will wrap the functions from open62541 and a drone
+control SDK (MAVSDK for example).

include/autopilot_wrapper.h

+#ifndef __AUTOPILOT_H__
+#define __AUTOPILOT_H__
+
+#ifndef DLL_PUBLIC
+#define DLL_PUBLIC __attribute__ ((visibility ("default")))
+#endif
+
+/*
+ * 0. latitude (double, degrees)
+ * 1. longitude (double, degrees)
+ * 2. absolute altitude (double, meters)
+ * 3. relative altitude (double, meters)
+ * 4. timestamp (double, milliseconds)
+*/
+#define POSITION_ARRAY_SIZE 5
+/*
+ * 0. yaw angle (float, degrees)
+ * 1. air speed (float, m/s)
+ * 2. climb rate (float, m/s)
+*/
+#define SPEED_ARRAY_SIZE 3
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include <stdint.h>
+// Connexion management functions
+DLL_PUBLIC int start(const char * ip, int port, const char * log_file, int timeout);
+DLL_PUBLIC int stop();
+DLL_PUBLIC int reboot(void);
+
+// Flight state management functions
+DLL_PUBLIC int arm(void);
+DLL_PUBLIC int takeOff(void);
+DLL_PUBLIC int takeOffAndWait(void);
+DLL_PUBLIC int triggerParachute(void);
+
+// Flight management functions
+DLL_PUBLIC void loiter(double la, double lo, float a, float radius);
+DLL_PUBLIC void setAirSpeed_async(float airspeed);
+DLL_PUBLIC void setTargetCoordinates(double la, double lo, float a);
+
+// Information functions
+DLL_PUBLIC float getAltitude(void);
+DLL_PUBLIC float getInitialAltitude(void);
+DLL_PUBLIC double getInitialLatitude(void);
+DLL_PUBLIC double getInitialLongitude(void);
+DLL_PUBLIC int64_t *getPositionArray(void);
+DLL_PUBLIC float *getSpeedArray(void);
+DLL_PUBLIC double getTakeOffAltitude(void);
+DLL_PUBLIC float getYaw(void);
+DLL_PUBLIC float getSpeed(void);
+DLL_PUBLIC float getClimbRate(void);
+DLL_PUBLIC int gpsIsOk(void);
+DLL_PUBLIC int healthAllOk(void);
+DLL_PUBLIC int isLanding(void);
+DLL_PUBLIC void updateLogAndProjection(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __AUTOPILOT_H__ */

include/dronedge.h

@@ -2,9 +2,11 @@
 #define __DRONEDGE_H__
 
 #include <open62541/server.h>
-#include "mavsdk_wrapper.h"
+#include "autopilot_wrapper.h"
 #include "pubsub.h"
 
+#define DRONE_VARIABLE_NB 3
+#define SUBSCRIBER_VARIABLE_NB 1
 struct messageNode {
   char *message;
   struct messageNode *next;
@@ -14,7 +16,7 @@ typedef struct {
   struct messageNode *tail;
 } MessageQueue;
 
-UA_Double positionArray[POSITION_ARRAY_SIZE] = { 0 };
+UA_Int64 positionArray[POSITION_ARRAY_SIZE] = { 0 };
 UA_UInt32 positionArrayDims[] = {POSITION_ARRAY_SIZE};
 
 UA_Double speedArray[SPEED_ARRAY_SIZE] = { 0 };
@@ -31,12 +33,12 @@ VariableData droneVariableArray[] = {
         .typeName = "Position Array Type",
         .description = "Position Array",
         .value = &positionArray,
-        .type = UA_TYPES_DOUBLE,
-        .builtInType = UA_NS0ID_DOUBLE,
+        .type = UA_TYPES_INT64,
+        .builtInType = UA_NS0ID_INT64,
         .valueRank = UA_VALUERANK_ONE_DIMENSION,
         .arrayDimensionsSize = 1,
         .arrayDimensions = positionArrayDims,
-        .getter.getArray = mavsdk_getPositionArray,
+        .getter.getArray = getPositionArray,
     },
     {
         .name = "speedArray",
@@ -48,7 +50,7 @@ VariableData droneVariableArray[] = {
         .valueRank = UA_VALUERANK_ONE_DIMENSION,
         .arrayDimensionsSize = 1,
         .arrayDimensions = speedArrayDims,
-        .getter.getArray = mavsdk_getSpeedArray,
+        .getter.getArray = getSpeedArray,
     },
     {
         .name = "message",
@@ -63,4 +65,31 @@ VariableData droneVariableArray[] = {
     },
 };
 
+VariableStruct droneVariables = {
+    .nbVariable = DRONE_VARIABLE_NB,
+    .variableArray = droneVariableArray,
+};
+
+VariableData subscriberVariableArray[] = {
+    {
+        .name = "message",
+        .description = "Message to send to the other drones",
+        .value = &message,
+        .type = UA_TYPES_STRING,
+        .builtInType = UA_NS0ID_STRING,
+        .valueRank = UA_VALUERANK_SCALAR,
+        .arrayDimensionsSize = 0,
+        .arrayDimensions = NULL,
+        .getter.getString = get_message,
+    },
+};
+
+VariableStruct subscriberVariables = {
+    .nbVariable = SUBSCRIBER_VARIABLE_NB,
+    .variableArray = subscriberVariableArray,
+};
+
+void Subscriber_log(void *context, UA_LogLevel level, UA_LogCategory category,
+                    const char *msg, va_list args);
+
 #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
-/*
- * 0. yaw angle (float, degrees)
- * 1. air speed (float, m/s)
- * 2. climb rate (float, m/s)
-*/
-#define SPEED_ARRAY_SIZE 3
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-#include <stdint.h>
-// Connexion management functions
-int mavsdk_start(const char * ip, int port, const char * log_file, int timeout);
-int mavsdk_stop(void);
-int mavsdk_reboot(void);
-
-// Flight state management functions
-int mavsdk_arm(void);
-int mavsdk_takeOff(void);
-int mavsdk_takeOffAndWait(void);
-int mavsdk_triggerParachute(void);
-
-// Flight management functions
-int mavsdk_loiter(float radius);
-int mavsdk_setAirspeed(float airspeed);
-int mavsdk_setTargetCoordinates(double la, double lo, float a);
-
-// Information functions
-float mavsdk_getAltitude(void);
-float mavsdk_getAltitudeRel(void);
-float mavsdk_getInitialAltitude(void);
-double mavsdk_getInitialLatitude(void);
-double mavsdk_getInitialLongitude(void);
-double mavsdk_getLatitude(void);
-double mavsdk_getLongitude(void);
-double *mavsdk_getPositionArray(void);
-float *mavsdk_getSpeedArray(void);
-double mavsdk_getTakeOffAltitude(void);
-float mavsdk_getYaw(void);
-float mavsdk_getSpeed(void);
-float mavsdk_getClimbRate(void);
-int mavsdk_healthAllOk(void);
-int mavsdk_landed(void);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __MAVSDK_H__ */

include/pubsub.h

@@ -6,7 +6,9 @@
 
 #include <quickjs/quickjs.h>
 
+#ifndef DLL_PUBLIC
 #define DLL_PUBLIC __attribute__ ((visibility ("default")))
+#endif
 
 #define countof(x) (sizeof(x) / sizeof((x)[0]))
 
@@ -23,6 +25,7 @@ typedef struct {
     UA_Double longitude;
     UA_Double altitudeAbs;
     UA_Double altitudeRel;
+    UA_Int64 timestamp;
     UA_Float  yaw;
     UA_Float  speed;
     UA_Float  climbRate;
@@ -30,6 +33,13 @@ typedef struct {
     UA_UInt32 messageId;
 } JSDroneData;
 
+typedef struct {
+    UA_Double x;
+    UA_Double y;
+    UA_Double z;
+    UA_Int64 timestamp;
+} JSPositionData;
+
 typedef struct {
     char*       name;
     char*       typeName;
@@ -47,21 +57,33 @@ typedef struct {
     } getter;
 } VariableData;
 
-int runPubsub(UA_String *transportProfile,
+typedef struct {
+    size_t nbVariable;
+    VariableData *variableArray;
+} VariableStruct;
+
+typedef struct {
+    VariableStruct variables;
+    void          (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
+} InstanceData;
+
+int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
-              VariableData *variableArray, size_t nbVariable, 
-              UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
-              void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
+              VariableStruct variables, UA_UInt32 id,
+              InstanceData *readerArray, UA_UInt32 nbReader,
+              UA_UInt32 maxVariableNb, UA_Duration interval,
               UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
               VariableData (*get_value)(UA_String identifier),
               void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
-              bool publish, UA_Boolean *running);
+              UA_Boolean *running);
 
 UA_String get_message(void);
 
 UA_UInt16 get_drone_id(UA_UInt32 nb);
 
-void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
+void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
+
+void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
 
 VariableData pubsub_get_value(UA_String identifier);
 

mavsdk_wrapper.cpp

-#include <chrono>
-#include <cmath>
-#include <cstdint>
-#include <mavsdk/mavsdk.h>
-#include <mavsdk/plugins/action/action.h>
-#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
-#include <mavsdk/plugins/telemetry/telemetry.h>
-#include <iostream>
-#include <fstream>
-#include <functional>
-#include <future>
-#include <memory>
-#include <thread>
-
-#include "mavsdk_wrapper.h"
-
-using namespace mavsdk;
-using std::chrono::seconds;
-using std::this_thread::sleep_for;
-
-#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
-#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour
-
-static std::ofstream log_file_fd;
-
-static Mavsdk _mavsdk;
-static Telemetry * telemetry;
-static Action * action;
-static MavlinkPassthrough * mavlink_passthrough;
-static std::shared_ptr<System> msystem;
-
-static auto prom = std::promise<std::shared_ptr<System>>{};
-static std::future<std::shared_ptr<System>> fut;
-
-static const float DEFAULT_RADIUS = 100;
-static const float EARTH_RADIUS = 6371000;
-
-static bool takingOff = false;
-static int mavsdk_started = 0;
-static uint64_t init_timestamp;
-static Telemetry::Position origin;
-
-// Logs functions
-
-static void log(std::string message) {
-    log_file_fd << message << std::endl;
-}
-
-static void log_error(std::string message) {
-    log(ERROR_CONSOLE_TEXT + message + NORMAL_CONSOLE_TEXT);
-}
-
-template <class Enumeration>
-static void log_error_from_result(std::string message, Enumeration result) {
-    std::ostringstream oss;
-    oss << message << ": " << result;
-    log_error(oss.str());
-}
-
-// Action functions
-
-static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
-    if(!mavsdk_started) {
-        log_error("Mavsdk not started");
-        return -1;
-    }
-
-    const Action::Result result = (action->*toDo)();
-    if(result != Action::Result::Success) {
-        log_error_from_result(failure_message, result);
-        return -1;
-    }
-    return 0;
-}
-
-static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string failure_message) {
-    const MavlinkPassthrough::Result result = mavlink_passthrough->send_command_long(command);
-    if(result != MavlinkPassthrough::Result::Success) {
-        log_error_from_result(failure_message, result);
-        return -1;
-    }
-    return 0;
-}
-
-// Connexion management functions
-
-static double toRad(double angle) {
-    return M_PI * angle / 180;
-}
-
-static double toDeg(double angle) {
-    return 180 * angle / M_PI;
-}
-
-int mavsdk_start(const char * ip, int port, const char * log_file, int timeout) {
-    char url[32];
-    sprintf(url, "udp://%s:%d", ip , port);
-    std::string connection_url(url);
-    ConnectionResult connection_result;
-    float abs_altitude;
-    log_file_fd.open(log_file);
-
-    connection_result = _mavsdk.add_any_connection(connection_url);
-    if (connection_result != ConnectionResult::Success) {
-        log_error_from_result("Connection failed", connection_result);
-        return -1;
-    }
-
-    log("Waiting to discover msystem...");
-    fut = prom.get_future();
-
-    _mavsdk.subscribe_on_new_system([]() {
-        auto msystem_tmp = _mavsdk.systems().back();
-
-        if (msystem_tmp->has_autopilot()) {
-            log("Discovered autopilot");
-
-            // Unsubscribe again as we only want to find one system.
-            _mavsdk.subscribe_on_new_system(nullptr);
-            prom.set_value(msystem_tmp);
-        }
-    });
-
-    if (fut.wait_for(seconds(timeout)) == std::future_status::timeout) {
-        log_error("No autopilot found, exiting.");
-        return -1;
-    }
-
-    msystem = fut.get();
-    telemetry = new Telemetry(msystem);
-    action = new Action(msystem);
-    mavlink_passthrough = new MavlinkPassthrough(msystem);
-
-    telemetry->subscribe_flight_mode([](Telemetry::FlightMode flight_mode) {
-        if (takingOff && flight_mode != Telemetry::FlightMode::Takeoff) {
-            log("Subscribing to raw GPS...");
-            telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
-                std::ostringstream oss;
-                oss << (gps.timestamp_us - init_timestamp) / 1000 << ";"
-                    << gps.latitude_deg << ";" << gps.longitude_deg << ";"
-                    << gps.absolute_altitude_m << ";" << telemetry->position().relative_altitude_m << ";"
-                    << telemetry->attitude_euler().yaw_deg << ";"
-                    << gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s;
-                log(oss.str());
-            });          
-        }
-
-        switch(flight_mode) {
-            case Telemetry::FlightMode::Takeoff:
-                  takingOff = true;
-                  log("Taking off...");
-                  break;
-
-            default:
-                if (takingOff) {
-                    takingOff = false;
-                    init_timestamp = telemetry->raw_gps().timestamp_us;
-                }
-        }
-    });
-
-    do {
-        log("Waiting for first telemetry");
-        sleep_for(seconds(1));
-        abs_altitude = mavsdk_getAltitude();
-    } while(isnan(abs_altitude) || abs_altitude == 0);
-    origin = telemetry->position();
-
-    log("MAVSDK started...");
-    mavsdk_started = 1;
-    log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);ground speed (m/s);climb rate (m/s)");
-    return 0;
-}
-
-int mavsdk_stop() {
-    if (!mavsdk_started)
-        return -1;
-    
-    if (!mavsdk_landed()) {
-      log_error("You must land first !");
-      return -1;
-    }
-
-    if (doAction(&Action::shutdown, "Shutdown failed"))
-        return -1;
-
-    // Delete pointers
-    delete action;
-    delete mavlink_passthrough;
-    delete telemetry;
-    log_file_fd.close();
-
-    return 0;
-}
-
-int mavsdk_reboot() {
-    return doAction(&Action::reboot, "Rebooting failed");
-}
-
-// Flight state management functions
-
-int mavsdk_arm(void) {
-    if(!mavsdk_started)
-        return -1;
-
-    while(!telemetry->health().is_home_position_ok) {
-        log("Waiting for home position to be set");
-        sleep_for(seconds(1));
-    }
-
-    log("Arming...");
-    return doAction(&Action::arm, "Arming failed");
-}
-
-int mavsdk_takeOff(void) {
-    if (doAction(&Action::takeoff, "Takeoff failed"))
-        return -1;
-
-    return 0;
-}
-
-int mavsdk_takeOffAndWait(void) {
-    if (mavsdk_takeOff() < 0)
-        return -1;
-
-    while (!takingOff)
-        sleep_for(seconds(1));
-    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
-        sleep_for(seconds(1));
-    return 0;
-}
-
-int mavsdk_triggerParachute(void) {
-    if (!mavsdk_started)
-        return -1;
-
-    log("Landing...");
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_PARACHUTE;
-    command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-    if (doMavlinkCommand(command, "Parachute release failed"))
-        return -1;
-
-    // Check if vehicule is still in air
-    while (telemetry->in_air()) {
-	      log("Vehicle is landing...");
-        sleep_for(seconds(1));
-    }
-    log("Landed!");
-
-    while (telemetry->armed())
-        sleep_for(seconds(1));
-    log("Finished...");
-    log_file_fd << std::flush;
-
-    return 0;
-}
-
-// Flight management functions
-
-static int doReposition(float la, float lo, float radius, float y) {
-    if (!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_REPOSITION;
-    command.param1 = -1; // Ground speed, -1 for default
-    command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
-    command.param3 = radius; // loiter radius
-    command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise
-    command.param5 = la;
-    command.param6 = lo;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, "Entering loiter mode failed");
-}
-
-int mavsdk_loiter(float radius) {
-    Telemetry::Position position = telemetry->position();
-    return doReposition(
-        (float)position.latitude_deg,
-        (float)position.longitude_deg,
-        radius,
-        0
-    );
-}
-
-int mavsdk_setAirspeed(float airspeed) {
-    if (!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_OVERRIDE;
-    command.param1 = 1 | 2 | 4 | 8;
-    command.param2 = 2 | 4 | 8;
-    command.param4 = airspeed;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, "Setting airspeed failed");
-}
-
-static int mavsdk_setAltitude(float altitude) {
-    if (!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_OVERRIDE;
-    command.param1 = 1 | 2 | 4 | 8;
-    command.param2 = 1 | 2 | 8;
-    command.param3 = altitude;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, "Setting altitude failed");
-}
-
-/*
- * compute the bearing angle between point 1 and point 2
- * the bearing angle is the direction to take to go from point 1 to point 2
- * values are in [-π; π] where 0 is North
-*/
-static double bearing(double lat1, double lon1, double lat2, double lon2) {
-    double lat1_rad = toRad(lat1);
-    double lat2_rad = toRad(lat2);
-    double dL = toRad(lon2 - lon1);
-    double x = cos(lat2_rad) * sin(dL);
-    double y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(dL);
-    return atan2(x, y);
-}
-
-int mavsdk_setTargetCoordinates(double la, double lo, float a) {
-    double b, laRad = toRad(la), loRad = toRad(lo), newLa, newLo;
-    int result;
-    float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
-    Telemetry::Position position = telemetry->position();
-
-    if (!mavsdk_started)
-        return -1;
-
-    b = bearing(position.latitude_deg, position.longitude_deg, la, lo);
-    newLa = asin(
-      sin(laRad) * cos(addedDistance)
-      + cos(laRad) * sin(addedDistance) * cos(b)
-    );
-    newLo = loRad + atan2(
-      sin(b) * sin(addedDistance) * cos(laRad),
-      cos(addedDistance) - sin(laRad) * sin(newLa)
-    );
-
-    result = doReposition(
-        (float)toDeg(newLa),
-        (float)toDeg(newLo),
-        DEFAULT_RADIUS,
-        0
-    );
-    result |= mavsdk_setAltitude(a);
-    return result;
-}
-
-// Information functions
-
-float mavsdk_getAltitude(void) {
-    return telemetry->position().absolute_altitude_m;
-}
-
-float mavsdk_getAltitudeRel(void) {
-    return telemetry->position().relative_altitude_m;
-}
-
-float mavsdk_getInitialAltitude(void) {
-    return origin.absolute_altitude_m;
-}
-
-double mavsdk_getInitialLatitude(void) {
-    return origin.latitude_deg;
-}
-
-double mavsdk_getInitialLongitude(void) {
-    return origin.longitude_deg;
-}
-
-double mavsdk_getLatitude(void) {
-    return telemetry->position().latitude_deg;
-}
-
-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;
-}
-
-float *mavsdk_getSpeedArray(void) {
-    Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
-    float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
-    speedArray[0] = mavsdk_getYaw();
-    speedArray[1] = metrics.airspeed_m_s;
-    speedArray[2] = metrics.climb_rate_m_s;
-    return speedArray;
-}
-
-double mavsdk_getTakeOffAltitude(void) {
-    const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
-
-    if (response.first != Action::Result::Success) {
-        log_error_from_result("Get takeoff altitude failed", response.first);
-        return -1;
-    }
-    return response.second;
-}
-
-float mavsdk_getYaw(void) {
-    return telemetry->attitude_euler().yaw_deg;
-}
-
-float mavsdk_getSpeed(void) {
-    return telemetry->fixedwing_metrics().airspeed_m_s;
-}
-
-float mavsdk_getClimbRate(void) {
-    return telemetry->fixedwing_metrics().climb_rate_m_s;
-}
-
-int mavsdk_healthAllOk(void) {
-    return telemetry->health_all_ok();
-}
-
-int mavsdk_landed(void) {
-    return !telemetry->in_air();
-}

pubsub.c

@@ -10,14 +10,14 @@ UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
           readerGroupIdent, readerIdent;
 
 UA_DataSetReaderConfig readerConfig;
-bool isPublisher;
+
+const UA_Logger *pubsubLogger;
 
 VariableData (*pubsubGetValue)(UA_String identifier);
 static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*, bool print);
 
 static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
-                                VariableData *variableArray,
-                                size_t nbVariable, int id);
+                                VariableStruct variables, int id);
 
 static UA_StatusCode
 addPubSubConnection(UA_Server *server, UA_String *transportProfile,
@@ -92,7 +92,7 @@ readVariable(UA_Server *server,
     }
 
     if(retval != UA_STATUSCODE_GOOD)
-        UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
+        UA_LOG_ERROR(pubsubLogger, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
     dataValue->hasValue = true;
     return retval;
 }
@@ -102,7 +102,7 @@ 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,
+    UA_LOG_INFO(pubsubLogger, UA_LOGCATEGORY_USERLAND,
                 "Updating variables manually is not implemented");
     return UA_STATUSCODE_BADINTERNALERROR;
 }
@@ -234,14 +234,12 @@ addReaderGroup(UA_Server *server) {
  * SubscribedDataSet and be contained within a ReaderGroup. */
 /* Add DataSetReader to the ReaderGroup */
 static UA_StatusCode
-addDataSetReader(UA_Server *server, VariableData *variableArray,
-                 size_t nbVariable, int id) {
+addDataSetReader(UA_Server *server, VariableStruct variables, int id) {
     if(server == NULL)
         return UA_STATUSCODE_BADINTERNALERROR;
 
     /* Setting up Meta data configuration in DataSetReader */
-    fillDataSetMetaData(&readerConfig.dataSetMetaData, variableArray,
-                        nbVariable, id);
+    fillDataSetMetaData(&readerConfig.dataSetMetaData, variables, id);
 
     return UA_Server_addDataSetReader(server, readerGroupIdent,
                                       &readerConfig, &readerIdent);
@@ -252,7 +250,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
                                void *monitoredItemContext, const UA_NodeId *nodeId,
                                void *nodeContext, UA_UInt32 attributeId,
                                const UA_DataValue *var) {
-    callbackUpdate(nodeId->identifier.numeric, var, !isPublisher);
+    callbackUpdate(nodeId->identifier.numeric, var, false);
 }
 
 /**
@@ -263,7 +261,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
 static UA_StatusCode
 addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
                        VariableData *variableArray, UA_UInt32 nb,
-                       UA_Duration samplingInterval,
+                       UA_UInt32 maxVariableNb, UA_Duration samplingInterval,
                        void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
     if(server == NULL)
         return UA_STATUSCODE_BADINTERNALERROR;
@@ -312,7 +310,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
 
         UA_NodeId newNode;
         retval |= UA_Server_addVariableNode(server,
-                                            UA_NODEID_NUMERIC(1, (UA_UInt32) readerConfig.dataSetMetaData.fieldsSize*nb + i + 50000),
+                                            UA_NODEID_NUMERIC(1, (UA_UInt32)maxVariableNb*nb + i + 50000),
                                             folderId,
                                             UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                                             UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
@@ -351,8 +349,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
  * and PublishedDataSetFields of Publisher */
 /* Define MetaData for TargetVariables */
 static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
-                                VariableData *variableArray,
-                                size_t nbVariable, int id) {
+                                VariableStruct variables, int id) {
     char name[12];
     UA_snprintf(name, sizeof(name) - 1, "DataSet %d", id);
 
@@ -364,34 +361,34 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
 
     /* Definition of number of fields sizeto create different
      * targetVariables of distinct datatype */
-    pMetaData->fieldsSize = nbVariable;
-    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "fieldsSize %d", (int) pMetaData->fieldsSize);
+    pMetaData->fieldsSize = variables.nbVariable;
+    UA_LOG_INFO(pubsubLogger, UA_LOGCATEGORY_USERLAND, "fieldsSize %d", (int) pMetaData->fieldsSize);
     pMetaData->fields = (UA_FieldMetaData*)UA_Array_new (pMetaData->fieldsSize,
                          &UA_TYPES[UA_TYPES_FIELDMETADATA]);
 
     for(size_t i = 0; i < pMetaData->fieldsSize; i++) {
         UA_FieldMetaData_init (&pMetaData->fields[i]);
  
-        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 = variableArray[i].valueRank;
-        pMetaData->fields[i].arrayDimensions = variableArray[i].arrayDimensions;
-        pMetaData->fields[i].arrayDimensionsSize = variableArray[i].arrayDimensionsSize;
+        UA_NodeId_copy(&UA_TYPES[variables.variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
+        pMetaData->fields[i].builtInType = variables.variableArray[i].builtInType;
+        pMetaData->fields[i].name = UA_STRING (variables.variableArray[i].name);
+        pMetaData->fields[i].valueRank = variables.variableArray[i].valueRank;
+        pMetaData->fields[i].arrayDimensions = variables.variableArray[i].arrayDimensions;
+        pMetaData->fields[i].arrayDimensionsSize = variables.variableArray[i].arrayDimensionsSize;
     }
 }
 
 static void
-setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariable) {
+setVariableType(UA_Server *server, VariableStruct variables) {
     VariableData vDetails;
     UA_VariableTypeAttributes vtAttr;
 
-    for(UA_UInt32 i = 0; i < nbVariable; i++) {
-        vDetails = variableArray[i];
+    for(UA_UInt32 i = 0; i < variables.nbVariable; i++) {
+        vDetails = variables.variableArray[i];
 
         switch(vDetails.valueRank) {
             case UA_VALUERANK_SCALAR:
-                variableArray[i].typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
+                vDetails.typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
                 break;
 
             case UA_VALUERANK_ONE_DIMENSION:
@@ -411,7 +408,7 @@ setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariabl
                 break;
 
             default:
-                UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Valuerank not handled");
+                UA_LOG_ERROR(pubsubLogger, UA_LOGCATEGORY_USERLAND, "Valuerank not handled");
                 break;
         }
     }
@@ -433,10 +430,8 @@ setServer(UA_String *transportProfile,
 }
 
 static UA_StatusCode
-subscribe(UA_Server *server,
-          VariableData *variableArray, size_t nbVariable,
-          UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
-          void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
+subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
+          UA_UInt32 nbReader, UA_UInt32 maxVariableNb, UA_Duration interval,
           UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
           void (*update)(UA_UInt32 id, const UA_DataValue*, bool print)) {
     UA_UInt16 publisherIdent;
@@ -460,12 +455,16 @@ subscribe(UA_Server *server,
         readerConfig.name = UA_STRING(readerName);
         readerConfig.publisherId.data = &publisherIdent;
 
-        retval = addDataSetReader(server, variableArray, nbVariable, publisherIdent);
+        retval = addDataSetReader(server, instanceArray[i].variables,
+                                  publisherIdent);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
 
         /* Add SubscribedVariables to the created DataSetReader */
-        retval = addSubscribedVariables(server, readerIdent, variableArray, i, interval, init_node_id);
+        retval = addSubscribedVariables(server, readerIdent,
+                                        instanceArray[i].variables.variableArray,
+                                        i, maxVariableNb, interval,
+                                        instanceArray[i].init_node_id);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
     }
@@ -473,45 +472,43 @@ subscribe(UA_Server *server,
     return retval;
 }
 
-int runPubsub(UA_String *transportProfile,
+int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
-              VariableData *variableArray, size_t nbVariable,
-              UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
-              void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
+              VariableStruct variables, UA_UInt32 id,
+              InstanceData *readerArray, UA_UInt32 nbReader,
+              UA_UInt32 maxVariableNb, UA_Duration interval,
               UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
               VariableData (*get_value)(UA_String identifier),
               void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
-              bool publish, UA_Boolean *running) {
+              UA_Boolean *running) {
     UA_Server *server;
     UA_StatusCode retval;
+    pubsubLogger = logger;
 
     server = setServer(transportProfile, networkAddressUrl, id);
-    setVariableType(server, variableArray, nbVariable);
+    setVariableType(server, variables);
 
     /* Publishing */
 
-    isPublisher = publish;
-    if (isPublisher) {
-        pubsubGetValue = get_value;
-
-        addPublishedDataSet(server, id);
-        for(UA_UInt32 i = 0; i < nbVariable; i++) {
-            retval = addDataSourceVariable(server, variableArray[i]);
-            if (retval != UA_STATUSCODE_GOOD)
-                return EXIT_FAILURE;
-            addDataSetField(server, variableArray[i]);
-        }
+    pubsubGetValue = get_value;
 
-        addWriterGroup(server, interval);
-        retval = addDataSetWriter(server);
+    addPublishedDataSet(server, id);
+    for(UA_UInt32 i = 0; i < variables.nbVariable; i++) {
+        retval = addDataSourceVariable(server, variables.variableArray[i]);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
+        addDataSetField(server, variables.variableArray[i]);
     }
 
+    addWriterGroup(server, interval);
+    retval = addDataSetWriter(server);
+    if (retval != UA_STATUSCODE_GOOD)
+        return EXIT_FAILURE;
+
     /* Subscribing */
 
-    subscribe(server, variableArray, nbVariable, id, nbReader, interval,
-              init_node_id, get_reader_id, update);
+    subscribe(server, readerArray, id, nbReader, maxVariableNb, interval,
+              get_reader_id, update);
 
     retval = UA_Server_run(server, running);
     UA_Server_delete(server);

qjs_wrapper.c

+#include <math.h>
 #include <pthread.h>
 #include "dronedge.h"
 
+# define ANSI_COLOR_RED     "\x1b[31m"
+# define ANSI_COLOR_GREEN   "\x1b[32m"
+# define ANSI_COLOR_YELLOW  "\x1b[33m"
+# define ANSI_COLOR_BLUE    "\x1b[34m"
+# define ANSI_COLOR_MAGENTA "\x1b[35m"
+# define ANSI_COLOR_CYAN    "\x1b[36m"
+# define ANSI_COLOR_RESET   "\x1b[0m"
+
 static JSClassID jsDroneClassId;
+static JSClassID jsPositionClassId;
 
 static UA_Boolean pubsubShouldRun = true;
-static UA_Boolean pubsubExited = false;
+static UA_Boolean pubsubExited = true;
 
 static UA_UInt32 nbDrone;
+static UA_UInt32 nbSubscriber;
 static JSValueConst *droneObjectIdList;
+static MessageQueue logQueue = {
+    .head = NULL,
+    .tail = NULL,
+};
 static MessageQueue messageQueue = {
     .head = NULL,
     .tail = NULL,
 };
 UA_String currentMessage;
 
+const UA_Logger Subscriber_Log = {Subscriber_log, NULL, UA_Log_Stdout_clear};
+const UA_Logger *logger;
+
 pthread_mutex_t mutex;
 pthread_cond_t threadCond;
 
+const char *logLevelNames[6] = {"trace", "debug",
+                                ANSI_COLOR_GREEN "info",
+                                ANSI_COLOR_YELLOW "warn",
+                                ANSI_COLOR_RED "error",
+                                ANSI_COLOR_MAGENTA "fatal"};
+const char *logCategoryNames[10] =
+    {"network", "channel", "session", "server", "client",
+     "userland", "securitypolicy", "eventloop", "pubsub", "discovery"};
+
+// Position class functions
+
+static void js_position_finalizer(JSRuntime *rt, JSValue val)
+{
+    JSPositionData *s = (JSPositionData *) JS_GetOpaque(val, jsPositionClassId);
+    js_free_rt(rt, s);
+}
+
+static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal,
+                               int argc, JSValueConst *argv)
+{
+    JSPositionData *s;
+    JSValue obj;
+    UA_Int64 *positionArray;
+    obj = JS_NewObjectClass(ctx, (int) jsPositionClassId);
+    if (JS_IsException(obj))
+        return obj;
+    s = (JSPositionData *) js_mallocz(ctx, sizeof(*s));
+    if (!s) {
+        JS_FreeValue(ctx, obj);
+        return JS_EXCEPTION;
+    }
+    positionArray = getPositionArray();
+    s->x = (double)positionArray[0] / 1e7;
+    s->y = (double)positionArray[1] / 1e7;
+    s->z = (UA_Double)(positionArray[3] / 1000); //relative altitude
+    s->timestamp = positionArray[4];
+    JS_SetOpaque(obj, s);
+    free(positionArray);
+    return obj;
+}
+
+static JSValue js_position_get(JSContext *ctx, JSValueConst thisVal, int magic)
+{
+    JSPositionData *s = (JSPositionData *) JS_GetOpaque2(ctx, thisVal, jsPositionClassId);
+    if (!s)
+        return JS_EXCEPTION;
+    switch(magic) {
+        case 0:
+            return JS_NewFloat64(ctx, s->x);
+        case 1:
+            return JS_NewFloat64(ctx, s->y);
+        case 2:
+            return JS_NewFloat64(ctx, s->z);
+        case 3:
+            return JS_NewInt64(ctx, s->timestamp);
+        default:
+            return JS_EXCEPTION;
+    }
+}
+
+static JSClassDef jsPositionClass = {
+    "Position",
+    .finalizer = js_position_finalizer,
+};
+
+static const JSCFunctionListEntry js_position_proto_funcs[] = {
+    JS_CGETSET_MAGIC_DEF("x", js_position_get, NULL, 0),
+    JS_CGETSET_MAGIC_DEF("y", js_position_get, NULL, 1),
+    JS_CGETSET_MAGIC_DEF("z", js_position_get, NULL, 2),
+    JS_CGETSET_MAGIC_DEF("timestamp", js_position_get, NULL, 3),
+};
+
 // Drone class functions
 
 static void js_drone_finalizer(JSRuntime *rt, JSValue val)
@@ -98,34 +188,39 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
             pthread_cond_signal(&threadCond);
             pthread_mutex_unlock(&mutex);
             return res;
+        case 9:
+            return JS_NewInt64(ctx, s->timestamp);
         default:
             return JS_EXCEPTION;
     }
 }
 
+static void addMessageToQueue(const char* msg, MessageQueue* pQueue)
+{
+    struct messageNode *newNode;
+    newNode = (struct messageNode*)malloc(sizeof(struct messageNode));
+    newNode->message = strdup(msg);
+    newNode->next = NULL;
+
+    if (pQueue->tail == NULL) {
+        pQueue->head = pQueue->tail = newNode;
+    } else {
+        pQueue->tail->next = newNode;
+        pQueue->tail = newNode;
+    }
+}
+
 static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
                                     int argc, JSValueConst *argv)
 {
-    struct messageNode *newNode;
     const char *message;
-
     message = JS_ToCString(ctx, argv[0]);
     if (strlen(message) > MAX_MESSAGE_SIZE) {
-        UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Message too long");
+        UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "Message too long");
         return JS_EXCEPTION;
     }
 
-    newNode = (struct messageNode*)malloc(sizeof(struct messageNode));
-    newNode->message = strdup(message);
-    newNode->next = NULL;
-
-    if (messageQueue.tail == NULL) {
-        messageQueue.head = messageQueue.tail = newNode;
-    } else {
-        messageQueue.tail->next = newNode;
-        messageQueue.tail = newNode;
-    }
-
+    addMessageToQueue(message, &messageQueue);
     JS_FreeCString(ctx, message);
     return JS_UNDEFINED;
 }
@@ -149,12 +244,7 @@ UA_String get_message(void)
     struct messageNode *current;
 
     current = messageQueue.head;
-    if (current == NULL) {
-        if (!UA_String_isEmpty(&currentMessage)) {
-            UA_String_clear(&currentMessage);
-            currentMessage = UA_STRING("");
-        }
-    } else {
+    if (current != NULL) {
         clear_message(currentMessage);
         currentMessage = UA_STRING_ALLOC(current->message);
         messageQueue.head = current->next == NULL ? (messageQueue.tail = NULL) : current->next;
@@ -178,6 +268,7 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
     JS_CGETSET_MAGIC_DEF("speed", js_drone_get, NULL, 6),
     JS_CGETSET_MAGIC_DEF("climbRate", js_drone_get, NULL, 7),
     JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 8),
+    JS_CGETSET_MAGIC_DEF("timestamp", js_drone_get, NULL, 9),
     JS_CFUNC_DEF("init", 1, js_drone_init),
 };
 
@@ -193,18 +284,18 @@ VariableData pubsub_get_value(UA_String identifier) {
     UA_DataType type;
     UA_Double *array;
 
-    for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) {
-        UA_String name = UA_STRING(droneVariableArray[i].name);
+    for(UA_UInt32 i = 0; i < droneVariables.nbVariable; i++) {
+        UA_String name = UA_STRING(droneVariables.variableArray[i].name);
         if(UA_String_equal(&identifier, &name)) {
-            varDetails = droneVariableArray[i];
+            varDetails = droneVariables.variableArray[i];
             switch(varDetails.valueRank) {
                 case UA_VALUERANK_SCALAR:
                     switch(varDetails.type) {
                         case UA_TYPES_STRING:
-                            *(UA_String*)varDetails.value = droneVariableArray[i].getter.getString();
+                            *(UA_String*)varDetails.value = varDetails.getter.getString();
                             break;
                         default:
-                            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
+                            UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
                             break;
                     }
                     break;
@@ -212,7 +303,7 @@ VariableData pubsub_get_value(UA_String identifier) {
                 case UA_VALUERANK_ONE_DIMENSION:
                     type = UA_TYPES[varDetails.type];
                     size_t size = varDetails.arrayDimensions[0];
-                    array = (UA_Double *) droneVariableArray[i].getter.getArray();
+                    array = (UA_Double *) varDetails.getter.getArray();
 
                     if(type.pointerFree) {
                         memcpy(varDetails.value, array, type.memSize * size);
@@ -226,14 +317,14 @@ VariableData pubsub_get_value(UA_String identifier) {
                             ptrd += type.memSize;
                         }
                         if(retval != UA_STATUSCODE_GOOD)
-                            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
+                            UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "Failed to update array");
                     }
 
                     free(array);
                     break;
 
                 default:
-                    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
+                    UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
                     break;
             }
         }
@@ -242,7 +333,7 @@ VariableData pubsub_get_value(UA_String identifier) {
     return varDetails;
 }
 
-void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
+void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
     JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
     switch(magic) {
         case 0:
@@ -255,7 +346,19 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
             s->messageId = id;
             break;
         default:
-            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
+            UA_LOG_ERROR(logger, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
+            break;
+    }
+}
+
+void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
+    JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
+    switch(magic) {
+        case 0:
+            s->messageId = id;
+            break;
+        default:
+            UA_LOG_ERROR(logger, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
             break;
     }
 }
@@ -264,21 +367,22 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
 {
     JSDroneData*  s;
     UA_String     uaStr;
-    UA_Double*    positionArray;
+    UA_Int64*    positionArray;
     UA_Float*     speedArray;
 
-    for(UA_UInt32 i = 0; i < nbDrone; i++) {
+    for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) {
         s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
         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];
+            positionArray = (UA_Int64*) var->value.data;
+            s->latitude = (double)positionArray[0] / 1e7;
+            s->longitude = (double)positionArray[1] / 1e7;
+            s->altitudeAbs = (UA_Double)(positionArray[2] / 1000);
+            s->altitudeRel = (UA_Double)(positionArray[3] / 1000);
+            s->timestamp = positionArray[4];
 
             if (print) {
-                UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
-                  "Received position of drone %d: %f° %f° %fm %fm",
+                UA_LOG_INFO(logger, UA_LOGCATEGORY_CLIENT,
+                  "Received position of drone %d: %.6f ° %.6f ° %.2f m %.2f m",
                   s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
             }
             return;
@@ -289,33 +393,70 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
             s->climbRate = speedArray[2];
 
             if (print) {
-                UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
-                  "Received speed of drone %d: %f° %fm/s %fm/s",
+                UA_LOG_INFO(logger, UA_LOGCATEGORY_CLIENT,
+                  "Received speed of drone %d: %.2f ° %.2f m/s %.2f m/s",
                   s->id, s->yaw, s->speed, s->climbRate);
             }
             return;
         } else if (s->messageId == id) {
             uaStr = *(UA_String*) var->value.data;
-
-            if (!print) {
-                pthread_mutex_lock(&mutex);
-                while(strlen(s->message) != 0)
-                    pthread_cond_wait(&threadCond, &mutex);
-            }
+            pthread_mutex_lock(&mutex);
+            while(strlen(s->message) != 0)
+                pthread_cond_wait(&threadCond, &mutex);
 
             memcpy(s->message, uaStr.data, uaStr.length);
             s->message[uaStr.length] = '\0';
 
-            if (print) {
-                UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
-                  "Received message for drone %d: %s", s->id, s->message);
-            } else {
-                pthread_mutex_unlock(&mutex);
-            }
+            pthread_mutex_unlock(&mutex);
             return;
         }
     }
-    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
+    UA_LOG_ERROR(logger, UA_LOGCATEGORY_CLIENT, "NodeId not found");
+}
+
+void Subscriber_log(void *context, UA_LogLevel level, UA_LogCategory category,
+                    const char *msg, va_list args) {
+    char log_str[MAX_MESSAGE_SIZE];
+    char formatted_msg[MAX_MESSAGE_SIZE];
+
+    /* MinLevel encoded in the context pointer */
+    UA_LogLevel minLevel = (UA_LogLevel)(uintptr_t)context;
+    if(minLevel > level)
+        return;
+
+    UA_Int64 tOffset = UA_DateTime_localTimeUtcOffset();
+    UA_DateTimeStruct dts = UA_DateTime_toStruct(UA_DateTime_now() + tOffset);
+
+    int logLevelSlot = (int)level;
+    if(logLevelSlot < 0 || logLevelSlot > 5)
+        logLevelSlot = 5; /* Set to fatal if the level is outside the range */
+
+    /* Log */
+    snprintf(log_str, MAX_MESSAGE_SIZE,
+             "[%04u-%02u-%02u %02u:%02u:%02u.%03u (UTC%+05d)] %s/%s" ANSI_COLOR_RESET "\t",
+             dts.year, dts.month, dts.day, dts.hour, dts.min, dts.sec, dts.milliSec,
+             (int)(tOffset / UA_DATETIME_SEC / 36), logLevelNames[logLevelSlot],
+             logCategoryNames[category]);
+    vsnprintf(formatted_msg, MAX_MESSAGE_SIZE, msg, args);
+    strncat(log_str, formatted_msg, MAX_MESSAGE_SIZE - strlen(log_str) - 1);
+    strcat(log_str, "\n");
+    addMessageToQueue(log_str, &logQueue);
+}
+
+static JSValue js_get_log(JSContext *ctx, JSValueConst this_val,
+                          int argc, JSValueConst *argv) {
+    JSValue res;
+    struct messageNode *current;
+
+    current = logQueue.head;
+    if (current != NULL) {
+        res = JS_NewString(ctx, current->message);
+        logQueue.head = current->next == NULL ? (logQueue.tail = NULL) : current->next;
+        delete_message_node(current);
+    } else {
+        res = JS_NewString(ctx, "");
+    }
+    return res;
 }
 
 /*
@@ -327,8 +468,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
  * arg 5 (bool): true if there will be data to publish
 */
 static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
-                             int argc, JSValueConst *argv)
-{
+                             int argc, JSValueConst *argv) {
     const char *ipv6;
     const char *port;
     const char *netIface;
@@ -336,6 +476,10 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
     char urlBuffer[44];
     UA_UInt32 id;
     UA_Duration interval;
+    bool isADrone;
+    VariableStruct variables;
+    InstanceData *instanceArray;
+    UA_UInt32 nbPeer = nbDrone + nbSubscriber;
     int res;
 
     ipv6 = JS_ToCString(ctx, argv[0]);
@@ -357,13 +501,29 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
     if (JS_ToFloat64(ctx, &interval, argv[4]))
         return JS_EXCEPTION;
 
-    res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
-                    countof(droneVariableArray), id, nbDrone, interval,
-                    init_node_id, get_drone_id, pubsub_get_value,
-                    pubsub_update_variables, JS_ToBool(ctx, argv[5]),
+    isADrone = JS_ToBool(ctx, argv[5]);
+    variables = isADrone ? droneVariables : subscriberVariables;
+    logger = isADrone ? UA_Log_Stdout : &Subscriber_Log;
+
+    instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData));
+    for(UA_UInt32 i = 0; i < nbDrone; i++) {
+      instanceArray[i].variables = droneVariables;
+      instanceArray[i].init_node_id = init_drone_node_id;
+    }
+    for(UA_UInt32 i = nbDrone; i < nbPeer; i++) {
+      instanceArray[i].variables = subscriberVariables;
+      instanceArray[i].init_node_id = init_subscriber_node_id;
+    }
+
+    pubsubExited = false;
+    res = runPubsub(logger, &transportProfile, &networkAddressUrl, variables,
+                    id, instanceArray, nbPeer,
+                    fmax(DRONE_VARIABLE_NB, SUBSCRIBER_VARIABLE_NB), interval,
+                    get_drone_id, pubsub_get_value, pubsub_update_variables,
                     &pubsubShouldRun);
     pubsubExited = true;
 
+    free(instanceArray);
     JS_FreeCString(ctx, ipv6);
     JS_FreeCString(ctx, port);
     free(notConstNetIface);
@@ -377,7 +537,11 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
     if (JS_ToUint32(ctx, &nbDrone, argv[0]))
         return JS_EXCEPTION;
 
-    droneObjectIdList = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
+    if (JS_ToUint32(ctx, &nbSubscriber, argv[1]))
+        return JS_EXCEPTION;
+
+    currentMessage = UA_STRING("");
+    droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
     return JS_NewInt32(ctx, 0);
 }
 
@@ -397,12 +561,19 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
         delete_message_node(current);
     }
     clear_message(currentMessage);
+
+    while(logQueue.head != NULL) {
+        current = logQueue.head;
+        logQueue.head = current->next;
+        delete_message_node(current);
+    }
+
     return JS_NewInt32(ctx, 0);
 }
 
 // Connexion management functions
 
-static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
+static JSValue js_start(JSContext *ctx, JSValueConst thisVal,
                                int argc, JSValueConst *argv)
 {
     const char *ip;
@@ -418,77 +589,89 @@ static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
     if (JS_ToInt32(ctx, &timeout, argv[3]))
         return JS_EXCEPTION;
 
-    res = mavsdk_start(ip, port, log_file, timeout);
+    res = start(ip, port, log_file, timeout);
     JS_FreeCString(ctx, ip);
     JS_FreeCString(ctx, log_file);
 
     return JS_NewInt32(ctx, res);
 }
 
-static JSValue js_mavsdk_stop(JSContext *ctx, JSValueConst thisVal,
+static JSValue js_stop(JSContext *ctx, JSValueConst thisVal,
                               int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_stop());
+    return JS_NewInt32(ctx, stop());
 }
 
-static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst thisVal,
-                                int argc, JSValueConst *argv)
+static JSValue js_reboot(JSContext *ctx, JSValueConst thisVal,
+                         int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_reboot());
+    return JS_NewInt32(ctx, reboot());
 }
 
 // Flight state management functions
 
-static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
+static JSValue js_arm(JSContext *ctx, JSValueConst this_val,
                              int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_arm());
+    return JS_NewInt32(ctx, arm());
 }
 
-static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst thisVal,
-                                 int argc, JSValueConst *argv)
+static JSValue js_takeOff(JSContext *ctx, JSValueConst thisVal,
+                          int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_takeOff());
+    return JS_NewInt32(ctx, takeOff());
 }
 
-static JSValue js_mavsdk_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
-                                        int argc, JSValueConst *argv)
+static JSValue js_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
+                                 int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_takeOffAndWait());
+    return JS_NewInt32(ctx, takeOffAndWait());
 }
 
-static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst thisVal,
-                                          int argc, JSValueConst *argv)
+static JSValue js_triggerParachute(JSContext *ctx, JSValueConst thisVal,
+                                   int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_triggerParachute());
+    return JS_NewInt32(ctx, triggerParachute());
 }
 
 // Flight management functions
 
-static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst thisVal,
-                                int argc, JSValueConst *argv)
+static JSValue js_loiter(JSContext *ctx, JSValueConst thisVal,
+                         int argc, JSValueConst *argv)
 {
+    double la_arg_double;
+    double lo_arg_double;
+    double a_arg_double;
     double radius;
-    if (JS_ToFloat64(ctx, &radius, argv[0]))
+
+    if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
+        return JS_EXCEPTION;
+    if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
+        return JS_EXCEPTION;
+    if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
+        return JS_EXCEPTION;
+    if (JS_ToFloat64(ctx, &radius, argv[3]))
         return JS_EXCEPTION;
 
-    return JS_NewInt32(ctx, mavsdk_loiter((float)radius));
+    loiter(la_arg_double, lo_arg_double, (float)a_arg_double, (float)radius);
+    return JS_NewInt32(ctx, 0);
 }
 
-static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst thisVal,
-                                     int argc, JSValueConst *argv)
+static JSValue js_setAirSpeed(JSContext *ctx, JSValueConst thisVal,
+                              int argc, JSValueConst *argv)
 {
     double altitude;
 
     if (JS_ToFloat64(ctx, &altitude, argv[0]))
         return JS_EXCEPTION;
 
-    return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
+    setAirSpeed_async((float)altitude);
+    return JS_NewInt32(ctx, 0);
 }
 
-static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
-                                              JSValueConst thisVal,
-                                              int argc, JSValueConst *argv)
+static JSValue js_setTargetCoordinates(JSContext *ctx,
+                                       JSValueConst thisVal,
+                                       int argc, JSValueConst *argv)
 {
     double la_arg_double;
     double lo_arg_double;
@@ -501,154 +684,155 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
     if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
         return JS_EXCEPTION;
 
-    return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
-                                                        lo_arg_double,
-                                                        (float)a_arg_double));
+    setTargetCoordinates(la_arg_double, lo_arg_double, (float)a_arg_double);
+    return JS_NewInt32(ctx, 0);
 }
 
 // Information functions
 
-static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst thisVal,
+static JSValue js_getAltitude(JSContext *ctx, JSValueConst thisVal,
                                      int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getAltitude());
-}
-
-static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst thisVal,
-                                        int argc, JSValueConst *argv)
-{
-    return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
+    return JS_NewFloat64(ctx, getAltitude());
 }
 
-static JSValue js_mavsdk_getInitialAltitude(JSContext *ctx,
-                                            JSValueConst thisVal,
-                                            int argc, JSValueConst *argv)
+static JSValue js_getInitialAltitude(JSContext *ctx,
+                                     JSValueConst thisVal,
+                                     int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getInitialAltitude());
+    return JS_NewFloat64(ctx, getInitialAltitude());
 }
 
-static JSValue js_mavsdk_getInitialLatitude(JSContext *ctx,
-                                            JSValueConst thisVal,
-                                            int argc, JSValueConst *argv)
+static JSValue js_getInitialLatitude(JSContext *ctx,
+                                     JSValueConst thisVal,
+                                     int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getInitialLatitude());
+    return JS_NewFloat64(ctx, getInitialLatitude());
 }
 
-static JSValue js_mavsdk_getInitialLongitude(JSContext *ctx,
-                                             JSValueConst thisVal,
-                                             int argc, JSValueConst *argv)
+static JSValue js_getInitialLongitude(JSContext *ctx,
+                                      JSValueConst thisVal,
+                                      int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getInitialLongitude());
+    return JS_NewFloat64(ctx, getInitialLongitude());
 }
 
-static JSValue js_mavsdk_getLatitude(JSContext *ctx, JSValueConst thisVal,
+static JSValue js_getTakeOffAltitude(JSContext *ctx,
+                                     JSValueConst thisVal,
                                      int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getLatitude());
+    return JS_NewFloat64(ctx, getTakeOffAltitude());
 }
 
-static JSValue js_mavsdk_getLongitude(JSContext *ctx, JSValueConst thisVal,
-                                      int argc, JSValueConst *argv)
+static JSValue js_getYaw(JSContext *ctx, JSValueConst thisVal,
+                                int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getLongitude());
+    return JS_NewFloat64(ctx, getYaw());
 }
 
-static JSValue js_mavsdk_getTakeOffAltitude(JSContext *ctx,
-                                            JSValueConst thisVal,
-                                            int argc, JSValueConst *argv)
+static JSValue js_getSpeed(JSContext *ctx, JSValueConst thisVal,
+                                  int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getTakeOffAltitude());
+    return JS_NewFloat64(ctx, getSpeed());
 }
 
-static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst thisVal,
-                                int argc, JSValueConst *argv)
+static JSValue js_getClimbRate(JSContext *ctx, JSValueConst thisVal,
+                               int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getYaw());
+    return JS_NewFloat64(ctx, getClimbRate());
 }
 
-static JSValue js_mavsdk_getSpeed(JSContext *ctx, JSValueConst thisVal,
-                                  int argc, JSValueConst *argv)
+static JSValue js_gpsIsOk(JSContext *ctx, JSValueConst this_val,
+                              int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getSpeed());
+    return JS_NewBool(ctx, gpsIsOk());
 }
 
-static JSValue js_mavsdk_getClimbRate(JSContext *ctx, JSValueConst thisVal,
-                                   int argc, JSValueConst *argv)
+static JSValue js_healthAllOk(JSContext *ctx, JSValueConst this_val,
+                              int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getClimbRate());
+    return JS_NewBool(ctx, healthAllOk());
 }
 
-static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
-                                     int argc, JSValueConst *argv)
+static JSValue js_isLanding(JSContext *ctx, JSValueConst this_val,
+                            int argc, JSValueConst *argv)
 {
-    return JS_NewBool(ctx, mavsdk_healthAllOk());
+    return JS_NewBool(ctx, isLanding());
 }
 
-static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
-                                int argc, JSValueConst *argv)
+static JSValue js_updateLogAndProjection(JSContext *ctx, JSValueConst this_val,
+                                         int argc, JSValueConst *argv)
 {
-    return JS_NewBool(ctx, mavsdk_landed());
+    updateLogAndProjection();
+    return JS_NewInt32(ctx, 0);
 }
 
-static const JSCFunctionListEntry js_mavsdk_funcs[] = {
+static const JSCFunctionListEntry js_funcs[] = {
     JS_CFUNC_DEF("setMessage", 1, js_drone_set_message ),
     JS_CFUNC_DEF("runPubsub", 6, js_run_pubsub ),
     JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
-    JS_CFUNC_DEF("start", 4, js_mavsdk_start ),
-    JS_CFUNC_DEF("stop", 0, js_mavsdk_stop ),
-    JS_CFUNC_DEF("reboot", 0, js_mavsdk_reboot ),
-    JS_CFUNC_DEF("arm", 0, js_mavsdk_arm ),
-    JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
-    JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
-    JS_CFUNC_DEF("triggerParachute", 0, js_mavsdk_triggerParachute ),
-    JS_CFUNC_DEF("loiter", 1, js_mavsdk_loiter ),
-    JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
-    JS_CFUNC_DEF("setTargetCoordinates", 3, js_mavsdk_setTargetCoordinates ),
-    JS_CFUNC_DEF("getAltitude", 0, js_mavsdk_getAltitude ),
-    JS_CFUNC_DEF("getAltitudeRel", 0, js_mavsdk_getAltitudeRel ),
-    JS_CFUNC_DEF("getInitialAltitude", 0, js_mavsdk_getInitialAltitude ),
-    JS_CFUNC_DEF("getInitialLatitude", 0, js_mavsdk_getInitialLatitude ),
-    JS_CFUNC_DEF("getInitialLongitude", 0, js_mavsdk_getInitialLongitude ),
-    JS_CFUNC_DEF("getLatitude", 0, js_mavsdk_getLatitude ),
-    JS_CFUNC_DEF("getLongitude", 0, js_mavsdk_getLongitude ),
-    JS_CFUNC_DEF("getTakeOffAltitude", 0, js_mavsdk_getTakeOffAltitude ),
-    JS_CFUNC_DEF("getYaw", 0, js_mavsdk_getYaw ),
-    JS_CFUNC_DEF("getAirspeed", 0, js_mavsdk_getSpeed ),
-    JS_CFUNC_DEF("getClimbRate", 0, js_mavsdk_getClimbRate ),
-    JS_CFUNC_DEF("healthAllOk", 0, js_mavsdk_healthAllOk ),
-    JS_CFUNC_DEF("landed", 0, js_mavsdk_landed ),
-    JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
+    JS_CFUNC_DEF("start", 4, js_start ),
+    JS_CFUNC_DEF("stop", 0, js_stop ),
+    JS_CFUNC_DEF("reboot", 0, js_reboot ),
+    JS_CFUNC_DEF("arm", 0, js_arm ),
+    JS_CFUNC_DEF("takeOff", 0, js_takeOff ),
+    JS_CFUNC_DEF("takeOffAndWait", 0, js_takeOffAndWait ),
+    JS_CFUNC_DEF("triggerParachute", 0, js_triggerParachute ),
+    JS_CFUNC_DEF("loiter", 4, js_loiter ),
+    JS_CFUNC_DEF("setAirSpeed", 1, js_setAirSpeed ),
+    JS_CFUNC_DEF("setTargetCoordinates", 3, js_setTargetCoordinates ),
+    JS_CFUNC_DEF("getPosition", 0, js_new_position ),
+    JS_CFUNC_DEF("getAltitude", 0, js_getAltitude ),
+    JS_CFUNC_DEF("getInitialAltitude", 0, js_getInitialAltitude ),
+    JS_CFUNC_DEF("getInitialLatitude", 0, js_getInitialLatitude ),
+    JS_CFUNC_DEF("getInitialLongitude", 0, js_getInitialLongitude ),
+    JS_CFUNC_DEF("getTakeOffAltitude", 0, js_getTakeOffAltitude ),
+    JS_CFUNC_DEF("getYaw", 0, js_getYaw ),
+    JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ),
+    JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ),
+    JS_CFUNC_DEF("gpsIsOk", 0, js_gpsIsOk ),
+    JS_CFUNC_DEF("isLanding", 0, js_isLanding ),
+    JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ),
+    JS_CFUNC_DEF("initPubsub", 2, js_init_pubsub ),
+    JS_CFUNC_DEF("getLog", 0, js_get_log ),
+    JS_CFUNC_DEF("updateLogAndProjection", 0, js_updateLogAndProjection ),
 };
 
-static int js_mavsdk_init(JSContext *ctx, JSModuleDef *m)
+static int js_init(JSContext *ctx, JSModuleDef *m)
 {
-    JSValue droneProto, droneClass;
+    JSValue droneProto, droneClass, positionProto;
 
     JS_NewClassID(&jsDroneClassId);
+    JS_NewClassID(&jsPositionClassId);
     JS_NewClass(JS_GetRuntime(ctx), jsDroneClassId, &jsDroneClass);
+    JS_NewClass(JS_GetRuntime(ctx), jsPositionClassId, &jsPositionClass);
 
     droneProto = JS_NewObject(ctx);
     JS_SetPropertyFunctionList(ctx, droneProto, js_drone_proto_funcs,
                                countof(js_drone_proto_funcs));
+    positionProto = JS_NewObject(ctx);
+    JS_SetPropertyFunctionList(ctx, positionProto, js_position_proto_funcs,
+                               countof(js_position_proto_funcs));
 
     droneClass = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
                                    JS_CFUNC_constructor, 0);
     JS_SetConstructor(ctx, droneClass, droneProto);
     JS_SetClassProto(ctx, jsDroneClassId, droneProto);
+    JS_SetClassProto(ctx, jsPositionClassId, positionProto);
 
     JS_SetModuleExport(ctx, m, "Drone", droneClass);
 
-    return JS_SetModuleExportList(ctx, m, js_mavsdk_funcs,
-                                  countof(js_mavsdk_funcs));
+    return JS_SetModuleExportList(ctx, m, js_funcs,
+                                  countof(js_funcs));
 }
 
 JSModuleDef *js_init_module(JSContext *ctx, const char *module_name)
 {
     JSModuleDef *m;
-    m = JS_NewCModule(ctx, module_name, js_mavsdk_init);
+    m = JS_NewCModule(ctx, module_name, js_init);
     if (!m)
         return NULL;
-    JS_AddModuleExportList(ctx, m, js_mavsdk_funcs, countof(js_mavsdk_funcs));
+    JS_AddModuleExportList(ctx, m, js_funcs, countof(js_funcs));
     JS_AddModuleExport(ctx, m, "Drone");
     return m;
 }