Merge pull request paparazzi#1450 from paparazzi/fp_utm_zones

[flight plan] force UTM zone of waypoints to be the same than ref point

fixes paparazzi#303

sw/airborne/subsystems/navigation/common_nav.c

@@ -35,7 +35,7 @@ float dist2_to_wp;
 bool_t too_far_from_home;
 
 const uint8_t nb_waypoint = NB_WAYPOINT;
-struct point waypoints[NB_WAYPOINT] = WAYPOINTS;
+struct point waypoints[NB_WAYPOINT] = WAYPOINTS_UTM;
 
 float ground_alt;
 

sw/lib/ocaml/latlong.ml

@@ -270,7 +270,7 @@ let coeff_proj_mercator_inverse =
     [|0.;0.;0.; 17./.30720.;283./.430080.|];
     [|0.;0.;0.;0.;4397./.41287680.|]|];;
 
-let utm_of' = fun geo ->
+let utm_of' = fun ?zone geo ->
   let ellipsoid =  ellipsoid_of geo in
   let k0 = 0.9996
   and xs = 500000. in
@@ -282,8 +282,13 @@ let utm_of' = fun geo ->
     if not (valid_geo pos) then
       invalid_arg "Latlong.utm_of";
     let lambda_deg = truncate (floor ((Rad>>Deg)lambda)) in
-    let zone = (lambda_deg + 180) / 6 + 1 in
-    let lambda_c = (Deg>>Rad) (float (lambda_deg - ((lambda_deg mod 6)+6)mod 6 + 3)) in
+    let zone, lambda_c =
+      match zone with
+      | None ->
+          (lambda_deg + 180) / 6 + 1,
+          (Deg>>Rad) (float (lambda_deg - ((lambda_deg mod 6)+6)mod 6 + 3))
+      | Some z -> z, (Deg>>Rad) (float ((z - 1)*6 - 180 + 3))
+    in
     let ll = latitude_isometrique phi e
     and dl = lambda -. lambda_c in
     let phi' = asin (sin dl /. cosh ll) in
@@ -309,11 +314,11 @@ let utm_of' = fun geo ->
 
 
 (** Static evaluation for better performance (~50% for cputime) *)
-let utm_of =
-  let u_WGS84 = utm_of' WGS84
-  and u_NTF = utm_of' NTF
-  and u_ED50 = utm_of' ED50
-  and u_NAD27 = utm_of' NAD27 in
+let utm_of = fun ?zone ->
+  let u_WGS84 = utm_of' ?zone WGS84
+  and u_NTF = utm_of' ?zone NTF
+  and u_ED50 = utm_of' ?zone ED50
+  and u_NAD27 = utm_of' ?zone NAD27 in
   fun geo -> match geo with
       WGS84 -> u_WGS84
     | NTF -> u_NTF

sw/lib/ocaml/latlong.mli

@@ -100,10 +100,11 @@ val of_lambert : lambert_zone -> lambert -> geographic
 val lambert_of : lambert_zone -> geographic -> lambert
 (** Conversions between geographic (in NTF or GRS80) and lambert *)
 
-val utm_of' : geodesic -> geographic -> utm
-val utm_of : geodesic -> geographic -> utm
+val utm_of' : ?zone:int -> geodesic -> geographic -> utm
+val utm_of : ?zone:int -> geodesic -> geographic -> utm
 val of_utm : geodesic -> utm -> geographic
-(** Conversions between geographic and UTM. May raise Invalid_argument. *)
+(** Conversions between geographic and UTM. UTM zone can be forced with a specific value (use with caution).
+ *  May raise Invalid_argument. *)
 
 val utm_distance : utm -> utm -> fmeter
 

sw/tools/generators/gen_flight_plan.ml

@@ -122,7 +122,7 @@ let localize_waypoint = fun rel_utm_of_wgs84 waypoint ->
         waypoint
 
 
-let print_waypoint = fun default_alt waypoint ->
+let print_waypoint_utm = fun 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
@@ -827,7 +827,8 @@ let () =
 
     let utm0 = utm_of WGS84 !fp_wgs84 in
     let rel_utm_of_wgs84 = fun wgs84 ->
-      let utm = utm_of WGS84 wgs84 in
+      (* force utm zone to be the same that reference point *)
+      let utm = utm_of ~zone:utm0.utm_zone WGS84 wgs84 in
       (utm.utm_x -. utm0.utm_x, utm.utm_y -. utm0.utm_y) in
     let waypoints =
       List.map (localize_waypoint rel_utm_of_wgs84) waypoints in
@@ -890,8 +891,8 @@ let () =
       List.iter (check_distance (hx, hy) mdfh) waypoints;
       define_waypoints_indices waypoints;
 
-      Xml2h.define "WAYPOINTS" "{ \\";
-      List.iter (print_waypoint alt) waypoints;
+      Xml2h.define "WAYPOINTS_UTM" "{ \\";
+      List.iter (print_waypoint_utm alt) waypoints;
       lprintf "};\n";
       Xml2h.define "WAYPOINTS_ENU" "{ \\";
       List.iter (print_waypoint_enu utm0 alt) waypoints;