movable waypoints on the 2d map

conf/messages.xml

@@ -424,6 +424,10 @@
   <field name="alt"    type="float" unit="m"></field>
  </message>
 
+ <message name="SEND_EVENT" ID="25">
+  <field name="ac_id" type="string"/>
+  <field name="event_id" type="uint8"/>
+ </message>
 </class>
 
 <class name="alert">

sw/airborne/autopilot/main.c

@@ -167,7 +167,6 @@ inline uint8_t mcu1_status_update( void ) {
     } \
   } else { \
     _cpt = 0; \
-    _event = FALSE; \
   }
 /** \def EventPos(_cpt, _channel, _event)
  *  @@@@@ A FIXER @@@@@

sw/ground_segment/cockpit/map2d.ml

@@ -57,7 +57,7 @@ let default_path_missions = home // "conf"
 type aircraft = {
     track : MapTrack.track;
     color: color;
-    mutable fp_group : MapWaypoints.group option
+    mutable fp_group : (MapWaypoints.group * (int * MapWaypoints.waypoint) list) option
   }
 
 let live_aircrafts = Hashtbl.create 3
@@ -119,7 +119,8 @@ let load_mission = fun color geomap url ->
   let xml = Xml.parse_file file in
   let xml = ExtXml.child xml "flight_plan" in
   let lat0 = float_attr xml "lat0"
-  and lon0 = float_attr xml "lon0" in
+  and lon0 = float_attr xml "lon0"
+  and alt0 = float_attr xml "alt" in
   let utm0 = utm_of WGS84 {posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 } in
   let waypoints = ExtXml.child xml "waypoints" in
   let max_dist_from_home = float_attr xml "MAX_DIST_FROM_HOME" in
@@ -136,17 +137,20 @@ let load_mission = fun color geomap url ->
     {G.east = x +. utm0.utm_x -. utm_ref.utm_x;
      G.north = y +. utm0.utm_y -. utm_ref.utm_y } in
 
-  let fp = new MapWaypoints.group ~color ~editable:false geomap in
-  List.iter
-    (fun wp ->
-      let en = en_of_xy (float_attr wp "x") (float_attr wp "y") in
-      let alt = try Some (float_attr wp "alt") with _ -> None in
-      ignore (MapWaypoints.waypoint fp ~name:(ExtXml.attrib wp "name") ?alt en);
-      if  ExtXml.attrib wp "name" = "HOME" then
-	ignore (geomap#circle ~color en max_dist_from_home)
-    ) 
-    (Xml.children waypoints);
-  fp
+  let fp = new MapWaypoints.group ~color ~editable:true geomap in
+  let i = ref 0 in
+  let wpts = List.map
+      (fun wp ->
+	let en = en_of_xy (float_attr wp "x") (float_attr wp "y") in
+	let alt = try float_attr wp "alt" with _ -> alt0 in
+	let w = MapWaypoints.waypoint fp ~name:(ExtXml.attrib wp "name") ~alt en in
+	if  ExtXml.attrib wp "name" = "HOME" then
+	  ignore (geomap#circle ~color en max_dist_from_home);
+	incr i;
+	!i, w
+      ) 
+      (Xml.children waypoints) in
+  fp, wpts
 
 
 let aircraft_pos_msg = fun track utm_x_ utm_y_ heading altitude speed climb ->
@@ -231,10 +235,34 @@ let show_mission = fun geomap ac on_off ->
     let a = Hashtbl.find live_aircrafts ac in
     match a.fp_group with
       None -> ()
-    | Some g -> 
+    | Some (g, _wpts) -> 
 	a.fp_group <- None;
 	g#group#destroy ()
 
+let commit_changes = fun ac ->
+  let a = Hashtbl.find live_aircrafts ac in
+  match a.fp_group, !map_ref with
+    Some (g, wpts), Some utm0 -> 
+      List.iter 
+	(fun (i, w) -> 
+	  if w#moved then 
+	    let {MapCanvas.east=e; MapCanvas.north=n} =w#en in
+	    let vs = ["ac_id", Pprz.String ac;
+		      "wp_id", Pprz.Int i;
+		      "utm_east", Pprz.Float (utm0.utm_x+.e);
+		      "utm_north", Pprz.Float (utm0.utm_y+.n);
+		      "alt", Pprz.Float w#alt
+		    ] in
+	    Ground_Pprz.message_send "map2d" "MOVE_WAYPOINT" vs)
+	wpts
+  | _ -> ()
+
+let send_event = fun ac e ->
+  Ground_Pprz.message_send "map2d" "SEND_EVENT" 
+    ["ac_id", Pprz.String ac; "event_id", Pprz.Int e]
+  
+
+
 let resize_track = fun ac track ->
   match GToolbox.input_string ~text:(string_of_int track#size) ~title:ac "Track size" with
     None -> ()
@@ -251,6 +279,9 @@ let one_new_ac = fun (geomap:MapCanvas.widget)(vertical_display:MapCanvas.basic_
     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));
+    ignore (ac_menu_fact#add_item "Commit Moves" ~callback:(fun () -> commit_changes ac));
+    ignore (ac_menu_fact#add_item "Event 1" ~callback:(fun () -> send_event ac 1));
+    ignore (ac_menu_fact#add_item "Event 2" ~callback:(fun () -> send_event ac 2));
     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

sw/ground_segment/tmtc/wavecard_connect.ml

@@ -79,7 +79,6 @@ let get_fp = fun ac _sender vs ->
 
 (** Got a MOVE_WAYPOINT and send a MOVE_WP *)
 let move_wp = fun ac _sender vs ->
-    prerr_endline "move";
   let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
   if ac_id = ac.id then
     let f = fun a -> Pprz.float_assoc a vs in
@@ -91,10 +90,19 @@ let move_wp = fun ac _sender vs ->
 	      "utm_east", cm_of_m ux;
 	      "utm_north", cm_of_m uy;
 	      "alt", cm_of_m alt] in
-    prerr_endline "move";
     let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in
     let s = Dl_Pprz.payload_of_values msg_id vs in
     send ac s
+
+(** Got a SEND_EVENT, and send an EVENT *)
+let send_event = fun ac _sender vs ->
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  if ac_id = ac.id then
+    let ev_id = Pprz.int_assoc "event_id" vs in
+    let vs = ["event", Pprz.Int ev_id] in
+    let msg_id, _ = Dl_Pprz.message_of_name "EVENT" in
+    let s = Dl_Pprz.payload_of_values msg_id vs in
+    send ac s
     
 
 let _ =
@@ -135,6 +143,7 @@ let _ =
 
     ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp ac));
     ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" (move_wp ac));
+    ignore (Ground_Pprz.message_bind "SEND_EVENT" (send_event ac));
 
     (* Main Loop *)
     let loop = Glib.Main.create true in

sw/lib/ocaml/mapCanvas.ml

@@ -208,7 +208,7 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
 	  let xyw = self#window_to_world xc yc in
 	  grouping <- Some xyw;
 	  true
-      | 2 ->
+      | 2 when Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) ->
 	  dragging <- Some (xc, yc);
 	  true
       |	_ -> false

sw/lib/ocaml/mapWaypoints.ml

@@ -31,12 +31,12 @@ let losange = [|s;0.; 0.;s; -.s;0.; 0.;-.s|]
 
 class group = fun ?(color="red") ?(editable=true) (geomap:MapCanvas.widget) ->
   let g = GnoCanvas.group geomap#canvas#root in
- object
+  object
     method group=g
     method geomap=geomap
     method color=color
-   method editable=editable
-end
+    method editable=editable
+  end
 
 class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
   let geomap=group#geomap
@@ -53,6 +53,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
     val label = GnoCanvas.text group#group ~props:[`TEXT name; `X s; `Y 0.; `ANCHOR `SW; `FILL_COLOR "green"]
     val mutable name = name
     val mutable alt = alt
+    val mutable moved = false
     initializer self#move xw yw
     method name = name
     method set_name n =
@@ -114,12 +115,15 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
 	      x0 <- x; y0 <- y
 	    end
 	| `BUTTON_RELEASE ev ->
-	    if GdkEvent.Button.button ev = 2 then
-	      item#ungrab (GdkEvent.Button.time ev)
+	    if GdkEvent.Button.button ev = 2 then begin
+	      item#ungrab (GdkEvent.Button.time ev);
+	      moved <- true
+	    end
 	| _ -> ()
       end;
       true
     initializer ignore(if editable then ignore (item#connect#event self#event))
+    method moved = moved
     method item = item
     method en =
       let (dx, dy) = self#xy in

sw/lib/ocaml/mapWaypoints.mli

@@ -41,12 +41,6 @@ class waypoint :
   ?alt:float ->
   MapCanvas.en ->
   object
-    val mutable alt : float
-    val item : GnoCanvas.polygon
-    val label : GnoCanvas.text
-    val mutable name : string
-    val mutable x0 : float
-    val mutable y0 : float
     method alt : float
     method delete : unit
     method edit : unit
@@ -60,6 +54,7 @@ class waypoint :
     method set_name : string -> unit
     method xy : float * float
     method zoom : float -> unit
+    method moved : bool
   end
 
 

sw/simulator/sim_ap.c

@@ -130,3 +130,13 @@ value move_waypoint(value wp_id, value ux, value uy, value a) {
   MoveWaypoint(Int_val(wp_id), Double_val(ux), Double_val(uy), Double_val(a));
   return Val_unit;
 }
+
+value send_event(value event_id) {
+  uint8_t event = Int_val(event_id);
+  switch (event) {
+    case 1 : rc_event_1 = TRUE; break; // FIXME !
+    case 2 : rc_event_2 = TRUE; break;
+    default: ;
+  }
+  return Val_unit;
+}

sw/simulator/sitl.ml

@@ -147,7 +147,7 @@ module Make(A:Data.MISSION) = struct
   external move_waypoint : int -> float -> float -> float -> unit = "move_waypoint"
   let get_move_waypoint = fun _sender vs ->
     let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
-    if ac_id == !my_id then
+    if ac_id = !my_id then
       let f = fun a -> Pprz.float_assoc a vs in
       let wp_id = Pprz.int_assoc "wp_id" vs
       and ux = f "utm_east"
@@ -155,13 +155,20 @@ module Make(A:Data.MISSION) = struct
       and alt = f "alt" in
       move_waypoint wp_id ux uy alt
 
+  external send_event : int -> unit = "send_event"
+  let get_send_event = fun _sender vs ->
+    let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+    if ac_id = !my_id then
+      send_event (Pprz.int_assoc "event_id" vs)
+
 
   let boot = fun time_scale ->
     Stdlib.timer ~scale:time_scale servos_period (update_servos bat_button);
     Stdlib.timer ~scale:time_scale periodic_period periodic_task;
     Stdlib.timer ~scale:time_scale rc_period rc_task;
     ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_flight_param);
-    ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" get_move_waypoint)
+    ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" get_move_waypoint);
+    ignore (Ground_Pprz.message_bind "SEND_EVENT" get_send_event)
 
 (* Functions called by the simulator *)
   let servos = fun s -> rservos := s