Add rough support for ILS approaches (#12)

* Add hacky initial approach steering implementation

Currently we just steer directly toward the airport when we've
intercepted the localizer, which *may* look okay but is certainly hacky.
Still need to implement localizer math, altitude adjustments, and then
finally detect landing

* Begin work to detect whether an aircraft is intercepting the localizer

* Include elevation in runway-coords

* Attempt to detect glide slope

* Attempt to update altitude when on glide slope to approach runway

We probably want to do something smarter here, like compute the
*expected altitude* for the glideslope, and apply that instead.

* Implement "cancel approach clearance" command

* Fix runway parsing

* Add *rough* support for handling aircraft landing

* Fix cancel-approach grammar to avoid false positives if unparseable

* Fix local linting

CI seems fine with this but it's struggling locally

.clj-kondo/config.edn

@@ -18,6 +18,13 @@
  :lint-as {applied-science.js-interop/defn clojure.core/defn
            applied-science.js-interop/let clojure.core/let
            applied-science.js-interop/fn clojure.core/fn
+
+           com.rpl.specter/defcollector clojure.core/defn
+           com.rpl.specter/defdynamicnav clojure.core/defn
+           com.rpl.specter/defmacroalias clojure.core/def
+           com.rpl.specter/defnav clojure.core/defn
+           com.rpl.specter/defrichnav clojure.core/defn
+
            gloss.core/defcodec clojure.core/def
            instaparse.core/defparser clojure.core/def
            promesa.core/let clojure.core/let

src/main/atc/data/aircraft_configs.cljs

@@ -5,4 +5,7 @@
 (def common-jet (aircraft-config/create
                   ; NOTE: 3 degrees per second is a standard rate turn
                   ; see: https://en.wikipedia.org/wiki/Standard_rate_turn
-                  {:turn-rate 3}))
+                  {:turn-rate 3
+
+                   :climb-rate 3000
+                   :descent-rate 3500}))

src/main/atc/data/airports.cljc

@@ -0,0 +1,22 @@
+(ns atc.data.airports 
+  (:require
+   [atc.data.core :refer [local-xy]]
+   [atc.data.units :refer [ft->m]]
+   [atc.engine.model :refer [vec3]]))
+
+(defn runway-coords [airport runway]
+  (when-let [runway-object (->> airport
+                                :runways
+                                (filter #(or (= runway (:start-id %))
+                                             (= runway (:end-id %))))
+                                first)]
+    (let [elevation (-> (:position airport)
+                        (nth 2)
+                        ft->m)
+          start (-> (local-xy (:start-threshold runway-object) airport)
+                    (vec3 elevation))
+          end (-> (local-xy (:end-threshold runway-object) airport)
+                  (vec3 elevation))]
+      (if (= (:start-id runway-object) runway)
+        [start end]
+        [end start]))))

src/main/atc/data/units.cljc

@@ -0,0 +1,18 @@
+(ns atc.data.units 
+  (:require
+   [clojure.math :refer [floor]]))
+
+(defn m->ft [meters]
+  ; NOTE: We probably *never* need to do this except for displaying
+  ; or verbally reporting an aircraft's altitude in feet, so let's
+  ; just make it clean:
+  (floor (* 3.28084 meters)))
+
+(defn m->nm [meters]
+  (* 0.000539957 meters))
+
+(defn ft->m [feet]
+  (* 0.3048 feet))
+
+(defn nm->m [nautical-miles]
+  (* 1852 nautical-miles))

src/main/atc/engine/aircraft.cljs

@@ -1,5 +1,6 @@
 (ns atc.engine.aircraft
   (:require
+   [atc.data.units :refer [ft->m]]
    [atc.engine.aircraft.commands :refer [apply-commanded-inputs]]
    [atc.engine.aircraft.instructions :as instructions :refer [dispatch-instruction]]
    [atc.engine.config :refer [AircraftConfig]]
@@ -64,7 +65,7 @@
                   :radio-name radio-name
                   :state :flight
                   :pilot (pilot/generate nil) ; TODO Pass in a preferred voice?
-                  :position (vec3 250 250 20000)
+                  :position (vec3 250 250 (ft->m 20000))
                   :heading 350
                   :speed 200
                   :commands {:heading 90}}))

src/main/atc/engine/aircraft/commands.cljs

@@ -1,13 +1,19 @@
 (ns atc.engine.aircraft.commands
   "Responding to commands"
   (:require
-   [atc.engine.model :refer [bearing-to]]))
+   [atc.data.airports :as airports]
+   [atc.data.units :refer [nm->m]]
+   [atc.engine.model :refer [angle-down-to bearing-to distance-to-squared]]
+   [clojure.math :refer [pow]]))
 
 (defn- normalize-heading [h]
   (if (< h 0)
     (+ h 360)
     (mod h 360)))
 
+
+; ======= Steering ========================================
+
 (defn shorter-steer-direction [from to]
   ; With thanks to: https://math.stackexchange.com/a/2898118
   (let [delta (- (mod (- to from -540)
@@ -35,19 +41,120 @@
               (* turn-amount 0.5))
         (-> aircraft
             (assoc :heading commanded-to)  ; close enough; snap to
-            (dissoc :steer-direction))
+            (update :commands dissoc :steer-direction))
         (assoc aircraft :heading new-heading)))))
 
+
+; ======= Direct-to-point nav =============================
+
 (defn- apply-direct [aircraft commanded-to dt]
   ; NOTE: This is temporary; the real logic should account for resuming course,
   ; intercept heading, crossing altitude, etc.
   (let [bearing-to-destination (bearing-to (:position aircraft) commanded-to)]
     (apply-steering aircraft bearing-to-destination dt)))
 
 
+; ======= Altitude ========================================
+
+(defn- apply-altitude [{from :altitude :as aircraft} commanded-altitude dt]
+  (if (= from commanded-altitude)
+    aircraft
+
+    (let [sign (if (> commanded-altitude from) 1 -1)
+          rate-key ({-1 :descent-rate
+                     1 :climb-rate} sign)
+          rate (get-in aircraft [:config rate-key])
+          new-altitude (+ from (* sign rate dt))]
+      (if (<= (abs (- commanded-altitude new-altitude))
+              (* rate 0.5))
+        (-> aircraft
+            (assoc :altitude commanded-altitude)  ; close enough; snap to
+            (update :commands dissoc :target-altitude))
+
+        (-> aircraft
+            (assoc :altitude new-altitude))))))
+
+
+; ======= Approach course following =======================
+
+; NOTE: We use squared distances to avoid having to compute sqrt
+(def ^:private localizer-narrow-distance-m2 (pow (nm->m 18) 2.))
+(def ^:private localizer-narrow-angle-degrees 10)
+(def ^:private localizer-wide-distance-m2 (pow (nm->m 10) 2.))
+(def ^:private localizer-wide-angle-degrees 35)
+
+(def ^:private glide-slope-angle-degrees 3)
+(def ^:private glide-slope-width-degrees 1.4)
+
+(def ^:private landed-distance-m 5)
+
+(defn- within-localizer?
+  "Returns the [runway-threshold distance2] if within the runway's localizer, else nil"
+  [aircraft airport runway]
+  (when-let [[start end] (airports/runway-coords airport runway)]
+    (let [runway-heading (normalize-heading (bearing-to start end))
+          angle-to-threshold (normalize-heading (bearing-to (:position aircraft) start))
+          delta (abs (- runway-heading angle-to-threshold))
+          distance-to-runway2 (distance-to-squared (:position aircraft) start)]
+      (when (cond
+              (<= distance-to-runway2 localizer-wide-distance-m2)
+              (<= delta localizer-wide-angle-degrees)
+
+              (<= distance-to-runway2 localizer-narrow-distance-m2)
+              (<= delta localizer-narrow-angle-degrees))
+        [start distance-to-runway2]))))
+
+(defn- apply-ils-approach [aircraft {:keys [airport runway]} dt]
+  (if-some [[runway-start distance-to-runway2] (within-localizer? aircraft airport runway)]
+    ; TODO: If we just become established on the localizer and are above the glide slope,
+    ;  that's no good. 
+    (cond-> aircraft
+      ; Detect "landing"
+      (<= distance-to-runway2
+          landed-distance-m)
+      (assoc :state :landed
+             :commands {})
+
+      ; Follow glide slope
+      (>= (- glide-slope-angle-degrees
+             glide-slope-width-degrees)
+          (angle-down-to (:position aircraft) runway-start)
+          (+ glide-slope-angle-degrees
+             glide-slope-width-degrees))
+      (->
+        ; Ensure there's no competing altitude
+        (update :commands dissoc :target-altitude)
+
+        ; TODO Slow down
+
+        ; Descend toward runway
+        (apply-altitude (:z runway-start) dt))
+
+      ; Always ensure we turn onto course while within the localizer
+      :always
+      (->
+        ; We no longer need to maintain any specific heading
+        (update :commands dissoc :heading :steer-direction)
+
+        ; Steer toward the airport
+        ; TODO: Perhaps, steer toward runway threshold? Or, maintain its heading? There's
+        ; definitely a more realistic approach than either of these, but this may be
+        ; sufficient for now...
+        (apply-direct airport dt)))
+
+      ; Just proceed on course, awaiting the intercept
+      aircraft))
+
+; I suppose this could be a defmulti...
+(defn- apply-approach [aircraft {:keys [approach-type] :as command} dt]
+  (case approach-type
+    :ils (apply-ils-approach aircraft command dt)))
+
+
 ; ======= Publc interface =================================
 
 (defn apply-commanded-inputs [aircraft, commands dt]
   (cond-> aircraft
     (:heading commands) (apply-steering (:heading commands) dt)
-    (:direct commands) (apply-direct (:direct commands) dt)))
+    (:direct commands) (apply-direct (:direct commands) dt)
+    (:cleared-approach commands) (apply-approach (:cleared-approach commands) dt)))

src/main/atc/engine/aircraft/instructions.cljs

@@ -17,11 +17,29 @@
 
     :else (-> craft
             (assoc :state :cleared-approach)
+            (update :commands dissoc :direct)
+            (update :commands assoc :cleared-approach {:approach-type approach-type
+                                                       :airport (:airport context)
+                                                       :runway runway})
             (utter "cleared approach runway" [:runway runway]))))
 
+(defmethod dispatch-instruction
+  :cancel-approach
+  [craft _ _]
+  (cond
+    ; If not currently cleared, this is a nop:
+    (not (get-in craft [:commands :cleared-approach]))
+    craft
+
+    :else (-> craft
+            (assoc :state :flight)
+            (update :commands dissoc :cleared-approach)
+            (utter "cancel approach"))))
+
 (defmethod dispatch-instruction
   :steer
   [craft _ [_ heading steer-direction]]
+  ; TODO Check state; if in approach, we should reject
   (-> craft
       (utter (when steer-direction (name steer-direction))
              [:heading heading])
@@ -32,6 +50,7 @@
 (defmethod dispatch-instruction
   :direct
   [craft context [_ fix-id]]
+  ; TODO Check state; if in approach, we should reject
   (if-let [fix (get-in context [:game/navaids-by-id fix-id])]
     (-> craft
         (utter "direct " fix)

src/main/atc/engine/config.cljs

@@ -1,8 +1,12 @@
-(ns atc.engine.config)
+(ns atc.engine.config
+  (:require
+   [atc.data.units :as units]))
 
-#_{:clj-kondo/ignore [:clojure-lsp/unused-public-var]}
-(defrecord AircraftConfig [turn-rate])
+(defrecord AircraftConfig [turn-rate climb-rate descent-rate])
 
 (defn create [kvs]
-  (map->AircraftConfig kvs))
+  (-> kvs
+      (update :climb-rate units/ft->m)
+      (update :descent-rate units/ft->m)
+      (map->AircraftConfig)))
 

src/main/atc/engine/core.cljs

@@ -26,7 +26,7 @@
           (consume-pending-communication simulated))))))
 
 #_{:clj-kondo/ignore [:clojure-lsp/unused-public-var]}
-(defrecord Engine [airport aircraft parsing-machine time-scale elapsed-s last-tick]
+(defrecord Engine [airport aircraft parsing-machine time-scale elapsed-s last-tick events]
   ; NOTE: It is sort of laziness that we're implementing Simulated here,
   ; since we aren't, properly. Technically we should have a separate Simulator
   ; protocol and implement that to be more correct...
@@ -40,15 +40,21 @@
                   0)
                 0.001)
 
+          ; Tick all aircraft, and detect events like landing, etc.
           updated-aircraft (reduce-kv
                              (fn [m callsign aircraft]
-                               (assoc m callsign (tick aircraft dt)))
+                               (let [updated (tick aircraft dt)]
+                                 (if (= :landed (:state updated))
+                                   (update m ::events conj {:type :aircraft-landed
+                                                            :aircraft updated})
+                                   (assoc m callsign updated))))
                              {}
                              aircraft)]
 
       (assoc this
-             :aircraft updated-aircraft
+             :aircraft (dissoc updated-aircraft ::events)
              :elapsed-s (+ (:elapsed-s this) dt)
+             :events (::events updated-aircraft)
              :last-tick (when-not (= 0 time-scale)
                           now))))
 
@@ -84,5 +90,6 @@
          :airport airport
          :parsing-machine (build-machine (airport-parsing/generate-parsing-context airport))
          :elapsed-s 0
+         :events nil
          :time-scale 1}
         (map->Engine))))

src/main/atc/engine/model.cljs → src/main/atc/engine/model.cljc

@@ -21,7 +21,8 @@
 (defprotocol Vector
   (v+ [this ^Vector other])
   (v- [this ^Vector other])
-  (v* [this other]))
+  (v* [this other])
+  (vmag2 [this] "Compute the square of the magnitude of this vector"))
 
 #_{:clj-kondo/ignore [:clojure-lsp/unused-public-var]}
 (defrecord Vec3 [x y z]
@@ -48,14 +49,25 @@
       (assoc this
              :x (* x (:x other))
              :y (* y (:y other))
-             :z (* z (:z other 1))))))
+             :z (* z (:z other 1)))))
+
+  (vmag2 [this]
+    (let [{dx :x dy :y dz :z} this]
+      (+ (* dx dx)
+         (* dy dy)
+         (* dz dz)))))
 
 (defn vec3
   ([v] (if (instance? Vec3 v) v
          (->Vec3 (:x v) (:y v) (:z v))))
+  ([v z] (let [{:keys [x y]} (vec3 v)]
+           (->Vec3 x y z)))
   ([x y z]
    (->Vec3 x y z)))
 
+(defn distance-to-squared [from to]
+  (vmag2 (v- (vec3 to) from)))
+
 (defn bearing-to [from to]
   (let [{dx :x dy :y} (v- (vec3 to) from)]
     ; NOTE: This may or may not be the right move, but We want 0 degrees to
@@ -64,3 +76,16 @@
     ; here....
     (+ (to-degrees (atan2 dy dx)) 90)))
 
+(defn angle-down-to
+  "Assuming `from` is an elevated position and `to` is a position on the ground,
+  return the angle between the ground and a line segment between `from` and `to`"
+  [from to]
+  (let [from (vec3 from)
+        to (vec3 to)
+        elevation (- (:z from) (:z to))]
+    (to-degrees
+      ; NOTE: Rather than involve any sqrts we just square elevation
+      (atan2 (* elevation elevation)
+             (vmag2
+               (v- (vec3 from 0)
+                   (vec3 to 0)))))))

src/main/atc/events.cljs

@@ -108,6 +108,9 @@
   (fn [{:keys [db]} _]
     (when-let [engine (:engine db)]
       (let [engine' (engine-model/tick engine nil)
+            new-events (:events engine)
+            engine' (assoc engine' :events nil)
+
             db' (assoc db :engine engine')
 
             seconds-since-last-snapshot (- (:elapsed-s engine')
@@ -118,7 +121,10 @@
          :fx [(when-let [delay-ms (engine/next-tick-delay engine')]
                 [:dispatch-later
                  {:ms delay-ms
-                  :dispatch [:game/tick]}])]}))))
+                  :dispatch [:game/tick]}])
+
+              (when (seq new-events)
+                (println "TODO: Handle engine events:" new-events))]}))))
 
 (reg-event-fx
   :game/reset