diff --git a/bin/updateCommands.js b/bin/updateCommands.js old mode 100644 new mode 100755 diff --git a/lib/bebop.js b/lib/bebop.js index 88a7b96..76a13ca 100644 --- a/lib/bebop.js +++ b/lib/bebop.js @@ -291,6 +291,74 @@ Bebop.prototype._packetReceiver = function(message) { this._writePacket(this._createARStreamACK(arstreamFrame)); } + if( networkFrame.id === constants.BD_NET_DC_NAVDATA_ID ) { + var commandProject = networkFrame.data.readUInt8(0), + commandClass = networkFrame.data.readUInt8(1), + commandId = networkFrame.data.readUInt16LE(2); + + // Attitude + if( commandProject==1 && commandClass == 4 && commandId == 6 ) { + + var _roll = networkFrame.data.readFloatLE(4), + _pitch = networkFrame.data.readFloatLE(8), + _yaw = networkFrame.data.readFloatLE(12); + + this.navData.attitude = { + roll: _roll, + pitch: _pitch, + yaw: _yaw + } + } + + // Speed + if( commandProject==1 && commandClass == 4 && commandId == 5 ) { + + var _vx = networkFrame.data.readFloatLE(4), + _vy = networkFrame.data.readFloatLE(8), + _vz = networkFrame.data.readFloatLE(12); + + this.navData.speed = { + vx: _vx, + vy: _vy, + vz: _vz + } + } + + // Altitude + if( commandProject==1 && commandClass == 4 && commandId == 8 ) { + + var _altitude = networkFrame.data.readDoubleLE(4); + this.navData.altitude = _altitude; + } + + // GPS + if( commandProject==1 && commandClass == 4 && commandId == 4 ) { + var _lat = networkFrame.data.readDoubleLE(4), + _lon = networkFrame.data.readDoubleLE(12), + _alt = networkFrame.data.readDoubleLE(20); + + this.navData.pos = { + lat: _lat, + lon: _lon, + alt: _alt + }; + } + + // Camera + if( commandProject==1 && commandClass == 25 && commandId == 0 ) { + var _tilt = networkFrame.data.readUInt8(4), + _pan = networkFrame.data.readUInt8(5); + + this.navData.cam = { + tilt: _tilt, + pan: _pan + }; + } + + + this.emit("navdata", this.navData ); + } + // // libARCommands/Sources/ARCOMMANDS_Decoder.c#ARCOMMANDS_Decoder_DecodeBuffer // @@ -467,6 +535,19 @@ Bebop.prototype.counterClockwise = function(val) { return this; }; +Bebop.prototype.setPCMD = function ( lr_mov, fw_bw_mov, up_down_mov, yaw_rot ) { + this._pcmd.flag = 1; + this._pcmd.roll = Math.max( -100, Math.min(lr_mov, 100 ) ); + this._pcmd.pitch = Math.max( -100, Math.min(fw_bw_mov,100)); + this._pcmd.gaz = Math.max( -100, Math.min(up_down_mov,100)); + this._pcmd.yaw = Math.max( -100, Math.min(yaw_rot,100)); + return this; +} + +Bebop.prototype.getPCMD = function() { + return [ this._pcmd.roll || 0, this._pcmd.pitch || 0, this._pcmd.gaz || 0, this._pcmd.yaw || 0 ]; +} + Bebop.prototype.stop = Bebop.prototype.level = function() { this._pcmd = { flag: 0,