<html lang="en" xml:lang="en" xmlns="http://www.w3.org/1999/xhtml"><head><meta content="text/html; charset=UTF-8" http-equiv="content-type" /><meta name="description" /><meta content="clojure flightgear" name="keywords" /><meta content="Nurullah Akkaya" name="author" /><link href="/images/favicon.ico" rel="icon" type="image/x-icon" /><link href="/images/favicon.ico" rel="shortcut icon" type="image/x-icon" /><link href="/default.css" rel="stylesheet" type="text/css" /><link href="/rss-feed" rel="alternate" title="An explorer&apos;s log" type="application/rss+xml" /><link href="http://nakkaya.com/2010/10/18/towards-a-clojure-autopilot-guidance/" rel="canonical" /><script src="http://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML" type="text/javascript"></script><title>Towards a Clojure Autopilot - Guidance</title></head><body><div id="wrap"><div id="header"><h1><a href="/">nakkaya<span class="fade-small">dot</span><span class="fade">com</span></a></h1><div class="pages"><a class="page" href="/">Home</a> | <a class="page" href="/projects.html">Projects</a> | <a class="page" href="/archives.html">Archives</a> | <a class="page" href="/tags/">Tags</a> | <a class="page" href="/contact.html" rel="author">About</a><form action="http://www.google.com/search" id="searchform" method="get"><div><input class="box" id="s" name="q" type="text" /><input name="sitesearch" type="hidden" value="nakkaya.com" /></div></form></div></div><div id="content"><div id="post"><h2 class="page-title">Towards a Clojure Autopilot - Guidance</h2><p>
Now that we have covered the <a href="http://nakkaya.com/2010/10/07/towards-a-clojure-autopilot-first-steps/">basics</a> , we can move on to the fun stuff,
getting from <i>A</i> to <i>B</i> by following a set of GPS coordinates. To
actually fly from one way point to another we need two sets of
controllers, the ones we defined earlier (roll and pitch hold) will
act as our bottom layer on top of that we are going to define another
set of controllers (altitude and heading hold) that will modify set
points for the bottom layer, i.e altitude hold controls pitch hold to
climb or decent to the desired altitude and heading hold controls roll
hold to turn the aircraft to the desired heading.
</p>

<img src="/images/post/clojure-autopilot.png" />
<p />

<p>
Even though a simple proportional controller worked for the previous
example it won't work for the real thing, the reason being that a
proportional controller only accounts for the current error so a
plane flying against the wind can't accelerate using a proportional
controller, it will eventually get stuck at a speed lower than the
desired speed. This is where <a href="http://en.wikipedia.org/wiki/PID_controller">PID controller</a> comes into
play, PID stands for Proportional, Integral, Derivative. In a nutshell,
proportional term just like before calculates a correction value
depending on how far we are away from the desired set point, integral
term acts as memory it will respond to accumulated error over time, as
accumulated error over time grows integral term will grow which in turn
will make our correction grow so even if the wind pushes us back
integral term will compensate for it and apply more throttle over time
just like a human would, derivative term tries to predict the future by
calculating a response to the rate of change of the error basically it
makes the controller anticipate approaching the set point. 
</p>

\begin{equation}
  correction = K_p \times error + K_d \times (error - prevError) + K_i \times (sumOfErrors)
\end{equation}

<p>
PID formula won't change from application to application, what will
change however are those constants (K<sub>p</sub>, K<sub>d</sub> and K<sub>i</sub>). i.e. the more you
decrease your K<sub>i</sub> constant the less memory your controller will
have. Despite the simple looking formula there is an entire theory
behind PID so I would suggest googling it for more information on theory
and tuning.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">ns</span> <span style="font-weight: bold; text-decoration: underline;">autopilot.core</span>
  (<span style="font-weight: bold; text-decoration: underline;">:use</span> clojure.contrib.prxml)
  (<span style="font-weight: bold; text-decoration: underline;">:use</span> clojure.contrib.swing-utils)
  (<span style="font-weight: bold; text-decoration: underline;">:import</span> (javax.swing JFrame JButton)
           (java.net InetAddress DatagramSocket DatagramPacket)))

(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">fg-host</span> (<span style="font-weight: bold; text-decoration: underline;">InetAddress</span>/getByName <span style="color: #afafff; font-style: italic;">"127.0.0.1"</span>))
(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">fg-port-out</span> 6666)
(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">fg-port-in</span> 6789)

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">in-thread</span> [f] (<span style="color: #ff5f00; font-weight: bold;">doto</span> (Thread. f) (.start)))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">scale</span> [x in-min in-max out-min out-max]
  (+ (/ (* (- x in-min) (- out-max out-min)) (- in-max in-min)) out-min))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">clamp</span> [x min max]
  (<span style="color: #ff5f00; font-weight: bold;">cond</span>
   (&gt; x max) max
   (&lt; x min) min
   <span style="font-weight: bold; text-decoration: underline;">:default</span> x))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">pid</span>
  ([s]
     (ref (assoc s <span style="font-weight: bold; text-decoration: underline;">:integrator</span> 0 <span style="font-weight: bold; text-decoration: underline;">:derivator</span> 0)))
  ([s v]
     (<span style="color: #ff5f00; font-weight: bold;">let</span> [{<span style="font-weight: bold; text-decoration: underline;">:keys</span> [set-point kp kd ki integrator derivator bounds]} @s
           [in-min in-max out-min out-max] bounds
           v (scale (clamp v in-min in-max) in-min in-max -1.0 1.0)
           sp (scale (clamp set-point in-min in-max) in-min in-max -1.0 1.0)
           error (- sp v)
           p-val (* kp error)
           d-val (* kd (- error derivator))
           integrator (clamp (+ integrator error) -1.0 1.0)
           i-val (* integrator ki)
           pid (scale (clamp (+ p-val i-val d-val) -1.0 1.0)
                      -1.0 1.0 out-min out-max)]
       (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (alter s assoc <span style="font-weight: bold; text-decoration: underline;">:integrator</span> integrator <span style="font-weight: bold; text-decoration: underline;">:derivator</span> error))
       pid)))
</pre>
</div>

<p>
First we make sure both set point and current value are within the input
range, then we map them to a range between -1/+1 so they are a
percentage between -100% and 100% of the scale, then we move on with our
calculation. The only part that needs special care is the integral part,
we need to take care of a problem called integral windup, again think
speed, going from 0 to 80 km/h is going to take a long time by the time
you reach 80 km/h your integral will be huge and it will make you
overshoot the target speed maybe accelerate to 100 km/h then decrease
and settle on 80 km/h thats why the integral is kept in range
-1/+1. Finally we sum all the terms and scale them to output range.
</p>

<p>
Moving on to controllers, we have 5 of them,
</p>

<ul class="org-ul">
<li>Speed Hold
</li>
<li>Heading Hold -&gt; Roll Hold
</li>
<li>Altitude Hold -&gt; Pitch Hold
</li>
</ul>

<p>
They all share the same structure a hold function that calculates the
correction and a set! function for modifying the set point.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">let</span> [p (pid {<span style="font-weight: bold; text-decoration: underline;">:kp</span> 4
              <span style="font-weight: bold; text-decoration: underline;">:ki</span> 0
              <span style="font-weight: bold; text-decoration: underline;">:kd</span> 0
              <span style="font-weight: bold; text-decoration: underline;">:set-point</span> 80
              <span style="font-weight: bold; text-decoration: underline;">:bounds</span> [0 120 0 1]})]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">speed-hold</span> [curr]
    (double (pid p curr))))

(<span style="color: #ff5f00; font-weight: bold;">let</span> [p (pid {<span style="font-weight: bold; text-decoration: underline;">:kp</span> 2
              <span style="font-weight: bold; text-decoration: underline;">:ki</span> 1/10
              <span style="font-weight: bold; text-decoration: underline;">:kd</span> 0
              <span style="font-weight: bold; text-decoration: underline;">:set-point</span> 0
              <span style="font-weight: bold; text-decoration: underline;">:bounds</span> [-180 180 -1 1]})]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">set!-roll</span> [ang]
    (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (alter p assoc <span style="font-weight: bold; text-decoration: underline;">:set-point</span> ang)))
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">roll-hold</span> [curr]
    (double (pid p curr))))

(<span style="color: #ff5f00; font-weight: bold;">let</span> [p (pid {<span style="font-weight: bold; text-decoration: underline;">:kp</span> 4
              <span style="font-weight: bold; text-decoration: underline;">:ki</span> 1/5
              <span style="font-weight: bold; text-decoration: underline;">:kd</span> 0
              <span style="font-weight: bold; text-decoration: underline;">:set-point</span> 0
              <span style="font-weight: bold; text-decoration: underline;">:bounds</span> [-90 90 -1 1]})]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">set!-pitch</span> [ang]
    (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (alter p assoc <span style="font-weight: bold; text-decoration: underline;">:set-point</span> ang)))
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">pitch-hold</span> [curr]
    (* (double (pid p curr)) -1)))

(<span style="color: #ff5f00; font-weight: bold;">let</span> [p (pid {<span style="font-weight: bold; text-decoration: underline;">:kp</span> 15
              <span style="font-weight: bold; text-decoration: underline;">:ki</span> 1/2
              <span style="font-weight: bold; text-decoration: underline;">:kd</span> 0.0
              <span style="font-weight: bold; text-decoration: underline;">:set-point</span> 1000
              <span style="font-weight: bold; text-decoration: underline;">:bounds</span> [0 10000 -10 10]})]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">set!-altitude</span> [alt]
    (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (alter p assoc <span style="font-weight: bold; text-decoration: underline;">:set-point</span> alt)))
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">altitude-hold</span> [curr]
    (set!-pitch (int (pid p curr)))))

(<span style="color: #ff5f00; font-weight: bold;">let</span> [p (pid {<span style="font-weight: bold; text-decoration: underline;">:kp</span> 2
              <span style="font-weight: bold; text-decoration: underline;">:ki</span> 0.0
              <span style="font-weight: bold; text-decoration: underline;">:kd</span> 0.0
              <span style="font-weight: bold; text-decoration: underline;">:set-point</span> 90
              <span style="font-weight: bold; text-decoration: underline;">:bounds</span> [0 180 -10 10]})
      norm-ang #(<span style="color: #ff5f00; font-weight: bold;">if</span> (<span style="color: #ff5f00; font-weight: bold;">and</span> (&gt;= <span style="font-weight: bold; font-style: italic;">%</span> 180)
                         (&lt;= <span style="font-weight: bold; font-style: italic;">%</span> 360))
                  (clamp (scale <span style="font-weight: bold; font-style: italic;">%</span> 270 360 0 90) 0 90)
                  (clamp (scale <span style="font-weight: bold; font-style: italic;">%</span> 0 90 90 180) 90 180))]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">set!-heading</span> [h]
    (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (alter p assoc <span style="font-weight: bold; text-decoration: underline;">:set-point</span> (norm-ang h))))
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">heading-hold</span> [curr]
    (set!-roll (int (pid p (norm-ang curr))))))
</pre>
</div>

<p>
Altitude hold takes the current altitude and calculates a pitch angle
for the aircraft that is between -10 and 10 degrees then it changes the
set point of the pitch hold to that angle that causes the plane to
climb, hold or decent to that altitude.
</p>

<p>
For navigation, we need to know two things course and distance to target,
following two functions are from <a href="http://williams.best.vwh.net/avform.htm">Aviation Formulary</a>,
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">bearing</span> [c1 c2]
  (<span style="color: #ff5f00; font-weight: bold;">let</span> [[lat1 lon1] (map #(<span style="font-weight: bold; text-decoration: underline;">Math</span>/toRadians <span style="font-weight: bold; font-style: italic;">%</span>) c1)
        [lat2 lon2] (map #(<span style="font-weight: bold; text-decoration: underline;">Math</span>/toRadians <span style="font-weight: bold; font-style: italic;">%</span>) c2)]
    (<span style="font-weight: bold; text-decoration: underline;">Math</span>/toDegrees
     (mod (<span style="font-weight: bold; text-decoration: underline;">Math</span>/atan2
           (* (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sin (- lon2 lon1)) (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos lat2))
           (- (* (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos lat1)
                 (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sin lat2))
              (* (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sin lat1) 
                 (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos lat2)
                 (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos (- lon2 lon1)))))
          (* 2 <span style="font-weight: bold; text-decoration: underline;">Math</span>/PI)))))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">distance</span> [c1 c2]
  (<span style="color: #ff5f00; font-weight: bold;">let</span> [[lat1 lon1] (map #(<span style="font-weight: bold; text-decoration: underline;">Math</span>/toRadians <span style="font-weight: bold; font-style: italic;">%</span>) c1)
        [lat2 lon2] (map #(<span style="font-weight: bold; text-decoration: underline;">Math</span>/toRadians <span style="font-weight: bold; font-style: italic;">%</span>) c2)]
    (* 2 6371.0
       (<span style="font-weight: bold; text-decoration: underline;">Math</span>/asin
        (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sqrt
         (+ (<span style="font-weight: bold; text-decoration: underline;">Math</span>/pow (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sin (/ (- lat1 lat2) 2)) 2)
            (* (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos lat1)
               (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos lat2)
               (<span style="font-weight: bold; text-decoration: underline;">Math</span>/pow (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sin (/ (- lon1 lon2) 2)) 2))))))))
</pre>
</div>

<p>
bearing returns the heading we need to take to reach c2 from c1,
distance returns the distance between points in kilometers.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">let</span> [route (ref [[38.702803 33.454353]
                  [38.756064 33.209744]
                  [38.908678 33.296394]])]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">dist-to-wp</span> [lat long]
    (distance [lat long] (first @route)))
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">guidance</span> [heading alt lat long]
    (<span style="color: #ff5f00; font-weight: bold;">when</span> (&gt; alt 400)
      (<span style="color: #ff5f00; font-weight: bold;">let</span> [wp (first @route)
            dist (distance [lat long] wp)]
        (set!-heading (bearing [lat long] wp))
        (heading-hold heading)
        (<span style="color: #ff5f00; font-weight: bold;">if</span> (<span style="color: #ff5f00; font-weight: bold;">and</span> (&lt; dist 1.0)
                 (not (empty? (rest @route))))
          (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (ref-set route (rest @route))))))))
</pre>
</div>

<p>
A simple KML writer for debugging,
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">let</span> [path (ref [])]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">add!-point</span> [long lat]
    (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (alter path conj [long lat])))
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">dump-log</span> []
    (spit <span style="color: #afafff; font-style: italic;">"path.kml"</span>
          (with-out-str
            (prxml
             [<span style="font-weight: bold; text-decoration: underline;">:decl!</span> {<span style="font-weight: bold; text-decoration: underline;">:version</span> <span style="color: #afafff; font-style: italic;">"1.0"</span>}]
             [<span style="font-weight: bold; text-decoration: underline;">:kml</span>  {<span style="font-weight: bold; text-decoration: underline;">:xmlns</span> <span style="color: #afafff; font-style: italic;">"http://www.opengis.net/kml/2.2"</span>}
              [<span style="font-weight: bold; text-decoration: underline;">:Document</span>
               [<span style="font-weight: bold; text-decoration: underline;">:name</span> <span style="color: #afafff; font-style: italic;">"Flight Path"</span>]
               [<span style="font-weight: bold; text-decoration: underline;">:Style</span> {<span style="font-weight: bold; text-decoration: underline;">:id</span> <span style="color: #afafff; font-style: italic;">"yellowLineGreenPoly"</span>}
                [<span style="font-weight: bold; text-decoration: underline;">:LineStyle</span>
                 [<span style="font-weight: bold; text-decoration: underline;">:color</span> <span style="color: #afafff; font-style: italic;">"7f00ffff"</span>]
                 [<span style="font-weight: bold; text-decoration: underline;">:width</span> 4]]]
               [<span style="font-weight: bold; text-decoration: underline;">:Placemark</span>
                [<span style="font-weight: bold; text-decoration: underline;">:name</span> <span style="color: #afafff; font-style: italic;">"WP-1"</span>]
                [<span style="font-weight: bold; text-decoration: underline;">:Point</span>
                 [<span style="font-weight: bold; text-decoration: underline;">:coordinates</span> <span style="color: #afafff; font-style: italic;">"33.454353,38.702803"</span>]]] <span style="color: #008787; font-weight: bold; font-style: italic;">;;</span><span style="color: #008787; font-weight: bold; font-style: italic;">long/lat</span>
               [<span style="font-weight: bold; text-decoration: underline;">:Placemark</span>
                [<span style="font-weight: bold; text-decoration: underline;">:name</span> <span style="color: #afafff; font-style: italic;">"WP-2"</span>]
                [<span style="font-weight: bold; text-decoration: underline;">:Point</span>
                 [<span style="font-weight: bold; text-decoration: underline;">:coordinates</span> <span style="color: #afafff; font-style: italic;">"33.209744,38.756064"</span>]]]
               [<span style="font-weight: bold; text-decoration: underline;">:Placemark</span>
                [<span style="font-weight: bold; text-decoration: underline;">:name</span> <span style="color: #afafff; font-style: italic;">"WP-3"</span>]
                [<span style="font-weight: bold; text-decoration: underline;">:Point</span>
                 [<span style="font-weight: bold; text-decoration: underline;">:coordinates</span> <span style="color: #afafff; font-style: italic;">"33.296394,38.908678"</span>]]]

               [<span style="font-weight: bold; text-decoration: underline;">:Placemark</span>
                [<span style="font-weight: bold; text-decoration: underline;">:name</span> <span style="color: #afafff; font-style: italic;">"Path"</span>]
                [<span style="font-weight: bold; text-decoration: underline;">:styleUrl</span> <span style="color: #afafff; font-style: italic;">"#yellowLineGreenPoly"</span>]
                [<span style="font-weight: bold; text-decoration: underline;">:LineString</span>
                 [<span style="font-weight: bold; text-decoration: underline;">:coordinates</span>
                  (map #(<span style="color: #ff5f00; font-weight: bold;">let</span> [[long lat] <span style="font-weight: bold; font-style: italic;">%</span>]
                          (str long <span style="color: #afafff; font-style: italic;">","</span> lat <span style="color: #afafff; font-style: italic;">", 0.</span><span style="color: #afafff; font-weight: bold; font-style: italic;">\n</span><span style="color: #afafff; font-style: italic;">"</span>)) @path)]]]]])))))
</pre>
</div>

<p>
Earth isn't flat so we can't calculate bearing once and be done with it,
bearing to way-point will change during the course of the flight so
every time guidance is called we calculate a new bearing to the
way-point, use it as the set point for the heading hold, heading hold
will in turn modify the set point for roll hold just like altitude
hold. Once we are within a kilometer of the way point we drop it and
move on to the next way-point.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">controller</span> [[roll pitch alt heading air-speed lat long]]
  (add!-point long lat)
  (altitude-hold alt)
  (guidance heading alt lat long)
  (<span style="color: #ff5f00; font-weight: bold;">let</span> [roll-cntrl (roll-hold roll)
        pitch-cntrl (pitch-hold pitch)
        speed-cntrl (speed-hold air-speed)]
    (println
     (format <span style="color: #afafff; font-style: italic;">"R %1$.2f %2$.2f"</span> roll roll-cntrl)
     (format <span style="color: #afafff; font-style: italic;">"P %1$.2f %2$.2f"</span> pitch pitch-cntrl)
     (format <span style="color: #afafff; font-style: italic;">"S %1$.2f %2$.2f"</span> air-speed speed-cntrl)
     (format <span style="color: #afafff; font-style: italic;">"A %1$.2f"</span> alt)
     (format <span style="color: #afafff; font-style: italic;">"H %1$.2f"</span> heading)
     (format <span style="color: #afafff; font-style: italic;">"D %1$.2f"</span> (dist-to-wp lat long)))
    [roll-cntrl pitch-cntrl speed-cntrl]))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">control-loop</span> [active fn-call]
  (<span style="color: #ff5f00; font-weight: bold;">let</span> [socket-in (DatagramSocket. fg-port-out)
        buffer-in (byte-array 2048)
        packet-in (DatagramPacket. buffer-in (count buffer-in))
        socket-out (DatagramSocket.)]
    (in-thread
     #(<span style="color: #ff5f00; font-weight: bold;">try</span>
        (while @active
          (.receive socket-in packet-in)
          (<span style="color: #ff5f00; font-weight: bold;">let</span> [state (read-string
                       (String. buffer-in 0 (dec (.getLength packet-in))))
                cntrl (fn-call state)
                msg (str (apply str (interpose <span style="color: #afafff; font-style: italic;">\,</span> cntrl)) <span style="color: #afafff; font-style: italic;">"</span><span style="color: #afafff; font-weight: bold; font-style: italic;">\n</span><span style="color: #afafff; font-style: italic;">"</span>)
                buf (.getBytes msg)
                packet (DatagramPacket. buf (count buf) fg-host fg-port-in)]
            (.setLength packet-in (count buffer-in))
            (.send socket-out packet)))
        (<span style="color: #ff5f00; font-weight: bold;">finally</span> (.close socket-in)
                 (.close socket-out))))))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">autopilot</span> []
  (<span style="color: #ff5f00; font-weight: bold;">let</span> [active (ref <span style="font-weight: bold; text-decoration: underline;">false</span>)
        button (JButton. <span style="color: #afafff; font-style: italic;">"Autopilot OFF"</span>)]
    (.setFont button (<span style="color: #ff5f00; font-weight: bold;">-&gt;</span> button .getFont (.deriveFont (float 40))))
    (add-action-listener
     button
     (<span style="color: #ff5f00; font-weight: bold;">fn</span> [_]
       (<span style="color: #ff5f00; font-weight: bold;">if</span> (= <span style="font-weight: bold; text-decoration: underline;">false</span> @active)
         (<span style="color: #ff5f00; font-weight: bold;">do</span> (.setText button <span style="color: #afafff; font-style: italic;">"Autopilot ON"</span>)
             (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (ref-set active <span style="font-weight: bold; text-decoration: underline;">true</span>))
             (control-loop active controller))
         (<span style="color: #ff5f00; font-weight: bold;">do</span> (.setText button <span style="color: #afafff; font-style: italic;">"Autopilot OFF"</span>)
             (<span style="color: #ff5f00; font-weight: bold;">dosync</span> (ref-set active <span style="font-weight: bold; text-decoration: underline;">false</span>))))))
    (<span style="color: #ff5f00; font-weight: bold;">doto</span> (JFrame.)
      (.add button)
      (.pack)
      (.setVisible <span style="font-weight: bold; text-decoration: underline;">true</span>))))
</pre>
</div>

<p>
Before we send a control packet back, we log our current position
that way after the flight we can take a look at the path taken in Google
Earth, then we let high level controllers calculate new set-points for
low level controllers and send calculated correction values from low
level controllers to FlightGear. 
</p>

<p>
input-protocol.xml,
</p>

<div class="org-src-container">

<pre class="src src-xml">&lt;?<span style="color: #ff5f00; font-weight: bold;">xml</span> <span style="font-weight: bold; font-style: italic;">version</span>=<span style="color: #afafff; font-style: italic;">"</span><span style="color: #afafff; font-style: italic;">1.0</span><span style="color: #afafff; font-style: italic;">"</span>?&gt;
&lt;<span style="color: #d7af00; font-weight: bold;">PropertyList</span>&gt;
  &lt;<span style="color: #d7af00; font-weight: bold;">generic</span>&gt;
    &lt;<span style="color: #d7af00; font-weight: bold;">input</span>&gt;
      &lt;<span style="color: #d7af00; font-weight: bold;">line_separator</span>&gt;newline&lt;/<span style="color: #d7af00; font-weight: bold;">line_separator</span>&gt;
      &lt;<span style="color: #d7af00; font-weight: bold;">var_separator</span>&gt;,&lt;/<span style="color: #d7af00; font-weight: bold;">var_separator</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/controls/flight/aileron&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/controls/flight/aileron&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/controls/flight/elevator&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/controls/flight/elevator&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/controls/engines/engine/throttle&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/controls/engines/engine/throttle&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

    &lt;/<span style="color: #d7af00; font-weight: bold;">input</span>&gt;
  &lt;/<span style="color: #d7af00; font-weight: bold;">generic</span>&gt;
&lt;/<span style="color: #d7af00; font-weight: bold;">PropertyList</span>&gt;
</pre>
</div>

<p>
output-protocol.xml,
</p>

<div class="org-src-container">

<pre class="src src-xml">&lt;?<span style="color: #ff5f00; font-weight: bold;">xml</span> <span style="font-weight: bold; font-style: italic;">version</span>=<span style="color: #afafff; font-style: italic;">"</span><span style="color: #afafff; font-style: italic;">1.0</span><span style="color: #afafff; font-style: italic;">"</span>?&gt;
&lt;<span style="color: #d7af00; font-weight: bold;">PropertyList</span>&gt;
  &lt;<span style="color: #d7af00; font-weight: bold;">generic</span>&gt;
    &lt;<span style="color: #d7af00; font-weight: bold;">output</span>&gt;
      &lt;<span style="color: #d7af00; font-weight: bold;">line_separator</span>&gt;newline&lt;/<span style="color: #d7af00; font-weight: bold;">line_separator</span>&gt;
      &lt;<span style="color: #d7af00; font-weight: bold;">var_separator</span>&gt;,&lt;/<span style="color: #d7af00; font-weight: bold;">var_separator</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/orientation/roll-deg&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/orientation/roll-deg&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;[ %f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/orientation/pitch-deg&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/orientation/pitch-deg&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/position/altitude-agl-ft&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/position/altitude-agl-ft&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/orientation/heading-deg&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/orientation/heading-deg&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/velocities/airspeed-kt&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/velocities/airspeed-kt&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/position/latitude-deg&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/position/latitude-deg&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
        &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

      &lt;<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;
         &lt;<span style="color: #d7af00; font-weight: bold;">name</span>&gt;/position/longitude-deg&lt;/<span style="color: #d7af00; font-weight: bold;">name</span>&gt;
         &lt;<span style="color: #d7af00; font-weight: bold;">node</span>&gt;/position/longitude-deg&lt;/<span style="color: #d7af00; font-weight: bold;">node</span>&gt;
         &lt;<span style="color: #d7af00; font-weight: bold;">type</span>&gt;float&lt;/<span style="color: #d7af00; font-weight: bold;">type</span>&gt;
         &lt;<span style="color: #d7af00; font-weight: bold;">format</span>&gt;%f]&lt;/<span style="color: #d7af00; font-weight: bold;">format</span>&gt;
      &lt;/<span style="color: #d7af00; font-weight: bold;">chunk</span>&gt;

    &lt;/<span style="color: #d7af00; font-weight: bold;">output</span>&gt;
  &lt;/<span style="color: #d7af00; font-weight: bold;">generic</span>&gt;
&lt;/<span style="color: #d7af00; font-weight: bold;">PropertyList</span>&gt;
</pre>
</div>
<div class="post-tags">Tags: <a href="/tags/#clojure">clojure </a><a href="/tags/#flightgear">flightgear </a></div></div><div id="related"><h3 class="random-posts">Random Posts</h3><ul class="posts"><li><span>23 Mar 2011</span><a href="/2011/03/23/streaming-opencv-video-over-the-network-using-mjpeg/">Streaming OpenCV Video over the Network Using M-JPEG</a></li><li><span>07 Oct 2010</span><a href="/2010/10/07/towards-a-clojure-autopilot-first-steps/">Towards a Clojure Autopilot - First Steps</a></li><li><span>10 Nov 2009</span><a href="/2009/11/10/using-java-mail-api-from-clojure/">Using Java Mail API from Clojure</a></li><li><span>16 Nov 2009</span><a href="/2009/11/16/java-native-access-from-clojure/">Java Native Access from Clojure</a></li><li><span>21 May 2010</span><a href="/2010/05/21/motor-control-via-esc-using-arduino-and-clodiuno/">Motor Control via ESC Using Arduino and Clodiuno</a></li></ul></div><div id="disqus"><div id="disqus_thread"></div><script type="text/javascript" src="//disqus.com/forums/nakkaya/embed.js"></script><noscript><a href="//disqus.com/forums/nakkaya/?url=ref">View the discussion thread.</a></noscript><a href="//disqus.com" class="dsq-brlink">blog comments powered by <span class="logo-disqus">Disqus</span></a></div></div><div id="footer"><a href="/rss-feed"> RSS Feed</a><p>&copy; 2018<a href="http://nakkaya.com"> Nurullah Akkaya</a></p></div></div><script type="text/javascript">
//<![CDATA[
(function() {
	     var links = document.getElementsByTagName('a');
	     var query = '?';
	     for(var i = 0; i < links.length; i++) {
		     if(links[i].href.indexOf('#disqus_thread') >= 0) {
								       query += 'url' + i + '=' + encodeURIComponent(links[i].href) + '&';
								       }
		     }
	     document.write('<script charset="utf-8" type="text/javascript" src="//disqus.com/forums/nakkaya/get_num_replies.js' + query + '"></' + 'script>');
	     })();
//]]>
</script></body></html>