Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

cam rectangle scale bug corrected. circle path and segment path displayed.

Browse Source
This commit is contained in:
rad
2005-08-24 09:07:44 +00:00
parent 9ab6365c68
commit a1055a1d97
3 changed files with 154 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/cockpit/map2d.ml
+58 -4
View File
@@ -43,8 +43,6 @@ let default_path_SRTM = home // "data" // "SRTM"
let default_path_maps = home // "data" // "maps" // ""
let default_path_missions = home // "conf"
type aircraft = {
track : MapTrack.track;
color: color;
@@ -158,6 +156,31 @@ let cam_pos_msg = fun track utm_x utm_y target_utm_x target_utm_y ->
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 circle_status_msg = fun track utm_x utm_y radius ->
match !map_ref with
None -> ()
| Some utm0 ->
let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in
track#draw_circle en radius
let segment_status_msg = fun track utm_x utm_y utm2_x utm2_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 en2 = {G.east = utm2_x -. utm0.utm_x; north = utm2_y -. utm0.utm_y } in
track#draw_segment en en2
let circle_status_msg = fun track utm_x utm_y radius ->
match !map_ref with
None -> ()
| Some utm0 ->
let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in
track#draw_circle en radius
let ap_status_msg = fun track horizontal_mode ->
track#update_ap_status horizontal_mode
let new_color =
let colors = ref ["red"; "blue"; "green"] in
fun () ->
@@ -203,6 +226,7 @@ let one_new_ac = fun (geomap:MapCanvas.widget) ac ->
let track = new MapTrack.track ~name:ac ~color:color geomap in
ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear));
ignore (ac_menu_fact#add_item "Resize Track" ~callback:(fun () -> resize_track ac track));
ignore (ac_menu_fact#add_item "Switch On-Off Cam Display" ~callback:(fun () -> track#switch_cam_state));
Hashtbl.add live_aircrafts ac { track = track; color = color; fp_group = None }
end
@@ -239,10 +263,40 @@ 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
cam_pos_msg ac.track (a "cam_east") (a "cam_north") (a "target_east") (a "target_north")
if (ac.track)#cam_state_on then 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 get_circle_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
circle_status_msg ac.track (a "circle_east") (a "circle_north") (int_of_string (Pprz.string_assoc "radius" vs))
with Not_found -> ()
in
ignore (Ground_Pprz.message_bind "CAM_STATUS" get_cam_status)
ignore (Ground_Pprz.message_bind "CIRCLE_STATUS" get_circle_status);
let get_segment_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
segment_status_msg ac.track (a "segment1_east") (a "segment1_north") (a "segment2_east") (a "segment2_north")
with Not_found -> ()
in
ignore (Ground_Pprz.message_bind "SEGMENT_STATUS" get_segment_status);
let get_ap_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.string_assoc s vs in
ap_status_msg ac.track (a "horiz_mode")
with Not_found -> ()
in
ignore (Ground_Pprz.message_bind "AP_STATUS" get_ap_status)
let _ =
let ivy_bus = ref "127.255.255.255:2010"
sw/lib/ocaml/mapCanvas.ml
+5 -1
View File
@@ -62,6 +62,8 @@ class widget = fun ?(height=800) ?width ?wgs84_of_en () ->
method set_world_unit = fun x -> world_unit <- x
method get_world_unit = fun () -> world_unit
method current_zoom = current_zoom
method canvas = canvas
@@ -73,7 +75,9 @@ class widget = fun ?(height=800) ?width ?wgs84_of_en () ->
method factory = factory
method world_of_en = fun en -> en.east /. world_unit, -. en.north /. world_unit
method world_of_en = fun en ->
en.east /. world_unit, -. en.north /. world_unit
method en_of_world = fun wx wy -> { east = wx *. world_unit; north = -. wy *. world_unit }
method geo_string = fun en ->
sw/lib/ocaml/mapTrack.ml
+91 -42
View File
@@ -48,16 +48,8 @@ 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 max_cam_half_height = 10000.0
let max_oblic_distance = 10000.0
let min_distance = 10.
let min_height = 0.1
let half_pi = m_pi /. 2.0
let sqrt_2_div_2 = sqrt 2.0
class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas.widget) ->
let group = GnoCanvas.group geomap#canvas#root in
@@ -74,21 +66,35 @@ 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_cover = ref ( GnoCanvas.rect cam) in
let cam_targeted = GnoCanvas.group group in
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
ignore ( GnoCanvas.ellipse ~x1: (-. 2.5) ~y1: (-. 2.5) ~x2: 2.5 ~y2: 2.5 ~fill_color:color ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR color; `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
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
let circle_mode = GnoCanvas.group group in
(** green circle: circle to be followed by the aircraft in circle mode *)
let circle_mode_icon = ref ( GnoCanvas.ellipse mission_target ) in
let segment_mode = GnoCanvas.group group in
(** green segment: line to be followed by the aircraft between two waypoints *)
let segment_mode_icon = ref (GnoCanvas.line segment_mode) in
(** data at map scale *)
let max_cam_half_height = 10000.0 /. (geomap#get_world_unit ()) in
let max_oblic_distance = 10000.0 /. (geomap#get_world_unit ()) in
let min_distance = 10. /. (geomap#get_world_unit ()) in
let min_height = 0.1 /. (geomap#get_world_unit ()) in
object (self)
val mutable segments = Array.create size empty
@@ -99,6 +105,10 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
val mutable last_height = 0.0
val mutable last_xw = 0.0
val mutable last_yw = 0.0
val mutable cam_on = true
val mutable previous_cam_state_on = true
val mutable previous_horizontal_mode = "WAYPOINT"
val mutable current_horizontal_mode = "WAYPOINT"
method track = track
method aircraft = aircraft
method clear_one = fun i ->
@@ -114,6 +124,12 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
self#clear_one i
done;
top <- 0
method switch_cam_state =
if cam_on then
(!ac_cam_cover)#destroy ();
previous_cam_state_on <- cam_on;
cam_on <- not cam_on
method cam_state_on = cam_on
method add_point = fun en ->
self#clear_one top;
begin
@@ -121,7 +137,7 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
None ->
segments.(top) <- (en, geomap#segment ~group:track ~fill_color:color en en)
| Some last ->
segments.(top) <- (en, geomap#segment ~group:track ~width:2 ~fill_color:color last en)
segments.(top) <- (en, geomap#segment ~group:track ~width:2 ~fill_color:color last en);
end;
self#incr;
last <- Some en
@@ -132,33 +148,64 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
last_altitude <- altitude;
last_xw <- xw;
last_yw <- yw;
last_height <- altitude -. relief_height;
last_height <- (altitude -. relief_height) /. (geomap#get_world_unit ());
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.)
(** updates the autopilot status *)
method update_ap_status = fun horizontal_mode ->
current_horizontal_mode <- horizontal_mode
(** draws the circular path to be followed by the aircraft in circle mode *)
method draw_circle = fun en radius ->
if ( current_horizontal_mode = "CIRCLE" || (***) current_horizontal_mode = "42" (***)) then
let fradius = (float_of_int radius) /. (geomap#get_world_unit ()) in
let (xw,yw) = geomap#world_of_en en in
begin
if previous_horizontal_mode ="CIRCLE" then (!circle_mode_icon)#destroy ()
else
if previous_horizontal_mode ="SEGMENT" then (!segment_mode_icon)#destroy ();
circle_mode_icon := ( GnoCanvas.ellipse ~x1: ( -.fradius ) ~y1: (-. fradius) ~x2: (fradius ) ~y2: (fradius) ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "green"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] circle_mode);
circle_mode#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.);
previous_horizontal_mode <- "CIRCLE";
end
(** draws the linear path to be followed by the aircraft between two waypoints *)
method draw_segment = fun en en2 ->
if ( current_horizontal_mode = "SEGMENT" || (***) current_horizontal_mode = "42" (***)) then
begin
if previous_horizontal_mode ="CIRCLE" then (!circle_mode_icon)#destroy ()
else
if previous_horizontal_mode = "SEGMENT" then (!segment_mode_icon)#destroy ();
segment_mode_icon := geomap#segment ~group:segment_mode ~fill_color:"green" en en2;
previous_horizontal_mode <- "SEGMENT"
end
(** 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
(** all data are at map scale *)
begin
(!ac_cam_cover)#destroy ();
if previous_cam_state_on then (!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
(** 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
let cam_heading =
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;
norm_angle_360 ( rad2deg (asin (cross_product vect_north cam_vect_normalized)))
else norm_angle_360 ( rad2deg (m_pi -. asin (cross_product vect_north cam_vect_normalized)))
else last_heading in
let (angle_of_view, oblic_distance) =
if last_height < min_height then
(half_pi, max_oblic_distance)
@@ -168,24 +215,26 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
in
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;
begin
let cam_field_half_height_1 =
if alpha_1 < half_pi then
(tan alpha_1) *. last_height -. d
else max_cam_half_height in
let cam_field_half_height_2 = d -. (tan ( angle_of_view -. cam_half_aperture)) *. last_height in
let cam_field_half_width = ( tan (cam_half_aperture) ) *. oblic_distance in
begin
(*** 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 *. (geomap#get_world_unit ()) ) angle_of_view (oblic_distance *. (geomap#get_world_unit ()) ) (cam_field_half_height_1 *. (geomap#get_world_unit ()) ) (cam_field_half_height_2 *. (geomap#get_world_unit ()) ) (cam_field_half_width *. (geomap#get_world_unit ()) ) last_xw last_yw cam_heading; ***)
(*** 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;
ac_cam_cover := GnoCanvas.rect ~x1:(-. cam_field_half_width) ~y1:(-. cam_field_half_height_1) ~x2:(cam_field_half_width) ~y2:(cam_field_half_height_2) ~fill_color:"grey" ~props:[`WIDTH_UNITS 1. ; `OUTLINE_COLOR color; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam;
previous_cam_state_on <- true
end
end;
cam#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw !cam_heading);
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