diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index 74b8899fe6..c5f3432f2c 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -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 diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml index 238af0cfa7..2170c8de9c 100644 --- a/sw/lib/ocaml/mapTrack.ml +++ b/sw/lib/ocaml/mapTrack.ml @@ -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