cam display fixed

sw/ground_segment/tmtc/Makefile

@@ -78,7 +78,7 @@ modem.cmo : modem.cmi
 %.cmi : %.mli
 	$(OCAMLC) $(INCLUDES) $<
 
-aircraft.cmo : aircraft.cmi
+airprox.cmi aircraft.cmo : aircraft.cmi
 airprox.cmo : airprox.cmi
 
 wavecard_c : wavecard_c.c wavecard_utils_c.c wavecard_foo_c.c

sw/ground_segment/tmtc/aircraft.ml

@@ -25,6 +25,7 @@
 type ac_cam = {
     mutable phi : float; (* Rad, right = >0 *)
     mutable theta : float; (* Rad, front = >0 *)
+    mutable target : (float * float) (* meter*meter relative *)
   }
 
 type inflight_calib = {

sw/ground_segment/tmtc/aircraft.mli

@@ -24,7 +24,12 @@
 
 (** State of an A/C handled by the server *)
 
-type ac_cam = { mutable phi : float; mutable theta : float; }
+type ac_cam = {
+    mutable phi : float; (* Rad, right = >0 *)
+    mutable theta : float; (* Rad, front = >0 *)
+    mutable target : (float * float) (* meter*meter relative *)
+  }
+
 type inflight_calib = {
   mutable if_mode : int;
   mutable if_val1 : float;

sw/ground_segment/tmtc/server.ml

@@ -218,6 +218,7 @@ let log_and_parse = fun logging ac_name a msg values ->
     | "CAM" ->
 	a.cam.phi <- (Deg>>Rad) (fvalue  "phi");
 	a.cam.theta <- (Deg>>Rad) (fvalue  "theta");
+	a.cam.target <- (fvalue  "target_x", fvalue  "target_y")
     | "RAD_OF_IR" ->
 	a.infrared.gps_hybrid_factor <- fvalue "rad_of_ir"
     | "CALIB_START" ->
@@ -273,18 +274,25 @@ let ac_msg = fun log ac_name a m ->
 
 let send_cam_status = fun a ->
   if a.gps_mode = gps_mode_3D then
-    let h = a.alt -. float (Srtm.of_utm a.pos) in
-    let dx = h *. tan (a.cam.phi -. a.roll)
-    and dy = h *. tan (a.cam.theta +. a.pitch) in
-    let alpha = -. a.course in
-    let east = dx *. cos alpha -. dy *. sin alpha
-    and north = dx *. sin alpha +. dy *. cos alpha in
-    let utm = Latlong.utm_add a.pos (east, north) in
-    let wgs84 = Latlong.of_utm WGS84 utm in
-    let values = ["ac_id", Pprz.String a.id; 
-		  "cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
-		  "cam_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long)] in
-    Ground_Pprz.message_send my_id "CAM_STATUS" values
+    match a.nav_ref with
+      None -> () (* No geo ref for camera target *)
+    | Some nav_ref ->
+	let h = a.alt -. float (Srtm.of_utm a.pos) in
+	let dx = h *. tan (a.cam.phi -. a.roll)
+	and dy = h *. tan (a.cam.theta +. a.pitch) in
+	let alpha = -. a.course in
+	let east = dx *. cos alpha -. dy *. sin alpha
+	and north = dx *. sin alpha +. dy *. cos alpha in
+	let utm = Latlong.utm_add a.pos (east, north) in
+	let wgs84 = Latlong.of_utm WGS84 utm in
+	let utm_target = Latlong.utm_add nav_ref a.cam.target in
+	let twgs84 =  Latlong.of_utm WGS84 utm_target in
+	let values = ["ac_id", Pprz.String a.id; 
+		      "cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
+		      "cam_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); 
+		      "cam_target_lat", Pprz.Float ((Rad>>Deg)twgs84.posn_lat);
+		      "cam_target_long", Pprz.Float ((Rad>>Deg)twgs84.posn_long)] in
+	Ground_Pprz.message_send my_id "CAM_STATUS" values
 
 let send_if_calib = fun a ->
   let if_mode = get_indexed_value if_modes a.inflight_calib.if_mode in
@@ -452,7 +460,7 @@ let new_aircraft = fun id ->
       desired_climb = 0.;
       pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
       nav_ref = None;
-      cam = { phi = 0.; theta = 0. };
+      cam = { phi = 0.; theta = 0. ; target=(0.,0.)};
       inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
       infrared = infrared_init;
       fbw = { rc_status = "???"; rc_mode = "???" };

sw/lib/ocaml/mapTrack.ml

@@ -121,7 +121,9 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva
 	self#clear_one i seg
       done;
       top := 0
-    method set_cam_state = fun b -> cam_on <- b
+    method set_cam_state = fun b ->
+      cam_on <- b;
+      if b then cam#show () else cam#hide ()
 
     method update_ap_status = fun time -> 
       last_flight_time <- time
@@ -177,9 +179,7 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva
 	  
 (** moves the rectangle representing the field covered by the camera *)
     method move_cam = fun wgs84 mission_target_wgs84 ->
-      if not cam_on then
-	cam#hide ()
-      else
+      if cam_on then
 	let (xw,yw) = geomap#world_of wgs84 in 
 	let (mission_target_xw, mission_target_yw) = geomap#world_of mission_target_wgs84 in
 	let last_height_scaled = last_height in