mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-28 01:53:48 +08:00
Fix modules and telemetry freq (#2553)
* [generator] set prescaler at run/compile time for modules * [generator] fix freq for periodic and cleaning * [datalink] fix freq in datalink modules extra_dl and mavlink
This commit is contained in:
committed by
GitHub
parent
a91ed27ec4
commit
de3858cd4b
@@ -23,8 +23,10 @@
|
|||||||
|
|
||||||
<makefile target="ap">
|
<makefile target="ap">
|
||||||
<configure name="EXTRA_DL_PORT" default="uart1" case="upper|lower"/>
|
<configure name="EXTRA_DL_PORT" default="uart1" case="upper|lower"/>
|
||||||
|
<configure name="TELEMETRY_FREQUENCY" default="$(PERIODIC_FREQUENCY)"/>
|
||||||
<define name="EXTRA_DOWNLINK_DEVICE" value="$(EXTRA_DL_PORT_LOWER)"/>
|
<define name="EXTRA_DOWNLINK_DEVICE" value="$(EXTRA_DL_PORT_LOWER)"/>
|
||||||
<define name="USE_$(EXTRA_DL_PORT_UPPER)"/>
|
<define name="USE_$(EXTRA_DL_PORT_UPPER)"/>
|
||||||
|
<define name="TELEMETRY_FREQUENCY" value="$(TELEMETRY_FREQUENCY)"/>
|
||||||
<raw>
|
<raw>
|
||||||
# Check for UDP port
|
# Check for UDP port
|
||||||
ifneq (,$(findstring udp,$(EXTRA_DL_PORT_LOWER)))
|
ifneq (,$(findstring udp,$(EXTRA_DL_PORT_LOWER)))
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
<!DOCTYPE module SYSTEM "module.dtd">
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
<module name="datalink">
|
<module name="mavlink" dir="datalink">
|
||||||
<doc>
|
<doc>
|
||||||
<description>Basic MAVLink implementation</description>
|
<description>Basic MAVLink implementation</description>
|
||||||
<configure name="MAVLINK_PORT" value="UARTx|UDPx|UsbS" description="The port device to use for mavlink (default: UART1)"/>
|
<configure name="MAVLINK_PORT" value="UARTx|UDPx|UsbS" description="The port device to use for mavlink (default: UART1)"/>
|
||||||
@@ -12,7 +12,7 @@
|
|||||||
|
|
||||||
<init fun="mavlink_init()"/>
|
<init fun="mavlink_init()"/>
|
||||||
<periodic fun="mavlink_periodic()" freq="10" autorun="TRUE"/>
|
<periodic fun="mavlink_periodic()" freq="10" autorun="TRUE"/>
|
||||||
<periodic fun="mavlink_periodic_telemetry()" freq="$TELEMETRY_FREQUENCY" autorun="TRUE"/>
|
<periodic fun="mavlink_periodic_telemetry()" freq="TELEMETRY_FREQUENCY" autorun="TRUE"/>
|
||||||
<event fun="mavlink_event()"/>
|
<event fun="mavlink_event()"/>
|
||||||
|
|
||||||
<makefile>
|
<makefile>
|
||||||
@@ -21,6 +21,8 @@
|
|||||||
<file name="waypoints.c" dir="modules/datalink/missionlib"/>
|
<file name="waypoints.c" dir="modules/datalink/missionlib"/>
|
||||||
<file name="blocks.c" dir="modules/datalink/missionlib"/>
|
<file name="blocks.c" dir="modules/datalink/missionlib"/>
|
||||||
<configure name="MAVLINK_PORT" default="uart1" case="upper|lower"/>
|
<configure name="MAVLINK_PORT" default="uart1" case="upper|lower"/>
|
||||||
|
<configure name="TELEMETRY_FREQUENCY" default="$(PERIODIC_FREQUENCY)"/>
|
||||||
|
<define name="TELEMETRY_FREQUENCY" value="$(TELEMETRY_FREQUENCY)"/>
|
||||||
<raw>
|
<raw>
|
||||||
ifneq (,$(findstring usb,$(MAVLINK_PORT_LOWER)))
|
ifneq (,$(findstring usb,$(MAVLINK_PORT_LOWER)))
|
||||||
ap.CFLAGS += -DUSE_USB_SERIAL
|
ap.CFLAGS += -DUSE_USB_SERIAL
|
||||||
|
|||||||
@@ -47,8 +47,6 @@ let periodic_h = "periodic_telemetry.h"
|
|||||||
let modules_h = "modules.h"
|
let modules_h = "modules.h"
|
||||||
let settings_h = "settings.h"
|
let settings_h = "settings.h"
|
||||||
let settings_xml = "settings.xml"
|
let settings_xml = "settings.xml"
|
||||||
let default_periodic_freq = 60
|
|
||||||
let default_modules_freq = 60
|
|
||||||
|
|
||||||
let get_string_opt = fun x -> match x with Some s -> s | None -> ""
|
let get_string_opt = fun x -> match x with Some s -> s | None -> ""
|
||||||
|
|
||||||
@@ -112,9 +110,7 @@ let () =
|
|||||||
and gen_ap = ref false
|
and gen_ap = ref false
|
||||||
and gen_all = ref false
|
and gen_all = ref false
|
||||||
and gen_ac_dir = ref None
|
and gen_ac_dir = ref None
|
||||||
and gen_conf_dir = ref None
|
and gen_conf_dir = ref None in
|
||||||
and modules_freq = ref default_modules_freq
|
|
||||||
and tl_freq = ref default_periodic_freq in
|
|
||||||
|
|
||||||
let options =
|
let options =
|
||||||
[ "-name", Arg.String (fun x -> ac_name := Some x), " Aircraft name (mandatory)";
|
[ "-name", Arg.String (fun x -> ac_name := Some x), " Aircraft name (mandatory)";
|
||||||
@@ -130,8 +126,6 @@ let () =
|
|||||||
"-all", Arg.Set gen_all, " Generate all files";
|
"-all", Arg.Set gen_all, " Generate all files";
|
||||||
"-ac_dir", Arg.String (fun x -> gen_ac_dir := Some x), "Directory for aircraft generated headers";
|
"-ac_dir", Arg.String (fun x -> gen_ac_dir := Some x), "Directory for aircraft generated headers";
|
||||||
"-conf_dir", Arg.String (fun x -> gen_conf_dir := Some x), "Directory for aircraft generated config";
|
"-conf_dir", Arg.String (fun x -> gen_conf_dir := Some x), "Directory for aircraft generated config";
|
||||||
"-periodic_freq", Arg.Int (fun x -> tl_freq := x), (sprintf " Periodic telemetry frequency (default %d)" default_periodic_freq);
|
|
||||||
"-modules_freq", Arg.Int (fun x -> modules_freq := x), (sprintf " Modules frequency (default %d)" default_modules_freq);
|
|
||||||
] in
|
] in
|
||||||
|
|
||||||
Arg.parse
|
Arg.parse
|
||||||
@@ -313,7 +307,7 @@ let () =
|
|||||||
| None -> Printf.printf "(skip)"
|
| None -> Printf.printf "(skip)"
|
||||||
| Some telemetry ->
|
| Some telemetry ->
|
||||||
generate_config_element telemetry
|
generate_config_element telemetry
|
||||||
(fun e -> Gen_periodic.generate e !tl_freq abs_telemetry_h)
|
(fun e -> Gen_periodic.generate e abs_telemetry_h)
|
||||||
[ (abs_telemetry_h, [telemetry.Telemetry.filename]) ] end;
|
[ (abs_telemetry_h, [telemetry.Telemetry.filename]) ] end;
|
||||||
Printf.printf " done\n%!";
|
Printf.printf " done\n%!";
|
||||||
|
|
||||||
@@ -321,7 +315,7 @@ let () =
|
|||||||
let loaded_modules = Aircraft.get_loaded_modules ac.Aircraft.config_by_target target in
|
let loaded_modules = Aircraft.get_loaded_modules ac.Aircraft.config_by_target target in
|
||||||
let abs_modules_h = aircraft_gen_dir // modules_h in
|
let abs_modules_h = aircraft_gen_dir // modules_h in
|
||||||
generate_config_element loaded_modules
|
generate_config_element loaded_modules
|
||||||
(fun e -> Gen_modules.generate e !modules_freq "" abs_modules_h)
|
(fun e -> Gen_modules.generate e "" abs_modules_h)
|
||||||
[ abs_modules_h, List.map (fun m -> m.Module.xml_filename) loaded_modules ];
|
[ abs_modules_h, List.map (fun m -> m.Module.xml_filename) loaded_modules ];
|
||||||
Printf.printf " done\n%!";
|
Printf.printf " done\n%!";
|
||||||
|
|
||||||
|
|||||||
@@ -70,16 +70,16 @@ let fprint_periodic_init = fun ch mod_name p ->
|
|||||||
let fprint_init = fun ch init -> Printf.fprintf ch "%s;\n" init
|
let fprint_init = fun ch init -> Printf.fprintf ch "%s;\n" init
|
||||||
*)
|
*)
|
||||||
|
|
||||||
let get_period_and_freq = fun f max_freq ->
|
let get_period_and_freq = fun f ->
|
||||||
let period = try Some (float_of_string (Xml.attrib f "period")) with _ -> None
|
let period = try Some (Xml.attrib f "period") with _ -> None
|
||||||
and freq = try Some (float_of_string (Xml.attrib f "freq")) with _ -> None in
|
and freq = try Some (Xml.attrib f "freq") with _ -> None in
|
||||||
match period, freq with
|
match period, freq with
|
||||||
| None, None -> (1. /. max_freq, max_freq)
|
| None, None -> ("(1.f / MODULES_FREQUENCY)", "(MODULES_FREQUENCY)")
|
||||||
| Some _p, None -> (_p, 1. /. _p)
|
| Some _p, None -> ("("^_p^")", "(1. / ("^_p^"))")
|
||||||
| None, Some _f -> (1. /. _f, _f)
|
| None, Some _f -> ("(1. / ("^_f^"))", "("^_f^")")
|
||||||
| Some _p, Some _ ->
|
| Some _p, Some _ ->
|
||||||
fprintf stderr "Warning: both period and freq are defined but only period is used for function %s\n" (ExtXml.attrib f "fun");
|
fprintf stderr "Warning: both period and freq are defined but only period is used for function %s\n" (ExtXml.attrib f "fun");
|
||||||
(_p, 1. /. _p)
|
("("^_p^")", "(1. / ("^_p^"))")
|
||||||
|
|
||||||
(*let fprint_period_freq = fun ch max_freq p ->
|
(*let fprint_period_freq = fun ch max_freq p ->
|
||||||
let period, freq = match p.period_freq with
|
let period, freq = match p.period_freq with
|
||||||
@@ -98,21 +98,62 @@ let get_cap_name = fun f ->
|
|||||||
| [Str.Text t; Str.Delim "("; Str.Text _ ; Str.Delim ")"] -> Compat.uppercase_ascii t
|
| [Str.Text t; Str.Delim "("; Str.Text _ ; Str.Delim ")"] -> Compat.uppercase_ascii t
|
||||||
| _ -> failwith "Gen_modules: not a valid function name"
|
| _ -> failwith "Gen_modules: not a valid function name"
|
||||||
|
|
||||||
let print_function_freq = fun out freq modules ->
|
|
||||||
let max_freq = float freq in
|
(** Computes the required modulos *)
|
||||||
|
let get_functions_modulos = fun modules ->
|
||||||
|
let found_modulos = Hashtbl.create 10 in
|
||||||
|
let idx = ref 0 in
|
||||||
|
let delay = ref 0. in (** Basic balancing: increment with a step of 0.1 *)
|
||||||
|
let functions_modulo = List.map (fun m ->
|
||||||
|
let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in
|
||||||
|
let module_name = m.Module.name in
|
||||||
|
List.map (fun x ->
|
||||||
|
let p, _ = get_period_and_freq x in
|
||||||
|
let d = begin try
|
||||||
|
let _d = float_of_string (Xml.attrib x "delay") in
|
||||||
|
if _d > 0.95 then _d /. 65536. else _d (* try to keep some backward compatibility *)
|
||||||
|
with _ ->
|
||||||
|
delay := !delay +. 0.1;
|
||||||
|
if !delay > 0.9 then delay := 0.;
|
||||||
|
!delay
|
||||||
|
end
|
||||||
|
in
|
||||||
|
try
|
||||||
|
((x, module_name, d), (p, Hashtbl.find found_modulos p))
|
||||||
|
with Not_found ->
|
||||||
|
incr idx; (* create new modulo *)
|
||||||
|
Hashtbl.add found_modulos p !idx;
|
||||||
|
((x, module_name, d), (p, !idx))
|
||||||
|
) periodic
|
||||||
|
) modules in
|
||||||
|
List.flatten functions_modulo
|
||||||
|
|
||||||
|
|
||||||
|
let print_function_freq = fun out modules ->
|
||||||
fprintf out "\n";
|
fprintf out "\n";
|
||||||
List.iter (fun m ->
|
List.iter (fun m ->
|
||||||
List.iter (fun i ->
|
List.iter (fun i ->
|
||||||
match Xml.tag i with
|
match Xml.tag i with
|
||||||
"periodic" ->
|
"periodic" ->
|
||||||
let fname = get_cap_name (Xml.attrib i "fun") in
|
let fname = get_cap_name (Xml.attrib i "fun") in
|
||||||
let p, f = get_period_and_freq i max_freq in
|
let p, f = get_period_and_freq i in
|
||||||
lprintf out "#define %s_PERIOD %f\n" fname p;
|
lprintf out "#define %s_PERIOD %s\n" fname p;
|
||||||
lprintf out "#define %s_FREQ %f\n" fname f;
|
lprintf out "#define %s_FREQ %s\n" fname f;
|
||||||
| _ -> ())
|
| _ -> ())
|
||||||
(Xml.children m.Module.xml))
|
(Xml.children m.Module.xml))
|
||||||
modules
|
modules
|
||||||
|
|
||||||
|
let print_function_prescalers = fun out functions_modulo ->
|
||||||
|
fprintf out "\n";
|
||||||
|
let found_modulos = Hashtbl.create 10 in
|
||||||
|
List.iter (fun (_, (p, m)) ->
|
||||||
|
if not (Hashtbl.mem found_modulos m) then
|
||||||
|
lprintf out "#define PRESCALER_%d (uint32_t)(MODULES_FREQUENCY * %s)\n" m p
|
||||||
|
else
|
||||||
|
Hashtbl.add found_modulos m p
|
||||||
|
) functions_modulo
|
||||||
|
|
||||||
|
|
||||||
let is_status_lock = fun p ->
|
let is_status_lock = fun p ->
|
||||||
let mode = ExtXml.attrib_or_default p "autorun" "LOCK" in
|
let mode = ExtXml.attrib_or_default p "autorun" "LOCK" in
|
||||||
mode = "LOCK"
|
mode = "LOCK"
|
||||||
@@ -172,45 +213,37 @@ let print_init_functions = fun out modules ->
|
|||||||
lprintf out "}\n"
|
lprintf out "}\n"
|
||||||
|
|
||||||
|
|
||||||
let print_periodic = fun out freq task modules ->
|
let print_periodic = fun out functions_modulo task modules ->
|
||||||
let min_period = 1. /. float freq
|
(* filter for a given task *)
|
||||||
and max_period = 65536. /. float freq
|
let functions_modulo = List.filter (fun m ->
|
||||||
and min_freq = float freq /. 65536.
|
let (_, name, _), _ = m in
|
||||||
and max_freq = float freq in
|
List.exists (fun m' -> m'.Module.name = name) modules
|
||||||
|
) functions_modulo in
|
||||||
|
(* start printing *)
|
||||||
lprintf out "\nstatic inline void modules_%s_periodic_task(void) {\n" task;
|
lprintf out "\nstatic inline void modules_%s_periodic_task(void) {\n" task;
|
||||||
right ();
|
right ();
|
||||||
(** Computes the required modulos *)
|
let modulos = GC.singletonize (List.map (fun (_, (_, m)) -> m) functions_modulo) in
|
||||||
let functions_modulo = List.flatten (List.map (fun m ->
|
|
||||||
let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in
|
|
||||||
let module_name = m.Module.name in
|
|
||||||
List.map (fun x ->
|
|
||||||
let p, _ = get_period_and_freq x max_freq in
|
|
||||||
if p < min_period || p > max_period then
|
|
||||||
fprintf stderr "Warning: period is bound between %.3fs and %.3fs (%fHz and %.1fHz) for function %s\n%!"
|
|
||||||
min_period max_period max_freq min_freq (ExtXml.attrib x "fun");
|
|
||||||
((x, module_name), min 65535 (max 1 (int_of_float (p *. float_of_int freq))))
|
|
||||||
) periodic)
|
|
||||||
modules) in
|
|
||||||
let modulos = GC.singletonize (List.map snd functions_modulo) in
|
|
||||||
(** Print modulos *)
|
(** Print modulos *)
|
||||||
List.iter (fun modulo ->
|
List.iter (fun modulo ->
|
||||||
let v = sprintf "i%d" modulo in
|
let v = sprintf "i%d" modulo in
|
||||||
let _type = if modulo >= 256 then "uint16_t" else "uint8_t" in
|
lprintf out "static uint32_t %s; %s++; if (%s>=PRESCALER_%d) %s=0;\n" v v v modulo v;)
|
||||||
lprintf out "static %s %s; %s++; if (%s>=%d) %s=0;\n" _type v v v modulo v;)
|
|
||||||
modulos;
|
modulos;
|
||||||
(** Print start and stop functions *)
|
(** Print start and stop functions *)
|
||||||
List.iter (fun m ->
|
List.iter (fun m ->
|
||||||
let module_name = m.Module.name in
|
let module_name = m.Module.name in
|
||||||
let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in
|
let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in
|
||||||
fprintf out "\n";
|
|
||||||
List.iter (fun f ->
|
List.iter (fun f ->
|
||||||
if (is_status_lock f) then begin
|
if (is_status_lock f) then begin
|
||||||
try lprintf out "%s;\n" (Xml.attrib f "start") with _ -> ();
|
try
|
||||||
try let stop = Xml.attrib f "stop" in fprintf stderr "Warning: stop %s function will not be called\n" stop with _ -> ();
|
let start = (Xml.attrib f "start") in
|
||||||
|
fprintf out "\n";
|
||||||
|
lprintf out "%s;\n" start
|
||||||
|
with _ -> ();
|
||||||
|
try let stop = Xml.attrib f "stop" in fprintf stderr "Warning: stop %s function will not be called\n" stop with _ -> ();
|
||||||
end
|
end
|
||||||
else begin
|
else begin
|
||||||
let status = get_status_name f module_name in
|
let status = get_status_name f module_name in
|
||||||
|
fprintf out "\n";
|
||||||
lprintf out "if (%s == MODULES_START) {\n" status;
|
lprintf out "if (%s == MODULES_START) {\n" status;
|
||||||
right ();
|
right ();
|
||||||
ignore(try lprintf out "%s;\n" (Xml.attrib f "start") with _ -> ());
|
ignore(try lprintf out "%s;\n" (Xml.attrib f "start") with _ -> ());
|
||||||
@@ -224,68 +257,46 @@ let print_periodic = fun out freq task modules ->
|
|||||||
left ();
|
left ();
|
||||||
lprintf out "}\n";
|
lprintf out "}\n";
|
||||||
end
|
end
|
||||||
)
|
) periodic
|
||||||
periodic)
|
) modules;
|
||||||
modules;
|
|
||||||
(** Print periodic functions *)
|
(** Print periodic functions *)
|
||||||
let functions = List.sort (fun (_,p) (_,p') -> compare p p') functions_modulo in
|
let functions = List.sort (fun (_,p) (_,p') -> compare p p') functions_modulo in
|
||||||
let i = ref 0 in (** Basic balancing:1 function every 10Hz FIXME *)
|
|
||||||
let l = ref [] in
|
|
||||||
fprintf out "\n";
|
fprintf out "\n";
|
||||||
let test_delay = fun x -> try let _ = Xml.attrib x "delay" in true with _ -> false in
|
List.iter (fun ((func, name, delay), (p, m)) ->
|
||||||
List.iter (fun ((func, name), p) ->
|
if (List.exists (fun _module -> _module.Module.name = name) modules) then begin
|
||||||
let function_name = ExtXml.attrib func "fun" in
|
let function_name = ExtXml.attrib func "fun" in
|
||||||
if p = 1 then
|
let p, f = get_period_and_freq func in
|
||||||
begin
|
if f = "(MODULES_FREQUENCY)" then
|
||||||
if (is_status_lock func) then
|
begin
|
||||||
lprintf out "%s;\n" function_name
|
if (is_status_lock func) then
|
||||||
else begin
|
lprintf out "%s;\n" function_name
|
||||||
lprintf out "if (%s == MODULES_RUN) {\n" (get_status_name func name);
|
else begin
|
||||||
|
lprintf out "if (%s == MODULES_RUN) {\n" (get_status_name func name);
|
||||||
|
right ();
|
||||||
|
lprintf out "%s;\n" function_name;
|
||||||
|
left ();
|
||||||
|
lprintf out "}\n";
|
||||||
|
end
|
||||||
|
end
|
||||||
|
else
|
||||||
|
begin
|
||||||
|
let run = if not (is_status_lock func) then sprintf " && %s == MODULES_RUN" (get_status_name func name)
|
||||||
|
else ""
|
||||||
|
in
|
||||||
|
lprintf out "if (i%d == (uint32_t)(%ff * PRESCALER_%d)%s) {\n" m delay m run;
|
||||||
right ();
|
right ();
|
||||||
lprintf out "%s;\n" function_name;
|
lprintf out "%s;\n" function_name;
|
||||||
left ();
|
left ();
|
||||||
lprintf out "}\n";
|
lprintf out "}\n"
|
||||||
end
|
|
||||||
end
|
|
||||||
else
|
|
||||||
begin
|
|
||||||
if (test_delay func) then begin
|
|
||||||
(** Delay is set by user *)
|
|
||||||
let delay = int_of_string (Xml.attrib func "delay") in
|
|
||||||
if delay >= p then fprintf stderr "Warning: delay is bound between 0 and %d for function %s\n" (p-1) function_name;
|
|
||||||
let delay_p = delay mod p in
|
|
||||||
let else_ = if List.mem_assoc p !l && not (List.mem (p, delay_p) !l) then
|
|
||||||
"else " else "" in
|
|
||||||
if (is_status_lock func) then
|
|
||||||
lprintf out "%sif (i%d == %d) {\n" else_ p delay_p
|
|
||||||
else
|
|
||||||
lprintf out "%sif (i%d == %d && %s == MODULES_RUN) {\n" else_ p delay_p (get_status_name func name);
|
|
||||||
l := (p, delay_p) :: !l;
|
|
||||||
end
|
|
||||||
else begin
|
|
||||||
(** Delay is automtically set *)
|
|
||||||
i := !i mod p;
|
|
||||||
let else_ = if List.mem_assoc p !l && not (List.mem (p, !i) !l) then "else " else "" in
|
|
||||||
if (is_status_lock func) then
|
|
||||||
lprintf out "%sif (i%d == %d) {\n" else_ p !i
|
|
||||||
else
|
|
||||||
lprintf out "%sif (i%d == %d && %s == MODULES_RUN) {\n" else_ p !i (get_status_name func name);
|
|
||||||
l := (p, !i) :: !l;
|
|
||||||
let incr = p / ((List.length (List.filter (fun (_,p') -> compare p p' == 0) functions)) + 1) in
|
|
||||||
i := !i + incr;
|
|
||||||
end;
|
end;
|
||||||
right ();
|
end
|
||||||
lprintf out "%s;\n" function_name;
|
|
||||||
left ();
|
|
||||||
lprintf out "}\n"
|
|
||||||
end;
|
|
||||||
) functions;
|
) functions;
|
||||||
left ();
|
left ();
|
||||||
lprintf out "}\n"
|
lprintf out "}\n"
|
||||||
|
|
||||||
let print_periodic_functions = fun out freq modules ->
|
let print_periodic_functions = fun out functions_modulo modules ->
|
||||||
let h = modules_of_task modules in
|
let h = modules_of_task modules in
|
||||||
Hashtbl.iter (print_periodic out freq) h;
|
Hashtbl.iter (print_periodic out functions_modulo) h;
|
||||||
lprintf out "\nstatic inline void modules_periodic_task(void) {\n";
|
lprintf out "\nstatic inline void modules_periodic_task(void) {\n";
|
||||||
right ();
|
right ();
|
||||||
Hashtbl.iter (fun t _ -> lprintf out "modules_%s_periodic_task();\n" t) h;
|
Hashtbl.iter (fun t _ -> lprintf out "modules_%s_periodic_task();\n" t) h;
|
||||||
@@ -337,13 +348,15 @@ let print_datalink_functions = fun out modules ->
|
|||||||
left ();
|
left ();
|
||||||
lprintf out "}\n"
|
lprintf out "}\n"
|
||||||
|
|
||||||
let parse_modules out freq modules =
|
let parse_modules out modules =
|
||||||
print_headers out modules;
|
print_headers out modules;
|
||||||
print_function_freq out freq modules;
|
print_function_freq out modules;
|
||||||
|
let functions_modulo = get_functions_modulos modules in
|
||||||
|
print_function_prescalers out functions_modulo;
|
||||||
print_status out modules;
|
print_status out modules;
|
||||||
fprintf out "\n";
|
fprintf out "\n";
|
||||||
print_init_functions out modules;
|
print_init_functions out modules;
|
||||||
print_periodic_functions out freq modules;
|
print_periodic_functions out functions_modulo modules;
|
||||||
print_event_functions out modules;
|
print_event_functions out modules;
|
||||||
fprintf out "\n";
|
fprintf out "\n";
|
||||||
fprintf out "#ifdef MODULES_DATALINK_C\n";
|
fprintf out "#ifdef MODULES_DATALINK_C\n";
|
||||||
@@ -408,7 +421,7 @@ let check_dependencies = fun modules names ->
|
|||||||
|
|
||||||
let h_name = "MODULES_H"
|
let h_name = "MODULES_H"
|
||||||
|
|
||||||
let generate = fun modules freq xml_file out_file ->
|
let generate = fun modules xml_file out_file ->
|
||||||
let out = open_out out_file in
|
let out = open_out out_file in
|
||||||
|
|
||||||
begin_out out xml_file h_name;
|
begin_out out xml_file h_name;
|
||||||
@@ -418,7 +431,13 @@ let generate = fun modules freq xml_file out_file ->
|
|||||||
define_out out "MODULES_STOP " "3";
|
define_out out "MODULES_STOP " "3";
|
||||||
fprintf out "\n";
|
fprintf out "\n";
|
||||||
|
|
||||||
define_out out "MODULES_FREQUENCY" (string_of_int freq);
|
fprintf out "#ifndef MODULES_FREQUENCY\n";
|
||||||
|
fprintf out "#ifdef PERIODIC_FREQUENCY\n";
|
||||||
|
fprintf out "#define MODULES_FREQUENCY PERIODIC_FREQUENCY\n";
|
||||||
|
fprintf out "#else\n";
|
||||||
|
fprintf out "#error \"neither MODULES_FREQUENCY or PERIODIC_FREQUENCY are defined\"\n";
|
||||||
|
fprintf out "#endif\n";
|
||||||
|
fprintf out "#endif\n";
|
||||||
fprintf out "\n";
|
fprintf out "\n";
|
||||||
fprintf out "#ifdef MODULES_C\n";
|
fprintf out "#ifdef MODULES_C\n";
|
||||||
fprintf out "#define EXTERN_MODULES\n";
|
fprintf out "#define EXTERN_MODULES\n";
|
||||||
@@ -426,7 +445,7 @@ let generate = fun modules freq xml_file out_file ->
|
|||||||
fprintf out "#define EXTERN_MODULES extern\n";
|
fprintf out "#define EXTERN_MODULES extern\n";
|
||||||
fprintf out "#endif\n";
|
fprintf out "#endif\n";
|
||||||
|
|
||||||
parse_modules out freq modules;
|
parse_modules out modules;
|
||||||
|
|
||||||
finish_out out h_name
|
finish_out out h_name
|
||||||
|
|
||||||
|
|||||||
@@ -40,10 +40,7 @@ let lprintf = fun c f ->
|
|||||||
fprintf c "%s" (String.make !margin ' ');
|
fprintf c "%s" (String.make !margin ' ');
|
||||||
fprintf c f
|
fprintf c f
|
||||||
|
|
||||||
let output_modes = fun out_h process_name telem_type modes freq ->
|
let output_modes = fun out_h process_name telem_type modes ->
|
||||||
let min_period = 1./.float freq in
|
|
||||||
let max_period = 65536. /. float freq in
|
|
||||||
|
|
||||||
(** For each mode in this process *)
|
(** For each mode in this process *)
|
||||||
List.iter
|
List.iter
|
||||||
(fun mode ->
|
(fun mode ->
|
||||||
@@ -52,39 +49,40 @@ let output_modes = fun out_h process_name telem_type modes freq ->
|
|||||||
right ();
|
right ();
|
||||||
|
|
||||||
(** Computes the required modulos *)
|
(** Computes the required modulos *)
|
||||||
|
let found_modulos = Hashtbl.create 15 in
|
||||||
|
let idx = ref 0 in
|
||||||
|
let phase = ref 0. in (** Basic balancing: increment with a step of 0.1 *)
|
||||||
let messages = List.map (fun x ->
|
let messages = List.map (fun x ->
|
||||||
let p = float_of_string (ExtXml.attrib x "period") in
|
let period = ExtXml.attrib x "period" in
|
||||||
if p < min_period || p > max_period then
|
let _phase = begin try
|
||||||
fprintf stderr "Warning: period is bound between %.3fs and %.3fs for message %s\n%!" min_period max_period (ExtXml.attrib x "name");
|
let _p = float_of_string (ExtXml.attrib x "phase") in
|
||||||
(x, min 65535 (max 1 (int_of_float (p*.float_of_int freq))))
|
if _p > 0.95 then _p /. 65536. else _p (* try to keep some backward compatibility *)
|
||||||
|
with _ ->
|
||||||
|
phase := !phase +. 0.1;
|
||||||
|
if !phase > 0.9 then phase := 0.;
|
||||||
|
!phase
|
||||||
|
end
|
||||||
|
in
|
||||||
|
try
|
||||||
|
((x, _phase), (period, Hashtbl.find found_modulos period))
|
||||||
|
with Not_found ->
|
||||||
|
incr idx; (* create new module *)
|
||||||
|
let v = sprintf "i%d" !idx in
|
||||||
|
lprintf out_h "static uint32_t %s = 0; %s++; if (%s>= (uint32_t)(TELEMETRY_FREQUENCY*%s)) %s=0;\n" v v v period v;
|
||||||
|
Hashtbl.add found_modulos period !idx;
|
||||||
|
((x, _phase), (period, !idx))
|
||||||
) (Xml.children mode) in
|
) (Xml.children mode) in
|
||||||
let modulos = GC.singletonize (List.map snd messages) in
|
|
||||||
List.iter (fun m ->
|
|
||||||
let v = sprintf "i%d" m in
|
|
||||||
let _type = if m >= 256 then "uint16_t" else "uint8_t" in
|
|
||||||
lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\n" _type v v v m v;
|
|
||||||
) modulos;
|
|
||||||
|
|
||||||
(* create var to loop trough callbacks if needed *)
|
(* create var to loop trough callbacks if needed *)
|
||||||
if (List.length messages > 0) then
|
if (List.length messages > 0) then
|
||||||
lprintf out_h "uint8_t j;\n";
|
lprintf out_h "uint8_t j;\n";
|
||||||
|
|
||||||
(** For each message in this mode *)
|
(** For each message in this mode *)
|
||||||
let messages = List.sort (fun (_,p) (_,p') -> compare p p') messages in
|
let messages = List.sort (fun (_,(p,_)) (_,(p',_)) -> compare p p') messages in
|
||||||
let i = ref 0 in (** Basic balancing:1 message every 10Hz *)
|
|
||||||
let phase = ref 0 in
|
|
||||||
let l = ref [] in
|
|
||||||
List.iter
|
List.iter
|
||||||
(fun (message, p) ->
|
(fun ((message, _phase), (p, i)) ->
|
||||||
let message_name = ExtXml.attrib message "name" in
|
let message_name = ExtXml.attrib message "name" in
|
||||||
i := !i mod p;
|
lprintf out_h "if (i%d == (uint32_t)(TELEMETRY_FREQUENCY*%s*%f)) {\n" i p _phase;
|
||||||
(* if phase attribute is present, use it, otherwise shedule at 10Hz *)
|
|
||||||
let message_phase = try int_of_float (float_of_string (ExtXml.attrib message "phase")*.float_of_int freq) with _ -> !i in
|
|
||||||
phase := message_phase;
|
|
||||||
let else_ = if List.mem_assoc p !l && not (List.mem (p, !phase) !l) then "else " else "" in
|
|
||||||
lprintf out_h "%sif (i%d == %d) {\n" else_ p !phase;
|
|
||||||
l := (p, !phase) :: !l;
|
|
||||||
i := !i + freq/10;
|
|
||||||
right ();
|
right ();
|
||||||
lprintf out_h "for (j = 0; j < TELEMETRY_NB_CBS; j++) {\n";
|
lprintf out_h "for (j = 0; j < TELEMETRY_NB_CBS; j++) {\n";
|
||||||
right ();
|
right ();
|
||||||
@@ -141,7 +139,7 @@ let print_message_table = fun out_h xml ->
|
|||||||
fprintf out_h "}\n\n"
|
fprintf out_h "}\n\n"
|
||||||
) telemetry_types
|
) telemetry_types
|
||||||
|
|
||||||
let print_process_send = fun out_h xml freq ->
|
let print_process_send = fun out_h xml ->
|
||||||
(** For each process *)
|
(** For each process *)
|
||||||
List.iter
|
List.iter
|
||||||
(fun process ->
|
(fun process ->
|
||||||
@@ -178,9 +176,9 @@ let print_process_send = fun out_h xml freq ->
|
|||||||
fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name;
|
fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name;
|
||||||
fprintf out_h "#endif /* PERIODIC_C_%s */\n" (Compat.uppercase_ascii process_name);
|
fprintf out_h "#endif /* PERIODIC_C_%s */\n" (Compat.uppercase_ascii process_name);
|
||||||
|
|
||||||
lprintf out_h "static inline void periodic_telemetry_send_%s(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) { /* %dHz */\n" process_name freq;
|
lprintf out_h "static inline void periodic_telemetry_send_%s(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) {\n" process_name;
|
||||||
right ();
|
right ();
|
||||||
output_modes out_h process_name telem_type modes freq;
|
output_modes out_h process_name telem_type modes;
|
||||||
left ();
|
left ();
|
||||||
lprintf out_h "}\n"
|
lprintf out_h "}\n"
|
||||||
)
|
)
|
||||||
@@ -190,7 +188,7 @@ let print_process_send = fun out_h xml freq ->
|
|||||||
(**
|
(**
|
||||||
* main generation function
|
* main generation function
|
||||||
*)
|
*)
|
||||||
let generate = fun telemetry freq out_file ->
|
let generate = fun telemetry out_file ->
|
||||||
let out = open_out out_file in
|
let out = open_out out_file in
|
||||||
|
|
||||||
(** Print header *)
|
(** Print header *)
|
||||||
@@ -201,14 +199,22 @@ let generate = fun telemetry freq out_file ->
|
|||||||
fprintf out "#define _VAR_PERIODIC_H_\n\n";
|
fprintf out "#define _VAR_PERIODIC_H_\n\n";
|
||||||
fprintf out "#include \"std.h\"\n";
|
fprintf out "#include \"std.h\"\n";
|
||||||
fprintf out "#include \"generated/airframe.h\"\n";
|
fprintf out "#include \"generated/airframe.h\"\n";
|
||||||
fprintf out "#include \"subsystems/datalink/telemetry_common.h\"\n\n";
|
fprintf out "#include \"subsystems/datalink/telemetry_common.h\"\n";
|
||||||
fprintf out "#define TELEMETRY_FREQUENCY %d\n\n" freq;
|
fprintf out "\n";
|
||||||
|
fprintf out "#ifndef TELEMETRY_FREQUENCY\n";
|
||||||
|
fprintf out "#ifdef PERIODIC_FREQUENCY\n";
|
||||||
|
fprintf out "#define TELEMETRY_FREQUENCY PERIODIC_FREQUENCY\n";
|
||||||
|
fprintf out "#else\n";
|
||||||
|
fprintf out "#error \"neither TELEMETRY_FREQUENCY or PERIODIC_FREQUENCY are defined\"\n";
|
||||||
|
fprintf out "#endif\n";
|
||||||
|
fprintf out "#endif\n";
|
||||||
|
fprintf out "\n";
|
||||||
|
|
||||||
(** Print the telemetry table with ID *)
|
(** Print the telemetry table with ID *)
|
||||||
print_message_table out telemetry.Telemetry.xml;
|
print_message_table out telemetry.Telemetry.xml;
|
||||||
|
|
||||||
(** Print process sending functions *)
|
(** Print process sending functions *)
|
||||||
print_process_send out telemetry.Telemetry.xml freq;
|
print_process_send out telemetry.Telemetry.xml;
|
||||||
|
|
||||||
fprintf out "#endif // _VAR_PERIODIC_H_\n";
|
fprintf out "#endif // _VAR_PERIODIC_H_\n";
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user