From 207e476cc394bdde7082d30d8772b2dd83c64dfe Mon Sep 17 00:00:00 2001
From: Pascal Brisset <pascal.brisset@free.fr>
Date: Mon, 27 Feb 2006 18:38:05 +0000
Subject: [PATCH] Sending params on the net (for flight gear)

---
 sw/simulator/Makefile |  5 ++++-
 sw/simulator/fg.c     | 29 +++++++++++++++++++++++++++++
 sw/simulator/sim.ml   | 35 +++++++++++++++++++++++++++++++++--
 3 files changed, 66 insertions(+), 3 deletions(-)
 create mode 100644 sw/simulator/fg.c

diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 66f3a5b38e..dd632a328c 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -60,7 +60,7 @@ sim_sitl : $(OBJDIR)/simsitl
 simhitl.out : $(SIMHCMO) simhitl.cmo
 	$(OCAMLC) $(INCLUDES) -o $@ str.cma xml-light.cma unix.cma lib.cma lablgtk.cma gtkInit.cmo $^
 
-sitl.cma : $(SIMSCMO)
+sitl.cma : fg.o $(SIMSCMO)
 	ocamlc -o $@ -a $^ 
 
 sitl.cmxa : $(SIMSCMX)
@@ -108,6 +108,9 @@ $(OBJDIR)/simsitl.ml : simsitl.ml
 %.cmo : %.ml
 	$(OCAMLC) $(INCLUDES) -c $<
 
+%.o : %.c
+	$(OCAMLC) -c $<
+
 %.cmx : %.ml
 	$(OCAMLOPT) $(INCLUDES) -c $<
 
diff --git a/sw/simulator/fg.c b/sw/simulator/fg.c
new file mode 100644
index 0000000000..36f664b807
--- /dev/null
+++ b/sw/simulator/fg.c
@@ -0,0 +1,29 @@
+/** Values boxing for Flight Gear */
+
+#include <string.h>
+#include <caml/alloc.h>
+#include <caml/mlvalues.h>
+#include <caml/memory.h>
+
+struct fg {
+  float x;
+  float y;
+  float z;
+  float phi;
+};
+
+value fg_msg(value x, value y, value z, value phi) {
+  CAMLparam4(x, y, z, phi);
+  CAMLlocal1(s);
+
+  struct fg msg;
+  msg.x = Double_val(x);
+  msg.y = Double_val(x);
+  msg.z = Double_val(x);
+  msg.phi = Double_val(x);
+
+  s = alloc_string(sizeof(struct fg));
+  strncpy(String_val(s), (char*)&msg, sizeof(struct fg));
+
+  CAMLreturn (s);
+}
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index c268440cf7..66dbfbe0d3 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -34,6 +34,7 @@ let float_attrib xml a = float_of_string (ExtXml.attrib xml a)
 (* Frequencies for perdiodic tasks are expressed in s *)
 let ir_period = 1./.20.
 let fm_period = 1./.25.
+let fg_period = 1./.50.
 
 
 module type AIRCRAFT = 
@@ -55,12 +56,19 @@ module type AIRCRAFT =
 
 module type AIRCRAFT_ITL = functor (A : Data.MISSION) -> AIRCRAFT
 
+external fg_msg : float -> float -> float -> float -> string = "fg_msg"
+
 
 let ac_name = ref ""
 
 let ivy_bus = ref "127.255.255.255:2010"
 
-let common_options = [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"]
+let fg_client = ref ""
+
+let common_options = [
+  "-b", Arg.Set_string ivy_bus, "Bus\tDefault is 127.255.255.25:2010";
+  "-fg", Arg.Set_string fg_client, "Flight gear client address"
+]
 
 module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
@@ -167,11 +175,34 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       last_gps_state := Some s;
       Aircraft.gps s in
 
+    (** Sending to Flight Gear *)
+    let fg_task = fun socket () ->
+      let (x,y,z) = FlightModel.get_xyz !state
+      and phi = FlightModel.get_phi !state in
+      let m = fg_msg x y z phi in
+      (*** for i = 0 to String.length m - 1 do fprintf stderr "%x " (Char.code m.[i]) done; fprintf stderr "\n"; ***)
+      try
+	ignore (Unix.send socket m 0 (String.length m) [])
+      with
+	Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a
+    in
+
     let boot = fun () ->
       Aircraft.boot (time_scale:>value);
       Stdlib.timer ~scale:time_scale fm_period fm_task;
       Stdlib.timer ~scale:time_scale ir_period ir_task;
-      Stdlib.timer ~scale:time_scale gps_period gps_task in
+      Stdlib.timer ~scale:time_scale gps_period gps_task;
+
+      (** Connection to Flight Gear client *)
+      if !fg_client <> "" then
+	try
+	  let inet_addr = Unix.inet_addr_of_string !fg_client in
+	  let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
+	  Unix.connect socket (Unix.ADDR_INET (inet_addr, 1234));
+	  Stdlib.timer ~scale:time_scale fg_period (fg_task socket)
+	with
+	  e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e)
+    in
     
     let take_off = fun () -> FlightModel.set_air_speed !state FM.nominal_airspeed in