Initial commit

cli.js

+/*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();
+};

demo.js

+/*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();
+};

manual-flight.js

+/*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);
+  }
+};