Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

upgrade from paparazzi2 ;-)

Browse Source
This commit is contained in:
Pascal Brisset
2005-08-10 16:34:25 +00:00
parent 47a0ac3cd9
commit 216d354269
12 changed files with 294 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/tools/fp_proc.ml
+10 -12
View File
@@ -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
sw/tools/fp_syntax.ml
+9 -8
View File
@@ -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"
]
sw/tools/gen_airframe.ml
+9 -3
View File
@@ -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
sw/tools/gen_flight_plan.ml
+73 -11
View File
@@ -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 <xml-flight-plan-file>" 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
sw/tools/gen_radio.ml
+74 -19
View File
@@ -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