Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Q
qjs-wrapper
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
nexedi
qjs-wrapper
Commits
2a933857
Commit
2a933857
authored
Jun 28, 2022
by
Léo-Paul Géneau
👾
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Add same compilation flags as open62541
parent
cad10958
Changes
6
Show whitespace changes
Inline
Side-by-side
Showing
6 changed files
with
95 additions
and
83 deletions
+95
-83
Makefile
Makefile
+3
-2
include/mavsdk_wrapper.h
include/mavsdk_wrapper.h
+7
-7
include/pubsub.h
include/pubsub.h
+17
-5
mavsdk_wrapper.cpp
mavsdk_wrapper.cpp
+14
-14
pubsub.c
pubsub.c
+19
-19
qjs_wrapper.c
qjs_wrapper.c
+35
-36
No files found.
Makefile
View file @
2a933857
CPPFLAGS
=
-g
-Wall
-Wno-array-bounds
-Wno-format-truncation
-std
=
c99
-Iinclude
-O2
-fPIC
LDFLAGS
+=
-g
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
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
LIB_NAME
:=
libqjswrapper.so
...
...
include/mavsdk_wrapper.h
View file @
2a933857
...
...
@@ -12,19 +12,19 @@ int mavsdk_reboot(void);
// Flight state management functions
int
mavsdk_arm
(
void
);
int
mavsdk_doParachute
(
in
t
param
);
int
mavsdk_doParachute
(
floa
t
param
);
int
mavsdk_loiter
();
int
mavsdk_land
(
void
);
int
mavsdk_takeOff
(
void
);
int
mavsdk_takeOffAndWait
(
void
);
// Flight management functions
int
mavsdk_doReposition
(
double
la
,
double
lo
,
double
a
,
double
y
);
int
mavsdk_setAirspeed
(
double
airspeed
);
int
mavsdk_setAltitude
(
double
altitude
);
int
mavsdk_setTargetAltitude
(
double
a
);
int
mavsdk_setTargetCoordinates
(
double
la
,
double
lo
,
double
a
,
double
y
);
int
mavsdk_setTargetCoordinatesXYZ
(
double
x
,
double
y
,
double
z
);
int
mavsdk_doReposition
(
float
la
,
float
lo
,
float
a
,
float
y
);
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
);
// Information functions
...
...
include/pubsub.h
View file @
2a933857
...
...
@@ -4,13 +4,17 @@
#include <open62541/server.h>
#include <open62541/plugin/log_stdout.h>
#include <quickjs/quickjs.h>
#define DLL_PUBLIC __attribute__ ((visibility ("default")))
#define countof(x) (sizeof(x) / sizeof((x)[0]))
#define DATA_SET_WRITER_ID 1
#define WRITER_GROUP_ID 1
typedef
struct
{
int
id
;
UA_UInt16
id
;
UA_Double
latitude
;
UA_UInt32
latitudeId
;
UA_Double
longitude
;
...
...
@@ -40,18 +44,26 @@ typedef struct {
int
runPubsub
(
UA_String
*
transportProfile
,
UA_NetworkAddressUrlDataType
*
networkAddressUrl
,
VariableData
*
variableArray
,
in
t
nbVariable
,
int
id
,
int
nbReader
,
void
(
*
init_node_id
)(
UA_UInt32
id
,
int
nb
,
int
magic
),
int
(
*
get_reader_id
)(
int
nb
),
VariableData
*
variableArray
,
size_
t
nbVariable
,
UA_UInt32
id
,
UA_UInt32
nbReader
,
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_UInt16
get_drone_id
(
UA_UInt32
nb
);
void
init_node_id
(
UA_UInt32
id
,
UA_UInt32
nb
,
UA_UInt32
magic
);
void
pubsub_update_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 @
2a933857
...
...
@@ -214,7 +214,7 @@ int mavsdk_arm(void) {
return
doAction
(
&
Action
::
arm
,
"Arming failed"
);
}
int
mavsdk_doParachute
(
in
t
param
)
{
int
mavsdk_doParachute
(
floa
t
param
)
{
if
(
!
mavsdk_started
)
return
-
1
;
...
...
@@ -251,9 +251,9 @@ int mavsdk_loiter() {
if
(
!
mavsdk_started
)
return
-
1
;
return
mavsdk_doReposition
((
float
)
position
.
latitude_deg
,
(
float
)
position
.
longitude_deg
,
position
.
absolute_altitude_m
,
0
);
return
mavsdk_doReposition
((
float
)
mavsdk_getLatitude
()
,
(
float
)
mavsdk_getLongitude
()
,
mavsdk_getAltitude
()
,
0
);
}
int
mavsdk_takeOff
(
void
)
{
...
...
@@ -282,7 +282,7 @@ int mavsdk_takeOffAndWait(void) {
// Flight management functions
int
mavsdk_doReposition
(
double
la
,
double
lo
,
double
a
,
double
y
)
{
int
mavsdk_doReposition
(
float
la
,
float
lo
,
float
a
,
float
y
)
{
if
(
!
mavsdk_started
)
return
-
1
;
...
...
@@ -300,7 +300,7 @@ int mavsdk_doReposition(double la, double lo, double a, double y) {
return
doMavlinkCommand
(
command
,
"Reposition failed"
);
}
int
mavsdk_setAirspeed
(
double
airspeed
)
{
int
mavsdk_setAirspeed
(
float
airspeed
)
{
if
(
!
mavsdk_started
)
return
-
1
;
...
...
@@ -308,14 +308,14 @@ int mavsdk_setAirspeed(double airspeed) {
command
.
command
=
MAV_CMD_DO_OVERRIDE
;
command
.
param1
=
1
|
2
|
4
|
8
;
command
.
param2
=
2
|
4
|
8
;
command
.
param3
=
(
float
)
airspeed
;
command
.
param3
=
airspeed
;
command
.
target_sysid
=
mavlink_passthrough
->
get_target_sysid
();
command
.
target_compid
=
mavlink_passthrough
->
get_target_compid
();
return
doMavlinkCommand
(
command
,
"Setting airspeed failed"
);
}
int
mavsdk_setAltitude
(
double
altitude
)
{
int
mavsdk_setAltitude
(
float
altitude
)
{
if
(
!
mavsdk_started
)
return
-
1
;
...
...
@@ -323,14 +323,14 @@ int mavsdk_setAltitude(double altitude) {
command
.
command
=
MAV_CMD_DO_OVERRIDE
;
command
.
param1
=
1
|
2
|
4
|
8
;
command
.
param2
=
1
|
2
|
8
;
command
.
param3
=
(
float
)
altitude
;
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"
);
}
int
mavsdk_setTargetAltitude
(
double
a
)
{
int
mavsdk_setTargetAltitude
(
float
a
)
{
if
(
!
mavsdk_started
)
return
-
1
;
...
...
@@ -345,13 +345,13 @@ int mavsdk_setTargetAltitude(double a) {
return
0
;
}
int
mavsdk_setTargetCoordinates
(
double
la
,
double
lo
,
double
a
,
double
y
)
{
int
mavsdk_setTargetCoordinates
(
double
la
,
double
lo
,
float
a
,
float
y
)
{
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
,
(
float
)
a
,
(
float
)
y
);
const
Action
::
Result
result
=
action
->
goto_location
(
la
,
lo
,
a
,
y
);
if
(
result
!=
Action
::
Result
::
Success
)
{
log_error_from_result
(
"Goto location failed"
,
result
);
return
-
1
;
...
...
@@ -359,7 +359,7 @@ int mavsdk_setTargetCoordinates(double la, double lo, double a, double y) {
return
0
;
}
int
mavsdk_setTargetCoordinatesXYZ
(
double
x
,
double
y
,
double
z
)
{
int
mavsdk_setTargetCoordinatesXYZ
(
double
x
,
double
y
,
float
z
)
{
double
la
,
lo
;
la
=
((
origin
.
latitude_deg
+
y
/
EARTH_RADIUS
)
*
180.
F
)
/
M_PI
;
...
...
@@ -369,7 +369,7 @@ int mavsdk_setTargetCoordinatesXYZ(double x, double y, double z) {
}
int
mavsdk_setTargetLatLong
(
double
la
,
double
lo
)
{
return
mavsdk_setTargetCoordinates
(
la
,
lo
,
position
.
absolute_altitude_m
,
0
);
return
mavsdk_setTargetCoordinates
(
la
,
lo
,
mavsdk_getAltitude
()
,
0
);
}
// Information functions
...
...
pubsub.c
View file @
2a933857
...
...
@@ -6,7 +6,6 @@
#include "ua_pubsub.h"
#include "pubsub.h"
static
UA_Server
*
server
;
UA_NodeId
connectionIdent
,
publishedDataSetIdent
,
writerGroupIdent
,
readerGroupIdent
,
readerIdent
;
...
...
@@ -17,12 +16,12 @@ static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*);
static
void
fillDataSetMetaData
(
UA_DataSetMetaDataType
*
pMetaData
,
VariableData
*
variableArray
,
in
t
nbVariable
,
int
id
);
size_
t
nbVariable
,
int
id
);
static
UA_StatusCode
addPubSubConnection
(
UA_Server
*
server
,
UA_String
*
transportProfile
,
UA_NetworkAddressUrlDataType
*
networkAddressUrl
,
int
id
){
UA_UInt32
id
){
// 18 characters for "UDPMC Connection " + 10 characters for id <= UINT32_MAX
char
name
[
28
];
if
((
server
==
NULL
)
||
(
transportProfile
==
NULL
)
||
...
...
@@ -33,7 +32,7 @@ addPubSubConnection(UA_Server *server, UA_String *transportProfile,
UA_PubSubConnectionConfig
connectionConfig
;
memset
(
&
connectionConfig
,
0
,
sizeof
(
connectionConfig
));
UA_snprintf
(
name
,
sizeof
(
name
-
1
)
,
"UDPMC Connection %d"
,
id
);
UA_snprintf
(
name
,
sizeof
(
name
)
-
1
,
"UDPMC Connection %d"
,
id
);
connectionConfig
.
name
=
UA_STRING
(
name
);
connectionConfig
.
transportProfileUri
=
*
transportProfile
;
...
...
@@ -52,11 +51,11 @@ addPubSubConnection(UA_Server *server, UA_String *transportProfile,
* other PubSub elements are directly or indirectly linked with the PDS or
* connection. */
static
void
addPublishedDataSet
(
UA_Server
*
server
,
int
id
)
{
addPublishedDataSet
(
UA_Server
*
server
,
UA_UInt32
id
)
{
/* The PublishedDataSetConfig contains all necessary public
* information for the creation of a new PublishedDataSet */
char
name
[
8
];
UA_snprintf
(
name
,
sizeof
(
name
-
1
)
,
"PDS %d"
,
id
);
UA_snprintf
(
name
,
sizeof
(
name
)
-
1
,
"PDS %d"
,
id
);
UA_PublishedDataSetConfig
publishedDataSetConfig
;
memset
(
&
publishedDataSetConfig
,
0
,
sizeof
(
UA_PublishedDataSetConfig
));
publishedDataSetConfig
.
publishedDataSetType
=
UA_PUBSUB_DATASET_PUBLISHEDITEMS
;
...
...
@@ -220,7 +219,7 @@ addReaderGroup(UA_Server *server) {
/* Add DataSetReader to the ReaderGroup */
static
UA_StatusCode
addDataSetReader
(
UA_Server
*
server
,
VariableData
*
variableArray
,
in
t
nbVariable
,
int
id
)
{
size_
t
nbVariable
,
int
id
)
{
if
(
server
==
NULL
)
{
return
UA_STATUSCODE_BADINTERNALERROR
;
}
...
...
@@ -250,8 +249,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
,
int
nb
,
void
(
*
init_node_id
)(
UA_UInt32
id
,
int
nb
,
int
magic
))
{
addSubscribedVariables
(
UA_Server
*
server
,
UA_NodeId
dataSetReaderId
,
UA_UInt32
nb
,
void
(
*
init_node_id
)(
UA_UInt32
id
,
UA_UInt32
nb
,
UA_UInt32
magic
))
{
if
(
server
==
NULL
)
return
UA_STATUSCODE_BADINTERNALERROR
;
...
...
@@ -286,7 +285,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, int nb,
/* Create the TargetVariables with respect to DataSetMetaData fields */
UA_FieldTargetVariable
*
targetVars
=
(
UA_FieldTargetVariable
*
)
UA_calloc
(
readerConfig
.
dataSetMetaData
.
fieldsSize
,
sizeof
(
UA_FieldTargetVariable
));
for
(
size_t
i
=
0
;
i
<
readerConfig
.
dataSetMetaData
.
fieldsSize
;
i
++
)
{
for
(
UA_UInt32
i
=
0
;
i
<
readerConfig
.
dataSetMetaData
.
fieldsSize
;
i
++
)
{
/* Variable to subscribe data */
UA_VariableAttributes
vAttr
=
UA_VariableAttributes_default
;
UA_LocalizedText_copy
(
&
readerConfig
.
dataSetMetaData
.
fields
[
i
].
description
,
...
...
@@ -336,9 +335,9 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, int nb,
/* Define MetaData for TargetVariables */
static
void
fillDataSetMetaData
(
UA_DataSetMetaDataType
*
pMetaData
,
VariableData
*
variableArray
,
in
t
nbVariable
,
int
id
)
{
size_
t
nbVariable
,
int
id
)
{
char
name
[
12
];
UA_snprintf
(
name
,
sizeof
(
name
-
1
)
,
"DataSet %d"
,
id
);
UA_snprintf
(
name
,
sizeof
(
name
)
-
1
,
"DataSet %d"
,
id
);
if
(
pMetaData
==
NULL
)
{
return
;
...
...
@@ -366,13 +365,14 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
int
runPubsub
(
UA_String
*
transportProfile
,
UA_NetworkAddressUrlDataType
*
networkAddressUrl
,
VariableData
*
variableArray
,
in
t
nbVariable
,
int
id
,
int
nbReader
,
void
(
*
init_node_id
)(
UA_UInt32
id
,
int
nb
,
int
magic
),
int
(
*
get_reader_id
)(
int
nb
),
VariableData
*
variableArray
,
size_
t
nbVariable
,
UA_UInt32
id
,
UA_UInt32
nbReader
,
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_Server
*
server
;
UA_UInt16
publisherIdent
;
char
readerName
[
19
];
...
...
@@ -392,7 +392,7 @@ int runPubsub(UA_String *transportProfile,
/* Publishing */
addPublishedDataSet
(
server
,
id
);
for
(
size_t
i
=
0
;
i
<
nbVariable
;
i
++
)
{
for
(
UA_UInt32
i
=
0
;
i
<
nbVariable
;
i
++
)
{
retval
|=
addDataSourceVariable
(
server
,
variableArray
[
i
]);
if
(
retval
!=
UA_STATUSCODE_GOOD
)
return
EXIT_FAILURE
;
...
...
@@ -415,9 +415,9 @@ int runPubsub(UA_String *transportProfile,
readerConfig
.
writerGroupId
=
WRITER_GROUP_ID
;
readerConfig
.
dataSetWriterId
=
DATA_SET_WRITER_ID
;
for
(
size_t
i
=
0
;
i
<
nbReader
;
i
++
)
{
for
(
UA_UInt32
i
=
0
;
i
<
nbReader
;
i
++
)
{
publisherIdent
=
get_reader_id
(
i
);
UA_snprintf
(
readerName
,
sizeof
(
readerName
-
1
)
,
"DataSet Reader %d"
,
publisherIdent
);
UA_snprintf
(
readerName
,
sizeof
(
readerName
)
-
1
,
"DataSet Reader %d"
,
publisherIdent
);
readerConfig
.
name
=
UA_STRING
(
readerName
);
readerConfig
.
publisherId
.
data
=
&
publisherIdent
;
...
...
qjs_wrapper.c
View file @
2a933857
#include <quickjs/quickjs.h>
#include "dronedge.h"
static
JSClassID
js_drone_class_id
;
static
UA_Boolean
pubsub_running
=
true
;
int
nbDrone
;
static
UA_UInt32
nbDrone
;
static
JSValueConst
*
drone_object_id_list
;
static
UA_UInt32
lastCheckpoint
=
0
;
static
inline
int
JS_ToUint16
(
JSContext
*
ctx
,
uint16_t
*
pres
,
JSValueConst
val
)
{
return
JS_ToInt32
(
ctx
,
(
int32_t
*
)
pres
,
val
);
}
static
void
js_drone_finalizer
(
JSRuntime
*
rt
,
JSValue
val
)
{
JSDroneData
*
s
=
JS_GetOpaque
(
val
,
js_drone_class_id
);
JSDroneData
*
s
=
(
JSDroneData
*
)
JS_GetOpaque
(
val
,
js_drone_class_id
);
js_free_rt
(
rt
,
s
);
}
...
...
@@ -27,12 +21,14 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst new_target,
JSDroneData
*
s
;
JSValue
obj
=
JS_UNDEFINED
;
JSValue
proto
;
uint32_t
uint32
;
s
=
js_mallocz
(
ctx
,
sizeof
(
*
s
));
s
=
(
JSDroneData
*
)
js_mallocz
(
ctx
,
sizeof
(
*
s
));
if
(
!
s
)
return
JS_EXCEPTION
;
if
(
JS_To
Int32
(
ctx
,
&
s
->
id
,
argv
[
0
]))
if
(
JS_To
Uint32
(
ctx
,
&
uint32
,
argv
[
0
]))
goto
fail
;
s
->
id
=
(
uint16_t
)
uint32
;
proto
=
JS_GetPropertyStr
(
ctx
,
new_target
,
"prototype"
);
if
(
JS_IsException
(
proto
))
...
...
@@ -53,7 +49,7 @@ static JSValue js_drone_init(JSContext *ctx, JSValueConst this_val,
int
argc
,
JSValueConst
*
argv
)
{
int
nb
;
JSDroneData
*
s
=
JS_GetOpaque2
(
ctx
,
this_val
,
js_drone_class_id
);
JSDroneData
*
s
=
(
JSDroneData
*
)
JS_GetOpaque2
(
ctx
,
this_val
,
js_drone_class_id
);
if
(
!
s
)
return
JS_EXCEPTION
;
if
(
JS_ToInt32
(
ctx
,
&
nb
,
argv
[
0
]))
...
...
@@ -64,12 +60,12 @@ static JSValue js_drone_init(JSContext *ctx, JSValueConst this_val,
static
JSValue
js_drone_get
(
JSContext
*
ctx
,
JSValueConst
this_val
,
int
magic
)
{
JSDroneData
*
s
=
JS_GetOpaque2
(
ctx
,
this_val
,
js_drone_class_id
);
JSDroneData
*
s
=
(
JSDroneData
*
)
JS_GetOpaque2
(
ctx
,
this_val
,
js_drone_class_id
);
if
(
!
s
)
return
JS_EXCEPTION
;
switch
(
magic
)
{
case
0
:
return
JS_New
I
nt32
(
ctx
,
s
->
id
);
return
JS_New
Ui
nt32
(
ctx
,
s
->
id
);
case
1
:
return
JS_NewFloat64
(
ctx
,
s
->
latitude
);
case
2
:
...
...
@@ -88,10 +84,10 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
static
JSValue
js_drone_set_checkpoint
(
JSContext
*
ctx
,
JSValueConst
this_val
,
int
argc
,
JSValueConst
*
argv
)
{
UA_UInt16
checkpoint
;
if
(
JS_ToUint
16
(
ctx
,
&
checkpoint
,
argv
[
0
]))
uint32_t
uint32
;
if
(
JS_ToUint
32
(
ctx
,
&
uint32
,
argv
[
0
]))
return
JS_EXCEPTION
;
lastCheckpoint
=
checkpoint
;
lastCheckpoint
=
(
uint16_t
)
uint32
;
return
JS_UNDEFINED
;
}
...
...
@@ -115,8 +111,8 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
JS_CFUNC_DEF
(
"init"
,
1
,
js_drone_init
),
};
int
get_drone_id
(
int
nb
)
{
JSDroneData
*
s
=
JS_GetOpaque
(
drone_object_id_list
[
nb
],
js_drone_class_id
);
UA_UInt16
get_drone_id
(
UA_UInt32
nb
)
{
JSDroneData
*
s
=
(
JSDroneData
*
)
JS_GetOpaque
(
drone_object_id_list
[
nb
],
js_drone_class_id
);
return
s
->
id
;
}
...
...
@@ -158,8 +154,8 @@ VariableData pubsub_get_value(UA_String identifier) {
return
var_details
;
}
void
init_node_id
(
UA_UInt32
id
,
int
nb
,
int
magic
)
{
JSDroneData
*
s
=
JS_GetOpaque
(
drone_object_id_list
[
nb
],
js_drone_class_id
);
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
);
switch
(
magic
)
{
case
0
:
s
->
latitudeId
=
id
;
...
...
@@ -184,8 +180,8 @@ void init_node_id(UA_UInt32 id, int nb, int magic) {
void
pubsub_update_coordinates
(
UA_UInt32
id
,
const
UA_DataValue
*
var
)
{
JSDroneData
*
s
;
for
(
size_t
i
=
0
;
i
<
nbDrone
;
i
++
)
{
s
=
JS_GetOpaque
(
drone_object_id_list
[
i
],
js_drone_class_id
);
for
(
UA_UInt32
i
=
0
;
i
<
nbDrone
;
i
++
)
{
s
=
(
JSDroneData
*
)
JS_GetOpaque
(
drone_object_id_list
[
i
],
js_drone_class_id
);
if
(
s
->
latitudeId
==
id
)
{
s
->
latitude
=
*
(
UA_Double
*
)
var
->
value
.
data
;
return
;
...
...
@@ -212,8 +208,9 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
const
char
*
ipv6
;
const
char
*
port
;
const
char
*
net_iface
;
char
*
not_const_net_iface
;
char
urlBuffer
[
44
];
int
id
;
UA_UInt32
id
;
int
res
;
ipv6
=
JS_ToCString
(
ctx
,
argv
[
0
]);
...
...
@@ -225,10 +222,11 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
UA_STRING
(
"http://opcfoundation.org/UA-Profile/Transport/pubsub-udp-uadp"
);
UA_NetworkAddressUrlDataType
networkAddressUrl
;
networkAddressUrl
.
networkInterface
=
UA_STRING
(
net_iface
);
not_const_net_iface
=
strdup
(
net_iface
);
networkAddressUrl
.
networkInterface
=
UA_STRING
(
not_const_net_iface
);
networkAddressUrl
.
url
=
UA_STRING
(
urlBuffer
);
if
(
JS_To
I
nt32
(
ctx
,
&
id
,
argv
[
3
]))
if
(
JS_To
Ui
nt32
(
ctx
,
&
id
,
argv
[
3
]))
return
JS_EXCEPTION
;
res
=
runPubsub
(
&
transportProfile
,
&
networkAddressUrl
,
droneVariableArray
,
...
...
@@ -238,6 +236,7 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
JS_FreeCString
(
ctx
,
ipv6
);
JS_FreeCString
(
ctx
,
port
);
free
(
not_const_net_iface
);
JS_FreeCString
(
ctx
,
net_iface
);
return
JS_NewInt32
(
ctx
,
res
);
}
...
...
@@ -245,10 +244,10 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
static
JSValue
js_init_pubsub
(
JSContext
*
ctx
,
JSValueConst
this_val
,
int
argc
,
JSValueConst
*
argv
)
{
if
(
JS_To
I
nt32
(
ctx
,
&
nbDrone
,
argv
[
0
]))
if
(
JS_To
Ui
nt32
(
ctx
,
&
nbDrone
,
argv
[
0
]))
return
JS_EXCEPTION
;
drone_object_id_list
=
malloc
(
nbDrone
*
sizeof
(
JSValueConst
));
drone_object_id_list
=
(
JSValue
*
)
malloc
(
nbDrone
*
sizeof
(
JSValueConst
));
return
JS_NewInt32
(
ctx
,
0
);
}
...
...
@@ -330,8 +329,8 @@ static JSValue js_mavsdk_setTargetCoordinates(JSContext *ctx,
return
JS_NewInt32
(
ctx
,
mavsdk_setTargetCoordinates
(
la_arg_double
,
lo_arg_double
,
a_arg_double
,
y_arg_double
));
(
float
)
a_arg_double
,
(
float
)
y_arg_double
));
}
static
JSValue
js_mavsdk_setTargetLatLong
(
JSContext
*
ctx
,
JSValueConst
this_val
,
...
...
@@ -357,7 +356,7 @@ 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
(
altitude
));
return
JS_NewInt32
(
ctx
,
mavsdk_setAltitude
(
(
float
)
altitude
));
}
static
JSValue
js_mavsdk_setAirspeed
(
JSContext
*
ctx
,
JSValueConst
this_val
,
...
...
@@ -368,7 +367,7 @@ 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
(
altitude
));
return
JS_NewInt32
(
ctx
,
mavsdk_setAirspeed
(
(
float
)
altitude
));
}
static
JSValue
js_mavsdk_getYaw
(
JSContext
*
ctx
,
JSValueConst
this_val
,
...
...
@@ -438,12 +437,12 @@ static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst this_val,
static
JSValue
js_mavsdk_doParachute
(
JSContext
*
ctx
,
JSValueConst
this_val
,
int
argc
,
JSValueConst
*
argv
)
{
int
param
;
double
param
;
if
(
JS_To
Int32
(
ctx
,
&
param
,
argv
[
0
]))
if
(
JS_To
Float64
(
ctx
,
&
param
,
argv
[
0
]))
return
JS_EXCEPTION
;
return
JS_NewInt32
(
ctx
,
mavsdk_doParachute
(
param
));
return
JS_NewInt32
(
ctx
,
mavsdk_doParachute
(
(
float
)
param
));
}
static
JSValue
js_mavsdk_setTargetCoordinatesXYZ
(
JSContext
*
ctx
,
...
...
@@ -463,7 +462,7 @@ static JSValue js_mavsdk_setTargetCoordinatesXYZ(JSContext *ctx,
return
JS_NewInt32
(
ctx
,
mavsdk_setTargetCoordinatesXYZ
(
x_arg_double
,
y_arg_double
,
z_arg_double
));
(
float
)
z_arg_double
));
}
static
JSValue
js_mavsdk_setTargetAltitude
(
JSContext
*
ctx
,
JSValueConst
this_val
,
...
...
@@ -474,7 +473,7 @@ static JSValue js_mavsdk_setTargetAltitude(JSContext *ctx, JSValueConst this_val
if
(
JS_ToFloat64
(
ctx
,
&
a_arg_double
,
argv
[
0
]))
return
JS_EXCEPTION
;
return
JS_NewInt32
(
ctx
,
mavsdk_setTargetAltitude
(
a_arg_double
));
return
JS_NewInt32
(
ctx
,
mavsdk_setTargetAltitude
(
(
float
)
a_arg_double
));
}
static
JSValue
js_mavsdk_takeOff
(
JSContext
*
ctx
,
JSValueConst
this_val
,
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment