handling of moved waypoints

conf/messages.xml

@@ -188,7 +188,7 @@
     <field name="yaw" type="int16"/>
   </message>
 
-  <message name="ACINFO" ID="27">
+  <message name="DEBUG_ACINFO" ID="27">
     <field name="east"  type="float" unit="m"/>
     <field name="north"  type="float" unit="m"/>
     <field name="course" type="float" unit="rad"/>
@@ -226,6 +226,13 @@
     <field name="foo" type="uint8[]"/>
  </message>
 
+ <message name="WP_MOVED" id="35">
+    <field name="wp_id" type="uint8"/>
+    <field name="utm_east" type="float" unit="m"></field>
+    <field name="utm_north" type="float" unit="m"></field>
+    <field name="alt" type="float" unit="m"></field>
+ </message>
+
 
   <message name="PPM" ID="100">
     <field name="values" type="uint16[]" unit="ticks"/>
@@ -493,6 +500,13 @@
   <field name="message" type="string" format=";sv"/>
  </message>
 
+ <message name="WAYPOINT_MOVED" id="30">
+    <field name="ac_id" type="uint8"/>
+    <field name="wp_id" type="uint8"/>
+    <field name="lat"  type="float" unit="deg"></field>
+    <field name="long"  type="float" unit="deg"></field>
+    <field name="alt" type="float" unit="m"></field>
+ </message>
 </class>
 
 <class name="alert">

conf/telemetry/default.xml

@@ -17,9 +17,10 @@
       <message name="CIRCLE"         period="1"/>
       <message name="SEGMENT"        period="1"/>
       <message name="IMU"            period="0.5"/>
-      <message name="ACINFO"         period="1"/>
+      <message name="DEBUG_ACINFO"   period="1"/>
       <message name="DEBUG_MODEM"    period="5"/>
       <message name="DL_VALUE"       period="1"/>
+      <message name="WP_MOVED"       period="0.5"/>
     </mode>
     <mode name="debug">
       <message name="CALIB_START"    period="0.5"/> 

sw/airborne/ap_downlink.h

@@ -75,14 +75,28 @@ extern uint8_t telemetry_mode_Ap;
 #define PERIODIC_SEND_NAVIGATION_REF()  DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
 
 #if defined DOWNLINK && defined DATALINK
-#define PERIODIC_SEND_ACINFO() { \
+#define PERIODIC_SEND_DEBUG_ACINFO() { \
   struct ac_info_ *s=get_ac_info(3); \
-  DOWNLINK_SEND_ACINFO(&s->east, &s->north, &s->course, &s->alt, &s->gspeed); \
+  DOWNLINK_SEND_DEBUG_ACINFO(&s->east, &s->north, &s->course, &s->alt, &s->gspeed); \
 }
 #else
-#define PERIODIC_SEND_ACINFO() {}
+#define PERIODIC_SEND_DEBUG_ACINFO() {}
 #endif
 
+#define PERIODIC_SEND_WP_MOVED() { \
+  static uint8_t i; \
+  uint8_t j = 0; \
+  i++; if (i > nb_waypoint) i = 0; \
+  while (! moved_waypoints[i] && j <= nb_waypoint) { \
+    j++; i++; if (i > nb_waypoint) i = 0; \
+  } \
+ if (j <= nb_waypoint) { \
+    float x = nav_utm_east0 +  waypoints[i].x; \
+    float y = nav_utm_north0 + waypoints[i].y; \
+    DOWNLINK_SEND_WP_MOVED(&i, &x, &y, &(waypoints[i].a)); \
+  } \
+}
+
 #ifdef RADIO_CONTROL_CALIB
 #define PERIODIC_SEND_SETTINGS() if (inflight_calib_mode != IF_CALIB_MODE_NONE)	DOWNLINK_SEND_SETTINGS(&slider_1_val, &slider_2_val);
 #else

sw/airborne/datalink.c

@@ -34,6 +34,8 @@
 #include "datalink.h"
 #include "flight_plan.h"
 #include "autopilot.h"
+#include "ap_downlink.h"
+#include "messages.h"
 
 #include "estimator.h"
 #include "pid.h"
@@ -62,6 +64,7 @@ void dl_parse_msg(void) {
     float uy = MOfCm(DL_MOVE_WP_utm_north(dl_buffer));
     float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
     MoveWaypoint(wp_id, ux, uy, a);
+    DOWNLINK_SEND_WP_MOVED(&wp_id, &ux, &uy, &a);
   } else if (msg_id == DL_EVENT) {
     uint8_t event = DL_EVENT_event(dl_buffer);
     switch (event) {

sw/airborne/nav.c

@@ -197,6 +197,7 @@ bool_t too_far_from_home;
 const uint8_t nb_waypoint = NB_WAYPOINT;
 
 struct point waypoints[NB_WAYPOINT+1] = WAYPOINTS;
+bool_t moved_waypoints[NB_WAYPOINT+1];
 
 
 

sw/airborne/nav.h

@@ -50,7 +50,9 @@ extern const int32_t nav_utm_north0;
 extern const uint8_t nav_utm_zone0;
 
 extern const uint8_t nb_waypoint;
-extern struct point waypoints[];
+extern struct point waypoints[]; /** size == nb_waypoint + 1 */
+extern bool_t moved_waypoints[]; /** size == nb_waypoint + 1 */
+
 extern float desired_altitude, desired_x, desired_y;
 
 extern uint16_t nav_desired_gaz;
@@ -85,6 +87,7 @@ extern uint8_t horizontal_mode;
     waypoints[_id].x = _ux - nav_utm_east0; \
     waypoints[_id].y = _uy - nav_utm_north0; \
     waypoints[_id].a = _a; \
+    moved_waypoints[_id] = TRUE; \
   } \
 }
 

sw/ground_segment/cockpit/map2d.ml

@@ -774,9 +774,9 @@ let button_press = fun (geomap:G.widget) ev ->
   let active_dl_settings = fun ac_id x ->
     let ac = Hashtbl.find live_aircrafts ac_id in
     let w = ac.dl_settings_window in
-    if x then w#show () else w#misc#hide ();;
+    if x then w#show () else w#misc#hide ()
 
-let blocks_of_stages = fun stages ->
+  let blocks_of_stages = fun stages ->
   let blocks = ref [] in
   List.iter (fun x ->
     let name = ExtXml.attrib x "block_name"
@@ -786,14 +786,20 @@ let blocks_of_stages = fun stages ->
     (Xml.children stages);
   List.sort compare !blocks
 
-let menu_entry_of_block = fun ac (id, name) ->
+  let menu_entry_of_block = fun ac (id, name) ->
     let send_msg = fun () ->
       Ground_Pprz.message_send "map2d" "JUMP_TO_BLOCK" 
 	["ac_id", Pprz.String ac; "block_id", Pprz.Int id] in
     `I (name, send_msg)
 
+  let reset_waypoints = fun fp () ->
+    List.iter (fun w ->
+      let (i, w) = fp#index w in
+      w#reset_moved ())
+      fp#waypoints
 
-let create_ac = fun (geomap:G.widget) ac_id config ->
+
+  let create_ac = fun (geomap:G.widget) ac_id config ->
     let color = Pprz.string_assoc "default_gui_color" config
     and name = Pprz.string_assoc "ac_name" config in
 
@@ -824,6 +830,7 @@ let create_ac = fun (geomap:G.widget) ac_id config ->
     
     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_id track));
+    let reset_wp_menu = ac_menu_fact#add_item "Reset Waypoints" in
 
     let jump_block_entries = List.map (menu_entry_of_block ac_id) blocks in
     
@@ -848,6 +855,7 @@ let create_ac = fun (geomap:G.widget) ac_id config ->
 
     let fp = load_mission color geomap fp_xml in
     fp#hide ();
+    ignore (reset_wp_menu#connect#activate (reset_waypoints fp));
     Hashtbl.add live_aircrafts ac_id { track = track; color = color; 
 				       fp_group = fp ; config = config ; 
 				       fp = fp_xml;
@@ -859,8 +867,14 @@ let create_ac = fun (geomap:G.widget) ac_id config ->
 				     };
     ignore (Strip.add strip_table ac_id config color)
 
+  (** Bind to message while catching all the esceptions of the callback *)
+  let safe_bind = fun msg cb ->
+    let safe_cb = fun sender vs ->
+      try cb sender vs with _ -> () in
+    ignore (Ground_Pprz.message_bind msg safe_cb)
       
-let ask_config = fun geomap ac ->
+
+  let ask_config = fun geomap ac ->
     let get_config = fun _sender values ->
       create_ac geomap ac values
     in
@@ -868,12 +882,12 @@ let ask_config = fun geomap ac ->
 
       
 
-let one_new_ac = fun (geomap:G.widget) ac ->
+  let one_new_ac = fun (geomap:G.widget) ac ->
     if not (Hashtbl.mem live_aircrafts ac) then begin
       ask_config geomap ac
     end
 
-let get_wind_msg = fun sender vs ->
+  let get_wind_msg = fun sender vs ->
     try
       let ac_strip = Strip.find (Pprz.string_assoc "ac_id" vs) in
       let set_label = fun label_name field_name ->
@@ -885,7 +899,7 @@ let get_wind_msg = fun sender vs ->
     with
       Not_found -> ()
 
-let get_fbw_msg = fun sender vs ->
+  let get_fbw_msg = fun sender vs ->
     try
       let ac_strip = Strip.find (Pprz.string_assoc "ac_id" vs) in
       Strip.set_label ac_strip "RC" (Pprz.string_assoc "rc_status" vs)
@@ -893,7 +907,7 @@ let get_fbw_msg = fun sender vs ->
       Not_found -> ()
 	  
 
-let get_engine_status_msg = fun sender vs ->
+  let get_engine_status_msg = fun sender vs ->
     try
       let ac_strip = Strip.find (Pprz.string_assoc "ac_id" vs) in
       Strip.set_label ac_strip "throttle" 
@@ -902,34 +916,34 @@ let get_engine_status_msg = fun sender vs ->
     with
       Not_found -> ()
 	  
-let get_if_calib_msg = fun sender vs ->
+  let get_if_calib_msg = fun sender vs ->
     try
       let ac_strip = Strip.find (Pprz.string_assoc "ac_id" vs) in
       Strip.set_label ac_strip "settings" (Pprz.string_assoc "if_mode" vs)
     with
       Not_found -> ()
 
-let listen_wind_msg = fun () ->
+  let listen_wind_msg = fun () ->
-  ignore (Ground_Pprz.message_bind "WIND" get_wind_msg)
+    safe_bind "WIND" get_wind_msg
 
-let listen_fbw_msg = fun () ->
+  let listen_fbw_msg = fun () ->
-  ignore (Ground_Pprz.message_bind "FLY_BY_WIRE" get_fbw_msg)
+    safe_bind "FLY_BY_WIRE" get_fbw_msg
 
-let listen_engine_status_msg = fun () ->
+  let listen_engine_status_msg = fun () ->
-  ignore (Ground_Pprz.message_bind "ENGINE_STATUS" get_engine_status_msg)
+    safe_bind "ENGINE_STATUS" get_engine_status_msg
 
-let listen_if_calib_msg = fun () ->
+  let listen_if_calib_msg = fun () ->
-  ignore (Ground_Pprz.message_bind "INFLIGH_CALIB" get_if_calib_msg)
+   safe_bind "INFLIGH_CALIB" get_if_calib_msg
 
-let list_separator = Str.regexp ","
+  let list_separator = Str.regexp ","
       
-let aircrafts_msg = fun (geomap:G.widget) acs ->
+  let aircrafts_msg = fun (geomap:G.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
 
 
-let listen_dl_value = fun () ->
+  let listen_dl_value = fun () ->
     let get_dl_value = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
       try
@@ -942,10 +956,10 @@ let listen_dl_value = fun () ->
 	done
       with Not_found -> ()
     in
-  ignore (Ground_Pprz.message_bind "DL_VALUES" get_dl_value)
+    safe_bind "DL_VALUES" get_dl_value
 
 
-let listen_flight_params = fun () ->
+  let listen_flight_params = fun () ->
     let get_fp = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
       try
@@ -969,7 +983,7 @@ let listen_flight_params = fun () ->
 	set_label "climb" "climb"
       with Not_found -> ()
     in
-  ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_fp);
+    safe_bind "FLIGHT_PARAM" get_fp;
 
     let get_ns = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
@@ -988,7 +1002,7 @@ let listen_flight_params = fun () ->
 	set_label "/" "target_climb"
       with Not_found -> ()
     in
-  ignore (Ground_Pprz.message_bind "NAV_STATUS" get_ns);
+    safe_bind "NAV_STATUS" get_ns;
 
     let get_cam_status = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
@@ -999,8 +1013,8 @@ let listen_flight_params = fun () ->
 	and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in
 	
 	cam_pos_msg ac.track wgs84 target_wgs84
-    with Not_found -> ()
+      with Not_found -> () in
-  in ignore (Ground_Pprz.message_bind "CAM_STATUS" get_cam_status);
+    safe_bind "CAM_STATUS" get_cam_status;
 
     let get_circle_status = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
@@ -1011,7 +1025,7 @@ let listen_flight_params = fun () ->
 	circle_status_msg ac.track wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) 
       with Not_found -> ()
     in
-  ignore (Ground_Pprz.message_bind "CIRCLE_STATUS" get_circle_status);
+    safe_bind "CIRCLE_STATUS" get_circle_status;
 
     let get_segment_status = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
@@ -1023,7 +1037,7 @@ let listen_flight_params = fun () ->
 	segment_status_msg ac.track geo1 geo2
       with Not_found -> ()
     in
-  ignore (Ground_Pprz.message_bind "SEGMENT_STATUS" get_segment_status);
+    safe_bind "SEGMENT_STATUS" get_segment_status;
 
     let get_ap_status = fun _sender vs ->
       let ac_id = Pprz.string_assoc "ac_id" vs in
@@ -1041,9 +1055,30 @@ let listen_flight_params = fun () ->
       with 
 	Not_found -> ()
     in
-  ignore (Ground_Pprz.message_bind "AP_STATUS" get_ap_status);
+    safe_bind "AP_STATUS" get_ap_status;
 
     listen_dl_value ()
+
+  let listen_waypoint_moved = fun () ->
+    let get_values = fun _sender vs ->
+      let ac_id = Pprz.string_assoc "ac_id" vs in
+      let ac = Hashtbl.find live_aircrafts ac_id in
+      (** Not_found catched by safe_bind *)
+      let wp_id = Pprz.int_assoc "wp_id" vs in
+      let a = fun s -> Pprz.float_assoc s vs in
+      let geo = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } in
+
+      (** No indexed access to waypoints: iter and compare: *)
+      List.iter (fun w ->
+	let (i, w) = ac.fp_group#index w in
+	if i = wp_id then begin
+	  if not w#moved then w#set geo;
+	  raise Exit (** catched by  safe_bind *)
+	end)
+	ac.fp_group#waypoints
+    in
+    safe_bind "WAYPOINT_MOVED" get_values
+    
 end (** module Live *)
 
 
@@ -1164,7 +1199,7 @@ let _main =
   ignore (Glib.Timeout.add 2000 (fun () -> Live.Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> Live.aircrafts_msg geomap vs); false));
 
   (** New aircraft message *)
-  ignore (Live.Ground_Pprz.message_bind "NEW_AIRCRAFT" (fun _sender vs -> Live.one_new_ac geomap (Pprz.string_assoc "ac_id" vs)));
+  Live.safe_bind "NEW_AIRCRAFT" (fun _sender vs -> Live.one_new_ac geomap (Pprz.string_assoc "ac_id" vs));
 
   (** Listen for all messages on ivy *)
   Live.listen_flight_params ();
@@ -1172,6 +1207,7 @@ let _main =
   Live.listen_fbw_msg ();
   Live.listen_engine_status_msg ();
   Live.listen_if_calib_msg ();
+  Live.listen_waypoint_moved ();
 
   (** Display the window *)
   window#add_accel_group accel_group;

sw/ground_segment/tmtc/aircraft.ml

@@ -73,6 +73,8 @@ type horiz_mode =
   | Segment of Latlong.utm * Latlong.utm
   | UnknownHorizMode
 
+type waypoint = { altitude : float; wp_utm : Latlong.utm }
+
 type aircraft = { 
     id : string;
     mutable pos : Latlong.utm;
@@ -107,6 +109,7 @@ type aircraft = {
     infrared : infrared;
     fbw : fbw;
     svinfo : svinfo array;
+    waypoints : (int, waypoint) Hashtbl.t;
     mutable flight_time : int;
     mutable stage_time : int;
     mutable block_time : int;

sw/ground_segment/tmtc/aircraft.mli

@@ -59,6 +59,9 @@ type horiz_mode =
     Circle of Latlong.utm * int
   | Segment of Latlong.utm * Latlong.utm
   | UnknownHorizMode
+
+type waypoint = { altitude : float; wp_utm : Latlong.utm }
+
 type aircraft = {
     id : string;
     mutable pos : Latlong.utm;
@@ -93,6 +96,7 @@ type aircraft = {
     infrared : infrared;
     fbw : fbw;
     svinfo : svinfo array;
+    waypoints : (int, waypoint) Hashtbl.t;
     mutable flight_time : int;
     mutable stage_time : int;
     mutable block_time : int;

sw/ground_segment/tmtc/server.ml

@@ -145,6 +145,9 @@ let ivalue = fun x ->
     Pprz.Int x -> x 
   | _ -> failwith "Receive.log_and_parse: int expected"
 
+let update_waypoint = fun ac wp_id p alt ->
+  Hashtbl.replace ac.waypoints wp_id {altitude = alt; wp_utm = p }
+
 
 
 let log_and_parse = fun logging ac_name a msg values ->
@@ -266,6 +269,16 @@ let log_and_parse = fun logging ac_name a msg values ->
 	  a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1)
 	end else
 	  failwith "Too much dl_setting values !!!"
+    | "WP_MOVED" ->
+	begin
+	  match a.nav_ref with
+	    Some nav_ref ->
+	      let p = { Latlong.utm_x = fvalue "utm_east";
+			 utm_y = fvalue "utm_north";
+			 utm_zone = nav_ref.Latlong.utm_zone } in
+	      update_waypoint a (ivalue "wp_id") p (fvalue "alt")
+	  | None -> () (** Can't use this message  *)
+	end
     | _ -> ()
 
 (** Callback for a message from a registered A/C *)
@@ -393,6 +406,19 @@ let send_wind = fun a ->
   with
     exc -> ()
 
+let send_moved_waypoints = fun a ->
+  Hashtbl.iter
+    (fun wp_id wp ->
+      let geo = Latlong.of_utm WGS84 wp.wp_utm in
+      let vs =
+	["ac_id", Pprz.String a.id;
+	 "wp_id", Pprz.Int wp_id;
+	 "long", Pprz.Float ((Rad>>Deg)geo.posn_long);
+	 "lat", Pprz.Float ((Rad>>Deg)geo.posn_lat);
+	 "alt", Pprz.Float wp.altitude] in
+      Ground_Pprz.message_send my_id "WAYPOINT_MOVED" vs)
+    a.waypoints
+	 
 
 let send_aircraft_msg = fun ac ->
   try
@@ -460,7 +486,8 @@ let send_aircraft_msg = fun ac ->
     send_infrared a;
     send_svsinfo a;
     send_horiz_status a;
-    send_dl_values a
+    send_dl_values a;
+    send_moved_waypoints a
   with
     Not_found -> prerr_endline ac
   | x -> prerr_endline (Printexc.to_string x)
@@ -487,7 +514,8 @@ let new_aircraft = fun id ->
       nb_dl_setting_values = 0;
       flight_time = 0; stage_time = 0; block_time = 0;
       horiz_mode = UnknownHorizMode;
-      horizontal_mode = 0
+      horizontal_mode = 0;
+      waypoints = Hashtbl.create 3
     }
 
 let check_alerts = fun a ->

sw/lib/ocaml/mapWaypoints.ml

@@ -130,6 +130,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
 	      and dy = geomap#current_zoom *. (y -. y0) in
 	      self#move dx dy ;
 	      updated ();
+	      moved <- true;
 	      x0 <- x; y0 <- y
 	    end
 	| `BUTTON_RELEASE ev ->
@@ -142,6 +143,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
       true
     initializer ignore(if editable then ignore (item#connect#event self#event))
     method moved = moved
+    method reset_moved () = moved <- false
     method deleted = deleted
     method item = item
     method pos = geomap#of_world self#xy

sw/lib/ocaml/mapWaypoints.mli

@@ -55,6 +55,7 @@ class waypoint :
     method xy : float * float
     method zoom : float -> unit
     method moved : bool
+    method reset_moved : unit -> unit
     method deleted : bool
     method connect : (unit -> unit) -> unit
   end

sw/tools/gen_messages.ml

@@ -173,23 +173,23 @@ module Gen_onboard = struct
     print_avr_macro_names avr_h fields;
     fprintf avr_h ") {}\n"
 
-  let print_enum = fun avr_h messages ->
+  let print_enum = fun avr_h class_ messages ->
     List.iter (fun m -> fprintf avr_h "#define DL_%s %d\n" m.name m.id) messages;
-    fprintf avr_h "#define DL_MSG_NB %d\n\n" (List.length messages)
+    fprintf avr_h "#define DL_MSG_%s_NB %d\n\n" class_ (List.length messages)
 
-  let print_lengths_array = fun avr_h messages ->
+  let print_lengths_array = fun avr_h class_ messages ->
     let sizes = List.map (fun m -> (m.id, size_of_message m)) messages in
     let max_id = List.fold_right (fun (id, _m) x -> max x id) sizes min_int in
     let n = max_id + 1 in
-    fprintf avr_h "#define MSG_LENGTHS {";
+    fprintf avr_h "#define MSG_%s_LENGTHS {" class_;
     for i = 0 to n - 1 do
       fprintf avr_h "%s," (try "(2+" ^ List.assoc i sizes^")" with Not_found -> "0")
     done;
     fprintf avr_h "}\n\n"
 
-  let print_avr_macros = fun filename avr_h messages ->
+  let print_avr_macros = fun filename avr_h class_ messages ->
-    print_enum avr_h messages;
+    print_enum avr_h class_ messages;
-    print_lengths_array avr_h messages;
+    print_lengths_array avr_h class_ messages;
     List.iter (print_avr_macro avr_h) messages;
     let md5sum = Digest.file filename in
     fprintf avr_h "#define MESSAGES_MD5SUM \"";
@@ -245,7 +245,7 @@ let _ =
   (** Macros for airborne downlink (sending) *)
   if class_name = "telemetry" then (** FIXME *)
     Printf.fprintf avr_h "#ifdef DOWNLINK\n";
-  Gen_onboard.print_avr_macros filename avr_h messages;
+  Gen_onboard.print_avr_macros filename avr_h class_name messages;
   if class_name = "telemetry" then begin
     Printf.fprintf avr_h "#else // DOWNLINK\n";
     Gen_onboard.print_null_avr_macros avr_h messages;