From f29eebb7662dbc13d48107c620a21b0d5d8598d9 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 4 Oct 2011 20:50:12 +0200 Subject: [PATCH] added timer and function for simulating attitude, only true value --- sw/simulator/hitl.ml | 3 +++ sw/simulator/sim.ml | 11 ++++++++++- sw/simulator/sim.mli | 1 + sw/simulator/sitl.ml | 4 ++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/sw/simulator/hitl.ml b/sw/simulator/hitl.ml index cde61c4e416..bb56c86ced4 100644 --- a/sw/simulator/hitl.ml +++ b/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 diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml index cf1f41b364e..5b095f783dd 100644 --- a/sw/simulator/sim.ml +++ b/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 diff --git a/sw/simulator/sim.mli b/sw/simulator/sim.mli index 24062562cc8..f219703c5e9 100644 --- a/sw/simulator/sim.mli +++ b/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 diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index d239cb6d413..e595d17ff1f 100644 --- a/sw/simulator/sitl.ml +++ b/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