Permalink
Browse files

Added Parrot AR.Drone controller

  • Loading branch information...
1 parent c625f9e commit 870b6039bdb19a01d81a966c6fa9daf3d1bb4a8c @paullewis paullewis committed Sep 18, 2012
No changes.
View
@@ -0,0 +1,35 @@
+<!DOCTYPE html>
+<html>
+<head>
+ <meta charset="utf-8">
+ <title>AR.Drone 2.0</title>
+ <link href="styles/drone.css" rel="stylesheet"></link>
+</head>
+<body>
+
+ <h1>AR.Drone 2.0</h1>
+ <p id="message">Connect to the AR.Drone's Wifi network. Then press A
+ on your gamepad to get started.</p>
+ <p id="instructions">
+ <strong>Left shoulder button:</strong> Land<br/>
+ <strong>Right shoulder button:</strong> Take off<br/>
+ <strong>Left stick:</strong> move<br/>
+ <strong>Right stick:</strong> rotate left-right / raise and lower<br/></p>
+
+ <div id="commands">
+ <h2>Command log</h2>
+ <textarea id="log"></textarea>
+ </div>
+
+ <script src="third-party/gamepad.js"></script>
+
+ <script src="lib/Util.js"></script>
+ <script src="lib/NavData.js"></script>
+ <script src="lib/Command.js"></script>
+ <script src="lib/Sequence.js"></script>
+ <script src="lib/API.js"></script>
+ <script src="lib/Gamepad.js"></script>
+
+ <script src="scripts/app.js"></script>
+</body>
+</html>
View
@@ -0,0 +1,345 @@
+var DRONE = DRONE || {};
+DRONE.API = (function() {
+
+ // Constants
+ var DRONE_IP = "192.168.1.1";
+ var CLIENT_IP = "192.168.1.2";
+ var CONNECTIONS = 3;
+ var ONE_BUFFER = DRONE.Util.uint8ToArrayBuffer(1);
+ var TAKEOFF = 290718208;
+ var LAND = 290717696;
+ var COMMANDS_ENABLED = 1;
+
+ // Vars
+ var send = false;
+ var noop = function() {};
+ var keepAliveTimeout = 0;
+ var connectionsOutstanding = CONNECTIONS;
+ var sockets = {
+ "nav": {
+ protocol: "udp",
+ port: 5554,
+ socket: null,
+ type: 'bind',
+ ip: CLIENT_IP
+ },
+ "vid": {
+ protocol: "tcp",
+ port: 5555,
+ socket: null,
+ type: 'connect',
+ ip: DRONE_IP
+ },
+ "at": {
+ protocol: "udp",
+ port: 5556,
+ socket: null,
+ type: 'bind',
+ ip: CLIENT_IP
+ },
+ "cmd": {
+ protocol: "udp",
+ port: 5559,
+ socket: null,
+ type: 'connect',
+ ip: DRONE_IP
+ }
+ };
+ var status = {
+ verticalSpeed: 0,
+ angularSpeed: 0,
+ leftRightTilt: 0,
+ frontBackTilt: 0,
+ enabled: 0,
+ mode: LAND
+ };
+
+ callbacks = {
+
+ /**
+ * Called whenever one of our sockets has connected
+ */
+ onConnected: function(connectionResult) {
+
+ // if we have 0 or higher we're good
+ if(connectionResult >= 0) {
+
+ connectionsOutstanding--;
+
+ // if we're all done with our connecting
+ if(connectionsOutstanding === 0 && callbacks.onAllConnected) {
+
+ // start sending commands
+ sendKeepAliveCommand();
+ sendFlatTrim();
+ sendSensitivity();
+
+ // now enable controls
+ status.enabled = COMMANDS_ENABLED;
+
+ // go into the main command loop
+ loop();
+
+ if(callbacks.onAllConnected) {
+ callbacks.onAllConnected();
+ }
+ }
+
+ } else {
+ // Flag that there was an issue
+ if(callbacks.onConnectionError) {
+ callbacks.onConnectionError();
+ }
+ }
+ },
+
+ /* Placeholder callbacks */
+ onAllConnected: null,
+ onConnectionError: null
+ };
+
+ // -- Bootstrap
+
+ /**
+ * Initialises the connections to the Drone
+ */
+ function init(cbConnected, cbConnectionError) {
+
+ // assign the callbacks
+ callbacks.onAllConnected = cbConnected;
+ callbacks.onConnectionError = cbConnectionError;
+
+ // send the drone AT commands
+ connect(sockets['at']);
+
+ // get navigation data from the drone
+ connect(sockets['nav']);
+
+ // get a video stream from the drone
+ // TODO: connect(sockets['vid'], 'tcp');
+
+ // send admin commands to the drone
+ connect(sockets['cmd']);
+ }
+
+ /**
+ * Closes and discards all the socket connections
+ */
+ function shutdown() {
+ disconnect(sockets['at'].socket);
+ disconnect(sockets['nav'].socket);
+ disconnect(sockets['cmd'].socket);
+ // TODO: disconnect(sockets['vid'].socket);
+ }
+
+ /**
+ * Creates and connects a socket connection
+ */
+ function connect(sockRef) {
+
+ // grab the protocol, type, port and IP from
+ // the sockRef passed in and use that
+ // to create the socket
+ chrome.socket.create(sockRef.protocol, undefined, function(sockInfo) {
+ sockRef.socket = sockInfo.socketId;
+ chrome.socket[sockRef.type](sockRef.socket,
+ sockRef.ip,
+ sockRef.port,
+ callbacks.onConnected);
+ });
+ }
+
+ /**
+ * Disconnects and destroys the socket connection
+ */
+ function disconnect(sockRef) {
+ chrome.socket.disconnect(sockRef);
+ chrome.socket.destroy(sockRef);
+ }
+
+ // -- Stream functions
+
+ /**
+ * Converts an array of commands to a string and then
+ * an ArrayBuffer to send over the socket to the drone
+ */
+ function sendCommands(commands) {
+
+ var atSock = sockets['at'];
+ var commandBuffer = DRONE.Util.stringToArrayBuffer(commands.join(""));
+
+ // output the commands
+ log(commands.join(""));
+
+ // send all the commands
+ chrome.socket.sendTo(
+ atSock.socket,
+ commandBuffer,
+ DRONE_IP,
+ atSock.port,
+ noop);
+
+ // set up a keepalive just in case we don't
+ // for whatever reason send the other commands
+ clearTimeout(keepAliveTimeout);
+ keepAliveTimeout = setTimeout(sendKeepAliveCommand, 1000);
+ }
+
+ /**
+ * Helper function we use before letting the drone take off
+ * that we use to let it know that it's horizontal
+ */
+ function sendFlatTrim() {
+ sendCommands([new DRONE.Command('FTRIM')]);
+ }
+
+ /**
+ * Helper function that tells the drone what sensitivity
+ * we want. This is quite a high value (min = 0, max = 0.52)
+ */
+ function sendSensitivity() {
+ sendCommands([
+ new DRONE.Command('CONFIG', ['"control:euler_angle_max"', '"0.31"']),
+ new DRONE.Command('CONFIG', ['"control:indoor_euler_angle_max"', '"0.31"']),
+ new DRONE.Command('CONFIG', ['"control:outdoor_euler_angle_max"', '"0.31"'])
+ ]);
+ }
+
+ /**
+ * Informs the drone that it is going to be flying outdoors
+ */
+ function sendOutdoor() {
+ sendCommands([new DRONE.Command('CONFIG', ['"control:outdoor"', '"TRUE"'])]);
+ }
+
+ /**
+ * Sends a keepalive command and attempts to read
+ * back the latest data from the drone
+ */
+ function sendKeepAliveCommand() {
+
+ var navSock = sockets['nav'];
+ chrome.socket.sendTo(
+ navSock.socket,
+ (new Uint8Array([1])).buffer,
+ DRONE_IP,
+ navSock.port,
+ noop);
+
+ chrome.socket.read(
+ navSock.socket,
+ function(data) {
+
+ // if we have data parse it
+ // otherwise shutdown and call it a day
+ if(data.data.byteLength > 0) {
+ DRONE.NavData.parse(data.data);
+ } else {
+ shutdown();
+ }
+ });
+
+ // ensure we call this again
+ setTimeout(sendKeepAliveCommand, 1000);
+ }
+
+ /**
+ * The main loop. This sends the commands to the drone, including
+ * the tilt, speed and whether or not we want it to take off or land
+ */
+ function loop() {
+
+ commands = [
+
+ new DRONE.Command('PCMD_MAG', [
+
+ // Enables/Disables commands
+ status.enabled,
+
+ // Left - Right tilt
+ DRONE.Util.float32ToInt32(status.leftRightTilt),
+
+ // Front - Back tilt
+ DRONE.Util.float32ToInt32(status.frontBackTilt),
+
+ // Vertical Speed
+ DRONE.Util.float32ToInt32(status.verticalSpeed),
+
+ // Angular Speed
+ DRONE.Util.float32ToInt32(status.angularSpeed),
+
+ 0,
+
+ 0
+
+ ]),
+
+ // Take off / land
+ new DRONE.Command('REF', [
+ status.mode
+ ])
+ ];
+
+ // send and schedule the update
+ sendCommands(commands);
+
+ setTimeout(loop, 30);
+ }
+
+ // -- Actions
+
+ function takeOff() {
+ status.mode = TAKEOFF;
+ }
+
+ function land() {
+ status.mode = LAND;
+ }
+
+ function raiseLower(val) {
+ val = check(val, 0);
+ status.verticalSpeed = val;
+ }
+
+ function tiltLeftRight(val) {
+ val = check(val, 0);
+ status.leftRightTilt = val;
+ }
+
+ function tiltFrontBack(val) {
+ val = check(val, 0);
+ status.frontBackTilt = val;
+ }
+
+ function rotateLeftRight(val) {
+ val = check(val, 0);
+ status.angularSpeed = -val;
+ }
+
+ function allStop() {
+ status.angularSpeed = 0;
+ status.verticalSpeed = 0;
+ status.frontBackTilt = 0;
+ status.leftRightTilt = 0;
+ }
+
+ function check(val, defaultVal) {
+ if(typeof val === "undefined") {
+ val = defaultVal;
+ }
+ return val;
+ }
+
+ return {
+ init: init,
+ takeOff: takeOff,
+ land: land,
+ raiseLower: raiseLower,
+ tiltLeftRight: tiltLeftRight,
+ tiltFrontBack: tiltFrontBack,
+ rotateLeftRight: rotateLeftRight,
+ allStop: allStop,
+ shutdown: shutdown
+ };
+
+})();
@@ -0,0 +1,15 @@
+var DRONE = DRONE || {};
+DRONE.Command = function(command, parts) {
+ this.command = command;
+ this.parts = parts || [];
+};
+
+DRONE.Command.prototype.create = function(command, parts) {
+ return new DRONE.Command(command, parts);
+};
+
+DRONE.Command.prototype.toString = function() {
+ return "AT*" + this.command + "=" +
+ DRONE.Sequence.next() + (this.parts.length ? "," : "") +
+ this.parts.join(",") + "\r";
+};
Oops, something went wrong.

0 comments on commit 870b603

Please sign in to comment.