Skip to content

slapos
slapos
Commit 18242903 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾
Browse files
Options

Simulator API 

See merge request nexedi/slapos!1230
parents 00528782 ead1b62e
Hide whitespace changes
Inline Side-by-side
Showing with 420 additions and 707 deletions
component/mavsdk/buildout.cfg
View file @ 18242903
...@@ -12,7 +12,7 @@ parts = ...@@ -12,7 +12,7 @@ parts =
[c-astral-headers] [c-astral-headers]
recipe = slapos.recipe.build:gitclone recipe = slapos.recipe.build:gitclone
repository = https://lab.nexedi.com/nexedi/c-astral-c-library repository = https://lab.nexedi.com/nexedi/c-astral-c-library
revision = v0.1 revision = v1.0
git-executable = ${git:location}/bin/git git-executable = ${git:location}/bin/git
[gcc] [gcc]
...@@ -21,7 +21,7 @@ min_version = 7.1 ...@@ -21,7 +21,7 @@ min_version = 7.1
[mavsdk-source] [mavsdk-source]
recipe = slapos.recipe.build:gitclone recipe = slapos.recipe.build:gitclone
repository = https://github.com/mavlink/MAVSDK.git repository = https://github.com/mavlink/MAVSDK.git
revision = v0.37.0 revision = v0.39.0
git-executable = ${git:location}/bin/git git-executable = ${git:location}/bin/git
ignore-cloning-submodules = true ignore-cloning-submodules = true
......
component/qjs-wrapper/buildout.cfg
View file @ 18242903
...@@ -10,8 +10,8 @@ parts = qjs-wrapper ...@@ -10,8 +10,8 @@ parts = qjs-wrapper
recipe = slapos.recipe.cmmi recipe = slapos.recipe.cmmi
shared = true shared = true
configure-command = true configure-command = true
url = https://lab.nexedi.com/nexedi/qjs-wrapper/-/archive/v0.1/qjs-wrapper-v0.1.tar.gz url = https://lab.nexedi.com/nexedi/qjs-wrapper/-/archive/v1.0/qjs-wrapper-v1.0.tar.gz
md5sum = 4f0eb6f3bc5719a8ec043ce4f4d20747 md5sum = 0f1393fa15d46b2b551836197af9de46
environment = environment =
C_INCLUDE_PATH=include:${open62541:location}/include:${open62541:location}/deps:${open62541:location}/src/pubsub:${quickjs:location}/include C_INCLUDE_PATH=include:${open62541:location}/include:${open62541:location}/deps:${open62541:location}/src/pubsub:${quickjs:location}/include
CPLUS_INCLUDE_PATH=include:${mavsdk:location}/include:${mavsdk:location}/include/mavsdk CPLUS_INCLUDE_PATH=include:${mavsdk:location}/include:${mavsdk:location}/include/mavsdk
......
software/js-drone/README.md
View file @ 18242903
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
## Presentation ## ## Presentation ##
* Deploy 3 different scripts (`cli.js`, `demo.js` and `manual-flight.js`) 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
...@@ -14,12 +14,14 @@ ...@@ -14,12 +14,14 @@
* is-a-simulation: Must be set to 'true' to automatically take off during simulation * is-a-simulation: Must be set to 'true' to automatically take off during simulation
* leader-id: Id of the drone chosen to be the leader of the swarm
* multicast-ipv6: IPv6 of the multicast group of the swarm * multicast-ipv6: IPv6 of the multicast group of the swarm
* drone-id-list: Comma seperated list of the drone IDs of the swarm (recommanded to add the current drone ID) * net-if: Network interface used for multicast traffic
* drone-id-list: List of the drone IDs of the swarm (recommended to add the current drone ID)
* flight-script: User script to execute to fly drone swarm
## How it works ## ## How it works ##
Run `quickjs binary location` `desired script location` Run `quickjs binary location` `scripts location`/main.js `scripts location`/user.js
software/js-drone/buildout.hash.cfg
View file @ 18242903
...@@ -14,24 +14,16 @@ ...@@ -14,24 +14,16 @@
# not need these here). # not need these here).
[instance-profile] [instance-profile]
filename = instance.cfg filename = instance.cfg
md5sum = 99017d061d3be30746f6daef3a7bd8c4 md5sum = 7d4969239eb9d46bb44d57fc32b68c44
[cli] [main]
filename = cli.js filename = main.js
md5sum = 33271aeec124301604fdd406f0b339d1 md5sum = 4b1b27ea3e06b8d40cbc33f0ec617601
[common]
filename = common.js
md5sum = bacc70f2683c279ba4e0751de616d4ff
[demo]
filename = demo.js
md5sum = 31d8511e6d297643e65febe9a3ed2428
[manual-flight]
filename = manual-flight.js
md5sum = 175813fc8b2f19f91dae27ad4e14ab03
[pubsub] [pubsub]
filename = pubsub.js filename = pubsub.js
md5sum = d949c9a6cdaaa94e7bdd22df5e52fbf4 md5sum = 4a0c63f9e088fa525a3699484d193c4d
[worker]
filename = worker.js
md5sum = 5ed534e9ca56b9c0ee321b96b5d7c458
software/js-drone/cli.js deleted 100644 → 0
View file @ 00528782
/*jslint indent2 */
/*global console, std */
import {
loiter,
setAirspeed,
setAltitude,
setTargetLatLong,
reboot
} from "{{ qjs_wrapper }}"; //jslint-quiet
import {
connect,
displayDronePositions,
land,
quit,
startPubsub,
takeOff
} from "{{ common }}"; //jslint-quiet
/*jslint-disable*/
import * as std from "std";
/*jslint-enable*/
var running = false;
const wrongParameters = displayMessage.bind(null, "Wrong parameters");
function checkNumber(value, toExecute) {
return (
Number.isNaN(value)
? wrongParameters
: toExecute.bind(null, value)
);
}
function displayMessage(message) {
console.log(message);
return 0;
}
function exit() {
running = false;
quit();
return 0;
}
function getInput() {
let undefined_cmd;
let altitude;
let cmd;
let latitude;
let longitude;
let s;
let speed;
const help = `
connect
takeoff
land
goto(point)
gotoCoord(latitude, longitude)
altitude(altitude)
speed(speed)
positions
reboot
exit
help
`;
const f = std.fdopen(std.in, "r");
running = true;
while (running) {
std.printf("> ");
s = f.getline();
undefined_cmd = false;
switch (s) {
case "altitude":
std.printf("Altitude: ");
altitude = parseFloat(f.getline());
cmd = checkNumber(altitude, setAltitude);
break;
case "connect":
cmd = connect;
startPubsub();
break;
case "exit":
cmd = exit;
break;
case "gotoCoord":
std.printf("Latitude: ");
latitude = parseFloat(f.getline());
std.printf("Longitude: ");
longitude = parseFloat(f.getline());
cmd = checkNumber(longitude, checkNumber(latitude, setTargetLatLong));
break;
case "help":
cmd = displayMessage.bind(null, help);
break;
case "land":
cmd = land;
break;
case "loiter":
cmd = loiter;
break;
case "positions":
cmd = displayDronePositions;
break;
case "reboot":
cmd = reboot;
break;
case "speed":
std.printf("Speed: ");
speed = parseFloat(f.getline());
cmd = checkNumber(speed, setAirspeed);
break;
case "takeoff":
cmd = takeOff.bind(null, 60);
break;
default:
undefined_cmd = true;
cmd = displayMessage.bind(null, " Undefined command");
}
let ret = cmd();
if (ret) {
console.log(" [ERROR] function:\n", cmd, "\nreturn value:", ret);
}
else if (s !== "help" && !undefined_cmd) {
console.log(" Command successful");
}
};
f.close();
}
getInput();
software/js-drone/common.js deleted 100644 → 0
View file @ 00528782
/*jslint-disable*/
{% set comma_separated_drone_id_list = ', '.join(drone_id_list.split()) -%}
/*jslint-enable*/
import {
arm,
doParachute,
getAltitude,
getLatitude,
getLongitude,
getYaw,
initPubsub,
setAltitude,
setTargetLatLong,
start,
stop,
stopPubsub,
takeOffAndWait,
Drone
} from "{{ qjs_wrapper }}"; //jslint-quiet
import {exit} from "std";
import {sleep, Worker} from "os";
const IP = "{{ autopilot_ip }}";
const PORT = "7909";
export const IS_LEADER = {{ is_leader }};
export const LEADER_ID = {{ leader_id }};
export const SIMULATION = {{ is_a_simulation }};
export const EPSILON = 105;
const EPSILON_YAW = 6;
const EPSILON_ALTITUDE = 5;
const TARGET_YAW = 0;
export const ALTITUDE_DIFF = 30;
const URL = "udp://" + IP + ":" + PORT;
const LOG_FILE = "{{ log_dir }}/mavsdk-log";
const droneIdList = [{{ comma_separated_drone_id_list }}];
const droneDict = {};
var pubsubRunning = false;
var pubsubWorker;
export function connect() {
console.log("Will connect to", URL);
exit_on_fail(start(URL, LOG_FILE, 60), "Failed to connect to " + URL);
}
export function distance(lat1, lon1, lat2, lon2) {
const R = 6371e3; // meters
const la1 = lat1 * Math.PI/180; // la, lo in radians
const la2 = lat2 * Math.PI/180;
const lo1 = lon1 * Math.PI/180;
const lo2 = lon2 * Math.PI/180;
//haversine formula
const sinLat = Math.sin((la2 - la1)/2);
const sinLon = Math.sin((lo2 - lo1)/2);
const h = sinLat*sinLat + Math.cos(la1)*Math.cos(la2)*sinLon*sinLon
return 2*R*Math.asin(Math.sqrt(h));
}
export function displayDronePositions() {
if(!pubsubRunning)
console.log("You must start pubsub first !");
else {
for (const [id, drone] of Object.entries(droneDict)) {
console.log(id, drone.latitude, drone.longitude, drone.altitudeAbs, drone.altitudeRel);
}
}
return 0;
}
function exit_on_fail(ret, msg) {
if(ret) {
console.log(msg);
quit();
exit(-1);
}
}
export function quit() {
stop();
if(pubsubRunning) {
stopPubsub();
}
}
export function goToAltitude(target_altitude, wait, go) {
if(go) {
exit_on_fail(
setAltitude(target_altitude),
`Failed to go to altitude ${target_altitude} m`
);
}
if(wait) {
waitForAltitude(target_altitude);
}
}
export function land() {
var yaw;
while(true) {
yaw = getYaw();
console.log(`[DEMO] Waiting for yaw... (${yaw} , ${TARGET_YAW})`);
if(Math.abs(yaw - TARGET_YAW) < EPSILON_YAW) {
break;
}
sleep(250);
}
console.log("[DEMO] Deploying parachute...");
exit_on_fail(doParachute(2), "Failed to deploy parachute");
}
export function setLatLong(latitude, longitude, target_altitude) {
var cur_latitude;
var cur_longitude;
var d;
if(target_altitude !== 0) {
setAltitude(target_altitude, false, true);
}
console.log(`Going to (${latitude}, ${longitude}) from
(${getLatitude()}, ${getLongitude()})`);
exit_on_fail(
setTargetLatLong(latitude, longitude),
`Failed to go to (${latitude}, ${longitude})`
);
sleep(500);
while(true) {
cur_latitude = getLatitude();
cur_longitude = getLongitude();
d = distance(cur_latitude, cur_longitude, latitude, longitude);
console.log(`Waiting for drone to get to destination (${d} m),
(${cur_latitude} , ${cur_longitude}), (${latitude}, ${longitude})`);
if(d < EPSILON) {
sleep(6000);
return;
}
sleep(1000);
}
}
export function startPubsub() {
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" });
pubsubRunning = true;
return droneDict;
}
export function takeOff(altitude) {
exit_on_fail(arm(), "Failed to arm");
takeOffAndWait();
goToAltitude(altitude, true, true);
}
function waitForAltitude(target_altitude) {
var altitude = getAltitude();
while(Math.abs(altitude - target_altitude) > EPSILON_ALTITUDE) {
console.log(
`[DEMO] Waiting for altitude... (${altitude} , ${target_altitude})`);
sleep(1000);
altitude = getAltitude();
}
}
software/js-drone/demo.js deleted 100644 → 0
View file @ 00528782
/*jslint indent2 */
/*global console */
import {
getAltitude,
getAltitudeRel,
getInitialAltitude,
getLatitude,
getLongitude,
landed,
loiter,
setCheckpoint,
setTargetCoordinates
} from "{{ qjs_wrapper }}"; //jslint-quiet
import {sleep} from "os";
import {
connect,
distance,
goToAltitude,
land,
quit,
setLatLong,
startPubsub,
takeOff,
ALTITUDE_DIFF,
IS_LEADER,
LEADER_ID,
SIMULATION
} from "{{ common }}"; //jslint-quiet
const FLIGH_ALTITUDE = 100;
const PARACHUTE_ALTITUDE = 35;
const checkpointList = [
{
"latitude": 45.64492790560583,
"longitude": 14.25334942966329,
"altitude": 585.1806861589965
},
{
"latitude": 45.64316335436476,
"longitude": 14.26332880184475,
"altitude": 589.8802607573035
},
{
"latitude": 45.64911917196595,
"longitude": 14.26214792790128,
"altitude": 608.6648153348965
},
{
"latitude": 45.64122685351364,
"longitude": 14.26590493128597,
"altitude": 606.1448368129072
},
{
"latitude": 45.64543355564817,
"longitude": 14.27242391207985,
"altitude": 630.0829598206344
},
{
"latitude": 45.6372792927328,
"longitude": 14.27533492411138,
"altitude": 616.1839898415284
},
{
"latitude": 45.64061299543953,
"longitude": 14.26161958465814,
"altitude": 598.0603137354178
},
{
"latitude": 45.64032340702919,
"longitude": 14.2682896662383,
"altitude": 607.1243119862851
}
];
const landingPoint = [
{
"latitude": 45.6398451,
"longitude": 14.2699217
}
];
let INITIAL_ALTITUDE;
let START_ALTITUDE;
var nextCheckpoint = 0;
var distanceToLandingPoint = 100;
var leaderAltitudeAbs;
var leaderAltitudeRel;
var leaderLatitude;
var leaderLongitude;
function followLeader(leaderId, initialAltitude, altitudeDiff) {
goToAltitude(START_ALTITUDE + ALTITUDE_DIFF, false, true);
while(droneDict[leaderId].altitudeAbs == 0) {
console.log("[DEMO] Waiting for leader to send its altitude");
sleep(1000);
}
while(droneDict[leaderId].altitudeAbs < initialAltitude) {
console.log(`[DEMO] Waiting for leader to reach altitude ${initialAltitude} (currently ${droneDict[leaderId].altitudeAbs})`);
sleep(1000);
}
console.log("[DEMO] Switching to following mode...\n");
do {
leaderAltitudeAbs = droneDict[leaderId].altitudeAbs;
leaderAltitudeRel = droneDict[leaderId].altitudeRel;
leaderLatitude = droneDict[leaderId].latitude;
leaderLongitude = droneDict[leaderId].longitude;
setTargetCoordinates(
leaderLatitude,
leaderLongitude,
leaderAltitudeAbs + altitudeDiff,
0
);
sleep(500);
} while(leaderAltitudeRel > PARACHUTE_ALTITUDE);
console.log("[DEMO] Stop following...\n");
nextCheckpoint = droneDict[leaderId].lastCheckpoint + 1;
}
function waitForAltitude(altitude) {
var curAltitude;
do {
sleep(1000);
curAltitude = getAltitude();
console.log(
`[DEMO] Waiting for altitude... (${curAltitude} , ${altitude})`);
}
while(curAltitude < altitude);
}
function waitForLanding() {
while(!landed()) {
sleep(1000);
}
}
console.log("[DEMO] Connecting...\n");
connect();
const droneDict = startPubsub();
INITIAL_ALTITUDE = getInitialAltitude();
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE;
if(SIMULATION) {
takeOff(START_ALTITUDE + 1);
}
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n");
loiter();
sleep(3000);
if(!IS_LEADER) {
followLeader(LEADER_ID, START_ALTITUDE, ALTITUDE_DIFF);
}
for (let i = nextCheckpoint; i < checkpointList.length; i++) {
console.log(`[DEMO] Going to Checkpoint ${i}\n`);
setLatLong(checkpointList[i].latitude, checkpointList[i].longitude, checkpointList[i].altitude + FLIGH_ALTITUDE);
console.log(`[DEMO] Reached Checkpoint ${i}\n`);
setCheckpoint(i);
sleep(30000);
}
console.log("[DEMO] Setting altitude...\n");
goToAltitude(getAltitude() - getAltitudeRel() + PARACHUTE_ALTITUDE, true, true);
if(!IS_LEADER) {
setLatLong(
checkpointList[checkpointList.length - 1].latitude,
checkpointList[checkpointList.length - 1].longitude,
0
);
}
while(distanceToLandingPoint > 20) {
console.log(`[DEMO] Waiting to reache landing point (current distance is ${distanceToLandingPoint})`);
distanceToLandingPoint = distance(getLatitude(), getLongitude(), landingPoint.latitude, landingPoint.longitude);
}
console.log("[DEMO] Landing...\n");
land();
waitForLanding();
quit();
software/js-drone/instance-input-schema.json 0 → 100644
View file @ 18242903
{
"$schema": "http://json-schema.org/draft-06/schema",
"type": "object",
"description": "Parameters to instantiate JS drone",
"additionalProperties": false,
"properties": {
"autopilot-ip": {
"title": "IP address of the drone's autopilot",
"description": "IP used to create a connection with the autopilot.",
"type": "string",
"default": "192.168.27.1"
},
"id": {
"title": "Drone ID",
"description": "Unique identifier of the drone.",
"type": "integer",
"default": 1
},
"is-a-simulation": {
"title": "Set the flight as a simulation",
"description": "The option used to determine if the flight is real or if it is a simulation. This affects the context of the flight (e.g. if the take off is manual or automatic).",
"type": "boolean",
"default": false
},
"multicast-ipv6": {
"title": "IP of the multicast group",
"description": "IP address used to communicate with the other drones.",
"type": "string",
"default": "ff15::1111"
},
"net-if": {
"title": "Network interface",
"description": "Interface used for multicast traffic.",
"type": "string",
"default": "eth0"
},
"drone-id-list": {
"title": "List of drones IDs",
"description": "List of identifiers of drones.",
"type": "array",
"default": []
},
"flight-script": {
"title": "Script of the flight",
"description": "Script which will be executed for the flight",
"type": "string",
"textarea": true
}
}
}
software/js-drone/instance.cfg
View file @ 18242903
[buildout] [buildout]
parts = parts =
cli main
demo user
manual-flight
eggs-directory = ${buildout:eggs-directory} eggs-directory = ${buildout:eggs-directory}
develop-eggs-directory = ${buildout:develop-eggs-directory} develop-eggs-directory = ${buildout:develop-eggs-directory}
...@@ -27,51 +26,71 @@ cert = $${slap_connection:cert_file} ...@@ -27,51 +26,71 @@ cert = $${slap_connection:cert_file}
recipe = slapos.recipe.build recipe = slapos.recipe.build
slapparameter-dict = $${slap-configuration:configuration} slapparameter-dict = $${slap-configuration:configuration}
init = init =
options['autopilot-ip'] = options['slapparameter-dict'].get('autopilot_ip', '192.168.27.1') options['autopilot-ip'] = options['slapparameter-dict'].get('autopilot-ip', '192.168.27.1')
options['id'] = options['slapparameter-dict'].get('id', '1') options['id'] = options['slapparameter-dict'].get('id', 1)
options['is-a-simulation'] = options['slapparameter-dict'].get('is_a_simulation', 'false') options['is-a-simulation'] = options['slapparameter-dict'].get('is-a-simulation', False)
options['leader-id'] = options['slapparameter-dict'].get('leader_id', '1') options['multicast-ipv6'] = options['slapparameter-dict'].get('multicast-ip', 'ff15::1111')
options['is-leader'] = 'true' if options['id'] == options['leader-id'] else 'false' options['net-if'] = options['slapparameter-dict'].get('net-if', 'eth0')
options['multicast-ipv6'] = options['slapparameter-dict'].get('multicast_ip', 'ff15::1111') options['drone-id-list'] = options['slapparameter-dict'].get('drone-id-list', [])
options['drone-id-list'] = options['slapparameter-dict'].get('drone_id_list', '') options['is-a-drone'] = 'flight-script' in options['slapparameter-dict']
subscription_script = '''
me.onStart = function() {
me.f = me.fdopen(me.in, "r");
console.log("Use q to quit");
};
me.onUpdate= function(timestamp) {
while(me.f.getline() != "q") {
continue;
}
try {
me.f.close();;
} catch (error) {
console.error(error);
}
me.exit(0);
};
'''
options['flight-script'] = options['slapparameter-dict'].get('flight-script', subscription_script)
[js-dynamic-template] [js-dynamic-template]
recipe = slapos.recipe.template:jinja2 recipe = slapos.recipe.template:jinja2
rendered = $${directory:etc}/$${:_buildout_section_name_} rendered = $${directory:etc}/$${:_buildout_section_name_}.js
template = ${buildout:directory}/$${:_buildout_section_name_}.js template = ${buildout:directory}/$${:_buildout_section_name_}.js
extra-context = extra-context =
context = context =
raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so
$${:extra-context} $${:extra-context}
[common] [main]
<= js-dynamic-template <= js-dynamic-template
extra-context = extra-context =
key autopilot_ip drone:autopilot-ip key autopilot_ip drone:autopilot-ip
key drone_id_list drone:drone-id-list key id drone:id
key is_a_simulation drone:is-a-simulation key is_a_simulation drone:is-a-simulation
key is_leader drone:is-leader key is_a_drone drone:is-a-drone
key leader_id drone:leader-id
key log_dir directory:log key log_dir directory:log
key pubsub_script pubsub:rendered key pubsub_script pubsub:rendered
key worker_script worker:rendered
[cli] [pubsub]
<= js-dynamic-template
extra-context =
key common common:rendered
[demo]
<= js-dynamic-template <= js-dynamic-template
extra-context = extra-context =
key common common:rendered key ipv6 drone:multicast-ipv6
key net_if drone:net-if
[manual-flight] [user]
<= js-dynamic-template recipe = slapos.recipe.template:jinja2
extra-context = output = $${directory:etc}/user.js
key common common:rendered context =
key script drone:flight-script
inline = {{ script }}
[pubsub] [worker]
<= js-dynamic-template <= js-dynamic-template
extra-context = extra-context =
key drone_id_list drone:drone-id-list
key id drone:id key id drone:id
key ipv6 drone:multicast-ipv6 key is_a_drone drone:is-a-drone
software/js-drone/main.js 0 → 100644
View file @ 18242903
/* global console */
import {
arm,
start,
stop,
stopPubsub,
takeOffAndWait
} from "{{ qjs_wrapper }}";
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;
function connect() {
console.log("Will connect to", URL);
exitOnFail(start(URL, LOG_FILE, 60), "Failed to connect to " + URL);
}
function exitOnFail(ret, msg) {
if (ret) {
console.log(msg);
quit(1);
}
}
function quit(is_a_drone, exit_code) {
if (is_a_drone) {
stop();
}
stopPubsub();
exit(exit_code);
}
if (IS_A_DRONE) {
console.log("Connecting to aupilot\n");
connect();
}
pubsubWorker = new Worker("{{ pubsub_script }}");
pubsubWorker.onmessage = function(e) {
if (!e.data.publishing) {
pubsubWorker.onmessage = null;
}
}
worker.postMessage({type: "initPubsub"});
function takeOff() {
exitOnFail(arm(), "Failed to arm");
takeOffAndWait();
}
function load() {
if (IS_A_DRONE && SIMULATION) {
takeOff();
}
// First argument must provide the user script path
if (user_script === undefined) {
console.log('Please provide the user_script path.');
quit(1);
}
worker.postMessage({
type: "load",
path: user_script
});
}
function loop() {
let 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) {
let type = e.data.type;
if (type === 'initialized') {
pubsubWorker.postMessage({
action: "run",
id: {{ id }},
interval: FPS,
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));
software/js-drone/manual-flight.js deleted 100644 → 0
View file @ 00528782
/*jslint indent2 */
/*global console */
import {
getAltitude,
getInitialAltitude,
landed,
loiter,
setTargetCoordinates
} from "{{ qjs_wrapper }}"; //jslint-quiet
import {sleep} from "os";
import {
connect,
goToAltitude,
quit,
startPubsub,
takeOff,
ALTITUDE_DIFF,
IS_LEADER,
LEADER_ID,
SIMULATION
} from "{{ common }}"; //jslint-quiet
const FLIGH_ALTITUDE = 100;
const PARACHUTE_ALTITUDE = 35;
let INITIAL_ALTITUDE;
let START_ALTITUDE;
var leaderAltitudeAbs;
var leaderAltitudeRel;
var leaderLatitude;
var leaderLongitude;
function followLeader(leaderId, initialAltitude, altitudeDiff) {
goToAltitude(START_ALTITUDE + ALTITUDE_DIFF, false, true);
while(droneDict[leaderId].altitudeAbs == 0) {
console.log("[DEMO] Waiting for leader to send its altitude");
sleep(1000);
}
while(droneDict[leaderId].altitudeAbs < initialAltitude) {
console.log(`[DEMO] Waiting for leader to reach altitude ${initialAltitude} (currently ${droneDict[leaderId].altitudeAbs})`);
sleep(1000);
}
console.log("[DEMO] Switching to following mode...\n");
do {
leaderAltitudeAbs = droneDict[leaderId].altitudeAbs;
leaderAltitudeRel = droneDict[leaderId].altitudeRel;
leaderLatitude = droneDict[leaderId].latitude;
leaderLongitude = droneDict[leaderId].longitude;
setTargetCoordinates(
leaderLatitude,
leaderLongitude,
leaderAltitudeAbs + altitudeDiff,
0
);
sleep(500);
} while(leaderAltitudeRel > PARACHUTE_ALTITUDE);
console.log("[DEMO] Stop following...\n");
}
function waitForAltitude(altitude) {
var curAltitude;
do {
sleep(1000);
curAltitude = getAltitude();
console.log(
`[DEMO] Waiting for altitude... (${curAltitude} , ${altitude})`);
}
while(curAltitude < altitude);
}
function waitForLanding() {
while(!landed()) {
sleep(1000);
}
}
const droneDict = startPubsub();
console.log("[DEMO] Connecting...\n");
connect();
while(getInitialAltitude() == 0) {
console.log("[DEMO] Waiting for first telemetry\n");
sleep(1000);
}
INITIAL_ALTITUDE = getInitialAltitude();
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE;
if(SIMULATION) {
takeOff(START_ALTITUDE + 1);
}
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n");
loiter();
sleep(3000);
if(!IS_LEADER) {
followLeader(LEADER_ID, START_ALTITUDE, ALTITUDE_DIFF);
}
console.log("[DEMO] Loitering until manual intructions are given\n")
waitForLanding();
quit();
software/js-drone/pubsub.js
View file @ 18242903
...@@ -4,12 +4,12 @@ import {Worker} from "os"; ...@@ -4,12 +4,12 @@ import {Worker} from "os";
const PORT = "4840"; const PORT = "4840";
const IPV6 = "{{ ipv6 }}"; const IPV6 = "{{ ipv6 }}";
var parent = Worker.parent; let parent = Worker.parent;
function handle_msg(e) { function handle_msg(e) {
switch(e.data.action) { switch(e.data.action) {
case "run": case "run":
runPubsub(IPV6, PORT, "eth0", {{ id }}); runPubsub(IPV6, PORT, "{{ net_if }}", e.data.id, e.data.interval, e.data.publish);
parent.postMessage({running: false}); parent.postMessage({running: false});
parent.onmessage = null; parent.onmessage = null;
break; break;
......
software/js-drone/software.cfg
View file @ 18242903
...@@ -6,11 +6,9 @@ extends = ...@@ -6,11 +6,9 @@ extends =
parts = parts =
instance-profile instance-profile
common main
cli
demo
manual-flight
pubsub pubsub
worker
slapos-cookbook slapos-cookbook
[download-file-base] [download-file-base]
...@@ -23,17 +21,11 @@ recipe = slapos.recipe.template ...@@ -23,17 +21,11 @@ recipe = slapos.recipe.template
url = ${:_profile_base_location_}/${:filename} url = ${:_profile_base_location_}/${:filename}
output = ${buildout:directory}/template.cfg output = ${buildout:directory}/template.cfg
[common] [main]
<= download-file-base <= download-file-base
[cli] [pubsub]
<= download-file-base
[demo]
<= download-file-base
[manual-flight]
<= download-file-base <= download-file-base
[pubsub] [worker]
<= download-file-base <= download-file-base
software/js-drone/software.cfg.json 0 → 100644
View file @ 18242903
{
"name": "JS Drone",
"description": "JS Drone",
"serialisation": "xml",
"software-type": {
"default": {
"title": "Default",
"software-type": "default",
"description": "Default",
"request": "instance-input-schema.json",
"index": 0
}
}
}
software/js-drone/worker.js 0 → 100644
View file @ 18242903
/* global console, std */
import {
Drone,
triggerParachute,
getAltitude,
getAltitudeRel,
getInitialAltitude,
getLatitude,
getLongitude,
getYaw,
initPubsub,
isInManualMode,
landed,
loiter,
setAirspeed,
setAltitude,
setManualControlInput,
setMessage,
setTargetCoordinates
} from "{{ qjs_wrapper }}";
import * as std from "std";
import { Worker } from "os";
(function (console, Worker) {
// Every script is evaluated per drone
"use strict";
const drone_dict = {},
drone_id_list = [{{ drone_id_list }}],
IS_A_DRONE = {{ 'true' if is_a_drone else 'false' }};
let parent = Worker.parent,
user_me = {
//for debugging purpose
fdopen: std.fdopen,
in: std.in,
//required to fly
triggerParachute: triggerParachute,
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,
sendMsg: function(msg, id = -1) {
setMessage(JSON.stringify({ content: msg, dest_id: id }));
},
setAirspeed: setAirspeed,
setAltitude: setAltitude,
setTargetCoordinates: setTargetCoordinates
};
function loadUserScript(path) {
let 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) {
let type = evt.data.type,
message,
drone_id;
if (type === "initPubsub") {
initPubsub(drone_id_list.length);
for (let i = 0; i < drone_id_list.length; i++) {
drone_id = drone_id_list[i];
user_me.drone_dict[drone_id] = new Drone(drone_id);
user_me.drone_dict[drone_id].init(i);
}
parent.postMessage({type: "initialized"});
} else if (type === "load") {
loadUserScript(evt.data.path);
parent.postMessage({type: "loaded"});
} else if (type === "update") {
for (const [id, drone] of Object.entries(user_me.drone_dict)) {
message = drone.message
if (message.length > 0) {
message = JSON.parse(message);
if (user_me.id === id) {
continue;
}
if (user_me.hasOwnProperty("onGetMsg") &&
[-1, user_me.id].includes(message.dest_id)) {
user_me.onGetMsg(message.content);
}
}
}
// Call the drone onStart function
if (user_me.hasOwnProperty("onUpdate")) {
if (IS_A_DRONE && isInManualMode()) {
setManualControlInput();
}
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
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7