Comply with qjs-wrapper v2.0

Makefile

+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 -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=-lmavsdk -lmavsdk_action -lmavsdk_mavlink_passthrough -lmavsdk_telemetry
+
+LIB_NAME := libautopilotwrapper.so
+
+SRCS := mavsdk_wrapper.cpp
+OBJS := mavsdk_wrapper.o
+
+all: $(LIB_NAME)
+
+%.o: 
+
+$(LIB_NAME): $(OBJS)
+	$(CC) $(LDFLAGS) -shared -o $@ $^ $(LIBS)
+
+install: all
+	mkdir -p "$(DESTDIR)$(prefix)/lib"
+	install -m644 $(LIB_NAME) "$(DESTDIR)$(prefix)/lib"
+
+clean:
+	$(RM) -f $(OBJS) $(LIB_NAME)

README.md

 # mavsdk-wrapper
 
+This project is holding the source code for [qjs-wrapper](https://lab.nexedi.com/nexedi/qjs-wrapper) component that will wrap the functions from [MAVSDK](https://mavsdk.mavlink.io/main/en/index.html).
\ No newline at end of file

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 "autopilot_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 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 = 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 stop(bool shutdown) {
+    if (!mavsdk_started)
+        return -1;
+
+    if (shutdown) {
+        while (telemetry->armed())
+            sleep_for(seconds(1));
+        if (doAction(&Action::shutdown, "Shutdown failed"))
+            return -1;
+    }
+
+    // Delete pointers
+    delete action;
+    delete mavlink_passthrough;
+    delete telemetry;
+
+    log_file_fd << std::flush;
+    log_file_fd.close();
+
+    return 0;
+}
+
+int reboot() {
+    return doAction(&Action::reboot, "Rebooting failed");
+}
+
+// Flight state management functions
+
+int 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 takeOff(void) {
+    if (doAction(&Action::takeoff, "Takeoff failed"))
+        return -1;
+
+    return 0;
+}
+
+int takeOffAndWait(void) {
+    if (takeOff() < 0)
+        return -1;
+
+    while (!takingOff)
+        sleep_for(seconds(1));
+    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
+        sleep_for(seconds(1));
+
+    return doAction(&Action::transition_to_fixedwing, "Transition to fixedwing failed");
+}
+
+int triggerParachute(void) {
+    if (!mavsdk_started)
+        return -1;
+
+    log("Landing...");
+    MavlinkPassthrough::CommandLong command;
+    command.command = MAV_CMD_DO_FLIGHTTERMINATION;
+    command.param1 = 1; //see https://mavlink.io/en/messages/common.html#MAV_CMD_DO_FLIGHTTERMINATION
+    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
+
+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 loiter(float radius) {
+    Telemetry::Position position = telemetry->position();
+    return doReposition(
+        (float)position.latitude_deg,
+        (float)position.longitude_deg,
+        radius,
+        0
+    );
+}
+
+int 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 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 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 |= setAltitude(a);
+    return result;
+}
+
+// Information functions
+
+float getAltitude(void) {
+    return telemetry->position().absolute_altitude_m;
+}
+
+float getAltitudeRel(void) {
+    return telemetry->position().relative_altitude_m;
+}
+
+float getInitialAltitude(void) {
+    return origin.absolute_altitude_m;
+}
+
+double getInitialLatitude(void) {
+    return origin.latitude_deg;
+}
+
+double getInitialLongitude(void) {
+    return origin.longitude_deg;
+}
+
+double getLatitude(void) {
+    return telemetry->position().latitude_deg;
+}
+
+double getLongitude(void) {
+    return telemetry->position().longitude_deg;
+}
+
+double *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 *getSpeedArray(void) {
+    Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
+    float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
+    speedArray[0] = getYaw();
+    speedArray[1] = metrics.airspeed_m_s;
+    speedArray[2] = metrics.climb_rate_m_s;
+    return speedArray;
+}
+
+double 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 getYaw(void) {
+    return telemetry->attitude_euler().yaw_deg;
+}
+
+float getSpeed(void) {
+    return telemetry->fixedwing_metrics().airspeed_m_s;
+}
+
+float getClimbRate(void) {
+    return telemetry->fixedwing_metrics().climb_rate_m_s;
+}
+
+int healthAllOk(void) {
+    return telemetry->health_all_ok();
+}