Use doReposition instead of manual control

Rather than using manual control and therefore losing benefits of the autopilot,
setTargetCoordinates now uses doReposition but with GPS coordinates 200 meters
further away (2 times default radius) than given coordinates.

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_manual_control -lmavsdk_mavlink_passthrough -lmavsdk_telemetry -lopen62541
+LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_mavlink_passthrough -lmavsdk_telemetry -lopen62541
 
 LIB_NAME := libqjswrapper.so
 

include/mavsdk_wrapper.h

@@ -33,9 +33,7 @@ int mavsdk_triggerParachute(void);
 // Flight management functions
 int mavsdk_loiter(float radius);
 int mavsdk_setAirspeed(float airspeed);
-int mavsdk_setAltitude(float altitude);
 int mavsdk_setTargetCoordinates(double la, double lo, float a);
-int mavsdk_setManualControlInput(void);
 
 // Information functions
 float mavsdk_getAltitude(void);
@@ -52,7 +50,6 @@ float mavsdk_getYaw(void);
 float mavsdk_getSpeed(void);
 float mavsdk_getClimbRate(void);
 int mavsdk_healthAllOk(void);
-bool mavsdk_isInManualMode(void);
 int mavsdk_landed(void);
 #ifdef __cplusplus
 }

mavsdk_wrapper.cpp

@@ -3,7 +3,6 @@
 #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>
@@ -27,26 +26,16 @@ 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 const double EARTH_GRAVITY = 9.81;
-static const double EARTH_RADIUS = 6371000.F;
-static const double MIN_YAW_DIFF = 1;
-static const double MAX_ROLL = 35;
-static const double YAW_VELOCITY_COEF = 0.5;
+static const float DEFAULT_RADIUS = 100;
+static const float EARTH_RADIUS = 6371000;
 
 static bool takingOff = false;
-static bool autocontinue = false;
-static double initialBearing;
-static double previousBearing;
-static double xy_ratio;
-static double target_latitude;
-static double target_longitude;
 static int mavsdk_started = 0;
 static uint64_t init_timestamp;
 static Telemetry::Position origin;
@@ -138,7 +127,6 @@ 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);
 
     telemetry->subscribe_flight_mode([](Telemetry::FlightMode flight_mode) {
@@ -175,7 +163,6 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
         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;
@@ -197,7 +184,6 @@ int mavsdk_stop() {
 
     // Delete pointers
     delete action;
-    delete manual_control;
     delete mavlink_passthrough;
     delete telemetry;
     log_file_fd.close();
@@ -272,25 +258,34 @@ int mavsdk_triggerParachute(void) {
 
 // Flight management functions
 
-int mavsdk_loiter(float radius) {
+static int doReposition(float la, float lo, float radius, float y) {
     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 = 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.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;
@@ -306,7 +301,7 @@ int mavsdk_setAirspeed(float airspeed) {
     return doMavlinkCommand(command, "Setting airspeed failed");
 }
 
-int mavsdk_setAltitude(float altitude) {
+static int mavsdk_setAltitude(float altitude) {
     if (!mavsdk_started)
         return -1;
 
@@ -321,106 +316,47 @@ int mavsdk_setAltitude(float altitude) {
     return doMavlinkCommand(command, "Setting altitude failed");
 }
 
-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;
-}
-
+/*
+ * 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) * 180 / M_PI;
+    return atan2(x, y);
 }
 
 int mavsdk_setTargetCoordinates(double la, double lo, float a) {
-    int result = 0;
+    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;
 
-    target_latitude = la;
-    target_longitude = lo;
-    initialBearing = 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_setManualControlInput(void) {
-    double b;
-    float speed = mavsdk_getSpeed();
-
-    if (autocontinue) {
-        b = initialBearing;
-    } else {
-        Telemetry::Position position = telemetry->position();
-        b = bearing(position.latitude_deg, position.longitude_deg,
-                    target_latitude, target_longitude);
-/*
-  If there is a difference of more than 160° (180° for a half circle and 20° of imprecision)
-  between the required bearing and the one some milliseconds ago,
-  it means that the target is now behind the drone so the drone just went over the target.
-
-  In this case, we don't follow the target anymore, we simply keep the same trajectory.
-*/
-        if (abs(b - previousBearing) > 160) {
-            autocontinue = true;
-            return 0;
-        }
-        previousBearing = b;
-    }
-
-    float angle_diff = (float)b - mavsdk_getYaw();
-    if (angle_diff < -180) {
-        angle_diff += 360;
-    } else if (angle_diff > 180) {
-        angle_diff -= 360;
-    }
-
-    if (abs(angle_diff) < MIN_YAW_DIFF) {
-        initialBearing = b;
-        return 0;
-    }
+    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)
+    );
 
-    /*
-    * radius is speed²/g*tan(B) where B is roll angle
-    * let's define yaw angular velocity Ys as speed*360/perimeter
-    * so tan(B) = 2PI*Ys*speed / 360*g
-    */
-    double wanted_yaw_velocity = angle_diff * YAW_VELOCITY_COEF;
-    double roll = toDeg(atan(
-      2 * M_PI * wanted_yaw_velocity * speed / (360 * EARTH_GRAVITY)
-    ));
-    return setManualControlInput(
-      0,
-      (float)((roll > 0 ? 1 : -1) * std::min(abs(roll), MAX_ROLL) / MAX_ROLL),
-      0,
+    result = doReposition(
+        (float)toDeg(newLa),
+        (float)toDeg(newLo),
+        DEFAULT_RADIUS,
         0
     );
+    result |= mavsdk_setAltitude(a);
+    return result;
 }
 
 // Information functions
@@ -498,10 +434,6 @@ 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();
 }

qjs_wrapper.c

@@ -483,17 +483,6 @@ static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst thisVal,
     return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
 }
 
-static JSValue js_mavsdk_setAltitude(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_setAltitude((float)altitude));
-}
-
 static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
                                               JSValueConst thisVal,
                                               int argc, JSValueConst *argv)
@@ -514,13 +503,6 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
                                                         (float)a_arg_double));
 }
 
-static JSValue js_mavsdk_setManualControlInput(JSContext *ctx,
-                                               JSValueConst thisVal,
-                                               int argc, JSValueConst *argv)
-{
-    return JS_NewInt32(ctx, mavsdk_setManualControlInput());
-}
-
 // Information functions
 
 static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst thisVal,
@@ -599,12 +581,6 @@ static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
     return JS_NewBool(ctx, mavsdk_healthAllOk());
 }
 
-static JSValue js_mavsdk_isInManualMode(JSContext *ctx, JSValueConst thisVal,
-                                        int argc, JSValueConst *argv)
-{
-    return JS_NewBool(ctx, mavsdk_isInManualMode());
-}
-
 static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
                                 int argc, JSValueConst *argv)
 {
@@ -624,9 +600,7 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     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("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 ),
@@ -638,7 +612,6 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     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("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 ),