├── .gitignore ├── ODC.graffle ├── README.md ├── apps ├── droneSimulator │ ├── Main.scala │ ├── README.txt │ ├── res │ │ ├── obj │ │ │ └── grid.obj │ │ └── shaders │ │ │ ├── default.frag │ │ │ ├── default.vert │ │ │ ├── firstPass.frag │ │ │ ├── firstPass.vert │ │ │ ├── secondPass.frag │ │ │ └── secondPass.vert │ └── sim.rb ├── faceTracking │ ├── Main.scala │ ├── README.txt │ ├── face.rb │ ├── haarcascade_eye.xml │ ├── haarcascade_frontalface_alt.xml │ └── haarcascade_frontalface_default.xml ├── leapController │ ├── Main.scala │ ├── README.txt │ └── leap.rb └── maxmsp-external │ ├── OpenDroneControl.maxhelp │ └── maxdrone.scala ├── downloads ├── ODC_MaxTemplate_no_xuggle.zip ├── ODC_MaxTemplate_osx.zip ├── ODC_ProcessingTemplate.zip └── darc_workshop.pdf ├── examples ├── GetSensorData.scala ├── TakeoffLand.scala ├── TakeoffMoveLand.scala ├── droneOSC.scala ├── maxmsp │ └── droneBeatTrackingUsingDuration.maxpat ├── openframeworks │ └── ODC_SendRecvOsc │ │ ├── ODC_SendRecvOSC.xcodeproj │ │ ├── project.pbxproj │ │ ├── project.xcworkspace │ │ │ ├── contents.xcworkspacedata │ │ │ └── xcuserdata │ │ │ │ └── rjduranjr.xcuserdatad │ │ │ │ ├── UserInterfaceState.xcuserstate │ │ │ │ └── WorkspaceSettings.xcsettings │ │ ├── xcshareddata │ │ │ └── xcschemes │ │ │ │ ├── ODC_SendRecvOSC Debug.xcscheme │ │ │ │ └── ODC_SendRecvOSC Release.xcscheme │ │ └── xcuserdata │ │ │ └── rjduranjr.xcuserdatad │ │ │ └── xcschemes │ │ │ └── xcschememanagement.plist │ │ ├── Project.xcconfig │ │ ├── addons.make │ │ ├── openFrameworks-Info.plist │ │ └── src │ │ ├── main.cpp │ │ ├── testApp.cpp │ │ └── testApp.h └── processing │ ├── ODC_BouncingDrone │ └── ODC_BouncingDrone.pde │ ├── ODC_DroneOscP5 │ └── ODC_DroneOscP5.pde │ └── ODC_DroneVideo │ └── ODC_DroneVideo.pde ├── images ├── ODC_framework-1024x546.jpg ├── ODC_framework.jpg └── odc_videostill.jpg ├── odc └── src │ └── main │ └── scala │ ├── drone │ ├── DroneBase.scala │ ├── SimDrone.scala │ ├── sensors │ │ └── SensorData.scala │ └── video │ │ └── VideoStream.scala │ ├── net │ ├── OSCInterface.scala │ └── SimpleTelnetClient.scala │ ├── numeric │ └── TimesFloatNumeric.scala │ ├── spatial │ ├── Nav.scala │ ├── Quat.scala │ └── Vec.scala │ └── tracking │ ├── PIDController.scala │ └── Tracking.scala ├── platforms └── ardrone │ └── src │ └── main │ ├── java │ └── com │ │ └── codeminders │ │ └── ardrone │ │ ├── ARDrone.java │ │ ├── CommandQueue.java │ │ ├── CommandSender.java │ │ ├── DroneCommand.java │ │ ├── DroneStatusChangeListener.java │ │ ├── DroneVideoListener.java │ │ ├── LICENSE.txt │ │ ├── NavData.java │ │ ├── NavDataDecoder.java │ │ ├── NavDataListener.java │ │ ├── TestH264DataDecoder.java │ │ ├── TestH264DataDecoder2.java │ │ ├── VideoDataDecoder.java │ │ ├── commands │ │ ├── ATCommand.java │ │ ├── ConfigureCommand.java │ │ ├── ControlCommand.java │ │ ├── EmergencyCommand.java │ │ ├── FlatTrimCommand.java │ │ ├── HoverCommand.java │ │ ├── KeepAliveCommand.java │ │ ├── LandCommand.java │ │ ├── MoveCommand.java │ │ ├── PCMDCommand.java │ │ ├── PlayAnimationCommand.java │ │ ├── PlayLEDCommand.java │ │ ├── QuitCommand.java │ │ ├── RefCommand.java │ │ └── TakeOffCommand.java │ │ ├── controllers │ │ ├── Controller.java │ │ ├── ControllerData.java │ │ ├── ControllerStateChange.java │ │ ├── GameControllerState.java │ │ └── decoders │ │ │ ├── AfterGlowControllerDecoder.java │ │ │ ├── ControllerStateDecoder.java │ │ │ ├── MotioninJoyVirtualStateDecoder.java │ │ │ └── SonyPS3ControllerStateDecoder.java │ │ ├── data │ │ ├── ARDroneDataReader.java │ │ ├── ChannelProcessor.java │ │ ├── DataDecoder.java │ │ ├── decoder │ │ │ ├── ardrone10 │ │ │ │ ├── ARDrone10NavDataDecoder.java │ │ │ │ ├── ARDrone10VideoDataDecoder.java │ │ │ │ ├── navdata │ │ │ │ │ └── ARDrone10NavData.java │ │ │ │ └── video │ │ │ │ │ ├── BufferedVideoImage.java │ │ │ │ │ ├── ImageSlice.java │ │ │ │ │ └── MacroBlock.java │ │ │ └── ardrone20 │ │ │ │ ├── ARDrone20VideoDataDecoder.java │ │ │ │ └── video │ │ │ │ └── BufferedVideoImage.java │ │ ├── logger │ │ │ ├── ARDroneDataReaderAndLogWrapper.java │ │ │ ├── ChannelDataChunk.java │ │ │ ├── DataLogger.java │ │ │ └── LogStreamWrapper.java │ │ ├── navdata │ │ │ ├── ControlAlgorithm.java │ │ │ ├── CtrlState.java │ │ │ ├── FlyingState.java │ │ │ ├── Mode.java │ │ │ ├── NavDataFormatException.java │ │ │ ├── NavDataTag.java │ │ │ └── vision │ │ │ │ ├── Dimension.java │ │ │ │ ├── Point.java │ │ │ │ └── VisionTag.java │ │ └── reader │ │ │ ├── LigthUDPDataReader.java │ │ │ ├── TCPDataRader.java │ │ │ └── UDPDataReader.java │ │ ├── examples │ │ └── TakeOffAndLand.java │ │ └── version │ │ ├── DroneVersionReader.java │ │ └── ftp │ │ └── DroneFTPversionReader.java │ └── scala │ ├── ARDrone.scala │ └── ARDroneWithMixins.scala ├── project ├── build.scala └── plugins.sbt └── sbt /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | sbt-launch.jar 3 | lib/ 4 | target/ 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | OpenDroneControl [ODC] 2 | === 3 | 4 | [ODC is not under currently active development as of early 2014. If you would like to contribute to the project, contact us or submit a pull request.] 5 | 6 | OpenDroneControl [ODC] is an open source software platform for developing interactive artworks and research projects with aerial robotics. ODC was developed to be a community-supported framework for connecting commercially available quadcopter platforms to a common programming interface. The framework provides access to platform specific sensors and optionally allows for additional functionality such as navigation and tracking. 7 | 8 | ODC is compatible with creative coding software such as Processing, Max, and open Frameworks (oF). Additionally, ODC is designed for expansion through code modules, third party peripherals, integration with emerging aerial robotic platforms, and user developed applications. Application developers can utilize this common interface to easily target multiple drone platforms without redesigning their code which allows for rapid project development. 9 | 10 | ![](https://raw.githubusercontent.com/opendronecontrol/odc/master/images/odc_videostill.jpg "") 11 | *A TouchOSC interface communicating with an ODC project in Max, and an AR-Drone.* 12 | 13 | The development of this project was initiated by the Transvergent Research Group, including [Media Art and Technology](http://www.mat.ucsb.edu/) graduate student researchers [Tim Wood](http://www.fishuyo.com/), [Sterling Crispin](http://www.sterlingcrispin.com/), and [RJ Duran](http://rjduran.net). The technology was developed under the direction of Professor Marcos Novak, at the transLAB, housed within the California NanoSystems Institute (CNSI) at the University of California Santa Barbara. ODC originated from experiments within the transLAB’s motion tracking environment using a Parrot AR.Drone at the beginning of 2012. Initially the focus was to create an external Max object which could algorithmically control the drone using available spatial information provided by the OptiTrack system. 14 | 15 | Feel free to fork and explore the github repo to add functionality and improve the platform. 16 | 17 | To get started look at our notes from the [Drones & Aerial Robotics Workshop](http://opendronecontrol.org/darc-workshop/) hosted by ITP/NYU in October 2013. 18 | 19 | Getting Started 20 | === 21 | 22 | OpenDroneControl [ODC] is an open source software platform for developing interactive artworks and research projects with aerial robotics. http://opendronecontrol.org 23 | 24 | 25 | Currently [ODC] is a first attempt at creating a generalized, open, extensible Drone API with a few extra benefits: 26 | 27 | * Simple OpenSoundControl Interface - send control messages and receive sensor data over the network 28 | * Built in Tracking support - adds ability to move drone to a coordinate in 3D space given a source of spatial tracking information (infrared camera system, computer vision,..) 29 | 30 | 31 | [ODC] currently supports the ARDrone 1 and 2 platforms by utilizing an extension of [JavaDrone](https://code.google.com/p/javadrone/). Implementing additional platforms should be fairly straight forward although most likely require using a JNI bridge. 32 | 33 | 34 | [ODC] comes with a [Max/MSP/Jitter](http://cycling74.com/products/max/) external object for ARDrone platform! 35 | * download a template project (OSX) [here](http://opendronecontrol.org/downloads/ODC_MaxTemplate_osx.zip) 36 | * download a template project (other platforms) [here](http://opendronecontrol.org/downloads/ODC_MaxTemplate_no_xuggle.zip) 37 | 38 | [ODC] works in [Processing](http://processing.org/) 39 | * download a template project [here](http://opendronecontrol.org/downloads/ODC_ProcessingTemplate.zip) 40 | 41 | [ODC] is controllable from any device via OpenSoundControl! 42 | * example app DroneOSC acts as a standalone ARDrone controller 43 | 44 | [ODC] comes with a simple simulator program to test out flight paths and ideas without the hardware 45 | 46 | 47 | 48 | Build 49 | === 50 | 51 | Build Max mxj external 52 | 53 | ```sbt 54 | ./sbt "project maxmsp-external" package 55 | ``` 56 | 57 | Run examples 58 | ```sbt 59 | ./sbt "project examples" run 60 | ``` 61 | 62 | Run DroneSimulatotr 63 | ```sbt 64 | ./sbt "project DroneSimulator" run 65 | ``` 66 | 67 | 68 | Example Usage 69 | === 70 | 71 | ```scala 72 | import org.opendronecontrol.platforms.ardrone._ 73 | 74 | val drone = new ARDrone("192.168.1.1") 75 | drone.connect() 76 | Thread.sleep(1000) 77 | 78 | drone.takeOff() 79 | Thread.sleep(5000) // wait for drone to takeoff and stabalize 80 | 81 | var t = 0 // keep track of approximate total time 82 | var dt = 30 // time per iteration of control loop 83 | 84 | var period = 3000 // movement oscillation period 85 | 86 | // control loop 87 | // for smooth flight move commands should be sent at a consistent interval of 30ms 88 | while( t < 10000){ 89 | 90 | var phase = (t % period) / period.toFloat * (2*math.Pi) 91 | 92 | var rot = math.sin(phase).toFloat 93 | 94 | drone.move(0.0f, 0.0f, 0.0f, rot) // oscillate drone left/right 95 | 96 | // print out some sensor data 97 | if( drone.hasSensors() ){ 98 | println( drone.sensors("velocity").vec ) 99 | println( drone.sensors("gyroscope").vec ) 100 | println( drone.sensors("altimeter").float ) 101 | println( drone.sensors("battery").int ) 102 | } 103 | 104 | t += dt 105 | Thread.sleep(dt) 106 | } 107 | 108 | drone.land() 109 | drone.disconnect() 110 | ``` 111 | Example using OSC interface 112 | 113 | ```scala 114 | import org.opendronecontrol.platforms.ardrone._ 115 | 116 | val drone = new ARDrone 117 | drone.osc.start(8000) 118 | drone.osc.sendSensors("192.168.1.255", 8001) 119 | 120 | ``` 121 | The application is now listening for messages on port 8000 and broadcasting sensor data on port 8001. 122 | Example messages: 123 | ``` 124 | /connect 125 | /sendSensors "192.168.1.255" 8001 126 | /takeOff 127 | /move 0.0 0.0 0.0 0.5 128 | /config "maxEulerAngle" 0.2 129 | /config "maxVerticalSpeed" 1.0 130 | /led [pattern:Int] [frequency:Float] [duration(seconds):Int] 131 | /animation [pattern:Int] [duration(seconds):Int] 132 | /land 133 | /disconnect 134 | ``` 135 | 136 | DARC Workshop 137 | === 138 | 139 | Drones & Aerial Robotics Conference 140 | ==== 141 | 142 | October 11–13, 2013 | NYU/ITP | 721 Broadway, New York, NY 10003 143 | 144 | We will be presenting the ODC framework at the Hackathon hosted by Nodecopter on Sunday, October 13th. Check out the [conference schedule](https://droneconference.org/schedule/) and [hackathon schedule](http://nodecopter.com/2013/new-york/oct-13). The hackathon is open for free registration here: http://www.eventbrite.com/event/8293334587. 145 | 146 | Follow the conference and ODC at [@droneconference](https://twitter.com/droneconference), [@opendronectrl](https://twitter.com/opendronectrl). 147 | 148 | [Download Handout](https://github.com/opendronecontrol/odc/raw/master/downloads/darc_workshop.pdf) -------------------------------------------------------------------------------- /apps/droneSimulator/Main.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package apps 4 | package sim 5 | 6 | import drone._ 7 | 8 | import com.fishuyo._ 9 | import maths._ 10 | import graphics._ 11 | import spatial._ 12 | import io._ 13 | import dynamic._ 14 | 15 | import scala.collection.mutable.ListBuffer 16 | 17 | object Main extends App with GLAnimatable{ 18 | 19 | SimpleAppRun.loadLibs() 20 | GLScene.push(this) 21 | 22 | //simulation objects 23 | val simDrone = new SimDrone 24 | 25 | // start osc server to control virtual drone 26 | simDrone.osc.start(8000) 27 | 28 | val simBody = Primitive3D.cube(Pose(), Vec3(0.5f,.05f,.5f)) 29 | simBody.color.set(RGB(0.f,0.6f,0.f)) 30 | 31 | //Trace - 3d path data 32 | var trace = new Trace3D(400) 33 | 34 | // ground 35 | val ground = Primitive3D.cube(Pose(Vec3(0,-.03f,0),Quat()), Vec3(6,-.01f,6)) 36 | ground.color.set(RGB(0.f,0.f,.6f)) 37 | 38 | // moveTo cursor cube 39 | val moveCube = Primitive3D.cube(Pose(), Vec3(.05f)) 40 | 41 | // Ruby script runner - reloads on save 42 | val live = new Ruby("sim.rb") 43 | 44 | // Run the app 45 | SimpleAppRun() 46 | 47 | 48 | /* member methods */ 49 | 50 | override def init(){ 51 | Camera.nav.pos.set(0.1f,2.f,4.5f) 52 | Camera.nav.quat.set(1.f, -.13f,0.f,0.f) 53 | } 54 | override def draw(){ 55 | Shader.lighting = 1.f 56 | ground.draw() 57 | moveCube.draw() 58 | simBody.draw() 59 | Shader.lighting = 0.f 60 | trace.draw() 61 | } 62 | override def step(dt:Float){ 63 | live.step(dt) 64 | 65 | simDrone.step(dt) 66 | simBody.pose = simDrone.sPose 67 | } 68 | 69 | // auto convert Pose to Pose 70 | implicit def pose2Pose( p:org.opendronecontrol.spatial.Pose ) : com.fishuyo.spatial.Pose = { 71 | Pose( Vec3(p.pos.x,p.pos.y,p.pos.z), Quat(p.quat.w,p.quat.x,p.quat.y,p.quat.z) ) 72 | } 73 | implicit def vec2Vec( p:org.opendronecontrol.spatial.Vec3 ) : com.fishuyo.maths.Vec3 = { 74 | Vec3(p.x,p.y,p.z) 75 | } 76 | 77 | 78 | } 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /apps/droneSimulator/README.txt: -------------------------------------------------------------------------------- 1 | 2 | Drone Simulator 3 | 4 | This examples visualizes the SimDrone platform and lets you fly a virtual drone and test out ideas without drone hardware. 5 | The OSC interface is enabled by default on port 8000, so you can control the simulator by sending it commands via OSC. 6 | 7 | The program runs and monitors a ruby script sim.rb which is reloaded when saved. It contains some code to handle keyboard events, macbook multitouch events, and leap events. 8 | 9 | Controls 10 | 11 | f - toggle flight 12 | i - move forward 13 | j - move left 14 | k - move back 15 | l - move right 16 | y - move up 17 | h - move down 18 | 19 | n - add random waypoint to move towards 20 | 21 | macbook multitouch events: 22 | 2 finger pan - drone moveTo xz plane 23 | 3 finger pan - drone moveTo xy plane 24 | 25 | leap will also control the flight of the drone: 26 | open hand to takeoff 27 | close hand to land 28 | tilt hand to move 29 | 30 | -------------------------------------------------------------------------------- /apps/droneSimulator/res/shaders/default.frag: -------------------------------------------------------------------------------- 1 | 2 | varying float depth; 3 | varying vec3 normal; 4 | varying vec3 eye; 5 | varying vec3 lightPosition; 6 | varying vec4 color; 7 | 8 | void main() 9 | { 10 | // these values are set empirically based on the dimensions 11 | // of the bunny model 12 | float near = -0.4; 13 | float far = 1.5; 14 | 15 | // get shifted versions for better visualization 16 | float depthShift = (depth + 1.5)/2.1; //1.0 - ((depth - near) / (far - near)); 17 | vec3 normalShift = (normal + vec3(1.0)) * 0.5; 18 | 19 | //gl_FragData[0] = color; 20 | gl_FragData[0] = color*vec4(normalShift, depthShift); 21 | //gl_FragData[0] = vec4(normal, 1.0); 22 | //gl_FragData[0] = vec4(vec3(depthShift), 0.9); 23 | //gl_FragData[1] = vec4(normalize(eye), 1.0); 24 | //gl_FragData[2] = vec4(normalize(lightPosition), 1.0); 25 | } 26 | -------------------------------------------------------------------------------- /apps/droneSimulator/res/shaders/default.vert: -------------------------------------------------------------------------------- 1 | 2 | attribute vec4 a_position; 3 | attribute vec4 a_normal; 4 | attribute vec4 a_color; 5 | 6 | uniform vec4 u_color; 7 | uniform mat4 u_projectionViewMatrix; 8 | uniform mat4 u_modelViewMatrix; 9 | uniform mat4 u_normalMatrix; 10 | 11 | //invariant gl_Position; 12 | 13 | varying float depth; 14 | varying vec3 normal; 15 | varying vec3 eye; 16 | varying vec3 lightPosition; 17 | varying vec4 color; 18 | 19 | 20 | void main() 21 | { 22 | // get the depth and eye position 23 | vec4 transformedVertex = u_modelViewMatrix * a_position; 24 | //depth = -transformedVertex.z; 25 | depth = a_position.z; 26 | eye = -(u_modelViewMatrix * a_position).xyz;//-transformedVertex.xyz; 27 | 28 | // transform normals to the current view 29 | normal = a_normal.xyz; //normalize(u_normalMatrix * a_normal).xyz; 30 | 31 | // pass the light position through 32 | lightPosition = vec3(10.0,10.0,10.0); 33 | 34 | if( a_color != vec4(0.0,0.0,0.0,1.0)){ 35 | color = a_color; 36 | }else{ 37 | color = u_color; 38 | } 39 | //color = a_color; 40 | 41 | gl_Position = u_projectionViewMatrix * a_position; 42 | } 43 | 44 | -------------------------------------------------------------------------------- /apps/droneSimulator/res/shaders/firstPass.frag: -------------------------------------------------------------------------------- 1 | 2 | varying float depth; 3 | varying vec3 normal; 4 | varying vec3 eye; 5 | varying vec3 lightPosition; 6 | varying vec4 color; 7 | 8 | void main() 9 | { 10 | // these values are set empirically based on the dimensions 11 | // of the bunny model 12 | float near = -0.4; 13 | float far = 1.5; 14 | 15 | // get shifted versions for better visualization 16 | float depthShift = (depth + 1.5)/2.1; //1.0 - ((depth - near) / (far - near)); 17 | vec3 normalShift = (normal + vec3(1.0)) * 0.5; 18 | 19 | //gl_FragData[0] = color; 20 | gl_FragData[0] = color*vec4(normalShift, depthShift); 21 | //gl_FragData[0] = vec4(normal, 1.0); 22 | //gl_FragData[0] = vec4(vec3(depthShift), 0.9); 23 | //gl_FragData[1] = vec4(normalize(eye), 1.0); 24 | //gl_FragData[2] = vec4(normalize(lightPosition), 1.0); 25 | } 26 | -------------------------------------------------------------------------------- /apps/droneSimulator/res/shaders/firstPass.vert: -------------------------------------------------------------------------------- 1 | 2 | attribute vec4 a_position; 3 | attribute vec4 a_normal; 4 | attribute vec4 a_color; 5 | 6 | uniform vec4 u_color; 7 | uniform mat4 u_projectionViewMatrix; 8 | uniform mat4 u_modelViewMatrix; 9 | uniform mat4 u_normalMatrix; 10 | 11 | //invariant gl_Position; 12 | 13 | varying float depth; 14 | varying vec3 normal; 15 | varying vec3 eye; 16 | varying vec3 lightPosition; 17 | varying vec4 color; 18 | 19 | 20 | void main() 21 | { 22 | // get the depth and eye position 23 | vec4 transformedVertex = u_modelViewMatrix * a_position; 24 | //depth = -transformedVertex.z; 25 | depth = a_position.z; 26 | eye = -(u_modelViewMatrix * a_position).xyz;//-transformedVertex.xyz; 27 | 28 | // transform normals to the current view 29 | normal = a_normal.xyz; //normalize(u_normalMatrix * a_normal).xyz; 30 | 31 | // pass the light position through 32 | lightPosition = vec3(10.0,10.0,10.0); 33 | 34 | if( a_color != vec4(0.0,0.0,0.0,1.0)){ 35 | color = a_color; 36 | }else{ 37 | color = u_color; 38 | } 39 | //color = a_color; 40 | 41 | gl_Position = u_projectionViewMatrix * a_position; 42 | } 43 | 44 | -------------------------------------------------------------------------------- /apps/droneSimulator/res/shaders/secondPass.frag: -------------------------------------------------------------------------------- 1 | uniform sampler2D depthTexture; 2 | uniform sampler2D eyeTexture; 3 | uniform sampler2D lightTexture; 4 | 5 | varying vec2 v_texCoords; 6 | 7 | void main() 8 | { 9 | // pull everything we want from the textures 10 | float depth = texture2D(depthTexture, v_texCoords).a; 11 | vec3 normal = texture2D(depthTexture, v_texCoords).rgb; 12 | vec3 eye = texture2D(eyeTexture, v_texCoords).rgb; 13 | vec3 light = texture2D(lightTexture, v_texCoords).rgb; 14 | 15 | //edge detect 16 | float d = 0.005; 17 | vec2 coord = v_texCoords; 18 | float diff = -4.0 * depth; 19 | diff += texture2D(depthTexture, vec2(coord.x+d, coord.y)).a; 20 | diff += texture2D(depthTexture, vec2(coord.x-d, coord.y)).a; 21 | diff += texture2D(depthTexture, vec2(coord.x, coord.y+d)).a; 22 | diff += texture2D(depthTexture, vec2(coord.x, coord.y-d)).a; 23 | float edge = 1.0 - step(0.05,abs(diff)); 24 | 25 | //average normals; 26 | normal += texture2D(depthTexture, vec2(coord.x+d, coord.y)).rgb; 27 | normal += texture2D(depthTexture, vec2(coord.x-d, coord.y)).rgb; 28 | normal += texture2D(depthTexture, vec2(coord.x, coord.y+d)).rgb; 29 | normal += texture2D(depthTexture, vec2(coord.x, coord.y-d)).rgb; 30 | normal += texture2D(depthTexture, vec2(coord.x+d, coord.y+d)).rgb; 31 | normal += texture2D(depthTexture, vec2(coord.x-d, coord.y+d)).rgb; 32 | normal += texture2D(depthTexture, vec2(coord.x-d, coord.y+d)).rgb; 33 | normal += texture2D(depthTexture, vec2(coord.x+d, coord.y-d)).rgb; 34 | normal /= 9.0; 35 | 36 | // repackage the normal component 37 | normal = normal * 2.0 - vec3(1.0); 38 | normal = normalize(normal); 39 | 40 | // get a diffuse lighting component 41 | float diffuse = dot(normal, light); 42 | 43 | // make it gray 44 | diffuse *= .5; 45 | if(diffuse < 1.0) diffuse = step(0.5,diffuse)*0.7; 46 | 47 | // specular 48 | vec3 reflectVec = reflect(-light, normal); 49 | float spec = pow( max( dot(reflectVec, eye), 0.0), 10.0); 50 | spec = step(.3, spec); 51 | 52 | // cartoon phong 53 | vec3 color = (vec3(diffuse) + vec3(spec) )*edge; 54 | 55 | //gl_FragColor = vec4(normal, 1.0); 56 | gl_FragColor = vec4(vec3(depth), 1.0); 57 | //gl_FragColor = vec4(vec3(diffuse), 1.0); 58 | //gl_FragColor = vec4(vec3(1.0)*edge,1.0); 59 | 60 | //gl_FragColor = vec4( color, 1.0); 61 | } 62 | -------------------------------------------------------------------------------- /apps/droneSimulator/res/shaders/secondPass.vert: -------------------------------------------------------------------------------- 1 | 2 | attribute vec4 a_position; 3 | attribute vec2 a_texCoord0; 4 | 5 | uniform mat4 u_projectionViewMatrix; 6 | 7 | varying vec2 v_texCoords; 8 | 9 | void main() 10 | { 11 | // pass through the texture coordinate 12 | v_texCoords = a_texCoord0; 13 | 14 | // pass through the quad position 15 | gl_Position = u_projectionViewMatrix * a_position; 16 | } 17 | -------------------------------------------------------------------------------- /apps/droneSimulator/sim.rb: -------------------------------------------------------------------------------- 1 | require 'java' 2 | 3 | #### import packages #### 4 | module M 5 | include_package "com.fishuyo.io" 6 | include_package "com.fishuyo.io.leap" 7 | include_package "com.fishuyo.maths" 8 | include_package "com.fishuyo.spatial" 9 | include_package "com.fishuyo.graphics" 10 | include_package "com.fishuyo.util" 11 | include_package "org.opendronecontrol.apps.sim" 12 | end 13 | 14 | class Object 15 | class << self 16 | alias :const_missing_old :const_missing 17 | def const_missing c 18 | M.const_get c 19 | end 20 | end 21 | end 22 | ########################### 23 | 24 | $drone = Main.simDrone 25 | 26 | Main.simDrone.sPose.setIdentity() 27 | Main.trace.color1.set(1,1,1) 28 | 29 | ######## Drone Control Config ######### 30 | 31 | # $drone.tracking.posKp.set(0.5,1.5,0.5) 32 | # $drone.tracking.posKi.set(0.0,0,0.0) 33 | # $drone.tracking.posKd.set(20,40,20) 34 | # $drone.tracking.posKdd.set(0,0,0) 35 | 36 | 37 | ########### Keyboard input ############# 38 | Keyboard.clear() 39 | Keyboard.use() 40 | 41 | fly = false 42 | Keyboard.bind("f", lambda{ 43 | if fly 44 | $drone.land(); 45 | else 46 | $drone.takeOff(); 47 | end 48 | fly = !fly 49 | }) 50 | 51 | xzSpeed = 0.7 52 | ySpeed = 1.0 53 | rotSpeed = 0.7 54 | x=0.0 55 | y=0.0 56 | z=0.0 57 | r=0.0 58 | Keyboard.bind("j", lambda{ x=-xzSpeed; $drone.move(x,y,z,r) }) 59 | Keyboard.bind("l", lambda{ x=xzSpeed; $drone.move(x,y,z,r) }) 60 | Keyboard.bind("i", lambda{ z=-xzSpeed; $drone.move(x,y,z,r) }) 61 | Keyboard.bind("k", lambda{ z=xzSpeed; $drone.move(x,y,z,r) }) 62 | Keyboard.bind("y", lambda{ y=ySpeed; $drone.move(x,y,z,r) }) 63 | Keyboard.bind("h", lambda{ y=-ySpeed; $drone.move(x,y,z,r) }) 64 | Keyboard.bind("u", lambda{ r=-rotSpeed; $drone.move(x,y,z,r) }) 65 | Keyboard.bind("o", lambda{ r=rotSpeed; $drone.move(x,y,z,r) }) 66 | Keyboard.bindUp("j", lambda{ x=0.0 }) 67 | Keyboard.bindUp("l", lambda{ x=0.0 }) 68 | Keyboard.bindUp("i", lambda{ z=0.0 }) 69 | Keyboard.bindUp("k", lambda{ z=0.0 }) 70 | Keyboard.bindUp("y", lambda{ y=0.0 }) 71 | Keyboard.bindUp("h", lambda{ y=0.0 }) 72 | Keyboard.bindUp("u", lambda{ r=0.0 }) 73 | Keyboard.bindUp("o", lambda{ r=0.0 }) 74 | 75 | Keyboard.bind("n", lambda{ 76 | ra = Randf.apply(-2.0,2.0,false) 77 | $drone.tracking.addWaypoint(ra[],1.0,ra[],0) 78 | }) 79 | 80 | Keyboard.bind("t", lambda{ 81 | $drone.tracking.stop() 82 | }) 83 | 84 | 85 | ######## Trackpad gesture input ######### 86 | 87 | mx=0.0 88 | my=0.0 89 | mz=0.0 90 | delay2 = 0 91 | Trackpad.clear() 92 | Trackpad.connect() 93 | Trackpad.bind( lambda{|i,f| # i -> number of fingers detected 94 | # f -> array of (x,y,dx,dy) 95 | xx = f[0]*2 - 1 96 | yy = f[1]*2 - 1 97 | 98 | # use two fingers to change destination on xz plane 99 | if i == 2 100 | delay2 += 1 101 | if delay2 > 5 102 | mx = mx + f[2]*0.05 103 | mz = mz + f[3]*-0.05 104 | $drone.tracking.moveTo(mx,my,mz,0.0) 105 | end 106 | 107 | # use three fingers to change destination on xy plane 108 | elsif i == 3 109 | mx = mx + f[2]*0.05 110 | my = my + f[3]*0.05 111 | $drone.tracking.moveTo(mx,my,mz,0.0) 112 | end 113 | 114 | delay2 = 0 if i != 2 115 | 116 | if mx > 6.0 then mx = 6.0 117 | elsif mx < -6.0 then mx = -6.0 end 118 | if my > 6.0 then my = 6.0 119 | elsif my < 0.0 then my = 0.0 end 120 | if mz > 6.0 then mz = 6.0 121 | elsif mz < -6.0 then mz = -6.0 end 122 | }) 123 | 124 | 125 | 126 | ######## Leap gesture input ######### 127 | 128 | Leap.clear() 129 | Leap.connect() 130 | Leap.bind( lambda{ |frame| 131 | return if frame.hands().isEmpty() 132 | hand = frame.hands().get(0) 133 | count = hand.fingers().count() 134 | if count >= 3 135 | $drone.takeOff() 136 | elsif count < 2 137 | $drone.land() 138 | return 139 | end 140 | 141 | normal = hand.palmNormal() 142 | dir = hand.direction() 143 | pos = hand.palmPosition() 144 | y = pos.getY() - 160.0 145 | 146 | if y > 0.0 and y < 10.0 then 147 | y = 0.0 148 | elsif y > 10.0 149 | y = y - 10.0 150 | end 151 | 152 | y = y / 100.0 153 | y = -1.0 if y < -1.0 154 | y = 1.0 if y > 1.0 155 | 156 | $drone.move(-normal.roll(),y,dir.pitch() - 0.15 , dir.yaw() * 0.0 ) 157 | }) 158 | 159 | ######## Step function called each frame ####### 160 | 161 | def step(dt) 162 | 163 | # Step Position Controller 164 | pos = Main.simDrone.sPose.pos 165 | Main.simDrone.tracking.step( pos.x,pos.y,pos.z,0.0 ) 166 | 167 | # update moveTo cube 168 | dest = $drone.tracking.destPose.pos 169 | Main.moveCube.pose.pos.set(dest.x,dest.y,dest.z) 170 | 171 | # add Vec3 point to 3d trace of drones position 172 | pos = Vec3.new(pos.x,pos.y,pos.z) 173 | Main.trace.apply(pos) 174 | 175 | Shader.lightPosition.set( pos + Vec3.new(0,5,0)) 176 | 177 | end 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /apps/faceTracking/Main.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package apps 4 | package faceTracking 5 | 6 | import platforms.ardrone._ 7 | 8 | import com.fishuyo._ 9 | import graphics._ 10 | import dynamic._ 11 | 12 | import cv._ 13 | 14 | import scala.collection.mutable.ListBuffer 15 | import scala.collection.JavaConversions._ 16 | 17 | import com.badlogic.gdx.graphics.Pixmap 18 | import com.badlogic.gdx.graphics.glutils._ 19 | 20 | import org.opencv.core._ 21 | import org.opencv.highgui._ 22 | import org.opencv.imgproc._ 23 | 24 | import java.awt.image.BufferedImage 25 | import java.awt.image.DataBufferByte 26 | import java.nio.ByteBuffer 27 | import java.nio.ByteOrder 28 | import java.nio.FloatBuffer 29 | 30 | object Main extends App with GLAnimatable{ 31 | 32 | SimpleAppRun.loadLibs() 33 | System.loadLibrary(org.opencv.core.Core.NATIVE_LIBRARY_NAME) 34 | GLScene.push(this) 35 | 36 | // ODC ARDrone platform 37 | val drone = new ARDrone("192.168.1.1") 38 | 39 | // quad to render video stream on 40 | val quad = Model(Quad()) 41 | 42 | var frame:BufferedImage = _ 43 | var texData:FloatBuffer = _ 44 | var pix:Pixmap = _ 45 | var bytes:Array[Byte] = _ 46 | var texID = 0 47 | var (w,h) = (0,0) 48 | 49 | implicit var camera = new CalibratedCamera() 50 | var faceDetector = new FaceDetector(0.17/2.0) 51 | 52 | // Ruby script runner - reloads on save 53 | val live = new Ruby("face.rb") 54 | 55 | // Run the app 56 | SimpleAppRun() 57 | 58 | 59 | /* member methods */ 60 | 61 | override def draw(){ 62 | 63 | if(drone.hasVideo()) frame = drone.video.getFrame() 64 | 65 | if( frame != null){ 66 | // allocate texture if frame size changed or first frame 67 | if( w == 0 || w != frame.getWidth || h != frame.getHeight){ 68 | w = frame.getWidth 69 | h = frame.getHeight 70 | 71 | // scale quad to correct aspect ratio 72 | quad.scale.set(1.f, -(h/w.toFloat), 1.f) 73 | 74 | // allocate texture and byte buffer 75 | pix = new Pixmap(w,h, Pixmap.Format.RGB888) 76 | bytes = new Array[Byte](h*w*3) 77 | texID = Texture(pix) 78 | 79 | } 80 | 81 | val data = frame.getRGB(0,0,w,h,null,0,w) 82 | for( i <- (0 until data.length)){ 83 | val c = data(i) 84 | bytes(3*i) = (c >> 16 & 0xFF).toByte 85 | bytes(3*i+1) = (c >> 8 & 0xFF).toByte 86 | bytes(3*i+2) = (c & 0xFF).toByte 87 | } 88 | val img = new Mat(h,w,CvType.CV_8UC3) 89 | img.put(0,0,bytes) 90 | 91 | val small = new Mat() 92 | // scale image run faster 93 | Imgproc.resize(img,small, new Size(), 0.5,0.5,0) 94 | val count = faceDetector(small) 95 | 96 | val bb = pix.getPixels() 97 | bb.put(bytes) 98 | bb.rewind() 99 | 100 | if( count > 0){ 101 | // get face position and draw scaling back up to match full size frame 102 | val x = faceDetector.face.x*2 103 | val y = faceDetector.face.y*2 104 | val w = faceDetector.face.width*2 105 | val h = faceDetector.face.height*2 106 | pix.setColor(0.f,1.f,0.f,1.f) 107 | pix.drawRectangle(x,y,w,h) 108 | } 109 | 110 | Texture(texID).draw(pix,0,0) 111 | 112 | 113 | // bind and update texture on gpu 114 | Texture.bind(texID) 115 | Shader.texture = 1.f 116 | Shader.lighting = 0.f 117 | } 118 | 119 | quad.draw() 120 | } 121 | override def step(dt:Float){ 122 | live.step(dt) 123 | } 124 | 125 | } 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /apps/faceTracking/README.txt: -------------------------------------------------------------------------------- 1 | 2 | Face Tracking 3 | 4 | This example lets you control the drone with a Leap Motion Controller. It also displays the drone's video stream with tracked faces in green squares. Doesn't use the face data for much yet.. 5 | 6 | The program runs and monitors a ruby script face.rb which is reloaded when saved. It contains some code to handle keyboard events, and leap events. 7 | 8 | Controls 9 | 10 | c - connect to drone 11 | x - disconnect from drone 12 | 13 | f - toggle flight 14 | i - move forward 15 | j - move left 16 | k - move back 17 | l - move right 18 | y - move up 19 | h - move down 20 | u - rotate left 21 | o - rotate right 22 | 23 | 24 | leap will also control the flight of the drone: 25 | open hand to takeoff 26 | close hand to land 27 | tilt hand to move 28 | 29 | -------------------------------------------------------------------------------- /apps/faceTracking/face.rb: -------------------------------------------------------------------------------- 1 | require 'java' 2 | 3 | #### import packages magic ##### 4 | module M 5 | include_package "com.fishuyo.io" 6 | include_package "com.fishuyo.io.leap" 7 | include_package "com.fishuyo.maths" 8 | include_package "com.fishuyo.spatial" 9 | include_package "com.fishuyo.graphics" 10 | include_package "com.fishuyo.util" 11 | include_package "org.opendronecontrol.apps.faceTracking" 12 | end 13 | 14 | class Object 15 | class << self 16 | alias :const_missing_old :const_missing 17 | def const_missing c 18 | M.const_get c 19 | end 20 | end 21 | end 22 | ########################### 23 | 24 | $drone = Main.drone 25 | 26 | # $drone.config("maxEulerAngle", 0.25) 27 | 28 | 29 | ########### Keyboard input ############# 30 | Keyboard.clear() 31 | Keyboard.use() 32 | 33 | fly = false 34 | Keyboard.bind("f", lambda{ 35 | if fly 36 | $drone.land(); 37 | else 38 | $drone.takeOff(); 39 | end 40 | fly = !fly 41 | }) 42 | Keyboard.bind(" ", lambda{ $drone.land(); puts "land" }) 43 | 44 | 45 | Keyboard.bind("c", lambda{ $drone.connect(); $drone.osc.start(8000) }) 46 | Keyboard.bind("x", lambda{ $drone.disconnect(); $drone.osc.stop() }) 47 | 48 | xzSpeed = 0.7 49 | ySpeed = 1.0 50 | rotSpeed = 0.7 51 | x=0.0 52 | y=0.0 53 | z=0.0 54 | r=0.0 55 | Keyboard.bind("j", lambda{ x=-xzSpeed; $drone.move(x,y,z,r) }) 56 | Keyboard.bind("l", lambda{ x=xzSpeed; $drone.move(x,y,z,r) }) 57 | Keyboard.bind("i", lambda{ z=-xzSpeed; $drone.move(x,y,z,r) }) 58 | Keyboard.bind("k", lambda{ z=xzSpeed; $drone.move(x,y,z,r) }) 59 | Keyboard.bind("y", lambda{ y=ySpeed; $drone.move(x,y,z,r) }) 60 | Keyboard.bind("h", lambda{ y=-ySpeed; $drone.move(x,y,z,r) }) 61 | Keyboard.bind("u", lambda{ r=-rotSpeed; $drone.move(x,y,z,r) }) 62 | Keyboard.bind("o", lambda{ r=rotSpeed; $drone.move(x,y,z,r) }) 63 | Keyboard.bindUp("j", lambda{ x=0.0 }) 64 | Keyboard.bindUp("l", lambda{ x=0.0 }) 65 | Keyboard.bindUp("i", lambda{ z=0.0 }) 66 | Keyboard.bindUp("k", lambda{ z=0.0 }) 67 | Keyboard.bindUp("y", lambda{ y=0.0 }) 68 | Keyboard.bindUp("h", lambda{ y=0.0 }) 69 | Keyboard.bindUp("u", lambda{ r=0.0 }) 70 | Keyboard.bindUp("o", lambda{ r=0.0 }) 71 | 72 | 73 | 74 | ######## Leap gesture input ######### 75 | 76 | Leap.clear() 77 | Leap.connect() 78 | Leap.bind( lambda{ |frame| 79 | return if frame.hands().isEmpty() 80 | hand = frame.hands().get(0) 81 | count = hand.fingers().count() 82 | if count >= 3 83 | if $drone.hasSensors() 84 | $drone.takeOff() unless $drone.sensors.get("flying").bool 85 | end 86 | elsif count < 2 87 | $drone.land() 88 | return 89 | end 90 | 91 | return unless $drone.sensors.get("flying").bool 92 | 93 | # return if hand.fingers().count() < 3 94 | normal = hand.palmNormal() 95 | dir = hand.direction() 96 | pos = hand.palmPosition() 97 | y = pos.getY() - 160.0 98 | 99 | if y > 0.0 and y < 10.0 then 100 | y = 0.0 101 | elsif y > 10.0 102 | y = y - 10.0 103 | end 104 | 105 | y = y / 100.0 106 | y = -1.0 if y < -1.0 107 | y = 1.0 if y > 1.0 108 | 109 | $drone.move(-normal.roll(), y, dir.pitch() - 0.15, dir.yaw() * 0.0 ) # yaw control disabled because can be confusing! 110 | }) 111 | 112 | ######## Step function called each frame ####### 113 | 114 | def step(dt) 115 | 116 | end 117 | 118 | 119 | 120 | 121 | 122 | -------------------------------------------------------------------------------- /apps/leapController/Main.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package apps 4 | package leapController 5 | 6 | import platforms.ardrone._ 7 | 8 | import com.fishuyo._ 9 | import graphics._ 10 | import dynamic._ 11 | 12 | import java.awt.image.BufferedImage 13 | import java.nio.ByteBuffer 14 | import java.nio.ByteOrder 15 | import java.nio.FloatBuffer 16 | 17 | object Main extends App with GLAnimatable{ 18 | 19 | SimpleAppRun.loadLibs() 20 | GLScene.push(this) 21 | 22 | // ODC ARDrone platform 23 | val drone = new ARDrone("192.168.1.1") 24 | 25 | // quad to render video stream on 26 | val quad = Model(Quad()) 27 | 28 | var frame:BufferedImage = _ 29 | var texData:FloatBuffer = _ 30 | var texID = 0 31 | var (w,h) = (0,0) 32 | 33 | // Ruby script runner - reloads on save 34 | val live = new Ruby("leap.rb") 35 | 36 | // Run the app 37 | SimpleAppRun() 38 | 39 | 40 | /* member methods */ 41 | 42 | override def draw(){ 43 | 44 | if(drone.hasVideo()) frame = drone.video.getFrame() 45 | 46 | if( frame != null){ 47 | // allocate texture if frame size changed or first frame 48 | if( w == 0 || w != frame.getWidth || h != frame.getHeight){ 49 | w = frame.getWidth 50 | h = frame.getHeight 51 | 52 | // scale quad to correct aspect ratio 53 | quad.scale.set(1.f, -(h/w.toFloat), 1.f) 54 | 55 | // allocate texture data as float buffer 56 | val buffer = ByteBuffer.allocateDirect(w*h*4 * 4); 57 | buffer.order(ByteOrder.nativeOrder()); 58 | texData = buffer.asFloatBuffer(); 59 | texID = Texture(w,h,texData) 60 | } 61 | 62 | // set texture data from frame 63 | val fdata = new Array[Float](w*h*4) 64 | val data = frame.getRGB(0,0,w,h,null,0,w) 65 | for( i <- (0 until data.length)){ 66 | val c = data(i) 67 | fdata(4*i) = (c >> 16 & 0xFF) / 255.f 68 | fdata(4*i+1) = (c >> 8 & 0xFF) /255.f 69 | fdata(4*i+2) = (c & 0xFF) / 255.f 70 | fdata(4*i+3) = 1.f 71 | } 72 | texData.put(fdata) 73 | texData.rewind 74 | 75 | // bind and update texture on gpu 76 | Texture.bind(texID) 77 | Texture(texID).getTextureData().consumeCompressedData() 78 | Shader.texture = 1.f 79 | Shader.lighting = 0.f 80 | } 81 | 82 | quad.draw() 83 | } 84 | override def step(dt:Float){ 85 | live.step(dt) 86 | } 87 | 88 | } 89 | 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /apps/leapController/README.txt: -------------------------------------------------------------------------------- 1 | 2 | Leap Drone Controller 3 | 4 | This example lets you control the drone with a Leap Motion Controller. It also displays the drone's video stream. 5 | 6 | The program runs and monitors a ruby script leap.rb which is reloaded when saved. It contains some code to handle keyboard events, and leap events. 7 | 8 | Controls 9 | 10 | c - connect to drone 11 | x - disconnect from drone 12 | 13 | f - toggle flight 14 | i - move forward 15 | j - move left 16 | k - move back 17 | l - move right 18 | y - move up 19 | h - move down 20 | u - rotate left 21 | o - rotate right 22 | 23 | 24 | leap will also control the flight of the drone: 25 | open hand to takeoff 26 | close hand to land 27 | tilt hand to move 28 | 29 | -------------------------------------------------------------------------------- /apps/leapController/leap.rb: -------------------------------------------------------------------------------- 1 | require 'java' 2 | 3 | #### import packages magic ##### 4 | module M 5 | include_package "com.fishuyo.io" 6 | include_package "com.fishuyo.io.leap" 7 | include_package "com.fishuyo.maths" 8 | include_package "com.fishuyo.spatial" 9 | include_package "com.fishuyo.graphics" 10 | include_package "com.fishuyo.util" 11 | include_package "org.opendronecontrol.apps.leapController" 12 | end 13 | 14 | class Object 15 | class << self 16 | alias :const_missing_old :const_missing 17 | def const_missing c 18 | M.const_get c 19 | end 20 | end 21 | end 22 | ########################### 23 | 24 | $drone = Main.drone 25 | # $drone.config("ip","192.168.3.1") 26 | 27 | # $drone.config("maxEulerAngle", 0.25) 28 | 29 | 30 | ########### Keyboard input ############# 31 | Keyboard.clear() 32 | Keyboard.use() 33 | 34 | fly = false 35 | Keyboard.bind("f", lambda{ 36 | if fly 37 | $drone.land(); 38 | else 39 | $drone.takeOff(); 40 | end 41 | fly = !fly 42 | }) 43 | Keyboard.bind(" ", lambda{ $drone.land(); puts "land" }) 44 | 45 | 46 | Keyboard.bind("c", lambda{ $drone.connect(); $drone.osc.start(8000) }) 47 | Keyboard.bind("x", lambda{ $drone.disconnect(); $drone.osc.stop() }) 48 | 49 | xzSpeed = 0.7 50 | ySpeed = 1.0 51 | rotSpeed = 0.7 52 | x=0.0 53 | y=0.0 54 | z=0.0 55 | r=0.0 56 | Keyboard.bind("j", lambda{ x=-xzSpeed; $drone.move(x,y,z,r) }) 57 | Keyboard.bind("l", lambda{ x=xzSpeed; $drone.move(x,y,z,r) }) 58 | Keyboard.bind("i", lambda{ z=-xzSpeed; $drone.move(x,y,z,r) }) 59 | Keyboard.bind("k", lambda{ z=xzSpeed; $drone.move(x,y,z,r) }) 60 | Keyboard.bind("y", lambda{ y=ySpeed; $drone.move(x,y,z,r) }) 61 | Keyboard.bind("h", lambda{ y=-ySpeed; $drone.move(x,y,z,r) }) 62 | Keyboard.bind("u", lambda{ r=-rotSpeed; $drone.move(x,y,z,r) }) 63 | Keyboard.bind("o", lambda{ r=rotSpeed; $drone.move(x,y,z,r) }) 64 | Keyboard.bindUp("j", lambda{ x=0.0 }) 65 | Keyboard.bindUp("l", lambda{ x=0.0 }) 66 | Keyboard.bindUp("i", lambda{ z=0.0 }) 67 | Keyboard.bindUp("k", lambda{ z=0.0 }) 68 | Keyboard.bindUp("y", lambda{ y=0.0 }) 69 | Keyboard.bindUp("h", lambda{ y=0.0 }) 70 | Keyboard.bindUp("u", lambda{ r=0.0 }) 71 | Keyboard.bindUp("o", lambda{ r=0.0 }) 72 | 73 | 74 | 75 | ######## Leap gesture input ######### 76 | 77 | Leap.clear() 78 | Leap.connect() 79 | Leap.bind( lambda{ |frame| 80 | return if frame.hands().isEmpty() 81 | hand = frame.hands().get(0) 82 | count = hand.fingers().count() 83 | if count >= 3 84 | if $drone.hasSensors() 85 | $drone.takeOff() unless $drone.sensors.get("flying").bool 86 | end 87 | elsif count < 2 88 | $drone.land() 89 | return 90 | end 91 | 92 | return unless $drone.sensors.get("flying").bool 93 | 94 | # return if hand.fingers().count() < 3 95 | normal = hand.palmNormal() 96 | dir = hand.direction() 97 | pos = hand.palmPosition() 98 | y = pos.getY() - 160.0 99 | 100 | if y > 0.0 and y < 10.0 then 101 | y = 0.0 102 | elsif y > 10.0 103 | y = y - 10.0 104 | end 105 | 106 | y = y / 100.0 107 | y = -1.0 if y < -1.0 108 | y = 1.0 if y > 1.0 109 | 110 | $drone.move(-normal.roll(), y, dir.pitch() - 0.15, dir.yaw() * 0.0 ) # yaw control disabled because can be confusing! 111 | }) 112 | 113 | ######## Step function called each frame ####### 114 | 115 | def step(dt) 116 | 117 | end 118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /apps/maxmsp-external/maxdrone.scala: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Max external for piloting ARDrone using OpenDroneControl with JavaDrone backend 4 | * 5 | * fishuyo - 2013 6 | * 7 | */ 8 | 9 | import org.opendronecontrol.platforms.ardrone.ARDrone 10 | import org.opendronecontrol.spatial._ 11 | 12 | import com.cycling74.max._ 13 | import com.cycling74.jitter._ 14 | import MaxObject._ 15 | 16 | import java.net._ 17 | 18 | class OpenDroneControl extends MaxObject { 19 | 20 | 21 | declareAttribute("ip") 22 | 23 | // default ip 24 | var ip = "192.168.1.1" 25 | 26 | // javadrone api ARDrone 27 | var drone = new ARDrone 28 | 29 | // tracking controller 30 | var tracking = drone.tracking 31 | 32 | // hold last video frame as jitter matrix 33 | private var mat: JitterMatrix = _ 34 | 35 | 36 | println("OpenDroneControl version 0.1") 37 | 38 | LoadLibraryPath.unsafeAddDir(".") //hack to make native libraries visible in current directory 39 | LoadLibraryPath.unsafeAddDir("./lib") //hack to make native libraries visible in ./lib directory 40 | 41 | 42 | 43 | ////////////////////////////////////////////////////////////////////// 44 | // member functions 45 | ////////////////////////////////////////////////////////////////////// 46 | 47 | def connect(){ 48 | drone.ip = ip 49 | val _this = this 50 | val t = new Thread(){ 51 | override def run(){ 52 | _this.drone.connect 53 | } 54 | } 55 | t.start 56 | } 57 | 58 | def disconnect() = drone.disconnect 59 | def reset() = drone.reset 60 | def takeOff() = drone.takeOff 61 | def land() = drone.land 62 | 63 | def toggleFly() = drone.toggleFly 64 | 65 | // def led(anim:Int, freq:Float, dur:Int) = drone.playLed(anim, freq, dur) 66 | // def dance(anim:Int, dur:Int) = drone.dance(anim, dur) 67 | 68 | def move( x: Float, y: Float, z: Float, rv: Float ){ 69 | tracking.stop 70 | drone.move(x,y,z,rv) 71 | } 72 | def forward(v:Float) = move(0,0,-v,0) 73 | def back(v:Float) = move(0,0,v,0) 74 | def left(v:Float) = move(-v,0,0,0) 75 | def right(v:Float) = move(v,0,0,0) 76 | def up(v:Float) = move(0,v,0,0) 77 | def down(v:Float) = move(0,-v,0,0) 78 | def cw(v:Float) = move(0,0,0,v) 79 | def ccw(v:Float) = move(0,0,0,-v) 80 | 81 | def hover = { tracking.stop; drone.hover } 82 | 83 | def reboot() = drone.reboot() 84 | 85 | def telnet(command:String) = drone.telnet(command) 86 | 87 | 88 | /* 89 | * PositionTracker commands 90 | */ 91 | 92 | def lookAt( x:Float, y:Float, z:Float ){ 93 | tracking.isLookingAt = true 94 | tracking.lookAtDest = false 95 | tracking.lookingAt = Vec3(x,y,z) 96 | } 97 | def dontLook(){ 98 | tracking.isLookingAt = false 99 | tracking.lookAtDest = false 100 | } 101 | 102 | def moveTo( x:Float,y:Float,z:Float,qx:Float,qy:Float,qz:Float,qw:Float ){ 103 | tracking.moveTo( Pose(Vec3(x,y,z),Quat(qw,qx,qy,qz)) ) 104 | } 105 | def moveTo( x:Float,y:Float,z:Float,w:Float=0.f ){ 106 | moveTo( Pose(Vec3(x,y,z),Quat().fromEuler((0.f,w*math.Pi.toFloat/180.f,0.f)) ) ) 107 | } 108 | def moveTo( p:Pose ) = tracking.moveTo(p) 109 | 110 | def addWaypoint( x:Float,y:Float,z:Float,w:Float ) = tracking.addWaypoint(x,y,z,w) 111 | def clearWaypoints() = tracking.clearWaypoints 112 | 113 | def step(x:Float,y:Float,z:Float,qx:Float,qy:Float,qz:Float,qw:Float){ 114 | step( Pose(Vec3(x,y,z),Quat(qw,qx,qy,qz))) 115 | } 116 | def step(x:Float,y:Float,z:Float,w:Float=0.f ){ 117 | step( Pose(Vec3(x,y,z),Quat().fromEuler((0.f,w*math.Pi.toFloat/180.f,0.f))) ) 118 | } 119 | 120 | def step(p:Pose) = tracking.step(p) 121 | 122 | def stepUsingInternalSensors() = tracking.stepUsingInternalSensors() 123 | 124 | def setPDGainsXZ( p1:Float, d1:Float, dd:Float ) = { 125 | tracking.posKp.set( p1, tracking.posKp.y, p1) 126 | tracking.posKd.set( d1, tracking.posKd.y, d1) 127 | tracking.posKdd.set( dd, tracking.posKdd.y, dd) 128 | } 129 | def setPDGainsY( p1:Float, d1:Float, dd:Float ) = { 130 | tracking.posKp.y = p1 131 | tracking.posKd.y = d1 132 | tracking.posKdd.y = dd 133 | } 134 | def setPDGainsRot( p1:Float, d1:Float ) = { 135 | tracking.rotKp = p1 136 | // tracking.rotKd = d1 137 | } 138 | 139 | def tracking( args:Array[Atom] ){ 140 | val com = args(0).getString 141 | com match { 142 | case "posThresh" => drone.tracking.posThresh = args(1).getFloat 143 | case "yawThresh" => drone.tracking.yawThresh = args(1).getFloat 144 | case "rotateFirst" => drone.tracking.yawThresh = args(1).getInt 145 | case "useHover" => drone.tracking.yawThresh = args(1).getInt 146 | case "patrol" => drone.tracking.yawThresh = args(1).getInt 147 | case _ => println(s"unknown command $com for tracking module") 148 | } 149 | } 150 | 151 | 152 | /* 153 | * Drone Configuration 154 | */ 155 | 156 | def config(value:Array[Atom]){ 157 | var a:Any = null 158 | var name = value(0).getString 159 | 160 | if( value.length > 1){ 161 | if( value(1).isInt ) a = value(1).getInt 162 | else if( value(1).isFloat ) a = value(1).getFloat 163 | else if( value(1).isString ) a = value(1).getString 164 | } 165 | 166 | drone.config(name,a) 167 | } 168 | 169 | def setConfigOption(name:String,value:String){ 170 | drone.setConfigOption(name,value) 171 | } 172 | 173 | def getVersion() = drone.getVersion() 174 | 175 | 176 | /* 177 | * General command interface 178 | * This interprets any unmatched message sent to the max object 179 | * as a general command sent directly to the drone backend 180 | */ 181 | override def anything(msg:String, args:Array[Atom]){ 182 | var list = List[Any]() 183 | for( i<-(0 until args.length)){ 184 | if( args(i).isInt ) list = args(i).getInt :: list 185 | else if( args(i).isFloat ) list = args(i).getFloat :: list 186 | else if( args(i).isString ) list = args(i).getString :: list 187 | } 188 | drone.command(msg, list: _*) 189 | } 190 | 191 | 192 | /* 193 | * Sensor Data 194 | */ 195 | 196 | // prints all available sensor names 197 | def listSensors(){ 198 | if( !drone.hasSensors){ 199 | println("no sensor data received") 200 | return 201 | } 202 | drone.sensors.listSensors() 203 | } 204 | 205 | // outputs sensor name and value out of the right outlet 206 | def sensor( name:String ){ 207 | if( !drone.hasSensors){ 208 | println("no sensor data received") 209 | return 210 | } 211 | drone.sensors(name).value match { 212 | case v:Float => outlet(1, Array[Atom](Atom.newAtom(name),Atom.newAtom(v))) 213 | case v:Int => outlet(1, Array[Atom](Atom.newAtom(name),Atom.newAtom(v))) 214 | case v:Boolean => outlet(1, Array[Atom](Atom.newAtom(name),Atom.newAtom(v))) 215 | case v:Vec3 => outlet(1, Array[Atom](Atom.newAtom(name),Atom.newAtom(v.x),Atom.newAtom(v.y),Atom.newAtom(v.z))) 216 | } 217 | } 218 | 219 | /* 220 | * OSC Interface 221 | */ 222 | def osc( args:Array[Atom] ){ 223 | val com = args(0).getString 224 | com match { 225 | case "start" => if( args.length > 1 ) drone.osc.start(args(1).getInt) else drone.osc.start() 226 | case "stop" => drone.osc.stop 227 | case "sendSensors" => drone.osc.sendSensors(args(1).getString, args(2).getInt) 228 | case _ => println(s"unknown command $com for osc module") 229 | } 230 | } 231 | 232 | /* 233 | * Video Interface 234 | */ 235 | def video( args:Array[Atom] ){ 236 | if( !drone.hasVideo ){ 237 | println("No video stream detected, please connect to a drone with a video stream") 238 | return 239 | } 240 | val com = args(0).getString 241 | com match { 242 | case "start" => drone.video.start 243 | case "stop" => drone.video.stop 244 | case _ => println(s"unknown command $com for video module") 245 | } 246 | } 247 | 248 | // bang outputs latest video frame 249 | override def bang(){ 250 | if( drone.hasVideo && drone.video.getFrame() != null ){ 251 | if( mat == null ) mat = new JitterMatrix 252 | mat.copyBufferedImage(drone.video.getFrame()) 253 | outlet(0,"jit_matrix",mat.getName()) 254 | }else post("no frames received.") 255 | } 256 | 257 | 258 | // When mxj object deleted 259 | override def notifyDeleted(){ 260 | disconnect 261 | } 262 | 263 | } 264 | 265 | 266 | 267 | 268 | object LoadLibraryPath{ 269 | // HACK: adds dir to load library path 270 | def unsafeAddDir(dir: String) = try { 271 | val field = classOf[ClassLoader].getDeclaredField("usr_paths") 272 | field.setAccessible(true) 273 | val paths = field.get(null).asInstanceOf[Array[String]] 274 | if(!(paths contains dir)) { 275 | field.set(null, paths :+ dir) 276 | System.setProperty("java.library.path", 277 | System.getProperty("java.library.path") + 278 | java.io.File.pathSeparator + 279 | dir) 280 | } 281 | } catch { 282 | case _: IllegalAccessException => 283 | error("Insufficient permissions; can't modify private variables.") 284 | case _: NoSuchFieldException => 285 | error("JVM implementation incompatible with path hack") 286 | } 287 | } -------------------------------------------------------------------------------- /downloads/ODC_MaxTemplate_no_xuggle.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/downloads/ODC_MaxTemplate_no_xuggle.zip -------------------------------------------------------------------------------- /downloads/ODC_MaxTemplate_osx.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/downloads/ODC_MaxTemplate_osx.zip -------------------------------------------------------------------------------- /downloads/ODC_ProcessingTemplate.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/downloads/ODC_ProcessingTemplate.zip -------------------------------------------------------------------------------- /downloads/darc_workshop.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/downloads/darc_workshop.pdf -------------------------------------------------------------------------------- /examples/GetSensorData.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package examples 4 | 5 | import spatial._ 6 | import platforms.ardrone._ 7 | import drone.sensors._ 8 | 9 | object GetSensorData extends App { 10 | 11 | val drone = new ARDrone 12 | drone.connect() 13 | 14 | // option 1 15 | // set a callback function called whenever a sensor is updated 16 | drone.sensors.bind( (s) => { 17 | println( s.name + ": " + s.value ) 18 | }) 19 | Thread.sleep(5000) 20 | 21 | 22 | // option 2 23 | // get sensor value on demand 24 | // 25 | // var t = 0 26 | // var dt = 30 27 | // while( t < 20000){ // loop for 20 seconds retreiving sensors every 30 ms 28 | // if( drone.hasSensors() ){ 29 | // println( "velocity: " + drone.sensors("velocity").value ) 30 | // println( "gyroscope: " + drone.sensors("gyroscope").value ) 31 | // println( "altimeter: " + drone.sensors("altimeter").value ) 32 | // println( "battery: " + drone.sensors("battery").value ) 33 | // } 34 | // t += dt 35 | // Thread.sleep(dt) 36 | // } 37 | 38 | drone.disconnect() 39 | 40 | System.exit(0) 41 | 42 | } -------------------------------------------------------------------------------- /examples/TakeoffLand.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package examples 4 | 5 | import spatial._ 6 | import platforms.ardrone._ 7 | 8 | object TakeoffLand extends App { 9 | 10 | val drone = new ARDrone 11 | drone.connect() 12 | Thread.sleep(1000) // sleep to make sure the drone is ready for commands after connecting 13 | 14 | drone.takeOff() 15 | Thread.sleep(10000) 16 | 17 | // hover for 10 seconds 18 | 19 | drone.land() 20 | 21 | drone.disconnect() 22 | 23 | System.exit(0) 24 | 25 | } -------------------------------------------------------------------------------- /examples/TakeoffMoveLand.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package examples 4 | 5 | import spatial._ 6 | import platforms.ardrone._ 7 | 8 | object TakeoffMoveLand extends App { 9 | 10 | val drone = new ARDrone 11 | drone.connect() 12 | Thread.sleep(1000) 13 | 14 | drone.takeOff() 15 | Thread.sleep(5000) // wait for drone to takeoff and stabalize 16 | 17 | var t = 0 // keep track of approximate total time 18 | var dt = 30 // time per iteration of control loop 19 | 20 | var period = 3000 // movement oscillation period 21 | 22 | // control loop 23 | // for smooth flight move commands should be sent at a consistent interval of 30ms 24 | while( t < 10000){ 25 | 26 | var phase = (t % period) / period.toFloat * (2*math.Pi) 27 | 28 | var rot = math.sin(phase).toFloat 29 | 30 | drone.move(0.0f, 0.0f, 0.0f, rot) // oscillate drone left/right 31 | 32 | // print out some sensor data 33 | if( drone.hasSensors() ){ 34 | println( drone.sensors("velocity").vec ) 35 | println( drone.sensors("gyroscope").vec ) 36 | println( drone.sensors("altimeter").float ) 37 | println( drone.sensors("battery").int ) 38 | } 39 | 40 | t += dt 41 | Thread.sleep(dt) 42 | } 43 | 44 | 45 | drone.land() 46 | 47 | drone.disconnect() 48 | 49 | System.exit(0) 50 | 51 | } -------------------------------------------------------------------------------- /examples/droneOSC.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package examples 4 | 5 | import spatial._ 6 | import platforms.ardrone._ 7 | 8 | object DroneOSC extends App { 9 | 10 | var port = 8000 11 | 12 | try{ 13 | if( args.length >= 1) port = args(0).toInt 14 | } catch{ 15 | case e:Exception => println( s"usage: droneOSC [port]") 16 | } 17 | 18 | // create a ARDrone client and start listening for OSC messages 19 | var drone = new ARDrone() 20 | drone.osc.start(port) 21 | 22 | // wait for return then exit 23 | Console.readLine 24 | System.exit(0) 25 | } 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/project.xcworkspace/contents.xcworkspacedata: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/project.xcworkspace/xcuserdata/rjduranjr.xcuserdatad/UserInterfaceState.xcuserstate: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/project.xcworkspace/xcuserdata/rjduranjr.xcuserdatad/UserInterfaceState.xcuserstate -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/project.xcworkspace/xcuserdata/rjduranjr.xcuserdatad/WorkspaceSettings.xcsettings: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | IDEWorkspaceUserSettings_HasAskedToTakeAutomaticSnapshotBeforeSignificantChanges 6 | 7 | IDEWorkspaceUserSettings_SnapshotAutomaticallyBeforeSignificantChanges 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/xcshareddata/xcschemes/ODC_SendRecvOSC Debug.xcscheme: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 8 | 14 | 20 | 21 | 22 | 23 | 24 | 29 | 30 | 31 | 32 | 38 | 39 | 40 | 41 | 49 | 50 | 56 | 57 | 58 | 59 | 60 | 61 | 67 | 68 | 74 | 75 | 76 | 77 | 79 | 80 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/xcshareddata/xcschemes/ODC_SendRecvOSC Release.xcscheme: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 8 | 14 | 20 | 21 | 22 | 23 | 24 | 29 | 30 | 31 | 32 | 38 | 39 | 40 | 41 | 49 | 50 | 56 | 57 | 58 | 59 | 60 | 61 | 67 | 68 | 74 | 75 | 76 | 77 | 79 | 80 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/ODC_SendRecvOSC.xcodeproj/xcuserdata/rjduranjr.xcuserdatad/xcschemes/xcschememanagement.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | SuppressBuildableAutocreation 6 | 7 | E4B69B5A0A3A1756003C02F2 8 | 9 | primary 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/Project.xcconfig: -------------------------------------------------------------------------------- 1 | //THE PATH TO THE ROOT OF OUR OF PATH RELATIVE TO THIS PROJECT. 2 | //THIS NEEDS TO BE DEFINED BEFORE CoreOF.xcconfig IS INCLUDED 3 | OF_PATH = ../../.. 4 | 5 | //THIS HAS ALL THE HEADER AND LIBS FOR OF CORE 6 | #include "../../../libs/openFrameworksCompiled/project/osx/CoreOF.xcconfig" 7 | 8 | OTHER_LDFLAGS = $(OF_CORE_LIBS) 9 | HEADER_SEARCH_PATHS = $(OF_CORE_HEADERS) 10 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/addons.make: -------------------------------------------------------------------------------- 1 | ofxOsc 2 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/openFrameworks-Info.plist: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | CFBundleDevelopmentRegion 6 | English 7 | CFBundleExecutable 8 | ${EXECUTABLE_NAME} 9 | CFBundleIdentifier 10 | com.yourcompany.openFrameworks 11 | CFBundleInfoDictionaryVersion 12 | 6.0 13 | CFBundlePackageType 14 | APPL 15 | CFBundleSignature 16 | ???? 17 | CFBundleVersion 18 | 1.0 19 | 20 | 21 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "ofMain.h" 2 | #include "testApp.h" 3 | #include "ofAppGlutWindow.h" 4 | 5 | //======================================================================== 6 | int main(){ 7 | 8 | ofAppGlutWindow window; 9 | ofSetupOpenGL(&window, 640, 480, OF_WINDOW); // <-------- setup the GL context 10 | 11 | // this kicks off the running of my app 12 | // can be OF_WINDOW or OF_FULLSCREEN 13 | // pass in width and height too: 14 | ofRunApp(new testApp()); 15 | 16 | } 17 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/src/testApp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenDroneControl [ODC] || http://www.opendronecontrol.org/ 3 | * 4 | * ODC_SendRecvOSC 5 | * RJ Duran || rjduranjr@gmail.com || http://rjduran.net || 2013 6 | * 7 | * Description: 8 | * This example sets up a basic OSC interface for connecting to the ARDrone using DroneOSC.scala. 9 | * Connect to the ARDrone over wifi. 10 | * Run the file DroneOSC.scala by using ./sbt "project examples" run, then selecting the project. 11 | * This connects to the device. You can send OSC messages from your OF application to DroneOSC, which 12 | * gets passed onto the drone. 13 | * 14 | * Note: As of openFrameworks 0073 the project directory needs to be located under apps/myApps. 15 | */ 16 | 17 | #include "testApp.h" 18 | 19 | //-------------------------------------------------------------- 20 | void testApp::setup(){ 21 | 22 | ofBackground(30, 30, 130); 23 | 24 | // open an outgoing connection to HOST:PORT 25 | sender.setup(HOST, OUTPORT); 26 | receiver.setup(RECVPORT); 27 | 28 | current_msg_string = 0; 29 | } 30 | 31 | //-------------------------------------------------------------- 32 | void testApp::update(){ 33 | // hide old messages 34 | for(int i = 0; i < NUM_MSG_STRINGS; i++){ 35 | if(timers[i] < ofGetElapsedTimef()){ 36 | msg_strings[i] = ""; 37 | } 38 | } 39 | 40 | // check for waiting messages 41 | while(receiver.hasWaitingMessages()){ 42 | // get the next message 43 | ofxOscMessage m; 44 | receiver.getNextMessage(&m); 45 | 46 | // check for gyroscope message 47 | if(m.getAddress() == "/gyroscope"){ 48 | x = m.getArgAsFloat(0); 49 | y = m.getArgAsFloat(1); 50 | z = m.getArgAsFloat(2); 51 | 52 | } else if(m.getAddress() == "/velocity") { 53 | v_x = m.getArgAsFloat(0); 54 | v_y = m.getArgAsFloat(0); 55 | v_z = m.getArgAsFloat(0); 56 | 57 | } else if(m.getAddress() == "/altimeter") { 58 | altimeter = m.getArgAsFloat(0); 59 | 60 | } else if(m.getAddress() == "/battery") { 61 | battery = m.getArgAsInt32(0); 62 | 63 | } else if(m.getAddress() == "/emergency") { 64 | emergency = m.getArgAsInt32(0); 65 | 66 | } else if(m.getAddress() == "/flying") { 67 | flying = m.getArgAsInt32(0); 68 | 69 | } else { 70 | // unrecognized message: display on the bottom of the screen 71 | string msg_string; 72 | msg_string = m.getAddress(); 73 | msg_string += ": "; 74 | for(int i = 0; i < m.getNumArgs(); i++){ 75 | // get the argument type 76 | msg_string += m.getArgTypeName(i); 77 | msg_string += ":"; 78 | // display the argument - make sure we get the right type 79 | if(m.getArgType(i) == OFXOSC_TYPE_INT32){ 80 | msg_string += ofToString(m.getArgAsInt32(i)); 81 | } 82 | else if(m.getArgType(i) == OFXOSC_TYPE_FLOAT){ 83 | msg_string += ofToString(m.getArgAsFloat(i)); 84 | } 85 | else if(m.getArgType(i) == OFXOSC_TYPE_STRING){ 86 | msg_string += m.getArgAsString(i); 87 | } 88 | else{ 89 | msg_string += "unknown"; 90 | } 91 | } 92 | // add to the list of strings to display 93 | msg_strings[current_msg_string] = msg_string; 94 | timers[current_msg_string] = ofGetElapsedTimef() + 5.0f; 95 | current_msg_string = (current_msg_string + 1) % NUM_MSG_STRINGS; 96 | // clear the next line 97 | msg_strings[current_msg_string] = ""; 98 | } 99 | 100 | } 101 | } 102 | 103 | //-------------------------------------------------------------- 104 | void testApp::draw(){ 105 | // display instructions 106 | string buf; 107 | buf = "sending OSC messages to " + string(HOST) + " " + ofToString(OUTPORT); 108 | ofDrawBitmapString(buf, 10, 20); 109 | 110 | string buf2; 111 | buf2 = "listening for OSC messages on port " + ofToString(RECVPORT); 112 | ofDrawBitmapString(buf2, 10, 50); 113 | 114 | string gyroscope_s; 115 | gyroscope_s = "/gyroscope: [" + ofToString(x) + ", " + ofToString(y) + ", " + ofToString(z) + "]"; 116 | ofDrawBitmapString(gyroscope_s, 10, 100); 117 | 118 | string velocity_s; 119 | velocity_s = "/velocity: [" + ofToString(v_x) + ", " + ofToString(v_y) + ", " + ofToString(v_z) + "]"; 120 | ofDrawBitmapString(velocity_s, 10, 120); 121 | 122 | string altimeter_s; 123 | altimeter_s = "/altimeter: " + ofToString(altimeter); 124 | ofDrawBitmapString(altimeter_s, 10, 140); 125 | 126 | string battery_s; 127 | battery_s = "/battery: " + ofToString(battery); 128 | ofDrawBitmapString(battery_s, 10, 160); 129 | 130 | string emergency_s;; 131 | emergency_s = "/emergency: " + ofToString(emergency); 132 | ofDrawBitmapString(emergency_s, 10, 180); 133 | 134 | string flying_s; 135 | flying_s = "/flying: " + ofToString(flying); 136 | ofDrawBitmapString(flying_s, 10, 200); 137 | 138 | // draw any other incomming messages 139 | for(int i = 0; i < NUM_MSG_STRINGS; i++){ 140 | ofDrawBitmapString(msg_strings[i], 10, 220 + 15 * i); 141 | } 142 | } 143 | 144 | //-------------------------------------------------------------- 145 | void testApp::keyPressed(int key){ 146 | if(key == 'c' || key == 'C'){ 147 | ofxOscMessage m; 148 | m.setAddress("/connect"); 149 | sender.sendMessage(m); 150 | } 151 | 152 | if(key == 'd' || key == 'D'){ 153 | ofxOscMessage m; 154 | m.setAddress("/disconnect"); 155 | sender.sendMessage(m); 156 | } 157 | 158 | if(key == 'r' || key == 'R'){ 159 | ofxOscMessage m; 160 | m.setAddress("/reset"); 161 | sender.sendMessage(m); 162 | } 163 | 164 | if(key == 's' || key == 'S'){ 165 | ofxOscMessage m; 166 | m.setAddress("/broadcastSensors"); 167 | m.addIntArg(RECVPORT); 168 | sender.sendMessage(m); 169 | } 170 | 171 | if(key == 't' || key == 'T'){ 172 | ofxOscMessage m; 173 | m.setAddress("/takeOff"); 174 | sender.sendMessage(m); 175 | } 176 | 177 | if(key == 'l' || key == 'L'){ 178 | ofxOscMessage m; 179 | m.setAddress("/land"); 180 | sender.sendMessage(m); 181 | } 182 | } 183 | 184 | //-------------------------------------------------------------- 185 | void testApp::keyReleased(int key){ 186 | } 187 | 188 | //-------------------------------------------------------------- 189 | void testApp::mouseMoved(int x, int y){ 190 | } 191 | 192 | //-------------------------------------------------------------- 193 | void testApp::mouseDragged(int x, int y, int button){ 194 | } 195 | 196 | //-------------------------------------------------------------- 197 | void testApp::mousePressed(int x, int y, int button){ 198 | } 199 | 200 | //-------------------------------------------------------------- 201 | void testApp::mouseReleased(int x, int y, int button){ 202 | } 203 | 204 | //-------------------------------------------------------------- 205 | void testApp::windowResized(int w, int h){ 206 | } 207 | 208 | //-------------------------------------------------------------- 209 | void testApp::gotMessage(ofMessage msg){ 210 | } 211 | 212 | //-------------------------------------------------------------- 213 | void testApp::dragEvent(ofDragInfo dragInfo){ 214 | } 215 | 216 | -------------------------------------------------------------------------------- /examples/openframeworks/ODC_SendRecvOsc/src/testApp.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ofMain.h" 4 | #include "ofxOsc.h" 5 | 6 | #define HOST "localhost" 7 | #define OUTPORT 8000 8 | #define RECVPORT 8001 9 | #define NUM_MSG_STRINGS 4 10 | 11 | //-------------------------------------------------------- 12 | class testApp : public ofBaseApp { 13 | 14 | public: 15 | 16 | void setup(); 17 | void update(); 18 | void draw(); 19 | 20 | void keyPressed(int key); 21 | void keyReleased(int key); 22 | void mouseMoved(int x, int y); 23 | void mouseDragged(int x, int y, int button); 24 | void mousePressed(int x, int y, int button); 25 | void mouseReleased(int x, int y, int button); 26 | void windowResized(int w, int h); 27 | void dragEvent(ofDragInfo dragInfo); 28 | void gotMessage(ofMessage msg); 29 | 30 | ofTrueTypeFont font; 31 | ofxOscSender sender; 32 | ofxOscReceiver receiver; 33 | 34 | int current_msg_string; 35 | string msg_strings[NUM_MSG_STRINGS]; 36 | float timers[NUM_MSG_STRINGS]; 37 | 38 | float x, y, z; // gyroscope 39 | float v_x, v_y, v_z; // velocity 40 | int flying, battery, emergency; 41 | float altimeter; 42 | 43 | }; 44 | 45 | -------------------------------------------------------------------------------- /examples/processing/ODC_BouncingDrone/ODC_BouncingDrone.pde: -------------------------------------------------------------------------------- 1 | /* 2 | /// [ODC] Processing Example - BouncingDrone - Sterling Crispin 2013 - sterlingcrispin@gmail.com - http://www.sterlingcrispin.com 3 | /// Press 'u' to takeoff and land 4 | /// Wait until the drone has lifted off, then click and drag to move the sphere and let go to drop the Sphere/drone 5 | /// The drone should 'bounce' with the sphere, try changing the gravity, bounce and velScale constants 6 | /// if you've downloaded this code from the ODC github you may be missing the ODC processing libraries, please visit the ODC website to download them 7 | /// http://www.opendronecontrol.org/ 8 | */ 9 | import org.opendronecontrol.platforms.ardrone.ARDrone; 10 | import org.opendronecontrol.spatial.Vec3; 11 | 12 | ARDrone drone; // this creates our drone class 13 | 14 | PImage img; 15 | PShape ball; 16 | int radius = 70; 17 | 18 | Vec3 gyro; // storing gyroscope data 19 | boolean flying; 20 | float droneX; 21 | float droneY; 22 | float droneZ; 23 | float droneYaw; 24 | float alti; // height of the drone as reported by the altimeter sensor 25 | 26 | float gravity; 27 | float spherePosition; 28 | float vel; // sphere velocity 29 | float velScale; // scaling the sphere velocity to the drone velocity 30 | float bounce; // reduction of velocity per impact of sphere with 'ground' 31 | boolean shouldPhysics; 32 | 33 | 34 | void setup(){ 35 | size(640,480, P3D); 36 | 37 | shouldPhysics = false; 38 | 39 | gyro = new Vec3(0.0,0.0,0.0); 40 | 41 | ball = createShape(SPHERE,radius); 42 | spherePosition = 0; 43 | gravity = 0.4; 44 | bounce = -0.85; 45 | velScale = -1.5; // be gentle with this variable as changes can result in dramatic behavior, try making it a positive number (1 to 1.5) to make the drone fall up, rather than down 46 | drone = new ARDrone("192.168.1.1"); // default IP is 192.168.1.1 47 | drone.connect(); 48 | drone.config("maxVerticalSpeed", 1.0); 49 | 50 | } 51 | 52 | void draw(){ 53 | background(100); 54 | 55 | if (drone.hasSensors()){ 56 | flying = drone.sensors().get("flying").bool(); 57 | gyro = drone.sensors().get("gyroscope").vec(); // orientation 58 | alti = drone.sensors().get("altimeter").getFloat(); // height of the drone 59 | } 60 | 61 | if(flying==true && shouldPhysics == true){ 62 | vel += gravity; 63 | spherePosition += vel; 64 | if(spherePosition >= height-radius*1.5){ // has the sphere reached the edge of the screen? *1.5 is a fudge factor based on the visual size of the sphere in the draw window 65 | vel *= bounce; // make the sphere bounce 66 | spherePosition = height-radius*1.5; // dont go beyond the edge of the screen 67 | droneYaw = random(3)-1.5; // randomly spin upon impact! 68 | 69 | } 70 | 71 | droneY = vel * velScale; // set the vertical move speed of the drone to the sphere velocity and scale the velocity to something within range for the drone 72 | } 73 | 74 | if(flying==true && alti < 0.2){ // fly up if the drone is nearly hitting the ground 75 | droneY += 1; 76 | } 77 | 78 | 79 | if(flying==true && alti > 0.2 && shouldPhysics == true){ // fall and/or spin 80 | drone.move(droneX, droneY, droneZ, droneYaw); 81 | } 82 | 83 | if(flying==true && alti > 0.2 && shouldPhysics == false){ // if the mouse is dragging, 84 | drone.tracking().moveTo(0.0, (1.4-(mouseY/height)), 0.0, 0.0); // fly to the height of the mouse, relative to the top of the screen 85 | drone.tracking().step(0.0, alti, 0.0, 0.0); // using the height sensor on the drone 86 | } 87 | 88 | 89 | pushMatrix(); 90 | translate( width/2, spherePosition, 50); 91 | rotateX(radians(gyro.x() )); //rotate the sphere with the drones gyroscope data 92 | rotateY(radians(gyro.y() * -1 )); // inverting this to match expected behavior, so when the drone tilts down, so does the sphere 93 | rotateZ(radians(gyro.z() )); 94 | shape(ball); 95 | popMatrix(); 96 | 97 | droneY = 0; // dont accumulate vertical move speed between frames 98 | droneYaw *= 0.8; // spin less per frame, so that the drone doesn't spiral constantly 99 | } 100 | 101 | void mouseDragged(){ 102 | shouldPhysics = false; 103 | spherePosition = mouseY; 104 | } 105 | 106 | void mouseReleased(){ 107 | shouldPhysics = true; 108 | } 109 | 110 | void keyPressed(){ 111 | if (key =='u'){ 112 | if(flying==false){ 113 | drone.takeOff(); 114 | } else{ 115 | drone.land(); 116 | } 117 | } 118 | } 119 | 120 | void mousePressed(){ 121 | shouldPhysics = false; 122 | spherePosition = mouseY; 123 | } 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /examples/processing/ODC_DroneOscP5/ODC_DroneOscP5.pde: -------------------------------------------------------------------------------- 1 | /* 2 | /// [ODC] Processing Example - DroneoscP5 - Tim Wood 2013 - fishuyo@gmail.com || http://fishuyo.com/ 3 | /// Control ARDrone using oscP5 4 | /// Connects to a OpenDroneControl client that is listening for OSC messages. 5 | /// This can be used to allow multiple connections / sketches communicating 6 | /// with the same drone. 7 | /// 8 | /// Press 'u' to takeoff and land 9 | /// 10 | /// if you've downloaded this code from the ODC github you may be missing the ODC processing libraries, please visit the ODC website to download them 11 | /// http://www.opendronecontrol.org/ 12 | */ 13 | 14 | import oscP5.*; 15 | import netP5.*; 16 | 17 | OscP5 oscP5; 18 | NetAddress address; 19 | 20 | String ip = "127.0.0.1"; /* ip address of ARDrone ODC client listening for osc messages */ 21 | int outPort = 8000; /* port of ODC client */ 22 | int recvPort = 8001; /* port to receive sensor data on */ 23 | 24 | void setup() { 25 | size(400,400); 26 | frameRate(25); 27 | /* start oscP5, listening for incoming messages at recvPort */ 28 | oscP5 = new OscP5(this,recvPort); 29 | address = new NetAddress(ip, outPort); 30 | 31 | OscMessage myMessage = new OscMessage("/connect"); 32 | oscP5.send(myMessage, address); 33 | 34 | myMessage = new OscMessage("/broadcastSensors"); 35 | myMessage.add(recvPort); /* add recvPort to the osc message */ 36 | oscP5.send(myMessage, address); 37 | } 38 | 39 | 40 | void draw() { 41 | background(0); 42 | } 43 | 44 | void mousePressed() { 45 | 46 | } 47 | 48 | /* incoming osc message are forwarded to the oscEvent method. */ 49 | void oscEvent(OscMessage theOscMessage) { 50 | 51 | /* print the gyroscope data */ 52 | if(theOscMessage.checkAddrPattern("/gyroscope") == true) { 53 | /* check if the typetag is the right one. */ 54 | if(theOscMessage.checkTypetag("fff")) { 55 | /* parse theOscMessage and extract the values from the osc message arguments. */ 56 | float x = theOscMessage.get(0).floatValue(); 57 | float y = theOscMessage.get(1).floatValue(); 58 | float z = theOscMessage.get(2).floatValue(); 59 | println(" gyroscope: "+x+", "+y+", "+z); 60 | return; 61 | } 62 | } 63 | } 64 | 65 | 66 | void keyPressed(){ 67 | if (key =='u'){ 68 | if(flying==false){ 69 | myMessage = new OscMessage("/takeOff"); 70 | oscP5.send(myMessage, address); 71 | } else{ 72 | myMessage = new OscMessage("/land"); 73 | oscP5.send(myMessage, address); 74 | } 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /examples/processing/ODC_DroneVideo/ODC_DroneVideo.pde: -------------------------------------------------------------------------------- 1 | /* 2 | /// [ODC] Processing Example - DroneVideo - Tim Wood and Sterling Crispin 2013 - fishuyo@gmail.com || sterlingcrispin@gmail.com || http://fishuyo.com/ || http://www.sterlingcrispin.com 3 | /// Press 'u' to takeoff and land 4 | /// fly the drone in the X and Z directions by moving your mouse around in the draw window 5 | 6 | /// if you've downloaded this code from the ODC github you may be missing the ODC processing libraries, please visit the ODC website to download them 7 | /// http://www.opendronecontrol.org/ 8 | */ 9 | 10 | import java.awt.image.BufferedImage; 11 | import org.opendronecontrol.platforms.ardrone.ARDrone; 12 | import org.opendronecontrol.spatial.Vec3; 13 | import scala.collection.immutable.List; 14 | 15 | ARDrone drone; // this creates our drone class 16 | BufferedImage bimg; // a 2D image from JAVA returns current video frame 17 | 18 | PImage img; 19 | Vec3 gyro; // storing gyroscope data 20 | boolean flying; 21 | float droneX; 22 | float droneY; 23 | float droneZ; 24 | float droneYaw; 25 | 26 | void setup(){ 27 | 28 | size(640,480, OPENGL); 29 | drone = new ARDrone("192.168.1.1"); // default IP is 192.168.1.1 30 | drone.connect(); 31 | gyro = new Vec3(0.0,0.0,0.0); 32 | 33 | 34 | } 35 | 36 | void draw(){ 37 | 38 | if (drone.hasSensors()){ 39 | flying = drone.sensors().get("flying").bool(); 40 | } 41 | 42 | if( drone.hasVideo()){ 43 | 44 | bimg = drone.video().getFrame(); // on each draw call get the current video frame 45 | 46 | if( bimg != null ){ 47 | img = new PImage(bimg.getWidth(),bimg.getHeight(),PConstants.ARGB); // create a new processing image to hold the current video frame 48 | bimg.getRGB(0, 0, img.width, img.height, img.pixels, 0, img.width); // fill the img with buffered frame data from drone 49 | img.updatePixels(); 50 | img.resize(640,480); 51 | image(img,0,0); // display the video frame 52 | } 53 | } 54 | 55 | 56 | if( drone.hasSensors()){ 57 | gyro = drone.sensors().get("gyroscope").vec(); 58 | } 59 | 60 | fill((gyro.y()+180)/360*255,0,60); 61 | ellipse(gyro.z()*10 + width/2, gyro.x()*10 + height/2, 80, 80); 62 | 63 | 64 | if(flying==true){ 65 | 66 | if(mouseX < width/2){ 67 | droneX = 0.1; 68 | } else { 69 | droneX = -0.1; 70 | } 71 | 72 | if(mouseY < height/2){ 73 | droneZ = 0.1; 74 | } else { 75 | droneZ = -0.1; 76 | } 77 | } 78 | 79 | drone.move(droneX,droneY,droneZ,droneYaw); 80 | } 81 | 82 | 83 | 84 | void keyPressed(){ 85 | if (key =='u'){ 86 | if(flying==false){ 87 | drone.takeOff(); 88 | } else{ 89 | drone.land(); 90 | } 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /images/ODC_framework-1024x546.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/images/ODC_framework-1024x546.jpg -------------------------------------------------------------------------------- /images/ODC_framework.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/images/ODC_framework.jpg -------------------------------------------------------------------------------- /images/odc_videostill.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/opendronecontrol/odc/90a31a7be0272c345f162a8e6a3bf88521da1cef/images/odc_videostill.jpg -------------------------------------------------------------------------------- /odc/src/main/scala/drone/DroneBase.scala: -------------------------------------------------------------------------------- 1 | 2 | 3 | package org.opendronecontrol 4 | package drone 5 | 6 | import sensors._ 7 | import video._ 8 | import net._ 9 | import tracking._ 10 | 11 | /** Abstract interface to a drone platform implementation. 12 | * 13 | * Implemented in [[org.opendronecontrol.platforms.ardrone.ARDrone]] and [[org.opendronecontrol.drone.SimDrone]] 14 | */ 15 | abstract class DroneBase { 16 | 17 | def connect() 18 | def disconnect() 19 | def reset(){} 20 | 21 | def takeOff() 22 | def land() 23 | 24 | /** move the drone relative to its local right handed coordinate frame where +x is to its right and -z is forward 25 | * @param x pecentage of maximum acceleration in x direction 26 | * @param y pecentage of maximum acceleration in y direction 27 | * @param z pecentage of maximum acceleration in z direction 28 | * @param r pecentage of maximum rotation negative r causes counter clockwise rotation 29 | */ 30 | def move(x:Float,y:Float,z:Float,r:Float) 31 | 32 | def hover(){} 33 | 34 | def forward(v:Float) = move(0,0,-v,0) 35 | def back(v:Float) = move(0,0,v,0) 36 | def left(v:Float) = move(-v,0,0,0) 37 | def right(v:Float) = move(v,0,0,0) 38 | def up(v:Float) = move(0,v,0,0) 39 | def down(v:Float) = move(0,-v,0,0) 40 | def cw(v:Float) = move(0,0,0,v) 41 | def ccw(v:Float) = move(0,0,0,-v) 42 | 43 | /** command function is to handle drone specific commands 44 | * @param com command name to be executed 45 | * @param args list of arguments for the command 46 | */ 47 | def command(com:String, args:Any* ){} 48 | 49 | /** config function is to handle drone specific configuration parameters 50 | * @param key configuration key 51 | * @param value value to be assigned 52 | */ 53 | def config(key:String, value:Any){} 54 | 55 | /** video stream should be implemented by extending [[org.opendronecontrol.drone.video.VideoStream]] 56 | * 57 | * {{{ 58 | * class MyVideoStream extends VideoStream {...} 59 | * drone.video = Some(new MyVideoStream) 60 | * }}} 61 | */ 62 | def hasVideo() = videoStream.isDefined 63 | def video = videoStream.get 64 | var videoStream:Option[VideoStream] = None 65 | 66 | /** sensor data should be implemented by extending [[org.opendronecontrol.drone.sensors.SensorData]] 67 | * 68 | * {{{ 69 | * class MySensorData extends SensorData {...} 70 | * drone.sensorData = Some(new MySensorData) 71 | * }}} 72 | */ 73 | def hasSensors() = sensorData.isDefined 74 | def sensors = sensorData.get 75 | var sensorData:Option[SensorData] = None 76 | 77 | 78 | /** osc interface module to control drone via network messages */ 79 | val osc = new OSCInterface(this) 80 | 81 | /** tracking adds moveTo command given some regularlly updated spatial information */ 82 | val tracking = new PositionTrackingController(this) 83 | 84 | } 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /odc/src/main/scala/drone/SimDrone.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package drone 4 | 5 | import spatial._ 6 | 7 | /* Very rudimentary simulation of a quadcoptor */ 8 | 9 | class SimDrone extends DroneBase { 10 | 11 | // simulation state -- represents the position velocity and acceleration for a given time 12 | var sPose = Pose() 13 | var sVelocity = Vec3() 14 | var sAcceleration = Vec3() 15 | var thrust = 0.f 16 | 17 | var g = Vec3(0,-9.8f,0) 18 | var mass = .433f // kg 19 | var kair = .5f 20 | 21 | var flying = false 22 | var takingOff = false 23 | 24 | var controls = Vec3() 25 | var rot = 0.f 26 | 27 | var maxEuler = .3f 28 | var maxVert = 1.f 29 | var maxRot = 3.0f 30 | 31 | var timeout = 0.f 32 | 33 | def connect(){} 34 | def disconnect(){} 35 | 36 | def takeOff() = { 37 | flying = true 38 | takingOff = true 39 | } 40 | def land() = { sPose.quat.setIdentity(); flying = false; takingOff=false; thrust = 5.f} 41 | 42 | def move(x:Float,y:Float,z:Float,r:Float){ 43 | if(!flying) return 44 | controls.set(x,y,z) 45 | rot = r 46 | sPose.quat.fromEuler(z*maxEuler,0.f,-x*maxEuler) 47 | thrust = (-g.y + y)/sPose.uu().y 48 | timeout = 0.f 49 | } 50 | 51 | def move(q:Quat){ 52 | sPose.quat.set(q) 53 | thrust = (-g.y)/sPose.uu().y 54 | } 55 | 56 | override def hover() = { sPose.quat.setIdentity(); sVelocity.set(0,0,0)} 57 | 58 | 59 | def step(dt:Float){ 60 | 61 | val p = sPose.pos 62 | val q = sPose.quat 63 | 64 | val angles = q.toEuler() 65 | 66 | if( takingOff && p.y < 0.25f){ 67 | thrust = 9.8f + 2.f 68 | } else if( takingOff ){ 69 | thrust = 9.8f 70 | takingOff = false 71 | } 72 | 73 | sAcceleration.set( sPose.uu()*thrust ) 74 | 75 | sPose.pos += sVelocity*dt 76 | sVelocity += (sAcceleration + g)*dt - sVelocity*kair*dt 77 | 78 | if( p.y < 0.f){ 79 | sPose.pos.y = 0.f 80 | if( flying ) sVelocity = sVelocity * Vec3( .5f, -.5f, .5f) 81 | else sVelocity.zero() 82 | } 83 | 84 | timeout += dt 85 | if( timeout > .42f) move(0,0,0,0) 86 | 87 | } 88 | 89 | 90 | 91 | 92 | } -------------------------------------------------------------------------------- /odc/src/main/scala/drone/sensors/SensorData.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package drone 4 | package sensors 5 | 6 | import spatial._ 7 | 8 | import scala.collection.mutable.Map 9 | 10 | 11 | /** Base Class for sensor data case classes 12 | */ 13 | class SensorBase[+T](val name:String, val value:T){ 14 | def vec = value.asInstanceOf[Vec3] 15 | def getVec3 = value.asInstanceOf[Vec3] 16 | def float = value.asInstanceOf[Float] 17 | def getFloat = value.asInstanceOf[Float] 18 | def int = value.asInstanceOf[Int] 19 | def getInt = value.asInstanceOf[Int] 20 | def bool = value.asInstanceOf[Boolean] 21 | def getBoolean = value.asInstanceOf[Boolean] 22 | } 23 | 24 | /** Case class extension of [[SensorBase]] 25 | * 26 | * @param n sensor's name 27 | * @param v sensor's value 28 | */ 29 | case class Sensor[+T](n:String,v:T) extends SensorBase[T](n,v) 30 | 31 | case class Accelerometer(v:Vec3) extends SensorBase[Vec3]("accelerometer",v) 32 | case class Gyroscope(v:Vec3) extends SensorBase[Vec3]("gyroscope",v) 33 | 34 | case class Velocity(v:Vec3) extends SensorBase[Vec3]("velocity",v) 35 | case class Position(v:Vec3) extends SensorBase[Vec3]("position",v) 36 | 37 | case class Altimeter(v:Float) extends SensorBase[Float]("altimeter",v) 38 | case class Battery(v:Int) extends SensorBase[Int]("battery",v) 39 | 40 | 41 | /** Holds map of all current Sensor readings 42 | * 43 | */ 44 | class SensorData { 45 | 46 | type Callback = (SensorBase[Any]) => Unit 47 | var callback = (s:SensorBase[Any]) => () 48 | val callbacks = Map[String, Callback]() 49 | 50 | val sensorData = Map[String,SensorBase[Any]]() 51 | 52 | def hasSensor(name:String) = sensorData.keys.exists( _ == name.toLowerCase ) 53 | def getSensorNames() = sensorData.keys 54 | def getSensorValues() = sensorData.values 55 | 56 | def listSensors() = this.getSensorNames().foreach( println(_) ) 57 | 58 | def apply(name:String) = sensorData(name) 59 | 60 | def get(name:String) = this(name) 61 | def set(s:SensorBase[Any]) = { 62 | sensorData(s.name) = s 63 | callback(s) 64 | } 65 | 66 | def bind(f:Callback) = callback = f 67 | def unbind() = callback = (s:SensorBase[Any]) => () 68 | } -------------------------------------------------------------------------------- /odc/src/main/scala/drone/video/VideoStream.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package drone 4 | package video 5 | 6 | import java.awt.image.BufferedImage 7 | 8 | trait VideoStream { 9 | 10 | def start(){} 11 | def stop(){} 12 | def apply() = getFrame() 13 | def getFrame():BufferedImage 14 | def config(key:String, value:Any){} 15 | 16 | } -------------------------------------------------------------------------------- /odc/src/main/scala/net/OSCInterface.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package net 4 | 5 | import drone._ 6 | import tracking._ 7 | import spatial._ 8 | 9 | import de.sciss.osc._ 10 | import Implicits._ 11 | 12 | import scala.collection.mutable.Map 13 | 14 | // Mixins unecessary for now- remove 15 | // /** OSCInterface Module 16 | // * this is a mixin trait for DroneBase that listens for 17 | // * commands over the network via the OpenSoundControl Protocol 18 | // */ 19 | // trait OSCMixin extends DroneBase{ 20 | // val osc = new OSCInterface(this) 21 | // } 22 | 23 | /** OSCInterface 24 | * Implements drone related OSC message handling. 25 | * 26 | * This can be started by running: 27 | * {{{ 28 | * val drone = new ARDrone 29 | * drone.osc.start(8000) // listen for messages on port 8000 30 | * }}} 31 | * 32 | * Example Messages: 33 | * {{{ 34 | * /connect 35 | * /sendSensors "192.168.1.255" 8001 36 | * /takeOff 37 | * /move 0.0 0.0 0.0 0.5 38 | * /config "maxEulerAngle" 0.2 39 | * /config "maxVerticalSpeed" 1.0 40 | * /led [pattern:Int] [frequency:Float] [duration(seconds):Int] 41 | * /animation [pattern:Int] [duration(seconds):Int] 42 | * /land 43 | * /disconnect 44 | * }}} 45 | */ 46 | class OSCInterface(val drone:DroneBase) { 47 | 48 | var dump = false 49 | val cfg = UDP.Config() 50 | var rcv = UDP.Receiver(cfg) 51 | var port = 8000 52 | 53 | val ccfg = UDP.Config() 54 | ccfg.codec = PacketCodec().doublesAsFloats().booleansAsInts() 55 | var out = UDP.Client( localhost -> 8001, ccfg ) 56 | 57 | def start(port:Int=8000){ 58 | 59 | this.port = port 60 | 61 | println( s"Listening for drone commands on port $port" ) 62 | println( " Example Commands: ") 63 | println( " /connect") 64 | println( " /sendSensors 192.168.1.255 8001") 65 | println( " /takeOff") 66 | println( " /move 0.1 0.0 0.0 0.0") 67 | println( " /land") 68 | println( " /disconnect") 69 | 70 | cfg.localPort = port 71 | rcv = UDP.Receiver( cfg ) 72 | 73 | if( dump ) rcv.dump( Dump.Both ) 74 | 75 | rcv.action = { 76 | 77 | case (b:Bundle, _) => handleBundle(b) 78 | case (m:Message, _) => handleMessage(m) 79 | 80 | case (p, addr) => println( "Ignoring: " + p + " from " + addr ) 81 | } 82 | 83 | rcv.connect() 84 | } 85 | 86 | def handleBundle(bundle:Bundle){ 87 | bundle match { 88 | case Bundle(t, msgs @ _*) => 89 | msgs.foreach{ 90 | case b:Bundle => handleBundle(b) 91 | case m:Message => handleMessage(m) 92 | case _ => println("unhandled object in bundle") 93 | } 94 | case _ => println("bad bundle") 95 | } 96 | } 97 | 98 | def handleMessage(msg:Message){ 99 | msg match { 100 | case Message("/connect") => drone.connect 101 | case Message("/disconnect") => drone.disconnect 102 | case Message("/reset") => drone.reset 103 | case Message("/takeOff") => drone.takeOff 104 | case Message("/land") => drone.land 105 | case Message("/move",x:Float,y:Float,z:Float,r:Float) => drone.move(x,y,z,r) 106 | case Message("/left",x:Float) => drone.left(x) 107 | case Message("/right",x:Float) => drone.right(x) 108 | case Message("/forward",x:Float) => drone.forward(x) 109 | case Message("/back",x:Float) => drone.back(x) 110 | case Message("/up",x:Float) => drone.up(x) 111 | case Message("/down",x:Float) => drone.down(x) 112 | case Message("/ccw",x:Float) => drone.ccw(x) 113 | case Message("/cw",x:Float) => drone.cw(x) 114 | case Message("/hover") => drone.hover 115 | 116 | 117 | case Message("/moveTo",a:Float,b:Float,c:Float,d:Float) => drone.tracking.moveTo(a,b,c,d) 118 | case Message("/step",a:Float,b:Float,c:Float,d:Float) => drone.tracking.step(a,b,c,d) 119 | 120 | 121 | case Message("/sendSensors", ip:String, port:Int) => sendSensors(ip,port) 122 | case Message("/broadcastSensors", port:Int) => broadcastSensors(port) 123 | 124 | case Message("/config", key:String, value:Any) => drone.config(key,value) 125 | 126 | case Message( name, vals @ _* ) => drone.command(name.replaceFirst("/",""), vals:_* ) 127 | case _ => println( "Ignoring: " + msg ) 128 | } 129 | } 130 | 131 | def stop() = { println("OSC server shutting down.."); drone.sensorData.foreach(_.unbind); rcv.close(); out.close(); } 132 | 133 | def broadcastSensors(port:Int=8001) = sendSensors("255.255.255.255",port) 134 | 135 | def sendSensors(ip:String="localhost", port:Int=8001){ 136 | if( !drone.hasSensors() ){ 137 | println("Drone has no available sensor data, make sure you are properly connected..") 138 | return 139 | } 140 | 141 | out.close 142 | out = UDP.Client( ip -> port, ccfg ) 143 | out.channel.socket.setBroadcast(true) 144 | out.connect 145 | 146 | drone.sensors.bind( (s) => { 147 | val name = s.name 148 | val path = s"/$name" 149 | try{ 150 | s.value match { 151 | case v:Float => out ! Message(path,v) 152 | case v:Int => out ! Message(path,v) 153 | case v:Boolean => out ! Message(path,v) 154 | case v:Vec3 => out ! Message(path,v.x,v.y,v.z) 155 | case v:Quat => out ! Message(path,v.w,v.x,v.y,v.z) 156 | case _ => () 157 | } 158 | } catch { 159 | case e:Exception => println(e) 160 | } 161 | }) 162 | 163 | } 164 | 165 | } 166 | 167 | 168 | 169 | -------------------------------------------------------------------------------- /odc/src/main/scala/net/SimpleTelnetClient.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol.net 3 | 4 | import java.io._ 5 | import java.net._ 6 | 7 | class SimpleTelnetClient( val ip:String="192.168.1.1", var port:Int=23 ) { 8 | 9 | val socket = new Socket(ip,port) 10 | socket.setKeepAlive(true) 11 | 12 | val r = new BufferedReader(new InputStreamReader(socket.getInputStream())) 13 | val w = new PrintWriter(socket.getOutputStream()) 14 | 15 | //read 16 | 17 | def send( m:String ) = { 18 | w.print(m+"\r\n") 19 | w.flush() 20 | } 21 | 22 | def read = while(r.ready) print(r.read.toChar) 23 | 24 | def disconnect = socket.close() 25 | } 26 | -------------------------------------------------------------------------------- /odc/src/main/scala/numeric/TimesFloatNumeric.scala: -------------------------------------------------------------------------------- 1 | package org.opendronecontrol 2 | package numeric 3 | 4 | import scala.math.Fractional 5 | import scala.math.Numeric._ 6 | 7 | import spatial.Vec3 8 | import Vec3.Vec3Numeric 9 | 10 | trait TimesFloatNumeric[T] extends Numeric[T] { 11 | def times(x:T, y:Float): T 12 | } 13 | object TimesFloatNumeric { 14 | trait FloatIsTimesFloatNumeric extends TimesFloatNumeric[Float] { 15 | def times(x: Float, y:Float) = x*y 16 | } 17 | implicit object FloatIsTimesFloatNumeric 18 | extends TimesFloatNumeric[Float] 19 | with FloatIsFractional 20 | with Ordering.FloatOrdering 21 | 22 | trait DoubleIsTimesFloatNumeric extends TimesFloatNumeric[Double] { 23 | def times(x: Double,y:Float) = x*y.toDouble 24 | } 25 | implicit object DoubleIsTimesFloatNumeric 26 | extends DoubleIsTimesFloatNumeric 27 | with DoubleIsFractional 28 | with Ordering.DoubleOrdering 29 | 30 | trait Vec3IsTimesFloatNumeric extends TimesFloatNumeric[Vec3] { 31 | def times(x:Vec3,y:Float) = x*y 32 | } 33 | implicit object Vec3IsTimesFloatNumeric 34 | extends Vec3IsTimesFloatNumeric 35 | with Vec3Numeric 36 | } -------------------------------------------------------------------------------- /odc/src/main/scala/spatial/Nav.scala: -------------------------------------------------------------------------------- 1 | /* 2 | * Port of al_Pose.hpp from AlloSystem 3 | * https://github.com/AlloSphere-Research-Group/AlloSystem/blob/master/allocore/allocore/spatial/al_Pose.hpp 4 | */ 5 | 6 | 7 | package org.opendronecontrol 8 | package spatial 9 | 10 | object Pose { 11 | def apply():Pose = apply(Vec3(),Quat()) 12 | def apply(p:Pose):Pose = apply(Vec3(p.pos),Quat(p.quat)) 13 | def apply( pos:Vec3=Vec3(0), quat:Quat=Quat(1,0,0,0)) = new Pose(pos,quat) 14 | } 15 | /** Class Pose represents a position and orientation in 3d space */ 16 | class Pose( var pos:Vec3=Vec3(0), var quat:Quat=Quat(1,0,0,0) ){ 17 | 18 | def vec = pos 19 | 20 | /** translate and rotate pose by another pose */ 21 | def *(p:Pose) = new Pose( pos+p.pos, quat*p.quat) 22 | def *=(p:Pose) = { pos += p.pos; quat *= p.quat; this } 23 | 24 | def set(p:Pose) = { pos.set(p.pos); quat.set(p.quat)} 25 | 26 | //return Azimuth Elevation and distance to point v 27 | //def getAED(v:Vec3): (Float,Float,Float) = {} 28 | 29 | def getUnitVectors():(Vec3,Vec3,Vec3) = (quat.toX, quat.toY, quat.toZ) 30 | def getDirVectors():(Vec3,Vec3,Vec3) = (quat.toX, quat.toY, -quat.toZ) 31 | def ur() = quat.toX 32 | def uu() = quat.toY 33 | def uf() = -quat.toZ 34 | 35 | def setIdentity = { pos.zero(); quat.setIdentity() } 36 | 37 | /** return linear interpolated Pose from this to p by amount d*/ 38 | def lerp(p:Pose, d:Float) = new Pose( pos.lerp(p.pos, d), quat.slerp(p.quat, d) ) 39 | 40 | } 41 | 42 | object Nav{ 43 | def apply() = new Nav() 44 | } 45 | /** Pose that moves through space */ 46 | class Nav( p:Vec3=Vec3(0) ) extends Pose(p) { 47 | var smooth = 0.f 48 | var scale = 1.f 49 | var vel = Vec3(0); var velS = Vec3(0) 50 | var angVel = Vec3(0); var angVelS = Vec3(0) 51 | var turn = Vec3(0); var nudge = Vec3(0) 52 | var mUR = Vec3(0); var mUU = Vec3(0); var mUF = Vec3(0) 53 | 54 | def view(eu:(Float,Float,Float)){ view(Quat().fromEuler(eu)) } 55 | def view(q:Quat){ quat = q; updateDirVectors() } 56 | 57 | def velPose() = new Pose( velS, Quat().fromEuler(angVelS) ) 58 | 59 | def stop() = { 60 | vel.zero; velS.zero; 61 | angVel.zero; angVelS.zero; 62 | turn.zero; nudge.zero; 63 | updateDirVectors() 64 | } 65 | 66 | def moveToOrigin() = { 67 | quat.setIdentity; pos.zero 68 | stop 69 | } 70 | 71 | def lookAt( p: Vec3, amt:Float=1.f) = { 72 | 73 | } 74 | def goTo( p:Vec3, amt:Float=1.f) = { 75 | val dir = (p - pos).normalize 76 | } 77 | 78 | def updateDirVectors() = { quat = quat.normalize; mUR = ur(); mUU = uu(); mUF = uf() } 79 | 80 | def step( dt:Float ) = { 81 | scale = dt 82 | val amt = 1.f - smooth 83 | 84 | //smooth velocities 85 | velS = velS.lerp( vel*dt + nudge, amt ) 86 | angVelS = angVelS.lerp( angVel*dt + turn, amt ) 87 | 88 | nudge.zero; turn.zero 89 | 90 | //rotate 91 | quat *= Quat().fromEuler(angVelS) 92 | updateDirVectors() 93 | 94 | //move 95 | pos.x += velS dot Vec3( mUR.x, mUU.x, mUF.x) 96 | pos.y += velS dot Vec3( mUR.y, mUU.y, mUF.y) 97 | pos.z += velS dot Vec3( mUR.z, mUU.z, mUF.z) 98 | 99 | } 100 | } 101 | 102 | 103 | -------------------------------------------------------------------------------- /odc/src/main/scala/spatial/Quat.scala: -------------------------------------------------------------------------------- 1 | /* 2 | * Quaternions! 3 | * Port of al_Quat.hpp from AlloSystem 4 | * https://github.com/AlloSphere-Research-Group/AlloSystem/blob/master/allocore/allocore/math/al_Quat.hpp 5 | */ 6 | 7 | package org.opendronecontrol 8 | package spatial 9 | 10 | object Quat { 11 | val eps = 0.0000001 12 | val acc_max = 1.000001 13 | val acc_min = 0.999999 14 | def apply( w:Float, x:Float, y:Float, z:Float ) = new Quat(w,x,y,z) 15 | def apply(q:Quat) = new Quat(q.w,q.x,q.y,q.z) 16 | def apply() = new Quat(1,0,0,0) 17 | } 18 | 19 | class Quat(var w:Float, var x:Float, var y:Float, var z:Float ){ 20 | implicit def toF( d: Double ) = d.toFloat 21 | 22 | def unary_- = Quat( -w, -x, -y, -z ) 23 | def +(v: Quat) = Quat( w+v.w, x+v.x, y+v.y, z+v.z ) 24 | def -(v: Quat) = Quat( w-v.w, x-v.x, y-v.y, z-v.z ) 25 | def *(q: Quat) = Quat( w*q.w-x*q.x-y*q.y-z*q.z, w*q.x+x*q.w+y*q.z-z*q.y, w*q.y+y*q.w+z*q.x-x*q.z, w*q.z+z*q.w+x*q.y-y*q.x ) 26 | def *(s: Float ) = Quat(s*w, s*x, s*y, s*z) 27 | def /(s: Float ) = Quat(w/s, x/s, y/s, z/s) 28 | 29 | def +=(v: Quat) = { w+=v.w; x+=v.x; y+=v.y; z+=v.z } 30 | def -=(v: Quat) = { w-=v.w; x-=v.x; y-=v.y; z-=v.z } 31 | def *=(q: Quat) = set(this*q) 32 | def *=(s: Float) = { w*=s; x*=s; y*=s; z*=s } 33 | 34 | def set(q: Quat) = { w=q.w; x=q.x; y=q.y; z=q.z } 35 | def set(qw:Float,a:Float,b:Float,c:Float) = { x=a; y=b; z=c; w=qw } 36 | 37 | def dot(v: Quat) : Float = w*v.w + x*v.x + y*v.y + z*v.z 38 | //def cross( v: Quat) = Quat( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x ) 39 | def magSq() = this dot this 40 | def mag() = math.sqrt( magSq() ) 41 | def normalize() = { 42 | val m = magSq() 43 | if( m*m < Quat.eps ) Quat().setIdentity() 44 | else if( m > Quat.acc_max || m < Quat.acc_min ) this * (1.0f / math.sqrt( m )) 45 | else this 46 | } 47 | 48 | def conj = Quat( w, -x,-y,-z ) 49 | def sgn = Quat(w,x,y,z).normalize 50 | def inverse = sgn conj 51 | def recip = conj / magSq 52 | 53 | def zero() = {w=0;x=0;y=0;z=0;this} 54 | def setIdentity() = {w=1;x=0;y=0;z=0;this} 55 | 56 | def fromAxisX( ang: Float ) = {w=math.cos(ang*.5f);x=math.sin(ang*.5f);y=0;z=0} 57 | def fromAxisY( ang: Float ) = {w=math.cos(ang*.5f);x=0;y=math.sin(ang*.5f);z=0} 58 | def fromAxisZ( ang: Float ) = {w=math.cos(ang*.5f);x=0;y=0;z=math.sin(ang*.5f)} 59 | def fromAxisAngle( ang: Float, axis:Vec3 ) = { 60 | val sin2a = math.sin(ang*.5f) 61 | w = math.cos(ang*.5f) 62 | x = axis.x * sin2a 63 | y = axis.y * sin2a 64 | z = axis.z * sin2a 65 | } 66 | // from euler angles ( elevation, azimuth, bank ) 67 | def fromEuler( eu:Vec3 ) : Quat = fromEuler((eu.x,eu.y,eu.z)) 68 | def fromEuler( eu:(Float,Float,Float) ) : Quat= { //eu = Vec3( el, az, ba ) 69 | val c1 = math.cos(eu._1*.5f); val c2 = math.cos(eu._2*.5f); val c3 = math.cos(eu._3*.5f) 70 | val s1 = math.sin(eu._1*.5f); val s2 = math.sin(eu._2*.5f); val s3 = math.sin(eu._3*.5f) 71 | val tw = c2*c1; val tx = c2*s1; val ty = s2*c1; val tz = -s2*s1 72 | w = tw*c3 - tz*s3; x = tx*c3 + ty*s3; y = ty*c3 - tx*s3; z = tw*s3 + tz*c3 73 | this 74 | } 75 | 76 | //local unit vectors 77 | def toX() = Vec3(1.0 - 2.0*y*y - 2.0*z*z, 2.0*x*y + 2.0*z*w, 2.0*x*z - 2.0*y*w) 78 | def toY() = Vec3(2.0*x*y - 2.0*z*w, 1.0 - 2.0*x*x - 2.0*z*z, 2.0*y*z + 2.0*x*w) 79 | def toZ() = Vec3(2.0*x*z + 2.0*y*w, 2.0*y*z - 2.0*x*w, 1.0 - 2.0*x*x - 2.0*y*y) 80 | 81 | def toEuler() : (Float,Float,Float) = { 82 | val az = math.asin( -2.0 * (x*z - w*y)) 83 | val el = math.atan2( 2.0 * (y*z + w*x), w*w - x*x - y*y + z*z) 84 | val bank = math.atan2( 2.0 * (x*y + w*z), w*w + x*x - y*y - z*z) 85 | (el,az,bank) 86 | } 87 | def toEulerVec() : Vec3 = { 88 | val az = math.asin( -2.0 * (x*z - w*y)) 89 | val el = math.atan2( 2.0 * (y*z + w*x), w*w - x*x - y*y + z*z) 90 | val bank = math.atan2( 2.0 * (x*y + w*z), w*w + x*x - y*y - z*z) 91 | Vec3(el,az,bank) 92 | } 93 | 94 | def slerp(q:Quat, d:Float): Quat = { 95 | var (a,b) = (0.f,0.f) 96 | var negb = false 97 | var dotprod = dot(q) 98 | 99 | if( dotprod < -1 ) dotprod = -1 100 | else if( dotprod > 1 ) dotprod = 1 101 | 102 | if( dotprod < 0){ 103 | dotprod = -dotprod 104 | negb = true 105 | } 106 | 107 | val ang = math.acos( dotprod ) 108 | if( math.abs(ang) > Quat.eps ){ 109 | val sini = 1.f / math.sin(ang) 110 | a = math.sin(ang * (1.-d))*sini 111 | b = math.sin(ang*d)*sini 112 | if(negb) b = -b 113 | } else { 114 | a = d 115 | b = 1.-d 116 | } 117 | 118 | val quat = Quat(a*w+b*q.w, a*x+b*q.x, a*y+b*q.y, a*z+b*q.z) 119 | quat.normalize 120 | } 121 | def slerpTo(q:Quat, d:Float) = this.set( this.slerp(q,d)) 122 | 123 | def rotate(v:Vec3) = { 124 | val p = Quat( 125 | -x*v.x - y*v.y - z*v.z, 126 | w*v.x + y*v.z - z*v.y, 127 | w*v.y - x*v.z + z*v.x, 128 | w*v.z + x*v.y - y*v.x 129 | ) 130 | Vec3( 131 | p.x*w - p.w*x + p.z*y - p.y*z, 132 | p.y*w - p.w*y + p.x*z - p.z*x, 133 | p.z*w - p.w*z + p.y*x - p.x*y 134 | ) 135 | } 136 | 137 | def getRotationTo(src:Vec3, dst:Vec3):Quat = { 138 | val q = Quat() 139 | 140 | val d = src dot dst 141 | if (d >= 1.f) { 142 | // vectors are the same 143 | return q; 144 | } 145 | if (d < -0.999999999f) { 146 | // pick an axis to rotate around 147 | var axis = Vec3(0, 1, 0) cross src 148 | // if colinear, pick another: 149 | if (axis.magSq() < 0.00000000001f) { 150 | axis = Vec3(0, 0, 1) cross src 151 | } 152 | //axis.normalize(); 153 | q.fromAxisAngle(math.Pi, axis); 154 | } else { 155 | val s = math.sqrt((d+1.f)*2.f) 156 | val invs = 1./s 157 | val c = src cross dst 158 | q.x = c(0) * invs; 159 | q.y = c(1) * invs; 160 | q.z = c(2) * invs; 161 | q.w = s * 0.5; 162 | } 163 | return q.normalize(); 164 | } 165 | 166 | override def toString() = "[" + w + " " + x + " " + y + " " + z + "]" 167 | } 168 | 169 | 170 | -------------------------------------------------------------------------------- /odc/src/main/scala/spatial/Vec.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package spatial 4 | 5 | object Vec3 { 6 | 7 | def apply() = new Vec3(0,0,0) 8 | def apply(v:Vec3) = new Vec3(v.x,v.y,v.z) 9 | def apply( v: Float=0.f) = new Vec3( v, v, v) 10 | def apply( vv: Double) = { val v=vv.toFloat; new Vec3( v, v, v) } 11 | def apply( x: Float, y: Float, z: Float) = new Vec3(x,y,z) 12 | def apply( x: Double, y: Double, z: Double) = new Vec3(x.toFloat,y.toFloat,z.toFloat) 13 | 14 | def unapply( v: Vec3): Some[(Float,Float,Float)] = Some((v.x,v.y,v.z)) 15 | 16 | trait Vec3Numeric extends math.Numeric[Vec3] { 17 | def plus(x: Vec3, y: Vec3) = x+y 18 | def minus(x: Vec3, y: Vec3) = x-y 19 | def times(x: Vec3, y: Vec3) = x*y 20 | def negate(x: Vec3): Vec3 = -x 21 | def fromInt(x: Int) = Vec3(x) 22 | def toInt(x: Vec3) = x.mag().toInt 23 | def toLong(x: Vec3) = x.mag().toLong 24 | def toFloat(x: Vec3) = x.mag() 25 | def toDouble(x: Vec3) = x.mag().toDouble 26 | def compare(x:Vec3,y:Vec3) = (x.mag() - y.mag()).toInt 27 | } 28 | implicit object Vec3Numeric extends Vec3Numeric 29 | } 30 | 31 | class Vec3( var x: Float, var y: Float, var z: Float ){ 32 | 33 | def apply(i:Int) = i match { case 0 => x; case 1 => y; case 2 => z;} 34 | def update(i:Int,v:Float) = i match { case 0 => x=v; case 1 => y=v; case 2 => z=v;} 35 | 36 | def set(v:Vec3) = { x=v.x; y=v.y; z=v.z } 37 | def set(v:Float) = { x=v; y=v; z=v } 38 | def set(a:Float,b:Float,c:Float) = { x=a; y=b; z=c; } 39 | def set(v:(Float,Float,Float)) = { x=v._1; y=v._2; z=v._3 } 40 | 41 | def +(v: Vec3) = Vec3( x+v.x, y+v.y, z+v.z ) 42 | def +=(v: Vec3) = { x+=v.x; y+=v.y; z+=v.z } 43 | def -(v: Vec3) = Vec3( x-v.x, y-v.y, z-v.z ) 44 | def -=(v: Vec3) = { x-=v.x; y-=v.y; z-=v.z } 45 | def unary_- = Vec3( -x, -y, -z ) 46 | def *(s: Float ) = Vec3(s*x, s*y, s*z) 47 | def *(v: Vec3 ) = Vec3(v.x*x, v.y*y, v.z*z) 48 | def *=(s: Float) = { x*=s; y*=s; z*=s } 49 | def /(s: Float ) = Vec3(x/s, y/s, z/s) 50 | 51 | def dot(v: Vec3) : Float = x*v.x + y*v.y + z*v.z 52 | def cross( v: Vec3) = Vec3( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x ) 53 | def magSq() = this dot this 54 | def mag() = math.sqrt( magSq() ).toFloat 55 | def normalize() = this * (1.0f / mag() ) 56 | 57 | def zero() = {x=0;y=0;z=0} 58 | override def toString() = "[" + x + " " + y + " " + z + "]" 59 | 60 | def lerp( v:Vec3, d:Float ) = this + (v-this)*d 61 | def lerpTo( v:Vec3, d:Float) = this.set(this.lerp(v,d)) 62 | 63 | } 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /odc/src/main/scala/tracking/PIDController.scala: -------------------------------------------------------------------------------- 1 | 2 | package org.opendronecontrol 3 | package tracking 4 | 5 | import numeric._ 6 | 7 | class PIDController[T](implicit num:TimesFloatNumeric[T]) { 8 | 9 | import num._ 10 | 11 | var Kp:T = null.asInstanceOf[T] // proportional gain on current error 12 | var Ki:T = null.asInstanceOf[T] // integral gain/reset on sum of error 13 | var Kd:T = null.asInstanceOf[T] // derivative gain/rate 14 | 15 | var dt = 0.f // change in time in ms 16 | var t0:Long = 0 // last time in ms 17 | 18 | var setpoint:T = null.asInstanceOf[T] // destination of process variable 19 | var last:T = null.asInstanceOf[T] // last process variable 20 | 21 | var error:T = null.asInstanceOf[T] // difference setpoint - process variable 22 | var integral:T = null.asInstanceOf[T] // accumulated error 23 | var derivative:T = null.asInstanceOf[T] // derivative of error 24 | 25 | var out:T = null.asInstanceOf[T] // PID controller output / Manipulated Variable 26 | 27 | def setGains(kp:T,ki:T,kd:T){ 28 | Kp = kp 29 | Ki = ki 30 | Kd = kd 31 | } 32 | 33 | def set(dest:T) = setpoint = dest 34 | 35 | def step(current:T) : Option[T] = { 36 | 37 | if( t0 == 0 ){ 38 | t0 = System.currentTimeMillis() 39 | return None; 40 | } 41 | val t = System.currentTimeMillis() 42 | dt = (t - t0).toFloat 43 | t0 = t 44 | 45 | val diff = setpoint - current 46 | derivative = times( (diff - error), (1.f/dt) ) 47 | integral += times( diff, dt ) 48 | error = diff 49 | 50 | last = current 51 | 52 | out = Kp*error + Ki*integral + Kd*derivative 53 | Some(out) 54 | } 55 | 56 | } 57 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/CommandQueue.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone; 3 | 4 | import java.util.Iterator; 5 | import java.util.LinkedList; 6 | import java.util.NoSuchElementException; 7 | 8 | import java.util.logging.Logger; 9 | 10 | public class CommandQueue 11 | { 12 | private LinkedList data; 13 | private int maxSize; 14 | @SuppressWarnings("unused") 15 | private Logger log = Logger.getLogger(getClass().getName()); 16 | 17 | public CommandQueue(int maxSize) 18 | { 19 | data = new LinkedList(); 20 | this.maxSize = maxSize; 21 | } 22 | 23 | public synchronized DroneCommand take() throws InterruptedException 24 | { 25 | while(true) 26 | { 27 | DroneCommand res = null; 28 | try { 29 | res = data.removeLast(); 30 | } catch (NoSuchElementException ex) { 31 | res = null; 32 | } 33 | if(res != null) 34 | { 35 | // log.debug("[" + data.size() + "] Returning " + res); 36 | if(res.isSticky()) 37 | { 38 | int sc = res.incrementStickyCounter(); 39 | data.addLast(res); 40 | if(sc > 1) 41 | Thread.sleep(res.getStickyRate()); 42 | } 43 | return res; 44 | } else 45 | { 46 | // log.debug("Waiting for data"); 47 | wait(); 48 | } 49 | } 50 | } 51 | 52 | public synchronized void add(DroneCommand cmd) 53 | { 54 | Iterator i = data.iterator(); 55 | int p = cmd.getPriority(); 56 | int pos = -1; 57 | 58 | while(i.hasNext()) 59 | { 60 | DroneCommand x = i.next(); 61 | pos++; 62 | int xp = x.getPriority(); 63 | if(xp < p) 64 | { 65 | // Skipping 66 | continue; 67 | } else 68 | { 69 | // Found insertion point. 70 | if(!x.replaces(cmd)) 71 | { 72 | // log.debug("[" + data.size() + "] Adding command " + 73 | // cmd); 74 | data.add(pos, cmd); 75 | notify(); 76 | } else 77 | { 78 | // log.debug("Replacing duplicate element " + cmd); 79 | data.set(pos, cmd); 80 | } 81 | cmd = null; // inserted 82 | break; 83 | } 84 | } 85 | 86 | if(cmd != null) 87 | { 88 | // log.debug("[" + data.size() + "] Adding command " + cmd); 89 | data.addLast(cmd); 90 | notify(); 91 | } 92 | 93 | if(data.size() > maxSize) 94 | { 95 | // TODO: trim 96 | } 97 | } 98 | 99 | public synchronized int size() 100 | { 101 | return data.size(); 102 | } 103 | 104 | public synchronized void clear() 105 | { 106 | data.clear(); 107 | notify(); 108 | } 109 | 110 | } 111 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/CommandSender.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone; 3 | 4 | import java.io.IOException; 5 | import java.net.*; 6 | 7 | import java.util.logging.Logger; 8 | 9 | import com.codeminders.ardrone.commands.*; 10 | 11 | public class CommandSender implements Runnable 12 | { 13 | private static final int CMD_PORT = 5556; 14 | 15 | private CommandQueue cmd_queue; 16 | private ARDrone drone; 17 | private InetAddress drone_addr; 18 | private DatagramSocket cmd_socket; 19 | private int sequence = 1; 20 | 21 | private Logger log = Logger.getLogger(getClass().getName()); 22 | 23 | public CommandSender(CommandQueue cmd_queue, ARDrone drone, InetAddress drone_addr, DatagramSocket cmd_socket) 24 | { 25 | this.cmd_queue = cmd_queue; 26 | this.drone = drone; 27 | this.drone_addr = drone_addr; 28 | this.cmd_socket = cmd_socket; 29 | } 30 | 31 | @Override 32 | public void run() 33 | { 34 | while(true) 35 | { 36 | try 37 | { 38 | DroneCommand c = cmd_queue.take(); 39 | if(c instanceof QuitCommand) 40 | { 41 | // Terminating 42 | break; 43 | } 44 | 45 | if(c instanceof ATCommand) 46 | { 47 | ATCommand cmd = (ATCommand) c; 48 | //if(!(c instanceof KeepAliveCommand) && !(c instanceof MoveCommand) && !(c instanceof HoverCommand) && c.getStickyCounter()==0) 49 | log.fine("Q[" + cmd_queue.size() + "]Sending AT command " + c); 50 | byte[] pdata = cmd.getPacket(sequence++); // TODO: pass 51 | // sequence number 52 | DatagramPacket p = new DatagramPacket(pdata, pdata.length, drone_addr, CMD_PORT); 53 | cmd_socket.send(p); 54 | } 55 | } catch(InterruptedException e) 56 | { 57 | // ignoring 58 | } catch(IOException e) 59 | { 60 | drone.changeToErrorState(e); 61 | break; 62 | } 63 | } 64 | } 65 | 66 | } 67 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/DroneCommand.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone; 3 | 4 | public abstract class DroneCommand 5 | { 6 | protected static final int MIN_PRIORITY = 0; 7 | protected static final int HIGH_PRIORITY = 50; 8 | protected static final int VERY_HIGH_PRIORITY = 90; 9 | protected static final int MAX_PRIORITY = 100; 10 | protected static final long DEFAULT_STICKY_RATE_MS = 100; 11 | 12 | protected static final String LAND_TAKEOFF_CATEGORY = "takeoffland"; 13 | 14 | private int sticky_counter = 0; 15 | 16 | public abstract int getPriority(); 17 | 18 | public String getCategory() 19 | { 20 | return null; 21 | } 22 | 23 | public boolean isSticky() 24 | { 25 | return false; 26 | } 27 | 28 | /** 29 | * For sticky packets indicates how many times it has been sent. 30 | * 31 | * @return current value 32 | */ 33 | public int incrementStickyCounter() 34 | { 35 | return ++sticky_counter; 36 | } 37 | 38 | public int getStickyCounter() 39 | { 40 | return sticky_counter; 41 | } 42 | 43 | /** 44 | * For sticky packets indicates delay between sending repeated packets; 45 | */ 46 | public long getStickyRate() 47 | { 48 | return DEFAULT_STICKY_RATE_MS; 49 | } 50 | 51 | /** 52 | * This method is used to check if 2 commands are are relaceable by each 53 | * other. 54 | * 55 | * @param cmd 56 | * @return true if they are replaceable 57 | */ 58 | public boolean replaces(DroneCommand cmd) 59 | { 60 | if(equals(cmd)) 61 | { 62 | return true; 63 | } else 64 | { 65 | String c = getCategory(); 66 | return c != null && c.equals(cmd.getCategory()); 67 | } 68 | } 69 | 70 | } 71 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/DroneStatusChangeListener.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone; 3 | 4 | public interface DroneStatusChangeListener 5 | { 6 | 7 | /** 8 | * This method is called whenever the drone changes from BOOTSTRAP or 9 | * ERROR modes to DEMO mode. Could be used for user-supplied initialization 10 | */ 11 | void ready(); 12 | 13 | } 14 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/DroneVideoListener.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone; 3 | 4 | import java.awt.image.BufferedImage; 5 | 6 | public interface DroneVideoListener 7 | { 8 | void frameReceived(int startX, int startY, int w, int h, int[] rgbArray, int offset, int scansize); 9 | void frameReceived(BufferedImage bi); 10 | } 11 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | New BSD License 4 | 5 | Copyright (c) 2012, com.codeminders 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 9 | 10 | Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 11 | Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 12 | Neither the name of the nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/NavData.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone; 2 | 3 | import java.util.List; 4 | 5 | import com.codeminders.ardrone.data.navdata.ControlAlgorithm; 6 | import com.codeminders.ardrone.data.navdata.CtrlState; 7 | import com.codeminders.ardrone.data.navdata.FlyingState; 8 | import com.codeminders.ardrone.data.navdata.Mode; 9 | import com.codeminders.ardrone.data.navdata.vision.VisionTag; 10 | 11 | public interface NavData { 12 | 13 | /** 14 | * 15 | * @return value in meters 16 | */ 17 | public abstract float getAltitude(); 18 | 19 | public abstract int getBattery(); 20 | 21 | public abstract ControlAlgorithm getControlAlgorithm(); 22 | 23 | public abstract CtrlState getControlState(); 24 | 25 | public abstract float getLongitude(); 26 | 27 | public abstract Mode getMode(); 28 | 29 | /** 30 | * 31 | * @return value in degrees 32 | */ 33 | public abstract float getPitch(); 34 | 35 | /** 36 | * 37 | * @return value in degrees 38 | */ 39 | public abstract float getRoll(); 40 | 41 | public abstract int getSequence(); 42 | 43 | public abstract float getVx(); 44 | 45 | public abstract float getVz(); 46 | 47 | /** 48 | * 49 | * @return value in degrees 50 | */ 51 | public abstract float getYaw(); 52 | 53 | public abstract boolean isAcquisitionThreadOn(); 54 | 55 | public abstract boolean isADCWatchdogDelayed(); 56 | 57 | public abstract boolean isAltitudeControlActive(); 58 | 59 | public abstract boolean isAngelsOutOufRange(); 60 | 61 | public abstract boolean isATCodedThreadOn(); 62 | 63 | public abstract boolean isBatteryTooHigh(); 64 | 65 | public abstract boolean isBatteryTooLow(); 66 | 67 | public abstract boolean isCommunicationProblemOccurred(); 68 | 69 | public abstract boolean isControlReceived(); 70 | 71 | public abstract boolean isControlWatchdogDelayed(); 72 | 73 | public abstract boolean isCutoutSystemDetected(); 74 | 75 | public abstract boolean isEmergency(); 76 | 77 | public abstract boolean isFlying(); 78 | 79 | public abstract boolean isGyrometersDown(); 80 | 81 | public abstract boolean isMotorsDown(); 82 | 83 | public abstract boolean isNavDataBootstrap(); 84 | 85 | public abstract boolean isNavDataDemoOnly(); 86 | 87 | public abstract boolean isNavDataThreadOn(); 88 | 89 | public abstract boolean isNotEnoughPower(); 90 | 91 | public abstract boolean isPICVersionNumberOK(); 92 | 93 | public abstract boolean isTimerElapsed(); 94 | 95 | public abstract boolean isTooMuchWind(); 96 | 97 | public abstract boolean isTrimReceived(); 98 | 99 | public abstract boolean isTrimRunning(); 100 | 101 | public abstract boolean isTrimSucceeded(); 102 | 103 | public abstract boolean isUltrasonicSensorDeaf(); 104 | 105 | public abstract boolean isUserFeedbackOn(); 106 | 107 | public abstract boolean isVideoEnabled(); 108 | 109 | public abstract boolean isVideoThreadOn(); 110 | 111 | public abstract boolean isVisionEnabled(); 112 | 113 | public abstract FlyingState getFlyingState(); 114 | 115 | public abstract List getVisionTags(); 116 | 117 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/NavDataDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone; 2 | 3 | import com.codeminders.ardrone.data.DataDecoder; 4 | 5 | public abstract class NavDataDecoder extends DataDecoder { 6 | 7 | private ARDrone drone; 8 | 9 | public NavDataDecoder(ARDrone drone) { 10 | this.drone = drone; 11 | } 12 | 13 | public void notifyDroneWithDecodedNavdata(NavData navdata) { 14 | drone.navDataReceived(navdata); 15 | } 16 | 17 | } 18 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/NavDataListener.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone; 3 | 4 | public interface NavDataListener 5 | { 6 | void navDataReceived(NavData nd); 7 | } 8 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/TestH264DataDecoder.java: -------------------------------------------------------------------------------- 1 | // package com.codeminders.ardrone.decoder; 2 | 3 | // import java.io.InputStream; 4 | // import java.io.DataInputStream; 5 | // import java.util.Arrays; 6 | // import java.util.logging.Level; 7 | // import java.util.logging.Logger; 8 | 9 | // import com.codeminders.ardrone.ARDrone; 10 | // import com.codeminders.ardrone.VideoDataDecoder; 11 | // import com.twilight.h264.decoder.AVFrame; 12 | // import com.twilight.h264.decoder.AVPacket; 13 | // import com.twilight.h264.decoder.H264Decoder; 14 | // import com.twilight.h264.decoder.MpegEncContext; 15 | // import com.twilight.h264.player.FrameUtils; 16 | 17 | // public class TestH264DataDecoder extends VideoDataDecoder { 18 | 19 | // Logger log = Logger.getLogger(this.getClass().getName()); 20 | 21 | // public static final int INBUF_SIZE = 65535; 22 | 23 | // H264Decoder codec; 24 | // MpegEncContext c = null; 25 | // int frame, len; 26 | // int[] got_picture = new int[1]; 27 | 28 | // AVFrame picture; 29 | 30 | // byte[] inbuf = new byte[INBUF_SIZE + MpegEncContext.FF_INPUT_BUFFER_PADDING_SIZE]; 31 | // int[] inbuf_int = new int[INBUF_SIZE + MpegEncContext.FF_INPUT_BUFFER_PADDING_SIZE]; 32 | // byte[] buf = new byte[1024]; 33 | // private int[] buffer = null; 34 | 35 | // AVPacket avpkt; 36 | 37 | // int dataPointer; 38 | 39 | 40 | // public TestH264DataDecoder(ARDrone drone) { 41 | // super(drone); 42 | 43 | // avpkt = new AVPacket(); 44 | // avpkt.av_init_packet(); 45 | 46 | // Arrays.fill(inbuf, INBUF_SIZE, MpegEncContext.FF_INPUT_BUFFER_PADDING_SIZE + INBUF_SIZE, (byte)0); 47 | 48 | // codec = new H264Decoder(); 49 | // if (codec == null) { 50 | // System.out.println("codec not found\n"); 51 | // System.exit(1); 52 | // } 53 | 54 | // c = MpegEncContext.avcodec_alloc_context(); 55 | // picture= AVFrame.avcodec_alloc_frame(); 56 | 57 | // if((codec.capabilities & H264Decoder.CODEC_CAP_TRUNCATED)!=0) 58 | // c.flags |= MpegEncContext.CODEC_FLAG_TRUNCATED; /* we do not send complete frames */ 59 | 60 | // if (c.avcodec_open(codec) < 0) { 61 | // System.out.println("could not open codec\n"); 62 | // System.exit(1); 63 | // } 64 | // } 65 | 66 | // @Override 67 | // public void run() { 68 | 69 | // //InputStream fin = getDataReader().getDataStream(); 70 | // DataInputStream fin = new DataInputStream(getDataReader().getDataStream()); 71 | // try { 72 | // // avpkt must contain exactly 1 NAL Unit in order for decoder to decode correctly. 73 | // // thus we must read until we get next NAL header before sending it to decoder. 74 | // // Find 1st NAL 75 | // int[] cacheRead = new int[3]; 76 | // cacheRead[0] = fin.read(); 77 | // cacheRead[1] = fin.read(); 78 | // cacheRead[2] = fin.read(); 79 | 80 | // while(!( 81 | // cacheRead[0] == 'a' && 82 | // cacheRead[1] == 'V' && 83 | // cacheRead[2] == 'E' 84 | // )) { 85 | // cacheRead[0] = cacheRead[1]; 86 | // cacheRead[1] = cacheRead[2]; 87 | // cacheRead[2] = fin.read(); 88 | // } // while 89 | 90 | // fin.skipBytes(2); 91 | // int paveSize = fin.readShort(); 92 | // int dataSize = fin.readInt(); 93 | // fin.skipBytes(paveSize - 12 + 4); 94 | 95 | // boolean hasMoreNAL = true; 96 | 97 | // // 4 first bytes always indicate NAL header 98 | // inbuf_int[0]=inbuf_int[1]=inbuf_int[2]=0x00; 99 | // inbuf_int[3]=0x01; 100 | 101 | // while(hasMoreNAL) { // TODO: Possible error because we use not file 102 | // dataPointer = 4; 103 | // // Find next NAL 104 | // cacheRead[0] = fin.read(); 105 | // if(cacheRead[0]==-1) hasMoreNAL = false; 106 | // cacheRead[1] = fin.read(); 107 | // if(cacheRead[1]==-1) hasMoreNAL = false; 108 | // cacheRead[2] = fin.read(); 109 | // if(cacheRead[2]==-1) hasMoreNAL = false; 110 | // while(!( 111 | // cacheRead[0] == 'P' && 112 | // cacheRead[1] == 'a' && 113 | // cacheRead[2] == 'V' 114 | // ) && hasMoreNAL) { 115 | // inbuf_int[dataPointer++] = cacheRead[0]; 116 | // cacheRead[0] = cacheRead[1]; 117 | // cacheRead[1] = cacheRead[2]; 118 | // cacheRead[2] = fin.read(); 119 | // if(cacheRead[2]==-1) hasMoreNAL = false; 120 | // } // while 121 | 122 | // avpkt.size = dataPointer; 123 | 124 | // avpkt.data_base = inbuf_int; 125 | // avpkt.data_offset = 0; 126 | 127 | // try { 128 | // while (avpkt.size > 0) { 129 | // len = c.avcodec_decode_video2(picture, got_picture, avpkt); 130 | // if (len < 0) { 131 | // System.out.println("Error while decoding frame "+ frame); 132 | // // Discard current packet and proceed to next packet 133 | // break; 134 | // } // if 135 | // if (got_picture[0]!=0) { 136 | // picture = c.priv_data.displayPicture; 137 | 138 | // int bufferSize = picture.imageWidth * picture.imageHeight; 139 | // if (buffer == null || bufferSize != buffer.length) { 140 | // buffer = new int[bufferSize]; 141 | // } 142 | // FrameUtils.YUV2RGB(picture, buffer); 143 | 144 | // notifyDroneWithDecodedFrame(0, 0, picture.imageWidth ,picture.imageHeight, buffer, 0, picture.imageWidth); 145 | // frame++; 146 | // } 147 | // avpkt.size -= len; 148 | // avpkt.data_offset += len; 149 | // } 150 | // } catch(Exception ie) { 151 | // // Any exception, we should try to proceed reading next packet! 152 | // log.log(Level.FINEST, "Error decodeing frame", ie); 153 | // } // try 154 | 155 | // } // while 156 | 157 | 158 | // } catch(Exception ex) { 159 | // log.log(Level.FINEST, "Error in decoder initialization", ex); 160 | // } 161 | 162 | 163 | // } 164 | 165 | // @Override 166 | // public void finish() { 167 | // c.avcodec_close(); 168 | // } 169 | 170 | // } 171 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/TestH264DataDecoder2.java: -------------------------------------------------------------------------------- 1 | // package com.codeminders.ardrone.decoder; 2 | 3 | // import java.io.InputStream; 4 | // import java.io.DataInputStream; 5 | // import java.util.Arrays; 6 | // import java.util.logging.Level; 7 | // import java.util.logging.Logger; 8 | 9 | // import com.codeminders.ardrone.ARDrone; 10 | // import com.codeminders.ardrone.VideoDataDecoder; 11 | // import com.twilight.h264.decoder.AVFrame; 12 | // import com.twilight.h264.decoder.AVPacket; 13 | // import com.twilight.h264.decoder.H264Decoder; 14 | // import com.twilight.h264.decoder.MpegEncContext; 15 | // import com.twilight.h264.player.FrameUtils; 16 | 17 | // public class TestH264DataDecoder2 extends VideoDataDecoder { 18 | 19 | // Logger log = Logger.getLogger(this.getClass().getName()); 20 | 21 | // public static final int INBUF_SIZE = 65535; 22 | 23 | // H264Decoder codec; 24 | // MpegEncContext c = null; 25 | // int frame, len; 26 | // int[] got_picture = new int[1]; 27 | 28 | // boolean done=false; 29 | 30 | // AVFrame picture; 31 | 32 | // byte[] inbuf = new byte[INBUF_SIZE + MpegEncContext.FF_INPUT_BUFFER_PADDING_SIZE]; 33 | // int[] inbuf_int = new int[INBUF_SIZE + MpegEncContext.FF_INPUT_BUFFER_PADDING_SIZE]; 34 | // byte[] buf = new byte[1024]; 35 | // private int[] buffer = null; 36 | 37 | // AVPacket avpkt; 38 | 39 | // int dataPointer; 40 | 41 | 42 | // public TestH264DataDecoder2(ARDrone drone) { 43 | // super(drone); 44 | 45 | // avpkt = new AVPacket(); 46 | // avpkt.av_init_packet(); 47 | 48 | // Arrays.fill(inbuf, INBUF_SIZE, MpegEncContext.FF_INPUT_BUFFER_PADDING_SIZE + INBUF_SIZE, (byte)0); 49 | 50 | // codec = new H264Decoder(); 51 | // if (codec == null) { 52 | // System.out.println("codec not found\n"); 53 | // System.exit(1); 54 | // } 55 | 56 | // c = MpegEncContext.avcodec_alloc_context(); 57 | // picture= AVFrame.avcodec_alloc_frame(); 58 | 59 | // if((codec.capabilities & H264Decoder.CODEC_CAP_TRUNCATED)!=0) 60 | // c.flags |= MpegEncContext.CODEC_FLAG_TRUNCATED; /* we do not send complete frames */ 61 | 62 | // if (c.avcodec_open(codec) < 0) { 63 | // System.out.println("could not open codec\n"); 64 | // System.exit(1); 65 | // } 66 | // } 67 | 68 | // public void skip(DataInputStream s, int n) throws java.io.IOException, java.lang.InterruptedException { 69 | // int left = n - s.skipBytes(n); 70 | // while( left > 0){ left -= s.skipBytes(left); Thread.sleep(5); } 71 | // } 72 | // public int readUntil(InputStream s) throws java.io.IOException, java.lang.InterruptedException { 73 | // int read = s.read(); 74 | // while( read == -1){ read = s.read(); Thread.sleep(5); } 75 | // return read; 76 | // } 77 | 78 | // public int readHeader(DataInputStream fin) throws java.io.IOException, java.lang.InterruptedException { 79 | // int[] cacheRead = new int[3]; 80 | // byte[] buf4 = new byte[4]; 81 | // byte[] buf2 = new byte[2]; 82 | 83 | // cacheRead[0] = readUntil(fin); 84 | // cacheRead[1] = readUntil(fin); 85 | // cacheRead[2] = readUntil(fin); 86 | 87 | // while(!(cacheRead[0] == 'P' && 88 | // cacheRead[1] == 'a' && 89 | // cacheRead[2] == 'V' )) { 90 | // cacheRead[0] = cacheRead[1]; 91 | // cacheRead[1] = cacheRead[2]; 92 | // cacheRead[2] = fin.read(); 93 | // } 94 | // skip(fin,3); //E version codec 95 | // fin.read(buf2); 96 | // fin.read(buf4); 97 | // //skip(fin,8); 98 | // int paveSize = little2bigs(buf2); 99 | // int dataSize = little2big(buf4); 100 | // System.out.println("paveSize: "+paveSize+" datasize: "+dataSize); 101 | // skip(fin, paveSize - 12); 102 | 103 | // return dataSize; 104 | // } 105 | 106 | // private int little2big(byte[ ] b) { 107 | // return ((b[3]&0xff)<<24)+((b[2]&0xff)<<16)+((b[1]&0xff)<<8)+(b[0]&0xff); 108 | // } 109 | // private int little2bigs(byte[ ] b) { 110 | // return ((b[1]&0xff)<<8)+(b[0]&0xff); 111 | // } 112 | 113 | // @Override 114 | // public void run() { 115 | 116 | // super.run(); 117 | // //InputStream fin = getDataReader().getDataStream(); 118 | // try { 119 | // int size=0; 120 | // DataInputStream fin = new DataInputStream(getDataReader().getDataStream()); 121 | 122 | // while(!done){ 123 | // pauseCheck(); 124 | // try{ 125 | 126 | // size = 15000; readHeader(fin); 127 | // int read = 0; 128 | // int left = size; 129 | // while( left > 0){ 130 | // int r = fin.read(inbuf,read,left); 131 | // if( r > 0){ 132 | // read += r; 133 | // left = size - read; 134 | // } 135 | // } 136 | // for( int i=0; i < size; i++){ inbuf_int[i] = inbuf[i]; } 137 | 138 | // avpkt.size = size; 139 | // avpkt.data_base = inbuf_int; 140 | // avpkt.data_offset = 0; 141 | 142 | // try { 143 | // while (avpkt.size > 0) { 144 | // len = c.avcodec_decode_video2(picture, got_picture, avpkt); 145 | // if (len < 0) { 146 | // System.out.println("Error while decoding frame "+ frame); 147 | // // Discard current packet and proceed to next packet 148 | // break; 149 | // } // if 150 | // if (got_picture[0]!=0) { 151 | // picture = c.priv_data.displayPicture; 152 | 153 | // int bufferSize = picture.imageWidth * picture.imageHeight; 154 | // if (buffer == null || bufferSize != buffer.length) { 155 | // buffer = new int[bufferSize]; 156 | // } 157 | // FrameUtils.YUV2RGB(picture, buffer); 158 | 159 | // notifyDroneWithDecodedFrame(0, 0, picture.imageWidth ,picture.imageHeight, buffer, 0, picture.imageWidth); 160 | // frame++; 161 | // } 162 | // avpkt.size -= len; 163 | // avpkt.data_offset += len; 164 | // } 165 | // } catch(Exception ie) { 166 | // // Any exception, we should try to proceed reading next packet! 167 | // log.log(Level.FINEST, "Error decodeing frame", ie); 168 | // } // try 169 | // }catch(Exception ie) { 170 | // // Any exception, we should try to proceed reading next packet! 171 | // System.out.println(ie); 172 | // } // try 173 | 174 | // } // while 175 | 176 | 177 | // } catch(Exception ex) { 178 | // log.log(Level.FINEST, "Error in decoder initialization", ex); 179 | // } 180 | 181 | 182 | // } 183 | 184 | // @Override 185 | // public void finish() { 186 | // done = true; 187 | // c.avcodec_close(); 188 | // } 189 | 190 | // } 191 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/VideoDataDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone; 2 | 3 | import com.codeminders.ardrone.data.DataDecoder; 4 | 5 | import java.awt.image.BufferedImage; 6 | 7 | 8 | public abstract class VideoDataDecoder extends DataDecoder { 9 | 10 | private ARDrone drone; 11 | 12 | public VideoDataDecoder(ARDrone drone) { 13 | this.drone = drone; 14 | } 15 | 16 | public void notifyDroneWithDecodedFrame(int startX, int startY, int width, int height, int[] rgbArray, int offset, int scansize) { 17 | drone.videoFrameReceived(startX, startY, width, height, rgbArray, offset, scansize); 18 | } 19 | public void notifyDroneWithDecodedFrame(BufferedImage bi) { 20 | drone.videoFrameReceived(bi); 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/ATCommand.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.commands; 3 | 4 | import java.io.UnsupportedEncodingException; 5 | 6 | import com.codeminders.ardrone.DroneCommand; 7 | 8 | public abstract class ATCommand extends DroneCommand 9 | { 10 | private String encodeParameter(Object p) 11 | { 12 | if(p instanceof Integer) 13 | return p.toString(); 14 | 15 | if(p instanceof Float) 16 | return Integer.toString(Float.floatToIntBits((Float) p)); 17 | 18 | if(p instanceof String) 19 | return "\"" + p + "\""; 20 | 21 | throw new IllegalArgumentException("Unsupported parameter type: " + p.getClass().getName() + " " + p); 22 | } 23 | 24 | public String getCommandString(int seq) 25 | { 26 | return "AT*" + getID() + "=" + seq + getParametersString() + "\r"; 27 | } 28 | 29 | protected abstract String getID(); 30 | 31 | public byte[] getPacket(int seq) 32 | { 33 | try 34 | { 35 | return getCommandString(seq).getBytes("ASCII"); 36 | } catch(UnsupportedEncodingException e) 37 | { 38 | // never happens 39 | return null; 40 | } 41 | } 42 | 43 | protected abstract Object[] getParameters(); 44 | 45 | private String getParametersString() 46 | { 47 | StringBuffer sb = new StringBuffer(); 48 | for(Object p : getParameters()) 49 | { 50 | sb.append(',').append(encodeParameter(p)); 51 | } 52 | 53 | return sb.toString(); 54 | } 55 | 56 | @Override 57 | public int getPriority() 58 | { 59 | return MIN_PRIORITY; 60 | } 61 | 62 | @Override 63 | public String toString() 64 | { 65 | StringBuilder builder = new StringBuilder(); 66 | builder.append(getClass().getSimpleName()); 67 | builder.append(" [ID="); 68 | builder.append(getID()); 69 | builder.append(", param="); 70 | builder.append(getParametersString()); 71 | builder.append("]"); 72 | return builder.toString(); 73 | } 74 | 75 | public boolean equals(Object obj) 76 | { 77 | if(obj == null || !(obj instanceof ATCommand)) 78 | return false; 79 | ATCommand o = (ATCommand) obj; 80 | return o.getCommandString(0).equals(getCommandString(0)); 81 | } 82 | 83 | } 84 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/ConfigureCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class ConfigureCommand extends ATCommand 4 | { 5 | protected String name; 6 | protected String value; 7 | 8 | public ConfigureCommand(String name, String value) 9 | { 10 | this.name = name; 11 | this.value = value; 12 | } 13 | 14 | @Override 15 | protected String getID() 16 | { 17 | return "CONFIG"; 18 | } 19 | 20 | @Override 21 | protected Object[] getParameters() 22 | { 23 | return new Object[] { name, value }; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/ControlCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class ControlCommand extends ATCommand 4 | { 5 | protected int arg1; 6 | protected int arg2; 7 | 8 | public ControlCommand(int arg1, int arg2) 9 | { 10 | this.arg1 = arg1; 11 | this.arg2 = arg2; 12 | } 13 | 14 | @Override 15 | protected String getID() 16 | { 17 | return "CTRL"; 18 | } 19 | 20 | @Override 21 | protected Object[] getParameters() 22 | { 23 | return new Object[] { arg1, arg2 }; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/EmergencyCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class EmergencyCommand extends RefCommand 4 | { 5 | public EmergencyCommand() 6 | { 7 | value |= (1<<8); 8 | } 9 | } 10 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/FlatTrimCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class FlatTrimCommand extends ATCommand 4 | { 5 | @Override 6 | protected String getID() 7 | { 8 | return "FTRIM"; 9 | } 10 | 11 | @Override 12 | protected Object[] getParameters() 13 | { 14 | return new Object[] {}; 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/HoverCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class HoverCommand extends PCMDCommand 4 | { 5 | public HoverCommand() 6 | { 7 | super(true); 8 | } 9 | } 10 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/KeepAliveCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class KeepAliveCommand extends ATCommand 4 | { 5 | 6 | @Override 7 | protected String getID() 8 | { 9 | return "COMWDG"; 10 | } 11 | 12 | @Override 13 | protected Object[] getParameters() 14 | { 15 | return new Object[] {}; 16 | } 17 | 18 | @Override 19 | public int getPriority() 20 | { 21 | return VERY_HIGH_PRIORITY; 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/LandCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class LandCommand extends RefCommand 4 | { 5 | public LandCommand() 6 | { 7 | // 9th bit set to 0 8 | } 9 | 10 | @Override 11 | public int getPriority() 12 | { 13 | return HIGH_PRIORITY; 14 | } 15 | 16 | public boolean isSticky() 17 | { 18 | return true; 19 | } 20 | 21 | public String getCategory() 22 | { 23 | return LAND_TAKEOFF_CATEGORY; 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/MoveCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class MoveCommand extends PCMDCommand 4 | { 5 | public MoveCommand(boolean combined_yaw_enabled, 6 | float left_right_tilt, 7 | float front_back_tilt, 8 | float vertical_speed, 9 | float angular_speed) 10 | { 11 | super(false); 12 | this.combined_yaw_enabled = combined_yaw_enabled; 13 | this.left_right_tilt = left_right_tilt; 14 | this.front_back_tilt = front_back_tilt; 15 | this.vertical_speed = vertical_speed; 16 | this.angular_speed = angular_speed; 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/PCMDCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class PCMDCommand extends ATCommand 4 | { 5 | protected boolean hover; 6 | protected boolean combined_yaw_enabled; 7 | protected float left_right_tilt; 8 | protected float front_back_tilt; 9 | protected float vertical_speed; 10 | protected float angular_speed; 11 | 12 | 13 | protected PCMDCommand(boolean hover) 14 | { 15 | this.hover = hover; 16 | } 17 | 18 | @Override 19 | protected String getID() 20 | { 21 | return "PCMD"; 22 | } 23 | 24 | @Override 25 | protected Object[] getParameters() 26 | { 27 | if(hover) 28 | return new Object[] { 0, 0f, 0f, 0f, 0f }; 29 | 30 | // int mode = combined_yaw_enabled ? 1 : 0; 31 | int mode = 1; 32 | if(combined_yaw_enabled) 33 | mode |= (1<<1); 34 | 35 | return new Object[] { mode, left_right_tilt, front_back_tilt, vertical_speed, angular_speed }; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/PlayAnimationCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class PlayAnimationCommand extends ATCommand 4 | { 5 | protected int animation_no; 6 | protected int duration; 7 | 8 | public PlayAnimationCommand(int animation_no, int duration) 9 | { 10 | this.animation_no = animation_no; 11 | this.duration = duration; 12 | } 13 | 14 | @Override 15 | protected String getID() 16 | { 17 | return "ANIM"; 18 | } 19 | 20 | @Override 21 | protected Object[] getParameters() 22 | { 23 | return new Object[] { animation_no, duration }; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/PlayLEDCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class PlayLEDCommand extends ATCommand 4 | { 5 | protected int animation_no; 6 | protected float frequency; 7 | protected int duration; 8 | 9 | public PlayLEDCommand(int animation_no, float frequency, int duration) 10 | { 11 | this.animation_no = animation_no; 12 | this.frequency = frequency; 13 | this.duration = duration; 14 | } 15 | 16 | @Override 17 | protected String getID() 18 | { 19 | return "LED"; 20 | } 21 | 22 | @Override 23 | protected Object[] getParameters() 24 | { 25 | return new Object[] { animation_no, frequency, duration }; 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/QuitCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | import com.codeminders.ardrone.DroneCommand; 4 | 5 | public class QuitCommand extends DroneCommand 6 | { 7 | @Override 8 | public int getPriority() 9 | { 10 | return MAX_PRIORITY; 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/RefCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class RefCommand extends ATCommand 4 | { 5 | protected int value; 6 | 7 | protected RefCommand() 8 | { 9 | value |= (1 << 18) | (1 << 20) | (1 << 22) | (1 << 24) | (1 << 28) ; 10 | } 11 | 12 | @Override 13 | protected String getID() 14 | { 15 | return "REF"; 16 | } 17 | 18 | @Override 19 | protected Object[] getParameters() 20 | { 21 | return new Object[] { value }; 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/commands/TakeOffCommand.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.commands; 2 | 3 | public class TakeOffCommand extends RefCommand 4 | { 5 | public TakeOffCommand() 6 | { 7 | value |= (1<<9); 8 | } 9 | 10 | public boolean isSticky() 11 | { 12 | return true; 13 | } 14 | 15 | public String getCategory() 16 | { 17 | return LAND_TAKEOFF_CATEGORY; 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/Controller.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.controllers; 3 | 4 | import java.io.IOException; 5 | 6 | /** 7 | * Base abstract class for supported controllers 8 | * 9 | * @author lord 10 | * 11 | */ 12 | public abstract class Controller 13 | { 14 | 15 | public abstract void close() throws IOException; 16 | 17 | public abstract String getName(); 18 | 19 | public abstract String getManufacturerString(); 20 | 21 | public abstract String getProductString(); 22 | 23 | public abstract GameControllerState read() throws IOException; 24 | 25 | @Override 26 | public String toString() 27 | { 28 | StringBuilder builder = new StringBuilder(); 29 | builder.append(getName() + " ["); 30 | builder.append("manufacturer="); 31 | builder.append(getManufacturerString()); 32 | builder.append(", product="); 33 | builder.append(getProductString()); 34 | builder.append("]"); 35 | return builder.toString(); 36 | } 37 | 38 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/ControllerData.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.controllers; 2 | 3 | public class ControllerData { 4 | private byte[] buf; 5 | int actualBufferDataLength; 6 | 7 | public ControllerData(byte[] buf, int actualBufferDataLength) { 8 | super(); 9 | this.buf = buf; 10 | this.actualBufferDataLength = actualBufferDataLength; 11 | } 12 | public byte[] getBuffer() { 13 | return buf; 14 | } 15 | public int getActualBufferDataLength() { 16 | return actualBufferDataLength; 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/ControllerStateChange.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.controllers; 3 | 4 | public class ControllerStateChange extends GameControllerState 5 | { 6 | protected boolean squareChanged; 7 | protected boolean crossChanged; 8 | protected boolean circleChanged; 9 | protected boolean triangleChanged; 10 | protected boolean L1Changed; 11 | protected boolean R1Changed; 12 | protected boolean L2Changed; 13 | protected boolean R2Changed; 14 | protected boolean selectChanged; 15 | protected boolean startChanged; 16 | protected boolean leftJoystickPressChanged; 17 | protected boolean rightJoystickPressChanged; 18 | protected boolean PSChanged; 19 | protected int hatSwitchLeftRightChange; 20 | protected int hatSwitchUpDownChange; 21 | protected int leftJoystickXChange; 22 | protected int leftJoystickYChange; 23 | protected int rightJoystickXChange; 24 | protected int rightJoystickYChange; 25 | 26 | // composite change flags 27 | private boolean buttonStateChanged; 28 | private boolean hatChanged; 29 | private boolean leftJoystickChanged; 30 | private boolean rightJoystickChanged; 31 | private boolean joysticksChanged; 32 | private boolean changed; 33 | 34 | /** 35 | * 36 | * @param o Old state 37 | * @param n New state 38 | */ 39 | public ControllerStateChange(GameControllerState o, GameControllerState n) 40 | { 41 | super(n); 42 | 43 | if(o==null) 44 | o=n; 45 | 46 | squareChanged = o.square != n.square; 47 | crossChanged = o.cross != n.cross; 48 | circleChanged = o.circle != n.circle; 49 | triangleChanged = o.triangle != n.triangle; 50 | L1Changed = o.L1 != n.L1; 51 | R1Changed = o.R1 != n.R1; 52 | L2Changed = o.L2 != n.L2; 53 | R2Changed = o.R2 != n.R2; 54 | selectChanged = o.select != n.select; 55 | startChanged = o.start != n.start; 56 | leftJoystickPressChanged = o.leftJoystickPress != n.leftJoystickPress; 57 | rightJoystickPressChanged = o.rightJoystickPress != n.rightJoystickPress; 58 | PSChanged = o.PS != n.PS; 59 | 60 | hatSwitchLeftRightChange = n.hatSwitchLeftRight - o.hatSwitchLeftRight; 61 | hatSwitchUpDownChange = n.hatSwitchUpDown - o.hatSwitchUpDown; 62 | 63 | leftJoystickXChange = n.leftJoystickX - o.leftJoystickX; 64 | leftJoystickYChange = n.leftJoystickY - o.leftJoystickY; 65 | rightJoystickXChange = n.rightJoystickX - o.rightJoystickX; 66 | rightJoystickYChange = n.rightJoystickY - o.rightJoystickY; 67 | 68 | buttonStateChanged = squareChanged || crossChanged || circleChanged || triangleChanged || L1Changed 69 | || R1Changed || L2Changed || R2Changed || selectChanged || startChanged || leftJoystickPressChanged 70 | || rightJoystickPressChanged || PSChanged; 71 | 72 | hatChanged = hatSwitchLeftRightChange != 0 || hatSwitchUpDownChange != 0; 73 | 74 | leftJoystickChanged = leftJoystickXChange != 0 || leftJoystickYChange != 0; 75 | rightJoystickChanged = rightJoystickXChange != 0 || rightJoystickYChange != 0; 76 | 77 | joysticksChanged = leftJoystickChanged || rightJoystickChanged; 78 | 79 | changed = joysticksChanged || hatChanged || buttonStateChanged; 80 | 81 | } 82 | 83 | public int getHatSwitchLeftRightChange() 84 | { 85 | return hatSwitchLeftRightChange; 86 | } 87 | 88 | public int getHatSwitchUpDownChange() 89 | { 90 | return hatSwitchUpDownChange; 91 | } 92 | 93 | public int getLeftJoystickXChange() 94 | { 95 | return leftJoystickXChange; 96 | } 97 | 98 | public int getLeftJoystickYChange() 99 | { 100 | return leftJoystickYChange; 101 | } 102 | 103 | public int getRightJoystickXChange() 104 | { 105 | return rightJoystickXChange; 106 | } 107 | 108 | public int getRightJoystickYChange() 109 | { 110 | return rightJoystickYChange; 111 | } 112 | 113 | public boolean isButtonStateChanged() 114 | { 115 | return buttonStateChanged; 116 | } 117 | 118 | public boolean isChanged() 119 | { 120 | return changed; 121 | } 122 | 123 | public boolean isCircleChanged() 124 | { 125 | return circleChanged; 126 | } 127 | 128 | public boolean isCrossChanged() 129 | { 130 | return crossChanged; 131 | } 132 | 133 | public boolean isHatChanged() 134 | { 135 | return hatChanged; 136 | } 137 | 138 | public boolean isJoysticksChanged() 139 | { 140 | return joysticksChanged; 141 | } 142 | 143 | public boolean isL1Changed() 144 | { 145 | return L1Changed; 146 | } 147 | 148 | public boolean isL2Changed() 149 | { 150 | return L2Changed; 151 | } 152 | 153 | public boolean isLeftJoystickChanged() 154 | { 155 | return leftJoystickChanged; 156 | } 157 | 158 | public boolean isLeftJoystickPressChanged() 159 | { 160 | return leftJoystickPressChanged; 161 | } 162 | 163 | public boolean isPSChanged() 164 | { 165 | return PSChanged; 166 | } 167 | 168 | public boolean isR1Changed() 169 | { 170 | return R1Changed; 171 | } 172 | 173 | public boolean isR2Changed() 174 | { 175 | return R2Changed; 176 | } 177 | 178 | public boolean isRightJoystickChanged() 179 | { 180 | return rightJoystickChanged; 181 | } 182 | 183 | public boolean isRightJoystickPressChanged() 184 | { 185 | return rightJoystickPressChanged; 186 | } 187 | 188 | public boolean isSelectChanged() 189 | { 190 | return selectChanged; 191 | } 192 | 193 | public boolean isSquareChanged() 194 | { 195 | return squareChanged; 196 | } 197 | 198 | public boolean isStartChanged() 199 | { 200 | return startChanged; 201 | } 202 | 203 | public boolean isTriangleChanged() 204 | { 205 | return triangleChanged; 206 | } 207 | } 208 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/GameControllerState.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.controllers; 3 | 4 | /** 5 | * Data structure describing state of generic compatible game controller. 6 | * 7 | * @author lord 8 | */ 9 | public class GameControllerState 10 | { 11 | // buttons with pictures 12 | protected boolean square; 13 | protected boolean cross; 14 | protected boolean circle; 15 | protected boolean triangle; 16 | 17 | // Front-side buttons 18 | protected boolean L1; 19 | protected boolean R1; 20 | protected boolean L2; 21 | protected boolean R2; 22 | 23 | // square small "select" button 24 | protected boolean select; 25 | // triangular small "start" button 26 | protected boolean start; 27 | 28 | // Pressing on joysticks (button) 29 | protected boolean leftJoystickPress; 30 | protected boolean rightJoystickPress; 31 | 32 | // PS3 button (sometimes labeled as Home on 3rd party models) 33 | protected boolean PS; 34 | 35 | // Direction pad (hatswitch) 36 | protected int hatSwitchLeftRight; 37 | protected int hatSwitchUpDown; 38 | 39 | // Analog joysticks 40 | 41 | protected int leftJoystickX; 42 | protected int leftJoystickY; 43 | 44 | protected int rightJoystickX; 45 | protected int rightJoystickY; 46 | 47 | public GameControllerState(boolean square, boolean cross, boolean circle, boolean triangle, boolean l1, boolean r1, 48 | boolean l2, boolean r2, boolean select, boolean start, boolean leftJoystickPress, 49 | boolean rightJoystickPress, boolean pS, int hatSwitchLeftRight, int hatSwitchUpDown, int leftJoystickX, 50 | int leftJoystickY, int rightJoystickX, int rightJoystickY) 51 | { 52 | this.square = square; 53 | this.cross = cross; 54 | this.circle = circle; 55 | this.triangle = triangle; 56 | L1 = l1; 57 | R1 = r1; 58 | L2 = l2; 59 | R2 = r2; 60 | this.select = select; 61 | this.start = start; 62 | this.leftJoystickPress = leftJoystickPress; 63 | this.rightJoystickPress = rightJoystickPress; 64 | PS = pS; 65 | this.hatSwitchLeftRight = hatSwitchLeftRight; 66 | this.hatSwitchUpDown = hatSwitchUpDown; 67 | this.leftJoystickX = leftJoystickX; 68 | this.leftJoystickY = leftJoystickY; 69 | this.rightJoystickX = rightJoystickX; 70 | this.rightJoystickY = rightJoystickY; 71 | } 72 | 73 | public GameControllerState(GameControllerState o) 74 | { 75 | this.square = o.square; 76 | this.cross = o.cross; 77 | this.circle = o.circle; 78 | this.triangle = o.triangle; 79 | L1 = o.L1; 80 | R1 = o.R1; 81 | L2 = o.L2; 82 | R2 = o.R2; 83 | this.select = o.select; 84 | this.start = o.start; 85 | this.leftJoystickPress = o.leftJoystickPress; 86 | this.rightJoystickPress = o.rightJoystickPress; 87 | PS = o.PS; 88 | this.hatSwitchLeftRight = o.hatSwitchLeftRight; 89 | this.hatSwitchUpDown = o.hatSwitchUpDown; 90 | this.leftJoystickX = o.leftJoystickX; 91 | this.leftJoystickY = o.leftJoystickY; 92 | this.rightJoystickX = o.rightJoystickX; 93 | this.rightJoystickY = o.rightJoystickY; 94 | 95 | } 96 | 97 | public GameControllerState() 98 | { 99 | } 100 | 101 | public int getHatSwitchLeftRight() 102 | { 103 | return hatSwitchLeftRight; 104 | } 105 | 106 | public int getHatSwitchUpDown() 107 | { 108 | return hatSwitchUpDown; 109 | } 110 | 111 | public int getLeftJoystickX() 112 | { 113 | return leftJoystickX; 114 | } 115 | 116 | public int getLeftJoystickY() 117 | { 118 | return leftJoystickY; 119 | } 120 | 121 | public int getRightJoystickX() 122 | { 123 | return rightJoystickX; 124 | } 125 | 126 | public int getRightJoystickY() 127 | { 128 | return rightJoystickY; 129 | } 130 | 131 | public boolean isCircle() 132 | { 133 | return circle; 134 | } 135 | 136 | public boolean isCross() 137 | { 138 | return cross; 139 | } 140 | 141 | public boolean isL1() 142 | { 143 | return L1; 144 | } 145 | 146 | public boolean isL2() 147 | { 148 | return L2; 149 | } 150 | 151 | public boolean isLeftJoystickPress() 152 | { 153 | return leftJoystickPress; 154 | } 155 | 156 | public boolean isPS() 157 | { 158 | return PS; 159 | } 160 | 161 | public boolean isR1() 162 | { 163 | return R1; 164 | } 165 | 166 | public boolean isR2() 167 | { 168 | return R2; 169 | } 170 | 171 | public boolean isRightJoystickPress() 172 | { 173 | return rightJoystickPress; 174 | } 175 | 176 | public boolean isSelect() 177 | { 178 | return select; 179 | } 180 | 181 | public boolean isSquare() 182 | { 183 | return square; 184 | } 185 | 186 | public boolean isStart() 187 | { 188 | return start; 189 | } 190 | 191 | public boolean isTriangle() 192 | { 193 | return triangle; 194 | } 195 | 196 | @Override 197 | public String toString() 198 | { 199 | StringBuilder builder = new StringBuilder(); 200 | builder.append("PS3ControllerState [square="); 201 | builder.append(square); 202 | builder.append(", cross="); 203 | builder.append(cross); 204 | builder.append(", circle="); 205 | builder.append(circle); 206 | builder.append(", triangle="); 207 | builder.append(triangle); 208 | builder.append(", L1="); 209 | builder.append(L1); 210 | builder.append(", R1="); 211 | builder.append(R1); 212 | builder.append(", L2="); 213 | builder.append(L2); 214 | builder.append(", R2="); 215 | builder.append(R2); 216 | builder.append(", select="); 217 | builder.append(select); 218 | builder.append(", start="); 219 | builder.append(start); 220 | builder.append(", rightJoystickPress="); 221 | builder.append(rightJoystickPress); 222 | builder.append(", leftJoystickPress="); 223 | builder.append(leftJoystickPress); 224 | builder.append(", PS="); 225 | builder.append(PS); 226 | builder.append(", hatSwitchLeftRight="); 227 | builder.append(hatSwitchLeftRight); 228 | builder.append(", hatSwitchUpDown="); 229 | builder.append(hatSwitchUpDown); 230 | builder.append(", leftJoystickX="); 231 | builder.append(leftJoystickX); 232 | builder.append(", leftJoystickY="); 233 | builder.append(leftJoystickY); 234 | builder.append(", rightJoystickX="); 235 | builder.append(rightJoystickX); 236 | builder.append(", rightJoystickY="); 237 | builder.append(rightJoystickY); 238 | builder.append("]"); 239 | return builder.toString(); 240 | } 241 | 242 | } 243 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/decoders/AfterGlowControllerDecoder.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.controllers.decoders; 3 | 4 | import java.io.IOException; 5 | import java.util.BitSet; 6 | 7 | import com.codeminders.ardrone.controllers.GameControllerState; 8 | import com.codeminders.ardrone.controllers.ControllerData; 9 | 10 | /** 11 | * "Afterglow" controller for PS3 state decodder 12 | * 13 | * @author lord 14 | * 15 | */ 16 | public class AfterGlowControllerDecoder implements ControllerStateDecoder 17 | { 18 | 19 | private int joystickCoordConv(byte b) 20 | { 21 | int v = b < 0 ? b + 256 : b; 22 | return v - 128; 23 | } 24 | 25 | @Override 26 | public GameControllerState decodeState(ControllerData data) throws IOException 27 | { 28 | byte[] buf = data.getBuffer(); 29 | 30 | BitSet bs = new BitSet(13); 31 | for(int i = 0; i < 8; i++) 32 | { 33 | if((1 & (buf[0] >> i)) == 1) 34 | bs.set(i); 35 | } 36 | for(int i = 0; i < 5; i++) 37 | { 38 | if((1 & (buf[1] >> i)) == 1) 39 | bs.set(8 + i); 40 | } 41 | 42 | int i = 0; 43 | boolean square = bs.get(i++); 44 | boolean cross = bs.get(i++); 45 | boolean circle = bs.get(i++); 46 | boolean triangle = bs.get(i++); 47 | boolean L1 = bs.get(i++); 48 | boolean R1 = bs.get(i++); 49 | boolean L2 = bs.get(i++); 50 | boolean R2 = bs.get(i++); 51 | boolean select = bs.get(i++); 52 | boolean start = bs.get(i++); 53 | boolean leftJoystickPress = bs.get(i++); 54 | boolean rightJoystickPress = bs.get(i++); 55 | boolean PS = bs.get(i++); 56 | 57 | int leftJoystickX = joystickCoordConv(buf[3]); 58 | int leftJoystickY = joystickCoordConv(buf[4]); 59 | int rightJoystickX = joystickCoordConv(buf[5]); 60 | int rightJoystickY = joystickCoordConv(buf[6]); 61 | 62 | // TODO: decode HAT switch 63 | int hatSwitchLeftRight = 0; 64 | int hatSwitchUpDown = 0; 65 | 66 | GameControllerState res = new GameControllerState(square, cross, circle, triangle, L1, R1, L2, R2, select, start, 67 | leftJoystickPress, rightJoystickPress, PS, hatSwitchLeftRight, hatSwitchUpDown, leftJoystickX, 68 | leftJoystickY, rightJoystickX, rightJoystickY); 69 | 70 | 71 | return res; 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/decoders/ControllerStateDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.controllers.decoders; 2 | 3 | import java.io.IOException; 4 | 5 | import com.codeminders.ardrone.controllers.GameControllerState; 6 | import com.codeminders.ardrone.controllers.ControllerData; 7 | 8 | public interface ControllerStateDecoder { 9 | 10 | public GameControllerState decodeState(ControllerData data) throws IOException; 11 | } 12 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/decoders/MotioninJoyVirtualStateDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.controllers.decoders; 2 | 3 | import java.io.IOException; 4 | import java.util.BitSet; 5 | 6 | import com.codeminders.ardrone.controllers.GameControllerState; 7 | import com.codeminders.ardrone.controllers.ControllerData; 8 | 9 | public class MotioninJoyVirtualStateDecoder implements ControllerStateDecoder { 10 | 11 | public GameControllerState decodeState(ControllerData data) throws IOException { 12 | 13 | byte[] buf = data.getBuffer(); 14 | 15 | BitSet bs = new BitSet(16); 16 | 17 | for(int i = 0; i < 8; i++) 18 | { 19 | if((1 & (buf[0] >> i)) == 1) 20 | bs.set(i); 21 | } 22 | 23 | for(int i = 0; i < 8; i++) 24 | { 25 | if((1 & (buf[1] >> i)) == 1) 26 | bs.set(8 + i); 27 | } 28 | 29 | int i = 0; 30 | 31 | boolean triangle = bs.get(i++); 32 | boolean circle = bs.get(i++); 33 | boolean cross = bs.get(i++); 34 | boolean square = bs.get(i++); 35 | boolean L1 = bs.get(i++); 36 | boolean R1 = bs.get(i++); 37 | boolean L2 = bs.get(i++); 38 | boolean R2 = bs.get(i++); 39 | boolean select = bs.get(i++); 40 | boolean leftJoystickPress = bs.get(i++); 41 | boolean rightJoystickPress = bs.get(i++); 42 | boolean start = bs.get(i++); 43 | boolean PS = bs.get(i++); 44 | 45 | 46 | int leftJoystickX = joystickCoordConv(buf[3]); 47 | int leftJoystickY = joystickCoordConv(buf[4]); 48 | int rightJoystickX = joystickCoordConv(buf[5]); 49 | int rightJoystickY = joystickCoordConv(buf[8]); 50 | 51 | // TODO: decode HAT switch 52 | int hatSwitchLeftRight = 0; 53 | int hatSwitchUpDown = 0; 54 | 55 | GameControllerState res = new GameControllerState(square, cross, circle, triangle, L1, R1, L2, R2, select, start, 56 | leftJoystickPress, rightJoystickPress, PS, hatSwitchLeftRight, hatSwitchUpDown, leftJoystickX, 57 | leftJoystickY, rightJoystickX, rightJoystickY); 58 | 59 | return res; 60 | } 61 | 62 | private int joystickCoordConv(byte b) 63 | { 64 | int v = b < 0 ? b + 256 : b; 65 | return(v - 128); 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/controllers/decoders/SonyPS3ControllerStateDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.controllers.decoders; 2 | 3 | import java.io.IOException; 4 | import java.util.BitSet; 5 | 6 | import com.codeminders.ardrone.controllers.GameControllerState; 7 | import com.codeminders.ardrone.controllers.ControllerData; 8 | 9 | public class SonyPS3ControllerStateDecoder implements ControllerStateDecoder { 10 | 11 | private int joystickCoordConv(byte b) 12 | { 13 | int v = b < 0 ? b + 256 : b; 14 | return(v - 128); 15 | } 16 | 17 | public GameControllerState decodeState(ControllerData data) throws IOException { 18 | 19 | byte[] buf = data.getBuffer(); 20 | 21 | BitSet bs = new BitSet(24); 22 | for(int i = 0; i < 8; i++) 23 | { 24 | if((1 & (buf[2] >> i)) == 1) 25 | bs.set(i); 26 | } 27 | 28 | for(int i = 0; i < 8; i++) 29 | { 30 | if((1 & (buf[3] >> i)) == 1) 31 | bs.set(8 + i); 32 | } 33 | for(int i = 0; i < 8; i++) 34 | { 35 | if((1 & (buf[4] >> i)) == 1) 36 | bs.set(16 + i); 37 | } 38 | 39 | int i = 0; 40 | boolean select = bs.get(i++); 41 | boolean leftJoystickPress = bs.get(i++); 42 | boolean rightJoystickPress = bs.get(i++); 43 | boolean start = bs.get(i++); 44 | bs.get(i++); 45 | bs.get(i++); 46 | bs.get(i++); 47 | bs.get(i++); 48 | boolean L2 = bs.get(i++); 49 | boolean R2 = bs.get(i++); 50 | boolean R1 = bs.get(i++); 51 | boolean L1 = bs.get(i++); 52 | boolean triangle = bs.get(i++); 53 | boolean circle = bs.get(i++); 54 | boolean cross = bs.get(i++); 55 | boolean square = bs.get(i++); 56 | boolean PS = bs.get(i++); 57 | 58 | int leftJoystickX = joystickCoordConv(buf[6]); 59 | int leftJoystickY = joystickCoordConv(buf[7]); 60 | int rightJoystickX = joystickCoordConv(buf[8]); 61 | int rightJoystickY = joystickCoordConv(buf[9]); 62 | 63 | // TODO: decode HAT switch 64 | int hatSwitchLeftRight = 0; 65 | int hatSwitchUpDown = 0; 66 | 67 | GameControllerState res = new GameControllerState(square, cross, circle, triangle, L1, R1, L2, R2, select, start, 68 | leftJoystickPress, rightJoystickPress, PS, hatSwitchLeftRight, hatSwitchUpDown, leftJoystickX, 69 | leftJoystickY, rightJoystickX, rightJoystickY); 70 | 71 | return res; 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/ARDroneDataReader.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | 6 | public interface ARDroneDataReader { 7 | /** 8 | * @param buf input data buffer to read; 9 | * @return length of data that is obtained from ardrone 10 | * @throws IOException 11 | */ 12 | public int readDataBlock(byte[] buf) throws IOException; 13 | 14 | public InputStream getDataStream(); 15 | 16 | public boolean isStreamSupported(); 17 | 18 | public void reconnect() throws IOException; 19 | 20 | public void finish(); 21 | } 22 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/ChannelProcessor.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data; 2 | 3 | import java.util.logging.Level; 4 | import java.util.logging.Logger; 5 | 6 | 7 | public class ChannelProcessor { 8 | 9 | private Logger log = Logger.getLogger(getClass().getName()); 10 | ARDroneDataReader reader; 11 | DataDecoder decoder; 12 | 13 | private static int STOP_TIMEOUT = 3000; // 3 sec. 14 | 15 | public ChannelProcessor(ARDroneDataReader reader, DataDecoder decoder) { 16 | super(); 17 | this.reader = reader; 18 | this.decoder = decoder; 19 | 20 | decoder.setDataReader(reader); // decoder and reader is now linked 21 | decoder.start(); 22 | } 23 | 24 | public void finish() { 25 | decoder.finish(); 26 | try { 27 | decoder.join(STOP_TIMEOUT); 28 | } catch (InterruptedException e) { 29 | log.log(Level.FINEST, "Waiting till decoder is stopped is interrupted", e); 30 | } 31 | reader.finish(); 32 | } 33 | 34 | public void pause() { 35 | decoder.pauseDecoding(); 36 | } 37 | 38 | public void resume() { 39 | decoder.pauseDecoding(); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/DataDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data; 2 | 3 | import java.util.logging.Level; 4 | import java.util.logging.Logger; 5 | 6 | 7 | public abstract class DataDecoder extends Thread { 8 | 9 | private Logger log = Logger.getLogger(this.getClass().getName()); 10 | 11 | private ARDroneDataReader datareader; 12 | private boolean pauseFlag; 13 | 14 | 15 | @Override 16 | public synchronized void start() { 17 | super.start(); 18 | if (null == datareader) { 19 | throw new RuntimeException("No reading thread is arrached"); 20 | } 21 | } 22 | 23 | protected void pauseCheck(){ 24 | if (pauseFlag) { 25 | synchronized(this) { 26 | if (pauseFlag) { 27 | try { 28 | wait(); 29 | } catch (InterruptedException e) { 30 | log.log(Level.SEVERE, "Pause is interrupted", e); 31 | } 32 | } 33 | } 34 | } 35 | } 36 | 37 | protected void setDataReader(ARDroneDataReader datareader) { 38 | if (this.isAlive()) { 39 | throw new RuntimeException("Rading Thrad already started. You can't change stream on fly"); 40 | } 41 | this.datareader = datareader; 42 | } 43 | 44 | public ARDroneDataReader getDataReader() { 45 | return datareader; 46 | } 47 | 48 | public synchronized void pauseDecoding() { 49 | pauseFlag = true; 50 | } 51 | 52 | public synchronized void resumeDecoding() { 53 | pauseFlag = false; 54 | notify(); 55 | } 56 | 57 | public abstract void finish(); 58 | 59 | } 60 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/ARDrone10NavDataDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.decoder.ardrone10; 2 | 3 | import java.io.IOException; 4 | import java.nio.ByteBuffer; 5 | import java.util.logging.Level; 6 | import java.util.logging.Logger; 7 | 8 | import com.codeminders.ardrone.ARDrone; 9 | import com.codeminders.ardrone.NavDataDecoder; 10 | import com.codeminders.ardrone.data.ARDroneDataReader; 11 | import com.codeminders.ardrone.data.decoder.ardrone10.navdata.ARDrone10NavData; 12 | import com.codeminders.ardrone.data.navdata.NavDataFormatException; 13 | 14 | public class ARDrone10NavDataDecoder extends NavDataDecoder { 15 | 16 | private Logger log = Logger.getLogger(this.getClass().getName()); 17 | 18 | private boolean done = false; 19 | 20 | byte[] buffer; 21 | 22 | public ARDrone10NavDataDecoder(ARDrone drone, int buffer_size) { 23 | super(drone); 24 | setName("ARDrone 1.0 NavData decodding thread"); 25 | buffer = new byte[buffer_size]; 26 | } 27 | 28 | @Override 29 | public void run() { 30 | 31 | super.run(); 32 | ARDroneDataReader reader = getDataReader(); 33 | while (!done) { 34 | try { 35 | 36 | // pauseCheck(); 37 | 38 | int len = reader.readDataBlock(buffer); 39 | 40 | if (len > 0) { 41 | try { 42 | notifyDroneWithDecodedNavdata(ARDrone10NavData.createFromData(ByteBuffer.wrap(buffer), len)); 43 | } catch (NavDataFormatException e) { 44 | log.log(Level.SEVERE, "Failed to decode receivd navdata information", e); 45 | } catch (Exception ex) { 46 | log.log(Level.SEVERE, "Failed to decode receivd navdata information", ex); 47 | } 48 | } 49 | } catch (IOException e) { 50 | System.out.println(e + "in NavDataDecoder"); 51 | // log.log(Level.SEVERE, " Error reading data from data input stream. Stopping decoding thread", e); 52 | try { 53 | reader.reconnect(); 54 | } catch (IOException e1) { 55 | log.log(Level.SEVERE, " Error reconnecting data reader", e); 56 | } 57 | } 58 | } 59 | log.fine("Decodding thread is stopped"); 60 | } 61 | 62 | @Override 63 | public void finish() { 64 | done = true; 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/ARDrone10VideoDataDecoder.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.decoder.ardrone10; 2 | 3 | import java.io.IOException; 4 | import java.util.logging.Level; 5 | import java.util.logging.Logger; 6 | 7 | import com.codeminders.ardrone.ARDrone; 8 | import com.codeminders.ardrone.VideoDataDecoder; 9 | import com.codeminders.ardrone.data.ARDroneDataReader; 10 | import com.codeminders.ardrone.data.decoder.ardrone10.video.BufferedVideoImage; 11 | 12 | public class ARDrone10VideoDataDecoder extends VideoDataDecoder { 13 | 14 | private Logger log = Logger.getLogger(this.getClass().getName()); 15 | 16 | final BufferedVideoImage vi = new BufferedVideoImage(); 17 | 18 | private boolean done = false; 19 | 20 | byte[] buffer; 21 | 22 | public ARDrone10VideoDataDecoder(ARDrone drone, int buffer_size) { 23 | super(drone); 24 | buffer = new byte[buffer_size]; 25 | 26 | setName("ARDrone 1.0 Video decoding thread"); 27 | } 28 | 29 | @Override 30 | public void run() { 31 | 32 | super.run(); 33 | ARDroneDataReader reader = getDataReader(); 34 | int len = 0; 35 | while (!done) { 36 | try { 37 | pauseCheck(); 38 | len = reader.readDataBlock(buffer); 39 | if (len > 0) { 40 | vi.addImageStream(buffer, len); 41 | notifyDroneWithDecodedFrame(0, 0, vi.getWidth(), vi.getHeight(), vi.getJavaPixelData(), 0, vi.getWidth()); 42 | } 43 | } catch (IOException e) { 44 | System.out.println(e); 45 | log.log(Level.SEVERE, " Error reading data from data input stream. Stopping decoding thread", e); 46 | try { 47 | reader.reconnect(); 48 | } catch (IOException e1) { 49 | log.log(Level.SEVERE, " Error reconnecting data reader", e); 50 | } 51 | } 52 | } 53 | 54 | log.fine("Video Decodding thread is stopped"); 55 | } 56 | 57 | @Override 58 | public void finish() { 59 | done = true; 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/video/ImageSlice.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.data.decoder.ardrone10.video; 3 | 4 | // Copyright � 2007-2011, PARROT SA, all rights reserved. 5 | 6 | // DISCLAIMER 7 | // The APIs is provided by PARROT and contributors "AS IS" and any express or 8 | // implied warranties, including, but not limited to, the implied warranties of 9 | // merchantability 10 | // and fitness for a particular purpose are disclaimed. In no event shall PARROT 11 | // and contributors be liable for any direct, indirect, incidental, special, 12 | // exemplary, or 13 | // consequential damages (including, but not limited to, procurement of 14 | // substitute goods or services; loss of use, data, or profits; or business 15 | // interruption) however 16 | // caused and on any theory of liability, whether in contract, strict liability, 17 | // or tort (including negligence or otherwise) arising in any way out of the use 18 | // of this 19 | // software, even if advised of the possibility of such damage. 20 | 21 | // Author : Daniel Schmidt 22 | // Publishing date : 2010-01-06 23 | // based on work by : Wilke Jansoone 24 | 25 | // Redistribution and use in source and binary forms, with or without 26 | // modification, are permitted provided that the following conditions 27 | // are met: 28 | // - Redistributions of source code must retain the above copyright notice, this 29 | // list of conditions, the disclaimer and the original author of the source 30 | // code. 31 | // - Neither the name of the PixVillage Team, nor the names of its contributors 32 | // may be used to endorse or promote products derived from this software without 33 | // specific prior written permission. 34 | 35 | class ImageSlice 36 | { 37 | MacroBlock[] MacroBlocks; 38 | 39 | ImageSlice(int macroBlockCount) 40 | { 41 | MacroBlocks = new MacroBlock[macroBlockCount]; 42 | 43 | for(int index = 0; index < macroBlockCount; index++) 44 | MacroBlocks[index] = new MacroBlock(); 45 | } 46 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/video/MacroBlock.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.data.decoder.ardrone10.video; 3 | 4 | // Copyright � 2007-2011, PARROT SA, all rights reserved. 5 | 6 | // DISCLAIMER 7 | // The APIs is provided by PARROT and contributors "AS IS" and any express or 8 | // implied warranties, including, but not limited to, the implied warranties of 9 | // merchantability 10 | // and fitness for a particular purpose are disclaimed. In no event shall PARROT 11 | // and contributors be liable for any direct, indirect, incidental, special, 12 | // exemplary, or 13 | // consequential damages (including, but not limited to, procurement of 14 | // substitute goods or services; loss of use, data, or profits; or business 15 | // interruption) however 16 | // caused and on any theory of liability, whether in contract, strict liability, 17 | // or tort (including negligence or otherwise) arising in any way out of the use 18 | // of this 19 | // software, even if advised of the possibility of such damage. 20 | 21 | // Author : Daniel Schmidt 22 | // Publishing date : 2010-01-06 23 | // based on work by : Wilke Jansoone 24 | 25 | // Redistribution and use in source and binary forms, with or without 26 | // modification, are permitted provided that the following conditions 27 | // are met: 28 | // - Redistributions of source code must retain the above copyright notice, this 29 | // list of conditions, the disclaimer and the original author of the source 30 | // code. 31 | // - Neither the name of the PixVillage Team, nor the names of its contributors 32 | // may be used to endorse or promote products derived from this software without 33 | // specific prior written permission. 34 | 35 | class MacroBlock 36 | { 37 | short[][] DataBlocks; 38 | 39 | MacroBlock() 40 | { 41 | DataBlocks = new short[6][]; 42 | 43 | for(int index = 0; index < 6; index++) 44 | DataBlocks[index] = new short[64]; 45 | } 46 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/logger/ARDroneDataReaderAndLogWrapper.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.logger; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | 6 | import com.codeminders.ardrone.data.ARDroneDataReader; 7 | 8 | public class ARDroneDataReaderAndLogWrapper implements ARDroneDataReader { 9 | 10 | ARDroneDataReader reader; 11 | DataLogger logger; 12 | 13 | public ARDroneDataReaderAndLogWrapper(ARDroneDataReader reader, DataLogger logger) { 14 | super(); 15 | this.reader = reader; 16 | this.logger = logger; 17 | } 18 | 19 | @Override 20 | public int readDataBlock(byte[] buf) throws IOException { 21 | int len = reader.readDataBlock(buf); 22 | if (len > 0) { 23 | byte[] data = new byte[len]; 24 | System.arraycopy(buf,0, data, 0, len); 25 | logger.log(new ChannelDataChunk(data, System.currentTimeMillis())); 26 | } 27 | return len; 28 | } 29 | 30 | @Override 31 | public InputStream getDataStream() { 32 | return new LogStreamWrapper(reader.getDataStream(), logger); 33 | } 34 | 35 | @Override 36 | public boolean isStreamSupported() { 37 | return reader.isStreamSupported(); 38 | } 39 | 40 | @Override 41 | public void reconnect() throws IOException { 42 | reader.reconnect(); 43 | } 44 | 45 | @Override 46 | public void finish() { 47 | reader.finish(); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/logger/ChannelDataChunk.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.logger; 2 | 3 | import java.io.DataInputStream; 4 | import java.io.DataOutputStream; 5 | import java.io.IOException; 6 | 7 | public class ChannelDataChunk { 8 | 9 | byte[] data; 10 | long timemark; 11 | 12 | public ChannelDataChunk(byte[] data, long timemark) { 13 | super(); 14 | this.data = data; 15 | this.timemark = timemark; 16 | } 17 | 18 | public byte[] getData() { 19 | return data; 20 | } 21 | public long getIoDelay() { 22 | return timemark; 23 | } 24 | 25 | public void writeToStream(DataOutputStream out) throws IOException { 26 | out.writeLong(timemark); 27 | out.writeInt(data.length); 28 | out.write(data, 0, data.length); 29 | } 30 | 31 | public static ChannelDataChunk readFromStream(DataInputStream in) throws IOException { 32 | long delay = in.readLong(); 33 | byte[] dt = new byte[in.readInt()]; 34 | in.readFully(dt); 35 | 36 | return new ChannelDataChunk(dt, delay); 37 | } 38 | 39 | } 40 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/logger/DataLogger.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.logger; 2 | 3 | public interface DataLogger { 4 | 5 | void log(ChannelDataChunk data); 6 | 7 | void logStreamContent(int data); 8 | 9 | public void finish(); 10 | } 11 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/logger/LogStreamWrapper.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.logger; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | 6 | public class LogStreamWrapper extends InputStream { 7 | 8 | InputStream dataStream; 9 | DataLogger logger; 10 | 11 | public LogStreamWrapper(InputStream dataStream, DataLogger logger) { 12 | this.dataStream = dataStream; 13 | this.logger = logger; 14 | } 15 | 16 | @Override 17 | public int read() throws IOException { 18 | int data = dataStream.read(); 19 | logger.logStreamContent(data); 20 | return data; 21 | } 22 | 23 | } 24 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/ControlAlgorithm.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.navdata; 2 | 3 | public enum ControlAlgorithm 4 | { 5 | EULER_ANGELS_CONTROL, ANGULAR_SPEED_CONTROL 6 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/CtrlState.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.navdata; 2 | 3 | 4 | public enum CtrlState 5 | { 6 | DEFAULT, INIT, LANDED, FLYING, HOVERING, TEST, TRANS_TAKEOFF, TRANS_GOTOFIX, TRANS_LANDING; 7 | 8 | public static CtrlState fromInt(int v) throws NavDataFormatException 9 | { 10 | switch(v) 11 | { 12 | case 0: 13 | return DEFAULT; 14 | case 1: 15 | return INIT; 16 | case 2: 17 | return LANDED; 18 | case 3: 19 | return FLYING; 20 | case 4: 21 | return HOVERING; 22 | case 5: 23 | return TEST; 24 | case 6: 25 | return TRANS_TAKEOFF; 26 | case 7: 27 | return TRANS_GOTOFIX; 28 | case 8: 29 | return TRANS_LANDING; 30 | default: 31 | throw new NavDataFormatException("Invalid control state " + v); 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/FlyingState.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.navdata; 2 | 3 | public enum FlyingState 4 | { 5 | FLYING, TAKING_OFF, LANDING, LANDED; 6 | 7 | public static FlyingState fromControlState(CtrlState state) 8 | { 9 | switch(state) 10 | { 11 | case FLYING: 12 | case HOVERING: 13 | case TRANS_GOTOFIX: 14 | return FlyingState.FLYING; 15 | 16 | case TRANS_TAKEOFF: 17 | return FlyingState.TAKING_OFF; 18 | 19 | case TRANS_LANDING: 20 | return FlyingState.LANDING; 21 | 22 | default: 23 | return FlyingState.LANDED; 24 | } 25 | } 26 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/Mode.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.navdata; 2 | 3 | public enum Mode 4 | { 5 | BOOTSTRAP, DEMO 6 | } 7 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/NavDataFormatException.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.data.navdata; 3 | 4 | @SuppressWarnings("serial") 5 | public class NavDataFormatException extends Exception 6 | { 7 | 8 | public NavDataFormatException(String why) 9 | { 10 | super(why); 11 | } 12 | 13 | } 14 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/NavDataTag.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.navdata; 2 | 3 | public enum NavDataTag 4 | { 5 | NAVDATA_DEMO_TAG(0), NAVDATA_TIME_TAG(1), NAVDATA_RAW_MEASURES_TAG(2), NAVDATA_PHYS_MEASURES_TAG(3), NAVDATA_GYROS_OFFSETS_TAG( 6 | 4), NAVDATA_EULER_ANGLES_TAG(5), NAVDATA_REFERENCES_TAG(6), NAVDATA_TRIMS_TAG(7), NAVDATA_RC_REFERENCES_TAG( 7 | 8), NAVDATA_PWM_TAG(9), NAVDATA_ALTITUDE_TAG(10), NAVDATA_VISION_RAW_TAG(11), NAVDATA_VISION_OF_TAG(12), NAVDATA_VISION_TAG( 8 | 13), NAVDATA_VISION_PERF_TAG(14), NAVDATA_TRACKERS_SEND_TAG(15), NAVDATA_VISION_DETECT_TAG(16), NAVDATA_WATCHDOG_TAG( 9 | 17), NAVDATA_ADC_DATA_FRAME_TAG(18), NAVDATA_VIDEO_STREAM_TAG(19), NAVDATA_CKS_TAG(0xFFFF); 10 | 11 | private int value; 12 | 13 | private NavDataTag(int value) 14 | { 15 | this.value = value; 16 | } 17 | 18 | public int getValue() 19 | { 20 | return value; 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/vision/Dimension.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.data.navdata.vision; 3 | 4 | public class Dimension 5 | { 6 | private int width; 7 | private int height; 8 | 9 | public Dimension(int width, int height) 10 | { 11 | super(); 12 | this.width = width; 13 | this.height = height; 14 | } 15 | 16 | public int getWidth() 17 | { 18 | return width; 19 | } 20 | 21 | public int getHeight() 22 | { 23 | return height; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/vision/Point.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.data.navdata.vision; 3 | 4 | public class Point 5 | { 6 | private int x; 7 | private int y; 8 | 9 | public Point(int x, int y) 10 | { 11 | super(); 12 | this.x = x; 13 | this.y = y; 14 | } 15 | 16 | public int getX() 17 | { 18 | return x; 19 | } 20 | 21 | public int getY() 22 | { 23 | return y; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/navdata/vision/VisionTag.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.data.navdata.vision; 3 | 4 | import com.codeminders.ardrone.data.navdata.NavDataFormatException; 5 | 6 | public class VisionTag 7 | { 8 | 9 | public VisionTag(VisionTagType type, Point position, Dimension dimensions, int distance) 10 | { 11 | super(); 12 | this.type = type; 13 | this.position = position; 14 | this.dimensions = dimensions; 15 | this.distance = distance; 16 | } 17 | 18 | /** 19 | * @brief Values for the detection type on drone cameras. 20 | */ 21 | public static enum VisionTagType 22 | { 23 | CAD_TYPE_HORIZONTL(0), /* Deprecated */ 24 | CAD_TYPE_VERTICAL(1), /* Deprecated */ 25 | CAD_TYPE_VISION(2), /* Detection of 2D horizontal tags on drone shells */ 26 | CAD_TYPE_NONE(3), /* Detection disabled */ 27 | CAD_TYPE_COCARDE(4), /* Detects a roundel under the drone */ 28 | CAD_TYPE_ORIENTED_COCARDE(5), /* 29 | * Detects an oriented roundel under the 30 | * drone 31 | */ 32 | CAD_TYPE_STRIPE(6), /* Detects a uniform stripe on the ground */ 33 | CAD_TYPE_H_COCARDE(7), /* Detects a roundel in front of the drone */ 34 | CAD_TYPE_H_ORIENTED_COCARDE(8), /* 35 | * Detects an oriented roundel in front 36 | * of the drone 37 | */ 38 | CAD_TYPE_STRIPE_V(9), CAD_TYPE_MULTIPLE_DETECTION_MODE(10), /* 39 | * The drone 40 | * uses 41 | * several 42 | * detections 43 | * at the 44 | * same time 45 | */ 46 | CAD_TYPE_NUM(11); /* Number of possible values for CAD_TYPE */ 47 | 48 | private int value; 49 | 50 | private VisionTagType(int value) 51 | { 52 | this.value = value; 53 | } 54 | 55 | public int getValue() 56 | { 57 | return value; 58 | } 59 | 60 | public static VisionTagType fromInt(int v) throws NavDataFormatException 61 | { 62 | switch(v) 63 | { 64 | case 0: 65 | return VisionTagType.CAD_TYPE_HORIZONTL; 66 | case 1: 67 | return VisionTagType.CAD_TYPE_VERTICAL; 68 | case 2: 69 | return VisionTagType.CAD_TYPE_VISION; 70 | case 3: 71 | return VisionTagType.CAD_TYPE_NONE; 72 | case 4: 73 | return VisionTagType.CAD_TYPE_COCARDE; 74 | case 5: 75 | return VisionTagType.CAD_TYPE_ORIENTED_COCARDE; 76 | case 6: 77 | return VisionTagType.CAD_TYPE_STRIPE; 78 | case 7: 79 | return VisionTagType.CAD_TYPE_H_COCARDE; 80 | case 8: 81 | return VisionTagType.CAD_TYPE_H_ORIENTED_COCARDE; 82 | case 9: 83 | return VisionTagType.CAD_TYPE_STRIPE_V; 84 | case 10: 85 | return VisionTagType.CAD_TYPE_MULTIPLE_DETECTION_MODE; 86 | case 11: 87 | return VisionTagType.CAD_TYPE_NUM; 88 | default: 89 | throw new NavDataFormatException("Invalid vision tag type " + v); 90 | 91 | } 92 | } 93 | }; 94 | 95 | // Type of the detected tag #i ; see the CAD_TYPE enumeration. 96 | private VisionTagType type; 97 | 98 | /** 99 | * X and Y coordinates of detected 2D-tag #i inside the picture, with (0, 0) 100 | * being the top-left corner, and (1000, 1000) the right-bottom corner 101 | * regardless the picture resolu- tion or the source camera. 102 | */ 103 | private Point position; 104 | 105 | /** 106 | * Width and height of the detection bounding-box (2D-tag #i), when 107 | * applicable. 108 | */ 109 | private Dimension dimensions; 110 | 111 | /** 112 | * Distance from camera to detected 2D-tag #i in centimeters, when 113 | * applicable. 114 | */ 115 | private int distance; 116 | 117 | public VisionTagType getType() 118 | { 119 | return type; 120 | } 121 | 122 | public Point getPosition() 123 | { 124 | return position; 125 | } 126 | 127 | public Dimension getDimensions() 128 | { 129 | return dimensions; 130 | } 131 | 132 | public int getDistance() 133 | { 134 | return distance; 135 | } 136 | 137 | @Override 138 | public String toString() 139 | { 140 | StringBuilder builder = new StringBuilder(); 141 | builder.append("VisionTag [type="); 142 | builder.append(type); 143 | builder.append(", position="); 144 | builder.append(position); 145 | builder.append(", dimensions="); 146 | builder.append(dimensions); 147 | builder.append(", distance="); 148 | builder.append(distance); 149 | builder.append("]"); 150 | return builder.toString(); 151 | } 152 | } 153 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/reader/LigthUDPDataReader.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.reader; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | import java.net.DatagramPacket; 6 | import java.net.DatagramSocket; 7 | import java.net.InetAddress; 8 | 9 | import com.codeminders.ardrone.data.ARDroneDataReader; 10 | 11 | public class LigthUDPDataReader implements ARDroneDataReader { 12 | 13 | private int timeout; 14 | 15 | private int data_port; 16 | 17 | static final byte[] TRIGGER_BYTES = { 0x01, 0x00, 0x00, 0x00 }; 18 | 19 | protected DatagramSocket socket; 20 | private DatagramPacket trigger_packet; 21 | 22 | private InetAddress drone_addr; 23 | 24 | public LigthUDPDataReader(InetAddress drone_addr, int data_port, int timeout) throws IOException { 25 | 26 | super(); 27 | this.data_port = data_port; 28 | this.timeout = timeout; 29 | this.drone_addr = drone_addr; 30 | 31 | trigger_packet = new DatagramPacket(TRIGGER_BYTES, TRIGGER_BYTES.length, drone_addr, data_port); 32 | 33 | connect(); 34 | } 35 | 36 | public InputStream getDataStream() { 37 | return null; 38 | } 39 | 40 | public void connect() throws IOException { 41 | disconnect(); 42 | socket = new DatagramSocket(); //data_port); 43 | socket.setSoTimeout(timeout); 44 | } 45 | 46 | private void disconnect() { 47 | 48 | if (null != socket && socket.isConnected()) { 49 | socket.disconnect(); 50 | } 51 | 52 | if (null != socket && !socket.isClosed()) { 53 | socket.close(); 54 | } 55 | 56 | } 57 | 58 | public int readDataBlock(byte[] buf) throws IOException { 59 | //send trigger data 60 | socket.send(trigger_packet); 61 | //receive data 62 | DatagramPacket packet = new DatagramPacket(buf, buf.length, drone_addr, data_port); 63 | socket.receive(packet); 64 | 65 | return packet.getLength(); 66 | } 67 | 68 | public synchronized void finish() 69 | { 70 | disconnect(); 71 | } 72 | 73 | @Override 74 | public boolean isStreamSupported() { 75 | return false; 76 | } 77 | 78 | @Override 79 | public void reconnect() throws IOException { 80 | disconnect(); 81 | connect(); 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/reader/TCPDataRader.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.reader; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | import java.net.InetAddress; 6 | import java.net.Socket; 7 | import java.util.logging.Level; 8 | import java.util.logging.Logger; 9 | 10 | import com.codeminders.ardrone.data.ARDroneDataReader; 11 | 12 | 13 | public class TCPDataRader implements ARDroneDataReader { 14 | 15 | Logger log = Logger.getLogger(this.getClass().getName()); 16 | 17 | private InetAddress drone_addr; 18 | private int data_port; 19 | private int timeout; 20 | 21 | private Socket socket; 22 | 23 | private InputStream socketInput; 24 | 25 | public TCPDataRader(InetAddress drone_addr, int data_port, int timeout) throws IOException { 26 | super(); 27 | 28 | this.drone_addr = drone_addr; 29 | this.data_port = data_port; 30 | this.timeout = timeout; 31 | 32 | connect(); 33 | } 34 | 35 | private void connect() throws IOException { 36 | 37 | socket = new Socket(drone_addr, data_port); 38 | socket.setSoTimeout(timeout); 39 | socketInput = socket.getInputStream(); 40 | } 41 | 42 | @Override 43 | public InputStream getDataStream() { 44 | return socketInput; 45 | } 46 | 47 | @Override 48 | public void finish() { 49 | disconnect(); 50 | } 51 | 52 | private void disconnect() { 53 | if (socket.isClosed()) { 54 | try { 55 | socket.close(); 56 | } catch (IOException e) { 57 | log.log(Level.FINER, "Excepton on stopping TCP reading", e); 58 | } 59 | } 60 | } 61 | 62 | @Override 63 | public int readDataBlock(byte[] buf) throws IOException { 64 | return socketInput.read(buf); 65 | } 66 | 67 | @Override 68 | public boolean isStreamSupported() { 69 | return true; 70 | } 71 | 72 | @Override 73 | public void reconnect() throws IOException { 74 | disconnect(); 75 | connect(); 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/data/reader/UDPDataReader.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.data.reader; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | import java.net.InetAddress; 6 | import java.net.InetSocketAddress; 7 | import java.nio.ByteBuffer; 8 | import java.nio.channels.ClosedChannelException; 9 | import java.nio.channels.DatagramChannel; 10 | import java.nio.channels.SelectionKey; 11 | import java.nio.channels.Selector; 12 | import java.util.Iterator; 13 | import java.util.Set; 14 | 15 | import com.codeminders.ardrone.data.ARDroneDataReader; 16 | 17 | public class UDPDataReader implements ARDroneDataReader { 18 | 19 | private int timeout; 20 | 21 | protected DatagramChannel channel; 22 | protected Selector selector; 23 | 24 | private InetAddress drone_addr; 25 | private int data_port; 26 | 27 | private int buffer_size; 28 | 29 | static final byte[] TRIGGER_BYTES = { 0x01, 0x00, 0x00, 0x00 }; 30 | 31 | ByteBuffer trigger_buffer = ByteBuffer.allocate(TRIGGER_BYTES.length); 32 | ByteBuffer inbuf = ByteBuffer.allocate(buffer_size); 33 | 34 | public UDPDataReader(InetAddress drone_addr, int data_port, int timeout) throws ClosedChannelException, IOException { 35 | super(); 36 | this.drone_addr = drone_addr; 37 | this.data_port = data_port; 38 | 39 | this.timeout = timeout; 40 | 41 | trigger_buffer.put(TRIGGER_BYTES); 42 | trigger_buffer.flip(); 43 | 44 | connect(); 45 | } 46 | 47 | private void connect() throws IOException, ClosedChannelException { 48 | 49 | channel = DatagramChannel.open(); 50 | channel.configureBlocking(false); 51 | channel.socket().bind(new InetSocketAddress(data_port)); 52 | channel.connect(new InetSocketAddress(drone_addr, data_port)); 53 | 54 | selector = Selector.open(); 55 | channel.register(selector, SelectionKey.OP_READ | SelectionKey.OP_WRITE); 56 | } 57 | 58 | private void disconnect() { 59 | try { 60 | if (selector.isOpen()) 61 | selector.close(); 62 | } catch (IOException iox) 63 | { 64 | // ignore 65 | } 66 | 67 | if (!channel.socket().isClosed()) { 68 | channel.socket().close(); 69 | } 70 | 71 | try { 72 | if (channel.isConnected()) 73 | channel.disconnect(); 74 | } catch (IOException iox) 75 | { 76 | // ignore 77 | } 78 | 79 | try { 80 | channel.close(); 81 | } catch (IOException iox) { 82 | // ignore 83 | } 84 | 85 | } 86 | 87 | @Override 88 | public int readDataBlock(byte[] buf) throws IOException 89 | { 90 | int len = 0; 91 | selector.select(timeout); 92 | Set readyKeys = selector.selectedKeys(); 93 | Iterator iterator = readyKeys.iterator(); 94 | 95 | while (iterator.hasNext()) { 96 | SelectionKey key = (SelectionKey) iterator.next(); 97 | iterator.remove(); 98 | 99 | if (key.isWritable()) { 100 | channel.write(trigger_buffer); 101 | channel.register(selector, SelectionKey.OP_READ); 102 | // prepare buffer for new reconnection attempt 103 | trigger_buffer.clear(); 104 | trigger_buffer.put(TRIGGER_BYTES); 105 | trigger_buffer.flip(); 106 | } else if (key.isReadable()) { 107 | return channel.read(ByteBuffer.wrap(buf)); 108 | } 109 | } 110 | 111 | return len; 112 | } 113 | 114 | public synchronized void finish() 115 | { 116 | if (null != selector) { 117 | selector.wakeup(); 118 | } 119 | } 120 | 121 | @Override 122 | public InputStream getDataStream() { 123 | return null; 124 | } 125 | 126 | @Override 127 | public boolean isStreamSupported() { 128 | return false; 129 | } 130 | 131 | @Override 132 | public void reconnect() throws IOException { 133 | disconnect(); 134 | connect(); 135 | } 136 | } -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/examples/TakeOffAndLand.java: -------------------------------------------------------------------------------- 1 | 2 | package com.codeminders.ardrone.examples; 3 | 4 | import com.codeminders.ardrone.ARDrone; 5 | 6 | public class TakeOffAndLand 7 | { 8 | 9 | private static final long CONNECT_TIMEOUT = 3000; 10 | 11 | /** 12 | * @param args 13 | */ 14 | public static void main(String[] args) 15 | { 16 | ARDrone drone; 17 | try 18 | { 19 | // Create ARDrone object, 20 | // connect to drone and initialize it. 21 | drone = new ARDrone(); 22 | drone.connect(); 23 | drone.clearEmergencySignal(); 24 | 25 | // Wait until drone is ready 26 | drone.waitForReady(CONNECT_TIMEOUT); 27 | 28 | // do TRIM operation 29 | drone.trim(); 30 | 31 | // Take off 32 | System.err.println("Taking off"); 33 | drone.takeOff(); 34 | 35 | // Fly a little :) 36 | Thread.sleep(5000); 37 | 38 | // Land 39 | System.err.println("Landing"); 40 | drone.land(); 41 | 42 | // Give it some time to land 43 | Thread.sleep(2000); 44 | 45 | // Disconnect from the done 46 | drone.disconnect(); 47 | 48 | } catch(Throwable e) 49 | { 50 | e.printStackTrace(); 51 | } 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/version/DroneVersionReader.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.version; 2 | 3 | import java.io.IOException; 4 | 5 | public interface DroneVersionReader { 6 | 7 | String readDroneVersion() throws IOException; 8 | 9 | } 10 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/java/com/codeminders/ardrone/version/ftp/DroneFTPversionReader.java: -------------------------------------------------------------------------------- 1 | package com.codeminders.ardrone.version.ftp; 2 | 3 | import java.io.ByteArrayOutputStream; 4 | import java.io.IOException; 5 | import java.io.InputStream; 6 | import java.net.InetAddress; 7 | import java.net.URL; 8 | import java.net.URLConnection; 9 | import java.util.logging.Logger; 10 | 11 | import com.codeminders.ardrone.version.DroneVersionReader; 12 | 13 | public class DroneFTPversionReader implements DroneVersionReader { 14 | 15 | private Logger log = Logger.getLogger(getClass().getName()); 16 | 17 | private static final int FTP_PORT = 5551; 18 | private static final String VERSION_FILE_NAME = "version.txt"; 19 | 20 | String ftpVersionFileLocation; 21 | 22 | public DroneFTPversionReader(InetAddress drone_addr) { 23 | this.ftpVersionFileLocation = "ftp://"+drone_addr.getHostAddress() + ":" + FTP_PORT + "/" + VERSION_FILE_NAME; 24 | } 25 | 26 | @Override 27 | public String readDroneVersion() throws IOException { 28 | 29 | InputStream is = null; 30 | ByteArrayOutputStream bos = null; 31 | try { 32 | // log.info("Attempting to read AR Drone version using FTP. Version file is: "+ ftpVersionFileLocation); 33 | URL url = new URL(ftpVersionFileLocation); 34 | URLConnection ftpConnection = url.openConnection(); 35 | ftpConnection.setConnectTimeout(1000*5); 36 | // log.info(ftpVersionFileLocation + "- Connection Opened"); 37 | 38 | is = ftpConnection.getInputStream(); 39 | bos = new ByteArrayOutputStream(); 40 | 41 | byte[] buffer = new byte[1024]; 42 | int readCount; 43 | 44 | while((readCount = is.read(buffer)) > 0) 45 | { 46 | bos.write(buffer, 0, readCount); 47 | } 48 | 49 | return bos.toString(); 50 | } finally { 51 | if (null != bos) { 52 | bos.close(); 53 | } 54 | if (null != is) { 55 | is.close (); 56 | } 57 | } 58 | } 59 | 60 | } 61 | -------------------------------------------------------------------------------- /platforms/ardrone/src/main/scala/ARDroneWithMixins.scala: -------------------------------------------------------------------------------- 1 | 2 | 3 | // package org.opendronecontrol 4 | // package platforms.ardrone 5 | 6 | // import net.OSCInterface 7 | // import net.OSCInterfaceTrack 8 | // import tracking.PositionController 9 | 10 | // class ARDroneWithOSC extends ARDrone("192.168.1.1") with OSCInterface 11 | // class ARDroneWithTracking extends ARDrone("192.168.1.1") with PositionController 12 | // class ARDronePlus extends ARDrone("192.168.1.1") with PositionController with OSCInterfaceTrack -------------------------------------------------------------------------------- /project/plugins.sbt: -------------------------------------------------------------------------------- 1 | 2 | //addSbtPlugin("com.github.retronym" % "sbt-onejar" % "0.8") 3 | 4 | addSbtPlugin("com.typesafe.sbt" % "sbt-proguard" % "0.2.2") 5 | 6 | //resolvers += Resolver.url("sbt-plugin-releases-scalasbt", url("http://repo.scala-sbt.org/scalasbt/sbt-plugin-releases/"))(Resolver.ivyStylePatterns) 7 | 8 | //addSbtPlugin("org.scala-sbt" % "xsbt-proguard-plugin" % "0.1.3") 9 | 10 | addSbtPlugin("com.eed3si9n" % "sbt-unidoc" % "0.2.0") -------------------------------------------------------------------------------- /sbt: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if [ -e "`dirname $0`/sbt-launch.jar" ] 3 | then 4 | echo "" 5 | else 6 | echo "Downloading sbt-launch.jar..." 7 | wget "http://repo.typesafe.com/typesafe/ivy-releases/org.scala-sbt/sbt-launch/0.13.0-RC4/sbt-launch.jar" > /dev/null 2>&1 8 | if [ $? != 0 ] 9 | then 10 | curl -O "http://repo.typesafe.com/typesafe/ivy-releases/org.scala-sbt/sbt-launch/0.13.0-RC4/sbt-launch.jar" 11 | if [ $? != 0 ] 12 | then 13 | echo "Failed to get sbt-launch.jar, please install wget or curl" 14 | fi 15 | fi 16 | java -Xms512M -Xmx1536M -Xss1M -XX:+CMSClassUnloadingEnabled -XX:MaxPermSize=384M -jar `dirname $0`/sbt-launch.jar "project odc" download-libs 17 | fi 18 | 19 | java -Xms512M -Xmx1536M -Xss1M -XX:+CMSClassUnloadingEnabled -XX:MaxPermSize=384M -jar `dirname $0`/sbt-launch.jar "$@" 20 | --------------------------------------------------------------------------------