Skip to content

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

Remove mavsdk dependence 

See merge request !8
parents 27ae36e4 20d651e2
Show whitespace changes
Inline Side-by-side
Showing with 536 additions and 740 deletions
Makefile
View file @ 19d6f938
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_mavlink_passthrough -lmavsdk_telemetry -lopen62541
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 -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
LIBS=-lautopilotwrapper -lopen62541
LIB_NAME := libqjswrapper.so
SRCS := mavsdk_wrapper.cpp pubsub.c qjs_wrapper.c
OBJS := mavsdk_wrapper.o pubsub.o qjs_wrapper.o
SRCS := pubsub.c qjs_wrapper.c
OBJS := pubsub.o qjs_wrapper.o
all: $(LIB_NAME)
......
README
View file @ 19d6f938
This project is holding the source code for QuickJS component that will wrap the functions from MAVSDK and open62541.
# qjs-wrapper
This project is holding the source code for QuickJS component that will wrap the functions from open62541 and a drone
control SDK (MAVSDK for example).
include/autopilot_wrapper.h 0 → 100644
View file @ 19d6f938
#ifndef __AUTOPILOT_H__
#define __AUTOPILOT_H__
#ifndef DLL_PUBLIC
#define DLL_PUBLIC __attribute__ ((visibility ("default")))
#endif
/*
* 0. latitude (double, degrees)
* 1. longitude (double, degrees)
* 2. absolute altitude (double, meters)
* 3. relative altitude (double, meters)
* 4. timestamp (double, milliseconds)
*/
#define POSITION_ARRAY_SIZE 5
/*
* 0. yaw angle (float, degrees)
* 1. air speed (float, m/s)
* 2. climb rate (float, m/s)
*/
#define SPEED_ARRAY_SIZE 3
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
// Connexion management functions
DLL_PUBLIC int start(const char * ip, int port, const char * log_file, int timeout);
DLL_PUBLIC int stop();
DLL_PUBLIC int reboot(void);
// Flight state management functions
DLL_PUBLIC int arm(void);
DLL_PUBLIC int takeOff(void);
DLL_PUBLIC int takeOffAndWait(void);
DLL_PUBLIC int triggerParachute(void);
// Flight management functions
DLL_PUBLIC void loiter(double la, double lo, float a, float radius);
DLL_PUBLIC void setAirSpeed_async(float airspeed);
DLL_PUBLIC void setTargetCoordinates(double la, double lo, float a);
// Information functions
DLL_PUBLIC float getAltitude(void);
DLL_PUBLIC float getInitialAltitude(void);
DLL_PUBLIC double getInitialLatitude(void);
DLL_PUBLIC double getInitialLongitude(void);
DLL_PUBLIC int64_t *getPositionArray(void);
DLL_PUBLIC float *getSpeedArray(void);
DLL_PUBLIC double getTakeOffAltitude(void);
DLL_PUBLIC float getYaw(void);
DLL_PUBLIC float getSpeed(void);
DLL_PUBLIC float getClimbRate(void);
DLL_PUBLIC int gpsIsOk(void);
DLL_PUBLIC int healthAllOk(void);
DLL_PUBLIC int isLanding(void);
DLL_PUBLIC void updateLogAndProjection(void);
#ifdef __cplusplus
}
#endif
#endif /* __AUTOPILOT_H__ */
include/dronedge.h
View file @ 19d6f938
......@@ -2,9 +2,11 @@
#define __DRONEDGE_H__
#include <open62541/server.h>
#include "mavsdk_wrapper.h"
#include "autopilot_wrapper.h"
#include "pubsub.h"
#define DRONE_VARIABLE_NB 3
#define SUBSCRIBER_VARIABLE_NB 1
struct messageNode {
char *message;
struct messageNode *next;
......@@ -14,7 +16,7 @@ typedef struct {
struct messageNode *tail;
} MessageQueue;
UA_Double positionArray[POSITION_ARRAY_SIZE] = { 0 };
UA_Int64 positionArray[POSITION_ARRAY_SIZE] = { 0 };
UA_UInt32 positionArrayDims[] = {POSITION_ARRAY_SIZE};
UA_Double speedArray[SPEED_ARRAY_SIZE] = { 0 };
......@@ -31,12 +33,12 @@ VariableData droneVariableArray[] = {
.typeName = "Position Array Type",
.description = "Position Array",
.value = &positionArray,
.type = UA_TYPES_DOUBLE,
.builtInType = UA_NS0ID_DOUBLE,
.type = UA_TYPES_INT64,
.builtInType = UA_NS0ID_INT64,
.valueRank = UA_VALUERANK_ONE_DIMENSION,
.arrayDimensionsSize = 1,
.arrayDimensions = positionArrayDims,
.getter.getArray = mavsdk_getPositionArray,
.getter.getArray = getPositionArray,
},
{
.name = "speedArray",
......@@ -48,7 +50,7 @@ VariableData droneVariableArray[] = {
.valueRank = UA_VALUERANK_ONE_DIMENSION,
.arrayDimensionsSize = 1,
.arrayDimensions = speedArrayDims,
.getter.getArray = mavsdk_getSpeedArray,
.getter.getArray = getSpeedArray,
},
{
.name = "message",
......@@ -63,4 +65,31 @@ VariableData droneVariableArray[] = {
},
};
VariableStruct droneVariables = {
.nbVariable = DRONE_VARIABLE_NB,
.variableArray = droneVariableArray,
};
VariableData subscriberVariableArray[] = {
{
.name = "message",
.description = "Message to send to the other drones",
.value = &message,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
},
};
VariableStruct subscriberVariables = {
.nbVariable = SUBSCRIBER_VARIABLE_NB,
.variableArray = subscriberVariableArray,
};
void Subscriber_log(void *context, UA_LogLevel level, UA_LogCategory category,
const char *msg, va_list args);
#endif /* __DRONEDGE_H__ */
include/mavsdk_wrapper.h deleted 100644 → 0
View file @ 27ae36e4
#ifndef __MAVSDK_H__
#define __MAVSDK_H__
/*
* 0. latitude (double, degrees)
* 1. longitude (double, degrees)
* 2. absolute altitude (double, meters)
* 3. relative altitude (double, meters)
*/
#define POSITION_ARRAY_SIZE 4
/*
* 0. yaw angle (float, degrees)
* 1. air speed (float, m/s)
* 2. climb rate (float, m/s)
*/
#define SPEED_ARRAY_SIZE 3
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
// Connexion management functions
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_takeOff(void);
int mavsdk_takeOffAndWait(void);
int mavsdk_triggerParachute(void);
// Flight management functions
int mavsdk_loiter(float radius);
int mavsdk_setAirspeed(float airspeed);
int mavsdk_setTargetCoordinates(double la, double lo, float a);
// Information functions
float mavsdk_getAltitude(void);
float mavsdk_getAltitudeRel(void);
float mavsdk_getInitialAltitude(void);
double mavsdk_getInitialLatitude(void);
double mavsdk_getInitialLongitude(void);
double mavsdk_getLatitude(void);
double mavsdk_getLongitude(void);
double *mavsdk_getPositionArray(void);
float *mavsdk_getSpeedArray(void);
double mavsdk_getTakeOffAltitude(void);
float mavsdk_getYaw(void);
float mavsdk_getSpeed(void);
float mavsdk_getClimbRate(void);
int mavsdk_healthAllOk(void);
int mavsdk_landed(void);
#ifdef __cplusplus
}
#endif
#endif /* __MAVSDK_H__ */
include/pubsub.h
View file @ 19d6f938
......@@ -6,7 +6,9 @@
#include <quickjs/quickjs.h>
#ifndef DLL_PUBLIC
#define DLL_PUBLIC __attribute__ ((visibility ("default")))
#endif
#define countof(x) (sizeof(x) / sizeof((x)[0]))
......@@ -23,6 +25,7 @@ typedef struct {
UA_Double longitude;
UA_Double altitudeAbs;
UA_Double altitudeRel;
UA_Int64 timestamp;
UA_Float yaw;
UA_Float speed;
UA_Float climbRate;
......@@ -30,6 +33,13 @@ typedef struct {
UA_UInt32 messageId;
} JSDroneData;
typedef struct {
UA_Double x;
UA_Double y;
UA_Double z;
UA_Int64 timestamp;
} JSPositionData;
typedef struct {
char* name;
char* typeName;
......@@ -47,21 +57,33 @@ typedef struct {
} getter;
} VariableData;
int runPubsub(UA_String *transportProfile,
typedef struct {
size_t nbVariable;
VariableData *variableArray;
} VariableStruct;
typedef struct {
VariableStruct variables;
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
} InstanceData;
int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
VariableStruct variables, UA_UInt32 id,
InstanceData *readerArray, UA_UInt32 nbReader,
UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
bool publish, UA_Boolean *running);
UA_Boolean *running);
UA_String get_message(void);
UA_UInt16 get_drone_id(UA_UInt32 nb);
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
VariableData pubsub_get_value(UA_String identifier);
......
mavsdk_wrapper.cpp deleted 100644 → 0
View file @ 27ae36e4
#include <chrono>
#include <cmath>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <fstream>
#include <functional>
#include <future>
#include <memory>
#include <thread>
#include "mavsdk_wrapper.h"
using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;
#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour
static std::ofstream log_file_fd;
static Mavsdk _mavsdk;
static Telemetry * telemetry;
static Action * action;
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 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
static void log(std::string message) {
log_file_fd << message << std::endl;
}
static void log_error(std::string message) {
log(ERROR_CONSOLE_TEXT + message + NORMAL_CONSOLE_TEXT);
}
template <class Enumeration>
static void log_error_from_result(std::string message, Enumeration result) {
std::ostringstream oss;
oss << message << ": " << result;
log_error(oss.str());
}
// Action functions
static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
if(!mavsdk_started) {
log_error("Mavsdk not started");
return -1;
}
const Action::Result result = (action->*toDo)();
if(result != Action::Result::Success) {
log_error_from_result(failure_message, result);
return -1;
}
return 0;
}
static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string failure_message) {
const MavlinkPassthrough::Result result = mavlink_passthrough->send_command_long(command);
if(result != MavlinkPassthrough::Result::Success) {
log_error_from_result(failure_message, result);
return -1;
}
return 0;
}
// Connexion management functions
static double toRad(double angle) {
return M_PI * angle / 180;
}
static double toDeg(double angle) {
return 180 * angle / M_PI;
}
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;
log_file_fd.open(log_file);
connection_result = _mavsdk.add_any_connection(connection_url);
if (connection_result != ConnectionResult::Success) {
log_error_from_result("Connection failed", connection_result);
return -1;
}
log("Waiting to discover msystem...");
fut = prom.get_future();
_mavsdk.subscribe_on_new_system([]() {
auto msystem_tmp = _mavsdk.systems().back();
if (msystem_tmp->has_autopilot()) {
log("Discovered autopilot");
// Unsubscribe again as we only want to find one system.
_mavsdk.subscribe_on_new_system(nullptr);
prom.set_value(msystem_tmp);
}
});
if (fut.wait_for(seconds(timeout)) == std::future_status::timeout) {
log_error("No autopilot found, exiting.");
return -1;
}
msystem = fut.get();
telemetry = new Telemetry(msystem);
action = new Action(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;
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");
sleep_for(seconds(1));
abs_altitude = mavsdk_getAltitude();
} while(isnan(abs_altitude) || abs_altitude == 0);
origin = telemetry->position();
log("MAVSDK started...");
mavsdk_started = 1;
log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);ground speed (m/s);climb rate (m/s)");
return 0;
}
int mavsdk_stop() {
if (!mavsdk_started)
return -1;
if (!mavsdk_landed()) {
log_error("You must land first !");
return -1;
}
if (doAction(&Action::shutdown, "Shutdown failed"))
return -1;
// Delete pointers
delete action;
delete mavlink_passthrough;
delete telemetry;
log_file_fd.close();
return 0;
}
int mavsdk_reboot() {
return doAction(&Action::reboot, "Rebooting failed");
}
// Flight state management functions
int mavsdk_arm(void) {
if(!mavsdk_started)
return -1;
while(!telemetry->health().is_home_position_ok) {
log("Waiting for home position to be set");
sleep_for(seconds(1));
}
log("Arming...");
return doAction(&Action::arm, "Arming failed");
}
int mavsdk_takeOff(void) {
if (doAction(&Action::takeoff, "Takeoff failed"))
return -1;
return 0;
}
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;
}
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();
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
static int doReposition(float la, float lo, float radius, float y) {
if (!mavsdk_started)
return -1;
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.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;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 2 | 4 | 8;
command.param4 = airspeed;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Setting airspeed failed");
}
static int mavsdk_setAltitude(float altitude) {
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 1 | 2 | 8;
command.param3 = altitude;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Setting altitude failed");
}
/*
* 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);
}
int mavsdk_setTargetCoordinates(double la, double lo, float a) {
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;
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)
);
result = doReposition(
(float)toDeg(newLa),
(float)toDeg(newLo),
DEFAULT_RADIUS,
0
);
result |= mavsdk_setAltitude(a);
return result;
}
// Information functions
float mavsdk_getAltitude(void) {
return telemetry->position().absolute_altitude_m;
}
float mavsdk_getAltitudeRel(void) {
return telemetry->position().relative_altitude_m;
}
float mavsdk_getInitialAltitude(void) {
return origin.absolute_altitude_m;
}
double mavsdk_getInitialLatitude(void) {
return origin.latitude_deg;
}
double mavsdk_getInitialLongitude(void) {
return origin.longitude_deg;
}
double mavsdk_getLatitude(void) {
return telemetry->position().latitude_deg;
}
double mavsdk_getLongitude(void) {
return telemetry->position().longitude_deg;
}
double *mavsdk_getPositionArray(void) {
Telemetry::Position position = telemetry->position();
double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
positionArray[0] = position.latitude_deg;
positionArray[1] = position.longitude_deg;
positionArray[2] = (double) position.absolute_altitude_m;
positionArray[3] = (double) position.relative_altitude_m;
return positionArray;
}
float *mavsdk_getSpeedArray(void) {
Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
speedArray[0] = mavsdk_getYaw();
speedArray[1] = metrics.airspeed_m_s;
speedArray[2] = metrics.climb_rate_m_s;
return speedArray;
}
double mavsdk_getTakeOffAltitude(void) {
const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
if (response.first != Action::Result::Success) {
log_error_from_result("Get takeoff altitude failed", response.first);
return -1;
}
return response.second;
}
float mavsdk_getYaw(void) {
return telemetry->attitude_euler().yaw_deg;
}
float mavsdk_getSpeed(void) {
return telemetry->fixedwing_metrics().airspeed_m_s;
}
float mavsdk_getClimbRate(void) {
return telemetry->fixedwing_metrics().climb_rate_m_s;
}
int mavsdk_healthAllOk(void) {
return telemetry->health_all_ok();
}
int mavsdk_landed(void) {
return !telemetry->in_air();
}
pubsub.c
View file @ 19d6f938
......@@ -10,14 +10,14 @@ UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
readerGroupIdent, readerIdent;
UA_DataSetReaderConfig readerConfig;
bool isPublisher;
const UA_Logger *pubsubLogger;
VariableData (*pubsubGetValue)(UA_String identifier);
static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*, bool print);
static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
VariableData *variableArray,
size_t nbVariable, int id);
VariableStruct variables, int id);
static UA_StatusCode
addPubSubConnection(UA_Server *server, UA_String *transportProfile,
......@@ -92,7 +92,7 @@ readVariable(UA_Server *server,
}
if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
UA_LOG_ERROR(pubsubLogger, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
dataValue->hasValue = true;
return retval;
}
......@@ -102,7 +102,7 @@ writeVariable(UA_Server *server,
const UA_NodeId *sessionId, void *sessionContext,
const UA_NodeId *nodeId, void *nodeContext,
const UA_NumericRange *range, const UA_DataValue *data) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
UA_LOG_INFO(pubsubLogger, UA_LOGCATEGORY_USERLAND,
"Updating variables manually is not implemented");
return UA_STATUSCODE_BADINTERNALERROR;
}
......@@ -234,14 +234,12 @@ addReaderGroup(UA_Server *server) {
* SubscribedDataSet and be contained within a ReaderGroup. */
/* Add DataSetReader to the ReaderGroup */
static UA_StatusCode
addDataSetReader(UA_Server *server, VariableData *variableArray,
size_t nbVariable, int id) {
addDataSetReader(UA_Server *server, VariableStruct variables, int id) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
/* Setting up Meta data configuration in DataSetReader */
fillDataSetMetaData(&readerConfig.dataSetMetaData, variableArray,
nbVariable, id);
fillDataSetMetaData(&readerConfig.dataSetMetaData, variables, id);
return UA_Server_addDataSetReader(server, readerGroupIdent,
&readerConfig, &readerIdent);
......@@ -252,7 +250,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
void *monitoredItemContext, const UA_NodeId *nodeId,
void *nodeContext, UA_UInt32 attributeId,
const UA_DataValue *var) {
callbackUpdate(nodeId->identifier.numeric, var, !isPublisher);
callbackUpdate(nodeId->identifier.numeric, var, false);
}
/**
......@@ -263,7 +261,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
static UA_StatusCode
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
VariableData *variableArray, UA_UInt32 nb,
UA_Duration samplingInterval,
UA_UInt32 maxVariableNb, UA_Duration samplingInterval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
......@@ -312,7 +310,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_NodeId newNode;
retval |= UA_Server_addVariableNode(server,
UA_NODEID_NUMERIC(1, (UA_UInt32) readerConfig.dataSetMetaData.fieldsSize*nb + i + 50000),
UA_NODEID_NUMERIC(1, (UA_UInt32)maxVariableNb*nb + i + 50000),
folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
......@@ -351,8 +349,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
* and PublishedDataSetFields of Publisher */
/* Define MetaData for TargetVariables */
static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
VariableData *variableArray,
size_t nbVariable, int id) {
VariableStruct variables, int id) {
char name[12];
UA_snprintf(name, sizeof(name) - 1, "DataSet %d", id);
......@@ -364,34 +361,34 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
/* Definition of number of fields sizeto create different
* targetVariables of distinct datatype */
pMetaData->fieldsSize = nbVariable;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "fieldsSize %d", (int) pMetaData->fieldsSize);
pMetaData->fieldsSize = variables.nbVariable;
UA_LOG_INFO(pubsubLogger, UA_LOGCATEGORY_USERLAND, "fieldsSize %d", (int) pMetaData->fieldsSize);
pMetaData->fields = (UA_FieldMetaData*)UA_Array_new (pMetaData->fieldsSize,
&UA_TYPES[UA_TYPES_FIELDMETADATA]);
for(size_t i = 0; i < pMetaData->fieldsSize; i++) {
UA_FieldMetaData_init (&pMetaData->fields[i]);
UA_NodeId_copy(&UA_TYPES[variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
pMetaData->fields[i].builtInType = variableArray[i].builtInType;
pMetaData->fields[i].name = UA_STRING (variableArray[i].name);
pMetaData->fields[i].valueRank = variableArray[i].valueRank;
pMetaData->fields[i].arrayDimensions = variableArray[i].arrayDimensions;
pMetaData->fields[i].arrayDimensionsSize = variableArray[i].arrayDimensionsSize;
UA_NodeId_copy(&UA_TYPES[variables.variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
pMetaData->fields[i].builtInType = variables.variableArray[i].builtInType;
pMetaData->fields[i].name = UA_STRING (variables.variableArray[i].name);
pMetaData->fields[i].valueRank = variables.variableArray[i].valueRank;
pMetaData->fields[i].arrayDimensions = variables.variableArray[i].arrayDimensions;
pMetaData->fields[i].arrayDimensionsSize = variables.variableArray[i].arrayDimensionsSize;
}
}
static void
setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariable) {
setVariableType(UA_Server *server, VariableStruct variables) {
VariableData vDetails;
UA_VariableTypeAttributes vtAttr;
for(UA_UInt32 i = 0; i < nbVariable; i++) {
vDetails = variableArray[i];
for(UA_UInt32 i = 0; i < variables.nbVariable; i++) {
vDetails = variables.variableArray[i];
switch(vDetails.valueRank) {
case UA_VALUERANK_SCALAR:
variableArray[i].typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
vDetails.typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
break;
case UA_VALUERANK_ONE_DIMENSION:
......@@ -411,7 +408,7 @@ setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariabl
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Valuerank not handled");
UA_LOG_ERROR(pubsubLogger, UA_LOGCATEGORY_USERLAND, "Valuerank not handled");
break;
}
}
......@@ -433,10 +430,8 @@ setServer(UA_String *transportProfile,
}
static UA_StatusCode
subscribe(UA_Server *server,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
UA_UInt32 nbReader, UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print)) {
UA_UInt16 publisherIdent;
......@@ -460,12 +455,16 @@ subscribe(UA_Server *server,
readerConfig.name = UA_STRING(readerName);
readerConfig.publisherId.data = &publisherIdent;
retval = addDataSetReader(server, variableArray, nbVariable, publisherIdent);
retval = addDataSetReader(server, instanceArray[i].variables,
publisherIdent);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */
retval = addSubscribedVariables(server, readerIdent, variableArray, i, interval, init_node_id);
retval = addSubscribedVariables(server, readerIdent,
instanceArray[i].variables.variableArray,
i, maxVariableNb, interval,
instanceArray[i].init_node_id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
......@@ -473,45 +472,43 @@ subscribe(UA_Server *server,
return retval;
}
int runPubsub(UA_String *transportProfile,
int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
VariableStruct variables, UA_UInt32 id,
InstanceData *readerArray, UA_UInt32 nbReader,
UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
bool publish, UA_Boolean *running) {
UA_Boolean *running) {
UA_Server *server;
UA_StatusCode retval;
pubsubLogger = logger;
server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
setVariableType(server, variables);
/* Publishing */
isPublisher = publish;
if (isPublisher) {
pubsubGetValue = get_value;
addPublishedDataSet(server, id);
for(UA_UInt32 i = 0; i < nbVariable; i++) {
retval = addDataSourceVariable(server, variableArray[i]);
for(UA_UInt32 i = 0; i < variables.nbVariable; i++) {
retval = addDataSourceVariable(server, variables.variableArray[i]);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variableArray[i]);
addDataSetField(server, variables.variableArray[i]);
}
addWriterGroup(server, interval);
retval = addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
/* Subscribing */
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update);
subscribe(server, readerArray, id, nbReader, maxVariableNb, interval,
get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
......
qjs_wrapper.c
View file @ 19d6f938
#include <math.h>
#include <pthread.h>
#include "dronedge.h"
# define ANSI_COLOR_RED "\x1b[31m"
# define ANSI_COLOR_GREEN "\x1b[32m"
# define ANSI_COLOR_YELLOW "\x1b[33m"
# define ANSI_COLOR_BLUE "\x1b[34m"
# define ANSI_COLOR_MAGENTA "\x1b[35m"
# define ANSI_COLOR_CYAN "\x1b[36m"
# define ANSI_COLOR_RESET "\x1b[0m"
static JSClassID jsDroneClassId;
static JSClassID jsPositionClassId;
static UA_Boolean pubsubShouldRun = true;
static UA_Boolean pubsubExited = false;
static UA_Boolean pubsubExited = true;
static UA_UInt32 nbDrone;
static UA_UInt32 nbSubscriber;
static JSValueConst *droneObjectIdList;
static MessageQueue logQueue = {
.head = NULL,
.tail = NULL,
};
static MessageQueue messageQueue = {
.head = NULL,
.tail = NULL,
};
UA_String currentMessage;
const UA_Logger Subscriber_Log = {Subscriber_log, NULL, UA_Log_Stdout_clear};
const UA_Logger *logger;
pthread_mutex_t mutex;
pthread_cond_t threadCond;
const char *logLevelNames[6] = {"trace", "debug",
ANSI_COLOR_GREEN "info",
ANSI_COLOR_YELLOW "warn",
ANSI_COLOR_RED "error",
ANSI_COLOR_MAGENTA "fatal"};
const char *logCategoryNames[10] =
{"network", "channel", "session", "server", "client",
"userland", "securitypolicy", "eventloop", "pubsub", "discovery"};
// Position class functions
static void js_position_finalizer(JSRuntime *rt, JSValue val)
{
JSPositionData *s = (JSPositionData *) JS_GetOpaque(val, jsPositionClassId);
js_free_rt(rt, s);
}
static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
JSPositionData *s;
JSValue obj;
UA_Int64 *positionArray;
obj = JS_NewObjectClass(ctx, (int) jsPositionClassId);
if (JS_IsException(obj))
return obj;
s = (JSPositionData *) js_mallocz(ctx, sizeof(*s));
if (!s) {
JS_FreeValue(ctx, obj);
return JS_EXCEPTION;
}
positionArray = getPositionArray();
s->x = (double)positionArray[0] / 1e7;
s->y = (double)positionArray[1] / 1e7;
s->z = (UA_Double)(positionArray[3] / 1000); //relative altitude
s->timestamp = positionArray[4];
JS_SetOpaque(obj, s);
free(positionArray);
return obj;
}
static JSValue js_position_get(JSContext *ctx, JSValueConst thisVal, int magic)
{
JSPositionData *s = (JSPositionData *) JS_GetOpaque2(ctx, thisVal, jsPositionClassId);
if (!s)
return JS_EXCEPTION;
switch(magic) {
case 0:
return JS_NewFloat64(ctx, s->x);
case 1:
return JS_NewFloat64(ctx, s->y);
case 2:
return JS_NewFloat64(ctx, s->z);
case 3:
return JS_NewInt64(ctx, s->timestamp);
default:
return JS_EXCEPTION;
}
}
static JSClassDef jsPositionClass = {
"Position",
.finalizer = js_position_finalizer,
};
static const JSCFunctionListEntry js_position_proto_funcs[] = {
JS_CGETSET_MAGIC_DEF("x", js_position_get, NULL, 0),
JS_CGETSET_MAGIC_DEF("y", js_position_get, NULL, 1),
JS_CGETSET_MAGIC_DEF("z", js_position_get, NULL, 2),
JS_CGETSET_MAGIC_DEF("timestamp", js_position_get, NULL, 3),
};
// Drone class functions
static void js_drone_finalizer(JSRuntime *rt, JSValue val)
......@@ -98,34 +188,39 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
pthread_cond_signal(&threadCond);
pthread_mutex_unlock(&mutex);
return res;
case 9:
return JS_NewInt64(ctx, s->timestamp);
default:
return JS_EXCEPTION;
}
}
static void addMessageToQueue(const char* msg, MessageQueue* pQueue)
{
struct messageNode *newNode;
newNode = (struct messageNode*)malloc(sizeof(struct messageNode));
newNode->message = strdup(msg);
newNode->next = NULL;
if (pQueue->tail == NULL) {
pQueue->head = pQueue->tail = newNode;
} else {
pQueue->tail->next = newNode;
pQueue->tail = newNode;
}
}
static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
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");
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "Message too long");
return JS_EXCEPTION;
}
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;
}
addMessageToQueue(message, &messageQueue);
JS_FreeCString(ctx, message);
return JS_UNDEFINED;
}
......@@ -149,12 +244,7 @@ UA_String get_message(void)
struct messageNode *current;
current = messageQueue.head;
if (current == NULL) {
if (!UA_String_isEmpty(&currentMessage)) {
UA_String_clear(&currentMessage);
currentMessage = UA_STRING("");
}
} else {
if (current != NULL) {
clear_message(currentMessage);
currentMessage = UA_STRING_ALLOC(current->message);
messageQueue.head = current->next == NULL ? (messageQueue.tail = NULL) : current->next;
......@@ -178,6 +268,7 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
JS_CGETSET_MAGIC_DEF("speed", js_drone_get, NULL, 6),
JS_CGETSET_MAGIC_DEF("climbRate", js_drone_get, NULL, 7),
JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 8),
JS_CGETSET_MAGIC_DEF("timestamp", js_drone_get, NULL, 9),
JS_CFUNC_DEF("init", 1, js_drone_init),
};
......@@ -193,18 +284,18 @@ VariableData pubsub_get_value(UA_String identifier) {
UA_DataType type;
UA_Double *array;
for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) {
UA_String name = UA_STRING(droneVariableArray[i].name);
for(UA_UInt32 i = 0; i < droneVariables.nbVariable; i++) {
UA_String name = UA_STRING(droneVariables.variableArray[i].name);
if(UA_String_equal(&identifier, &name)) {
varDetails = droneVariableArray[i];
varDetails = droneVariables.variableArray[i];
switch(varDetails.valueRank) {
case UA_VALUERANK_SCALAR:
switch(varDetails.type) {
case UA_TYPES_STRING:
*(UA_String*)varDetails.value = droneVariableArray[i].getter.getString();
*(UA_String*)varDetails.value = varDetails.getter.getString();
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
break;
}
break;
......@@ -212,7 +303,7 @@ VariableData pubsub_get_value(UA_String identifier) {
case UA_VALUERANK_ONE_DIMENSION:
type = UA_TYPES[varDetails.type];
size_t size = varDetails.arrayDimensions[0];
array = (UA_Double *) droneVariableArray[i].getter.getArray();
array = (UA_Double *) varDetails.getter.getArray();
if(type.pointerFree) {
memcpy(varDetails.value, array, type.memSize * size);
......@@ -226,14 +317,14 @@ VariableData pubsub_get_value(UA_String identifier) {
ptrd += type.memSize;
}
if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "Failed to update array");
}
free(array);
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
break;
}
}
......@@ -242,7 +333,7 @@ VariableData pubsub_get_value(UA_String identifier) {
return varDetails;
}
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
......@@ -255,7 +346,19 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
s->messageId = id;
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
UA_LOG_ERROR(logger, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break;
}
}
void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
s->messageId = id;
break;
default:
UA_LOG_ERROR(logger, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break;
}
}
......@@ -264,21 +367,22 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
{
JSDroneData* s;
UA_String uaStr;
UA_Double* positionArray;
UA_Int64* positionArray;
UA_Float* speedArray;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->positionArrayId == id) {
positionArray = (UA_Double*) var->value.data;
s->latitude = positionArray[0];
s->longitude = positionArray[1];
s->altitudeAbs = positionArray[2];
s->altitudeRel = positionArray[3];
positionArray = (UA_Int64*) var->value.data;
s->latitude = (double)positionArray[0] / 1e7;
s->longitude = (double)positionArray[1] / 1e7;
s->altitudeAbs = (UA_Double)(positionArray[2] / 1000);
s->altitudeRel = (UA_Double)(positionArray[3] / 1000);
s->timestamp = positionArray[4];
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received position of drone %d: %f° %f° %fm %fm",
UA_LOG_INFO(logger, UA_LOGCATEGORY_CLIENT,
"Received position of drone %d: %.6f ° %.6f ° %.2f m %.2f m",
s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
}
return;
......@@ -289,33 +393,70 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
s->climbRate = speedArray[2];
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received speed of drone %d: %f° %fm/s %fm/s",
UA_LOG_INFO(logger, UA_LOGCATEGORY_CLIENT,
"Received speed of drone %d: %.2f ° %.2f m/s %.2f m/s",
s->id, s->yaw, s->speed, s->climbRate);
}
return;
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
if (!print) {
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';
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received message for drone %d: %s", s->id, s->message);
} else {
pthread_mutex_unlock(&mutex);
}
return;
}
}
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
UA_LOG_ERROR(logger, UA_LOGCATEGORY_CLIENT, "NodeId not found");
}
void Subscriber_log(void *context, UA_LogLevel level, UA_LogCategory category,
const char *msg, va_list args) {
char log_str[MAX_MESSAGE_SIZE];
char formatted_msg[MAX_MESSAGE_SIZE];
/* MinLevel encoded in the context pointer */
UA_LogLevel minLevel = (UA_LogLevel)(uintptr_t)context;
if(minLevel > level)
return;
UA_Int64 tOffset = UA_DateTime_localTimeUtcOffset();
UA_DateTimeStruct dts = UA_DateTime_toStruct(UA_DateTime_now() + tOffset);
int logLevelSlot = (int)level;
if(logLevelSlot < 0 || logLevelSlot > 5)
logLevelSlot = 5; /* Set to fatal if the level is outside the range */
/* Log */
snprintf(log_str, MAX_MESSAGE_SIZE,
"[%04u-%02u-%02u %02u:%02u:%02u.%03u (UTC%+05d)] %s/%s" ANSI_COLOR_RESET "\t",
dts.year, dts.month, dts.day, dts.hour, dts.min, dts.sec, dts.milliSec,
(int)(tOffset / UA_DATETIME_SEC / 36), logLevelNames[logLevelSlot],
logCategoryNames[category]);
vsnprintf(formatted_msg, MAX_MESSAGE_SIZE, msg, args);
strncat(log_str, formatted_msg, MAX_MESSAGE_SIZE - strlen(log_str) - 1);
strcat(log_str, "\n");
addMessageToQueue(log_str, &logQueue);
}
static JSValue js_get_log(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) {
JSValue res;
struct messageNode *current;
current = logQueue.head;
if (current != NULL) {
res = JS_NewString(ctx, current->message);
logQueue.head = current->next == NULL ? (logQueue.tail = NULL) : current->next;
delete_message_node(current);
} else {
res = JS_NewString(ctx, "");
}
return res;
}
/*
......@@ -327,8 +468,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
* 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)
{
int argc, JSValueConst *argv) {
const char *ipv6;
const char *port;
const char *netIface;
......@@ -336,6 +476,10 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
char urlBuffer[44];
UA_UInt32 id;
UA_Duration interval;
bool isADrone;
VariableStruct variables;
InstanceData *instanceArray;
UA_UInt32 nbPeer = nbDrone + nbSubscriber;
int res;
ipv6 = JS_ToCString(ctx, argv[0]);
......@@ -357,13 +501,29 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
if (JS_ToFloat64(ctx, &interval, argv[4]))
return JS_EXCEPTION;
res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
countof(droneVariableArray), id, nbDrone, interval,
init_node_id, get_drone_id, pubsub_get_value,
pubsub_update_variables, JS_ToBool(ctx, argv[5]),
isADrone = JS_ToBool(ctx, argv[5]);
variables = isADrone ? droneVariables : subscriberVariables;
logger = isADrone ? UA_Log_Stdout : &Subscriber_Log;
instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData));
for(UA_UInt32 i = 0; i < nbDrone; i++) {
instanceArray[i].variables = droneVariables;
instanceArray[i].init_node_id = init_drone_node_id;
}
for(UA_UInt32 i = nbDrone; i < nbPeer; i++) {
instanceArray[i].variables = subscriberVariables;
instanceArray[i].init_node_id = init_subscriber_node_id;
}
pubsubExited = false;
res = runPubsub(logger, &transportProfile, &networkAddressUrl, variables,
id, instanceArray, nbPeer,
fmax(DRONE_VARIABLE_NB, SUBSCRIBER_VARIABLE_NB), interval,
get_drone_id, pubsub_get_value, pubsub_update_variables,
&pubsubShouldRun);
pubsubExited = true;
free(instanceArray);
JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port);
free(notConstNetIface);
......@@ -377,7 +537,11 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
if (JS_ToUint32(ctx, &nbDrone, argv[0]))
return JS_EXCEPTION;
droneObjectIdList = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
if (JS_ToUint32(ctx, &nbSubscriber, argv[1]))
return JS_EXCEPTION;
currentMessage = UA_STRING("");
droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0);
}
......@@ -397,12 +561,19 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
delete_message_node(current);
}
clear_message(currentMessage);
while(logQueue.head != NULL) {
current = logQueue.head;
logQueue.head = current->next;
delete_message_node(current);
}
return JS_NewInt32(ctx, 0);
}
// Connexion management functions
static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
static JSValue js_start(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
const char *ip;
......@@ -418,64 +589,75 @@ static JSValue js_mavsdk_start(JSContext *ctx, JSValueConst thisVal,
if (JS_ToInt32(ctx, &timeout, argv[3]))
return JS_EXCEPTION;
res = mavsdk_start(ip, port, log_file, timeout);
res = start(ip, port, log_file, timeout);
JS_FreeCString(ctx, ip);
JS_FreeCString(ctx, log_file);
return JS_NewInt32(ctx, res);
}
static JSValue js_mavsdk_stop(JSContext *ctx, JSValueConst thisVal,
static JSValue js_stop(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_stop());
return JS_NewInt32(ctx, stop());
}
static JSValue js_mavsdk_reboot(JSContext *ctx, JSValueConst thisVal,
static JSValue js_reboot(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_reboot());
return JS_NewInt32(ctx, reboot());
}
// Flight state management functions
static JSValue js_mavsdk_arm(JSContext *ctx, JSValueConst this_val,
static JSValue js_arm(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_arm());
return JS_NewInt32(ctx, arm());
}
static JSValue js_mavsdk_takeOff(JSContext *ctx, JSValueConst thisVal,
static JSValue js_takeOff(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_takeOff());
return JS_NewInt32(ctx, takeOff());
}
static JSValue js_mavsdk_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
static JSValue js_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_takeOffAndWait());
return JS_NewInt32(ctx, takeOffAndWait());
}
static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst thisVal,
static JSValue js_triggerParachute(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, mavsdk_triggerParachute());
return JS_NewInt32(ctx, triggerParachute());
}
// Flight management functions
static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst thisVal,
static JSValue js_loiter(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double la_arg_double;
double lo_arg_double;
double a_arg_double;
double radius;
if (JS_ToFloat64(ctx, &radius, argv[0]))
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, &radius, argv[3]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_loiter((float)radius));
loiter(la_arg_double, lo_arg_double, (float)a_arg_double, (float)radius);
return JS_NewInt32(ctx, 0);
}
static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst thisVal,
static JSValue js_setAirSpeed(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double altitude;
......@@ -483,10 +665,11 @@ static JSValue js_mavsdk_setAirspeed(JSContext *ctx, JSValueConst thisVal,
if (JS_ToFloat64(ctx, &altitude, argv[0]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setAirspeed((float)altitude));
setAirSpeed_async((float)altitude);
return JS_NewInt32(ctx, 0);
}
static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
static JSValue js_setTargetCoordinates(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
......@@ -501,154 +684,155 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_setTargetCoordinates(la_arg_double,
lo_arg_double,
(float)a_arg_double));
setTargetCoordinates(la_arg_double, lo_arg_double, (float)a_arg_double);
return JS_NewInt32(ctx, 0);
}
// Information functions
static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst thisVal,
static JSValue js_getAltitude(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getAltitude());
return JS_NewFloat64(ctx, getAltitude());
}
static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
}
static JSValue js_mavsdk_getInitialAltitude(JSContext *ctx,
static JSValue js_getInitialAltitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getInitialAltitude());
return JS_NewFloat64(ctx, getInitialAltitude());
}
static JSValue js_mavsdk_getInitialLatitude(JSContext *ctx,
static JSValue js_getInitialLatitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getInitialLatitude());
return JS_NewFloat64(ctx, getInitialLatitude());
}
static JSValue js_mavsdk_getInitialLongitude(JSContext *ctx,
static JSValue js_getInitialLongitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getInitialLongitude());
return JS_NewFloat64(ctx, getInitialLongitude());
}
static JSValue js_mavsdk_getLatitude(JSContext *ctx, JSValueConst thisVal,
static JSValue js_getTakeOffAltitude(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getLatitude());
return JS_NewFloat64(ctx, getTakeOffAltitude());
}
static JSValue js_mavsdk_getLongitude(JSContext *ctx, JSValueConst thisVal,
static JSValue js_getYaw(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getLongitude());
return JS_NewFloat64(ctx, getYaw());
}
static JSValue js_mavsdk_getTakeOffAltitude(JSContext *ctx,
JSValueConst thisVal,
static JSValue js_getSpeed(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getTakeOffAltitude());
return JS_NewFloat64(ctx, getSpeed());
}
static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst thisVal,
static JSValue js_getClimbRate(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getYaw());
return JS_NewFloat64(ctx, getClimbRate());
}
static JSValue js_mavsdk_getSpeed(JSContext *ctx, JSValueConst thisVal,
static JSValue js_gpsIsOk(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getSpeed());
return JS_NewBool(ctx, gpsIsOk());
}
static JSValue js_mavsdk_getClimbRate(JSContext *ctx, JSValueConst thisVal,
static JSValue js_healthAllOk(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewFloat64(ctx, mavsdk_getClimbRate());
return JS_NewBool(ctx, healthAllOk());
}
static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
static JSValue js_isLanding(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewBool(ctx, mavsdk_healthAllOk());
return JS_NewBool(ctx, isLanding());
}
static JSValue js_mavsdk_landed(JSContext *ctx, JSValueConst this_val,
static JSValue js_updateLogAndProjection(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewBool(ctx, mavsdk_landed());
updateLogAndProjection();
return JS_NewInt32(ctx, 0);
}
static const JSCFunctionListEntry js_mavsdk_funcs[] = {
static const JSCFunctionListEntry js_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", 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("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("setTargetCoordinates", 3, js_mavsdk_setTargetCoordinates ),
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("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("getYaw", 0, js_mavsdk_getYaw ),
JS_CFUNC_DEF("getAirspeed", 0, js_mavsdk_getSpeed ),
JS_CFUNC_DEF("getClimbRate", 0, js_mavsdk_getClimbRate ),
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("start", 4, js_start ),
JS_CFUNC_DEF("stop", 0, js_stop ),
JS_CFUNC_DEF("reboot", 0, js_reboot ),
JS_CFUNC_DEF("arm", 0, js_arm ),
JS_CFUNC_DEF("takeOff", 0, js_takeOff ),
JS_CFUNC_DEF("takeOffAndWait", 0, js_takeOffAndWait ),
JS_CFUNC_DEF("triggerParachute", 0, js_triggerParachute ),
JS_CFUNC_DEF("loiter", 4, js_loiter ),
JS_CFUNC_DEF("setAirSpeed", 1, js_setAirSpeed ),
JS_CFUNC_DEF("setTargetCoordinates", 3, js_setTargetCoordinates ),
JS_CFUNC_DEF("getPosition", 0, js_new_position ),
JS_CFUNC_DEF("getAltitude", 0, js_getAltitude ),
JS_CFUNC_DEF("getInitialAltitude", 0, js_getInitialAltitude ),
JS_CFUNC_DEF("getInitialLatitude", 0, js_getInitialLatitude ),
JS_CFUNC_DEF("getInitialLongitude", 0, js_getInitialLongitude ),
JS_CFUNC_DEF("getTakeOffAltitude", 0, js_getTakeOffAltitude ),
JS_CFUNC_DEF("getYaw", 0, js_getYaw ),
JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ),
JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ),
JS_CFUNC_DEF("gpsIsOk", 0, js_gpsIsOk ),
JS_CFUNC_DEF("isLanding", 0, js_isLanding ),
JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ),
JS_CFUNC_DEF("initPubsub", 2, js_init_pubsub ),
JS_CFUNC_DEF("getLog", 0, js_get_log ),
JS_CFUNC_DEF("updateLogAndProjection", 0, js_updateLogAndProjection ),
};
static int js_mavsdk_init(JSContext *ctx, JSModuleDef *m)
static int js_init(JSContext *ctx, JSModuleDef *m)
{
JSValue droneProto, droneClass;
JSValue droneProto, droneClass, positionProto;
JS_NewClassID(&jsDroneClassId);
JS_NewClassID(&jsPositionClassId);
JS_NewClass(JS_GetRuntime(ctx), jsDroneClassId, &jsDroneClass);
JS_NewClass(JS_GetRuntime(ctx), jsPositionClassId, &jsPositionClass);
droneProto = JS_NewObject(ctx);
JS_SetPropertyFunctionList(ctx, droneProto, js_drone_proto_funcs,
countof(js_drone_proto_funcs));
positionProto = JS_NewObject(ctx);
JS_SetPropertyFunctionList(ctx, positionProto, js_position_proto_funcs,
countof(js_position_proto_funcs));
droneClass = JS_NewCFunction2(ctx, js_drone_ctor, "Drone", 1,
JS_CFUNC_constructor, 0);
JS_SetConstructor(ctx, droneClass, droneProto);
JS_SetClassProto(ctx, jsDroneClassId, droneProto);
JS_SetClassProto(ctx, jsPositionClassId, positionProto);
JS_SetModuleExport(ctx, m, "Drone", droneClass);
return JS_SetModuleExportList(ctx, m, js_mavsdk_funcs,
countof(js_mavsdk_funcs));
return JS_SetModuleExportList(ctx, m, js_funcs,
countof(js_funcs));
}
JSModuleDef *js_init_module(JSContext *ctx, const char *module_name)
{
JSModuleDef *m;
m = JS_NewCModule(ctx, module_name, js_mavsdk_init);
m = JS_NewCModule(ctx, module_name, js_init);
if (!m)
return NULL;
JS_AddModuleExportList(ctx, m, js_mavsdk_funcs, countof(js_mavsdk_funcs));
JS_AddModuleExportList(ctx, m, js_funcs, countof(js_funcs));
JS_AddModuleExport(ctx, m, "Drone");
return m;
}
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