diff --git a/conf/conf.xml b/conf/conf.xml index a3319e9a86..500d137b5c 100644 --- a/conf/conf.xml +++ b/conf/conf.xml @@ -14,7 +14,7 @@ ac_id="1" airframe="airframes/twinstar1.xml" radio="radios/mc3030.xml" - flight_plan="flight_plans/muret_mini.xml" + flight_plan="flight_plans/mav05.xml" /> diff --git a/conf/control_panel.xml b/conf/control_panel.xml index b2f8197140..59c6de567c 100644 --- a/conf/control_panel.xml +++ b/conf/control_panel.xml @@ -87,7 +87,7 @@ - + @@ -115,6 +115,16 @@ + + + + + + + + + + diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 3107e8a9f5..264dff955e 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -35,6 +35,10 @@ + + + + diff --git a/conf/flight_plans/mav05.xml b/conf/flight_plans/mav05.xml index 2f1273588d..d366f89b07 100644 --- a/conf/flight_plans/mav05.xml +++ b/conf/flight_plans/mav05.xml @@ -1,4 +1,4 @@ - + diff --git a/data/maps/garmisch.xml b/data/maps/garmisch.xml index c62c25a832..be72b7e29b 100644 --- a/data/maps/garmisch.xml +++ b/data/maps/garmisch.xml @@ -1,4 +1,4 @@ - + diff --git a/data/maps/muret_UTM.xml b/data/maps/muret_UTM.xml index 939e195353..af0a0f39a9 100644 --- a/data/maps/muret_UTM.xml +++ b/data/maps/muret_UTM.xml @@ -1,4 +1,4 @@ - + diff --git a/data/maps/sinsat.xml b/data/maps/sinsat.xml index 8ea7bd01c2..80a1d56508 100644 --- a/data/maps/sinsat.xml +++ b/data/maps/sinsat.xml @@ -1,4 +1,4 @@ - diff --git a/sw/airborne/autopilot/estimator.c b/sw/airborne/autopilot/estimator.c index 4ecf207d3f..68421a6c38 100644 --- a/sw/airborne/autopilot/estimator.c +++ b/sw/airborne/autopilot/estimator.c @@ -4,7 +4,7 @@ * Copyright (C) 2004 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. - * + * * paparazzi is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) @@ -33,6 +33,14 @@ #include "flight_plan.h" +/** if defined, ir relief correction done */ + +/***/ #define IR_RELIEF_CORRECTION /***/ + + +/** stepping value in meters, the area of correction */ +#define ir_correction_step 90 + /* position in meters */ float estimator_x; float estimator_y; @@ -59,7 +67,9 @@ float estimator_rad_of_ir, estimator_ir, estimator_rad; /* array of horizon angles computed for a given height (at flight_plan.h generation) */ -int8_t angles[NB_HEIGHTS] = HEIGHTS; + +int8_t angles[IR_CORRECTION_MAX_INDEX + 1][IR_CORRECTION_MAX_INDEX + 1][NB_HEIGHTS] = HEIGHTS; + /* (aircraft axis, ir axis) angle */ @@ -68,6 +78,7 @@ int8_t angles[NB_HEIGHTS] = HEIGHTS; + #define height_index_coeff (NB_HEIGHTS / ( 2 * M_PI )) #define NORM_RAD_ANGLE2(x) { \ @@ -139,32 +150,60 @@ void estimator_update_state_infrared( void ) { float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ? estimator_rad_of_ir : ir_rad_of_ir; - /* phi correction because of the relief effect on ir data */ +#if defined IR_RELIEF_CORRECTION + /** phi correction because of the relief effect on ir data */ - /*** int8_t index_left, index_right; + int8_t lat_index = ( (int) (IR_SQUARE_UTMY_MAX - gps_utm_north / 100.0) ) / ir_correction_step; + lat_index = (lat_index < 0 ? 0 : lat_index); + lat_index = (lat_index > IR_CORRECTION_MAX_INDEX ? IR_CORRECTION_MAX_INDEX : lat_index); - float angle = gps_fcourse - aircraft_ir_angle; + int8_t long_index = ( (int) (gps_utm_east / 100. - IR_SQUARE_UTMX_MIN) ) / ir_correction_step; + long_index = (long_index > IR_CORRECTION_MAX_INDEX ? IR_CORRECTION_MAX_INDEX : long_index); + long_index = (long_index < 0 ? 0 : long_index); + + /*** printf( "utmlowx %.2f gps_x %.2f utmlowy %.2f gps_y %.2f \n", IR_SQUARE_UTMX_MIN, gps_utm_east / 100.0 , IR_SQUARE_UTMY_MAX, gps_utm_north / 100.0); ***/ - NORM_RAD_ANGLE2(angle); + int8_t index_left, index_right; - index_left = angle * height_index_coeff ; + float degrees_left = estimator_hspeed_dir - aircraft_ir_angle; + float degrees_right = estimator_hspeed_dir + aircraft_ir_angle; - angle = gps_fcourse + aircraft_ir_angle; + NORM_RAD_ANGLE2(degrees_left); - NORM_RAD_ANGLE2(angle); + index_left = degrees_left * height_index_coeff ; - index_right = angle * height_index_coeff ; + degrees_right = estimator_hspeed_dir + aircraft_ir_angle; - int8_t angle_left = angles[index_left]; + NORM_RAD_ANGLE2(degrees_right); - int8_t angle_right = angles[index_right]; + index_right = degrees_right * height_index_coeff ; + + int8_t angle_left = angles[long_index][lat_index][index_left]; + + int8_t angle_right = angles[long_index][lat_index][index_right]; float correction = angle_left - angle_right; + - printf(" degres_left %d angle_left %d degres_right %d angle_right %d correction %.2f \n", (index_left*15), angle_left, (index_right*15), angle_right, correction); ***/ - - estimator_phi = rad_of_ir * ir_roll - ir_roll_neutral /*** + RadOfDeg( correction ) ***/; + + + + /*** printf(" estimator_hspeed_dir %.2f aircraft_ir_angle %.2f degres_left %.2f angle_left %d degres_right %.2f angle_right %d correction %.2f long_index %d lat_index %d \n", estimator_hspeed_dir, aircraft_ir_angle, degrees_left, angle_left, degrees_right, angle_right, correction, long_index, lat_index); ***/ + +#endif + + float rad_of_ir_roll = rad_of_ir * ir_roll; + + estimator_phi = rad_of_ir * ir_roll; + +#if defined IR_RELIEF_CORRECTION + + estimator_phi += RadOfDeg(correction) ; + + /*** printf(" estimator phi = %.4f ; riroll = %.4f ; correction = %.4f\n", estimator_phi, rad_of_ir_roll, correction); ***/ + +#endif estimator_theta = rad_of_ir * ir_pitch - ir_pitch_neutral; } diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index 59aebea84e..ff03fcaedd 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -34,6 +34,17 @@ type color = string let fos = float_of_string let list_separator = Str.regexp "," +(** parameters used for creating the vertical display window *) +let max_graduations = 20 + +let vertical_delta = 5.0 + +let max_east = 2000.0 + +let max_label = 4 + +let approx_ground_altitude = ref 0.0 + module G = MapCanvas let home = Env.paparazzi_home @@ -42,6 +53,7 @@ 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; @@ -54,7 +66,7 @@ let map_ref = ref None let float_attr = fun xml a -> float_of_string (ExtXml.attrib xml a) -let load_map = fun (geomap:G.widget) xml_map -> +let load_map = fun (geomap:G.widget) (vertical_display:MapCanvas.basic_widget) xml_map -> let dir = Filename.dirname xml_map in let xml_map = Xml.parse_file xml_map in let image = dir // ExtXml.attrib xml_map "file" @@ -63,6 +75,9 @@ let load_map = fun (geomap:G.widget) xml_map -> try int_of_string (Xml.attrib xml_map "utm_zone") with _ -> 31 in geomap#set_world_unit scale; + approx_ground_altitude := ( try (float_attr xml_map "approx_ground_altitude") + with _ -> 0.0); + vertical_display#set_world_unit scale; let one_ref = ExtXml.child xml_map "point" in let x = float_attr one_ref "x" and y = float_attr one_ref "y" and utm_x = float_attr one_ref "utm_x" and utm_y = float_attr one_ref "utm_y" in @@ -134,19 +149,18 @@ let load_mission = fun color geomap url -> fp -let aircraft_pos_msg = fun track utm_x_ utm_y_ heading altitude speed -> +let aircraft_pos_msg = fun track utm_x_ utm_y_ heading altitude speed climb -> match !map_ref with None -> () | Some utm0 -> let en = {G.east = utm_x_ -. utm0.utm_x; north = utm_y_ -. utm0.utm_y } in - track#add_point en; let h = try Srtm.of_utm { utm_zone = utm0.utm_zone; utm_x = utm_x_; utm_y = utm_y_} with _ -> truncate altitude in - track#move_icon en heading altitude (float_of_int h) speed + track#move_icon en heading altitude (float_of_int h) speed climb let carrot_pos_msg = fun track utm_x utm_y -> match !map_ref with @@ -185,8 +199,8 @@ let circle_status_msg = fun track utm_x utm_y radius -> 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 -> - () +let ap_status_msg = fun track flight_time -> + track#update_ap_status flight_time let new_color = @@ -227,21 +241,25 @@ let resize_track = fun ac track -> | Some s -> track#resize (int_of_string s) -let one_new_ac = fun (geomap:MapCanvas.widget) ac -> +let one_new_ac = fun (geomap:MapCanvas.widget)(vertical_display:MapCanvas.basic_widget) ac -> if not (Hashtbl.mem live_aircrafts ac) then begin let ac_menu = geomap#factory#add_submenu ac in let ac_menu_fact = new GMenu.factory ac_menu in let fp = ac_menu_fact#add_check_item "Fligh Plan" ~active:false in ignore (fp#connect#toggled (fun () -> show_mission geomap ac fp#active)); let color = new_color () in - let track = new MapTrack.track ~name:ac ~color:color geomap in - ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear)); + let track = new MapTrack.track ~name:ac ~color:color geomap vertical_display in + ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear_map2D)); ignore (ac_menu_fact#add_item "Resize Track" ~callback:(fun () -> resize_track ac track)); let cam = ac_menu_fact#add_check_item "Cam Display" ~active:false in ignore (cam#connect#toggled (fun () -> track#set_cam_state cam#active)); + let ac_menu_vertical = vertical_display#factory#add_submenu ac in + let ac_menu_fact_vertical = new GMenu.factory ac_menu_vertical in let params = ac_menu_fact#add_check_item "flight param. display" ~active:false in ignore (params#connect#toggled (fun () -> track#set_params_state params#active)); - let event_ac = fun e -> + let v_params = ac_menu_fact_vertical#add_check_item "flight param. display" ~active:false in + ignore (v_params#connect#toggled (fun () -> track#set_v_params_state v_params#active)); + let event_ac = fun e -> match e with `BUTTON_PRESS _ | `BUTTON_RELEASE _ -> Ground_Pprz.message_send "ground" "SELECTED" ["aircraft_id", Pprz.String ac]; @@ -252,10 +270,10 @@ let one_new_ac = fun (geomap:MapCanvas.widget) ac -> end -let live_aircrafts_msg = fun (geomap:MapCanvas.widget) acs -> +let live_aircrafts_msg = fun (geomap:MapCanvas.widget)(vertical_display:MapCanvas.basic_widget) acs -> let acs = Pprz.string_assoc "ac_list" acs in let acs = Str.split list_separator acs in - List.iter (one_new_ac geomap) acs + List.iter (one_new_ac geomap vertical_display) acs let listen_flight_params = fun () -> @@ -264,7 +282,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") (a "alt") (a "speed") + aircraft_pos_msg ac.track (a "east") (a "north") (a "course") (a "alt") (a "speed") (a "climb") with Not_found -> () in ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_fp); @@ -274,7 +292,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 - carrot_pos_msg ac.track (a "target_east") (a "target_north") + carrot_pos_msg ac.track (a "target_east") (a "target_north") with Not_found -> () in ignore (Ground_Pprz.message_bind "NAV_STATUS" get_ns); @@ -313,8 +331,9 @@ let listen_flight_params = fun () -> 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 -> () + ap_status_msg ac.track ( float_of_int (Pprz.int32_assoc "flight_time" vs )) + with + Not_found -> () in ignore (Ground_Pprz.message_bind "AP_STATUS" get_ap_status) @@ -337,27 +356,57 @@ let _ = let window = GWindow.window ~title: "Map2d" ~border_width:1 ~width:400 () in let vbox= GPack.vbox ~packing: window#add () in + let vertical_situation = GWindow.window ~title: "Vertical" ~border_width:1 ~width:400 () in + let vertical_vbox= GPack.vbox ~packing: vertical_situation#add () in let quit = fun () -> GMain.Main.quit (); exit 0 in ignore (window#connect#destroy ~callback:quit); + ignore (vertical_situation#connect#destroy ~callback:quit); let geomap = new MapCanvas.widget ~height:400 () in let accel_group = geomap#menu_fact#accel_group in + + (** widget displaying aircraft vertical position *) + let vertical_display = new MapCanvas.basic_widget ~height:400 () in + let ac_vertical_fact = new GMenu.factory vertical_display#file_menu in + let time_axis = ac_vertical_fact#add_check_item "x_axis : Time" ~active:false in + ignore (time_axis#connect#toggled (fun () -> + let set_one_track = (fun a b -> + (b.track)#set_vertical_time_axis time_axis#active) in + Hashtbl.iter (set_one_track) live_aircrafts)); + let vertical_graduations = GnoCanvas.group vertical_display#canvas#root in + vertical_display#set_vertical_factor 10.0; + + ignore (geomap#menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); + vbox#pack ~expand:true geomap#frame#coerce; + vertical_vbox#pack ~expand:true vertical_display#frame#coerce; (* Loading an initial map *) if !map_file <> "" then begin let xml_map_file = Filename.concat default_path_maps !map_file in - load_map geomap xml_map_file + load_map geomap vertical_display xml_map_file end; - ignore (Glib.Timeout.add 5000 (fun () -> Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> live_aircrafts_msg geomap vs); false)); + let max_level = (float_of_int max_graduations) *. vertical_delta +. !approx_ground_altitude in + vertical_display#set_vertical_max_level max_level; + for i = 0 to max_graduations do + let level = (float_of_int i) *. vertical_delta in + ignore ( vertical_display#segment ~group:vertical_graduations ~fill_color:"blue" {G.east = 0.0 ; G.north = level *. (-. vertical_display#get_vertical_factor) } {G.east = max_east ; G.north = level *. (-. vertical_display#get_vertical_factor) } ) ; + for j = 0 to max_label do + ignore( vertical_display#text ~group:vertical_graduations ~fill_color:"red" ~x_offset:30.0 ~y_offset:(-.0.5) {G.east = (float_of_int j) *. max_east /. (float_of_int max_label) ; G.north = level *. (-. vertical_display#get_vertical_factor) } ((string_of_float ( max_level -. level) )^" m") ) + done; + done; - ignore (Ground_Pprz.message_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac geomap (Pprz.string_assoc "ac_id" vs))); + ignore (Glib.Timeout.add 5000 (fun () -> Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> live_aircrafts_msg geomap vertical_display vs); false)); + + ignore (Ground_Pprz.message_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac geomap vertical_display (Pprz.string_assoc "ac_id" vs))); listen_flight_params (); window#add_accel_group accel_group; window#show (); + + vertical_situation#show (); GMain.Main.main () diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml index 9bf0a4330c..6b9f3dd54f 100644 --- a/sw/lib/ocaml/mapCanvas.ml +++ b/sw/lib/ocaml/mapCanvas.ml @@ -1,234 +1,342 @@ open Latlong open Printf - + let pan_step = 50 - + type meter = float type en = { east : meter; north : meter } - + let _ = Srtm.add_path "SRTM" -(* world_unit: m:pixel at scale 1. *) -class widget = fun ?(height=800) ?width ?wgs84_of_en () -> - let frame = GPack.vbox ~height ?width () in - - let menubar = GMenu.menu_bar ~packing:frame#pack () in - - let adj = GData.adjustment - ~value:1. ~lower:0.25 ~upper:10. - ~step_incr:0.25 ~page_incr:1.0 ~page_size:1.0 () in - - let canvas = GnoCanvas.canvas ~height ~packing:(frame#pack ~expand:true) () in - - let bottom = GPack.hbox ~height:30 ~packing:frame#pack () in - let _w = GEdit.spin_button ~adjustment:adj ~rate:0. ~digits:2 ~width:50 ~height:20 ~packing:bottom#pack () in - - - let lbl_xy = GMisc.label ~height:50 ~packing:bottom#pack () in - let lbl_geo = GMisc.label ~height:50 ~packing:bottom#pack () in - let lbl_alt = GMisc.label ~height:50 ~packing:bottom#pack () in - let lbl_group = GMisc.label ~height:50 ~packing:bottom#pack () in - - let factory = new GMenu.factory menubar in - let file_menu = factory#add_submenu "Nav" in - let menu_fact = new GMenu.factory file_menu in - - let srtm = menu_fact#add_check_item "SRTM" ~active:false in - - object (self) - initializer canvas#coerce#misc#modify_bg [`NORMAL, `NAME "black"] - - initializer ignore (menu_fact#add_check_item "Background" ~active:true ~callback:self#switch_background) - initializer ignore (menu_fact#add_item "Goto" ~callback:self#goto) - initializer ignore (canvas#event#connect#motion_notify (self#mouse_motion)); - initializer ignore (canvas#event#connect#button_press (self#button_press)); - initializer ignore (canvas#event#connect#button_release self#button_release); - initializer ignore (canvas#event#connect#after#key_press self#key_press) ; - initializer ignore (canvas#event#connect#enter_notify (fun _ -> self#canvas#misc#grab_focus () ; false)); - initializer ignore (canvas#event#connect#any self#any_event); - - initializer ignore (adj#connect#value_changed (fun () -> self#zoom adj#value)); - initializer canvas#set_center_scroll_region false ; - initializer canvas#set_scroll_region (-2500000.) (-2500000.) 2500000. 2500000.; - - - val mutable current_zoom = 1. - val mutable dragging = None - val mutable grouping = None - val mutable rectangle = None - val mutable world_unit = 1. - val mutable wgs84_of_en = wgs84_of_en - val mutable background = GnoCanvas.pixbuf canvas#root - - method switch_background = fun x -> if x then background#show () else background#hide (); - - method set_wgs84_of_en = fun x -> wgs84_of_en <- Some x - - method set_world_unit = fun x -> world_unit <- x - - method get_world_unit = fun () -> world_unit - - method current_zoom = current_zoom - - method canvas = canvas - method frame = frame - method menu_fact = menu_fact - method window_to_world = canvas#window_to_world - method root = canvas#root - method zoom_adj = adj - method factory = factory - - - 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 } +(** basic canvas with menubar ************************************** + * (the vertical display in map2.ml is an instance of basic_widget)* + *******************************************************************) + +(* world_unit: m:pixel at scale 1. *) +class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () -> + + object (self) + +(** GUI attributes *) - method geo_string = fun en -> - match wgs84_of_en with - None -> "" - | Some f -> string_degrees_of_geographic (f en) - - method altitude = fun wgs84 -> - try - Srtm.of_wgs84 wgs84 - with - Srtm.Tile_not_found x -> - srtm#set_active false; - GToolbox.message_box "SRTM" (sprintf "SRTM tile %s not found: %s ?" x (Srtm.error x)); - 0 - - method moveto = fun en -> - let (xw, yw) = self#world_of_en en in - let (xc, yc) = canvas#world_to_window xw yw in - canvas#scroll_to (truncate xc) (truncate yc) - - method goto = fun () -> - let dialog = GWindow.window ~border_width:10 ~title:"Geo ref" () in - let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in - let lat = GEdit.entry ~packing:dvbx#add () in - let lon = GEdit.entry ~packing:dvbx#add () in - let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in - let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in - ignore(cancel#connect#clicked ~callback:dialog#destroy); - ignore(ok#connect#clicked ~callback: - begin fun _ -> - let x = float_of_string lat#text in - let y = float_of_string lon#text in - self#moveto {east=x; north=y}; - dialog#destroy () - end); - dialog#show () - - - method display_map = fun ?(scale = 1.) en image -> - background <- GnoCanvas.pixbuf ~pixbuf:image ~props:[`ANCHOR `NW] self#root; - background#lower_to_bottom (); - let wx, wy = self#world_of_en en in - background#move wx wy; - let a = background#i2w_affine in - a.(0) <- scale; a.(3) <- scale; - background#affine_absolute a; - background - - method zoom = fun value -> - canvas#set_pixels_per_unit value; - current_zoom <- value - - - method mouse_motion = fun ev -> - let xc = GdkEvent.Motion.x ev - and yc = GdkEvent.Motion.y ev in - let (xw, yw) = self#window_to_world xc yc in - let en = self#en_of_world xw yw in - lbl_xy#set_text (sprintf "%.0fm %.0fm\t" en.east en.north); - lbl_geo#set_text (self#geo_string en); - begin - match wgs84_of_en, srtm#active with - Some wgs84_of_en, true -> - lbl_alt#set_text (sprintf "\t%dm"(self#altitude (wgs84_of_en en))) - | _ -> () - end; - begin - match dragging with - Some (x0, y0 ) -> - let (x, y) = self#canvas#get_scroll_offsets in - self#canvas#scroll_to (x+truncate (x0-.xc)) (y+truncate (y0-.yc)) - | None -> () - end; - begin - match grouping with - Some (xw1, yw1) -> - let en1 = self#en_of_world xw1 yw1 in - lbl_group#set_text (sprintf "[%.1fkm %.1fkm]" ((en1.east -. en.east)/.1000.) ((en1.north-.en.north)/.1000.)) - | None -> () - end; - false - - method button_release = fun ev -> - match GdkEvent.Button.button ev, grouping with - 2, _ -> - dragging <- None; false - | 1, Some (xw1, yw1) -> - let xc = GdkEvent.Button.x ev in - let yc = GdkEvent.Button.y ev in - let (xw2, yw2) = self#window_to_world xc yc in - rectangle <- Some ((xw1, yw1), (xw2, yw2)); - lbl_group#set_text ""; - grouping <- None; - false - | _ -> false - - method button_press = fun ev -> - let xc = GdkEvent.Button.x ev in - let yc = GdkEvent.Button.y ev in - match GdkEvent.Button.button ev with - 1 -> - let xyw = self#window_to_world xc yc in - grouping <- Some xyw; - true - | 2 -> - dragging <- Some (xc, yc); - true - | _ -> false + val frame = GPack.vbox ~height ?width () - method key_press = fun ev -> - let (x, y) = canvas#get_scroll_offsets in - match GdkEvent.Key.keyval ev with - | k when k = GdkKeysyms._Up -> canvas#scroll_to x (y-pan_step) ; true - | k when k = GdkKeysyms._Down -> canvas#scroll_to x (y+pan_step) ; true - | k when k = GdkKeysyms._Left -> canvas#scroll_to (x-pan_step) y ; true - | k when k = GdkKeysyms._Right -> canvas#scroll_to (x+pan_step) y ; true - | k when k = GdkKeysyms._Page_Up -> adj#set_value (adj#value+.adj#step_increment) ; true - | k when k = GdkKeysyms._Page_Down -> adj#set_value (adj#value-.adj#step_increment) ; true - | _ -> false - - method any_event = fun ev -> - match GdkEvent.get_type ev with - | `SCROLL -> begin - match GdkEvent.Scroll.direction (GdkEvent.Scroll.cast ev) with - `UP -> - adj#set_value (adj#value+.adj#step_increment) ; - true - | `DOWN -> adj#set_value (adj#value-.adj#step_increment) ; true - | _ -> false - end - | _ -> false - + val menubar = GMenu.menu_bar () + + val adj = GData.adjustment + ~value:1. ~lower:0.25 ~upper:10. + ~step_incr:0.25 ~page_incr:1.0 ~page_size:1.0 () + + val canvas = GnoCanvas.canvas () + + val bottom = GPack.hbox ~height:30 () + + val _w = GEdit.spin_button ~rate:0. ~digits:2 ~width:50 + ~height:20 () + +(***) val mutable factory = new GMenu.factory (GMenu.menu_bar ()) + + val mutable file_menu = GMenu.menu () + + val mutable lbl_x_axis = GMisc.label ~height:50 () + +(** other attributes *) + + val mutable current_zoom = 1. + val mutable dragging = None + val mutable grouping = None + val mutable rectangle = None + val mutable world_unit = 1. + val mutable wgs84_of_en = wgs84_of_en +(***) val mutable background = GnoCanvas.pixbuf (GnoCanvas.canvas ())#root + val mutable vertical_factor = 10.0 + val mutable vertical_max_level = 0.0 - method segment = fun ?(group = canvas#root) ?(width=1) ?fill_color en1 en2 -> - let (x1, y1) = self#world_of_en en1 - and (x2, y2) = self#world_of_en en2 in - let l = GnoCanvas.line ?fill_color ~props:[`WIDTH_PIXELS width] ~points:[|x1;y1;x2;y2|] group in - l#show (); - l - - method circle = fun ?(group = canvas#root) ?(width=1) ?fill_color ?(color="black") en rad -> - let (x, y) = self#world_of_en en in - let rad = rad /. world_unit in - let l = GnoCanvas.ellipse ?fill_color ~props:[`WIDTH_PIXELS width; `OUTLINE_COLOR color] ~x1:(x-.rad) ~y1:(y -.rad) ~x2:(x +.rad) ~y2:(y+.rad) group in - l#show (); - l + + method pack = + frame#pack menubar#coerce; + frame#pack ~expand:true canvas#coerce; + frame#pack bottom#coerce; + bottom#pack _w#coerce; + bottom#pack lbl_x_axis#coerce; + +(** initialization of instance attributes *) + + initializer ( + + self#pack; + + _w#set_adjustment adj; + + background#destroy (); + background <- GnoCanvas.pixbuf canvas#root; + + (*** factory#destroy (); ***) + + factory <- new GMenu.factory menubar; + + file_menu#destroy (); + + file_menu <- factory#add_submenu "Nav"; + +(** callback bindings *) + + canvas#coerce#misc#modify_bg [`NORMAL, `NAME "black"]; + ignore (canvas#event#connect#motion_notify (self#mouse_motion)); + ignore (canvas#event#connect#button_press (self#button_press)); + ignore (canvas#event#connect#button_release self#button_release); + ignore (canvas#event#connect#after#key_press self#key_press) ; + ignore (canvas#event#connect#enter_notify (fun _ -> self#canvas#misc#grab_focus () ; false)); + ignore (canvas#event#connect#any self#any_event); + ignore (adj#connect#value_changed (fun () -> self#zoom adj#value)); + + canvas#set_center_scroll_region false ; + canvas#set_scroll_region (-2500000.) (-2500000.) 2500000. 2500000.; + + ) + + + +(** methods *) + + + method set_wgs84_of_en = fun x -> wgs84_of_en <- Some x + + method set_world_unit = fun x -> world_unit <- x + + method get_world_unit = fun () -> world_unit + + method set_lbl_x_axis = fun s -> lbl_x_axis#set_text s + +(** accessors to instance variables *) + method current_zoom = current_zoom + method get_vertical_factor = vertical_factor + method get_vertical_max_level = vertical_max_level + method set_vertical_factor = fun x -> vertical_factor <- x + method set_vertical_max_level = fun x -> vertical_max_level <- x + method canvas = canvas + method frame = frame + method factory = factory + method file_menu = file_menu + method window_to_world = canvas#window_to_world + method root = canvas#root + method zoom_adj = adj + +(** following display functions can be redefined by subclasses. + they do nothing in the basic_widget *) + method display_xy = fun s -> () + method display_geo = fun s -> () + method display_alt = fun en -> () + method display_group = fun s -> () + + (** converts relative utm coordinates into world (ie map) coordinates *) + 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 -> + match wgs84_of_en with + None -> "" + | Some f -> string_degrees_of_geographic (f en) + + + method moveto = fun en -> + let (xw, yw) = self#world_of_en en in + let (xc, yc) = canvas#world_to_window xw yw in + canvas#scroll_to (truncate xc) (truncate yc) + + method display_map = fun ?(scale = 1.) en image -> + background <- GnoCanvas.pixbuf ~pixbuf:image ~props:[`ANCHOR `NW] self#root; + background#lower_to_bottom (); + let wx, wy = self#world_of_en en in + background#move wx wy; + let a = background#i2w_affine in + a.(0) <- scale; a.(3) <- scale; + background#affine_absolute a; + background + + method zoom = fun value -> + canvas#set_pixels_per_unit value; + current_zoom <- value + + + method mouse_motion = fun ev -> + let xc = GdkEvent.Motion.x ev + and yc = GdkEvent.Motion.y ev in + let (xw, yw) = self#window_to_world xc yc in + let en = self#en_of_world xw yw in + self#display_xy (sprintf "%.0fm %.0fm\t" en.east en.north); + self#display_geo (self#geo_string en); + self#display_alt en; + begin + match dragging with + Some (x0, y0 ) -> + let (x, y) = self#canvas#get_scroll_offsets in + self#canvas#scroll_to (x+truncate (x0-.xc)) (y+truncate (y0-.yc)) + | None -> () + end; + begin + match grouping with + Some (xw1, yw1) -> + let en1 = self#en_of_world xw1 yw1 in + self#display_group (sprintf "[%.1fkm %.1fkm]" ((en1.east -. en.east)/.1000.) ((en1.north-.en.north)/.1000.)) + | None -> () + end; + false + + method button_release = fun ev -> + match GdkEvent.Button.button ev, grouping with + 2, _ -> + dragging <- None; false + | 1, Some (xw1, yw1) -> + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + let (xw2, yw2) = self#window_to_world xc yc in + rectangle <- Some ((xw1, yw1), (xw2, yw2)); + self#display_group ""; + grouping <- None; + false + | _ -> false + + method button_press = fun ev -> + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + match GdkEvent.Button.button ev with + 1 -> + let xyw = self#window_to_world xc yc in + grouping <- Some xyw; + true + | 2 -> + dragging <- Some (xc, yc); + true + | _ -> false + + method key_press = fun ev -> + let (x, y) = canvas#get_scroll_offsets in + match GdkEvent.Key.keyval ev with + | k when k = GdkKeysyms._Up -> canvas#scroll_to x (y-pan_step) ; true + | k when k = GdkKeysyms._Down -> canvas#scroll_to x (y+pan_step) ; true + | k when k = GdkKeysyms._Left -> canvas#scroll_to (x-pan_step) y ; true + | k when k = GdkKeysyms._Right -> canvas#scroll_to (x+pan_step) y ; true + | k when k = GdkKeysyms._Page_Up -> adj#set_value (adj#value+.adj#step_increment) ; true + | k when k = GdkKeysyms._Page_Down -> adj#set_value (adj#value-.adj#step_increment) ; true + | _ -> false + + method any_event = fun ev -> + match GdkEvent.get_type ev with + | `SCROLL -> begin + match GdkEvent.Scroll.direction (GdkEvent.Scroll.cast ev) with + `UP -> + adj#set_value (adj#value+.adj#step_increment) ; + true + | `DOWN -> adj#set_value (adj#value-.adj#step_increment) ; true + | _ -> false + end + | _ -> false + + + method segment = fun ?(group = canvas#root) ?(width=1) ?fill_color en1 en2 -> + let (x1, y1) = self#world_of_en en1 + and (x2, y2) = self#world_of_en en2 in + let l = GnoCanvas.line ?fill_color ~props:[`WIDTH_PIXELS width] ~points:[|x1;y1;x2;y2|] group in + l#show (); + l + + method circle = fun ?(group = canvas#root) ?(width=1) ?fill_color ?(color="black") en rad -> + let (x, y) = self#world_of_en en in + let rad = rad /. world_unit in + let l = GnoCanvas.ellipse ?fill_color ~props:[`WIDTH_PIXELS width; `OUTLINE_COLOR color] ~x1:(x-.rad) ~y1:(y -.rad) ~x2:(x +.rad) ~y2:(y+.rad) group in + l#show (); + l + method text = fun ?(group = canvas#root) ?(fill_color = "blue") ?(x_offset = 0.0) ?(y_offset = 0.0) en1 text -> + let (x1, y1) = self#world_of_en en1 in + let t = GnoCanvas.text ~x:x1 ~y:y1 ~text:text ~props:[`FILL_COLOR fill_color; `X_OFFSET x_offset; `Y_OFFSET y_offset] group in + t#show (); + t + + end + + +(** canvas which inherits from basic_widget ******************** + * - labels for displaying mouse coordinates on the map * + * - background switching * + *(the horizontal map in map2.ml is an instance of basic_widget)* + ****************************************************************) + + +class widget = fun ?(height=800) ?width ?wgs84_of_en () -> + object(self) + inherit (basic_widget ~height:height ?width ?wgs84_of_en ()) + + val mutable lbl_xy = GMisc.label ~height:50 () + val mutable lbl_geo = GMisc.label ~height:50 () + val mutable lbl_alt = GMisc.label ~height:50 () + val mutable lbl_group = GMisc.label ~height:50 () +(***) val mutable menu_fact = new GMenu.factory (GMenu.menu ()) + val mutable srtm = GMenu.check_menu_item () + + method pack_labels = + bottom#pack lbl_xy#coerce; + bottom#pack lbl_geo#coerce; + bottom#pack lbl_alt#coerce; + bottom#pack lbl_group#coerce; + + initializer ( + self#pack_labels; + (*** menu_fact#destroy (); ***) + menu_fact <- new GMenu.factory file_menu; + srtm#destroy (); + srtm <- menu_fact#add_check_item "SRTM" ~active:false; + ignore (menu_fact#add_check_item "Background" ~active:true ~callback:self#switch_background); + ignore (menu_fact#add_item "Goto" ~callback:self#goto); + ) + + method menu_fact = menu_fact + + (** ground altitude extraction from srtm data *) + method altitude = fun wgs84 -> + try + Srtm.of_wgs84 wgs84 + with + Srtm.Tile_not_found x -> + srtm#set_active false; + GToolbox.message_box "SRTM" (sprintf "SRTM tile %s not found: %s ?" x (Srtm.error x)); + 0 + + (** display methods *) + + method display_xy = fun s -> lbl_xy#set_text s + method display_geo = fun s -> lbl_geo#set_text s + method display_alt = fun en -> + begin + match wgs84_of_en, srtm#active with + Some wgs84_of_en, true -> + lbl_alt#set_text (sprintf "\t%dm"(self#altitude (wgs84_of_en en))) + | _ -> () + end + + method display_group = fun s -> lbl_group#set_text s + + + method switch_background = fun x -> if x then background#show () else background#hide () + + method goto = fun () -> + let dialog = GWindow.window ~border_width:10 ~title:"Geo ref" () in + let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in + let lat = GEdit.entry ~packing:dvbx#add () in + let lon = GEdit.entry ~packing:dvbx#add () in + let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in + let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in + ignore(cancel#connect#clicked ~callback:dialog#destroy); + ignore(ok#connect#clicked ~callback: + begin fun _ -> + let x = float_of_string lat#text in + let y = float_of_string lon#text in + self#moveto {east=x; north=y}; + dialog#destroy () + end); + dialog#show () + end - diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml index 1e438f2713..f6095f381a 100644 --- a/sw/lib/ocaml/mapTrack.ml +++ b/sw/lib/ocaml/mapTrack.ml @@ -51,8 +51,10 @@ let cam_half_aperture = m_pi /. 4.0 let half_pi = m_pi /. 2.0 let sqrt_2_div_2 = sqrt 2.0 -class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanvas.widget) -> + +class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_widget) -> let group = GnoCanvas.group geomap#canvas#root in + let v_group = GnoCanvas.group vertical_display#canvas#root in let empty = ({ G.east = 0.; north = 0. }, GnoCanvas.line group) in let aircraft = GnoCanvas.group group @@ -71,7 +73,7 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva (** 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:color ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR color; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam) 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) in let mission_target = GnoCanvas.group group in @@ -84,63 +86,133 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva let max_oblic_distance_scaled = 10000.0 /. (geomap#get_world_unit ()) in let min_distance_scaled = 10. /. (geomap#get_world_unit ()) in let min_height_scaled = 0.1 /. (geomap#get_world_unit ()) in - + + +(** vertical display items *) + + let vertical_group = GnoCanvas.group vertical_display#canvas#root in + let vertical_aircraft = GnoCanvas.group vertical_group in + let vertical_plot = + ignore ( GnoCanvas.ellipse ~x1: (-. 5.0) ~y1: (-. 5.0) ~x2: 5.0 ~y2: 5.0 ~fill_color:color ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR color; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] vertical_aircraft); + (*** ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 2;`CAP_STYLE `ROUND] ~points:[|0.;0.;0.; -10.|] vertical_aircraft); ***) + in +let ac_v_label = + GnoCanvas.text v_group ~props:[`TEXT name; `X 25.; `Y 25.; `ANCHOR `SW; `FILL_COLOR color] + in + + let top = ref 0 and v_top = ref 0 in object (self) val mutable segments = Array.create size empty - val mutable top = 0 + val mutable v_segments = Array.create size empty val mutable last = None + val mutable v_last = None val mutable last_heading = 0.0 val mutable last_altitude = 0.0 val mutable last_speed = 0.0 + val mutable last_climb = 0.0 val mutable last_height = 0.0 val mutable last_xw = 0.0 val mutable last_yw = 0.0 + val mutable last_flight_time = 0.0 + val mutable last_x_val = 0.0 val mutable cam_on = false + val mutable vertical_time_axis_on = false val mutable params_on = false + val mutable v_params_on = false val mutable desired_track = ((GnoCanvas.ellipse group) :> GnoCanvas.base_item) val mutable ac_cam_cover = GnoCanvas.rect cam method track = track method aircraft = aircraft method set_label = fun s -> ac_label#set [`TEXT s] - method clear_one = fun i -> - if segments.(i) != empty then begin - (snd segments.(i))#destroy (); - segments.(i) <- empty + method clear_one = fun i seg -> + if seg.(i) != empty then begin + (snd seg.(i))#destroy (); + seg.(i) <- empty end - method incr = - let s = Array.length segments in - top <- (top + 1) mod s - method clear = - for i = 0 to Array.length segments - 1 do - self#clear_one i + method incr = fun seg top_ -> + let s = Array.length seg in + top_ := (!(top_) + 1) mod s + method clear = fun seg top -> + for i = 0 to Array.length seg - 1 do + self#clear_one i seg done; - top <- 0 + top := 0 method set_cam_state = fun b -> cam_on <- b - method set_params_state = fun b -> params_on <- b - method add_point = fun en -> - self#clear_one top; + + (** switches time and longitude on the vertical display x axis. + tracks are cleared *) + method set_vertical_time_axis = fun b -> + vertical_time_axis_on <- b; + if vertical_time_axis_on then vertical_display#set_lbl_x_axis "x-axis: time" + else vertical_display#set_lbl_x_axis "x-axis: longitude"; + self#clear v_segments v_top; + v_last <- None + + method update_ap_status = fun time -> + last_flight_time <- time + method set_params_state = fun b -> params_on <- b + method set_v_params_state = fun b -> v_params_on <- b + method set_last = fun x -> last <- x + method set_v_last = fun x -> v_last <- x + + (** add track points on map2D or vertical display, according to the + track parameter *) + method add_point = fun en seg set_last_point last_point top track -> + self#clear_one (!top) seg ; begin - match last with + match last_point with 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); + seg.((!top)) <- (en, geomap#segment ~group:track ~fill_color:color en en) + | Some pt -> + seg.((!top)) <- (en, geomap#segment ~group:track ~width:2 ~fill_color:color pt en); end; - self#incr; - last <- Some en - method move_icon = fun en heading altitude relief_height speed -> + self#incr (seg) top ; + set_last_point (Some en) + + method clear_map2D = self#clear segments top + + method move_icon = fun en heading altitude relief_height speed climb -> 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); last_speed <- speed ; - if params_on then - ac_label#set [`TEXT ( name^" \n"^(string_of_float last_height)^" m\n"^(string_of_float last_speed)^" m/s\n" ); `Y 70. ] else + last_climb <- climb; + + if params_on then + ac_label#set [`TEXT ( name^" \n"^(string_of_float last_height)^" m\n"^(string_of_float last_speed)^" m/s\n" ); `Y 70. ] else ac_label#set [`TEXT name; `Y 25.]; - ac_label#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.) + ac_label#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.); + + let y_val = ((vertical_display#get_vertical_max_level -. altitude) *. (vertical_display#get_vertical_factor) /. ( vertical_display#get_world_unit () ) ) in + let x_val = if vertical_time_axis_on then + last_flight_time /. ( vertical_display#get_world_unit () ) + else xw in + vertical_aircraft#affine_absolute (affine_pos_and_angle vertical_display#zoom_adj#value x_val y_val 0.0 ); + + let v_en = { MapCanvas.east = x_val *. ( vertical_display#get_world_unit () ); MapCanvas.north = y_val *. ( -. vertical_display#get_world_unit () ) } in + + + (** on the vertical_display, + params displayed are different if we have time or longitude on x-axis *) + + if vertical_time_axis_on then + begin + self#add_point v_en v_segments (self#set_v_last) v_last v_top v_group; + if v_params_on then ac_v_label#set [`TEXT ( name^" \n alt: "^(string_of_float altitude)^" m\n"^" height: "^(string_of_float last_height)^" m\n flight_time: "^( sprintf"%.2f sec\n" last_flight_time)); `Y 70. ] + else ac_v_label#set [`TEXT name; `Y 25.] + end + else + begin + if v_params_on then ac_v_label#set [`TEXT ( name^" \n alt: "^(string_of_float altitude)^" m\n"^" height: "^(string_of_float last_height)^" m\n long(utm_world): "^(sprintf"%.2f m\n" en.MapCanvas.east)); `Y 70. ] + else ac_v_label#set [`TEXT name; `Y 25.] + end; + ac_v_label#affine_absolute (affine_pos_and_angle vertical_display#zoom_adj#value x_val y_val 0.); + self#add_point en segments (self#set_last) last top group; + last_altitude <- altitude; + last_xw <- xw; + last_x_val <- x_val; 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.); @@ -148,7 +220,7 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva (** draws the circular path to be followed by the aircraft in circle mode *) method draw_circle = fun en radius -> desired_track#destroy (); - desired_track <- ((geomap#circle ~color:"green" en radius) :> GnoCanvas.base_item) + desired_track <- ((geomap#circle ~color:"green" en radius) :> GnoCanvas.base_item); (** draws the linear path to be followed by the aircraft between two waypoints *) method draw_segment = fun en en2 -> @@ -216,21 +288,20 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva for i = 0 to 3 do a.(i) <- a.(i) /. z' *. 1./.z done; aircraft#affine_absolute a method resize = fun new_size -> - let a = Array.create new_size empty in - let size = Array.length segments in - - let m = min new_size size in - let j = ref ((top - m + size) mod size) in - for i = 0 to m - 1 do - a.(i) <- segments.(!j); - j := (!j + 1) mod size - done; - for i = 1 to size - new_size do (* Never done if new_size > size *) - self#clear_one !j; - j := (!j + 1) mod size - done; - top <- m mod new_size; - segments <- a + let a = Array.create new_size empty in + let size = Array.length segments in + let m = min new_size size in + let j = ref ((!top - m + size) mod size) in + for i = 0 to m - 1 do + a.(i) <- segments.(!j); + j := (!j + 1) mod size + done; + for i = 1 to size - new_size do (* Never done if new_size > size *) + self#clear_one !j segments; + j := (!j + 1) mod size + done; + top := m mod new_size; + segments <- a method size = Array.length segments initializer ignore(geomap#zoom_adj#connect#value_changed (fun () -> self#zoom geomap#zoom_adj#value)) end diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 6e49e0aa6f..742af890b0 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -137,6 +137,11 @@ let int_assoc = fun (a:string) vs -> Int x -> x | _ -> invalid_arg "Pprz.int_assoc" +let int32_assoc = fun (a:string) vs -> + match assoc a vs with + Int32 x -> int_of_string (string_of_value (Int32 x)) + | _ -> invalid_arg "Pprz.int_assoc" + let string_assoc = fun (a:string) (vs:values) -> string_of_value (assoc a vs) diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 41848d12e9..c92ccd1732 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -52,6 +52,7 @@ val string_assoc : string -> values -> string val float_assoc : string -> values -> float val int_assoc : string -> values -> int +val int32_assoc : string -> values -> int (** May raise Not_found or Invalid_argument *) exception Unknown_msg_name of string diff --git a/sw/logalizer/play.ml b/sw/logalizer/play.ml index 5c1bf84e1e..ee6443a605 100644 --- a/sw/logalizer/play.ml +++ b/sw/logalizer/play.ml @@ -109,7 +109,7 @@ let _ = (***) let timescale = GRange.scale `HORIZONTAL ~adjustment:adj ~packing:window#vbox#pack () in - let speed_button = GEdit.spin_button (*** ~adjustment:speed ***) ~rate:0. ~digits:2 ~width:50 ~packing:window#vbox#add () in (***) + (*** let speed_button = GEdit.spin_button (*** ~adjustment:speed ***) ~rate:0. ~digits:2 ~width:50 ~packing:window#vbox#add () in ***) (** #move_slider is not working ??? **) (*** ignore (timescale#event#connect#button_release ~callback:(fun _ -> if !was_running then play adj speed; false)); ignore (timescale#event#connect#button_press ~callback:(fun _ -> was_running := !timer <> None; stop (); false)); ***) diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 4091db84aa..e56aa404e5 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -492,18 +492,51 @@ let dummy_waypoint = let nb_heights = 24 let half_aperture = pi /. 4. let horizon_distance = 1000. -let print_heights = fun wgs84 alt -> - Srtm.add_path (Env.paparazzi_home ^ "/data/srtm"); - Xml2h.define "NB_HEIGHTS" (string_of_int nb_heights); - Xml2h.define "HEIGHTS" "{ /* Degrees from default alt*/\\"; - let a =2. *. pi /. float nb_heights in - for i = 0 to nb_heights - 1 do - let psi = float i *. a in - let horizon = Srtm.horizon_slope wgs84 alt psi half_aperture horizon_distance in - let a = deg_of_rad horizon in - lprintf "%.0f, /* heading=%.2f */\\\n" a psi - done; - lprintf " }\n" +let print_heights = fun xml wgs84 alt -> + begin + let compute = ref true in + let utm0_low_x = try (float_attrib xml "ir_square_utmx_min") + with _ -> compute:= false; 657530.0 in + + let utm0_low_y = try (float_attrib xml "ir_square_utmy_max") + with _ -> compute:= false; 5261130.0 in + + let ir_correction_width = try int_of_string ( Xml.attrib xml "ir_square_width") + with _ -> 180 in + + let max_index = try int_of_string ( Xml.attrib xml "ir_correct_steps") + with _ -> 2 in + + Xml2h.define "IR_SQUARE_UTMX_MIN" (string_of_float utm0_low_x); + Xml2h.define "IR_SQUARE_UTMY_MAX" (string_of_float utm0_low_y); + Xml2h.define "IR_CORRECTION_MAX_INDEX" (string_of_int max_index); + Srtm.add_path (Env.paparazzi_home ^ "/data/srtm"); + Xml2h.define "NB_HEIGHTS" (string_of_int nb_heights); + Xml2h.define "HEIGHTS" "{ /* Degrees from default alt*/\\"; + + let utm0 = utm_of WGS84 wgs84 in + + let ir_correction_step = float_of_int (ir_correction_width / max_index) in + + for k = 0 to max_index do + lprintf "{ "; + for l = 0 to max_index do + lprintf "{ "; + let wgs84_2 = of_utm WGS84 { utm_zone= utm0.utm_zone; utm_x = utm0_low_x +. ir_correction_step *. (float_of_int k) ; utm_y = utm0_low_y -. ir_correction_step *. (float_of_int l)} in + let a =2. *. pi /. float nb_heights in + for i = 0 to nb_heights - 1 do + let psi = float i *. a in + let horizon = if !compute then Srtm.horizon_slope wgs84_2 alt psi half_aperture horizon_distance else 0.0 in + let a = deg_of_rad horizon in + lprintf "%.0f, /* heading=%.2f */\\\n" a psi + done; + lprintf " }, \\\n"; + done; + lprintf "}, \\\n"; + done; + lprintf "} \n"; + end + let _ = @@ -598,7 +631,7 @@ let _ = print_blocks index_of_waypoints blocks; - print_heights wgs84 (int_of_string alt); + print_heights xml wgs84 (int_of_string alt); Xml2h.finish h_name end