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
Pipeline #24852 failed with stage
    in 0 seconds
    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 =
    [c-astral-headers]
    recipe = slapos.recipe.build:gitclone
    repository = https://lab.nexedi.com/nexedi/c-astral-c-library
    revision = v0.1
    revision = v1.0
    git-executable = ${git:location}/bin/git
    [gcc]
    ......@@ -21,7 +21,7 @@ min_version = 7.1
    [mavsdk-source]
    recipe = slapos.recipe.build:gitclone
    repository = https://github.com/mavlink/MAVSDK.git
    revision = v0.37.0
    revision = v0.39.0
    git-executable = ${git:location}/bin/git
    ignore-cloning-submodules = true
    ......
    component/qjs-wrapper/buildout.cfg
    View file @ 18242903
    ......@@ -10,8 +10,8 @@ parts = qjs-wrapper
    recipe = slapos.recipe.cmmi
    shared = true
    configure-command = true
    url = https://lab.nexedi.com/nexedi/qjs-wrapper/-/archive/v0.1/qjs-wrapper-v0.1.tar.gz
    md5sum = 4f0eb6f3bc5719a8ec043ce4f4d20747
    url = https://lab.nexedi.com/nexedi/qjs-wrapper/-/archive/v1.0/qjs-wrapper-v1.0.tar.gz
    md5sum = 0f1393fa15d46b2b551836197af9de46
    environment =
    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
    ......
    software/js-drone/README.md
    View file @ 18242903
    ......@@ -2,7 +2,7 @@
    ## 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
    ......@@ -14,12 +14,14 @@
    * 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
    * 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 ##
    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 @@
    # not need these here).
    [instance-profile]
    filename = instance.cfg
    md5sum = 99017d061d3be30746f6daef3a7bd8c4
    md5sum = 7d4969239eb9d46bb44d57fc32b68c44
    [cli]
    filename = cli.js
    md5sum = 33271aeec124301604fdd406f0b339d1
    [common]
    filename = common.js
    md5sum = bacc70f2683c279ba4e0751de616d4ff
    [demo]
    filename = demo.js
    md5sum = 31d8511e6d297643e65febe9a3ed2428
    [manual-flight]
    filename = manual-flight.js
    md5sum = 175813fc8b2f19f91dae27ad4e14ab03
    [main]
    filename = main.js
    md5sum = 4b1b27ea3e06b8d40cbc33f0ec617601
    [pubsub]
    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]
    parts =
    cli
    demo
    manual-flight
    main
    user
    eggs-directory = ${buildout:eggs-directory}
    develop-eggs-directory = ${buildout:develop-eggs-directory}
    ......@@ -27,51 +26,71 @@ cert = $${slap_connection:cert_file}
    recipe = slapos.recipe.build
    slapparameter-dict = $${slap-configuration:configuration}
    init =
    options['autopilot-ip'] = options['slapparameter-dict'].get('autopilot_ip', '192.168.27.1')
    options['id'] = options['slapparameter-dict'].get('id', '1')
    options['is-a-simulation'] = options['slapparameter-dict'].get('is_a_simulation', 'false')
    options['leader-id'] = options['slapparameter-dict'].get('leader_id', '1')
    options['is-leader'] = 'true' if options['id'] == options['leader-id'] else 'false'
    options['multicast-ipv6'] = options['slapparameter-dict'].get('multicast_ip', 'ff15::1111')
    options['drone-id-list'] = options['slapparameter-dict'].get('drone_id_list', '')
    options['autopilot-ip'] = options['slapparameter-dict'].get('autopilot-ip', '192.168.27.1')
    options['id'] = options['slapparameter-dict'].get('id', 1)
    options['is-a-simulation'] = options['slapparameter-dict'].get('is-a-simulation', False)
    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-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]
    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
    extra-context =
    context =
    raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so
    $${:extra-context}
    [common]
    [main]
    <= js-dynamic-template
    extra-context =
    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_leader drone:is-leader
    key leader_id drone:leader-id
    key is_a_drone drone:is-a-drone
    key log_dir directory:log
    key pubsub_script pubsub:rendered
    key worker_script worker:rendered
    [cli]
    <= js-dynamic-template
    extra-context =
    key common common:rendered
    [demo]
    [pubsub]
    <= js-dynamic-template
    extra-context =
    key common common:rendered
    key ipv6 drone:multicast-ipv6
    key net_if drone:net-if
    [manual-flight]
    <= js-dynamic-template
    extra-context =
    key common common:rendered
    [user]
    recipe = slapos.recipe.template:jinja2
    output = $${directory:etc}/user.js
    context =
    key script drone:flight-script
    inline = {{ script }}
    [pubsub]
    [worker]
    <= js-dynamic-template
    extra-context =
    key drone_id_list drone:drone-id-list
    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";
    const PORT = "4840";
    const IPV6 = "{{ ipv6 }}";
    var parent = Worker.parent;
    let parent = Worker.parent;
    function handle_msg(e) {
    switch(e.data.action) {
    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.onmessage = null;
    break;
    ......
    software/js-drone/software.cfg
    View file @ 18242903
    ......@@ -6,11 +6,9 @@ extends =
    parts =
    instance-profile
    common
    cli
    demo
    manual-flight
    main
    pubsub
    worker
    slapos-cookbook
    [download-file-base]
    ......@@ -23,17 +21,11 @@ recipe = slapos.recipe.template
    url = ${:_profile_base_location_}/${:filename}
    output = ${buildout:directory}/template.cfg
    [common]
    [main]
    <= download-file-base
    [cli]
    <= download-file-base
    [demo]
    <= download-file-base
    [manual-flight]
    [pubsub]
    <= download-file-base
    [pubsub]
    [worker]
    <= 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