GPS OFF added in sim

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

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)
 
 

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

sw/simulator/flightModel.ml

@@ -60,27 +60,8 @@ 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
@@ -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;  **)
+	  let left = - sign_aileron_left * (servo.(no_aileron_left) - n_delta_a) in
-	  state.delta_a <- c_lda *. float (- sign_aileron_left * (servo.(no_aileron_left) - n_delta_a));
+	(** if left <> 0 then Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout;  **)
-	  do_thrust state servo
+	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 *)
 

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;
+      "wind_north", Pprz.Float wind_north;
-	       "ir_contrast", Pprz.Float infrared_contrast_adj#value;
+      "ir_contrast", Pprz.Float infrared_contrast_adj#value;
-	       "time_scale", Pprz.Float time_scale#value ] in
+      "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;
 

sw/simulator/gps.ml

@@ -28,6 +28,7 @@ open Stdlib
 open Latlong
 
 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;
    }
       

sw/simulator/gps.mli

@@ -25,13 +25,14 @@
  *)
 
 type state = {
-  wgs84 : Latlong.geographic;
+    mutable availability : bool;
-  alt : float;
+    wgs84 : Latlong.geographic;
-  time : float;
+    alt : float;
-  climb : float;
+    time : float;
-  gspeed : float;
+    climb : float;
-  course : float; (** North = 0 *)
+    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

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
 

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;
 

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) {
+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 = 3;
+  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);
+  assert(argn == 9);
-  return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7]);
+  return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7], a[8]);
 }

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 = []