for loops in flight plans

sw/tools/Makefile

@@ -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 $@

sw/tools/fp_lexer.mll

@@ -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 }

sw/tools/fp_syntax.ml

@@ -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 _ -> ()

sw/tools/fp_syntax.mli

@@ -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

sw/tools/gen_flight_plan.ml

@@ -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;