Comply with simulator API

See merge request !3

Makefile

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

include/dronedge.h

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

include/mavsdk_wrapper.h

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

include/pubsub.h

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

mavsdk_wrapper.cpp

@@ -3,6 +3,7 @@
 #include <cstdint>
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/manual_control/manual_control.h>
 #include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 #include <iostream>
@@ -26,24 +27,22 @@ static std::ofstream log_file_fd;
 static Mavsdk _mavsdk;
 static Telemetry * telemetry;
 static Action * action;
+static ManualControl * manual_control;
 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 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 xy_ratio;
-
 static const double EARTH_RADIUS = 6371000.F;
 
+static bool autocontinue = false;
+static double previousBearing;
+static double xy_ratio;
+static double target_latitude;
+static double target_longitude;
 static int mavsdk_started = 0;
+static Telemetry::Position origin;
 
 // Logs functions
 
@@ -89,9 +88,14 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string
 
 // Connexion management functions
 
+static double toRad(double angle) {
+    return M_PI * angle / 180;
+}
+
 int mavsdk_start(const char * url, const char * log_file, int timeout) {
     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);
@@ -123,49 +127,31 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
     msystem = fut.get();
     telemetry = new Telemetry(msystem);
     action = new Action(msystem);
+    manual_control = new ManualControl(msystem);
     mavlink_passthrough = new MavlinkPassthrough(msystem);
 
-    log("Subscribing to flight mode...");
-    // Subscribe to receive updates on flight mode. You can find out whether FollowMe is active.
-    telemetry->subscribe_flight_mode([](Telemetry::FlightMode _flight_mode) {
-        if(flight_mode != _flight_mode) {
-            flight_mode = _flight_mode;
-        }
-    });
-
-    log("Subscribing to Euler angle...");
-    telemetry->subscribe_attitude_euler([](Telemetry::EulerAngle euler_angle) {
-        angle = euler_angle;
-    });
-
-    log("Subscribing to Fixedwing Metrics...");
-    telemetry->subscribe_fixedwing_metrics([](Telemetry::FixedwingMetrics m) {
-        metric = m;
-    });
-
-    log("Subscribing to 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();
-
+    log("Subscribing to raw GPS...");
+    telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
         std::ostringstream oss;
-	      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();
+        Telemetry::EulerAngle euler_angle = telemetry->attitude_euler();
+        Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
+        Telemetry::Position position = telemetry->position();
+
+        oss << gps.timestamp_us << "; "
+            << gps.latitude_deg << "; " << gps.longitude_deg << "; "
+            << gps.absolute_altitude_m << "; " << position.relative_altitude_m << "; "
+            << euler_angle.pitch_deg << "; " << euler_angle.roll_deg << "; " << euler_angle.yaw_deg << "; "
+            << metrics.airspeed_m_s << "; " << metrics.throttle_percentage << "; " << metrics.climb_rate_m_s;
         log(oss.str());
     });
 
-    while(isnan(position.absolute_altitude_m)) {
+    do {
         log("Waiting for first telemetry");
         sleep_for(seconds(1));
-    }
-    origin = position;
-    xy_ratio = std::cos((M_PI * origin.latitude_deg) / 180.F);
+        abs_altitude = mavsdk_getAltitude();
+    } while(isnan(abs_altitude) || abs_altitude == 0);
+    origin = telemetry->position();
+    xy_ratio = std::cos(toRad(origin.latitude_deg));
 
     log("MAVSDK started...");
     mavsdk_started = 1;
@@ -174,20 +160,20 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
 }
 
 int mavsdk_stop() {
-    if(!mavsdk_started)
+    if (!mavsdk_started)
         return -1;
     
-    if(!mavsdk_landed()) {
+    if (!mavsdk_landed()) {
       log_error("You must land first !");
       return -1;
     }
 
-    if(doAction(&Action::shutdown, "Shutdown failed")) {
+    if (doAction(&Action::shutdown, "Shutdown failed"))
         return -1;
-    }
 
     // Delete pointers
     delete action;
+    delete manual_control;
     delete mavlink_passthrough;
     delete telemetry;
     log_file_fd.close();
@@ -214,101 +200,87 @@ int mavsdk_arm(void) {
     return doAction(&Action::arm, "Arming failed");
 }
 
-int mavsdk_doParachute(float param) {
-    if(!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_PARACHUTE;
-    command.param1 = param; //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();
-    return doMavlinkCommand(command, "Parachute failed");
-}
-
 int mavsdk_land(void) {
     log("Landing...");
-    if(doAction(&Action::terminate, "Land failed")) {
+    if (doAction(&Action::terminate, "Land failed"))
         return -1;
-    }
 
     // Check if vehicle is still in air
-    while(telemetry->in_air()) {
+    while (telemetry->in_air()) {
         log("Vehicle is landing...");
         sleep_for(seconds(1));
     }
     log("Landed!");
 
-    while(telemetry->armed()) {
+    while (telemetry->armed())
         sleep_for(seconds(1));
-    }
     log("Finished...");
 
     return 0;
 }
 
-int mavsdk_loiter() {
-    if(!mavsdk_started)
-        return -1;
-
-    return mavsdk_doReposition((float)mavsdk_getLatitude(),
-                               (float)mavsdk_getLongitude(),
-                               mavsdk_getAltitude(), 0);
-}
-
 int mavsdk_takeOff(void) {
-    if(doAction(&Action::takeoff, "Takeoff failed")) {
+    if (doAction(&Action::takeoff, "Takeoff failed"))
         return -1;
-    }
 
-    while(flight_mode != Telemetry::FlightMode::Takeoff) {
+    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
-    }
 
     log("Taking off...");
     return 0;
 }
 
 int mavsdk_takeOffAndWait(void) {
-    if(mavsdk_takeOff() < 0) {
+    if (mavsdk_takeOff() < 0)
         return -1;
-    }
 
-    while(flight_mode == Telemetry::FlightMode::Takeoff) {
+    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
-    }
     return 0;
 }
 
+int mavsdk_triggerParachute(void) {
+    if (!mavsdk_started)
+        return -1;
+
+    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();
+    return doMavlinkCommand(command, "Parachute release failed");
+}
+
 // Flight management functions
 
-int mavsdk_doReposition(float la, float lo, float a, float y) {
-    if(!mavsdk_started)
+int mavsdk_loiter(float radius) {
+    if (!mavsdk_started)
         return -1;
+    Telemetry::Position position = telemetry->position();
 
     MavlinkPassthrough::CommandLong command;
     command.command = MAV_CMD_DO_REPOSITION;
     command.param1 = -1; // Ground speed, -1 for default
-    command.param2 = 1; // Bitmask of option flags (https://mavlink.io/en/messages/common.html#MAV_DO_REPOSITION_FLAGS)
-    command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise
-    command.param5 = la;
-    command.param6 = lo;
-    command.param7 = a;
+    command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
+    command.param3 = radius; // loiter radius
+    command.param5 = (float)position.latitude_deg;
+    command.param6 = (float)position.longitude_deg;
+    command.param7 = position.absolute_altitude_m; //altitude is ignored, use altitude override package
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
 
-    return doMavlinkCommand(command, "Reposition failed");
+    return doMavlinkCommand(command, "Entering loiter mode failed");
 }
 
 int mavsdk_setAirspeed(float airspeed) {
-    if(!mavsdk_started)
+    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.param3 = airspeed;
+    command.param4 = airspeed;
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
 
@@ -316,7 +288,7 @@ int mavsdk_setAirspeed(float airspeed) {
 }
 
 int mavsdk_setAltitude(float altitude) {
-    if(!mavsdk_started)
+    if (!mavsdk_started)
         return -1;
 
     MavlinkPassthrough::CommandLong command;
@@ -330,56 +302,79 @@ int mavsdk_setAltitude(float altitude) {
     return doMavlinkCommand(command, "Setting altitude failed");
 }
 
-int mavsdk_setTargetAltitude(float a) {
-    if(!mavsdk_started)
-        return -1;
-
-    log("Going to altitude " + std::to_string(a) + "m");
-    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);
+static int startAltitudeControl(void) {
+    const ManualControl::Result result = manual_control->start_altitude_control();
+    if (result != ManualControl::Result::Success) {
+        log_error_from_result("Set manual control failed!", result);
         return -1;
     }
     return 0;
 }
 
-int mavsdk_setTargetCoordinates(double la, double lo, float a, float y) {
-    if(!mavsdk_started)
+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) * 180 / M_PI;
+}
+
+int mavsdk_setTargetCoordinates(double la, double lo, float a) {
+    int result = 0;
+    Telemetry::Position position = telemetry->position();
+
+    if (!mavsdk_started)
         return -1;
 
-    std::cout << "Going to location (" << la << " , " << lo << ") "
-              << a <<  " m " << std::endl;
-    const Action::Result result = action->goto_location(la, lo, a, y);
-    if (result != Action::Result::Success) {
-        log_error_from_result("Goto location failed", result);
+    target_latitude = la;
+    target_longitude = lo;
+    previousBearing = bearing(position.latitude_deg, position.longitude_deg,
+                              la, lo);
+    autocontinue = false;
+
+    if (!mavsdk_isInManualMode()) {
+        result = mavsdk_setManualControlInput();
+        result |= startAltitudeControl();
+    }
+    result |= mavsdk_setAltitude(a);
+    return result;
+}
+
+static int setManualControlInput(float x, float y, float z, float r) {
+    const ManualControl::Result result = manual_control->set_manual_control_input(x, y, z, r);
+    if(result != ManualControl::Result::Success) {
+        log_error_from_result("Set manual control input failed!", result);
         return -1;
     }
     return 0;
 }
 
-int mavsdk_setTargetCoordinatesXYZ(double x, double y, float z) {
-    double la, lo;
-
-    la = ((origin.latitude_deg + y / EARTH_RADIUS) * 180.F) / M_PI;
-    lo = ((origin.longitude_deg + x / (EARTH_RADIUS * xy_ratio)) * 180.F) / M_PI;
+int mavsdk_setManualControlInput(void) {
+    if (autocontinue)
+        return setManualControlInput(0, 0, 0, 0);
 
-    return mavsdk_setTargetCoordinates(la, lo, z, 0);
-}
+    Telemetry::Position position = telemetry->position();
+    double b = bearing(position.latitude_deg, position.longitude_deg,
+                       target_latitude, target_longitude);
+    if (abs(b - previousBearing) > 160) {
+        autocontinue = true;
+    } else {
+        previousBearing = b;
+    }
 
-int mavsdk_setTargetLatLong(double la, double lo) {
-    return mavsdk_setTargetCoordinates(la, lo, mavsdk_getAltitude(), 0);
+    float angle_diff = (float)b - mavsdk_getYaw();
+    return setManualControlInput(0, (angle_diff > 0 ? 1 : -1) * std::min(abs(angle_diff), 30.0f) / 30, 0, 0);
 }
 
 // Information functions
 
 float mavsdk_getAltitude(void) {
-    return position.absolute_altitude_m;
+    return telemetry->position().absolute_altitude_m;
 }
 
 float mavsdk_getAltitudeRel(void) {
-    return position.relative_altitude_m;
+    return telemetry->position().relative_altitude_m;
 }
 
 float mavsdk_getInitialAltitude(void) {
@@ -395,55 +390,35 @@ double mavsdk_getInitialLongitude(void) {
 }
 
 double mavsdk_getLatitude(void) {
-    return position.latitude_deg;
+    return telemetry->position().latitude_deg;
 }
 
 double mavsdk_getLongitude(void) {
-    return position.longitude_deg;
-}
-
-uint64_t mavsdk_getTimestamp(void) {
-    return timestamp_us;
+    return telemetry->position().longitude_deg;
 }
 
 double mavsdk_getTakeOffAltitude(void) {
     const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
 
-    if(response.first != Action::Result::Success) {
+    if (response.first != Action::Result::Success) {
         log_error_from_result("Get takeoff altitude failed", response.first);
         return -1;
     }
     return response.second;
 }
 
-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;
+    return telemetry->attitude_euler().yaw_deg;
 }
 
 int mavsdk_healthAllOk(void) {
     return telemetry->health_all_ok();
 }
 
+bool mavsdk_isInManualMode(void) {
+    return telemetry->flight_mode() == Telemetry::FlightMode::Altctl;
+}
+
 int mavsdk_landed(void) {
     return !telemetry->in_air();
 }

pubsub.c

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

qjs_wrapper.c

+#include <pthread.h>
 #include "dronedge.h"
 
-static JSClassID js_drone_class_id;
+static JSClassID jsDroneClassId;
 
-static UA_Boolean pubsub_running = true;
+static UA_Boolean pubsubRunning = true;
 
 static UA_UInt32 nbDrone;
-static JSValueConst *drone_object_id_list;
+static JSValueConst *droneObjectIdList;
+static MessageQueue messageQueue = {
+    .head = NULL,
+    .tail = NULL,
+};
+UA_String currentMessage;
+
+pthread_mutex_t mutex;
+pthread_cond_t threadCond;
 
-static UA_UInt32 lastCheckpoint = 0;
+// Drone class functions
 
 static void js_drone_finalizer(JSRuntime *rt, JSValue val)
 {
-    JSDroneData *s = (JSDroneData *) JS_GetOpaque(val, js_drone_class_id);
+    JSDroneData *s = (JSDroneData *) JS_GetOpaque(val, jsDroneClassId);
     js_free_rt(rt, s);
 }
 
-static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target, 
+static JSValue js_drone_ctor(JSContext *ctx, JSValueConst newTarget, 
                              int argc, JSValueConst *argv)
 {
     JSDroneData *s;
@@ -30,10 +39,10 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target,
         goto fail;
     s->id = (uint16_t)uint32;
 
-    proto = JS_GetPropertyStr(ctx, new_target, "prototype");
+    proto = JS_GetPropertyStr(ctx, newTarget, "prototype");
     if (JS_IsException(proto))
         goto fail;
-    obj = JS_NewObjectProtoClass(ctx, proto, js_drone_class_id);
+    obj = JS_NewObjectProtoClass(ctx, proto, jsDroneClassId);
     JS_FreeValue(ctx, proto);
     if (JS_IsException(obj))
         goto fail;
@@ -45,22 +54,23 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target,
     return JS_EXCEPTION;
 }
 
-static JSValue js_drone_init(JSContext *ctx, JSValueConst this_val,
+static JSValue js_drone_init(JSContext *ctx, JSValueConst thisVal,
                              int argc, JSValueConst *argv)
 {
     int nb;
-    JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, this_val, js_drone_class_id);
+    JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, thisVal, jsDroneClassId);
     if (!s)
         return JS_EXCEPTION;
     if (JS_ToInt32(ctx, &nb, argv[0]))
         return JS_EXCEPTION;
-    drone_object_id_list[nb] = this_val;
+    droneObjectIdList[nb] = thisVal;
     return JS_UNDEFINED;
 }
 
-static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
+static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
 {
-    JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, this_val, js_drone_class_id);
+    JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, thisVal, jsDroneClassId);
+    JSValue res;
     if (!s)
         return JS_EXCEPTION;
     switch(magic) {
@@ -75,28 +85,78 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
         case 4:
             return JS_NewFloat64(ctx, s->altitudeRel);
         case 5:
-            return JS_NewUint32(ctx, s->lastCheckpoint);
+            pthread_mutex_lock(&mutex);
+            res = JS_NewString(ctx, s->message);
+            strcpy(s->message, "");
+            pthread_cond_signal(&threadCond);
+            pthread_mutex_unlock(&mutex);
+            return res;
         default:
             return JS_EXCEPTION;
     }
 }
 
-static JSValue js_drone_set_checkpoint(JSContext *ctx, JSValueConst this_val,
+static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
                                     int argc, JSValueConst *argv)
 {
-    uint32_t uint32;
-    if (JS_ToUint32(ctx, &uint32, argv[0]))
+    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");
         return JS_EXCEPTION;
-    lastCheckpoint = (uint16_t)uint32;
+    }
+
+    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;
+    }
+
+    JS_FreeCString(ctx, message);
     return JS_UNDEFINED;
 }
 
-UA_UInt32 getLastCheckpoint(void)
+static void delete_message_node(struct messageNode *node) {
+    free(node->message);
+    free(node);
+}
+
+static UA_Boolean UA_String_isEmpty(const UA_String *s) {
+    return (s->length == 0 || s->data == NULL);
+}
+
+static void clear_message(UA_String message) {
+    if (!UA_String_isEmpty(&message))
+        UA_String_clear(&message); 
+}
+
+UA_String get_message(void)
 {
-    return lastCheckpoint;
+    struct messageNode *current;
+
+    current = messageQueue.head;
+    if (current == NULL) {
+        if (!UA_String_isEmpty(&currentMessage)) {
+            UA_String_clear(&currentMessage);
+            currentMessage = UA_STRING("");
+        }
+    } else {
+        clear_message(currentMessage);
+        currentMessage = UA_STRING_ALLOC(current->message);
+        messageQueue.head = current->next == NULL ? (messageQueue.tail = NULL) : current->next;
+        delete_message_node(current);
+    }
+    return currentMessage;
 }
 
-static JSClassDef js_drone_class = {
+static JSClassDef jsDroneClass = {
     "Drone",
     .finalizer = js_drone_finalizer,
 };
@@ -107,20 +167,17 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
     JS_CGETSET_MAGIC_DEF("longitude", js_drone_get, NULL, 2),
     JS_CGETSET_MAGIC_DEF("altitudeAbs", js_drone_get, NULL, 3),
     JS_CGETSET_MAGIC_DEF("altitudeRel", js_drone_get, NULL, 4),
-    JS_CGETSET_MAGIC_DEF("lastCheckpoint", js_drone_get, NULL, 5),
+    JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 5),
     JS_CFUNC_DEF("init", 1, js_drone_init),
 };
 
+// pubsub functions
+
 UA_UInt16 get_drone_id(UA_UInt32 nb) {
-    JSDroneData *s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
+    JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
     return s->id;
 }
 
-void stop_pubsub(void) {
-    pubsub_running = false;
-    free(drone_object_id_list);
-}
-
 VariableData pubsub_get_value(UA_String identifier) {
     VariableData var_details;
     UA_String name;
@@ -143,6 +200,9 @@ VariableData pubsub_get_value(UA_String identifier) {
                 case UA_TYPES_DATETIME:
                     *(UA_DateTime*)var_details.value = droneVariableArray[i].getter.getTimestamp();
                     break;
+                case UA_TYPES_STRING:
+                    *(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
+                    break;
                 default:
                     UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
                     break;
@@ -155,7 +215,7 @@ VariableData pubsub_get_value(UA_String identifier) {
 }
 
 void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
-    JSDroneData *s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
+    JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
     switch(magic) {
         case 0:
             s->latitudeId = id;
@@ -170,7 +230,7 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
             s->altitudeRelId = id;
             break;
         case 4:
-            s->lastCheckpointId = id;
+            s->messageId = id;
             break;
         default:
             break;
@@ -180,8 +240,9 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
 void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
 {
     JSDroneData *s;
+    UA_String uaStr;;
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
-        s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[i], js_drone_class_id);
+        s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
         if (s->latitudeId == id) {
             s->latitude = *(UA_Double*) var->value.data;
             return;
@@ -194,8 +255,14 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
         } else if (s->altitudeRelId == id) {
             s->altitudeRel = *(UA_Float*) var->value.data;
             return;
-        } else if (s->lastCheckpointId == id) {
-            s->lastCheckpoint = *(UA_UInt32*) var->value.data;
+        } else if (s->messageId == id) {
+            uaStr = *(UA_String*) var->value.data;
+            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';
+            pthread_mutex_unlock(&mutex);
             return;
         }
     }
@@ -205,8 +272,9 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
 void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
 {
     JSDroneData *s;
+    UA_String uaStr;
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
-        s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[i], js_drone_class_id);
+        s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
         if (s->latitudeId == id) {
             s->latitude = *(UA_Double*) var->value.data;
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received latitude of drone %d: %f°", s->id, s->latitude);
@@ -223,9 +291,11 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
             s->altitudeRel = *(UA_Float*) var->value.data;
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received relative altitude of drone %d: %fm", s->id, s->altitudeRel);
             return;
-        } else if (s->lastCheckpointId == id) {
-            s->lastCheckpoint = *(UA_UInt32*) var->value.data;
-            UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received checkpoint of drone %d: %dm", s->id, s->lastCheckpoint);
+        } else if (s->messageId == id) {
+            uaStr = *(UA_String*) var->value.data;
+            memcpy(s->message, uaStr.data, uaStr.length);
+            s->message[uaStr.length] = '\0';
+            UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received message of drone %d: %s", s->id, s->message);
             return;
         }
     }
@@ -237,73 +307,90 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
  * arg 1 (string): port used for multicast communication
  * arg 2 (string): network interface used for multicast communication
  * arg 3 (int): ID of the drone
- * arg 4 (bool): true if there will be data to publish
+ * arg 4 (double): publication/subscription interval in ms
+ * 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)
 {
     const char *ipv6;
     const char *port;
-    const char *net_iface;
-    char *not_const_net_iface;
+    const char *netIface;
+    char *notConstNetIface;
     char urlBuffer[44];
     UA_UInt32 id;
+    UA_Duration interval;
     int res;
 
     ipv6 = JS_ToCString(ctx, argv[0]);
     port = JS_ToCString(ctx, argv[1]);
-    net_iface = JS_ToCString(ctx, argv[2]);
+    netIface = JS_ToCString(ctx, argv[2]);
     UA_snprintf(urlBuffer, sizeof(urlBuffer), "opc.udp://[%s]:%s/", ipv6, port);
 
     UA_String transportProfile =
         UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-udp-uadp");
 
     UA_NetworkAddressUrlDataType networkAddressUrl;
-    not_const_net_iface = strdup(net_iface);
-    networkAddressUrl.networkInterface = UA_STRING(not_const_net_iface);
+    notConstNetIface = strdup(netIface);
+    networkAddressUrl.networkInterface = UA_STRING(notConstNetIface);
     networkAddressUrl.url              = UA_STRING(urlBuffer);
 
     if (JS_ToUint32(ctx, &id, argv[3]))
         return JS_EXCEPTION;
 
-    if (JS_ToBool(ctx, argv[4])) {
+    if (JS_ToFloat64(ctx, &interval, argv[4]))
+        return JS_EXCEPTION;
+
+    if (JS_ToBool(ctx, argv[5])) {
         res = runPubsub(&transportProfile, &networkAddressUrl,
                         droneVariableArray, countof(droneVariableArray), id,
-                        nbDrone, init_node_id, get_drone_id, pubsub_get_value,
-                        pubsub_update_coordinates, &pubsub_running);
+                        nbDrone, interval, init_node_id, get_drone_id,
+                        pubsub_get_value, pubsub_update_coordinates,
+                        &pubsubRunning);
     } else {
         res = subscribeOnly(&transportProfile, &networkAddressUrl,
                             droneVariableArray, countof(droneVariableArray), id,
-                            nbDrone, init_node_id, get_drone_id,
-                            pubsub_print_coordinates, &pubsub_running);
+                            nbDrone, interval, init_node_id, get_drone_id,
+                            pubsub_print_coordinates, &pubsubRunning);
     }
 
     JS_FreeCString(ctx, ipv6);
     JS_FreeCString(ctx, port);
-    free(not_const_net_iface);
-    JS_FreeCString(ctx, net_iface);
+    free(notConstNetIface);
+    JS_FreeCString(ctx, netIface);
     return JS_NewInt32(ctx, res);
 }
 
-static JSValue js_init_pubsub(JSContext *ctx, JSValueConst this_val,
+static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
                               int argc, JSValueConst *argv)
 {
     if (JS_ToUint32(ctx, &nbDrone, argv[0]))
         return JS_EXCEPTION;
 
-    drone_object_id_list = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
+    droneObjectIdList = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
     return JS_NewInt32(ctx, 0);
 }
 
-static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst this_val,
+static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
                               int argc, JSValueConst *argv)
 {
-    pubsub_running = false;
-    free(drone_object_id_list);
+    struct messageNode *current;
+
+    pubsubRunning = false;
+    free(droneObjectIdList);
+
+    while (messageQueue.head != NULL) {
+        current = messageQueue.head;
+        messageQueue.head = current->next;
+        delete_message_node(current);
+    }
+    clear_message(currentMessage);
     return JS_NewInt32(ctx, 0);
 }
 
-static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst this_val,
+// Connexion management functions
+
+static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
                                int argc, JSValueConst *argv)
 {
     const char *url;
@@ -323,76 +410,65 @@ static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst this_val,
     return JS_NewInt32(ctx, res);
 }
 
-static JSValue js_mavsdk_stop(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_stop(JSContext *ctx, JSValueConst thisVal,
                               int argc, JSValueConst *argv)
 {
     return JS_NewInt32(ctx, mavsdk_stop());
 }
 
-static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst thisVal,
                                 int argc, JSValueConst *argv)
 {
     return JS_NewInt32(ctx, mavsdk_reboot());
 }
 
-static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
+// Flight state management functions
+
+static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
                              int argc, JSValueConst *argv)
 {
-    return JS_NewBool(ctx, mavsdk_healthAllOk());
+    return JS_NewInt32(ctx, mavsdk_arm());
 }
 
-static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_land(JSContext *ctx, JSValueConst thisVal,
                               int argc, JSValueConst *argv)
 {
-    return JS_NewBool(ctx, mavsdk_landed());
+    return JS_NewInt32(ctx, mavsdk_land());
 }
 
-static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst thisVal,
                                  int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_arm());
+    return JS_NewInt32(ctx, mavsdk_takeOff());
 }
 
-static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
-                                              JSValueConst this_val,
+static JSValue js_mavsdk_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
                                         int argc, JSValueConst *argv)
 {
-    double la_arg_double;
-    double lo_arg_double;
-    double a_arg_double;
-    double y_arg_double;
-
-    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, &y_arg_double, argv[3]))
-        return JS_EXCEPTION;
-
-    return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
-                                                        lo_arg_double,
-                                                        (float)a_arg_double,
-                                                        (float)y_arg_double));
+    return JS_NewInt32(ctx, mavsdk_takeOffAndWait());
 }
 
-static JSValue js_mavsdk_setTargetLatLong(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst thisVal,
                                           int argc, JSValueConst *argv)
 {
-    double la_arg_double;
-    double lo_arg_double;
+    return JS_NewInt32(ctx, mavsdk_triggerParachute());
+}
 
-    if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
-        return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
+// Flight management functions
+
+static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst thisVal,
+                                int argc, JSValueConst *argv)
+{
+    double radius = 100;
+    if (argc > 0) {
+        if (JS_ToFloat64(ctx, &radius, argv[0]))
             return JS_EXCEPTION;
+    } 
 
-    return JS_NewInt32(ctx, mavsdk_setTargetLatLong(la_arg_double,
-                                                    lo_arg_double));
+    return JS_NewInt32(ctx, mavsdk_loiter((float)radius));
 }
 
-static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst thisVal,
                                      int argc, JSValueConst *argv)
 {
     double altitude;
@@ -400,10 +476,10 @@ static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst this_val,
     if (JS_ToFloat64(ctx, &altitude, argv[0]))
         return JS_EXCEPTION;
 
-    return JS_NewInt32(ctx, mavsdk_setAltitude((float)altitude));
+    return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
 }
 
-static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst thisVal,
                                      int argc, JSValueConst *argv)
 {
     double altitude;
@@ -411,183 +487,163 @@ static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst this_val,
     if (JS_ToFloat64(ctx, &altitude, argv[0]))
         return JS_EXCEPTION;
 
-    return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
+    return JS_NewInt32(ctx, mavsdk_setAltitude((float)altitude));
 }
 
-static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
+                                              JSValueConst thisVal,
                                               int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getYaw());
-}
+    double la_arg_double;
+    double lo_arg_double;
+    double a_arg_double;
 
-static JSValue js_mavsdk_getInitialLatitude(JSContext *ctx,
-                                            JSValueConst this_val,
-					                                  int argc, JSValueConst *argv)
-{
-    return JS_NewFloat64(ctx, mavsdk_getInitialLatitude());
-}
+    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;
 
-static JSValue js_mavsdk_getLatitude(JSContext *ctx, JSValueConst this_val,
-                                     int argc, JSValueConst *argv)
-{
-    return JS_NewFloat64(ctx, mavsdk_getLatitude());
+    return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
+                                                        lo_arg_double,
+                                                        (float)a_arg_double));
 }
 
-static JSValue js_mavsdk_getInitialLongitude(JSContext *ctx,
-                                             JSValueConst this_val,
+static JSValue js_mavsdk_setManualControlInput(JSContext *ctx,
+                                               JSValueConst thisVal,
                                                int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getInitialLongitude());
+    return JS_NewInt32(ctx, mavsdk_setManualControlInput());
 }
 
-static JSValue js_mavsdk_getLongitude(JSContext *ctx, JSValueConst this_val,
+// Information functions
+
+static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst thisVal,
                                      int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getLongitude());
+    return JS_NewFloat64(ctx, mavsdk_getAltitude());
 }
 
-static JSValue js_mavsdk_getTakeOffAltitude(JSContext *ctx,
-                                            JSValueConst this_val,
+static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst thisVal,
                                         int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getTakeOffAltitude());
+    return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
 }
 
 static JSValue js_mavsdk_getInitialAltitude(JSContext *ctx,
-                                            JSValueConst this_val,
+                                            JSValueConst thisVal,
                                             int argc, JSValueConst *argv)
 {
     return JS_NewFloat64(ctx, mavsdk_getInitialAltitude());
 }
 
-static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_getInitialLatitude(JSContext *ctx,
+                                            JSValueConst thisVal,
                                             int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getAltitude());
+    return JS_NewFloat64(ctx, mavsdk_getInitialLatitude());
 }
 
-static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_getInitialLongitude(JSContext *ctx,
+                                             JSValueConst thisVal,
                                              int argc, JSValueConst *argv)
 {
-    return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
+    return JS_NewFloat64(ctx, mavsdk_getInitialLongitude());
 }
 
-static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_getLatitude(JSContext *ctx, JSValueConst thisVal,
                                      int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_loiter());
+    return JS_NewFloat64(ctx, mavsdk_getLatitude());
 }
 
-static JSValue js_mavsdk_doParachute(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_getLongitude(JSContext *ctx, JSValueConst thisVal,
                                       int argc, JSValueConst *argv)
 {
-    double param;
-
-    if (JS_ToFloat64(ctx, &param, argv[0]))
-        return JS_EXCEPTION;
-
-    return JS_NewInt32(ctx, mavsdk_doParachute((float)param));
+    return JS_NewFloat64(ctx, mavsdk_getLongitude());
 }
 
-static JSValue js_mavsdk_setTargetCoordinatesXYZ(JSContext *ctx,
-                                                 JSValueConst this_val,
+static JSValue js_mavsdk_getTakeOffAltitude(JSContext *ctx,
+                                            JSValueConst thisVal,
                                             int argc, JSValueConst *argv)
 {
-    double x_arg_double;
-    double y_arg_double;
-    double z_arg_double;
-
-    if (JS_ToFloat64(ctx, &x_arg_double, argv[0]))
-        return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &y_arg_double, argv[1]))
-        return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &z_arg_double, argv[2]))
-        return JS_EXCEPTION;
-
-    return JS_NewInt32(ctx, mavsdk_setTargetCoordinatesXYZ(x_arg_double,
-                                                           y_arg_double,
-                                                           (float)z_arg_double));
+    return JS_NewFloat64(ctx, mavsdk_getTakeOffAltitude());
 }
 
-static JSValue js_mavsdk_setTargetAltitude(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst thisVal,
                                 int argc, JSValueConst *argv)
 {
-    double a_arg_double;
-
-    if (JS_ToFloat64(ctx, &a_arg_double, argv[0]))
-        return JS_EXCEPTION;
-
-    return JS_NewInt32(ctx, mavsdk_setTargetAltitude((float)a_arg_double));
+    return JS_NewFloat64(ctx, mavsdk_getYaw());
 }
 
-static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
                                      int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_takeOff());
+    return JS_NewBool(ctx, mavsdk_healthAllOk());
 }
 
-static JSValue js_mavsdk_takeOffAndWait(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_isInManualMode(JSContext *ctx, JSValueConst thisVal,
                                         int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_takeOffAndWait());
+    return JS_NewBool(ctx, mavsdk_isInManualMode());
 }
 
-static JSValue js_mavsdk_land(JSContext *ctx, JSValueConst this_val,
+static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
                                 int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_land());
+    return JS_NewBool(ctx, mavsdk_landed());
 }
 
 static const JSCFunctionListEntry js_mavsdk_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", 3, js_mavsdk_start ),
     JS_CFUNC_DEF("stop", 0, js_mavsdk_stop ),
     JS_CFUNC_DEF("reboot", 0, js_mavsdk_reboot ),
-    JS_CFUNC_DEF("healthAllOk", 0, js_mavsdk_healthAllOk ),
-    JS_CFUNC_DEF("landed", 0, js_mavsdk_landed ),
     JS_CFUNC_DEF("arm", 0, js_mavsdk_arm ),
-    JS_CFUNC_DEF("setTargetCoordinates", 4, js_mavsdk_setTargetCoordinates ),
-    JS_CFUNC_DEF("setTargetLatLong", 2, js_mavsdk_setTargetLatLong ),
-    JS_CFUNC_DEF("setAltitude", 1, js_mavsdk_setAltitude ),
-    JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
+    JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
+    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", 0, js_mavsdk_loiter ),
-    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("getYaw", 0, js_mavsdk_getYaw ),
+    JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
+    JS_CFUNC_DEF("setAltitude", 1, js_mavsdk_setAltitude ),
+    JS_CFUNC_DEF("setTargetCoordinates", 3, js_mavsdk_setTargetCoordinates ),
+    JS_CFUNC_DEF("setManualControlInput", 0, js_mavsdk_setManualControlInput ),
+    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("getLatitude", 0, js_mavsdk_getLatitude ),
     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("getInitialAltitude", 0, js_mavsdk_getInitialAltitude ),
-    JS_CFUNC_DEF("getAltitude", 0, js_mavsdk_getAltitude ),
-    JS_CFUNC_DEF("getAltitudeRel", 0, js_mavsdk_getAltitudeRel ),
-    JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
-    JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
-    JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
+    JS_CFUNC_DEF("getYaw", 0, js_mavsdk_getYaw ),
+    JS_CFUNC_DEF("isInManualMode", 0, js_mavsdk_isInManualMode ),
+    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("runPubsub", 5, js_run_pubsub ),
-    JS_CFUNC_DEF("setCheckpoint", 1, js_drone_set_checkpoint ),
-    JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
 };
 
 static int js_mavsdk_init(JSContext *ctx, JSModuleDef *m)
 {
-    JSValue drone_proto, drone_class;
+    JSValue droneProto, droneClass;
 
-    JS_NewClassID(&js_drone_class_id);
-    JS_NewClass(JS_GetRuntime(ctx), js_drone_class_id, &js_drone_class);
+    JS_NewClassID(&jsDroneClassId);
+    JS_NewClass(JS_GetRuntime(ctx), jsDroneClassId, &jsDroneClass);
 
-    drone_proto = JS_NewObject(ctx);
-    JS_SetPropertyFunctionList(ctx, drone_proto, js_drone_proto_funcs,
+    droneProto = JS_NewObject(ctx);
+    JS_SetPropertyFunctionList(ctx, droneProto, js_drone_proto_funcs,
                                countof(js_drone_proto_funcs));
 
-    drone_class = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
+    droneClass = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
                                    JS_CFUNC_constructor, 0);
-    JS_SetConstructor(ctx, drone_class, drone_proto);
-    JS_SetClassProto(ctx, js_drone_class_id, drone_proto);
+    JS_SetConstructor(ctx, droneClass, droneProto);
+    JS_SetClassProto(ctx, jsDroneClassId, droneProto);
 
-    JS_SetModuleExport(ctx, m, "Drone", drone_class);
+    JS_SetModuleExport(ctx, m, "Drone", droneClass);
 
     return JS_SetModuleExportList(ctx, m, js_mavsdk_funcs,
                                   countof(js_mavsdk_funcs));