real values for datalink settings

conf/flight_plans/mav05_ccw.xml

@@ -12,6 +12,8 @@
   <dl_settings>
     <dl_setting VAR="ir_roll_neutral" MAX="0.2" STEP="0.01" MIN="-0.2"/>
     <dl_setting VAR="roll_pgain" MAX="15000" STEP="100" MIN="5000"/>
+    <dl_setting VAR="phi_c" MIN="-1." STEP="0.05" MAX="1."/>
+    <dl_setting VAR="theta_c" MIN="-1." STEP="0.05" MAX="1."/>
   </dl_settings>
   <waypoints>
     <waypoint Y="30" NAME="HOME" X="-30"/>

conf/messages.xml

@@ -206,10 +206,15 @@
   <field name="range" type="uint16"/>
  </message>
 
-
-<message name="DEBUG_MODEM" id="30" period="5">
+ <message name="DEBUG_MODEM" id="30" period="5">
     <field name="modem_nb_err" type="uint8"></field>
-</message>
+ </message>
+
+<!-- Current value of one variable from the dl_settings (airframe.xml) -->
+ <message name="DL_VALUE" id="31" period="1">
+    <field name="index" type="uint8"/>
+    <field name="value" type="float"/>
+ </message>
 
 </class>
 
@@ -454,6 +459,11 @@
   <field name="block_id" type="uint8"/>
  </message>
 
+ <message name="DL_VALUES" ID="28">
+  <field name="ac_id" type="string"/>
+  <field name="values" type="string" format="csv"/>
+ </message>
+
 </class>
 
 <class name="alert">

sw/airborne/datalink.c

@@ -37,6 +37,7 @@
 
 #include "estimator.h"
 #include "pid.h"
+#include "cam.h"
 
 #define MOfCm(_x) (((float)_x)/100.)
 

sw/airborne/downlink.h

@@ -117,5 +117,7 @@
 
 #define SEND_CAM() Downlink({ int16_t x = target_x; int16_t y = target_y; int8_t phi = DegOfRad(phi_c); int8_t theta = DegOfRad(theta_c); DOWNLINK_SEND_CAM(&phi, &theta, &x, &y);})
 
+#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() /** from flight_plan.h*/
+
 
 #endif /* DOWNLINK_H */

sw/airborne/infrared.c

@@ -62,16 +62,15 @@ float estimator_rad_of_ir, estimator_ir, estimator_rad;
 static struct adc_buf buf_ir1;
 static struct adc_buf buf_ir2;
 
-/** \fn void ir_init(void)
- *  \brief Initialisation of \a ir */
-/** Initialize \a ir with the \a IR_DEFAULT_CONTRAST \n
- *  Initialize \a adc_buf_channel
- */
 #ifndef ADC_CHANNEL_IR_NB_SAMPLES
 #define ADC_CHANNEL_IR_NB_SAMPLES DEFAULT_AV_NB_SAMPLE
 #else
 #warning " ADC_CHANNEL_IR_NB_SAMPLES";
 #endif
+/** \brief Initialisation of \a ir */
+/** Initialize \a ir with the \a IR_DEFAULT_CONTRAST \n
+ *  Initialize \a adc_buf_channel
+ */
 void ir_init(void) {
   RadOfIrFromConstrast(IR_DEFAULT_CONTRAST);
   adc_buf_channel(ADC_CHANNEL_IR1, &buf_ir1, ADC_CHANNEL_IR_NB_SAMPLES);

sw/airborne/main_ap.c

@@ -37,6 +37,7 @@
 #include "traffic_info.h"
 #include "link_mcu.h"
 #include "sys_time.h"
+#include "flight_plan.h"
 
 #ifdef LED
 #include "led.h"

sw/ground_segment/cockpit/map2d.ml

@@ -80,7 +80,8 @@ type aircraft = {
     color: color;
     fp_group : MapFP.flight_plan;
     fp : Xml.xml;
-    dl_settings : GWindow.window
+    dl_settings_window : GWindow.window;
+    dl_settings_adjustments : float array
   }
 
 let live_aircrafts = Hashtbl.create 3
@@ -257,31 +258,38 @@ let dl_settings = fun ac_id xml ->
   begin
     try
       let settings = Xml.children (ExtXml.child xml "dl_settings") in
+      let n = List.length settings in
+      let current_values = Array.create n 42. in
       let i = ref 0 in
       List.iter
-	(fun s ->
-	  let f = fun a -> float_of_string (ExtXml.attrib s a) in
-	  let lower = f "min"
-	  and upper = f "max"
-	  and step_incr = f "step" in
-	  let value = (lower +. upper) /. 2. in
-	  let text = ExtXml.attrib s "var" in
-	  let adj = GData.adjustment ~value ~lower ~upper:(upper+.10.) ~step_incr () in
-	  let hbox = GPack.hbox ~width:400 ~packing:vbox#add () in
-	  let _l = GMisc.label ~width:100 ~text ~packing:hbox#pack () in
-	  let _scale = GRange.scale `HORIZONTAL ~digits:2 ~adjustment:adj ~packing:hbox#add () in
-	  let ii = !i in
-	  let callback = fun () -> 
-	    let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int ii;"value", Pprz.Float adj#value] in
-	    Ground_Pprz.message_send "dl" "DL_SETTING" vs in
-	  let b = GButton.button ~label:"Commit" ~stock:`OK ~packing:hbox#pack () in
-	  ignore (b#connect#clicked ~callback);
-	  incr i
-	)
-      settings
-    with _ -> ()
-  end;
-  window
+	  (fun s ->
+	    let f = fun a -> float_of_string (ExtXml.attrib s a) in
+	    let lower = f "min"
+	    and upper = f "max"
+	    and step_incr = f "step" in
+	    let value = (lower +. upper) /. 2. in
+	    let text = ExtXml.attrib s "var" in
+	    let adj = GData.adjustment ~value ~lower ~upper:(upper+.10.) ~step_incr () in
+	    let hbox = GPack.hbox ~width:400 ~packing:vbox#add () in
+	    let _l = GMisc.label ~width:100 ~text ~packing:hbox#pack () in
+	    let _scale = GRange.scale `HORIZONTAL ~digits:2 ~adjustment:adj ~packing:hbox#add () in
+	    let ii = !i in
+	    let b = GButton.button ~label:"Update" ~stock:`REFRESH ~packing:hbox#pack () in
+	    let update = fun () -> adj#set_value current_values.(ii) in
+	    ignore (b#connect#clicked ~callback:update);
+
+	    let callback = fun () -> 
+	      let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int ii;"value", Pprz.Float adj#value] in
+	      Ground_Pprz.message_send "dl" "DL_SETTING" vs in
+	    let b = GButton.button ~label:"Commit" ~stock:`APPLY ~packing:hbox#pack () in
+	    ignore (b#connect#clicked ~callback);
+
+	    incr i
+	  )
+	  settings;
+      window, current_values
+    with _ -> window, [||]
+  end
 
 let center = fun geomap track () ->
   match track#last with
@@ -292,7 +300,7 @@ let center = fun geomap track () ->
 
 let active_dl_settings = fun ac_id x ->
   let ac = Hashtbl.find live_aircrafts ac_id in
-  let w = ac.dl_settings in
+  let w = ac.dl_settings_window in
   if x then w#show () else w#misc#hide ();;
 
 
@@ -333,11 +341,15 @@ let create_ac = fun (geomap:MapCanvas.widget) ac_id config ->
   let fp_xml_dump = Xml.parse_file fp_file in
   let fp_xml = ExtXml.child fp_xml_dump "flight_plan" in
 
-  let ds = dl_settings ac_id fp_xml in
+  let ds_window, ds_adjs = dl_settings ac_id fp_xml in
 
   let fp = load_mission color geomap fp_xml in
   fp#hide ();
-  Hashtbl.add live_aircrafts ac_id { track = track; color = color; fp_group = fp ; config = config ; fp = fp_xml; dl_settings = ds}
+  Hashtbl.add live_aircrafts ac_id { track = track; color = color; 
+				     fp_group = fp ; config = config ; 
+				     fp = fp_xml;
+				     dl_settings_adjustments = ds_adjs;
+				     dl_settings_window = ds_window}
   
 
 
@@ -362,6 +374,22 @@ let live_aircrafts_msg = fun (geomap:MapCanvas.widget) acs ->
   List.iter (one_new_ac geomap) acs
 
 
+let listen_dl_value = fun () ->
+  let get_dl_value = fun _sender vs ->
+    let ac_id = Pprz.string_assoc "ac_id" vs in
+    try
+      let ac = Hashtbl.find live_aircrafts ac_id in
+      let adjs = ac.dl_settings_adjustments in
+      let csv = Pprz.string_assoc "values" vs in
+      let values = Array.of_list (Str.split list_separator csv) in
+      for i = 0 to Array.length values - 1 do
+	adjs.(i) <- float_of_string values.(i)
+      done
+    with Not_found -> ()
+  in
+  ignore (Ground_Pprz.message_bind "DL_VALUES" get_dl_value)
+
+
 let listen_flight_params = fun () ->
   let get_fp = fun _sender vs ->
     let ac_id = Pprz.string_assoc "ac_id" vs in
@@ -428,7 +456,10 @@ let listen_flight_params = fun () ->
     with 
       Not_found -> ()
   in
-  ignore (Ground_Pprz.message_bind "AP_STATUS" get_ap_status);;
+  ignore (Ground_Pprz.message_bind "AP_STATUS" get_ap_status);
+
+ listen_dl_value ()
+
 
 let active_gm_http = fun x -> 
   Gm.no_http := not x

sw/ground_segment/tmtc/aircraft.ml

@@ -110,5 +110,7 @@ type aircraft = {
     mutable flight_time : int;
     mutable stage_time : int;
     mutable block_time : int;
-    mutable horiz_mode : horiz_mode
+    mutable horiz_mode : horiz_mode;
+    dl_setting_values : float array;
+    mutable nb_dl_setting_values : int
   }

sw/ground_segment/tmtc/aircraft.mli

@@ -60,41 +60,43 @@ type horiz_mode =
   | Segment of Latlong.utm * Latlong.utm
   | UnknownHorizMode
 type aircraft = {
-  id : string;
-  mutable pos : Latlong.utm;
-  mutable roll : float;
-  mutable pitch : float;
-  mutable nav_ref : Latlong.utm option;
-  mutable desired_east : float;
-  mutable desired_north : float;
-  mutable desired_altitude : float;
-  mutable desired_course : float;
-  mutable desired_climb : float;
-  mutable gspeed : float;
-  mutable course : float;
-  mutable alt : float;
+    id : string;
+    mutable pos : Latlong.utm;
+    mutable roll : float;
+    mutable pitch : float;
+    mutable nav_ref : Latlong.utm option;
+    mutable desired_east : float;
+    mutable desired_north : float;
+    mutable desired_altitude : float;
+    mutable desired_course : float;
+    mutable desired_climb : float;
+    mutable gspeed : float;
+    mutable course : float;
+    mutable alt : float;
   mutable climb : float;
-  mutable cur_block : int;
-  mutable cur_stage : int;
-  mutable throttle : float;
-  mutable throttle_accu : float;
-  mutable rpm : float;
-  mutable temp : float;
-  mutable bat : float;
-  mutable amp : float;
-  mutable energy : int;
-  mutable ap_mode : int;
-  mutable gaz_mode : int;
-  mutable lateral_mode : int;
-  mutable horizontal_mode : int;
-  cam : ac_cam;
-  mutable gps_mode : int;
-  inflight_calib : inflight_calib;
-  infrared : infrared;
-  fbw : fbw;
-  svinfo : svinfo array;
-  mutable flight_time : int;
-  mutable stage_time : int;
-  mutable block_time : int;
-  mutable horiz_mode : horiz_mode;
+    mutable cur_block : int;
+    mutable cur_stage : int;
+    mutable throttle : float;
+    mutable throttle_accu : float;
+    mutable rpm : float;
+    mutable temp : float;
+    mutable bat : float;
+    mutable amp : float;
+    mutable energy : int;
+    mutable ap_mode : int;
+    mutable gaz_mode : int;
+    mutable lateral_mode : int;
+    mutable horizontal_mode : int;
+    cam : ac_cam;
+    mutable gps_mode : int;
+    inflight_calib : inflight_calib;
+    infrared : infrared;
+    fbw : fbw;
+    svinfo : svinfo array;
+    mutable flight_time : int;
+    mutable stage_time : int;
+    mutable block_time : int;
+    mutable horiz_mode : horiz_mode;
+    dl_setting_values : float array;
+    mutable nb_dl_setting_values : int
 }

sw/ground_segment/tmtc/server.ml

@@ -26,6 +26,7 @@
 
 let my_id = "ground"
 let gps_mode_3D = 3
+let max_nb_dl_setting_values = 42 (** FIXME *)
 
 open Printf
 open Latlong
@@ -248,7 +249,7 @@ let log_and_parse = fun logging ac_name a msg values ->
 	  | None -> ()
 	end
     | "SEGMENT" ->
-		begin
+	begin
 	  match a.nav_ref with
 	    Some nav_ref ->
 	      let p1 = Latlong.utm_add nav_ref (fvalue "segment_east_1", fvalue "segment_north_1")
@@ -258,6 +259,13 @@ let log_and_parse = fun logging ac_name a msg values ->
 	end
     | "CALIBRATION" ->
 	a.throttle_accu <- fvalue "climb_sum_err"
+    | "DL_VALUE" ->
+	let i = ivalue "index" in
+	if i < max_nb_dl_setting_values then begin
+	  a.dl_setting_values.(i) <- fvalue "value";
+	  a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1)
+	end else
+	  failwith "Too much dl_setting values !!!"
     | _ -> ()
 
 (** Callback for a message from a registered A/C *)
@@ -318,6 +326,15 @@ let send_infrared = fun a ->
 	       ] in
   Ground_Pprz.message_send my_id "INFRARED"  values
 
+let send_dl_values = fun a ->
+  if a.nb_dl_setting_values > 0 then
+    let csv = ref "" in
+    for i = 0 to a.nb_dl_setting_values - 1 do
+      csv := sprintf "%s%f," !csv a.dl_setting_values.(i)
+    done;
+    let vs = ["ac_id", Pprz.String a.id; "values", Pprz.String !csv] in
+    Ground_Pprz.message_send my_id "DL_VALUES" vs
+
 let send_svsinfo = fun a ->
   let svid = ref ""
   and flags= ref ""
@@ -325,12 +342,12 @@ let send_svsinfo = fun a ->
   and cno = ref ""
   and elev = ref ""
   and azim = ref "" in
+  let concat = fun ref v ->
+    ref := !ref ^ string_of_int v ^ "," in
   for i = 0 to gps_nb_channels - 1 do
-    let concat = fun ref v ->
-      ref := !ref ^ string_of_int v ^ "," in
     concat svid a.svinfo.(i).svid;
     concat flags a.svinfo.(i).flags;
-      concat qi a.svinfo.(i).qi;
+    concat qi a.svinfo.(i).qi;
     concat cno a.svinfo.(i).cno;
     concat elev a.svinfo.(i).elev;
     concat azim a.svinfo.(i).azim
@@ -442,7 +459,8 @@ let send_aircraft_msg = fun ac ->
     send_fbw a;
     send_infrared a;
     send_svsinfo a;
-    send_horiz_status a
+    send_horiz_status a;
+    send_dl_values a
   with
     Not_found -> prerr_endline ac
   | x -> prerr_endline (Printexc.to_string x)
@@ -465,6 +483,8 @@ let new_aircraft = fun id ->
       infrared = infrared_init;
       fbw = { rc_status = "???"; rc_mode = "???" };
       svinfo = Array.create gps_nb_channels svinfo_init;
+      dl_setting_values = Array.create max_nb_dl_setting_values 42.;
+      nb_dl_setting_values = 0;
       flight_time = 0; stage_time = 0; block_time = 0;
       horiz_mode = UnknownHorizMode;
       horizontal_mode = 0

sw/tools/Makefile

@@ -9,10 +9,7 @@ FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.ml
 ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)
 
 gen_flight_plan.out : $(FP_CMO)
-	$(OCAMLC) -o $@ ivy-ocaml.cma lib-pprz.cma $^
-	@cat ../../pprz_src_test.sh > $@
-	@echo '$(OCAML) -I $$PAPARAZZI_SRC/sw/lib/ocaml -I $$PAPARAZZI_SRC/sw/tools ivy-ocaml.cma lib-pprz.cma $(ABS_FP) $$*' >> $@
-	@chmod a+x $@
+	$(OCAMLC) -custom -o $@ ivy-ocaml.cma lib-pprz.cma $^
 
 fp_parser.cmo : fp_parser.cmi fp_syntax.cmi
 fp_parser.cmi : fp_parser.ml fp_syntax.cmi

sw/tools/gen_flight_plan.ml

@@ -535,6 +535,7 @@ let print_heights = fun xml wgs84 alt ->
   end
 
 let print_dl_settings = fun settings ->
+  (** Macro to call to set one variable *)
   lprintf "#define DlSetting(_idx, _value) { \\\n";
   right ();
   let idx = ref 0 in
@@ -544,6 +545,26 @@ let print_dl_settings = fun settings ->
       lprintf "if (_idx == %d) %s = _value;\\\n" !idx v; incr idx) 
     settings;
   left ();
+  lprintf "}\n";
+  let nb_values = !idx in
+
+  (** Macro to call to download current values *)
+  lprintf "#define PeriodicSendDlValue() { \\\n";
+  if nb_values > 0 then begin
+    right ();
+    lprintf "static uint8_t i;\\\n";
+    lprintf "float *var = 0;\\\n";
+    lprintf "if (i >= %d) i = 0;;\\\n" nb_values;
+    let idx = ref 0 in
+    List.iter 
+      (fun s ->
+	let v = ExtXml.attrib s "var" in
+	lprintf "if (i == %d) var = &%s;\\\n" !idx v; incr idx) 
+      settings;
+    lprintf "DOWNLINK_SEND_DL_VALUE(&i, var);\\\n";
+    lprintf "i++;\\\n";
+    left ()
+  end;
   lprintf "}\n"