From 591b273a8b31207762068d53bfb36e5ba0f689ca Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Sat, 8 Feb 2014 15:05:00 +0100
Subject: [PATCH] [sim] add button to enable/disable datalink

---
 sw/airborne/arch/sim/ivy_transport.c |  1 +
 sw/airborne/arch/sim/ivy_transport.h |  3 ++-
 sw/airborne/arch/sim/sim_ap.c        |  5 +++++
 sw/simulator/sitl.ml                 | 17 +++++++++++++++--
 4 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/arch/sim/ivy_transport.c b/sw/airborne/arch/sim/ivy_transport.c
index b84f41b831..3afcc4bc9c 100644
--- a/sw/airborne/arch/sim/ivy_transport.c
+++ b/sw/airborne/arch/sim/ivy_transport.c
@@ -1,2 +1,3 @@
 char ivy_buf[256];
 char *ivy_p = ivy_buf;
+int ivy_dl_status;
diff --git a/sw/airborne/arch/sim/ivy_transport.h b/sw/airborne/arch/sim/ivy_transport.h
index 948e68cfc6..295d237f0d 100644
--- a/sw/airborne/arch/sim/ivy_transport.h
+++ b/sw/airborne/arch/sim/ivy_transport.h
@@ -3,6 +3,7 @@
 
 extern char ivy_buf[];
 extern char* ivy_p;
+extern int ivy_dl_status;
 
 #define IvyTransportCheckFreeSpace(_dev,_) TRUE
 
@@ -10,7 +11,7 @@ extern char* ivy_p;
 
 #define IvyTransportHeader(_dev,len) ivy_p=ivy_buf;
 
-#define IvyTransportTrailer(_dev) { *(--ivy_p) = '\0'; IvySendMsg("%s",ivy_buf); }
+#define IvyTransportTrailer(_dev) { *(--ivy_p) = '\0'; if (ivy_dl_status) { IvySendMsg("%s",ivy_buf); } }
 
 #define IvyTransportPutUint8(_dev,x) { ivy_p += sprintf(ivy_p, "%u ", x); }
 #define IvyTransportPutNamedUint8(_dev,_name, _x) { ivy_p += sprintf(ivy_p, "%s ", _name); }
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index 377ca70e6f..3924f4ee1d 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -109,6 +109,11 @@ value update_bat(value bat) {
   return Val_unit;
 }
 
+value update_dl_status(value dl_status) {
+  ivy_dl_status = Int_val(dl_status);
+  return Val_unit;
+}
+
 
 value get_commands(value val_commands) {
   int i;
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 4f7a5998aa..6f9a7d73fa 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -130,8 +130,10 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
   external sim_init : unit -> unit = "sim_init"
   external update_bat : int -> unit = "update_bat"
   external update_adc1 : int -> unit = "update_adc1"
+  external update_dl_status : int -> unit = "update_dl_status"
 
   let bat_button = GButton.check_button ~label:"Auto" ~active:false ()
+  let dl_button = GButton.check_button ~label:"Enable Datalink/Telemetry" ~active:true ()
 
   let my_id = ref (-1)
   let init = fun id vbox ->
@@ -140,6 +142,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     my_id := id;
     sim_init ();
 
+    (* Bat level *)
     let hbox = GPack.hbox ~spacing:4 ~packing:vbox#add () in
     let _label = GMisc.label ~text:"Bat (V) " ~packing:hbox#pack () in
     let _scale = GRange.scale `HORIZONTAL ~adjustment:adj_bat ~packing:hbox#add () in
@@ -150,6 +153,16 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     ignore (adj_bat#connect#value_changed update);
     update ();
 
+    (* Datalink status *)
+    let hbox = GPack.hbox ~spacing:4 ~packing:vbox#add () in
+    let update = fun () -> update_dl_status (if dl_button#active then 1 else 0) in
+    hbox#pack dl_button#coerce;
+    let tips = GData.tooltips () in
+    tips#set_tip dl_button#coerce ~text:"Enable/disable communication with the aircraft";
+    ignore (dl_button#connect#toggled update);
+    update();
+
+    (* ADC1 *)
     if !adc1 then
       let hbox = GPack.hbox ~spacing:4 ~packing:vbox#add () in
       let _label = GMisc.label ~text:"Generic ADC 1 " ~packing:hbox#pack () in
@@ -169,8 +182,8 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
       set_message (Serial.string_of_payload s) in
     let ac_id = Pprz.int_assoc "ac_id" vs in
     match link_mode with
-      Pprz.Forwarded when ac_id = !my_id -> set ()
-    | Pprz.Broadcasted when ac_id <> !my_id -> set ()
+      Pprz.Forwarded when ac_id = !my_id -> if dl_button#active then set ()
+    | Pprz.Broadcasted when ac_id <> !my_id -> if dl_button#active then set ()
     | _ -> ()
 
   let message_bind = fun name link_mode ->