mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
Pascal Brisset
229 lines
7.6 KiB
OCaml
229 lines
7.6 KiB
OCaml
(*
|
|
* $Id$
|
|
*
|
|
* Hardware in the loop basic simulator (handling GPS, infrared and servos)
|
|
*
|
|
* Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
|
*
|
|
* This file is part of paparazzi.
|
|
*
|
|
* paparazzi is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2, or (at your option)
|
|
* any later version.
|
|
*
|
|
* paparazzi is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with paparazzi; see the file COPYING. If not, write to
|
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
* Boston, MA 02111-1307, USA.
|
|
*
|
|
*)
|
|
|
|
open Printf
|
|
open Stdlib
|
|
|
|
module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end)
|
|
|
|
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 =
|
|
sig
|
|
val init : int -> GPack.box -> unit
|
|
val boot : Stdlib.value -> unit
|
|
(** [boot time_acceleration] *)
|
|
|
|
val servos : us array -> unit
|
|
(** Called once at init *)
|
|
|
|
val infrared : float -> float -> float -> unit
|
|
(** [infrared ir_left ir_front ir_top] Called on timer *)
|
|
|
|
val gps : Gps.state -> unit
|
|
(** [gps state] Called on timer *)
|
|
end
|
|
|
|
|
|
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 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
|
|
|
|
module A = struct
|
|
let ac = Data.aircraft !ac_name
|
|
end
|
|
|
|
module Aircraft = AircraftItl(A)
|
|
|
|
module FM = FlightModel.Make(A)
|
|
|
|
let flight_plan = A.ac.Data.flight_plan
|
|
|
|
let lat0 = rad_of_deg (float_attrib flight_plan "lat0")
|
|
let lon0 = rad_of_deg (float_attrib flight_plan "lon0")
|
|
let qfu = (float_attrib flight_plan "qfu")
|
|
let alt0 = (float_attrib flight_plan "ground_alt")
|
|
|
|
let main () =
|
|
let window = GWindow.window ~title:("Aircraft "^ !ac_name) () in
|
|
let quit = fun () -> GMain.Main.quit (); exit 0 in
|
|
ignore (window#connect#destroy ~callback:quit);
|
|
let vbox = GPack.vbox ~packing:window#add () in
|
|
Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
|
|
|
|
Aircraft.init A.ac.Data.id vbox;
|
|
|
|
let gps_period = 0.25 in
|
|
|
|
let compute_gps_state = Gps.state lat0 lon0 (alt0) in
|
|
|
|
let utm0 = Latlong.utm_of Latlong.WGS84 {Latlong.posn_long = lon0; Latlong.posn_lat = lat0} in
|
|
|
|
let initial_state = FlightModel.init (pi/.2. -. qfu/.180.*.pi) in
|
|
|
|
let state = ref initial_state in
|
|
|
|
let reset = fun () -> state := initial_state in
|
|
|
|
let servos = Array.create FM.nb_servos 0 in
|
|
|
|
Aircraft.servos servos;
|
|
|
|
let north_label = GMisc.label ~text:"000" ()
|
|
and east_label = GMisc.label ~text:"000" ()
|
|
and alt_label = GMisc.label ~text:"000" () in
|
|
|
|
let half_aperture = (Latlong.pi /. 4.) in
|
|
let last_gps_state = ref None in
|
|
let run = ref false in
|
|
let ir_srtm = ref false in
|
|
|
|
let wind_x = ref 0.
|
|
and wind_y = ref 0. in
|
|
let infrared_contrast = ref 266.
|
|
and time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end
|
|
and gps_availability = ref 1 in
|
|
|
|
let world_update = fun _ vs ->
|
|
gps_availability := Pprz.int_assoc "gps_availability" vs;
|
|
wind_x := Pprz.float_assoc "wind_east" vs;
|
|
wind_y := Pprz.float_assoc "wind_north" vs;
|
|
infrared_contrast := Pprz.float_assoc "ir_contrast" vs;
|
|
time_scale#set_value (Pprz.float_assoc "time_scale" vs)
|
|
in
|
|
|
|
ignore (Ground_Pprz.message_bind "WORLD_ENV" world_update);
|
|
|
|
|
|
let fm_task = fun () ->
|
|
FM.do_servos !state servos;
|
|
FM.state_update !state (!wind_x, !wind_y) fm_period
|
|
|
|
and ir_task = fun () ->
|
|
let phi = FlightModel.get_phi !state in
|
|
let horizon_distance = 1000. in
|
|
try
|
|
match !last_gps_state with
|
|
None -> ()
|
|
| Some gps_state ->
|
|
let delta_ir =
|
|
if !ir_srtm then
|
|
let altitude = (int_of_float gps_state.Gps.alt) in
|
|
let horizon_right = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course +. Latlong.pi /. 2.) half_aperture horizon_distance in
|
|
let horizon_left = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course -. Latlong.pi /. 2.) half_aperture horizon_distance in
|
|
horizon_right -. horizon_left
|
|
else
|
|
0. in
|
|
let phi = phi +. FM.roll_neutral_default in
|
|
let ir_left = (phi +. delta_ir ) *. !infrared_contrast
|
|
and ir_front = 0.
|
|
and ir_top = pi /. 2. *. !infrared_contrast in
|
|
Aircraft.infrared ir_left ir_front ir_top
|
|
with
|
|
x -> Printf.printf "%s\n%!" (Printexc.to_string x)
|
|
|
|
and gps_task = fun () ->
|
|
let (x,y,z) = FlightModel.get_xyz !state in
|
|
east_label#set_text (Printf.sprintf "%.0f" x);
|
|
north_label#set_text (Printf.sprintf "%.0f" y);
|
|
alt_label#set_text (Printf.sprintf "%.0f" z);
|
|
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
|
|
|
|
(** 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;
|
|
|
|
(** 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
|
|
|
|
let hbox = GPack.hbox ~packing:vbox#pack () in
|
|
let s = GButton.button ~label:"Boot" ~packing:(hbox#pack ~padding:5) () in
|
|
ignore (s#connect#clicked ~callback:boot);
|
|
let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in
|
|
ignore (t#connect#clicked ~callback:take_off);
|
|
let ir_srtm_button = GButton.toggle_button ~label:"IR/srtm" ~packing:hbox#pack () in
|
|
ignore (ir_srtm_button#connect#toggled (fun () -> ir_srtm := not !ir_srtm));
|
|
|
|
let hbox = GPack.hbox ~packing:vbox#pack () in
|
|
let l = fun s -> ignore(GMisc.label ~text:s ~packing:hbox#pack ()) in
|
|
l "East:"; hbox#pack east_label#coerce;
|
|
l " North:"; hbox#pack north_label#coerce;
|
|
l " Height:"; hbox#pack alt_label#coerce;
|
|
|
|
Ivy.init (sprintf "Paparazzi sim %d" A.ac.Data.id) "READY" (fun _ _ -> ());
|
|
Ivy.start !ivy_bus;
|
|
|
|
window#show ();
|
|
Unix.handle_unix_error GMain.Main.main ()
|
|
end
|