added timer and function for simulating attitude, only true value

sw/simulator/hitl.ml

@@ -97,6 +97,9 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     with
       exc -> prerr_endline (Printexc.to_string exc)
 
+  let attitude = fun phi theta psi ->
+    prerr_endline "HITL attitude sim not implemented..."
+
   let sep_reg = Str.regexp Pprz.separator
   let read_commands = fun commands _sender values ->
     let s = Pprz.string_assoc "values" values in

sw/simulator/sim.ml

@@ -43,6 +43,7 @@ let georef_of_xml = fun xml ->
 let ir_period = 1./.20.
 let fm_period = 1./.25.
 let fg_period = 1./.25.
+let ahrs_period = 1./.20.
 
 let gensym = let n = ref 0 in fun p -> incr n; p ^ string_of_int !n
 let cb_register = fun closure ->
@@ -64,6 +65,9 @@ module type AIRCRAFT =
     val infrared_and_airspeed : float -> float -> float -> float -> unit
 	(** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
 
+    val attitude : float -> float -> float -> unit
+    (** [ahrs phi theta psi] Called on timer *)
+
     val gps : Gps.state -> unit
 	(** [gps state] Called on timer *)
   end
@@ -217,7 +221,11 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       let s = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
       s.Gps.availability <- not (!gps_availability = 0);
       last_gps_state := Some s;
-      Aircraft.gps s in
+      Aircraft.gps s
+
+    and ahrs_task = fun () ->
+      let (phi, theta, psi) = FlightModel.get_attitude !state in
+      Aircraft.attitude phi theta psi in
 
     (** Sending to Flight Gear *)
     let fg_task = fun socket buffer () ->
@@ -257,6 +265,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       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;
+      Stdlib.timer ~scale:time_scale ahrs_period ahrs_task;
 
       (** Connection to Flight Gear client *)
       if !fg_client <> "" then

sw/simulator/sim.mli

@@ -10,6 +10,7 @@ module type AIRCRAFT =
     val boot : Stdlib.value -> unit
     val commands : Stdlib.pprz_t array -> unit
     val infrared_and_airspeed : float -> float -> float -> float -> unit
+    val attitude: float -> float -> float -> unit
     val gps : Gps.state -> unit
   end
 

sw/simulator/sitl.ml

@@ -194,6 +194,10 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     (** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
     set_ir_and_airspeed (truncate ir_left) (truncate ir_front) (truncate ir_top) air_speed
 
+  external set_attitude : float -> float -> float -> unit = "set_attitude"
+  let attitude = fun phi theta psi ->
+    set_attitude phi theta psi
+
   external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> float -> float -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos"
   let gps = fun gps ->
     let utm = utm_of WGS84 gps.Gps.wgs84 in