Skip to content

Q
qjs-wrapper
Commit 27ae36e4 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾
Browse files
Options

Format logging and stop manual control 

See merge request !7
parents 6dd2502f ba7bc987
Show whitespace changes
Inline Side-by-side
Showing with 108 additions and 199 deletions
Makefile
View file @ 27ae36e4
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
View file @ 27ae36e4
......@@ -20,13 +20,12 @@ extern "C" {
#endif
#include <stdint.h>
// Connexion management functions
int mavsdk_start(const char * url, const char * log_file, int timeout);
int mavsdk_start(const char * ip, int port, const char * log_file, int timeout);
int mavsdk_stop(void);
int mavsdk_reboot(void);
// Flight state management functions
int mavsdk_arm(void);
int mavsdk_land(void);
int mavsdk_takeOff(void);
int mavsdk_takeOffAndWait(void);
int mavsdk_triggerParachute(void);
......@@ -34,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);
......@@ -53,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
View file @ 27ae36e4
......@@ -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,18 @@ 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 bool autocontinue = false;
static double initialBearing;
static double previousBearing;
static double xy_ratio;
static double target_latitude;
static double target_longitude;
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
......@@ -101,7 +92,9 @@ static double toDeg(double angle) {
return 180 * angle / M_PI;
}
int mavsdk_start(const char * url, const char * log_file, int timeout) {
int mavsdk_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;
......@@ -136,23 +129,35 @@ 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) {
if (takingOff && flight_mode != Telemetry::FlightMode::Takeoff) {
log("Subscribing to raw GPS...");
telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
std::ostringstream oss;
Telemetry::EulerAngle euler_angle = telemetry->attitude_euler();
Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
Telemetry::Position position = telemetry->position();
oss << gps.timestamp_us << "; "
<< gps.latitude_deg << "; " << gps.longitude_deg << "; "
<< gps.absolute_altitude_m << "; " << position.relative_altitude_m << "; "
<< euler_angle.pitch_deg << "; " << euler_angle.roll_deg << "; " << euler_angle.yaw_deg << "; "
<< metrics.airspeed_m_s << "; " << metrics.throttle_percentage << "; " << metrics.climb_rate_m_s;
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");
......@@ -160,11 +165,10 @@ 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;
log("timestamp; latitude; longitude; AMSL (m); rel altitude (m); pitch (°); roll(°); yaw(°); air speed (m/s); throttle(%); climb rate(m/s)");
log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);ground speed (m/s);climb rate (m/s)");
return 0;
}
......@@ -182,7 +186,6 @@ int mavsdk_stop() {
// Delete pointers
delete action;
delete manual_control;
delete mavlink_passthrough;
delete telemetry;
log_file_fd.close();
......@@ -209,33 +212,10 @@ int mavsdk_arm(void) {
return doAction(&Action::arm, "Arming failed");
}
int mavsdk_land(void) {
log("Landing...");
if (doAction(&Action::terminate, "Land failed"))
return -1;
// Check if vehicle is still in air
while (telemetry->in_air()) {
log("Vehicle is landing...");
sleep_for(seconds(1));
}
log("Landed!");
while (telemetry->armed())
sleep_for(seconds(1));
log("Finished...");
return 0;
}
int mavsdk_takeOff(void) {
if (doAction(&Action::takeoff, "Takeoff failed"))
return -1;
while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
log("Taking off...");
return 0;
}
......@@ -243,6 +223,8 @@ int mavsdk_takeOffAndWait(void) {
if (mavsdk_takeOff() < 0)
return -1;
while (!takingOff)
sleep_for(seconds(1));
while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
return 0;
......@@ -252,35 +234,60 @@ int mavsdk_triggerParachute(void) {
if (!mavsdk_started)
return -1;
log("Landing...");
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_PARACHUTE;
command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Parachute release failed");
if (doMavlinkCommand(command, "Parachute release failed"))
return -1;
// Check if vehicule is still in air
while (telemetry->in_air()) {
log("Vehicle is landing...");
sleep_for(seconds(1));
}
log("Landed!");
while (telemetry->armed())
sleep_for(seconds(1));
log("Finished...");
log_file_fd << std::flush;
return 0;
}
// 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;
......@@ -296,7 +303,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;
......@@ -311,106 +318,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
......@@ -488,10 +436,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
View file @ 27ae36e4
......@@ -405,18 +405,21 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
const char *url;
const char *ip;
const char *log_file;
int port;
int timeout;
int res;
url = JS_ToCString(ctx, argv[0]);
log_file = JS_ToCString(ctx, argv[1]);
if (JS_ToInt32(ctx, &timeout, argv[2]))
ip = JS_ToCString(ctx, argv[0]);
if (JS_ToInt32(ctx, &port, argv[1]))
return JS_EXCEPTION;
log_file = JS_ToCString(ctx, argv[2]);
if (JS_ToInt32(ctx, &timeout, argv[3]))
return JS_EXCEPTION;
res = mavsdk_start(url, log_file, timeout);
JS_FreeCString(ctx, url);
res = mavsdk_start(ip, port, log_file, timeout);
JS_FreeCString(ctx, ip);
JS_FreeCString(ctx, log_file);
return JS_NewInt32(ctx, res);
......@@ -442,12 +445,6 @@ static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
return JS_NewInt32(ctx, mavsdk_arm());
}
static JSValue js_mavsdk_land(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_land());
}
static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
......@@ -489,17 +486,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)
......@@ -520,13 +506,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,
......@@ -605,12 +584,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)
{
......@@ -621,19 +594,16 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
JS_CFUNC_DEF("setMessage", 1, js_drone_set_message ),
JS_CFUNC_DEF("runPubsub", 6, js_run_pubsub ),
JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
JS_CFUNC_DEF("start", 3, js_mavsdk_start ),
JS_CFUNC_DEF("start", 4, js_mavsdk_start ),
JS_CFUNC_DEF("stop", 0, js_mavsdk_stop ),
JS_CFUNC_DEF("reboot", 0, js_mavsdk_reboot ),
JS_CFUNC_DEF("arm", 0, js_mavsdk_arm ),
JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
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 ),
......@@ -645,7 +615,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 ),
......
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
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7