diff --git a/sw/airborne/autopilot/datalink.c b/sw/airborne/autopilot/datalink.c index 02f6011506..5e39b21860 100644 --- a/sw/airborne/autopilot/datalink.c +++ b/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)); } } diff --git a/sw/ground_segment/tmtc/wavecard_connect.ml b/sw/ground_segment/tmtc/wavecard_connect.ml index 3477e10303..1113e4b733 100644 --- a/sw/ground_segment/tmtc/wavecard_connect.ml +++ b/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 +(.*)"); diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c index fd4dfe5f20..4536597d74 100644 --- a/sw/simulator/sim_ap.c +++ b/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; +} diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index f3e2ac9843..9a21323792 100644 --- a/sw/simulator/sitl.ml +++ b/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 diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index f389a33680..658dc32589 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/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