UNPKG

leap-drone

Version:

Allows users to fly Parrot Bluetooth drones using Leap Motion

977 lines (833 loc) 24.4 kB
/* global Buffer */ /*jslint node: true*/ 'use strict'; var noble = require('noble'); // Bluetooth Connection Module var debug = require('debug')('leapdrone'); // Debugging Module var EventEmitter = require('events').EventEmitter; // var e = EventEmitter.prototype._maxListeners = 100; // Handles memory issues when sending commands to drone. var util = require('util'); var _ = require('lodash'); /** * Constructs a new DroneDriver * * @param {Object} options to construct the drone with: * - {String} uuid to connect to. If this is omitted then it will connect to the first device starting with 'RS_' as the local name. * - logger function to call if/when errors occur. If omitted then uses console#log * @constructor */ var Drone = function(options) { EventEmitter.call(this); var uuid = (typeof options === 'string' ? options : undefined); options = options || {}; this.uuid = null; this.targets = uuid || options.uuid; if (this.targets && !util.isArray(this.targets)) { this.targets = this.targets.split(','); } this.logger = options.logger || debug; //use debug instead of console.log this.forceConnect = options.forceConnect || false; this.connected = false; this.discovered = false; this.ble = noble; this.peripheral = null; this.takenOff = false; this.driveStepsRemaining = 0; this.speeds = { yaw: 0, // turn pitch: 0, // forward/backward roll: 0, // left/right altitude: 0 // up/down }; /** * Used to store the 'counter' that's sent to each characteristic */ this.steps = { 'fa0a': 0, 'fa0b': 0, 'fa0c': 0 }; this.status = { stateValue: 0, flying: false, battery: 100 }; // handle disconnect gracefully this.ble.on('warning', function(message) { this.onDisconnect(); }.bind(this)); }; util.inherits(Drone, EventEmitter); /** * Drone.isDronePeripheral * * Accepts a BLE peripheral object record and returns true|false * if that record represents a Rolling Spider Drone or not. * * @param {Object} peripheral A BLE peripheral record * @return {Boolean} */ Drone.isDronePeripheral = function(peripheral) { if (!peripheral) { return false; } var localName = peripheral.advertisement.localName; var manufacturer = peripheral.advertisement.manufacturerData; var localNameMatch = localName && (localName.indexOf('RS_') === 0 || localName.indexOf('Mars_') === 0 || localName.indexOf('Travis_') === 0 || localName.indexOf('Maclan_')=== 0); var manufacturerMatch = manufacturer && (['4300cf1900090100', '4300cf1909090100', '4300cf1907090100'].indexOf(manufacturer) >= 0); // Is true for EITHER an "RS_" name OR manufacturer code. return localNameMatch || manufacturerMatch; }; // create client helper function to match ar-drone Drone.createClient = function(options) { return new Drone(options); }; /** * Connects to the drone over BLE * * @param callback to be called once connected * @todo Make the callback be called with an error if encountered */ Drone.prototype.connect = function(callback) { this.logger('DroneDriver#connect'); if (this.targets) { this.logger('DroneDriver finding: ' + this.targets.join(', ')); } this.ble.on('discover', function(peripheral) { this.logger('DroneDriver.on(discover)'); this.logger(peripheral); var isFound = false; var connectedRun = false; var matchType = 'Fuzzy'; // Peripheral specific var localName = peripheral.advertisement.localName; var uuid = peripheral.uuid; // Is this peripheral a Parrot Rolling Spider? var isDrone = Drone.isDronePeripheral(peripheral); var onConnected = function(error) { if (connectedRun) { return; } else { connectedRun = true; } if (error) { if (typeof callback === 'function') { callback(error); } } else { this.logger('Connected to: ' + localName); this.ble.stopScanning(); this.connected = true; this.setup(callback); } }.bind(this); this.logger(localName); if (this.targets) { this.logger(this.targets.indexOf(uuid)); this.logger(this.targets.indexOf(localName)); } if (!this.discovered) { if (this.targets && (this.targets.indexOf(uuid) >= 0 || this.targets.indexOf(localName) >= 0)) { matchType = 'Exact'; isFound = true; } else if ((typeof this.targets === 'undefined' || this.targets.length === 0) && isDrone) { isFound = true; } if (isFound) { this.logger(matchType + ' match found: ' + localName + ' <' + uuid + '>'); this.connectPeripheral(peripheral, onConnected); } } }.bind(this)); if (this.forceConnect || this.ble.state === 'poweredOn') { this.logger('DroneDriver.forceConnect'); this.ble.startScanning(); } else { this.logger('DroneDriver.on(stateChange)'); this.ble.on('stateChange', function(state) { if (state === 'poweredOn') { this.logger('DroneDriver#poweredOn'); this.ble.startScanning(); } else { this.logger('stateChange == ' + state); this.ble.stopScanning(); if (typeof callback === 'function') { callback(new Error('Error with Bluetooth Adapter, please retry')); } } }.bind(this)); } }; Drone.prototype.connectPeripheral = function(peripheral, onConnected) { this.discovered = true; this.uuid = peripheral.uuid; this.name = peripheral.advertisement.localName; this.peripheral = peripheral; this.ble.stopScanning(); this.peripheral.connect(onConnected); this.peripheral.on('disconnect', function() { this.onDisconnect(); }.bind(this)); }; /** * Sets up the connection to the drone and enumerate all of the services and characteristics. * * * @param callback to be called once set up * @private */ Drone.prototype.setup = function(callback) { this.logger('DroneDriver#setup'); this.peripheral.discoverAllServicesAndCharacteristics(function(error, services, characteristics) { if (error) { if (typeof callback === 'function') { callback(error); } } else { this.services = services; this.characteristics = characteristics; this.handshake(callback); } }.bind(this)); }; /** * Performs necessary handshake to initiate communication with the device. Also configures all notification handlers. * * * @param callback to be called once set up * @private */ Drone.prototype.handshake = function(callback) { this.logger('DroneDriver#handshake'); ['fb0f', 'fb0e', 'fb1b', 'fb1c', 'fd22', 'fd23', 'fd24', 'fd52', 'fd53', 'fd54'].forEach(function(key) { var characteristic = this.getCharacteristic(key); characteristic.notify(true); }.bind(this)); // Register listener for battery notifications. this.getCharacteristic('fb0f').on('data', function(data, isNotification) { if (!isNotification) { return; } this.status.battery = data[data.length - 1]; this.emit('battery'); this.logger('Battery level: ' + this.status.battery + '%'); }.bind(this)); /** * Flying statuses: * * 0: Landed * 1: Taking off * 2: Hovering * 3: ?? * 4: Landing * 5: Emergency / Cut out */ this.getCharacteristic('fb0e').on('data', function(data, isNotification) { if (!isNotification) { return; } if (data[2] !== 2) { return; } var prevState = this.status.flying, prevFlyingStatus = this.status.stateValue; this.logger('Flying status: ' + data[6]); if ([1, 2, 3, 4].indexOf(data[6]) >= 0) { this.status.flying = true; } this.status.stateValue = data[6]; if (prevState !== this.status.flying) { this.emit('stateChange'); } if (prevFlyingStatus !== this.status.stateValue) { this.emit('flyingStatusChange', this.status.stateValue); } }.bind(this)); setTimeout(function() { this.writeTo( 'fa0b', new Buffer([0x04, ++this.steps.fa0b, 0x00, 0x04, 0x01, 0x00, 0x32, 0x30, 0x31, 0x34, 0x2D, 0x31, 0x30, 0x2D, 0x32, 0x38, 0x00]), function(error) { setTimeout(function() { if (typeof callback === 'function') { callback(error); } }, 100); } ); }.bind(this), 100); }; /** * Gets a Characteristic by it's unique_uuid_segment * * @param {String} unique_uuid_segment * @returns Characteristic */ Drone.prototype.getCharacteristic = function(unique_uuid_segment) { var filtered = this.characteristics.filter(function(c) { return c.uuid.search(new RegExp(unique_uuid_segment)) !== -1; }); return filtered[0]; }; /** * Writes a Buffer to a Characteristic by it's unique_uuid_segment * * @param {String} unique_uuid_segment * @param {Buffer} buffer */ Drone.prototype.writeTo = function(unique_uuid_segment, buffer, callback) { if (!this.characteristics) { var e = new Error('You must have bluetooth enabled and be connected to a drone before executing a command. Please ensure Bluetooth is enabled on your machine and you are connected.'); if (callback) { callback(e); } else { throw e; } } else { if (typeof callback === 'function') { this.getCharacteristic(unique_uuid_segment).write(buffer, true, callback); } else { this.getCharacteristic(unique_uuid_segment).write(buffer, true); } } }; Drone.prototype.onDisconnect = function() { if (this.connected) { this.logger('Disconnected from drone: ' + this.name); if (this.ping) { clearInterval(this.ping); } this.ble.removeAllListeners(); this.connected = false; this.discovered = false; // // CSW - Removed because we do not know if the device is flying or not, so leave state as is. // var prevState = this.status.flying; // this.status.flying = false; // if (prevState !== this.status.flying) { // this.emit('stateChange'); // } // this.status.stateValue = 0; // this.emit('disconnected'); } }; /** * 'Disconnects' from the drone * * @param callback to be called once disconnected */ Drone.prototype.disconnect = function(callback) { this.logger('DroneDriver#disconnect'); if (this.connected) { this.peripheral.disconnect(function(error) { this.onDisconnect(); if (typeof callback === 'function') { callback(error); } }.bind(this)); } else { if (typeof callback === 'function') { callback(); } } }; /** * Starts sending the current speed values to the drone every 50 milliseconds * * This is only sent when the drone is in the air * * @param callback to be called once the ping is started */ Drone.prototype.startPing = function() { this.logger('DroneDriver#startPing'); this.ping = setInterval(function() { var buffer = new Buffer(19); buffer.fill(0); buffer.writeInt16LE(2, 0); buffer.writeInt16LE(++this.steps.fa0a, 1); buffer.writeInt16LE(2, 2); buffer.writeInt16LE(0, 3); buffer.writeInt16LE(2, 4); buffer.writeInt16LE(0, 5); buffer.writeInt16LE((this.driveStepsRemaining ? 1 : 0), 6); buffer.writeInt16LE(this.speeds.roll, 7); buffer.writeInt16LE(this.speeds.pitch, 8); buffer.writeInt16LE(this.speeds.yaw, 9); buffer.writeInt16LE(this.speeds.altitude, 10); buffer.writeFloatLE(0, 11); this.writeTo('fa0a', buffer); if (this.driveStepsRemaining < 0) { // go on the last command blindly } else if (this.driveStepsRemaining > 1) { // decrement the drive chain this.driveStepsRemaining--; } else { // reset to hover states this.emit('driveComplete', this.speeds); this.driveStepsRemaining = 0; this.hover(); } }.bind(this), 25); }; /** * Obtains the signal strength of the connected drone as a dBm metric. * * @param callback to be called once the signal strength has been identified */ Drone.prototype.signalStrength = function(callback) { this.logger('DroneDriver#signalStrength'); if (this.connected) { this.peripheral.updateRssi(callback); } else { if (typeof callback === 'function') { callback(new Error('Not connected to device')); } } }; Drone.prototype.drive = function(parameters, steps) { this.logger('DroneDriver#drive'); this.logger('driveStepsRemaining', this.driveStepsRemaining); var params = parameters || {}; if (!this.driveStepsRemaining || steps < 0) { this.logger('setting state'); // only apply when not driving currently, this causes you to exactly move -- prevents fluid this.driveStepsRemaining = steps || 1; this.speeds.roll = parameters.tilt || 0; this.speeds.pitch = parameters.forward || 0; this.speeds.yaw = parameters.turn || 0; this.speeds.altitude = parameters.up || 0; this.logger(this.speeds); // inject into ping flow. } }; // Operational Functions // Multiple use cases provided to support initial build API as well as // NodeCopter API and parity with the ar-drone library. /** * Instructs the drone to take off if it isn't already in the air */ function takeOff(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#takeOff'); if (this.status.battery < 10) { console.log('Battery level below 10%: ' + this.status.battery + '%'); this.logger('!!! BATTERY LEVEL TOO LOW !!!'); } if (!this.status.flying) { this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x00, 0x01, 0x00]) ); this.status.flying = true; } this.on('flyingStatusChange', function(newStatus) { if (newStatus === 2) { if (typeof callback === 'function') { callback(); } } }); } /** * Configures the drone to fly in 'wheel on' or protected mode. * */ function wheelOn(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#wheelOn'); this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x01, 0x02, 0x00, 0x01]) ); if (callback) { callback(); } } /** * Configures the drone to fly in 'wheel off' or unprotected mode. * */ function wheelOff(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#wheelOff'); this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x01, 0x02, 0x00, 0x00]) ); if (callback) { callback(); } } /** * Instructs the drone to land if it's in the air. */ function land(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#land'); if (this.status.flying) { this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x00, 0x03, 0x00]) ); this.on('flyingStatusChange', function(newStatus) { if (newStatus === 0) { this.status.flying = false; if (typeof callback === 'function') { callback(); } } }); } else { this.logger('Calling DroneDriver#land when it\'s not in the air isn\'t going to do anything'); if (callback) { callback(); } } } function toggle(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#toggle'); if (this.status.flying) { this.land(options, callback); } else { this.takeOff(options, callback); } } /** * Instructs the drone to do an emergency landing. */ function cutOff(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#cutOff'); this.status.flying = false; this.writeTo( 'fa0c', new Buffer([0x02, ++this.steps.fa0c & 0xFF, 0x02, 0x00, 0x04, 0x00]) , callback); } /** * Instructs the drone to trim. Make sure to call this before taking off. */ function flatTrim(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#flatTrim'); this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x00, 0x00, 0x00]), callback ); } /** * Instructs the drone to do a front flip. * * It will only do this if it's in the air * */ function frontFlip(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#frontFlip'); if (this.status.flying) { this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]), callback ); } else { this.logger('Calling DroneDriver#frontFlip when it\'s not in the air isn\'t going to do anything'); if (typeof callback === 'function') { callback(); } } if (callback) { callback(); } } /** * Instructs the drone to do a back flip. * * It will only do this if it's in the air * */ function backFlip(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#backFlip'); if (this.status.flying) { this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00]), callback ); } else { this.logger('Calling DroneDriver#backFlip when it\'s not in the air isn\'t going to do anything'); if (typeof callback === 'function') { callback(); } } if (callback) { callback(); } } /** * Instructs the drone to do a right flip. * * It will only do this if it's in the air * */ function rightFlip(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#rightFlip'); if (this.status.flying) { this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x04, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00]), callback ); } else { this.logger('Calling DroneDriver#rightFlip when it\'s not in the air isn\'t going to do anything'); if (typeof callback === 'function') { callback(); } } if (callback) { callback(); } } /** * Instructs the drone to do a left flip. * * It will only do this if it's in the air * */ function leftFlip(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } this.logger('DroneDriver#leftFlip'); if (this.status.flying) { this.writeTo( 'fa0b', new Buffer([0x02, ++this.steps.fa0b & 0xFF, 0x02, 0x04, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00]), callback ); } else { this.logger('Calling DroneDriver#leftFlip when it\'s not in the air isn\'t going to do anything'); if (typeof callback === 'function') { callback(); } } if (callback) { callback(); } } function driveBuilder(parameters) { var name = parameters.name, parameterToChange = parameters.parameterToChange, scaleFactor = parameters.scaleFactor; scaleFactor = scaleFactor || 1; return function(possibleOptions, possibleCallback) { var options, callback; if (_.isPlainObject(possibleOptions)) { options = possibleOptions; callback = _.isFunction(possibleCallback) ? possibleCallback : _.noop; } else if (_.isFunction(possibleOptions)) { callback = possibleOptions; } else { callback = _.noop; } this.logger('DroneDriver#' + name); if (this.status.flying) { options = options || {}; var speed = options.speed || 50; var steps = options.steps || 50; if (!validSpeed(speed)) { console.log('DroneDriver#' + name + 'was called with an invalid speed: ' + speed); this.logger('DroneDriver#' + name + 'was called with an invalid speed: ' + speed); callback(); } else { var driveParams = {}; driveParams[parameterToChange] = speed * scaleFactor; this.drive(driveParams, steps); // console.log('Drive Params: ', driveParams); this.once('driveComplete', callback); } } else { this.logger('DroneDriver#' + name + ' when it\'s not in the air isn\'t going to do anything'); callback(); } }; } /** * Instructs the drone to start moving upward at speed * * @param {float} speed at which the drive should occur * @param {float} steps the length of steps (time) the drive should happen */ var up = driveBuilder({ name: 'up', parameterToChange: 'up' }); /** * Instructs the drone to start moving downward at speed * * @param {float} speed at which the drive should occur * @param {float} steps the length of steps (time) the drive should happen */ var down = driveBuilder({ name: 'down', parameterToChange: 'up', scaleFactor: -1 }); /** * Instructs the drone to start moving forward at speed * * @param {float} speed at which the drive should occur. 0-100 values. * @param {float} steps the length of steps (time) the drive should happen */ var forward = driveBuilder({ name: 'forward', parameterToChange: 'forward' }); /** * Instructs the drone to start moving backward at speed * * @param {float} speed at which the drive should occur * @param {float} steps the length of steps (time) the drive should happen */ var backward = driveBuilder({ name: 'backward', parameterToChange: 'forward', scaleFactor: -1 }); /** * Instructs the drone to start spinning clockwise at speed * * @param {float} speed at which the rotation should occur * @param {float} steps the length of steps (time) the turning should happen */ var turnRight = driveBuilder({ name: 'turnRight', parameterToChange: 'turn' }); /** * Instructs the drone to start spinning counter-clockwise at speed * * @param {float} speed at which the rotation should occur * @param {float} steps the length of steps (time) the turning should happen */ var turnLeft = driveBuilder({ name: 'turnLeft', parameterToChange: 'turn', scaleFactor: -1 }); /** * Instructs the drone to start moving right at speed * * @param {float} speed at which the rotation should occur * @param {float} steps the length of steps (time) the turning should happen */ var tiltRight = driveBuilder({ name: 'tiltRight', parameterToChange: 'tilt' }); /** * Instructs the drone to start moving left at speed * * @param {float} speed at which the rotation should occur * @param {float} steps the length of steps (time) the turning should happen */ var tiltLeft = driveBuilder({ name: 'tiltLeft', parameterToChange: 'tilt', scaleFactor: -1 }); function hover(options, callback) { if (typeof options === 'function') { callback = options; options = {}; } //this.logger('DroneDriver#hover'); this.driveStepsRemaining = 0; this.speeds.roll = 0; this.speeds.pitch = 0; this.speeds.yaw = 0; this.speeds.altitude = 0; if (callback) { callback(); } } /** * Checks whether a speed is valid or not * * @private * @param {float} speed * @returns {boolean} */ function validSpeed(speed) { return (0 <= speed && speed <= 100); } // provide options for use case Drone.prototype.takeoff = takeOff; Drone.prototype.takeOff = takeOff; Drone.prototype.wheelOff = wheelOff; Drone.prototype.wheelOn = wheelOn; Drone.prototype.land = land; Drone.prototype.toggle = toggle; Drone.prototype.emergency = cutOff; Drone.prototype.emergancy = cutOff; Drone.prototype.flatTrim = flatTrim; Drone.prototype.calibrate = flatTrim; Drone.prototype.up = up; Drone.prototype.down = down; // animation Drone.prototype.frontFlip = frontFlip; Drone.prototype.backFlip = backFlip; Drone.prototype.rightFlip = rightFlip; Drone.prototype.leftFlip = leftFlip; // rotational Drone.prototype.turnRight = turnRight; Drone.prototype.clockwise = turnRight; Drone.prototype.turnLeft = turnLeft; Drone.prototype.counterClockwise = turnLeft; // directional Drone.prototype.forward = forward; Drone.prototype.backward = backward; Drone.prototype.tiltRight = tiltRight; Drone.prototype.tiltLeft = tiltLeft; Drone.prototype.right = tiltRight; Drone.prototype.left = tiltLeft; Drone.prototype.hover = hover; module.exports = Drone;