diff --git a/conf/flight_plans/mav05_ccw.xml b/conf/flight_plans/mav05_ccw.xml
index 98eda60db7..6d14c46e13 100644
--- a/conf/flight_plans/mav05_ccw.xml
+++ b/conf/flight_plans/mav05_ccw.xml
@@ -12,6 +12,8 @@
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index e607603a7a..9101fb8b33 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -206,10 +206,15 @@
-
-
+
-
+
+
+
+
+
+
+
@@ -454,6 +459,11 @@
+
+
+
+
+
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index 679fcc438f..a7aeb64170 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -37,6 +37,7 @@
#include "estimator.h"
#include "pid.h"
+#include "cam.h"
#define MOfCm(_x) (((float)_x)/100.)
diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h
index 5e5ff6cde5..e6e5777b84 100644
--- a/sw/airborne/downlink.h
+++ b/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 */
diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c
index f2aeeb77ad..90dc1a3893 100644
--- a/sw/airborne/infrared.c
+++ b/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);
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 9e954b5fcf..8db265aaf6 100644
--- a/sw/airborne/main_ap.c
+++ b/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"
diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml
index 53d719e979..3362c0ef29 100644
--- a/sw/ground_segment/cockpit/map2d.ml
+++ b/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
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index 83503f61de..9ec42bfa7e 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/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
}
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index cc7b79e2c7..605ca49772 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/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
}
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index edc8b32fe5..bbd79947bf 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/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
diff --git a/sw/tools/Makefile b/sw/tools/Makefile
index 35a113d695..30205d4810 100644
--- a/sw/tools/Makefile
+++ b/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
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index b240d14277..1c236d00f1 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/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"