diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 765b813ae7..00331baa07 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -27,7 +27,7 @@ include ../../conf/Makefile.local ACDIR= $(PAPARAZZI_HOME)/var/$(AIRCRAFT) OBJDIR= $(ACDIR)/sim -SIMHML = stdlib.ml types.ml data.ml flightModel.ml sirf.ml gps.ml hitl.ml sim.ml +SIMHML = stdlib.ml data.ml flightModel.ml gps.ml hitl.ml sim.ml SIMHCMO=$(SIMHML:%.ml=%.cmo) SIMSML = stdlib.ml data.ml flightModel.ml gps.ml sitl.ml sim.ml SIMSCMO=$(SIMSML:%.ml=%.cmo) @@ -126,3 +126,6 @@ simhitl.cmo: hitl.cmi sim.cmi simsitl.cmo: sim.cmi sitl.cmi sitl.cmo: sitl.cmi sitl.cmi: sim.cmi +data.cmo : data.cmi +stdlib.cmo : stdlib.cmi +gps.cmo : gps.cmi diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml index 8e4de393e8..f90e8292b6 100644 --- a/sw/simulator/data.ml +++ b/sw/simulator/data.ml @@ -1,3 +1,29 @@ +(* + * $Id$ + * + * Usefull data for simulation + * + * 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. + * + *) + let (//) = Filename.concat (* let pprz_conf_path = Env.paparazzi_src // "conf" *) @@ -14,9 +40,6 @@ let messages_ap = with Not_found -> failwith "'telemetry_ap' class missing in messages.xml" -(* let ubx_xml = Xml.parse_file (pprz_conf_path // "ubx.xml") *) -let ubx_xml = Xml.parse_file (user_conf_path // "ubx.xml") - type aircraft = { name : string; id : int; @@ -32,7 +55,7 @@ let aircraft = fun name -> [] -> failwith ("Aicraft not found : "^name) | x::_ when Xml.tag x = "aircraft" && Xml.attrib x "name" = name -> (x, i) - | x::xs -> loop (i+1) xs in + | _x::xs -> loop (i+1) xs in loop 0 (Xml.children conf_xml) in let airframe_file = user_conf_path // ExtXml.attrib aircraft_xml "airframe" in diff --git a/sw/simulator/data.mli b/sw/simulator/data.mli new file mode 100644 index 0000000000..054dea2965 --- /dev/null +++ b/sw/simulator/data.mli @@ -0,0 +1,39 @@ +(* + * $Id$ + * + * Usefull data for simulation + * + * 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. + * + *) + +val user_conf_path : string +val conf_xml : Xml.xml +val ground : Xml.xml +val messages_ap : Xml.xml +type aircraft = { + name : string; + id : int; + airframe : Xml.xml; + flight_plan : Xml.xml; + radio : Xml.xml; +} +val aircraft : string -> aircraft +module type MISSION = sig val ac : aircraft end diff --git a/sw/simulator/events.ml b/sw/simulator/events.ml deleted file mode 100644 index dca6d0aba8..0000000000 --- a/sw/simulator/events.ml +++ /dev/null @@ -1,105 +0,0 @@ -(* - * $Id$ - * - * High-level events handling - * - * 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. - * - *) - -type callback = unit -> unit -type time = float (* Unix time *) -type period = float (* Seconds *) - -type fd = Unix.file_descr - -type timer = { - mutable next_wake : time; - period : period; - cb : callback - } - - -type on_input_id = Unix.file_descr -let dummy_on_input_id = Unix.stdin -let on_fds = Hashtbl.create 11 -let register_on_input fd cb = - Hashtbl.add on_fds fd cb; fd -let remove_on_input fd = - Hashtbl.remove on_fds fd - -type timer_id = callback -let timers = ref [] (* Is a priority queue really necessary ? *) -let register_timer period cb = - timers := { next_wake = Unix.gettimeofday () +. period; period = period; cb = cb } :: !timers; cb -let remove_timer cb = - let rec loop = function - [] -> [] - | t::ts -> if t.cb == cb then ts else t :: loop ts in - timers := loop !timers - - -let get_input_fds () = - let l = ref [] in - Hashtbl.iter (fun fd _ -> l := fd :: !l) on_fds; - !l -let get_fd_callbacks fds = - List.map (Hashtbl.find on_fds) fds - -(** Returns next timer timeout and callback. May return a dummy callback - if no timers are set. Wrap the update of the selected timer in the - callback *) -let never = 2e9 -let get_next_timeout () = - let rec loop earlier_wake earlier_cb = function - [] -> (earlier_wake, earlier_cb) - | timer :: es -> - if timer.next_wake < earlier_wake then - let t = timer.next_wake in - loop t (fun () -> timer.next_wake <- t +. timer.period; timer.cb ()) es - else - loop earlier_wake earlier_cb es in - loop never (fun () -> ()) !timers - - - - - - - - -let mainloop () = - while true do - let (next_timeout, timeout_cb) = get_next_timeout () in - let timeout = next_timeout -. Unix.gettimeofday () in - if timeout <= 0. then - timeout_cb () - else - let input_fds = get_input_fds () in - let (ready_inputs, _, _) = Unix.select input_fds [] [] timeout in - - match ready_inputs with - [] -> timeout_cb () - | _ -> - let fd_callbacks = get_fd_callbacks ready_inputs in - List.iter (fun cb -> cb ()) fd_callbacks - done - - diff --git a/sw/simulator/gps.ml b/sw/simulator/gps.ml index 1764c1cda0..11effa2391 100644 --- a/sw/simulator/gps.ml +++ b/sw/simulator/gps.ml @@ -1,3 +1,29 @@ +(* + * $Id$ + * + * Basic GPS parameters simulation + * + * 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 Stdlib open Latlong @@ -39,7 +65,7 @@ let state = fun lat0 lon0 alt0 -> { wgs84 = { posn_lat=lat;posn_long=long }; - alt = alt0 +. z; + alt = alt; time = t; climb = climb; gspeed = gspeed; diff --git a/sw/simulator/gps.mli b/sw/simulator/gps.mli new file mode 100644 index 0000000000..28a9881ce7 --- /dev/null +++ b/sw/simulator/gps.mli @@ -0,0 +1,38 @@ +(* + * $Id$ + * + * Basic GPS parameters simulation + * + * 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. + * + *) + +type state = { + wgs84 : Latlong.geographic; + alt : float; + time : float; + climb : float; + gspeed : float; + course : float; +} +val state : + float -> float -> float -> (float * float * float -> float -> state) +(** [state lat0 lon0 alt0] Returns a function which must be called with +an updated position [xyz] and time [t] *) diff --git a/sw/simulator/gui.ml b/sw/simulator/gui.ml deleted file mode 100644 index 4b51574c0b..0000000000 --- a/sw/simulator/gui.ml +++ /dev/null @@ -1,49 +0,0 @@ -(* ocamlc -w a -I +lablgtk2 lablgtk.cma lablgnomecanvas.cma gtkInit.cmo gui.ml -o gui.out *) - -let main () = - - (* window *) - let window = GWindow.window ~title: "Paparazzi simulator" - ~border_width: 5 ~width: 400 ~height: 200 () in - ignore (window#connect#destroy ~callback:GMain.quit); - - - let hb = GPack.hbox ~border_width:5 ~spacing:5 ~packing:window#add () in - - - (* wind *) - let frame_w = GBin.frame ~label:"Wind" ~shadow_type:`IN ~packing:hb#pack () in - - let vb_w = GPack.vbox ~packing:frame_w#add () in - - let adj_d = - GData.adjustment ~lower:0. ~upper:100. ~step_incr:1. ~page_incr:10. () in - let sc_d = GRange.scale `HORIZONTAL ~adjustment:adj_d ~draw_value:false - ~packing:vb_w#pack () in - - let adj_s = - GData.adjustment ~lower:0. ~upper:100. ~step_incr:1. ~page_incr:10. () in - let sc_s = GRange.scale `HORIZONTAL ~adjustment:adj_s ~draw_value:false - ~packing:vb_w#pack () in - - (* infrared *) - let frame_i = GBin.frame ~label:"Infrared" ~shadow_type:`IN ~packing:hb#pack () in - - let vb_i = GPack.vbox ~packing:frame_i#add () in - - let adj_i = - GData.adjustment ~lower:0. ~upper:100. ~step_incr:1. ~page_incr:10. () in - let sc_i = GRange.scale `HORIZONTAL ~adjustment:adj_i ~draw_value:false - ~packing:vb_i#pack () in - - let button = GButton.button ~use_mnemonic:true ~label:"_Coucou" ~packing:(hb#pack ~padding:5) () in - ignore(button#connect#clicked ~callback: - (fun () -> prerr_endline "Coucou")); - - - - window#show (); - GMain.Main.main () - -let _ = main () - diff --git a/sw/simulator/hitl.ml b/sw/simulator/hitl.ml index 76e6cac2cf..d5ebce0090 100644 --- a/sw/simulator/hitl.ml +++ b/sw/simulator/hitl.ml @@ -64,7 +64,7 @@ module Make(A:Data.MISSION) = struct ["ITOW",scale gps.time 1e3; "VEL_D", -scale gps.climb 1e2; "GSpeed", scale gps.gspeed 1e2; - "Heading", scale (deg gps.course) 1e5] + "Heading", scale (deg_of_rad gps.course) 1e5] diff --git a/sw/simulator/sirf.ml b/sw/simulator/sirf.ml deleted file mode 100644 index 89c6a5aaac..0000000000 --- a/sw/simulator/sirf.ml +++ /dev/null @@ -1,102 +0,0 @@ -open Types - - exception BadChecksum - exception BadEndSequence - - let rec skip_until_start_sequence = fun gps -> - while input_byte gps <> 0xA0 do () done; - if input_byte gps <> 0xA2 then skip_until_start_sequence gps - - let send_start_sequence = fun gps -> - output_byte gps 0xA0; - output_byte gps 0xA2 - - let get_end_sequence = fun gps -> - if input_byte gps <> 0xB0 || input_byte gps <> 0xB3 then - raise BadEndSequence - - let send_end_sequence = fun gps -> - output_byte gps 0xB0; - output_byte gps 0xB3 - - let checksum = fun data -> - let cs = ref 0 in - String.iter (fun c -> cs := (!cs + Char.code c) land 0x7fff) data; - !cs - - let receive = fun gps -> - let gps = Unix.in_channel_of_descr gps in - skip_until_start_sequence gps; - let length_h = input_byte gps in - let length_l = input_byte gps in - let length = (length_h lsl 8) lor length_l in - let payload = String.create length in - for i = 0 to length - 1 do - payload.[i] <- input_char gps; - done; - let checksum_h = input_byte gps in - let checksum_l = input_byte gps in - get_end_sequence gps; - if checksum payload <> (checksum_h lsl 8) lor checksum_l then - raise BadChecksum; - payload - - let output_2bytes = fun gps x -> - output_byte gps ((x land 0xff00) lsr 8); - output_byte gps (x land 0xff) - - let send = fun gps payload -> - let n = String.length payload in - assert(n < 1023); - send_start_sequence gps; - output_2bytes gps n; - String.iter (output_char gps) payload; - output_2bytes gps (checksum payload); - send_end_sequence gps; - flush gps - - -let send1 = fun gps c -> - Printf.printf "send 0x%2x\n" c; flush stdout - -let send_int32 = fun gps x -> - Printf.printf "send32 0x%8x\n" x; flush stdout - - -let get = fun gps n -> - let buf = String.create n in - buf.[0] <- Char.chr 0x79; - assert(input gps buf 5 (n-5) = n-5); - buf - -let log_info = Bytes [ "MID", 1; - "S_First", 1; "S_Last", 1; - "A_First", 4; "A_Last", 4; "A_Start", 4; - "Size", 4 ] -let log_data = Bytes [ "MID", 1; "Start", 4; "Data", 256*2 ] - -let extended_nav = Bytes [ - "MID", 1; - "Latitude", 4; "Longitude", 4; "Altitude", 4; - "Speed", 4; "ClimbRate", 4; "Course", 4; - "Mode", 1; - "Year", 2; "Month", 1; "Day", 1; "Hour", 1; "Minute", 1; - "Second", 2; - "GDOP", 1; "HDOP", 1; "PDOP", 1; "TDOP", 1; "VDOP", 1;] - - -let get_message = fun gps expected -> - let s = size_of_message expected in - let data = get gps s in - (expected, data, 0) - - -let log_poll_info = fun gps -> - send1 gps 0xbb; - get_message gps log_info - -let log_read = fun gps a -> - send1 gps 0xb8; - send_int32 gps a; - get_message gps log_data - diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index 8697921a6b..a33a77ed84 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -1,4 +1,28 @@ -(* Software in the loop *) +(* + * $Id$ + * + * Software 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 diff --git a/sw/simulator/stdlib.ml b/sw/simulator/stdlib.ml index 8b0f65bbf4..dd4ee954ee 100644 --- a/sw/simulator/stdlib.ml +++ b/sw/simulator/stdlib.ml @@ -1,3 +1,29 @@ +(* + * $Id$ + * + * Utilities for the simulators + * + * 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. + * + *) + type us = int let pi = 4. *. atan 1. @@ -6,7 +32,7 @@ let rec norm_angle = fun x -> else if x < -.pi then norm_angle (x+.2.*.pi) else x -let deg = fun rad -> rad /. pi *. 180. +let deg_of_rad = fun rad -> rad /. pi *. 180. let rad_of_deg = fun x -> x /. 180. *. pi diff --git a/sw/simulator/stdlib.mli b/sw/simulator/stdlib.mli new file mode 100644 index 0000000000..361bc31104 --- /dev/null +++ b/sw/simulator/stdlib.mli @@ -0,0 +1,33 @@ +(* + * $Id$ + * + * Utilities for the simulators + * + * 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. + * + *) + +type us = int +val pi : float +val norm_angle : float -> float +val deg_of_rad : float -> float +val rad_of_deg : float -> float +val set_float : string -> float ref -> string -> string * Arg.spec * string +val set_string : string -> string ref -> string -> string * Arg.spec * string diff --git a/sw/simulator/types.ml b/sw/simulator/types.ml deleted file mode 100644 index b0a2564ca9..0000000000 --- a/sw/simulator/types.ml +++ /dev/null @@ -1,116 +0,0 @@ -type size = int -type label = string -type data_layout = - Bits of (label * size) list - | Bytes of (label * size) list - - - -type record = data_layout * string * int -let get_record = fun record_layout string offset -> - (record_layout, string, offset) - - -(* Little endian *) -let make_int_from_bytes = fun data pos size -> - if size < 4 then - let rec mk = fun pos s i -> - if s = 0 then i else mk (pos+1) (s-1) ((i lsl 8) lor Char.code data.[pos]) in - mk pos size 0 - else if size = 4 then begin - let c = fun i -> Int32.shift_left (Int32.of_int (Char.code data.[pos+i])) (8*(3-i)) in - let lor32 = Int32.logor in - Int32.to_int (lor32 (c 0) (lor32 (c 1) (lor32 (c 2) (c 3)))) - - end else invalid_arg "make_int_from_bytes" - - -(* Little endian *) -let make_int_from_bits = fun data offset pos size -> - assert(pos < 8); - assert(size < 31); - let nb_bits_in_first_byte = min (8-pos) size in - let i = ((Char.code data.[offset] lsl pos) land 0xff) lsr (8-nb_bits_in_first_byte) in - let rec mk = fun offset s i -> - if s = 0 - then i - else if s < 8 - then (i lsl s) lor (Char.code data.[offset] lsr (8 - s)) - else mk (offset+1) (s-8) ((i lsl 8) lor Char.code data.[offset]) in - mk (offset+1) (size-nb_bits_in_first_byte) i - -let assoc = fun label layout -> - let rec assoc pos = function - [] -> failwith ("get_int: unknown field "^label) - | (l, s)::lss -> - if l = label - then (pos, s) - else assoc (pos + s) lss in - assoc 0 layout - - -let get_int = fun signed label (record_layout, data, offset) -> - match record_layout with - Bits l -> - let (pos, size) = assoc label l in - let i = - if pos mod 8 = 0 && size mod 8 = 0 then - let pos = pos / 8 and size = size / 8 in - make_int_from_bytes data (offset+pos) size - else - make_int_from_bits data (offset+pos/8) (pos mod 8) size in - if signed then (i lsl (31-size)) asr (31-size) else i - | Bytes l -> - let (pos, size) = assoc label l in - let i = make_int_from_bytes data (offset+pos) size in - if size < 4 then - if signed then (i lsl (31-4*size)) asr (31-4*size) else i - else begin - assert(not signed); - i - end - -let get_int32 = get_int true -let get_u32 = get_int false -let get_uint = get_int false -let get_int = get_int true - -let get_raw = fun label (record_layout, data, offset) -> - match record_layout with - Bytes layout -> - let (pos, size) = assoc label layout in - String.sub data (offset+pos) size - | _ -> failwith "get_raw" - - - -let sum_sizes = List.fold_left (fun a (_, s) -> a+s) 0 -let size_of_message = function (* In bytes *) - Bytes l -> sum_sizes l - | Bits l -> sum_sizes l / 8 - -let make_payload = fun layout values -> - match layout with - (Bytes layout) -> - let p = String.create (sum_sizes layout) in - List.iter - (fun (label, value) -> - let (pos, size) = assoc label layout in - let byte = fun x -> Char.chr (x land 0xff) in - match size with - 1 -> p.[pos] <- byte value - | 2 -> - p.[pos] <- byte (value asr 8); - p.[pos+1] <- byte value - | 4 -> - p.[pos] <- byte (value asr 24); - p.[pos+1] <- byte (value lsr 16); - p.[pos+2] <- byte (value lsr 8); - p.[pos+3] <- byte value - | _ -> failwith "make_payload: unknown int size" - ) - values; - p - | _ -> failwith "make_payload" - -