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/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 *)
@@ -392,7 +405,20 @@ let send_wind = fun a ->
     Ground_Pprz.message_send my_id "WIND" vs
   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;