diff --git a/sw/ground_segment/tmtc/boa b/sw/ground_segment/tmtc/boa index ca6782c3e2..75446c3443 100755 --- a/sw/ground_segment/tmtc/boa +++ b/sw/ground_segment/tmtc/boa @@ -2,4 +2,5 @@ if test -z "$PAPARAZZI_SRC"; then PAPARAZZI_SRC=/usr/share/paparazzi fi +mkdir -p $PAPARAZZI_HOME/var/logs /usr/sbin/boa -d -f $PAPARAZZI_HOME/var/boa.conf diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 710c0def93..5644aadaaa 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -125,6 +125,11 @@ let float_assoc = fun (a:string) vs -> Float x -> x | _ -> invalid_arg "Pprz.float_assoc" +let int_assoc = fun (a:string) vs -> + match List.assoc a vs with + Int x -> x + | _ -> invalid_arg "Pprz.int_assoc" + let string_assoc = fun (a:string) (vs:values) -> string_of_value (List.assoc a vs) diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index a375f863eb..41848d12e9 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -51,6 +51,7 @@ val string_assoc : string -> values -> string (** May raise Not_found *) val float_assoc : string -> values -> float +val int_assoc : string -> values -> int (** May raise Not_found or Invalid_argument *) exception Unknown_msg_name of string diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml index f7373cbbfb..460f2c11e2 100644 --- a/sw/simulator/flightModel.ml +++ b/sw/simulator/flightModel.ml @@ -60,28 +60,9 @@ let drag = 0.45 let c_lp = -10. let g = 9.81 -let max_phi = 0.7 (* rad *) -let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x - - - module Make(A:Data.MISSION) = struct open Data -(* Minimum complexity *) -(* - http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf - http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf - *) - - - let servos = - try - ExtXml.child A.ac.airframe "servos" - with - Not_found -> - failwith (Printf.sprintf "Child 'servos' expected in '%s'\n" (Xml.to_string A.ac.airframe)) - let section = fun name -> try ExtXml.child A.ac.airframe ~select:(fun x -> ExtXml.attrib x "name" = name) "section" @@ -89,9 +70,6 @@ let section = fun name -> Not_found -> failwith (Printf.sprintf "Child 'section' with 'name=%s' expected in '%s'\n" name (Xml.to_string A.ac.airframe)) -let misc_section = section "MISC" - -let infrared_section = section "INFRARED" let simu_section = section "SIMU" @@ -100,7 +78,7 @@ let defined_value = fun sect name -> (Xml.attrib (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value") with Not_found -> - failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string misc_section)) + failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect)) let float_value = fun section s -> float_of_string (defined_value section s) @@ -110,6 +88,16 @@ let yaw_response_factor = float_value simu_section "YAW_RESPONSE_FACTOR" let weight = float_value simu_section "WEIGHT" +let max_phi = 0.7 (* rad *) +let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x + + + +(* Minimum complexity *) +(* + http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf + http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf + *) let state_update = fun state (wx, wy) dt -> let now = state.t +. dt in if state.air_speed > 0. then begin @@ -130,6 +118,19 @@ let weight = float_value simu_section "WEIGHT" + + let servos = + try + ExtXml.child A.ac.airframe "servos" + with + Not_found -> + failwith (Printf.sprintf "Child 'servos' expected in '%s'\n" (Xml.to_string A.ac.airframe)) + +let misc_section = section "MISC" + +let infrared_section = section "INFRARED" + + let nominal_airspeed = float_of_string (defined_value misc_section "NOMINAL_AIRSPEED") let ir_roll_neutral = int_of_string (defined_value infrared_section "ROLL_NEUTRAL_DEFAULT") @@ -143,7 +144,7 @@ let ir_roll_neutral = int_of_string (defined_value infrared_section "ROLL_NEUTRA let us_attrib = fun x a -> int_of_string (ExtXml.attrib x a) - let gaz = get_servo "GAZ" + let gaz = try get_servo "GAZ" with _ -> get_servo "MOTOR_RIGHT" let min_thrust = us_attrib gaz "min" let max_thrust = us_attrib gaz "max" @@ -176,9 +177,10 @@ let ir_roll_neutral = int_of_string (defined_value infrared_section "ROLL_NEUTRA and n_delta_a = us_attrib aileron_left "neutral" and no_aileron_left = int_attrib aileron_left "no" in fun state servo -> - (** Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout; **) - state.delta_a <- c_lda *. float (- sign_aileron_left * (servo.(no_aileron_left) - n_delta_a)); - do_thrust state servo + let left = - sign_aileron_left * (servo.(no_aileron_left) - n_delta_a) in + (** if left <> 0 then Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout; **) + state.delta_a <- c_lda *. float left; + do_thrust state servo | None, Some ailevon_left, Some ailevon_right -> let c_lda = 2.5e-4 in (* phi_dot_dot from aileron *) diff --git a/sw/simulator/gaia.ml b/sw/simulator/gaia.ml index 116d2e86ff..9aa6d3a540 100644 --- a/sw/simulator/gaia.ml +++ b/sw/simulator/gaia.ml @@ -28,6 +28,7 @@ open Printf open Latlong let my_id = "gaia" +let sending_period = 5000 (* ms *) module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) @@ -51,15 +52,18 @@ let _ = let wind_speed_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in let gust_norm_max_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in let infrared_contrast_adj = GData.adjustment ~value:500. ~lower:(0.) ~upper:1010. ~step_incr:10. () in + let gps_sa = GButton.toggle_button ~label:"GPS OFF" () in let world_values = fun () -> let wind_dir_rad = Latlong.pi /. 2. -. (Deg>>Rad) wind_dir_adj#value in let wind_east = wind_speed_adj#value *. cos wind_dir_rad and wind_north = wind_speed_adj#value *. sin wind_dir_rad in [ "wind_east", Pprz.Float wind_east; - "wind_north", Pprz.Float wind_north; - "ir_contrast", Pprz.Float infrared_contrast_adj#value; - "time_scale", Pprz.Float time_scale#value ] in + "wind_north", Pprz.Float wind_north; + "ir_contrast", Pprz.Float infrared_contrast_adj#value; + "time_scale", Pprz.Float time_scale#value; + "gps_availability", Pprz.Int (if gps_sa#active then 0 else 1) + ] in let world_send = fun () -> Ground_Pprz.message_send my_id "WORLD_ENV" (world_values ()) in @@ -67,6 +71,9 @@ let _ = (fun (a:GData.adjustment) -> ignore (a#connect#value_changed world_send)) [time_scale; wind_dir_adj; wind_speed_adj; gust_norm_max_adj; infrared_contrast_adj]; + ignore (gps_sa#connect#toggled world_send); + + ignore (Glib.Timeout.add sending_period (fun () -> world_send (); true)); let vbox = GPack.vbox ~packing:window#add () in @@ -92,6 +99,8 @@ let _ = ignore (GMisc.label ~text:"infrared:" ~packing:hbox#pack ()); ignore (GRange.scale `HORIZONTAL ~adjustment:infrared_contrast_adj ~packing:hbox#add ()); + vbox#pack gps_sa#coerce; + Ivy.init "Paparazzi gaia" "READY" (fun _ _ -> ()); Ivy.start !ivy_bus; diff --git a/sw/simulator/gps.ml b/sw/simulator/gps.ml index 11effa2391..4e0dbe9c1d 100644 --- a/sw/simulator/gps.ml +++ b/sw/simulator/gps.ml @@ -27,7 +27,8 @@ open Stdlib open Latlong -type state = { +type state = { + mutable availability : bool; wgs84 : Latlong.geographic; alt : float; time : float; @@ -69,6 +70,7 @@ let state = fun lat0 lon0 alt0 -> time = t; climb = climb; gspeed = gspeed; - course = course + course = course; + availability = true; } diff --git a/sw/simulator/gps.mli b/sw/simulator/gps.mli index c559448b03..bfbe18f9dc 100644 --- a/sw/simulator/gps.mli +++ b/sw/simulator/gps.mli @@ -25,13 +25,14 @@ *) type state = { - wgs84 : Latlong.geographic; - alt : float; - time : float; - climb : float; - gspeed : float; - course : float; (** North = 0 *) -} + mutable availability : bool; + wgs84 : Latlong.geographic; + alt : float; + time : float; + climb : float; + gspeed : float; + course : float; (** North = 0 *) + } val state : float -> float -> float -> (float * float * float -> float -> state) (** [state lat0 lon0 alt0] Returns a function which must be called with diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml index 1a68f33ea3..b7742da1eb 100644 --- a/sw/simulator/sim.ml +++ b/sw/simulator/sim.ml @@ -116,9 +116,11 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct let wind_x = ref 0. and wind_y = ref 0. in let infrared_contrast = ref 2000. - and time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end in + 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 := 4. *. Pprz.float_assoc "ir_contrast" vs; (** FIXME *) @@ -159,6 +161,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct 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 diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c index 2d00be98b4..2a42254d18 100644 --- a/sw/simulator/sim_ap.c +++ b/sw/simulator/sim_ap.c @@ -94,6 +94,10 @@ value update_rc_channel(value c, value v) { #define SERVO_MAX (SERVO_MAX_US) #define ChopServo(x) ((x) < SERVO_MIN ? SERVO_MIN : ((x) > SERVO_MAX ? SERVO_MAX : (x))) +#ifdef SERVO_MOTOR_RIGHT +#define SERVO_GAZ SERVO_MOTOR_RIGHT +#endif + value set_servos(value servos) { int i; diff --git a/sw/simulator/sim_gps.c b/sw/simulator/sim_gps.c index 34ce59e505..67cef7a5d2 100644 --- a/sw/simulator/sim_gps.c +++ b/sw/simulator/sim_gps.c @@ -23,8 +23,8 @@ uint8_t gps_utm_zone; struct svinfo gps_svinfos[NB_CHANNELS]; uint8_t gps_nb_channels = 0; -value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t) { - gps_mode = 3; +value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m) { + gps_mode = (Bool_val(m) ? 3 : 0); gps_utm_east = Int_val(x); gps_utm_north = Int_val(y); gps_utm_zone = Int_val(z); @@ -54,6 +54,6 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu /* Second binding required because number of args > 5 */ value sim_use_gps_pos_bytecode(value *a, int argn) { - assert(argn == 8); - return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7]); + assert(argn == 9); + return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7], a[8]); } diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index d4ef6d3a6d..1eccd51a9b 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -35,11 +35,6 @@ module Make(A:Data.MISSION) = struct let periodic_period = 1./.61. (* s *) let rc_period = 1./.40. (* s *) - let periodic = fun p f -> - f (); - ignore (GMain.Timeout.add p (fun () -> f (); true)) - - let msg = fun name -> ExtXml.child Data.messages_ap ~select:(fun x -> ExtXml.attrib x "name" = name) "message" let gps_msg = msg "GPS" @@ -110,7 +105,7 @@ module Make(A:Data.MISSION) = struct incr t; if on_off#active then t := 0; set_really_lost (!t > 2) in - periodic 1000 monitor_on_off; + Stdlib.timer 1. monitor_on_off; (** FIXME: should use time_scale *) window#show () external periodic_task : unit -> unit = "sim_periodic_task" @@ -146,12 +141,12 @@ module Make(A:Data.MISSION) = struct let infrared = fun ir_left ir_front -> set_ir_roll (truncate ir_left) - external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos" + external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos" open Latlong let gps = fun gps -> let utm = utm_of WGS84 gps.Gps.wgs84 in let cm = fun f -> truncate (f *. 100.) in - use_gps_pos (cm utm.utm_x) (cm utm.utm_y) utm.utm_zone gps.Gps.course gps.Gps.alt gps.Gps.gspeed gps.Gps.climb gps.Gps.time + use_gps_pos (cm utm.utm_x) (cm utm.utm_y) utm.utm_zone gps.Gps.course gps.Gps.alt gps.Gps.gspeed gps.Gps.climb gps.Gps.time gps.Gps.availability end let options = []