X-Plane Autopilot POC

Makefile

build:
	@ocamlbuild \
		-use-ocamlfind \
		-tags thread \
		-syntax camlp4o \
		-pkgs bitstring.syntax,bitstring,core,async \
		x_plane_autopilot.byte

deps:
	@opam install core async bitstring

clean:
	@ocamlbuild -clean

x_plane_autopilot.ml

open Core.Std
open Async.Std

module Data : sig
  module Datum : sig
    type t =
      { index : int
      ;    v1 : float
      ;    v2 : float
      ;    v3 : float
      ;    v4 : float
      ;    v5 : float
      ;    v6 : float
      ;    v7 : float
      ;    v8 : float
      }

    val show : t -> string
  end

  type t = Datum.t list

  val of_string : string -> t

  val to_string : t -> string
end = struct
  module Datum = struct
    type t =
      { index : int
      ;    v1 : float
      ;    v2 : float
      ;    v3 : float
      ;    v4 : float
      ;    v5 : float
      ;    v6 : float
      ;    v7 : float
      ;    v8 : float
      }

    let of_bitstring bits =
      bitmatch bits with
      | { index : 32 : littleendian
        ;    v1 : 32 : littleendian
        ;    v2 : 32 : littleendian
        ;    v3 : 32 : littleendian
        ;    v4 : 32 : littleendian
        ;    v5 : 32 : littleendian
        ;    v6 : 32 : littleendian
        ;    v7 : 32 : littleendian
        ;    v8 : 32 : littleendian
        } ->
          { index = Option.value_exn (Int32.to_int index)
          ;    v1 = Int32.float_of_bits v1
          ;    v2 = Int32.float_of_bits v2
          ;    v3 = Int32.float_of_bits v3
          ;    v4 = Int32.float_of_bits v4
          ;    v5 = Int32.float_of_bits v5
          ;    v6 = Int32.float_of_bits v6
          ;    v7 = Int32.float_of_bits v7
          ;    v8 = Int32.float_of_bits v8
          }

    let to_bitstring {index; v1; v2; v3; v4; v5; v6; v7; v8} =
      let index = Option.value_exn (Int32.of_int index) in
      ( BITSTRING
        {                    index : 32 : littleendian
        ; (Int32.bits_of_float v1) : 32 : littleendian
        ; (Int32.bits_of_float v2) : 32 : littleendian
        ; (Int32.bits_of_float v3) : 32 : littleendian
        ; (Int32.bits_of_float v4) : 32 : littleendian
        ; (Int32.bits_of_float v5) : 32 : littleendian
        ; (Int32.bits_of_float v6) : 32 : littleendian
        ; (Int32.bits_of_float v7) : 32 : littleendian
        ; (Int32.bits_of_float v8) : 32 : littleendian
        }
      )

    let show {index=i; v1; v2; v3; v4; v5; v6; v7; v8} =
      sprintf
        "| %3d | %11f | %11f | %11f | %11f | %11f | %11f | %11f | %11f |"
            i     v1     v2     v3     v4     v5     v6     v7     v8
  end

  type t =
    Datum.t list

  let of_string s =
    let rec split blocks =
      bitmatch blocks with
      | { block  :  9 * 32 : bitstring
        ; blocks : -1      : bitstring
        } ->
          block :: (split blocks)
      | {_ : 0 : bitstring} ->
          []
    in
    let packet = Bitstring.bitstring_of_string s in
    ( bitmatch packet with
    | { "DATA" : 4 * 8 : string
      ; "@"    : 1 * 8 : string
      ; blocks : -1    : bitstring
      } ->
        List.map (split blocks) ~f:Datum.of_bitstring
    )

  let to_string t =
    let rec join = function
      | [] ->
          Bitstring.empty_bitstring
      | datum :: data ->
          ( BITSTRING
            { (Datum.to_bitstring datum) :  9 * 32 : bitstring
            ; (join data)                : -1      : bitstring
            }
          )
    in
    let data = join t in
    Bitstring.string_of_bitstring (BITSTRING
    { "DATA" : 32 : string
    ; "0"    :  8 : string
    ; data   : -1 : bitstring
    })
end

let displayer ~status_packets_r =
  let term_clear () = print_string "\027[2J" in
  let term_reset () = print_string "\027[1;1H" in
  let rec loop () =
    Pipe.read status_packets_r >>= function
    | `Eof ->
        return ()
    | `Ok packet ->
        term_reset ();
        List.iter
          (Data.of_string packet)
          ~f:(fun d -> print_endline (Data.Datum.show d));
        loop ()
  in
  term_clear ();
  loop ()

let listener ~address ~port ~status_packets_w =
  Udp.bind (Unix.Socket.Address.Inet.create address ~port)
  >>= fun sock ->
  Udp.read_loop
    (Socket.fd sock)
    (fun buffer ->
      let packet = Iobuf.to_string buffer in
      Pipe.write_without_pushback status_packets_w packet
    )

let sender ~address ~port ~control_packets_r =
  (* Doing all this blocking-Unix in In_thread gymnastics because,
   * at least on Mac OS X, Udp.sendto fails with:
   *
   *  (unimplemented Bigstring.sendto_nonblocking_no_sigpipe)
   *
   * TODO: Try sending with Udp.sendto on Linux.
   *)
  In_thread.run (fun () ->
    Core.Std.Unix.socket
      ~domain:Core.Std.Unix.PF_INET
      ~kind:Core.Std.Unix.SOCK_DGRAM
      ~protocol:0
  )
  >>= fun socket ->
  let send packet =
    In_thread.run (fun () ->
      let _len_sent =
        Core.Std.Unix.sendto
          socket
          ~buf:packet
          ~pos:0
          ~len:(String.length packet)
          ~mode:[]
          ~addr:(Core.Std.Unix.ADDR_INET (address, port))
      in
      ()
    )
  in
  let rec loop () =
    Pipe.read control_packets_r >>= function
    | `Eof ->
        return ()
    | `Ok packet ->
        send packet
        >>= fun () ->
        loop ()
  in
  loop ()

let controller ~control_triggers_r ~control_packets_w =
  let control_packet =
    Data.to_string
      [ { Data.Datum.index = 8
        ; v1 = -0.5
        ; v2 =  0.5
        ; v3 =  0.0
        ; v4 = -999.0
        ; v5 = -999.0
        ; v6 = -999.0
        ; v7 = -999.0
        ; v8 = -999.0
        }
      ]
  in
  let rec loop () =
    Pipe.read control_triggers_r >>= function
    | `Eof ->
        return ()
    | `Ok `Apply_control ->
        Pipe.write_without_pushback control_packets_w control_packet;
        loop ()
  in
  loop ()

let main ~our_address ~our_port ~xplane_address ~xplane_port =
  let (status_packets_r, status_packets_w) = Pipe.create () in
  let (control_packets_r, control_packets_w) = Pipe.create () in
  let (control_triggers_r, control_triggers_w) = Pipe.create () in
  Clock.run_at_intervals
    (Time.Span.of_sec 1.0)
    (fun () -> Pipe.write_without_pushback control_triggers_w `Apply_control);
  don't_wait_for
    (displayer ~status_packets_r);
  don't_wait_for
    (controller ~control_triggers_r ~control_packets_w);
  don't_wait_for
    (sender
      ~address:xplane_address
      ~port:xplane_port
      ~control_packets_r
    );
  don't_wait_for
    (listener
      ~address:our_address
      ~port:our_port
      ~status_packets_w
    );
  (* TODO: Create error paths and cleanup pipes *)
  Deferred.never ()

let () =
  let our_address    = Unix.Inet_addr.localhost in
  let our_port       = int_of_string Sys.argv.(1) in
  let xplane_address = Core.Std.Unix.Inet_addr.localhost in
  let xplane_port    = int_of_string Sys.argv.(2) in
  don't_wait_for (main ~our_address ~our_port ~xplane_address ~xplane_port);
  never_returns (Scheduler.go ())