[paparazzi-commits] [4071] add height option in flight plans

[Gautier Hattenberger]

Revision: 4071
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4071
Author:   gautier
Date:     2009-09-03 11:50:22 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
add height option in flight plans

Modified Paths:
--------------
    paparazzi3/trunk/conf/flight_plans/flight_plan.dtd
    paparazzi3/trunk/sw/airborne/common_nav.h
    paparazzi3/trunk/sw/tools/gen_flight_plan.ml

Modified: paparazzi3/trunk/conf/flight_plans/flight_plan.dtd
===================================================================
--- paparazzi3/trunk/conf/flight_plans/flight_plan.dtd  2009-09-03 11:46:57 UTC 
(rev 4070)
+++ paparazzi3/trunk/conf/flight_plans/flight_plan.dtd  2009-09-03 11:50:22 UTC 
(rev 4071)
@@ -67,7 +67,8 @@
 y CDATA #IMPLIED
 lat CDATA #IMPLIED
 lon CDATA #IMPLIED
-alt CDATA #IMPLIED>
+alt CDATA #IMPLIED
+height CDATA #IMPLIED>
 
 <!ATTLIST exceptions>
 
@@ -109,6 +110,7 @@
 course CDATA #REQUIRED
 vmode CDATA #IMPLIED
 alt CDATA #IMPLIED
+height CDATA #IMPLIED
 throttle CDATA #IMPLIED
 climb CDATA #IMPLIED
 pitch CDATA #IMPLIED
@@ -118,6 +120,7 @@
 roll CDATA #REQUIRED
 vmode CDATA #IMPLIED
 alt CDATA #IMPLIED
+height CDATA #IMPLIED
 throttle CDATA #IMPLIED
 climb CDATA #IMPLIED
 pitch CDATA #IMPLIED
@@ -134,6 +137,7 @@
 vmode CDATA #IMPLIED
 pitch CDATA #IMPLIED
 alt CDATA #IMPLIED
+height CDATA #IMPLIED
 approaching_time CDATA #IMPLIED
 throttle CDATA #IMPLIED
 climb CDATA #IMPLIED>
@@ -170,6 +174,7 @@
 wp_dist CDATA #IMPLIED
 radius CDATA #REQUIRED
 alt CDATA #IMPLIED
+height CDATA #IMPLIED
 vmode CDATA #IMPLIED
 climb CDATA #IMPLIED
 pitch CDATA #IMPLIED
@@ -199,7 +204,10 @@
 <!ATTLIST stay
 wp CDATA #REQUIRED
 vmode CDATA #IMPLIED
-throttle CDATA #IMPLIED>
+throttle CDATA #IMPLIED
+climb CDATA #IMPLIED
+alt CDATA #IMPLIED
+height CDATA #IMPLIED>
 
 <!ATTLIST deroute
 block CDATA #REQUIRED>

Modified: paparazzi3/trunk/sw/airborne/common_nav.h
===================================================================
--- paparazzi3/trunk/sw/airborne/common_nav.h   2009-09-03 11:46:57 UTC (rev 
4070)
+++ paparazzi3/trunk/sw/airborne/common_nav.h   2009-09-03 11:50:22 UTC (rev 
4071)
@@ -41,6 +41,7 @@
 #define WaypointX(_wp) (waypoints[_wp].x)
 #define WaypointY(_wp) (waypoints[_wp].y)
 #define WaypointAlt(_wp) (waypoints[_wp].a)
+#define Height(_h) (_h + ground_alt)
 
 extern void nav_move_waypoint(uint8_t wp_id, float utm_east, float utm_north, 
float alt);
 

Modified: paparazzi3/trunk/sw/tools/gen_flight_plan.ml
===================================================================
--- paparazzi3/trunk/sw/tools/gen_flight_plan.ml        2009-09-03 11:46:57 UTC 
(rev 4070)
+++ paparazzi3/trunk/sw/tools/gen_flight_plan.ml        2009-09-03 11:50:22 UTC 
(rev 4071)
@@ -113,10 +113,12 @@
 
 let print_waypoint = fun default_alt waypoint ->
   let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y")
-  and alt = try Xml.attrib waypoint "alt" with _ -> default_alt in
+  and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> 
default_alt in
+  let alt = try Xml.attrib waypoint "alt" with _ -> alt in
   check_altitude (float_of_string alt) waypoint;
   printf " {%.1f, %.1f, %s},\\\n" x y alt
 
+(*
 let print_waypoint_int32 = fun default_alt waypoint ->
   let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y")
   and alt = float_of_string (try Xml.attrib waypoint "alt" with _ -> 
default_alt) in
@@ -125,6 +127,7 @@
   y_int = truncate (y *. pow8) and
   alt_int = truncate (alt *. pow8) in
   printf " {%d, %d, %d},\\\n" x_int y_int alt_int
+*)
 
 let convert_angle = fun rad -> truncate (1e7 *. (Rad>>Deg)rad)
 (*
@@ -202,9 +205,20 @@
            end;
            a
          with _ ->
-           if wp = "" 
-           then failwith "alt or waypoint required in alt vmode" 
-           else sprintf "WaypointAlt(%s)" wp in
+      try
+        let h = parsed_attrib stage_xml "height" in
+        begin
+          try
+            check_altitude ((float_of_string h) +. !ground_alt) stage_xml
+          with
+          (* Impossible to check the altitude on an expression: *)
+          Failure "float_of_string" -> ()
+          end;
+             sprintf "Height(%s)" h
+      with _ ->
+         if wp = "" 
+             then failwith "alt or waypoint required in alt vmode" 
+             else sprintf "WaypointAlt(%s)" wp in
        lprintf "NavVerticalAltitudeMode(%s, 0.);\n" alt;
     | "xyz" -> () (** Handled in Goto3D() *)
     | "glide" ->
@@ -759,9 +773,9 @@
       Xml2h.define "WAYPOINTS" "{ \\";
       List.iter (print_waypoint alt) waypoints;
       lprintf "};\n";
-      Xml2h.define "WAYPOINTS_INT32" "{ \\";
+      (*Xml2h.define "WAYPOINTS_INT32" "{ \\";
       List.iter (print_waypoint_int32 alt) waypoints;
-      lprintf "};\n";
+      lprintf "};\n";*)
       Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints));
 
       Xml2h.define "NB_BLOCK" (string_of_int (List.length blocks));