cam moves handling

2026-05-31 03:57:45 +08:00 · 2005-08-20 11:49:31 +00:00
parent 4729ed177f
commit d247cd1f5c
2 changed files with 134 additions and 7 deletions
@@ -25,7 +25,9 @@
 *)

 open Printf
+open Srtm
 open Latlong
+
 module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end)

 type color = string
@@ -137,13 +139,14 @@ let load_mission = fun color geomap url ->
  fp


-let aircraft_pos_msg = fun track utm_x utm_y heading ->
+let aircraft_pos_msg = fun track utm_x_ utm_y_ heading altitude ->
  match !map_ref with
    None -> ()
  | Some utm0 ->
-      let en =  {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in
+      let en =  {G.east = utm_x_ -. utm0.utm_x; north = utm_y_ -. utm0.utm_y } in
      track#add_point en;
-      track#move_icon en heading
+      let h = Srtm.of_utm { utm_zone = utm0.utm_zone; utm_x = utm_x_; utm_y = utm_y_} in
+      track#move_icon en heading altitude (float_of_int h)

 let carrot_pos_msg = fun track utm_x utm_y ->
  match !map_ref with
@@ -152,6 +155,14 @@ let carrot_pos_msg = fun track utm_x utm_y ->
      let en =  {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in
      track#move_carrot en

+let cam_pos_msg = fun track utm_x utm_y target_utm_x target_utm_y ->
+  match !map_ref with
+    None -> ()
+  | Some utm0 ->
+      let en =  {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in
+      let target_en =  {G.east = target_utm_x -. utm0.utm_x; north = target_utm_y -. utm0.utm_y } in  
+      track#move_cam en target_en
+
 let new_color =
  let colors = ref ["red"; "blue"; "green"] in
  fun () ->
@@ -213,7 +224,7 @@ let listen_flight_params = fun () ->
    try
      let ac = Hashtbl.find live_aircrafts ac_id in
      let a = fun s -> Pprz.float_assoc s vs in
-      aircraft_pos_msg ac.track (a "east") (a "north") (a "course")
+      aircraft_pos_msg ac.track (a "east") (a "north") (a "course") (a "alt")
    with Not_found -> ()
  in
  ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_fp);
@@ -226,7 +237,17 @@ let listen_flight_params = fun () ->
      carrot_pos_msg ac.track (a "target_east") (a "target_north") 
    with Not_found -> ()
  in
-  ignore (Ground_Pprz.message_bind "NAV_STATUS" get_ns)
+  ignore (Ground_Pprz.message_bind "NAV_STATUS" get_ns);
+
+  let get_cam_status = fun _sender vs ->
+    let ac_id = Pprz.string_assoc "ac_id" vs in
+    try
+      let ac = Hashtbl.find live_aircrafts ac_id in
+      let a = fun s -> Pprz.float_assoc s vs in
+      cam_pos_msg ac.track (a "cam_east") (a "cam_north") (a "target_east") (a "target_north") 
+    with Not_found -> ()
+  in
+  ignore (Ground_Pprz.message_bind "CAM_STATUS" get_cam_status)

 let _ =
  let ivy_bus = ref "127.255.255.255:2010"
@@ -244,6 +265,8 @@ let _ =

  Srtm.add_path default_path_SRTM;

+  Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
+
  let window = GWindow.window ~title: "Map2d" ~border_width:1 ~width:400 () in
  let vbox= GPack.vbox ~packing: window#add () in
  let quit = fun () -> GMain.Main.quit (); exit 0 in
@@ -25,15 +25,42 @@
 *)

 open Printf
+open Geometry_2d

 module G = MapCanvas

 let affine_pos_and_angle z xw yw angle =
-  let rad_angle = angle /. 180. *. acos (-1.) in
+  let rad_angle = angle /. 180. *. acos(-1.) in
  let cos_a = cos rad_angle in
  let sin_a = sin rad_angle in
  [| cos_a /. z ; sin_a /. z ; ~-. sin_a /. z; cos_a /. z; xw ; yw |]

+let rec norm_angle_360 = fun alpha ->
+  if alpha > 360.0 then norm_angle_360 (alpha -. 360.0)
+  else if alpha < 0.0 then norm_angle_360 (alpha +. 360.0)
+  else alpha
+
+(** for tests, coordinates of a fixed target: *)
+ 
+let fixed_cam_targeted_xw = 500.0
+let fixed_cam_targeted_yw = 500.0
+
+(** variables used for handling cam moves: *)
+
+let cam_half_aperture = m_pi /. 4.0
+let cam_field_half_width = ref 0.0
+let cam_field_half_height_1 = ref 0.0
+let cam_field_half_height_2 = ref 0.0
+let cam_heading = ref 0.0
+let angle_of_view = ref 0.0
+let oblic_distance = ref 0.0
+let max_cam_half_height = 10000.0
+let max_oblic_distance = 10000.0
+let min_distance = 0.1
+let min_height = 0.1
+
+let half_pi = m_pi /. 2.0
+
 class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas.widget) ->
  let group = GnoCanvas.group geomap#canvas#root in
  let empty = ({ G.east = 0.; north = 0. },  GnoCanvas.line group) in
@@ -49,11 +76,31 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
  let carrot = GnoCanvas.group group in
  let _ac_carrot =
    ignore (GnoCanvas.polygon ~points:[|0.;0.;-5.;-10.;5.;-10.|] ~props:[`WIDTH_UNITS 1.;`FILL_COLOR "orange"; `OUTLINE_COLOR "orange"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] carrot) in
+
+  let cam = GnoCanvas.group group in
+  let ac_cam_cover = ref ( GnoCanvas.rect ~x1:(-. !cam_field_half_width) ~y1: (-. !cam_field_half_height_2) ~x2:(!cam_field_half_width) ~y2:(!cam_field_half_height_1) ~fill_color:"none" ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "brown"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam) in
+
+  let cam_targeted = GnoCanvas.group group in
+
+(** rectangle representing the field covered by the cam *)
+  let _ac_cam_targeted =
+    ignore ( GnoCanvas.ellipse ~x1: (-. 2.5) ~y1: (-. 2.5) ~x2: 2.5 ~y2: 2.5 ~fill_color:"blue" ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "blue"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam_targeted ) in
+
+  let mission_target = GnoCanvas.group group in
+
+(** red circle : target of the mission *)
+  let ac_mission_target =
+    ignore ( GnoCanvas.ellipse ~x1: (-5.) ~y1: (-5.) ~x2: 5. ~y2: 5. ~fill_color:"red" ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "red"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] mission_target ) in 
  
  object (self)
    val mutable segments = Array.create size empty
    val mutable top = 0
    val mutable last = None
+    val mutable last_heading = 0.0
+    val mutable last_altitude = 0.0
+    val mutable last_height = 0.0
+    val mutable last_xw = 0.0
+    val mutable last_yw = 0.0
    method track = track
    method aircraft = aircraft
    method clear_one = fun i ->
@@ -80,13 +127,70 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
      end;
      self#incr;
      last <- Some en
-    method move_icon = fun en heading ->
+    method move_icon = fun en heading altitude relief_height ->
      let (xw,yw) = geomap#world_of_en en in
      aircraft#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw heading);
+      last_heading <- heading;
+      last_altitude <- altitude;
+      last_xw <- xw;
+      last_yw <- yw;
+      last_height <- altitude -. relief_height;
      ac_label#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.);
    method move_carrot = fun en ->
      let (xw,yw) = geomap#world_of_en en in
      carrot#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.)
+
+
+(** moves the rectangle representing the field covered by the camera *)
+    method move_cam = fun en mission_target_en ->
+      let (xw,yw) = geomap#world_of_en en in 
+      let (mission_target_xw, mission_target_yw) = geomap#world_of_en mission_target_en in
+      begin
+	(!ac_cam_cover)#destroy ();
+	let pt1 = { x2D = last_xw; y2D = last_yw} in
+	let pt2 = { x2D = xw ; y2D = yw } in
+
+	(* y axis is downwards so North vector is as follows *)
+
+	let vect_north = (vect_make  { x2D = 0.0 ; y2D = 0.0 } { x2D = 0.0 ; y2D = -. 1.0 } ) in
+	let d = distance pt1 pt2 in
+	begin
+	  if d > min_distance then
+	    let cam_vect_normalized = (vect_normalize (vect_make pt1 pt2)) in
+	      if (dot_product vect_north cam_vect_normalized) > 0.0 then
+	    cam_heading := norm_angle_360 ( rad2deg (asin (cross_product vect_north cam_vect_normalized)))
+	      else cam_heading := norm_angle_360 ( rad2deg (m_pi -. asin (cross_product vect_north cam_vect_normalized)))
+	  else cam_heading := last_heading;
+	  if last_height < min_height then 
+	    begin
+	      angle_of_view := half_pi;
+	      oblic_distance := max_oblic_distance
+	    end
+	  else 
+	    begin
+	      angle_of_view := (atan ( d /. last_height) );
+	      oblic_distance := last_height /. (cos !angle_of_view)
+	    end;
+	  let alpha_1 = !angle_of_view +. cam_half_aperture in
+	  let alpha_2 = !angle_of_view -. cam_half_aperture in
+	  begin 
+	    if alpha_1 < half_pi then
+	      cam_field_half_height_1 := (tan alpha_1) *. last_height -. d
+	    else cam_field_half_height_1 := max_cam_half_height;
+	    cam_field_half_height_2 := d -. (tan ( !angle_of_view -. cam_half_aperture)) *. last_height;
+	    cam_field_half_width := ( tan (cam_half_aperture) ) *. !oblic_distance;
+	      
+(***	  Printf.printf "dist %.2f aoview %.2f oblic_distance %.2f cfh1 %.2f cfh2 %.2f cfhw %.2f last_xw %.2f last_yw %.2f cam_heading %.2f \n " d !angle_of_view !oblic_distance !cam_field_half_height_1 !cam_field_half_height_2 !cam_field_half_width last_xw last_yw !cam_heading; 
+   flush stdout;   ***)
+  
+	    ac_cam_cover := GnoCanvas.rect ~x1:( !cam_field_half_width) ~y1:( !cam_field_half_height_2) ~x2:(-. !cam_field_half_width) ~y2:(-. !cam_field_half_height_1) ~fill_color:"grey" ~props:[`WIDTH_UNITS 0.5 ; `OUTLINE_COLOR "blue"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam;
+	  end;
+	  cam#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw !cam_heading);
+	  cam_targeted#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.0);
+	  mission_target#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value mission_target_xw mission_target_yw 0.0)
+	end;
+      end
+
    method zoom = fun z ->
      let a = aircraft#i2w_affine in
      let z' = sqrt (a.(0)*.a.(0)+.a.(1)*.a.(1)) in