A Clojure Autopilot for Parrot A.R. Drone

Another round of code dump, I've been playing with an A.R. Drone,
following is a simple autopilot implementation which uses an overhead
camera for tracking and guidance.

The way this whole setup works is, I've glued two circles on top of
the drone, an overhead camera tracks these two circles. Big blue
circle which sits at the center of the drone gives its location,
combined with the pink/reddish circle that sits towards the front of
the drone, taking the angle between the centers of the circles gives
its orientation.

Once you know the position and orientation of the drone, all it takes
is simple vector math to get it follow a pre-defined path.

Calculations assume origin is at the center of the frame, +x going
towards the right of the frame and +y going up the frame. OpenCV
assumes origin is at the top left corner. Above functions converts
cam coordinates to world coordinates and vice versa.

locate-circle takes the frame from the camera, a copy of the frame
converted to HSV color space and lower/upper limits (Hue
Saturation Value) for the color we are trying to isolate. We isolate
the color range using in-range-s, it returns a binary
image in which pixels that fall with in the range are white rest is
black. eroding and then dilating the image removes noise (specks of
white from the white tape on the floor.) Finally we find the
contours of the match using bounding-rects calculate its center
and return it or nil if there is no match.

To fly to a certain point on the map, we begin by drawing a unit
vector to the target (to-target) as long as drone is pointing
towards 0 degrees (direction of +x) we can use the vectors' x
component as pitch and y component as roll but since this is a
quadrotor which can move in any direction without turning towards the
target, we rotate to-target using the drones current orientation
that allows us to map x , y to picth , roll whatever the
drones orientation. Finally we scale local-target using a PID
controller so it slows down as it approaches the target.

If you are familiar with Craig Reynolds steering behaviors this is
basically the arrive behavior with a caveat, what I don't track is the
drones' speed, even though local-target gets smaller as it
approaches the target it does not apply any breaking so if it is
travelling from one end of the map to the other it will fly past the
target and come back to it.

To take off, we reset the communication watch dog, tell drone to start sending
telemetry (battery level, altitude, yaw etc.) back, send a trim command to
let it calibrate itself then a takeoff command to take off.

To land, we send a hover command which levels the drone and send land
command bunch of times (since communication is done over UDP sending
it once works 90% of the time) and stop the thread that listens for
telemetry data.

navigation take a list of waypoint accuracy pairs and puts all of
the above together. It will return a sequence that will take off, wait
until the drone is half a meter in the air, start flying towards a
waypoint until it is within given accuracy/distance and move on to the
next one. Finally when it goes through all the waypoints it will
land.