[forward_rotorcraft] last step to fly fixedwings with rotorcraft code…

…: forward flightplan logic!

conf/flight_plans/flight_plan.dtd

@@ -198,6 +198,7 @@ approaching_time CDATA #IMPLIED
 exceeding_time CDATA #IMPLIED
 throttle CDATA #IMPLIED
 climb CDATA #IMPLIED
+forward CDATA #IMPLIED
 until CDATA #IMPLIED>
 
 <!ATTLIST path

sw/airborne/subsystems/navigation/common_flight_plan.c

@@ -54,5 +54,6 @@ void nav_goto_block(uint8_t b)
     last_block = nav_block;
     last_stage = nav_stage;
   }
-  GotoBlock(b);
+  nav_block = b;
+  nav_init_block();
 }

sw/airborne/subsystems/navigation/common_flight_plan.h

@@ -44,8 +44,8 @@ void nav_goto_block(uint8_t block_id);
 #define InitStage() nav_init_stage();
 
 #define Block(x) case x: nav_block=x;
-#define NextBlock() { nav_block++; nav_init_block(); }
-#define GotoBlock(b) { nav_block=b; nav_init_block(); }
+#define NextBlock() nav_goto_block(nav_block + 1)
+#define GotoBlock(b) nav_goto_block(b)
 
 #define Stage(s) case s: nav_stage=s;
 #define NextStage() { nav_stage++; InitStage(); }
@@ -55,6 +55,7 @@ void nav_goto_block(uint8_t block_id);
 #define Label(x) label_ ## x:
 #define Goto(x) { goto label_ ## x; }
 #define Return() { nav_block=last_block; nav_stage=last_stage; block_time=0;}
+#define ReturnToStartPrevBlock() { nav_block=last_block; nav_stage=0; block_time=0;}
 
 #define And(x, y) ((x) && (y))
 #define Or(x, y) ((x) || (y))

sw/tools/generators/gen_airframe.ml

@@ -269,14 +269,28 @@ let parse_command = fun command no ->
   let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
   { failsafe_value = failsafe_value; foo = 0}
 
-let parse_heli_curves = fun heli_surves ->
-  let a = fun s -> ExtXml.attrib heli_surves s in
-  match Xml.tag heli_surves with
+let parse_heli_curves = fun heli_curves ->
+  let a = fun s -> ExtXml.attrib heli_curves s in
+  match Xml.tag heli_curves with
       "curve" ->
         let throttle = a "throttle" in
+        let rpm = ExtXml.attrib_or_default heli_curves "rpm" "" in
         let collective = a "collective" in
-        printf "  {.nb_points = %i, \\\n" (List.length (Str.split (Str.regexp ",") throttle));
+        let nb_throttle = List.length (Str.split (Str.regexp ",") throttle) in
+        let nb_rpm = List.length (Str.split (Str.regexp ",") rpm) in
+        let nb_collective = List.length (Str.split (Str.regexp ",") collective) in
+        if nb_throttle < 1 then
+          failwith (Printf.sprintf "Need at least one value in throttle curve for a throttle ('%s', '%s')" throttle collective);
+        if nb_throttle <> nb_collective then
+          failwith (Printf.sprintf "Amount of throttle points not the same as collective in throttle curve ('%s', '%s')" throttle collective);
+        if nb_throttle <> nb_rpm && nb_rpm <> 0 then
+          failwith (Printf.sprintf "Amount of throttle points not the same as rpm in throttle curve ('%s', '%s', '%s')" throttle collective rpm);
+        printf "  {.nb_points = %i, \\\n" nb_throttle;
         printf "   .throttle = {%s}, \\\n" throttle;
+        if nb_rpm <> 0 then
+          printf "   .rpm = {%s}, \\\n" rpm
+        else
+          printf "   .rpm = {0xFFFF}, \\\n";
         printf "   .collective = {%s}}, \\\n" collective
     | _ -> xml_error "mixer"
 

sw/tools/generators/gen_flight_plan.ml

@@ -427,6 +427,7 @@ let rec print_stage = fun index_of_waypoints x ->
         in
         let at = try Some (ExtXml.attrib x "approaching_time") with _ -> None in
         let et = try Some (ExtXml.attrib x "exceeding_time") with _ -> None in
+        let fwd = try Some (ExtXml.attrib x "forward") with _ -> None in
         let at = match at, et with
           | Some a, None -> a
           | None, Some e -> "-"^e
@@ -446,6 +447,11 @@ let rec print_stage = fun index_of_waypoints x ->
         let vmode = output_vmode x wp last_wp in
         if vmode = "glide" && hmode <> "route" then
           failwith "glide vmode requires route hmode";
+        if fwd = Some("true") then begin
+          lprintf "outback_hybrid_mode = HB_FORWARD;\n";
+          lprintf "nav_set_heading_towards_target();\n"
+        end else
+          lprintf "outback_hybrid_mode = HB_HOVER;\n";
         left (); lprintf "}\n";
         begin
           try
@@ -462,10 +468,12 @@ let rec print_stage = fun index_of_waypoints x ->
             let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in
             ignore (output_hmode x wp "");
             ignore (output_vmode x wp "");
+            lprintf "outback_hybrid_mode = HB_HOVER;\n";
           with
               Xml2h.Error _ ->
                 lprintf "NavGotoXY(last_x, last_y);\n";
-                ignore(output_vmode x "" "")
+                ignore(output_vmode x "" "");
+                lprintf "outback_hybrid_mode = HB_HOVER;\n"
         end;
         begin
           try