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

software/js-drone: use same logic as the simulator

Use an update loop to decide the instructions to give to the autopilot.
parent b013e21e
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
## Presentation ## ## Presentation ##
* Deploy `main.js` script on a drone to fly it * Deploy `user.js` script on a drone to fly it
* Compile all required libraries to run flight scripts * Compile all required libraries to run flight scripts
...@@ -26,4 +26,4 @@ ...@@ -26,4 +26,4 @@
## How it works ## ## How it works ##
Run `quickjs binary location` `scripts location`/main.js Run `quickjs binary location` `scripts location`/main.js `scripts location`/user.js
...@@ -14,12 +14,16 @@ ...@@ -14,12 +14,16 @@
# not need these here). # not need these here).
[instance-profile] [instance-profile]
filename = instance.cfg filename = instance.cfg
md5sum = ae1ccd9272303ee0102cdcec8ddef562 md5sum = c0aa1ae91895b77bf223a386f228f302
[main] [main]
filename = main.js filename = main.js
md5sum = 195c4ba934fb05f46dec18a5a132c450 md5sum = 271736c5286f0032b22878f915964429
[pubsub] [pubsub]
filename = pubsub.js filename = pubsub.js
md5sum = d8798c3206f129e8715afd3ca23afa1a md5sum = d8798c3206f129e8715afd3ca23afa1a
[worker]
filename = worker.js
md5sum = 6d0d359bb59b2270042a04e0e35dda10
...@@ -22,12 +22,6 @@ ...@@ -22,12 +22,6 @@
"type": "boolean", "type": "boolean",
"default": false "default": false
}, },
"leader-id": {
"title": "Leader ID",
"description": "Unique identifier of the drone chosen to be the leader.",
"type": "integer",
"default": 1
},
"multicast-ipv6": { "multicast-ipv6": {
"title": "IP of the multicast group", "title": "IP of the multicast group",
"description": "IP address used to communicate with the other drones.", "description": "IP address used to communicate with the other drones.",
......
[buildout] [buildout]
parts = parts =
main main
user
eggs-directory = ${buildout:eggs-directory} eggs-directory = ${buildout:eggs-directory}
develop-eggs-directory = ${buildout:develop-eggs-directory} develop-eggs-directory = ${buildout:develop-eggs-directory}
...@@ -33,19 +34,24 @@ init = ...@@ -33,19 +34,24 @@ init =
options['multicast-ipv6'] = options['slapparameter-dict'].get('multicast-ip', 'ff15::1111') options['multicast-ipv6'] = options['slapparameter-dict'].get('multicast-ip', 'ff15::1111')
options['net-if'] = options['slapparameter-dict'].get('net-if', 'eth0') options['net-if'] = options['slapparameter-dict'].get('net-if', 'eth0')
options['drone-id-list'] = options['slapparameter-dict'].get('drone-id-list', []) options['drone-id-list'] = options['slapparameter-dict'].get('drone-id-list', [])
options['is-publisher'] = 'flight-script' in options['slapparameter-dict'] options['is-a-drone'] = 'flight-script' in options['slapparameter-dict']
subscription_script = ''' subscription_script = '''
me.onStart = function() { me.onStart = function() {
const f = std.fdopen(std.in, "r"); me.f = me.fdopen(me.in, "r");
console.log("Use q to quit"); console.log("Use q to quit");
}; };
me.onUpdate= function() { me.onUpdate= function(timestamp) {
while(f.getline() != "q") { while(me.f.getline() != "q") {
continue; continue;
} }
f.close(); try {
me.f.close();;
} catch (error) {
console.error(error);
}
me.exit(0);
}; };
''' '''
...@@ -60,22 +66,34 @@ context = ...@@ -60,22 +66,34 @@ context =
raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so
$${:extra-context} $${:extra-context}
[main]
<= js-dynamic-template
extra-context =
key autopilot_ip drone:autopilot-ip
key id drone:id
key leader_id drone:leader-id
key is_leader drone:is-leader
key is_a_simulation drone:is-a-simulation
key is_a_drone drone:is-a-drone
key log_dir directory:log
key pubsub_script pubsub:rendered
key worker_script worker:rendered
[pubsub] [pubsub]
<= js-dynamic-template <= js-dynamic-template
extra-context = extra-context =
key ipv6 drone:multicast-ipv6 key ipv6 drone:multicast-ipv6
key net_if drone:net-if key net_if drone:net-if
[main] [user]
recipe = slapos.recipe.template:jinja2
output = $${directory:etc}/user.js
context =
key script drone:flight-script
inline = {{ script }}
[worker]
<= js-dynamic-template <= js-dynamic-template
extra-context = extra-context =
key autopilot_ip drone:autopilot-ip
key drone_id_list drone:drone-id-list key drone_id_list drone:drone-id-list
key flight_script drone:flight-script
key id drone:id key id drone:id
key is_a_simulation drone:is-a-simulation
key is_leader drone:is-leader
key is_publisher drone:is-publisher
key leader_id drone:leader-id
key log_dir directory:log
key pubsub_script pubsub:rendered
/*global console*/
import { import {
arm, arm,
doParachute,
getAltitude,
getAltitudeRel,
getInitialAltitude,
getLatitude,
getLongitude,
getYaw,
initPubsub,
landed,
loiter,
reboot,
setAirspeed,
setAltitude,
setCheckpoint,
setTargetCoordinates,
start, start,
stop, stop,
stopPubsub, stopPubsub,
takeOffAndWait, takeOffAndWait
Drone
} from "{{ qjs_wrapper }}"; } from "{{ qjs_wrapper }}";
import {sleep, Worker} from "os"; import { setTimeout, Worker } from "os";
import * as std from "std"; import { exit } from "std";
const IP = "{{ autopilot_ip }}"; (function (console, setTimeout, Worker) {
const URL = "udp://" + IP + ":7909"; "use strict";
const LOG_FILE = "{{ log_dir }}/mavsdk-log"; const IP = "{{ autopilot_ip }}",
URL = "udp://" + IP + ":7909",
const IS_LEADER = {{ 'true' if is_leader else 'false' }}; LOG_FILE = "{{ log_dir }}/mavsdk-log",
const LEADER_ID = {{ leader_id }}; IS_A_DRONE = {{ 'true' if is_a_drone else 'false' }},
const IS_PUBLISHER = {{ 'true' if is_publisher else 'false' }} SIMULATION = {{ 'true' if is_a_simulation else 'false' }};
const SIMULATION = {{ 'true' if is_a_simulation else 'false' }};
// Use a Worker to ensure the user script
const droneIdList = {{ drone_id_list }}; // does not block the main script
const droneDict = {}; // (preventing it to be stopped for example)
const pubsubScript = "{{ pubsub_script }}"; // Create the update loop in the main script
var pubsubWorker; // to prevent it to finish (and so, exit the quickjs process)
var pubsubRunning = false; var pubsubWorker,
worker = new Worker("{{ worker_script }}"),
const me = { user_script = scriptArgs[1],
'id': "{{ id }}", // Use the same FPS than browser's requestAnimationFrame
'getCurrentPosition': function() { FPS = 1000 / 60,
return { previous_timestamp,
'x': getLatitude(), can_update = false,
'y': getLongitude(), i = 0;
'z': getAltitudeRel()
}; function connect() {
},
'onStart': function() {},
'onUpdate': function() {},
'setAirspeed': setAirspeed,
'setTargetCoordinates': setTargetCoordinates
}
function connect() {
console.log("Will connect to", URL); console.log("Will connect to", URL);
exit_on_fail(start(URL, LOG_FILE, 60), "Failed to connect to " + URL); exit_on_fail(start(URL, LOG_FILE, 60), "Failed to connect to " + URL);
} }
function exit_on_fail(ret, msg) { function exit_on_fail(ret, msg) {
if(ret) { if (ret) {
console.log(msg); console.log(msg);
quit(); quit(1);
std.exit(-1); }
} }
}
function quit() { function quit(is_a_drone, exit_code) {
if (is_a_drone) {
stop(); stop();
if(pubsubRunning) {
stopPubsub();
} }
} stopPubsub();
exit(exit_code);
function takeOff() {
exit_on_fail(arm(), "Failed to arm");
takeOffAndWait();
}
function waitForLanding() {
while(!landed()) {
sleep(1000);
} }
}
if(IS_PUBLISHER) { if (IS_A_DRONE) {
console.log("Connecting to aupilot\n"); console.log("Connecting to aupilot\n");
connect(); connect();
} }
pubsubWorker = new Worker(pubsubScript); pubsubWorker = new Worker("{{ pubsub_script }}");
pubsubWorker.onmessage = function(e) { pubsubWorker.onmessage = function(e) {
if (!e.data.publishing) if (!e.data.publishing)
pubsubWorker.onmessage = null; pubsubWorker.onmessage = null;
} }
initPubsub(droneIdList.length);
for (let i = 0; i < droneIdList.length; i++) {
let id = droneIdList[i]
droneDict[id] = new Drone(id);
droneDict[id].init(i);
}
pubsubWorker.postMessage({ action: "run", id: me.id, publish: IS_PUBLISHER }); worker.postMessage({type: "initPubsub"});
pubsubRunning = true;
{{ flight_script }} function takeOff() {
exit_on_fail(arm(), "Failed to arm");
takeOffAndWait();
}
if(IS_PUBLISHER && SIMULATION) { function load() {
if (IS_A_DRONE && SIMULATION) {
takeOff(); takeOff();
} }
me.onStart() // First argument must provide the user script path
me.onUpdate(); if (user_script === undefined) {
console.log('Please provide the user_script path.');
quit(1);
}
if(IS_PUBLISHER) { worker.postMessage({
waitForLanding(); type: "load",
quit(); path: user_script
} else { });
stopPubsub(); }
};
function loop() {
var timestamp = Date.now(),
timeout;
if (can_update) {
if (FPS <= (timestamp - previous_timestamp)) {
// Expected timeout between every update
can_update = false;
worker.postMessage({
type: "update",
timestamp: timestamp
});
// Try to stick to the expected FPS
timeout = FPS - (timestamp - previous_timestamp - FPS);
previous_timestamp = timestamp;
} else {
timeout = FPS - (timestamp - previous_timestamp);
}
} else {
// If timeout occurs, but update is not yet finished
// wait a bit
timeout = FPS / 4;
}
// Ensure loop is not done with timeout < 1ms
setTimeout(loop, Math.max(1, timeout));
}
worker.onmessage = function (e) {
var type = e.data.type;
if (type === 'initialized') {
pubsubWorker.postMessage({
action: "run",
id: {{ id }},
publish: IS_A_DRONE
});
load();
} else if (type === 'loaded') {
previous_timestamp = -FPS;
can_update = true;
// Start the update loop
loop();
} else if (type === 'updated') {
can_update = true;
} else if (type === 'exited') {
worker.onmessage = null;
quit(IS_A_DRONE, e.data.exit);
} else {
console.log('Unsupported message type', type);
quit(IS_A_DRONE, 1);
}
};
}(console, setTimeout, Worker));
...@@ -8,6 +8,7 @@ parts = ...@@ -8,6 +8,7 @@ parts =
instance-profile instance-profile
main main
pubsub pubsub
worker
slapos-cookbook slapos-cookbook
[download-file-base] [download-file-base]
...@@ -25,3 +26,6 @@ output = ${buildout:directory}/template.cfg ...@@ -25,3 +26,6 @@ output = ${buildout:directory}/template.cfg
[pubsub] [pubsub]
<= download-file-base <= download-file-base
[worker]
<= download-file-base
/*global console*/
import {
Drone,
doParachute,
getAltitude,
getAltitudeRel,
getInitialAltitude,
getLatitude,
getLongitude,
getYaw,
initPubsub,
landed,
loiter,
setAirspeed,
setAltitude,
setCheckpoint,
setTargetCoordinates
} from "{{ qjs_wrapper }}";
import { Worker } from "os"
import * as std from "std";
(function (console, Worker) {
// Every script is evaluated per drone
"use strict";
const drone_dict = {},
drone_id_list = [{{ drone_id_list }}];
var parent = Worker.parent,
user_me = {
//for debugging purpose
fdopen: std.fdopen,
in: std.in,
//to move into user script
setCheckpoint: setCheckpoint,
//required to fly
doParachute: doParachute,
drone_dict: {},
exit: function(exit_code) {
parent.postMessage({type: "exited", exit: exit_code});
parent.onmessage = null;
},
getAltitudeAbs: getAltitude,
getCurrentPosition: function() {
return {
x: getLatitude(),
y: getLongitude(),
z: getAltitudeRel()
};
},
getInitialAltitude: getInitialAltitude,
getYaw: getYaw,
id: {{ id }},
landed: landed,
loiter: loiter,
setAirspeed: setAirspeed,
setAltitude: setAltitude,
setTargetCoordinates: setTargetCoordinates
};
function loadUserScript(path) {
var script_content = std.loadFile(path);
if (script_content === null) {
console.log('Failed to load user script ' + path);
std.exit(1);
}
try {
std.evalScript(
'function execUserScript(from, me) {' +
script_content +
'};'
);
} catch (e) {
console.log('Failed to evaluate user script', e);
std.exit(1);
}
execUserScript(null, user_me);
// Call the drone onStart function
if (user_me.hasOwnProperty('onStart')) {
user_me.onStart();
}
}
function handleMainMessage(evt) {
var type = evt.data.type;
if (type === 'initPubsub') {
initPubsub(drone_id_list.length);
for (let i = 0; i < drone_id_list.length; i++) {
let id = drone_id_list[i];
user_me.drone_dict[id] = new Drone(id);
user_me.drone_dict[id].init(i);
}
parent.postMessage({type: "initialized"});
}
else if (type === 'load') {
loadUserScript(evt.data.path);
parent.postMessage({type: "loaded"});
} else if (type === 'update') {
// Call the drone onStart function
if (user_me.hasOwnProperty('onUpdate')) {
user_me.onUpdate(evt.data.timestamp);
}
parent.postMessage({type: "updated"});
} else {
throw new Error('Unsupported message type', type);
}
}
parent.onmessage = function (evt) {
try {
handleMainMessage(evt);
} catch (error) {
// Catch all potential bug to exit the main process
// if it occurs
console.log(error);
std.exit(1);
}
};
}(console, Worker));
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