for loops in flight plans

2026-05-26 08:22:43 +08:00 · 2005-04-09 08:43:20 +00:00
parent 7e26f199af
commit 27b5a45c55
8 changed files with 100 additions and 13 deletions
@@ -19,7 +19,7 @@
    name="Thon1"
    airframe="airframes/twinstar1.xml" 
    radio="radios/cockpitMM.xml" 
-    flight_plan="flight_plans/muret1.xml"
+    flight_plan="flight_plans/muret_for.xml"
  />

  <aircraft 
@@ -10,13 +10,14 @@
 <!ELEMENT waypoint EMPTY>

 <!ELEMENT blocks (block+)>
-<!ELEMENT block (exception|while|heading|go|xyz|circle|deroute|stay)*>
+<!ELEMENT block (exception|while|heading|go|xyz|circle|deroute|stay|for)*>

 <!ELEMENT include (arg|with)*>
 <!ELEMENT arg EMPTY>
 <!ELEMENT with EMPTY>

-<!ELEMENT while (while|heading|go|xyz|circle|deroute|stay)*>
+<!ELEMENT while (while|heading|go|xyz|circle|deroute|stay|for)*>
+<!ELEMENT for (while|heading|go|xyz|circle|deroute|stay|for)*>
 <!ELEMENT exception EMPTY>
 <!ELEMENT heading EMPTY>
 <!ELEMENT go EMPTY>
@@ -55,6 +56,11 @@ alt CDATA #IMPLIED>

 <!ATTLIST while cond CDATA #IMPLIED>

+<!ATTLIST for
+var CDATA #REQUIRED
+from CDATA #REQUIRED
+to CDATA #REQUIRED>
+
 <!ATTLIST exception
 cond CDATA #REQUIRED
 deroute CDATA #REQUIRED>
@@ -67,7 +73,9 @@ pitch CDATA #IMPLIED
 until CDATA #IMPLIED>

 <!ATTLIST go 
-wp CDATA #REQUIRED
+wp CDATA #IMPLIED
+x CDATA #IMPLIED
+y CDATA #IMPLIED
 wp_qdr CDATA #IMPLIED
 wp_dist CDATA #IMPLIED
 from CDATA #IMPLIED
@@ -80,7 +88,8 @@ alt CDATA #IMPLIED
 until CDATA #IMPLIED
 gaz CDATA #IMPLIED>

-<!ATTLIST xyz radius CDATA #IMPLIED>
+<!ATTLIST xyz
+radius CDATA #IMPLIED>

 <!ATTLIST circle
 wp CDATA #REQUIRED
@@ -0,0 +1,37 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan NAME="example - Muret for loops" LON0="1.27289" MAX_DIST_FROM_HOME="1000" GROUND_ALT="185" SECURITY_HEIGHT="25" QFU="270" ALT="250" LAT0="43.46223">
+  <rc_control>
+    <mode NAME="AUTO1">
+      <setting VAR="ir_pitch_neutral" RANGE="60." RC="gain_1_up" TYPE="int16"/>
+      <setting VAR="ir_roll_neutral" RANGE="-60." RC="gain_1_down" TYPE="int16"/>
+    </mode>
+    <mode NAME="AUTO2">
+      <setting VAR="course_pgain" RANGE="0.1" RC="gain_1_up" TYPE="float"/>
+      <setting VAR="pitch_of_roll" RANGE=".2" RC="gain_1_down" TYPE="float"/>
+    </mode>
+  </rc_control>
+  <waypoints utm_x0="360284.8" utm_y0="4813595.5">
+    <waypoint name="HOME" x="0.0" y="30.0" lat="43.4625000078" lon="1.27288231142" alt="250."/>
+     </waypoints>
+
+  <blocks>
+    <block NAME="init">
+      <while COND="(!launch)"/>
+      <heading VMODE="gaz" GAZ="0.8" PITCH="0.15" COURSE="QFU" UNTIL="(estimator_flight_time > 8)"/>
+      <heading CLIMB="3.0" VMODE="climb" PITCH="0.15" COURSE="QFU" UNTIL="(estimator_z > SECURITY_ALT)"/>
+    </block>
+
+    <block NAME="for">
+      <exception COND="(RcEvent1())" DEROUTE="for"/>
+      <for var="i" from="0" to="3">
+        <go x="-200" y="$i*100"/>
+        <go x="-200" y="($i+3)*100"/>
+        <go x="200" y="($i+3)*100"/>
+        <go x="200" y="($i+1)*100"/>
+      </for>
+      <circle WP="HOME" alt="GROUND_ALT+50" radius="75"/>
+    </block>
+  </blocks>
+</flight_plan>
+
@@ -9,6 +9,7 @@ FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.ml
 ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)

 gen_flight_plan.out : $(FP_CMO)
+	$(OCAMLC) -o /dev/null lib-pprz.cma $^ # To check
 	cat ../../pprz_src_test.sh > $@
 	echo '$(OCAML) -I $$PAPARAZZI_SRC/sw/lib/ocaml -I $$PAPARAZZI_SRC/sw/tools lib-pprz.cma $(ABS_FP) $$*' >> $@
 	chmod a+x $@
@@ -6,7 +6,7 @@ rule token = parse
  | "/*"([^'*']|'*'[^'/'])*'*'*'/' { token lexbuf}
  | ['0'-'9']+ { INT (int_of_string (Lexing.lexeme lexbuf)) }
  | ['0'-'9']+'.'['0'-'9']* { FLOAT (float_of_string (Lexing.lexeme lexbuf)) }
-  | ['a'-'z' 'A'-'Z'] (['a'-'z' 'A'-'Z' '_' '.' '0'-'9']*) { IDENT (Lexing.lexeme lexbuf) }
+  | '$'?['a'-'z' 'A'-'Z'] (['a'-'z' 'A'-'Z' '_' '.' '0'-'9']*) { IDENT (Lexing.lexeme lexbuf) }
  | ',' { COMMA }
  | ';' { SEMICOLON }
  | ':' { COLON }
@@ -16,15 +16,18 @@ type expression =
  | Call of ident * (expression list)
  | Index of ident * expression

+let c_var_of_ident = fun x -> "_var_" ^ x
+
 (* Valid unary and binary opetarors *)
-let binary_operators = ["+"; ">"; "-"]
+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 -> sprintf "%s" i
+    Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1)))
+  | Ident i -> sprintf "%s" i
  | Int i -> sprintf "%d" i
  | Float i -> sprintf "%f" i
  | Call (op, [e1;e2]) when is_binary op ->
@@ -63,7 +66,8 @@ exception Unknown_function of string

 let rec check_expression = fun e ->
  match e with
-    Ident i ->
+    Ident i when i.[0] = '$' -> ()
+  | Ident i ->
      if not (List.mem i variables) then
 	raise (Unknown_ident i)
  | Int _  | Float _ -> ()
@@ -33,6 +33,9 @@ type expression =
  | Call of ident * expression list
  | Index of ident * expression

+val c_var_of_ident : ident -> string
+(** Encapsulate a user ident into a C variable *)
+
 val sprint_expression : expression -> string

 exception Unknown_ident of string
@@ -35,7 +35,7 @@ let parse = fun s ->
  if !check_expressions then
    let e = parse_expression s in
    let unexpected = fun kind x ->
-      fprintf stderr "Paring error in '%s': unexpected %s: '%s' \n" s kind x;
+      fprintf stderr "Parsing error in '%s': unexpected %s: '%s' \n" s kind x;
      exit 1 in
    begin
      try
@@ -191,6 +191,9 @@ let rec compile_stage = fun block x ->
     "while" ->
       List.iter (compile_stage block) (Xml.children x);
       incr stage (* To count the loop stage *)
+    | "for" ->
+       List.iter (compile_stage block) (Xml.children x);
+       incr stage (* To count the loop stage *)
    | "return_from_excpt" | "goto"  | "deroute" | "exit_block"
    | "heading" | "go" | "stay" | "xyz" | "circle" -> ()
    | s -> failwith (sprintf "Unknown stage: %s\n" s)
@@ -223,6 +226,21 @@ let rec print_stage = fun index_of_waypoints x ->
 	List.iter (print_stage index_of_waypoints) (Xml.children x);
 	print_stage index_of_waypoints (goto w);
 	output_label e
+    | "for" ->
+	let f = gen_label "for" in
+	let e = gen_label "endfor" in
+	let v = Fp_syntax.c_var_of_ident (ExtXml.attrib x "var")
+	and from_ = parsed_attrib x "from" 
+	and to_expr = parsed_attrib x "to"  in
+	let to_var = v ^ "_to" in
+	lprintf "static int8_t %s = %s - 1;\n" v from_;
+	lprintf "static const int8_t %s = %s;\n" to_var to_expr;
+	output_label f;
+	stage ();
+	lprintf "if (++%s > %s) Goto(%s) else NextStage();\n" v to_var e;
+	List.iter (print_stage index_of_waypoints) (Xml.children x);
+	print_stage index_of_waypoints (goto f);
+	output_label e
    | "heading" ->
 	stage ();
 	let until = parsed_attrib x "until" in
@@ -234,7 +252,15 @@ let rec print_stage = fun index_of_waypoints x ->
 	lprintf "return;\n"
    | "go" ->
 	stage ();
-	let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in
+	let wp = 
+	  try
+	    get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints
+	  with
+	    _ ->
+	      lprintf "waypoints[0].x = %s;\n" (parsed_attrib x "x");
+	      lprintf "waypoints[0].y = %s;\n" (parsed_attrib x "y");
+	      "0"
+	in
 	lprintf "if (approaching(%s)) NextStageFrom(%s) else {\n" wp wp;
 	right ();
 	let last_wp =
@@ -354,7 +380,14 @@ 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
-  
+
+
+let dummy_waypoint = 
+  Xml.Element ("waypoint", 
+	       ["name", "dummy"; 
+		"x", "42."; 
+		"y", "42." ], 
+	       [])


 let _ =
@@ -426,7 +459,7 @@ let _ =
      Xml2h.define "QFU" (sprintf "%.1f" qfu);

      
-      let waypoints = Xml.children waypoints in
+      let waypoints = dummy_waypoint :: Xml.children waypoints in
      let (hx, hy) = define_home waypoints in
      List.iter (check_distance (hx, hy) mdfh) waypoints;