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 @@
## 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
......@@ -26,4 +26,4 @@
## 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 @@
# not need these here).
[instance-profile]
filename = instance.cfg
md5sum = ae1ccd9272303ee0102cdcec8ddef562
md5sum = c0aa1ae91895b77bf223a386f228f302
[main]
filename = main.js
md5sum = 195c4ba934fb05f46dec18a5a132c450
md5sum = 271736c5286f0032b22878f915964429
[pubsub]
filename = pubsub.js
md5sum = d8798c3206f129e8715afd3ca23afa1a
[worker]
filename = worker.js
md5sum = 6d0d359bb59b2270042a04e0e35dda10
......@@ -22,12 +22,6 @@
"type": "boolean",
"default": false
},
"leader-id": {
"title": "Leader ID",
"description": "Unique identifier of the drone chosen to be the leader.",
"type": "integer",
"default": 1
},
"multicast-ipv6": {
"title": "IP of the multicast group",
"description": "IP address used to communicate with the other drones.",
......
[buildout]
parts =
main
user
eggs-directory = ${buildout:eggs-directory}
develop-eggs-directory = ${buildout:develop-eggs-directory}
......@@ -33,19 +34,24 @@ init =
options['multicast-ipv6'] = options['slapparameter-dict'].get('multicast-ip', 'ff15::1111')
options['net-if'] = options['slapparameter-dict'].get('net-if', 'eth0')
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 = '''
me.onStart = function() {
const f = std.fdopen(std.in, "r");
me.f = me.fdopen(me.in, "r");
console.log("Use q to quit");
};
me.onUpdate= function() {
while(f.getline() != "q") {
me.onUpdate= function(timestamp) {
while(me.f.getline() != "q") {
continue;
}
f.close();
try {
me.f.close();;
} catch (error) {
console.error(error);
}
me.exit(0);
};
'''
......@@ -60,22 +66,34 @@ context =
raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so
$${: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]
<= js-dynamic-template
extra-context =
key ipv6 drone:multicast-ipv6
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
extra-context =
key autopilot_ip drone:autopilot-ip
key drone_id_list drone:drone-id-list
key flight_script drone:flight-script
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 {
arm,
doParachute,
getAltitude,
getAltitudeRel,
getInitialAltitude,
getLatitude,
getLongitude,
getYaw,
initPubsub,
landed,
loiter,
reboot,
setAirspeed,
setAltitude,
setCheckpoint,
setTargetCoordinates,
start,
stop,
stopPubsub,
takeOffAndWait,
Drone
takeOffAndWait
} from "{{ qjs_wrapper }}";
import {sleep, Worker} from "os";
import * as std from "std";
const IP = "{{ autopilot_ip }}";
const URL = "udp://" + IP + ":7909";
const LOG_FILE = "{{ log_dir }}/mavsdk-log";
const IS_LEADER = {{ 'true' if is_leader else 'false' }};
const LEADER_ID = {{ leader_id }};
const IS_PUBLISHER = {{ 'true' if is_publisher else 'false' }}
const SIMULATION = {{ 'true' if is_a_simulation else 'false' }};
const droneIdList = {{ drone_id_list }};
const droneDict = {};
const pubsubScript = "{{ pubsub_script }}";
var pubsubWorker;
var pubsubRunning = false;
const me = {
'id': "{{ id }}",
'getCurrentPosition': function() {
return {
'x': getLatitude(),
'y': getLongitude(),
'z': getAltitudeRel()
};
},
'onStart': function() {},
'onUpdate': function() {},
'setAirspeed': setAirspeed,
'setTargetCoordinates': setTargetCoordinates
}
function connect() {
import { setTimeout, Worker } from "os";
import { exit } from "std";
(function (console, setTimeout, Worker) {
"use strict";
const IP = "{{ autopilot_ip }}",
URL = "udp://" + IP + ":7909",
LOG_FILE = "{{ log_dir }}/mavsdk-log",
IS_A_DRONE = {{ 'true' if is_a_drone else 'false' }},
SIMULATION = {{ 'true' if is_a_simulation else 'false' }};
// Use a Worker to ensure the user script
// does not block the main script
// (preventing it to be stopped for example)
// Create the update loop in the main script
// to prevent it to finish (and so, exit the quickjs process)
var pubsubWorker,
worker = new Worker("{{ worker_script }}"),
user_script = scriptArgs[1],
// Use the same FPS than browser's requestAnimationFrame
FPS = 1000 / 60,
previous_timestamp,
can_update = false,
i = 0;
function connect() {
console.log("Will connect to", URL);
exit_on_fail(start(URL, LOG_FILE, 60), "Failed to connect to " + URL);
}
}
function exit_on_fail(ret, msg) {
if(ret) {
function exit_on_fail(ret, msg) {
if (ret) {
console.log(msg);
quit();
std.exit(-1);
quit(1);
}
}
}
function quit() {
function quit(is_a_drone, exit_code) {
if (is_a_drone) {
stop();
if(pubsubRunning) {
stopPubsub();
}
}
function takeOff() {
exit_on_fail(arm(), "Failed to arm");
takeOffAndWait();
}
function waitForLanding() {
while(!landed()) {
sleep(1000);
stopPubsub();
exit(exit_code);
}
}
if(IS_PUBLISHER) {
if (IS_A_DRONE) {
console.log("Connecting to aupilot\n");
connect();
}
}
pubsubWorker = new Worker(pubsubScript);
pubsubWorker.onmessage = function(e) {
pubsubWorker = new Worker("{{ pubsub_script }}");
pubsubWorker.onmessage = function(e) {
if (!e.data.publishing)
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 });
pubsubRunning = true;
worker.postMessage({type: "initPubsub"});
{{ flight_script }}
function takeOff() {
exit_on_fail(arm(), "Failed to arm");
takeOffAndWait();
}
if(IS_PUBLISHER && SIMULATION) {
function load() {
if (IS_A_DRONE && SIMULATION) {
takeOff();
}
}
me.onStart()
me.onUpdate();
// First argument must provide the user script path
if (user_script === undefined) {
console.log('Please provide the user_script path.');
quit(1);
}
if(IS_PUBLISHER) {
waitForLanding();
quit();
} else {
stopPubsub();
};
worker.postMessage({
type: "load",
path: user_script
});
}
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 =
instance-profile
main
pubsub
worker
slapos-cookbook
[download-file-base]
......@@ -25,3 +26,6 @@ output = ${buildout:directory}/template.cfg
[pubsub]
<= 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