Improve message header handling. (#2837)

* Improve message header handling.

* Update pprzlink

sw/ground_segment/tmtc/ivy2serial.ml

@@ -71,7 +71,8 @@ let () =
   let get_ivy_message = fun _ args ->
     try
       let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in
-      let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) vs in
+      (* destination is always 0 (ground) ? *)
+      let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) 0 vs in
       let buf = Pprz_transport.Transport.packet payload in
       let o = Unix.out_channel_of_descr fd in
       Printf.fprintf o "%s" buf; flush o
@@ -91,8 +92,8 @@ let () =
 
         let use_dl_message = fun payload ->
           Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload));
-          let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
-          let msg = Dl_Pprz.message_of_id msg_id in
+          let (header, values) = Dl_Pprz.values_of_payload payload in
+          let msg = Dl_Pprz.message_of_id header.message_id in
           Dl_Pprz.message_send "ground_dl" msg.PprzLink.name values in
 
         assert (PprzTransport.parse use_dl_message (Bytes.to_string b) = n)

sw/ground_segment/tmtc/ivy2udp.ml

@@ -60,7 +60,8 @@ let () =
   let get_ivy_message = fun _ args ->
     try
       let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in
-      let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) vs in
+      (* receiver_id unknown, using 0 instead... *)
+      let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) 0 vs in
       let buf = Pprz_transport.Transport.packet payload in
       let n = String.length buf in
       let n' = Unix.sendto socket (Bytes.of_string buf) 0 n [] sockaddr in
@@ -85,8 +86,8 @@ let () =
 
         let use_dl_message = fun payload ->
           Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload));
-          let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
-          let msg = Dl_Pprz.message_of_id msg_id in
+          let (header, values) = Dl_Pprz.values_of_payload payload in
+          let msg = Dl_Pprz.message_of_id header.message_id in
           Dl_Pprz.message_send "ground_dl" msg.PprzLink.name values in
 
         assert (PprzTransport.parse use_dl_message (Bytes.to_string b) = n)

sw/ground_segment/tmtc/ivy_tcp_aircraft.ml

@@ -33,7 +33,8 @@ let () =
   let get_ivy_message = fun _ args ->
     try
       let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in
-      let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) vs in
+      (* receiver_id unknown, using 0 instead... *)
+      let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) 0 vs in
       let buf = Pprz_transport.Transport.packet payload in
       fprintf o "%s%!" buf
     with _ -> () in
@@ -51,8 +52,8 @@ let () =
 
         let use_dl_message = fun payload ->
           Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload));
-          let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
-          let msg = Dl_Pprz.message_of_id msg_id in
+          let (header, values) = Dl_Pprz.values_of_payload payload in
+          let msg = Dl_Pprz.message_of_id header.message_id in
           Dl_Pprz.message_send "ground_dl" msg.PprzLink.name values in
 
         assert (PprzTransport.parse use_dl_message (Bytes.to_string b) = n)

sw/ground_segment/tmtc/ivy_tcp_controller.ml

@@ -39,9 +39,9 @@ let () =
 
         let use_tele_message = fun payload ->
           Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload));
-          let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in
-          let msg = Tm_Pprz.message_of_id msg_id in
-          Tm_Pprz.message_send (string_of_int ac_id) msg.PprzLink.name values in
+          let (header, values) = Tm_Pprz.values_of_payload payload in
+          let msg = Tm_Pprz.message_of_id header.message_id in
+          Tm_Pprz.message_send (string_of_int header.sender_id) msg.PprzLink.name values in
 
         ignore (PprzTransport.parse use_tele_message (Bytes.to_string b))
       with
@@ -58,7 +58,8 @@ let () =
     try
       let (msg_id, vs) = Dl_Pprz.values_of_string args.(0) in
       let ac_id = PprzLink.int_assoc "ac_id" vs in
-      let payload = Dl_Pprz.payload_of_values msg_id ac_id vs in
+      (* receiver_id unknow, using 0 instead... *)
+      let payload = Dl_Pprz.payload_of_values msg_id ac_id 0 vs in
       let buf = Pprz_transport.Transport.packet payload in
       fprintf o "%s%!" buf
     with exc -> prerr_endline (Printexc.to_string exc) in

sw/ground_segment/tmtc/link.ml

@@ -199,8 +199,9 @@ let use_tele_message = fun ?udp_peername ?raw_data_size payload ->
   let buf = Protocol.string_of_payload payload in
   Debug.call 'l' (fun f ->  fprintf f "pprz receiving: %s\n" (Debug.xprint buf));
   try
-    let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in
-    let msg = Tm_Pprz.message_of_id msg_id in
+    let (header, values) = Tm_Pprz.values_of_payload payload in
+    let ac_id = header.sender_id in
+    let msg = Tm_Pprz.message_of_id header.message_id in
     send_message_over_ivy (string_of_int ac_id) msg.PprzLink.name values;
     update_status ?udp_peername ac_id raw_data_size (msg.PprzLink.name = "PONG")
   with
@@ -394,7 +395,7 @@ let message_uplink = fun device ->
     Debug.call 'f' (fun f -> fprintf f "forward %s\n" name);
     let ac_id = PprzLink.int_assoc "ac_id" vs in
     let msg_id, _ = Dl_Pprz.message_of_name name in
-    let s = Dl_Pprz.payload_of_values msg_id my_id vs in
+    let s = Dl_Pprz.payload_of_values msg_id my_id ac_id vs in
     send ac_id device s High in
   let set_forwarder = fun name ->
     ignore (Dl_Pprz.message_bind name (forwarder name)) in
@@ -402,7 +403,7 @@ let message_uplink = fun device ->
   let broadcaster = fun name _sender vs ->
     Debug.call 'f' (fun f -> fprintf f "broadcast %s\n" name);
     let msg_id, _ = Dl_Pprz.message_of_name name in
-    let payload = Dl_Pprz.payload_of_values msg_id my_id vs in
+    let payload = Dl_Pprz.payload_of_values msg_id my_id PprzLink.broadcast_id vs in
     broadcast device payload Low in
   let set_broadcaster = fun name ->
     ignore (Dl_Pprz.message_bind name (broadcaster name)) in
@@ -420,7 +421,7 @@ let send_ping_msg = fun device ->
   Hashtbl.iter
     (fun ac_id status ->
       let msg_id, _ = Dl_Pprz.message_of_name "PING" in
-      let s = Dl_Pprz.payload_of_values msg_id my_id [] in
+      let s = Dl_Pprz.payload_of_values msg_id my_id ac_id [] in
       send ac_id device s High;
       status.last_ping <- Unix.gettimeofday ()
     )

sw/logalizer/play_core.ml

@@ -169,7 +169,8 @@ let run = fun serial_port log adj i0 speed no_gui ->
         | Some channel ->
             try
               let msg_id, vs = Tm_Pprz.values_of_string m in
-              let payload = Tm_Pprz.payload_of_values msg_id (int_of_string ac) vs in
+              (* receiver_id lost in Ivy logs, but it was probably 0. *)
+              let payload = Tm_Pprz.payload_of_values msg_id (int_of_string ac) 0 vs in
               let buf = Pprz_transport.Transport.packet payload in
               Debug.call 'o' (fun f -> fprintf f "%s\n" (Debug.xprint buf));
               fprintf channel "%s%!" buf

sw/logalizer/sd2log.ml

@@ -142,7 +142,9 @@ let convert_file = fun ?(output_dir=None) file ->
     if log_msg.Pprzlog_transport.source > 1 then
       fprintf stderr "Invalid source (%d), skipping message\n" log_msg.Pprzlog_transport.source
     else
-    let (msg_id, ac_id, vs) = values_of_payload log_msg log_msg.Pprzlog_transport.pprz_data in
+    let (header, vs) = values_of_payload log_msg log_msg.Pprzlog_transport.pprz_data in
+    let msg_id = header.message_id in
+    let ac_id = header.sender_id in
 
     if log_msg.Pprzlog_transport.source = 0 && !single_ac_id < 0 then
       single_ac_id := ac_id;

sw/simulator/sitl.ml

@@ -171,14 +171,14 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
 
   external set_message : string -> unit = "set_datalink_message"
   let get_message = fun name link_mode _sender vs ->
-    let set = fun () ->
+    let set = fun rcv_id ->
       let msg_id, _ = Dl_Pprz.message_of_name name in
-      let s = Dl_Pprz.payload_of_values msg_id ground_id vs in
+      let s = Dl_Pprz.payload_of_values msg_id ground_id rcv_id vs in
       set_message (Protocol.string_of_payload s) in
     let ac_id = try Some (PprzLink.int_assoc "ac_id" vs) with _ -> None in
     match link_mode, ac_id with
-      PprzLink.Forwarded, Some x when x = !my_id -> if dl_button#active then set ()
-    | PprzLink.Broadcasted, _ -> if dl_button#active then set ()
+      PprzLink.Forwarded, Some x when x = !my_id -> if dl_button#active then set x
+    | PprzLink.Broadcasted, _ -> if dl_button#active then set PprzLink.broadcast_id
     | _ -> ()
 
   let message_bind = fun name link_mode ->