Skip to content

Q
qjs-wrapper
Commit 460a8cf2 authored by Thomas Gambier's avatar Thomas Gambier 🚴🏼
Browse files
Options

Comply with simulator API 

See merge request !3
parents 3ea4a41c 598b5b79
Hide whitespace changes
Inline Side-by-side
Showing with 440 additions and 402 deletions
Makefile
View file @ 460a8cf2
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/dronedge.h
View file @ 460a8cf2
......@@ -5,11 +5,23 @@
#include "mavsdk_wrapper.h"
#include "pubsub.h"
struct messageNode {
char *message;
struct messageNode *next;
};
typedef struct {
struct messageNode *head;
struct messageNode *tail;
} MessageQueue;
UA_Double latitude = 0;
UA_Double longitude = 0;
UA_Float altitude_abs = 0;
UA_Float altitude_rel = 0;
UA_UInt32 last_checkpoint = 0;
UA_String message = {
.length = 0,
.data = NULL,
};
VariableData droneVariableArray[] = {
{
......@@ -45,12 +57,12 @@ VariableData droneVariableArray[] = {
.getter.getFloat = mavsdk_getAltitudeRel,
},
{
.name = "last_checkpoint",
.description = "Last checkpoint flown over",
.value = &last_checkpoint,
.type = UA_TYPES_UINT32,
.builtInType = UA_NS0ID_UINT32,
.getter.getUint32 = getLastCheckpoint
.name = "message",
.description = "Message to send to the other drones",
.value = &message,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.getter.getString = get_message
},
};
......
include/mavsdk_wrapper.h
View file @ 460a8cf2
......@@ -12,20 +12,17 @@ int mavsdk_reboot(void);
// Flight state management functions
int mavsdk_arm(void);
int mavsdk_doParachute(float param);
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_setTargetAltitude(float a);
int mavsdk_setTargetCoordinates(double la, double lo, float a, float y);
int mavsdk_setTargetCoordinatesXYZ(double x, double y, float z);
int mavsdk_setTargetLatLong(double la, double lo);
int mavsdk_setTargetCoordinates(double la, double lo, float a);
int mavsdk_setManualControlInput(void);
// Information functions
float mavsdk_getAltitude(void);
......@@ -35,15 +32,10 @@ double mavsdk_getInitialLatitude(void);
double mavsdk_getInitialLongitude(void);
double mavsdk_getLatitude(void);
double mavsdk_getLongitude(void);
uint64_t mavsdk_getTimestamp(void);
double mavsdk_getTakeOffAltitude(void);
float mavsdk_getPitch(void);
float mavsdk_getRoll(void);
float mavsdk_getYaw(void);
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
}
......
include/pubsub.h
View file @ 460a8cf2
......@@ -13,6 +13,8 @@
#define DATA_SET_WRITER_ID 1
#define WRITER_GROUP_ID 1
#define MAX_MESSAGE_SIZE 1024
typedef struct {
UA_UInt16 id;
UA_Double latitude;
......@@ -23,8 +25,8 @@ typedef struct {
UA_UInt32 altitudeAbsId;
UA_Float altitudeRel;
UA_UInt32 altitudeRelId;
UA_UInt32 lastCheckpoint;
UA_UInt32 lastCheckpointId;
char message[MAX_MESSAGE_SIZE];
UA_UInt32 messageId;
} JSDroneData;
typedef struct {
......@@ -34,10 +36,11 @@ typedef struct {
int type;
UA_Byte builtInType;
union {
UA_UInt32 (*getUint32)(void);
UA_Float (*getFloat)(void);
UA_Double (*getDouble)(void);
UA_DateTime (*getTimestamp)(void);
UA_Double (*getDouble)(void);
UA_Float (*getFloat)(void);
UA_String (*getString)(void);
UA_UInt32 (*getUint32)(void);
} getter;
} VariableData;
......@@ -45,7 +48,7 @@ typedef struct {
int subscribeOnly(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*),
......@@ -54,14 +57,14 @@ int subscribeOnly(UA_String *transportProfile,
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running);
UA_UInt32 getLastCheckpoint(void);
UA_String get_message(void);
UA_UInt16 get_drone_id(UA_UInt32 nb);
......@@ -73,8 +76,6 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var);
VariableData pubsub_get_value(UA_String identifier);
void stop_pubsub(void);
DLL_PUBLIC JSModuleDef *js_init_module(JSContext *ctx, const char *module_name);
#endif /* __PUBSUB_H__ */
mavsdk_wrapper.cpp
View file @ 460a8cf2
......@@ -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,24 +27,22 @@ 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 Telemetry::FlightMode flight_mode;
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,9 +88,14 @@ 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;
float abs_altitude;
log_file_fd.open(log_file);
connection_result = _mavsdk.add_any_connection(connection_url);
......@@ -123,49 +127,31 @@ 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...");
// Subscribe to receive updates on flight mode. You can find out whether FollowMe is active.
telemetry->subscribe_flight_mode([](Telemetry::FlightMode _flight_mode) {
if(flight_mode != _flight_mode) {
flight_mode = _flight_mode;
}
});
log("Subscribing to Euler angle...");
telemetry->subscribe_attitude_euler([](Telemetry::EulerAngle euler_angle) {
angle = euler_angle;
});
log("Subscribing to Fixedwing Metrics...");
telemetry->subscribe_fixedwing_metrics([](Telemetry::FixedwingMetrics m) {
metric = m;
});
log("Subscribing to position...");
telemetry->subscribe_position([](Telemetry::Position pos) {
timestamp_us = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()
).count();
log("Subscribing to raw GPS...");
telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
std::ostringstream oss;
position = pos;
oss << mavsdk_getTimestamp() << "; "
<< mavsdk_getLatitude() << "; " << mavsdk_getLongitude() << "; "
<< mavsdk_getAltitude() << "; " << mavsdk_getAltitudeRel() << "; "
<< mavsdk_getPitch() << "; " << mavsdk_getRoll() << "; " << mavsdk_getYaw() << "; "
<< mavsdk_getAirspeed() << "; " << mavsdk_getThrottle() << "; " << mavsdk_getClimbRate();
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;
log(oss.str());
});
while(isnan(position.absolute_altitude_m)) {
do {
log("Waiting for first telemetry");
sleep_for(seconds(1));
}
origin = position;
xy_ratio = std::cos((M_PI * origin.latitude_deg) / 180.F);
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;
......@@ -174,20 +160,20 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
}
int mavsdk_stop() {
if(!mavsdk_started)
if (!mavsdk_started)
return -1;
if(!mavsdk_landed()) {
if (!mavsdk_landed()) {
log_error("You must land first !");
return -1;
}
if(doAction(&Action::shutdown, "Shutdown failed")) {
if (doAction(&Action::shutdown, "Shutdown failed"))
return -1;
}
// Delete pointers
delete action;
delete manual_control;
delete mavlink_passthrough;
delete telemetry;
log_file_fd.close();
......@@ -214,101 +200,87 @@ int mavsdk_arm(void) {
return doAction(&Action::arm, "Arming failed");
}
int mavsdk_doParachute(float param) {
if(!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_PARACHUTE;
command.param1 = param; //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 failed");
}
int mavsdk_land(void) {
log("Landing...");
if(doAction(&Action::terminate, "Land failed")) {
if (doAction(&Action::terminate, "Land failed"))
return -1;
}
// Check if vehicle is still in air
while(telemetry->in_air()) {
while (telemetry->in_air()) {
log("Vehicle is landing...");
sleep_for(seconds(1));
}
log("Landed!");
while(telemetry->armed()) {
while (telemetry->armed())
sleep_for(seconds(1));
}
log("Finished...");
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")) {
if (doAction(&Action::takeoff, "Takeoff failed"))
return -1;
}
while(flight_mode != Telemetry::FlightMode::Takeoff) {
while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
}
log("Taking off...");
return 0;
}
int mavsdk_takeOffAndWait(void) {
if(mavsdk_takeOff() < 0) {
if (mavsdk_takeOff() < 0)
return -1;
}
while(flight_mode == Telemetry::FlightMode::Takeoff) {
while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
}
return 0;
}
int mavsdk_triggerParachute(void) {
if (!mavsdk_started)
return -1;
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");
}
// 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) {
if(!mavsdk_started)
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 2 | 4 | 8;
command.param3 = airspeed;
command.param4 = airspeed;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
......@@ -316,7 +288,7 @@ int mavsdk_setAirspeed(float airspeed) {
}
int mavsdk_setAltitude(float altitude) {
if(!mavsdk_started)
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
......@@ -330,56 +302,79 @@ int mavsdk_setAltitude(float altitude) {
return doMavlinkCommand(command, "Setting altitude failed");
}
int mavsdk_setTargetAltitude(float a) {
if(!mavsdk_started)
return -1;
log("Going to altitude " + std::to_string(a) + "m");
const Action::Result result = action->goto_location(position.latitude_deg,
position.longitude_deg,
a, 0);
if (result != Action::Result::Success) {
log_error_from_result("Goto location failed", result);
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;
}
int mavsdk_setTargetCoordinates(double la, double lo, float a, float y) {
if(!mavsdk_started)
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_setTargetCoordinatesXYZ(double x, double y, float z) {
double la, lo;
la = ((origin.latitude_deg + y / EARTH_RADIUS) * 180.F) / M_PI;
lo = ((origin.longitude_deg + x / (EARTH_RADIUS * xy_ratio)) * 180.F) / M_PI;
int mavsdk_setManualControlInput(void) {
if (autocontinue)
return setManualControlInput(0, 0, 0, 0);
return mavsdk_setTargetCoordinates(la, lo, z, 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;
}
int mavsdk_setTargetLatLong(double la, double lo) {
return mavsdk_setTargetCoordinates(la, lo, mavsdk_getAltitude(), 0);
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) {
return position.absolute_altitude_m;
return telemetry->position().absolute_altitude_m;
}
float mavsdk_getAltitudeRel(void) {
return position.relative_altitude_m;
return telemetry->position().relative_altitude_m;
}
float mavsdk_getInitialAltitude(void) {
......@@ -395,55 +390,35 @@ double mavsdk_getInitialLongitude(void) {
}
double mavsdk_getLatitude(void) {
return position.latitude_deg;
return telemetry->position().latitude_deg;
}
double mavsdk_getLongitude(void) {
return position.longitude_deg;
}
uint64_t mavsdk_getTimestamp(void) {
return timestamp_us;
return telemetry->position().longitude_deg;
}
double mavsdk_getTakeOffAltitude(void) {
const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
if(response.first != Action::Result::Success) {
if (response.first != Action::Result::Success) {
log_error_from_result("Get takeoff altitude failed", response.first);
return -1;
}
return response.second;
}
float mavsdk_getPitch(void) {
return angle.pitch_deg;
}
float mavsdk_getRoll(void) {
return angle.roll_deg;
}
float mavsdk_getYaw(void) {
return angle.yaw_deg;
}
float mavsdk_getAirspeed(void) {
return metric.airspeed_m_s;
}
float mavsdk_getClimbRate(void) {
return metric.climb_rate_m_s;
}
float mavsdk_getThrottle(void) {
return metric.throttle_percentage;
return telemetry->attitude_euler().yaw_deg;
}
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();
}
pubsub.c
View file @ 460a8cf2
......@@ -135,13 +135,13 @@ addDataSetField(UA_Server *server, VariableData varDetails) {
* The WriterGroup (WG) is part of the connection and contains the primary
* configuration parameters for the message creation. */
static void
addWriterGroup(UA_Server *server) {
addWriterGroup(UA_Server *server, UA_Duration publishingInterval) {
/* Now we create a new WriterGroupConfig and add the group to the existing
* PubSubConnection. */
UA_WriterGroupConfig writerGroupConfig;
memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
writerGroupConfig.name = UA_STRING("Demo WriterGroup");
writerGroupConfig.publishingInterval = 100;
writerGroupConfig.publishingInterval = publishingInterval;
writerGroupConfig.enabled = UA_FALSE;
writerGroupConfig.writerGroupId = WRITER_GROUP_ID;
writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
......@@ -245,7 +245,8 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
* Set SubscribedDataSet type to TargetVariables data type.
* Add subscribedvariables to the DataSetReader */
static UA_StatusCode
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, UA_UInt32 nb,
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_UInt32 nb, UA_Duration samplingInterval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
......@@ -302,6 +303,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, UA_UInt32 n
/*monitor variable*/
UA_MonitoredItemCreateRequest monRequest = UA_MonitoredItemCreateRequest_default(newNode);
init_node_id(newNode.identifier.numeric, nb, i);
monRequest.requestedParameters.samplingInterval = samplingInterval;
UA_Server_createDataChangeMonitoredItem(server, UA_TIMESTAMPSTORETURN_SOURCE,
monRequest, NULL, dataChangeNotificationCallback);
......@@ -375,7 +377,7 @@ setServer(UA_String *transportProfile,
static UA_StatusCode
subscribe(UA_Server *server,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*)) {
......@@ -405,7 +407,7 @@ subscribe(UA_Server *server,
return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */
retval = addSubscribedVariables(server, readerIdent, i, init_node_id);
retval = addSubscribedVariables(server, readerIdent, i, interval, init_node_id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
......@@ -416,7 +418,7 @@ subscribe(UA_Server *server,
int subscribeOnly(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*),
......@@ -426,8 +428,8 @@ int subscribeOnly(UA_String *transportProfile,
server = setServer(transportProfile, networkAddressUrl, id);
subscribe(server, variableArray, nbVariable, id, nbReader, init_node_id,
get_reader_id, update);
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
......@@ -437,7 +439,7 @@ int subscribeOnly(UA_String *transportProfile,
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
......@@ -460,15 +462,15 @@ int runPubsub(UA_String *transportProfile,
addDataSetField(server, variableArray[i]);
}
addWriterGroup(server);
addWriterGroup(server, interval);
retval = addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Subscribing */
subscribe(server, variableArray, nbVariable, id, nbReader, init_node_id,
get_reader_id, update);
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
......
qjs_wrapper.c
View file @ 460a8cf2
#include <pthread.h>
#include "dronedge.h"
static JSClassID js_drone_class_id;
static JSClassID jsDroneClassId;
static UA_Boolean pubsub_running = true;
static UA_Boolean pubsubRunning = true;
static UA_UInt32 nbDrone;
static JSValueConst *drone_object_id_list;
static JSValueConst *droneObjectIdList;
static MessageQueue messageQueue = {
.head = NULL,
.tail = NULL,
};
UA_String currentMessage;
pthread_mutex_t mutex;
pthread_cond_t threadCond;
static UA_UInt32 lastCheckpoint = 0;
// Drone class functions
static void js_drone_finalizer(JSRuntime *rt, JSValue val)
{
JSDroneData *s = (JSDroneData *) JS_GetOpaque(val, js_drone_class_id);
JSDroneData *s = (JSDroneData *) JS_GetOpaque(val, jsDroneClassId);
js_free_rt(rt, s);
}
static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target,
static JSValue js_drone_ctor(JSContext *ctx, JSValueConst newTarget,
int argc, JSValueConst *argv)
{
JSDroneData *s;
......@@ -30,10 +39,10 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target,
goto fail;
s->id = (uint16_t)uint32;
proto = JS_GetPropertyStr(ctx, new_target, "prototype");
proto = JS_GetPropertyStr(ctx, newTarget, "prototype");
if (JS_IsException(proto))
goto fail;
obj = JS_NewObjectProtoClass(ctx, proto, js_drone_class_id);
obj = JS_NewObjectProtoClass(ctx, proto, jsDroneClassId);
JS_FreeValue(ctx, proto);
if (JS_IsException(obj))
goto fail;
......@@ -45,22 +54,23 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target,
return JS_EXCEPTION;
}
static JSValue js_drone_init(JSContext *ctx, JSValueConst this_val,
static JSValue js_drone_init(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
int nb;
JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, this_val, js_drone_class_id);
JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, thisVal, jsDroneClassId);
if (!s)
return JS_EXCEPTION;
if (JS_ToInt32(ctx, &nb, argv[0]))
return JS_EXCEPTION;
drone_object_id_list[nb] = this_val;
droneObjectIdList[nb] = thisVal;
return JS_UNDEFINED;
}
static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
{
JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, this_val, js_drone_class_id);
JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, thisVal, jsDroneClassId);
JSValue res;
if (!s)
return JS_EXCEPTION;
switch(magic) {
......@@ -75,28 +85,78 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
case 4:
return JS_NewFloat64(ctx, s->altitudeRel);
case 5:
return JS_NewUint32(ctx, s->lastCheckpoint);
pthread_mutex_lock(&mutex);
res = JS_NewString(ctx, s->message);
strcpy(s->message, "");
pthread_cond_signal(&threadCond);
pthread_mutex_unlock(&mutex);
return res;
default:
return JS_EXCEPTION;
}
}
static JSValue js_drone_set_checkpoint(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
uint32_t uint32;
if (JS_ToUint32(ctx, &uint32, argv[0]))
struct messageNode *newNode;
const char *message;
message = JS_ToCString(ctx, argv[0]);
if (strlen(message) > MAX_MESSAGE_SIZE) {
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Message too long");
return JS_EXCEPTION;
lastCheckpoint = (uint16_t)uint32;
}
newNode = (struct messageNode*)malloc(sizeof(struct messageNode));
newNode->message = strdup(message);
newNode->next = NULL;
if (messageQueue.tail == NULL) {
messageQueue.head = messageQueue.tail = newNode;
} else {
messageQueue.tail->next = newNode;
messageQueue.tail = newNode;
}
JS_FreeCString(ctx, message);
return JS_UNDEFINED;
}
UA_UInt32 getLastCheckpoint(void)
static void delete_message_node(struct messageNode *node) {
free(node->message);
free(node);
}
static UA_Boolean UA_String_isEmpty(const UA_String *s) {
return (s->length == 0 || s->data == NULL);
}
static void clear_message(UA_String message) {
if (!UA_String_isEmpty(&message))
UA_String_clear(&message);
}
UA_String get_message(void)
{
return lastCheckpoint;
struct messageNode *current;
current = messageQueue.head;
if (current == NULL) {
if (!UA_String_isEmpty(&currentMessage)) {
UA_String_clear(&currentMessage);
currentMessage = UA_STRING("");
}
} else {
clear_message(currentMessage);
currentMessage = UA_STRING_ALLOC(current->message);
messageQueue.head = current->next == NULL ? (messageQueue.tail = NULL) : current->next;
delete_message_node(current);
}
return currentMessage;
}
static JSClassDef js_drone_class = {
static JSClassDef jsDroneClass = {
"Drone",
.finalizer = js_drone_finalizer,
};
......@@ -107,20 +167,17 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
JS_CGETSET_MAGIC_DEF("longitude", js_drone_get, NULL, 2),
JS_CGETSET_MAGIC_DEF("altitudeAbs", js_drone_get, NULL, 3),
JS_CGETSET_MAGIC_DEF("altitudeRel", js_drone_get, NULL, 4),
JS_CGETSET_MAGIC_DEF("lastCheckpoint", js_drone_get, NULL, 5),
JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 5),
JS_CFUNC_DEF("init", 1, js_drone_init),
};
// pubsub functions
UA_UInt16 get_drone_id(UA_UInt32 nb) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
return s->id;
}
void stop_pubsub(void) {
pubsub_running = false;
free(drone_object_id_list);
}
VariableData pubsub_get_value(UA_String identifier) {
VariableData var_details;
UA_String name;
......@@ -143,6 +200,9 @@ VariableData pubsub_get_value(UA_String identifier) {
case UA_TYPES_DATETIME:
*(UA_DateTime*)var_details.value = droneVariableArray[i].getter.getTimestamp();
break;
case UA_TYPES_STRING:
*(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
break;
......@@ -155,7 +215,7 @@ VariableData pubsub_get_value(UA_String identifier) {
}
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
s->latitudeId = id;
......@@ -170,7 +230,7 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
s->altitudeRelId = id;
break;
case 4:
s->lastCheckpointId = id;
s->messageId = id;
break;
default:
break;
......@@ -180,8 +240,9 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
{
JSDroneData *s;
UA_String uaStr;;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[i], js_drone_class_id);
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->latitudeId == id) {
s->latitude = *(UA_Double*) var->value.data;
return;
......@@ -194,8 +255,14 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
} else if (s->altitudeRelId == id) {
s->altitudeRel = *(UA_Float*) var->value.data;
return;
} else if (s->lastCheckpointId == id) {
s->lastCheckpoint = *(UA_UInt32*) var->value.data;
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
pthread_mutex_lock(&mutex);
while(strlen(s->message) != 0)
pthread_cond_wait(&threadCond, &mutex);
memcpy(s->message, uaStr.data, uaStr.length);
s->message[uaStr.length] = '\0';
pthread_mutex_unlock(&mutex);
return;
}
}
......@@ -205,8 +272,9 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
{
JSDroneData *s;
UA_String uaStr;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[i], js_drone_class_id);
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->latitudeId == id) {
s->latitude = *(UA_Double*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received latitude of drone %d: %f°", s->id, s->latitude);
......@@ -223,9 +291,11 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
s->altitudeRel = *(UA_Float*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received relative altitude of drone %d: %fm", s->id, s->altitudeRel);
return;
} else if (s->lastCheckpointId == id) {
s->lastCheckpoint = *(UA_UInt32*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received checkpoint of drone %d: %dm", s->id, s->lastCheckpoint);
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
memcpy(s->message, uaStr.data, uaStr.length);
s->message[uaStr.length] = '\0';
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received message of drone %d: %s", s->id, s->message);
return;
}
}
......@@ -237,73 +307,90 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
* arg 1 (string): port used for multicast communication
* arg 2 (string): network interface used for multicast communication
* arg 3 (int): ID of the drone
* arg 4 (bool): true if there will be data to publish
* arg 4 (double): publication/subscription interval in ms
* arg 5 (bool): true if there will be data to publish
*/
static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
const char *ipv6;
const char *port;
const char *net_iface;
char *not_const_net_iface;
const char *netIface;
char *notConstNetIface;
char urlBuffer[44];
UA_UInt32 id;
UA_Duration interval;
int res;
ipv6 = JS_ToCString(ctx, argv[0]);
port = JS_ToCString(ctx, argv[1]);
net_iface = JS_ToCString(ctx, argv[2]);
netIface = JS_ToCString(ctx, argv[2]);
UA_snprintf(urlBuffer, sizeof(urlBuffer), "opc.udp://[%s]:%s/", ipv6, port);
UA_String transportProfile =
UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-udp-uadp");
UA_NetworkAddressUrlDataType networkAddressUrl;
not_const_net_iface = strdup(net_iface);
networkAddressUrl.networkInterface = UA_STRING(not_const_net_iface);
notConstNetIface = strdup(netIface);
networkAddressUrl.networkInterface = UA_STRING(notConstNetIface);
networkAddressUrl.url = UA_STRING(urlBuffer);
if (JS_ToUint32(ctx, &id, argv[3]))
return JS_EXCEPTION;
if (JS_ToBool(ctx, argv[4])) {
if (JS_ToFloat64(ctx, &interval, argv[4]))
return JS_EXCEPTION;
if (JS_ToBool(ctx, argv[5])) {
res = runPubsub(&transportProfile, &networkAddressUrl,
droneVariableArray, countof(droneVariableArray), id,
nbDrone, init_node_id, get_drone_id, pubsub_get_value,
pubsub_update_coordinates, &pubsub_running);
nbDrone, interval, init_node_id, get_drone_id,
pubsub_get_value, pubsub_update_coordinates,
&pubsubRunning);
} else {
res = subscribeOnly(&transportProfile, &networkAddressUrl,
droneVariableArray, countof(droneVariableArray), id,
nbDrone, init_node_id, get_drone_id,
pubsub_print_coordinates, &pubsub_running);
nbDrone, interval, init_node_id, get_drone_id,
pubsub_print_coordinates, &pubsubRunning);
}
JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port);
free(not_const_net_iface);
JS_FreeCString(ctx, net_iface);
free(notConstNetIface);
JS_FreeCString(ctx, netIface);
return JS_NewInt32(ctx, res);
}
static JSValue js_init_pubsub(JSContext *ctx, JSValueConst this_val,
static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
if (JS_ToUint32(ctx, &nbDrone, argv[0]))
return JS_EXCEPTION;
drone_object_id_list = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
droneObjectIdList = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0);
}
static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst this_val,
static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
pubsub_running = false;
free(drone_object_id_list);
struct messageNode *current;
pubsubRunning = false;
free(droneObjectIdList);
while (messageQueue.head != NULL) {
current = messageQueue.head;
messageQueue.head = current->next;
delete_message_node(current);
}
clear_message(currentMessage);
return JS_NewInt32(ctx, 0);
}
static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst this_val,
// Connexion management functions
static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
const char *url;
......@@ -323,29 +410,19 @@ static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst this_val,
return JS_NewInt32(ctx, res);
}
static JSValue js_mavsdk_stop(JSContext *ctx, JSValueConst this_val,
static JSValue js_mavsdk_stop(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_stop());
}
static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst this_val,
static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_reboot());
}
static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewBool(ctx, mavsdk_healthAllOk());
}
static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewBool(ctx, mavsdk_landed());
}
// Flight state management functions
static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
......@@ -353,46 +430,45 @@ static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
return JS_NewInt32(ctx, mavsdk_arm());
}
static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_land(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double la_arg_double;
double lo_arg_double;
double a_arg_double;
double y_arg_double;
return JS_NewInt32(ctx, mavsdk_land());
}
if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
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;
static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_takeOff());
}
return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
lo_arg_double,
(float)a_arg_double,
(float)y_arg_double));
static JSValue js_mavsdk_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_takeOffAndWait());
}
static JSValue js_mavsdk_setTargetLatLong(JSContext *ctx, JSValueConst this_val,
static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double la_arg_double;
double lo_arg_double;
return JS_NewInt32(ctx, mavsdk_triggerParachute());
}
if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
return JS_EXCEPTION;
// Flight management functions
static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double radius = 100;
if (argc > 0) {
if (JS_ToFloat64(ctx, &radius, argv[0]))
return JS_EXCEPTION;
}
return JS_NewInt32(ctx, mavsdk_setTargetLatLong(la_arg_double,
lo_arg_double));
return JS_NewInt32(ctx, mavsdk_loiter((float)radius));
}
static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst this_val,
static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double altitude;
......@@ -400,10 +476,10 @@ static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst this_val,
if (JS_ToFloat64(ctx, &altitude, argv[0]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setAltitude((float)altitude));
return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
}
static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst this_val,
static JSValue js_mavsdk_setAltitude(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double altitude;
......@@ -411,183 +487,163 @@ static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst this_val,
if (JS_ToFloat64(ctx, &altitude, argv[0]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
return JS_NewInt32(ctx, mavsdk_setAltitude((float)altitude));
}
static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getYaw());
}
double la_arg_double;
double lo_arg_double;
double a_arg_double;
static JSValue js_mavsdk_getInitialLatitude(JSContext *ctx,
JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getInitialLatitude());
}
if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
return JS_EXCEPTION;
static JSValue js_mavsdk_getLatitude(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getLatitude());
return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
lo_arg_double,
(float)a_arg_double));
}
static JSValue js_mavsdk_getInitialLongitude(JSContext *ctx,
JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_setManualControlInput(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getInitialLongitude());
return JS_NewInt32(ctx, mavsdk_setManualControlInput());
}
static JSValue js_mavsdk_getLongitude(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
// Information functions
static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getLongitude());
return JS_NewFloat64(ctx, mavsdk_getAltitude());
}
static JSValue js_mavsdk_getTakeOffAltitude(JSContext *ctx,
JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getTakeOffAltitude());
return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
}
static JSValue js_mavsdk_getInitialAltitude(JSContext *ctx,
JSValueConst this_val,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getInitialAltitude());
}
static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getInitialLatitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getAltitude());
return JS_NewFloat64(ctx, mavsdk_getInitialLatitude());
}
static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getInitialLongitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
return JS_NewFloat64(ctx, mavsdk_getInitialLongitude());
}
static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getLatitude(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_loiter());
return JS_NewFloat64(ctx, mavsdk_getLatitude());
}
static JSValue js_mavsdk_doParachute(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getLongitude(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double param;
if (JS_ToFloat64(ctx, &param, argv[0]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_doParachute((float)param));
return JS_NewFloat64(ctx, mavsdk_getLongitude());
}
static JSValue js_mavsdk_setTargetCoordinatesXYZ(JSContext *ctx,
JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getTakeOffAltitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double x_arg_double;
double y_arg_double;
double z_arg_double;
if (JS_ToFloat64(ctx, &x_arg_double, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &y_arg_double, argv[1]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &z_arg_double, argv[2]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setTargetCoordinatesXYZ(x_arg_double,
y_arg_double,
(float)z_arg_double));
return JS_NewFloat64(ctx, mavsdk_getTakeOffAltitude());
}
static JSValue js_mavsdk_setTargetAltitude(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double a_arg_double;
if (JS_ToFloat64(ctx, &a_arg_double, argv[0]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setTargetAltitude((float)a_arg_double));
return JS_NewFloat64(ctx, mavsdk_getYaw());
}
static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_takeOff());
return JS_NewBool(ctx, mavsdk_healthAllOk());
}
static JSValue js_mavsdk_takeOffAndWait(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_isInManualMode(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_takeOffAndWait());
return JS_NewBool(ctx, mavsdk_isInManualMode());
}
static JSValue js_mavsdk_land(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_land());
return JS_NewBool(ctx, mavsdk_landed());
}
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("stop", 0, js_mavsdk_stop ),
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("arm", 0, js_mavsdk_arm ),
JS_CFUNC_DEF("setTargetCoordinates", 4, js_mavsdk_setTargetCoordinates ),
JS_CFUNC_DEF("setTargetLatLong", 2, js_mavsdk_setTargetLatLong ),
JS_CFUNC_DEF("setAltitude", 1, js_mavsdk_setAltitude ),
JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
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", 0, js_mavsdk_loiter ),
JS_CFUNC_DEF("doParachute", 1, js_mavsdk_doParachute ),
JS_CFUNC_DEF("setTargetCoordinatesXYZ", 3, js_mavsdk_setTargetCoordinatesXYZ ),
JS_CFUNC_DEF("setTargetAltitude", 1, js_mavsdk_setTargetAltitude ),
JS_CFUNC_DEF("getYaw", 0, js_mavsdk_getYaw ),
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 ),
JS_CFUNC_DEF("getInitialLatitude", 0, js_mavsdk_getInitialLatitude ),
JS_CFUNC_DEF("getLatitude", 0, js_mavsdk_getLatitude ),
JS_CFUNC_DEF("getInitialLongitude", 0, js_mavsdk_getInitialLongitude ),
JS_CFUNC_DEF("getLatitude", 0, js_mavsdk_getLatitude ),
JS_CFUNC_DEF("getLongitude", 0, js_mavsdk_getLongitude ),
JS_CFUNC_DEF("getTakeOffAltitude", 0, js_mavsdk_getTakeOffAltitude ),
JS_CFUNC_DEF("getInitialAltitude", 0, js_mavsdk_getInitialAltitude ),
JS_CFUNC_DEF("getAltitude", 0, js_mavsdk_getAltitude ),
JS_CFUNC_DEF("getAltitudeRel", 0, js_mavsdk_getAltitudeRel ),
JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
JS_CFUNC_DEF("getYaw", 0, js_mavsdk_getYaw ),
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 ),
JS_CFUNC_DEF("runPubsub", 5, js_run_pubsub ),
JS_CFUNC_DEF("setCheckpoint", 1, js_drone_set_checkpoint ),
JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
};
static int js_mavsdk_init(JSContext *ctx, JSModuleDef *m)
{
JSValue drone_proto, drone_class;
JSValue droneProto, droneClass;
JS_NewClassID(&js_drone_class_id);
JS_NewClass(JS_GetRuntime(ctx), js_drone_class_id, &js_drone_class);
JS_NewClassID(&jsDroneClassId);
JS_NewClass(JS_GetRuntime(ctx), jsDroneClassId, &jsDroneClass);
drone_proto = JS_NewObject(ctx);
JS_SetPropertyFunctionList(ctx, drone_proto, js_drone_proto_funcs,
droneProto = JS_NewObject(ctx);
JS_SetPropertyFunctionList(ctx, droneProto, js_drone_proto_funcs,
countof(js_drone_proto_funcs));
drone_class = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
droneClass = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
JS_CFUNC_constructor, 0);
JS_SetConstructor(ctx, drone_class, drone_proto);
JS_SetClassProto(ctx, js_drone_class_id, drone_proto);
JS_SetConstructor(ctx, droneClass, droneProto);
JS_SetClassProto(ctx, jsDroneClassId, droneProto);
JS_SetModuleExport(ctx, m, "Drone", drone_class);
JS_SetModuleExport(ctx, m, "Drone", droneClass);
return JS_SetModuleExportList(ctx, m, js_mavsdk_funcs,
countof(js_mavsdk_funcs));
......
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