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

Initial commit

parents
/*jslint indent2 */
/*global console, std */
const EPSILON_YAW = 6;
const EPSILON_ALTITUDE = 5;
const TARGET_YAW = 0;
var running = false;
const wrongParameters = displayMessage.bind(null, "Wrong parameters");
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 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");
}
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();
}
}
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);
}
}
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;
return 0;
}
function getInput() {
let undefined_cmd;
let altitude;
let cmd;
let latitude;
let longitude;
let s;
let speed;
const help = `
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 "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;
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();
}
me.onUpdate = function() {
getInput();
};
/*jslint indent2 */
/*global console */
const ALTITUDE_DIFF = 30;
const FLIGH_ALTITUDE = 100;
const PARACHUTE_ALTITUDE = 35;
const EPSILON = 105;
const EPSILON_YAW = 6;
const EPSILON_ALTITUDE = 5;
const TARGET_YAW = 0;
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 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));
}
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();
}
}
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);
}
}
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");
}
function goTo(latitude, longitude, altitude, radius, sleep_time, epsilon) {
var cur_latitude;
var cur_longitude;
var d;
console.log(`Going to (${latitude}, ${longitude}) from (${getLatitude()}, ${getLongitude()})`);
exit_on_fail(
setTargetCoordinates(latitude, longitude, altitude, radius),
`Failed to go to (${latitude}, ${longitude})`
);
do {
sleep(sleep_time);
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})`);
} while(d > epsilon);
}
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,
30.001
);
sleep(500);
} while(leaderAltitudeRel > PARACHUTE_ALTITUDE);
console.log("[DEMO] Stop following...\n");
nextCheckpoint = droneDict[leaderId].lastCheckpoint + 1;
}
me.onStart = function() {
INITIAL_ALTITUDE = getInitialAltitude();
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE;
goToAltitude(START_ALTITUDE + 1, true, true);
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n");
loiter();
sleep(3000);
};
me.onUpdate = function() {
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`);
goTo(
checkpointList[i].latitude,
checkpointList[i].longitude,
checkpointList[i].altitude + FLIGH_ALTITUDE,
100,
1000,
105
);
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) {
goTo(
checkpointList[checkpointList.length - 1].latitude,
checkpointList[checkpointList.length - 1].longitude,
getAltitude(),
100,
1000,
105
);
}
while(distanceToLandingPoint > 20) {
console.log(`[DEMO] Waiting to reach landing point (current distance is ${distanceToLandingPoint})`);
distanceToLandingPoint = distance(getLatitude(), getLongitude(), landingPoint.latitude, landingPoint.longitude);
}
console.log("[DEMO] Landing...\n");
land();
};
/*jslint indent2 */
/*global console */
const ALTITUDE_DIFF = 30;
const FLIGH_ALTITUDE = 100;
const PARACHUTE_ALTITUDE = 35;
const EPSILON_ALTITUDE = 5;
let INITIAL_ALTITUDE;
let START_ALTITUDE;
var leaderAltitudeAbs;
var leaderAltitudeRel;
var leaderLatitude;
var leaderLongitude;
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();
}
}
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);
}
}
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,
30.001
);
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);
}
me.onStart = function() {
INITIAL_ALTITUDE = getInitialAltitude();
START_ALTITUDE = INITIAL_ALTITUDE + FLIGH_ALTITUDE;
goToAltitude(altitude, true, true);
waitForAltitude(START_ALTITUDE);
console.log("[DEMO] Setting loiter mode...\n");
loiter();
sleep(3000);
};
me.onUpdate = function() {
if(!IS_LEADER) {
followLeader(LEADER_ID, START_ALTITUDE, ALTITUDE_DIFF);
}
console.log("[DEMO] Loitering until manual intructions are given\n")
while(!landed()) {
sleep(1000);
}
};
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