settings using datalink

sw/airborne/autopilot/datalink.c

@@ -35,8 +35,7 @@
 #include "flight_plan.h"
 #include "autopilot.h"
 
-/***/
-#include "uart.h"
+#include "estimator.h"
 
 #define MOfCm(_x) (((float)_x)/100.)
 
@@ -64,5 +63,7 @@ void dl_parse_msg(void) {
     case 2 : rc_event_2 = TRUE; break;
     default: ;
     }
+  } else if (msg_id == DL_SETTING_ID) {
+    DlSetting(DL_SETTING_index(dl_buffer), DL_SETTING_value(dl_buffer));
   }
 }

sw/ground_segment/tmtc/wavecard_connect.ml

@@ -113,6 +113,17 @@ let send_event = fun ac _sender vs ->
     send ac s
     
 
+(** Got a DL_SETTING, and send an SETTING *)
+let setting = fun ac _sender vs ->
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  if ac_id = ac.id then
+    let idx = Pprz.int_assoc "index" vs in
+    let vs = ["event", Pprz.Int idx; "value", List.assoc "value" vs] in
+    let msg_id, _ = Dl_Pprz.message_of_name "SETTING" in
+    let s = Dl_Pprz.payload_of_values msg_id vs in
+    send ac s
+    
+
 let _ =
   let ivy_bus = ref "127.255.255.255:2010" in
   let port = ref "/dev/ttyS0" in
@@ -150,6 +161,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));
+    ignore (Ground_Pprz.message_bind "DL_SETTING" (setting ac));
     (* For debug *)
     ignore (Ivy.bind (fun _ a -> send_dl_msg ac a.(0)) "TO_WAVECARD +(.*)");
 

sw/simulator/sim_ap.c

@@ -140,3 +140,10 @@ value send_event(value event_id) {
   }
   return Val_unit;
 }
+
+value dl_setting(value index, value val) {
+#if defined DlSetting
+  DlSetting(Int_val(index), Double_val(val));
+#endif
+  return Val_unit;
+}

sw/simulator/sitl.ml

@@ -162,13 +162,21 @@ module Make(A:Data.MISSION) = struct
       send_event (Pprz.int_assoc "event_id" vs)
 
 
+  external dl_setting : int -> float -> unit = "dl_setting"
+  let get_dl_setting = fun _sender vs ->
+    let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+    if ac_id = !my_id then
+      dl_setting (Pprz.int_assoc "index" vs) (Pprz.float_assoc "value" 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 "SEND_EVENT" get_send_event)
+    ignore (Ground_Pprz.message_bind "SEND_EVENT" get_send_event);
+    ignore (Ground_Pprz.message_bind "DL_SETTING" get_dl_setting)
 
 (* Functions called by the simulator *)
   let servos = fun s -> rservos := s

sw/tools/gen_flight_plan.ml

@@ -542,6 +542,18 @@ let print_heights = fun xml wgs84 alt ->
     lprintf "} \n"; 
   end
 
+let print_dl_settings = fun settings ->
+  lprintf "#define DlSetting(_idx, _value) { \\\n";
+  right ();
+  let idx = ref 0 in
+  List.iter 
+    (fun s ->
+      let v = ExtXml.attrib s "var" in
+      lprintf "if (_idx == %d) %s = _value;\\\n" !idx v; incr idx) 
+    settings;
+  left ();
+  lprintf "}\n"
+
 
 
 let _ =
@@ -562,6 +574,7 @@ let _ =
     let xml = Fp_proc.process_includes dir xml in
     let xml = Fp_proc.process_relative_waypoints xml in
     let waypoints = ExtXml.child xml "waypoints"
+    and dl_settings = try Xml.children (ExtXml.child xml "dl_settings") with Not_found -> []
     and blocks = Xml.children (ExtXml.child xml "blocks") in
 
     compile_blocks blocks;
@@ -638,6 +651,8 @@ let _ =
 
       print_heights xml wgs84 (int_of_string alt);
 
+      print_dl_settings dl_settings;
+
       Xml2h.finish h_name
     end
   with