[generators] fix altitude in WAYPOINTS_ENU and WAYPOINTS_LLA

paparazzi · Aug 20, 2014 · 7a481cb · 7a481cb
1 parent 8fbf4f9
commit 7a481cb
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -148,7 +148,7 @@ void nav_init(void) {
   for (i = 0; i < nb_waypoint; i++) {
     waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x);
     waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y);
-    waypoints[i].z = POS_BFP_OF_REAL((wp_tmp_float[i].z - GROUND_ALT));
+    waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z);
   }
   nav_block = 0;
   nav_stage = 0;

diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
@@ -122,17 +122,17 @@ let print_waypoint_enu = fun utm0 default_alt waypoint ->
   let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y")
   and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in
   let alt = try Xml.attrib waypoint "alt" with _ -> alt in
-  let ecef0 = Latlong.ecef_of_geo Latlong.WGS84 (Latlong.of_utm Latlong.WGS84 utm0) (float_of_string alt) in
+  let ecef0 = Latlong.ecef_of_geo Latlong.WGS84 (Latlong.of_utm Latlong.WGS84 utm0) !ground_alt in
   let ecef = Latlong.ecef_of_geo Latlong.WGS84 (Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm0 (x, y))) (float_of_string alt) in
   let ned = Latlong.array_of_ned (Latlong.ned_of_ecef ecef0 ecef) in
   printf " {%.2f, %.2f, %.2f}, /* ENU in meters  */ \\\n" ned.(1) ned.(0) (-.ned.(2))
 
 let convert_angle = fun rad -> Int64.of_float (1e7 *. (Rad>>Deg)rad)
 
 let print_waypoint_lla = fun utm0 default_alt waypoint ->
-  let x = float_attrib waypoint "x"
-  and y = float_attrib waypoint "y"
-  and alt = try Xml.attrib waypoint "alt" with _ -> default_alt in
+  let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y")
+  and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in
+  let alt = try Xml.attrib waypoint "alt" with _ -> alt in
   let wgs84 = Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm0 (x, y)) in
   printf " {%Ld, %Ld, %.0f}, /* 1e7deg, 1e7deg, cm (hmsl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (100. *. float_of_string alt) (Egm96.of_wgs84 wgs84)