diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index e0f84cb147..079a87eb70 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -14,8 +14,9 @@ - + + @@ -114,6 +115,11 @@ alt_unit CDATA #IMPLIED alt_unit_coef CDATA #IMPLIED values CDATA #IMPLIED> + + (sector_name, List.map p2D_of (Xml.children xml)) +(** FP variables and ABI auto bindings *) +type fp_var = FP_var of (string * string * string) | FP_binding of (string * string list * string) + +(* get a Hashtbl of ABI messages (name, field_types) *) +let extract_abi_msg = fun filename class_ -> + let xml = ExtXml.parse_file filename in + try + let xml_class = ExtXml.child ~select:(fun x -> Xml.attrib x "name" = class_) xml "msg_class" in + let t = Hashtbl.create (List.length (Xml.children xml_class)) in + List.iter (fun x -> + let name = ExtXml.attrib x "name" + and field_types = List.map (fun field -> ExtXml.attrib field "type") (Xml.children x) in + Hashtbl.add t name field_types + ) (Xml.children xml_class); + t + with + Not_found -> failwith (sprintf "No msg_class '%s' found" class_) + let parse_variables = fun xml -> List.map (fun var -> - let v = ExtXml.attrib var "var" - and t = ExtXml.attrib_or_default var "type" "float" - and i = ExtXml.attrib_or_default var "init" "0" in - (t, v, i) + match Xml.tag var with + | "variable" -> + let v = ExtXml.attrib var "var" + and t = ExtXml.attrib_or_default var "type" "float" + and i = ExtXml.attrib_or_default var "init" "0" in + FP_var (v, t, i) + | "abi_binding" -> + let n = ExtXml.attrib var "name" + and vs = ExtXml.attrib var "vars" + and i = ExtXml.attrib_or_default var "id" "ABI_BROADCAST" in + let vs = Str.split (Str.regexp "[ ]*,[ ]*") vs in + FP_binding (n, vs, i) + | _ -> failwith "Gen_flight_plan: unexpected variables tag" ) xml +let print_var_decl abi_msgs = function + | FP_var (v, t, _) -> printf "extern %s %s;\n" t v + | _ -> () (* ABI variables are not public *) + +let print_var_impl abi_msgs = function + | FP_var (v, t, i) -> printf "%s %s = %s;\n" t v i + | FP_binding (n, vs, _) -> + printf "static abi_event FP_%s_ev;\n" n; + let field_types = Hashtbl.find abi_msgs n in + List.iter2 (fun abi_t user -> if not (user = "_") then printf "static %s %s;\n" abi_t user) field_types vs + +let print_auto_init_bindings = fun abi_msgs variables -> + let print_cb = function + | FP_binding (n, vs, _) -> + let field_types = Hashtbl.find abi_msgs n in + printf "static void FP_%s_cb(uint8_t sender_id __attribute__((unused))" n; + List.iteri (fun i v -> + if v = "_" then printf ", %s _unused_%d __attribute__((unused))" (List.nth field_types i) i + else printf ", %s _%s" (List.nth field_types i) v + ) vs; + printf ") {\n"; + List.iter (fun v -> + if not (v = "_") then printf " %s = _%s;\n" v v + ) vs; + printf "}\n\n" + | _ -> () + in + let print_bindings = function + | FP_binding (n, _, i) -> + printf " AbiBindMsg%s(%s, &FP_%s_ev, FP_%s_cb);\n" n i n n + | _ -> () + in + List.iter print_cb variables; + printf "static inline void auto_nav_init(void) {\n"; + List.iter print_bindings variables; + printf "}\n\n" + let write_settings = fun xml_file out_set variables -> fprintf out_set "\n" xml_file; fprintf out_set "\n" (Env.get_paparazzi_version ()); fprintf out_set "\n\n"; fprintf out_set "\n"; fprintf out_set " \n"; + (* remove some incompatible variables and all ABI bindings *) + let att_exists = fun x a -> + try let _ = Xml.attrib x a in true with Xml.No_attribute _ -> false + in + let variables = List.filter (fun x -> + if Xml.tag x = "variable" && att_exists x "min" && att_exists x "max" && att_exists x "step" then + true + else false) variables in (* add tab only if their are some variables *) if List.length variables > 0 then fprintf out_set " \n"; @@ -905,6 +977,8 @@ let () = printf "#include \"std.h\"\n"; printf "#include \"generated/modules.h\"\n"; + printf "#include \"subsystems/abi.h\"\n"; + printf "#include \"autopilot.h\"\n"; begin try @@ -1043,12 +1117,14 @@ let () = end; lprintf "\n"; let variables = parse_variables variables in - List.iter (fun (t, v, _) -> printf "extern %s %s;\n" t v) variables; + let abi_msgs = extract_abi_msg (Env.paparazzi_home ^ "/conf/abi.xml") "airborne" in + List.iter (fun v -> print_var_decl abi_msgs v) variables; lprintf "\n#ifdef NAV_C\n\n"; - List.iter (fun (t, v, i) -> printf "%s %s = %s;\n" t v i) variables; + List.iter (fun v -> print_var_impl abi_msgs v) variables; lprintf "\n"; + print_auto_init_bindings abi_msgs variables; let index_of_waypoints = let i = ref (-1) in