Last active
February 25, 2016 07:38
-
-
Save xandkar/6a460caf6df0c5d9d933 to your computer and use it in GitHub Desktop.
X-Plane Autopilot POC
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 ()) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment