Use manual control for setTargetCoordinates

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/mavsdk_wrapper.h

@@ -12,17 +12,17 @@ int mavsdk_reboot(void);
 
 // Flight state management functions
 int mavsdk_arm(void);
-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_setTargetCoordinates(double la, double lo, float a, float y);
+int mavsdk_setTargetCoordinates(double la, double lo, float a);
+int mavsdk_setManualControlInput(void);
 
 // Information functions
 float mavsdk_getAltitude(void);
@@ -41,6 +41,7 @@ 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
 }

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,6 +27,7 @@ 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;
 
@@ -37,13 +39,16 @@ 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,6 +94,10 @@ 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;
@@ -123,6 +132,7 @@ 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...");
@@ -165,7 +175,7 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
         sleep_for(seconds(1));
     }
     origin = position;
-    xy_ratio = std::cos((M_PI * origin.latitude_deg) / 180.F);
+    xy_ratio = std::cos(toRad(origin.latitude_deg));
 
     log("MAVSDK started...");
     mavsdk_started = 1;
@@ -188,6 +198,7 @@ int mavsdk_stop() {
 
     // Delete pointers
     delete action;
+    delete manual_control;
     delete mavlink_passthrough;
     delete telemetry;
     log_file_fd.close();
@@ -235,15 +246,6 @@ int mavsdk_land(void) {
     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")) {
         return -1;
@@ -282,22 +284,23 @@ int mavsdk_triggerParachute(void) {
 
 // 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) {
@@ -316,7 +319,7 @@ int mavsdk_setAirspeed(float airspeed) {
 }
 
 int mavsdk_setAltitude(float altitude) {
-    if(!mavsdk_started)
+    if (!mavsdk_started)
         return -1;
 
     MavlinkPassthrough::CommandLong command;
@@ -330,20 +333,72 @@ int mavsdk_setAltitude(float altitude) {
     return doMavlinkCommand(command, "Setting altitude failed");
 }
 
-int mavsdk_setTargetCoordinates(double la, double lo, float a, float y) {
-    if(!mavsdk_started)
+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;
+}
+
+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_setManualControlInput(void) {
+    if (autocontinue)
+        return setManualControlInput(0, 0, 0, 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;
+    }
+
+    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) {
@@ -416,6 +471,10 @@ 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

@@ -336,17 +336,23 @@ static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst this_val,
 }
 
 static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
-				     int argc, JSValueConst *argv)
+                                     int argc, JSValueConst *argv)
 {
     return JS_NewBool(ctx, mavsdk_healthAllOk());
 }
 
 static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
-				                             int argc, JSValueConst *argv)
+                                int argc, JSValueConst *argv)
 {
     return JS_NewBool(ctx, mavsdk_landed());
 }
 
+static JSValue js_mavsdk_isInManualMode(JSContext *ctx, JSValueConst thisVal,
+                                        int argc, JSValueConst *argv)
+{
+    return JS_NewBool(ctx, mavsdk_isInManualMode());
+}
+
 static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
                              int argc, JSValueConst *argv)
 {
@@ -360,7 +366,6 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
     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;
@@ -368,13 +373,17 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
         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));
+                                                        (float)a_arg_double));
+}
+
+static JSValue js_mavsdk_setManualControlInput(JSContext *ctx,
+                                               JSValueConst thisVal,
+                                               int argc, JSValueConst *argv)
+{
+    return JS_NewInt32(ctx, mavsdk_setManualControlInput());
 }
 
 static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst this_val,
@@ -460,7 +469,13 @@ static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst this_val,
 static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst this_val,
                                 int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, mavsdk_loiter());
+    double radius = 100;
+    if (argc > 0) {
+        if (JS_ToFloat64(ctx, &radius, argv[0]))
+            return JS_EXCEPTION;
+    } 
+
+    return JS_NewInt32(ctx, mavsdk_loiter((float)radius));
 }
 
 static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst this_val,
@@ -493,8 +508,10 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     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("isInManualMode", 0, js_mavsdk_isInManualMode ),
     JS_CFUNC_DEF("arm", 0, js_mavsdk_arm ),
-    JS_CFUNC_DEF("setTargetCoordinates", 4, js_mavsdk_setTargetCoordinates ),
+    JS_CFUNC_DEF("setTargetCoordinates", 3, js_mavsdk_setTargetCoordinates ),
+    JS_CFUNC_DEF("setManualControlInput", 0, js_mavsdk_setManualControlInput ),
     JS_CFUNC_DEF("setAltitude", 1, js_mavsdk_setAltitude ),
     JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
     JS_CFUNC_DEF("loiter", 0, js_mavsdk_loiter ),