Commit 55abbce6 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Use manual control for setTargetCoordinates

parent f616af35
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 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 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_mavlink_passthrough -lmavsdk_telemetry -lopen62541 LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_manual_control -lmavsdk_mavlink_passthrough -lmavsdk_telemetry -lopen62541
LIB_NAME := libqjswrapper.so LIB_NAME := libqjswrapper.so
......
...@@ -12,17 +12,17 @@ int mavsdk_reboot(void); ...@@ -12,17 +12,17 @@ int mavsdk_reboot(void);
// Flight state management functions // Flight state management functions
int mavsdk_arm(void); int mavsdk_arm(void);
int mavsdk_loiter();
int mavsdk_land(void); int mavsdk_land(void);
int mavsdk_takeOff(void); int mavsdk_takeOff(void);
int mavsdk_takeOffAndWait(void); int mavsdk_takeOffAndWait(void);
int mavsdk_triggerParachute(void); int mavsdk_triggerParachute(void);
// Flight management functions // 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_setAirspeed(float airspeed);
int mavsdk_setAltitude(float altitude); 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 // Information functions
float mavsdk_getAltitude(void); float mavsdk_getAltitude(void);
...@@ -41,6 +41,7 @@ float mavsdk_getAirspeed(void); ...@@ -41,6 +41,7 @@ float mavsdk_getAirspeed(void);
float mavsdk_getClimbRate(void); float mavsdk_getClimbRate(void);
float mavsdk_getThrottle(void); float mavsdk_getThrottle(void);
int mavsdk_healthAllOk(void); int mavsdk_healthAllOk(void);
bool mavsdk_isInManualMode(void);
int mavsdk_landed(void); int mavsdk_landed(void);
#ifdef __cplusplus #ifdef __cplusplus
} }
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include <cstdint> #include <cstdint>
#include <mavsdk/mavsdk.h> #include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.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/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h> #include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream> #include <iostream>
...@@ -26,6 +27,7 @@ static std::ofstream log_file_fd; ...@@ -26,6 +27,7 @@ static std::ofstream log_file_fd;
static Mavsdk _mavsdk; static Mavsdk _mavsdk;
static Telemetry * telemetry; static Telemetry * telemetry;
static Action * action; static Action * action;
static ManualControl * manual_control;
static MavlinkPassthrough * mavlink_passthrough; static MavlinkPassthrough * mavlink_passthrough;
static std::shared_ptr<System> msystem; static std::shared_ptr<System> msystem;
...@@ -37,13 +39,16 @@ static Telemetry::EulerAngle angle; ...@@ -37,13 +39,16 @@ static Telemetry::EulerAngle angle;
static Telemetry::FixedwingMetrics metric; static Telemetry::FixedwingMetrics metric;
static Telemetry::Position position; static Telemetry::Position position;
static uint64_t timestamp_us; static uint64_t timestamp_us;
static Telemetry::Position origin;
static double xy_ratio;
static const double EARTH_RADIUS = 6371000.F; 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 int mavsdk_started = 0;
static Telemetry::Position origin;
// Logs functions // Logs functions
...@@ -89,6 +94,10 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string ...@@ -89,6 +94,10 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string
// Connexion management functions // 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) { int mavsdk_start(const char * url, const char * log_file, int timeout) {
std::string connection_url(url); std::string connection_url(url);
ConnectionResult connection_result; ConnectionResult connection_result;
...@@ -123,6 +132,7 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) { ...@@ -123,6 +132,7 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
msystem = fut.get(); msystem = fut.get();
telemetry = new Telemetry(msystem); telemetry = new Telemetry(msystem);
action = new Action(msystem); action = new Action(msystem);
manual_control = new ManualControl(msystem);
mavlink_passthrough = new MavlinkPassthrough(msystem); mavlink_passthrough = new MavlinkPassthrough(msystem);
log("Subscribing to flight mode..."); log("Subscribing to flight mode...");
...@@ -165,7 +175,7 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) { ...@@ -165,7 +175,7 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
sleep_for(seconds(1)); sleep_for(seconds(1));
} }
origin = position; origin = position;
xy_ratio = std::cos((M_PI * origin.latitude_deg) / 180.F); xy_ratio = std::cos(toRad(origin.latitude_deg));
log("MAVSDK started..."); log("MAVSDK started...");
mavsdk_started = 1; mavsdk_started = 1;
...@@ -188,6 +198,7 @@ int mavsdk_stop() { ...@@ -188,6 +198,7 @@ int mavsdk_stop() {
// Delete pointers // Delete pointers
delete action; delete action;
delete manual_control;
delete mavlink_passthrough; delete mavlink_passthrough;
delete telemetry; delete telemetry;
log_file_fd.close(); log_file_fd.close();
...@@ -235,15 +246,6 @@ int mavsdk_land(void) { ...@@ -235,15 +246,6 @@ int mavsdk_land(void) {
return 0; 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) { int mavsdk_takeOff(void) {
if(doAction(&Action::takeoff, "Takeoff failed")) { if(doAction(&Action::takeoff, "Takeoff failed")) {
return -1; return -1;
...@@ -282,22 +284,23 @@ int mavsdk_triggerParachute(void) { ...@@ -282,22 +284,23 @@ int mavsdk_triggerParachute(void) {
// Flight management functions // Flight management functions
int mavsdk_doReposition(float la, float lo, float a, float y) { int mavsdk_loiter(float radius) {
if(!mavsdk_started) if (!mavsdk_started)
return -1; return -1;
Telemetry::Position position = telemetry->position();
MavlinkPassthrough::CommandLong command; MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_REPOSITION; command.command = MAV_CMD_DO_REPOSITION;
command.param1 = -1; // Ground speed, -1 for default 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.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise command.param3 = radius; // loiter radius
command.param5 = la; command.param5 = (float)position.latitude_deg;
command.param6 = lo; command.param6 = (float)position.longitude_deg;
command.param7 = a; command.param7 = position.absolute_altitude_m; //altitude is ignored, use altitude override package
command.target_sysid = mavlink_passthrough->get_target_sysid(); command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid(); 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) { int mavsdk_setAirspeed(float airspeed) {
...@@ -316,7 +319,7 @@ int mavsdk_setAirspeed(float airspeed) { ...@@ -316,7 +319,7 @@ int mavsdk_setAirspeed(float airspeed) {
} }
int mavsdk_setAltitude(float altitude) { int mavsdk_setAltitude(float altitude) {
if(!mavsdk_started) if (!mavsdk_started)
return -1; return -1;
MavlinkPassthrough::CommandLong command; MavlinkPassthrough::CommandLong command;
...@@ -330,20 +333,72 @@ int mavsdk_setAltitude(float altitude) { ...@@ -330,20 +333,72 @@ int mavsdk_setAltitude(float altitude) {
return doMavlinkCommand(command, "Setting altitude failed"); return doMavlinkCommand(command, "Setting altitude failed");
} }
int mavsdk_setTargetCoordinates(double la, double lo, float a, float y) { static int startAltitudeControl(void) {
if(!mavsdk_started) 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; return -1;
std::cout << "Going to location (" << la << " , " << lo << ") " target_latitude = la;
<< a << " m " << std::endl; target_longitude = lo;
const Action::Result result = action->goto_location(la, lo, a, y); previousBearing = bearing(position.latitude_deg, position.longitude_deg,
if (result != Action::Result::Success) { la, lo);
log_error_from_result("Goto location failed", result); 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 -1;
} }
return 0; 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 // Information functions
float mavsdk_getAltitude(void) { float mavsdk_getAltitude(void) {
...@@ -416,6 +471,10 @@ int mavsdk_healthAllOk(void) { ...@@ -416,6 +471,10 @@ int mavsdk_healthAllOk(void) {
return telemetry->health_all_ok(); return telemetry->health_all_ok();
} }
bool mavsdk_isInManualMode(void) {
return telemetry->flight_mode() == Telemetry::FlightMode::Altctl;
}
int mavsdk_landed(void) { int mavsdk_landed(void) {
return !telemetry->in_air(); return !telemetry->in_air();
} }
...@@ -336,17 +336,23 @@ static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst this_val, ...@@ -336,17 +336,23 @@ static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst this_val,
} }
static JSValue js_mavsdk_healthAllOk(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()); return JS_NewBool(ctx, mavsdk_healthAllOk());
} }
static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val, static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{ {
return JS_NewBool(ctx, mavsdk_landed()); 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, static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{ {
...@@ -360,7 +366,6 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx, ...@@ -360,7 +366,6 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
double la_arg_double; double la_arg_double;
double lo_arg_double; double lo_arg_double;
double a_arg_double; double a_arg_double;
double y_arg_double;
if (JS_ToFloat64(ctx, &la_arg_double, argv[0])) if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
return JS_EXCEPTION; return JS_EXCEPTION;
...@@ -368,13 +373,17 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx, ...@@ -368,13 +373,17 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
return JS_EXCEPTION; return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &a_arg_double, argv[2])) if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
return JS_EXCEPTION; return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &y_arg_double, argv[3]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double, return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
lo_arg_double, lo_arg_double,
(float)a_arg_double, (float)a_arg_double));
(float)y_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, static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst this_val,
...@@ -460,7 +469,13 @@ static JSValue js_mavsdk_getAltitudeRel(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, static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) 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, static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst this_val,
...@@ -493,8 +508,10 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = { ...@@ -493,8 +508,10 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
JS_CFUNC_DEF("reboot", 0, js_mavsdk_reboot ), JS_CFUNC_DEF("reboot", 0, js_mavsdk_reboot ),
JS_CFUNC_DEF("healthAllOk", 0, js_mavsdk_healthAllOk ), JS_CFUNC_DEF("healthAllOk", 0, js_mavsdk_healthAllOk ),
JS_CFUNC_DEF("landed", 0, js_mavsdk_landed ), 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("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("setAltitude", 1, js_mavsdk_setAltitude ),
JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ), JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
JS_CFUNC_DEF("loiter", 0, js_mavsdk_loiter ), JS_CFUNC_DEF("loiter", 0, js_mavsdk_loiter ),
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment