Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Cleaning (values, files), interfaces

Browse Source
This commit is contained in:
Pascal Brisset
2005-03-10 11:27:28 +00:00
parent e2c967506e
commit c581e6ad7b
13 changed files with 221 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/Makefile
+4 -1
View File
@@ -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
sw/simulator/data.ml
+27 -4
View File
@@ -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
sw/simulator/data.mli
+39
View File
@@ -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
sw/simulator/events.ml
-105
View File
@@ -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
sw/simulator/gps.ml
+27 -1
View File
@@ -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;
sw/simulator/gps.mli
+38
View File
@@ -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] *)
sw/simulator/gui.ml
-49
View File
@@ -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 ()
sw/simulator/hitl.ml
+1 -1
View File
@@ -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]
sw/simulator/sirf.ml
-102
View File
@@ -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
sw/simulator/sitl.ml
+25 -1
View File
@@ -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
sw/simulator/stdlib.ml
+27 -1
View File
@@ -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
sw/simulator/stdlib.mli
+33
View File
@@ -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
sw/simulator/types.ml
-116
View File
@@ -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"