Comply with qjs-wrapper v2.0

See merge request !1

Makefile

+CXXFLAGS=-std=gnu++17 -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
+LIBS=-lmavsdk
+
+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
+# c-astral-wrapper
 
+This project is holding the c-astral's autopilot specific source code 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
+
+struct Coordinates {
+  double latitude;
+  double longitude;
+};
+
+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 DEFAULT_OVERRIDE_ALTITUDE = 30;
+static const float EARTH_RADIUS = 6371000;
+static const float ADDED_DISTANCE = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
+static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE);
+static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
+
+static int mavsdk_started = 0;
+static Telemetry::Position origin;
+static int64_t current_position[POSITION_ARRAY_SIZE] = {0};
+static int64_t timestamp_difference = 0;
+
+static Coordinates targeted_destination;
+static float targeted_radius = DEFAULT_RADIUS;
+static Coordinates projected_destination;
+
+static const float DEFAULT_SPEED = 17;
+static float last_override_altitude;
+static float last_override_speed;
+
+static bool do_projection = false;
+static bool first_coordinate_entered = false;
+static bool is_in_air = false;
+
+// 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;
+}
+
+// Coordinates related functions
+
+static double toRad(double angle) {
+    return M_PI * angle / 180;
+}
+
+static double toDeg(double angle) {
+    return 180 * angle / M_PI;
+}
+
+/*
+ * 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);
+}
+
+/*
+ * To ensure that a drone goes through the target point and to avoid loitering
+ * trajectory, the target point is projected to a distance equal to twice the
+ * default loiter radius.
+*/
+static Coordinates getProjectionCoordinates(Coordinates destination, Coordinates position) {
+    double laRad = toRad(destination.latitude);
+    double sinLa = sin(laRad);
+    double cosLa = cos(laRad);
+    double loRad = toRad(destination.longitude);
+    double bearing_angle = bearing(position.latitude, position.longitude,
+                                   targeted_destination.latitude,
+                                   targeted_destination.longitude);
+    double newLa = asin(sinLa * COS_ADDED_DISTANCE
+                        + cosLa * SIN_ADDED_DISTANCE * cos(bearing_angle));
+    double newLo = loRad + atan2(sin(bearing_angle) * SIN_ADDED_DISTANCE * cosLa,
+                                 COS_ADDED_DISTANCE - sinLa * sin(newLa));
+    Coordinates projected{ toDeg(newLa), toDeg(newLo) };
+    return projected;
+}
+
+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");
+}
+
+static void doReposition_async(float la, float lo, float radius, float y) {
+    if (!first_coordinate_entered) {
+      first_coordinate_entered = true;
+    }
+    std::thread(doReposition, la, lo, radius, y).detach();
+}
+
+static void updateProjection(int64_t current_lat, int64_t current_lon) {
+    Coordinates position{ (double)current_lat / 1e7, (double)current_lon / 1e7 };
+    projected_destination = getProjectionCoordinates(targeted_destination,
+                                                     position);
+
+    doReposition_async(
+        (float)projected_destination.latitude,
+        (float)projected_destination.longitude,
+        DEFAULT_RADIUS,
+        0
+    );
+}
+
+static long long int getTimestamp() {
+    const auto now = std::chrono::system_clock::now();
+    return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
+}
+
+static int doOverride(float altitude, float speed, const char* failure_msg) {
+    if (!mavsdk_started)
+        return -1;
+
+    if (!first_coordinate_entered) {
+        log_error("Not overriding altitude before user sets coordinates");
+        return -1;
+    }
+
+    MavlinkPassthrough::CommandLong command;
+    command.command = MAV_CMD_DO_OVERRIDE;
+    command.param1 = 1 | 2 | 4 | 8;
+    command.param2 = 1 | 2 | 4 | 8;
+    command.param3 = altitude;
+    command.param4 = speed;
+    command.target_sysid = mavlink_passthrough->get_target_sysid();
+    command.target_compid = mavlink_passthrough->get_target_compid();
+
+    return doMavlinkCommand(command, failure_msg);
+}
+
+static int setAltitude(float altitude) {
+    last_override_altitude = altitude;
+    return doOverride(altitude, last_override_speed, "Setting altitude failed");
+}
+
+static void setAltitude_async(float altitude) {
+    std::thread(setAltitude, altitude).detach();
+}
+
+void updateLogAndProjection(void) {
+    std::ostringstream oss;
+    Telemetry::FixedwingMetrics metrics;
+
+    if(mavsdk_started) {
+        metrics = telemetry->fixedwing_metrics();
+        oss << current_position[4] << ";"
+            << current_position[0] << ";" << current_position[1] << ";"
+            << current_position[2] << ";" << current_position[3] << ";"
+            << telemetry->attitude_euler().yaw_deg << ";"
+            << metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
+        log(oss.str());
+
+        if (first_coordinate_entered) {
+          if (do_projection) {
+              updateProjection(current_position[0], current_position[1]);
+          } else {
+            doReposition_async(
+                (float)targeted_destination.latitude,
+                (float)targeted_destination.longitude,
+                targeted_radius,
+                0
+            ); 
+          }
+        }
+    }
+}
+
+// Connexion management functions
+
+int start(const char * ip, int port, const char * log_file, int timeout) {
+    std::string remote_ip(ip);
+    ConnectionResult connection_result;
+    float abs_altitude;
+    log_file_fd.open(log_file);
+
+    connection_result = _mavsdk.add_udp_connection(remote_ip, port);
+    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);
+
+    mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, [](const mavlink_message_t& message) {
+        mavlink_global_position_int_t position;
+        mavlink_msg_global_position_int_decode(&message, &position);
+
+        if (timestamp_difference == 0)
+            timestamp_difference = getTimestamp() - position.time_boot_ms;
+
+        current_position[0] = position.lat;
+        current_position[1] = position.lon;
+        current_position[2] = position.alt;
+        current_position[3] = position.relative_alt;
+        current_position[4] = position.time_boot_ms + timestamp_difference;
+    });
+
+    telemetry->subscribe_landed_state([](const Telemetry::LandedState landed_state) {
+        if (landed_state == Telemetry::LandedState::InAir) {
+            is_in_air = true;
+        }
+    });
+
+    do {
+        log("Waiting for first telemetry");
+        sleep_for(seconds(1));
+        abs_altitude = getAltitude();
+    } while(isnan(abs_altitude) || abs_altitude == 0);
+
+    origin = telemetry->position();
+    last_override_altitude = origin.absolute_altitude_m + DEFAULT_OVERRIDE_ALTITUDE;
+    last_override_speed = DEFAULT_SPEED;
+
+    log("MAVSDK started...");
+    mavsdk_started = 1;
+    log("timestamp (ms);latitude (°E7);longitude (°E7);AMSL (mm);rel altitude (mm);yaw (°);air speed (m/s);climb rate (m/s)");
+
+    return 0;
+}
+
+int stop() {
+    // 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;
+
+    if (telemetry->armed()) {
+      return 0;
+    }
+
+    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) {
+    action->set_takeoff_altitude(DEFAULT_OVERRIDE_ALTITUDE);
+    if (doAction(&Action::takeoff, "Takeoff failed"))
+        return -1;
+
+    return 0;
+}
+
+int takeOffAndWait(void) {
+    if (takeOff() < 0)
+        return -1;
+
+    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
+        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_PARACHUTE;
+    //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
+    command.param1 = 2;
+    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
+
+void loiter(double la, double lo, float a, float radius) {
+    targeted_destination.latitude = la;
+    targeted_destination.longitude = lo;
+    targeted_radius = radius;
+    do_projection = false;
+
+    doReposition_async((float)la, (float)lo, radius, 0);
+    setAltitude_async(a);
+}
+
+static int setAirSpeed(float airspeed) {
+    last_override_speed = airspeed;
+    return doOverride(last_override_altitude, airspeed,
+                      "Setting airspeed failed");
+}
+
+void setAirSpeed_async(float airspeed) {
+    std::thread(setAirSpeed, airspeed).detach();
+}
+
+void setTargetCoordinates(double la, double lo, float a) {
+    if (!mavsdk_started) {
+        log_error("Mavsdk not started");
+        return;
+    }
+
+    targeted_destination.latitude = la;
+    targeted_destination.longitude = lo;
+    do_projection = true;
+    
+    updateProjection(current_position[0], current_position[1]);
+    setAltitude_async(a);
+}
+
+// Information functions
+
+float getAltitude(void) {
+    return (float)(current_position[2] / 1000);
+}
+
+float getInitialAltitude(void) {
+    return origin.absolute_altitude_m;
+}
+
+double getInitialLatitude(void) {
+    return origin.latitude_deg;
+}
+
+double getInitialLongitude(void) {
+    return origin.longitude_deg;
+}
+
+int64_t *getPositionArray(void) {
+    int64_t *positionArray = (int64_t*) malloc(POSITION_ARRAY_SIZE * sizeof(int64_t));
+    memcpy(positionArray, current_position, POSITION_ARRAY_SIZE * sizeof(int64_t));
+    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 gpsIsOk(void) {
+    Telemetry::FixType fixType = telemetry->gps_info().fix_type;
+    return (fixType != Telemetry::FixType::NoGps) && (fixType != Telemetry::FixType::NoFix);
+}
+
+int healthAllOk(void) {
+    return telemetry->health_all_ok();
+}
+
+int isLanding(void) {
+    if (is_in_air && telemetry->landed_state() != Telemetry::LandedState::InAir)
+        return 1;
+    return 0;
+}