From 216d354269b3631c27fd7844c0b82ffd22bade4c Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Wed, 10 Aug 2005 16:34:25 +0000 Subject: [PATCH] upgrade from paparazzi2 ;-) --- Makefile | 2 +- conf/radios/cockpitMM.xml | 61 +++++++++++++++++++--- conf/radios/radio.dtd | 18 +++++++ sw/airborne/autopilot/Makefile | 1 - sw/include/std.h | 7 +++ sw/lib/ocaml/extXml.ml | 36 ++++++++++++- sw/lib/ocaml/extXml.mli | 5 ++ sw/tools/fp_proc.ml | 22 ++++---- sw/tools/fp_syntax.ml | 17 ++++--- sw/tools/gen_airframe.ml | 12 +++-- sw/tools/gen_flight_plan.ml | 84 ++++++++++++++++++++++++++---- sw/tools/gen_radio.ml | 93 +++++++++++++++++++++++++++------- 12 files changed, 294 insertions(+), 64 deletions(-) create mode 100644 conf/radios/radio.dtd diff --git a/Makefile b/Makefile index 17fbab031b..4446c14333 100644 --- a/Makefile +++ b/Makefile @@ -103,7 +103,7 @@ receive: tmtc static_h : PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` make -f Makefile.gen -ac_h : +ac_h : static_h PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` $(TOOLS)/gen_aircraft.out $(AIRCRAFT) sim_ac: ac_h sim_sitl diff --git a/conf/radios/cockpitMM.xml b/conf/radios/cockpitMM.xml index ea56003e1b..c40aa2744d 100644 --- a/conf/radios/cockpitMM.xml +++ b/conf/radios/cockpitMM.xml @@ -1,9 +1,54 @@ - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/conf/radios/radio.dtd b/conf/radios/radio.dtd new file mode 100644 index 0000000000..f84c2b6304 --- /dev/null +++ b/conf/radios/radio.dtd @@ -0,0 +1,18 @@ + + + + + + + + diff --git a/sw/airborne/autopilot/Makefile b/sw/airborne/autopilot/Makefile index ee44b7acf8..3a87b10629 100644 --- a/sw/airborne/autopilot/Makefile +++ b/sw/airborne/autopilot/Makefile @@ -65,7 +65,6 @@ $(TARGET).srcs = \ uart.c \ estimator.c \ if_calib.c \ - traffic_info.c \ mainloop.c include ../../../conf/Makefile.local diff --git a/sw/include/std.h b/sw/include/std.h index 6103fdebd2..05d910df7e 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -35,4 +35,11 @@ /* Boolean values */ typedef uint8_t bool_t; +#ifndef cbi +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + #endif /* STD_H */ diff --git a/sw/lib/ocaml/extXml.ml b/sw/lib/ocaml/extXml.ml index 5379807429..9f30dfb298 100644 --- a/sw/lib/ocaml/extXml.ml +++ b/sw/lib/ocaml/extXml.ml @@ -30,7 +30,7 @@ let sep = Str.regexp "\\." let child xml ?select c = let rec find = function - Xml.Element (tag, _attributes, _children) as elt :: elts -> + Xml.Element (tag, attributes, _children) as elt :: elts -> if tag = c then match select with None -> elt @@ -85,3 +85,37 @@ let to_string_fmt = fun xml -> List.map (fun (a,v) -> (l a, v)) ats, List.map lower cs) in Xml.to_string_fmt (lower xml) + + +let subst_attrib = fun attrib value xml -> + let u = String.uppercase in + let uattrib = u attrib in + match xml with + Xml.Element (tag, attrs, children) -> + let rec loop = function + [] -> [(attrib, value)] + | (a,v) as c::ats -> + if u a = uattrib then loop ats else c::loop ats in + Xml.Element (tag, + loop attrs, + children) + | Xml.PCData _ -> xml + + +let subst_child = fun t x xml -> + let u = String.uppercase in + match xml with + Xml.Element (tag, attrs, children) -> + Xml.Element (tag, + attrs, + List.map (fun xml -> if u (Xml.tag xml) = u t then x else xml) children) + | Xml.PCData _ -> xml + + +let float_attrib = fun xml a -> + let v = attrib xml a in + try + float_of_string v + with + _ -> failwith (Printf.sprintf "Error: float expected in '%s'" v) + diff --git a/sw/lib/ocaml/extXml.mli b/sw/lib/ocaml/extXml.mli index df33e0f402..ada4ecee14 100644 --- a/sw/lib/ocaml/extXml.mli +++ b/sw/lib/ocaml/extXml.mli @@ -46,3 +46,8 @@ val attrib_or_default : Xml.xml -> string -> string -> string val to_string_fmt : Xml.xml -> string (** [to_string_fmt xml] Returns a formatted string where tag and attribute names are lowercase *) + +val subst_attrib : string -> string -> Xml.xml -> Xml.xml +val subst_child : string -> Xml.xml -> Xml.xml -> Xml.xml +val float_attrib : Xml.xml -> string -> float + diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml index 3f514c48b4..f260719056 100644 --- a/sw/tools/fp_proc.ml +++ b/sw/tools/fp_proc.ml @@ -44,8 +44,6 @@ let parse_expression = fun s -> open Latlong -let float_attrib = fun xml a -> float_of_string (ExtXml.attrib xml a) - (* Translation and rotation *) type affine = { dx : float; dy : float; angle : float (* Deg Clockwise *) } @@ -103,8 +101,8 @@ let transform_values = fun attribs_not_modified affine env attribs -> attribs let transform_waypoint = fun prefix affine xml -> - let x = float_attrib xml "x" - and y = float_attrib xml "y" in + let x = ExtXml.float_attrib xml "x" + and y = ExtXml.float_attrib xml "y" in let (x, y) = rotate affine.angle (x, y) in let (x, y) = (x+.affine.dx, y+.affine.dy) in let alt = try ["alt", ExtXml.attrib xml "alt"] with ExtXml.Error _ -> [] in @@ -214,9 +212,9 @@ let parse_include = fun dir include_xml -> let proc_name = ExtXml.attrib include_xml "name" in let prefix = fun x -> proc_name ^ "." ^ x in let affine = { - dx = float_attrib include_xml "x"; - dy = float_attrib include_xml "y"; - angle = float_attrib include_xml "rotate" + dx = ExtXml.float_attrib include_xml "x"; + dy = ExtXml.float_attrib include_xml "y"; + angle = ExtXml.float_attrib include_xml "rotate" } in let reroutes = List.filter @@ -312,7 +310,7 @@ let xml_assoc_attrib = fun a v xmls -> | _ -> failwith "xml_assoc_attrib" let coords_of_waypoint = fun wp -> - (float_attrib wp "x", float_attrib wp "y") + (ExtXml.float_attrib wp "x", ExtXml.float_attrib wp "y") let new_waypoint = fun wp qdr dist waypoints -> @@ -329,8 +327,8 @@ let new_waypoint = fun wp qdr dist waypoints -> let replace_wp = fun stage waypoints -> try - let qdr = float_attrib stage "wp_qdr" - and dist = float_attrib stage "wp_dist" in + let qdr = ExtXml.float_attrib stage "wp_qdr" + and dist = ExtXml.float_attrib stage "wp_dist" in let wp = ExtXml.attrib stage "wp" in let name = new_waypoint wp qdr dist waypoints in @@ -343,8 +341,8 @@ let replace_wp = fun stage waypoints -> let replace_from = fun stage waypoints -> try - let qdr = float_attrib stage "from_qdr" - and dist = float_attrib stage "from_dist" in + let qdr = ExtXml.float_attrib stage "from_qdr" + and dist = ExtXml.float_attrib stage "from_dist" in let wp = ExtXml.attrib stage "from" in let name = new_waypoint wp qdr dist waypoints in diff --git a/sw/tools/fp_syntax.ml b/sw/tools/fp_syntax.ml index 2c7e90c415..f9ffc0a442 100644 --- a/sw/tools/fp_syntax.ml +++ b/sw/tools/fp_syntax.ml @@ -19,13 +19,6 @@ type expression = let c_var_of_ident = fun x -> "_var_" ^ x -(* Valid unary and binary opetarors *) -(*let binary_operators = ["+"; ">"; "-"; "*"] -let unary_operators = ["!"; "-"] - -let is_binary = fun op -> List.mem op binary_operators -let is_unary = fun op -> List.mem op unary_operators*) - let rec sprint_expression = function Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1))) | Ident i -> sprintf "%s" i @@ -46,19 +39,27 @@ let functions = [ "Qdr"; "And"; "Or"; + "RcRoll"; "RcEvent1"; - "RcEvent2"] (*@ binary_operators @ unary_operators*) + "RcEvent2"; + "RadOfDeg"] (*@ binary_operators @ unary_operators*) (* Valid identifiers *) let variables = [ "launch"; "estimator_z"; "estimator_flight_time"; + "estimator_hspeed_mod"; + "estimator_theta"; + "circle_count"; + "vsupply"; "stage_time"; + "stage_time_ds"; "block_time"; "SECURITY_ALT"; "GROUND_ALT"; "TRUE"; + "FALSE"; "QFU" ] diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml index f0c188565f..a054e8c422 100644 --- a/sw/tools/gen_airframe.ml +++ b/sw/tools/gen_airframe.ml @@ -48,8 +48,12 @@ let define_macro name n x = let parse_element = fun prefix s -> match Xml.tag s with - "define" -> - define (prefix^ExtXml.attrib s "name") (ExtXml.attrib s "value") + "define" -> begin + try + define (prefix^ExtXml.attrib s "name") (ExtXml.attrib s "value"); + define (prefix^(ExtXml.attrib s "name")^"_NB_SAMPLE") (ExtXml.attrib s "nb_sample"); + with _ -> (); + end | "linear" -> let name = ExtXml.attrib s "name" and n = int_of_string (ExtXml.attrib s "arity") in @@ -74,7 +78,9 @@ let parse_servo = servo_params.(no_servo) <- { min = min; neutral = neutral; max = max } -let pprz_value = Str.regexp "@\\([A-Z]+\\)" +(* Characters checked in Gen_radio.checl_function_name *) +let pprz_value = Str.regexp "@\\([A-Z_0-9]+\\)" + let var_value = Str.regexp "\\$\\([_a-z0-9]+\\)" let preprocess_command = fun s -> let s = Str.global_replace pprz_value "values[RADIO_\\1]" s in diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 012a77eef9..dce707293e 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -27,6 +27,8 @@ open Printf open Latlong +let sof = string_of_float + let check_expressions = ref true let parse_expression = Fp_proc.parse_expression @@ -124,8 +126,15 @@ let stage = ref 0 let output_label l = lprintf "Label(%s)\n" l let output_vmode x wp last_wp = - let pitch = try parsed_attrib x "pitch" with _ -> "0.0" in - lprintf "nav_pitch = %s;\n" pitch; + let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in + if pitch = "auto" + then begin + lprintf "auto_pitch = TRUE;\n"; + lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz") + end else begin + lprintf "auto_pitch = FALSE;\n"; + lprintf "nav_pitch = %s;\n" (parse pitch); + end; let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in begin match vmode with @@ -141,18 +150,22 @@ let output_vmode x wp last_wp = try check_altitude (float_of_string a) x with + (* Impossible to check the altitude on an expression: *) Failure "float_of_string" -> () end; a with _ -> - if wp = "" then failwith "alt or waypoint required in alt vmode" else - sprintf "waypoints[%s].a" wp in + if wp = "" + then failwith "alt or waypoint required in alt vmode" + else sprintf "waypoints[%s].a" wp in lprintf "desired_altitude = %s;\n" alt; lprintf "pre_climb = 0.;\n" | "glide" -> lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n"; lprintf "glide_to(%s, %s);\n" last_wp wp | "gaz" -> + if (pitch = "auto") then + failwith "auto pich mode not compatible with vmode=gaz"; lprintf "vertical_mode = VERTICAL_MODE_AUTO_GAZ;\n"; lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz") | x -> failwith (sprintf "Unknown vmode '%s'" x) @@ -194,8 +207,8 @@ let rec compile_stage = fun block x -> | "for" -> List.iter (compile_stage block) (Xml.children x); incr stage (* To count the loop stage *) - | "return_from_excpt" | "goto" | "deroute" | "exit_block" | "follow" - | "heading" | "go" | "stay" | "xyz" | "circle" -> () + | "return_from_excpt" | "goto" | "deroute" | "exit_block" + | "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" -> () | s -> failwith (sprintf "Unknown stage: %s\n" s) end @@ -256,6 +269,16 @@ let rec print_stage = fun index_of_waypoints x -> let d = ExtXml.attrib x "distance" in lprintf "Follow(%s, %s);\n" id d; lprintf "return;\n" + | "attitude" -> + stage (); + let until = parsed_attrib x "until" in + lprintf "if (%s) NextStage() else {\n" until; + right (); + lprintf "lateral_mode = LATERAL_MODE_ROLL;\n"; + lprintf "nav_desired_roll = RadOfDeg(%s);\n" (parsed_attrib x "roll"); + ignore (output_vmode x "" ""); + left (); lprintf "}\n"; + lprintf "return;\n" | "go" -> stage (); let wp = @@ -311,6 +334,20 @@ let rec print_stage = fun index_of_waypoints x -> ExtXml.Error _ -> () end; lprintf "return;\n" + | "set" -> + stage (); + let var = parsed_attrib x "var" in + let valuee = parsed_attrib x "value" in + lprintf "%s = %s;\n" var valuee; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStage();\n" c + with + ExtXml.Error _ -> + lprintf "NextStage();\n"; + end; + lprintf "return;\n" | s -> failwith "Unreachable" end; left () @@ -386,7 +423,29 @@ let check_distance = fun (hx, hy) max_d wp -> let d = sqrt ((x-.hx)**2. +. (y-.hy)**2.) in if d > max_d then fprintf stderr "\nWARNING: Waypoint '%s' too far from HOME (%.0f>%.0f)\n\n" (name_of wp) d max_d + +(* Check coherence between global ref and waypoints ref *) +(* Returns a patched xml with utm_x0 and utm_y0 set *) +let check_geo_ref = fun xml -> + let get_float = fun x -> float_attrib xml x in + let lat0_deg = get_float "lat0" + and lon0_deg = get_float "lon0" in + let utm0 = utm_of WGS84 { posn_lat=(Deg>>Rad)lat0_deg; + posn_long=(Deg>>Rad)lon0_deg } in + + let max_d = get_float "max_dist_from_home" in + let check_zone = fun u -> + if (utm_of WGS84 (of_utm WGS84 u)).utm_zone <> utm0.utm_zone then + failwith "Fatal error: You are too close (less than twice the max distance) to an UTM zone border !" in + check_zone { utm0 with utm_x = utm0.utm_x +. 2.*.max_d }; + check_zone { utm0 with utm_x = utm0.utm_x -. 2.*.max_d }; + + let wpts = ExtXml.child xml "waypoints" in + let wpts = ExtXml.subst_attrib "utm_x0" (sof utm0.utm_x) wpts in + let wpts = ExtXml.subst_attrib "utm_y0" (sof utm0.utm_y) wpts in + let x = ExtXml.subst_child "waypoints" wpts xml in + x let dummy_waypoint = Xml.Element ("waypoint", @@ -397,7 +456,7 @@ let dummy_waypoint = let _ = - let xml_file = ref "" + let xml_file = ref "fligh_plan.xml" and dump = ref false in Arg.parse [("-dump", Arg.Set dump, "Dump compile result"); ("-nocheck", Arg.Clear check_expressions, "Disable expression checking")] @@ -407,9 +466,11 @@ let _ = failwith (sprintf "Usage: %s " Sys.argv.(0)); try let xml = Xml.parse_file !xml_file in + + let xml = check_geo_ref xml in + let dir = Filename.dirname !xml_file in let xml = Fp_proc.process_includes dir xml in - (*** prerr_endline (Xml.to_string_fmt xml); prerr_endline "\n\n\n"; ***) let xml = Fp_proc.process_relative_waypoints xml in let waypoints = ExtXml.child xml "waypoints" and blocks = Xml.children (ExtXml.child xml "blocks") in @@ -474,9 +535,9 @@ let _ = lprintf "};\n"; Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); - Xml2h.define "GROUND_ALT" (string_of_float !ground_alt); - Xml2h.define "SECURITY_ALT" (string_of_float (!security_height +. !ground_alt)); - Xml2h.define "MAX_DIST_FROM_HOME" (string_of_float mdfh); + Xml2h.define "GROUND_ALT" (sof !ground_alt); + Xml2h.define "SECURITY_ALT" (sof (!security_height +. !ground_alt)); + Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh); let index_of_waypoints = let i = ref (-1) in @@ -489,3 +550,4 @@ let _ = with Xml.Error e -> prerr_endline (Xml.error e); exit 1 | Dtd.Prove_error e -> prerr_endline (Dtd.prove_error e); exit 1 + diff --git a/sw/tools/gen_radio.ml b/sw/tools/gen_radio.ml index 9af47429b6..d2cfdd8e35 100644 --- a/sw/tools/gen_radio.ml +++ b/sw/tools/gen_radio.ml @@ -30,28 +30,83 @@ open Xml2h let h_name = "RADIO_H" let fos = float_of_string +type us = int -type channel = { min : string; max : string; neutral : string; averaged : string } +type channel = { + name : string; + min : us; + max : us; + neutral : us; + averaged : bool } -let default_neutral = "1600" -let default_min = "1000" -let default_max = "2200" + +(* Characters used in Gen_airframe.pprz_value *) +let check_function_name = fun s -> + for i = 0 to String.length s - 1 do + match s.[i] with + 'A'..'Z' | '0'..'9' | '_' -> () + | _ -> + failwith (sprintf "Character '%c' not allowed in function name '%s'" s.[i] s) + done let parse_channel = let no_channel = ref 0 in fun c -> + let name = ExtXml.attrib c "function" in + check_function_name name; let ctl = "RADIO_CTL_"^ExtXml.attrib c "ctl" - and fct = "RADIO_" ^ ExtXml.attrib c "function" in + and fct = "RADIO_" ^ name in define ctl (string_of_int !no_channel); define fct ctl; no_channel := !no_channel + 1; - { min = ExtXml.attrib_or_default c "min" default_min; - neutral = ExtXml.attrib_or_default c "neutral" default_neutral; - max = ExtXml.attrib_or_default c "max" default_max; - averaged = ExtXml.attrib_or_default c "average" "0" + let int_attrib = fun x -> int_of_string (ExtXml.attrib c x) in + { min = int_attrib "min"; + neutral = int_attrib "neutral"; + max = int_attrib "max"; + averaged = ExtXml.attrib_or_default c "average" "0" <> "0"; + name = name } +let gen_last_radio_from_ppm = fun channels -> + printf "#define LastRadioFromPpm() {\\\n"; + printf " static uint8_t avg_cpt = 0; /* Counter for averaging */\\\n"; + printf " int16_t tmp_radio;\\\n"; + List.iter + (fun c -> + printf " tmp_radio = ppm_pulses[RADIO_%s] - (CLOCK*%d);\\\n" c.name c.neutral; + let period = if c.averaged then "AVERAGING_PERIOD" else "1" in + let value, min_pprz = + if c.neutral = c.min then + sprintf "tmp_radio * (MAX_PPRZ / %s / (float)(CLOCK*(%d-%d)))" period c.max c.min, "0" + else + sprintf "tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/%s/(float)(CLOCK*(%d-%d))) : (MIN_PPRZ/%s/(float)(CLOCK*(%d-%d))))" period c.max c.neutral period c.min c.neutral, "MIN_PPRZ" in + if c.averaged then begin + printf " avg_last_radio[RADIO_%s] += %s;\\\n" c.name value + end else begin + printf " last_radio[RADIO_%s] = %s;\\\n" c.name value; + printf " if (last_radio[RADIO_%s] > MAX_PPRZ) last_radio[RADIO_%s] = MAX_PPRZ;\\\n else if (last_radio[RADIO_%s] < %s) last_radio[RADIO_%s] = %s; \\\n\\\n" c.name c.name c.name min_pprz c.name min_pprz; + end + ) + channels; + printf "avg_cpt++;\\\n"; + printf " if (avg_cpt == AVERAGING_PERIOD) {\\\n"; + printf " avg_cpt = 0;\\\n"; + List.iter + (fun c -> + if c.averaged then begin + printf " last_radio[RADIO_%s] = avg_last_radio[RADIO_%s];\\\n" c.name c.name; + printf " avg_last_radio[RADIO_%s] = 0;\\\n" c.name; + printf " if (last_radio[RADIO_%s] > MAX_PPRZ) last_radio[RADIO_%s] = MAX_PPRZ;\\\n else if (last_radio[RADIO_%s] < MIN_PPRZ) last_radio[RADIO_%s] = MIN_PPRZ; \\\n\\\n" c.name c.name c.name c.name; + end + ) + channels; + printf " last_radio_contains_avg_channels = TRUE;\\\n"; + printf " }\\\n"; + printf "}\n" + + + let _ = if Array.length Sys.argv < 2 then failwith "Usage: gen_radio xml_file"; @@ -71,19 +126,19 @@ let _ = define "RADIO_CTL_NB" (string_of_int (List.length channels)); nl (); - (* For compatibility *) - define "PPM_PULSE_NEUTRAL_US" default_neutral; - nl (); let channels_params = List.map parse_channel channels in nl (); - define "RADIO_MINS_US" (sprint_float_array (List.map (fun x -> x.min) channels_params)); - define "RADIO_NEUTRALS_US" (sprint_float_array (List.map (fun x -> x.neutral) channels_params)); - define "RADIO_MAXS_US" (sprint_float_array (List.map (fun x -> x.max) channels_params)); - define "RADIO_AVERAGED" (sprint_float_array (List.map (fun x -> x.averaged) channels_params)); - define "RADIO_NEUTRALS_PPM" (sprint_float_array (List.map (fun x -> string_of_int ((int_of_string x.neutral)*16)) channels_params)); - define "RADIO_TRAVEL_PPM" (sprint_float_array (List.map (fun x -> string_of_float (9600. *. 2. /. (float ((int_of_string x.max) - (int_of_string x.min))) /. 16.)) channels_params)); + + let ppm_min = ExtXml.attrib xml "min" in + let ppm_max = ExtXml.attrib xml "max" in + let ppm_sync= ExtXml.attrib xml "sync" in + + printf "#define PPM_MIN_PULSE_WIDTH %sul*CLOCK\n" ppm_min; + printf "#define PPM_MAX_PULSE_WIDTH %sul*CLOCK\n" ppm_max; + printf "#define PPM_SYNC_PULSE (uint8_t)(((uint32_t)(%sul*CLOCK))/1024ul)\n" ppm_sync; nl (); - define "AveragedChannel(ch)" "(((int[])RADIO_AVERAGED)[ch])"; + + gen_last_radio_from_ppm channels_params; printf "\n#endif // %s\n" h_name