[waypoints] start fixing waypoint accuracy issue (#762)

- all transformation are done in double on ground - 7 digits in degrees for Ivy messages and GCS display
paparazzi · Aug 26, 2014 · 582d897 · 582d897
1 parent c95b2a2
commit 582d897
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 15 deletions.
diff --git a/conf/messages.xml b/conf/messages.xml
@@ -2693,9 +2693,9 @@
  <message name="MOVE_WAYPOINT" id="24">
   <field name="ac_id" type="string"/>
   <field name="wp_id" type="uint8"/>
-  <field name="lat"  type="float" unit="deg"></field>
-  <field name="long"  type="float" unit="deg"></field>
-  <field name="alt"    type="float" unit="m"></field>
+  <field name="lat"  type="float" unit="deg" format="%.7f"/>
+  <field name="long"  type="float" unit="deg" format="%.7f"/>
+  <field name="alt"    type="float" unit="m"/>
  </message>
 
  <message name="GET_DL_SETTING" id="25">
@@ -2727,9 +2727,9 @@
  <message name="WAYPOINT_MOVED" id="30">
     <field name="ac_id" type="string"/>
     <field name="wp_id" type="uint8"/>
-    <field name="lat"  type="float" unit="deg"></field>
-    <field name="long"  type="float" unit="deg"></field>
-    <field name="alt" type="float" unit="m"></field>
+    <field name="lat"  type="float" unit="deg" format="%.7f"/>
+    <field name="long"  type="float" unit="deg" format="%.7f"/>
+    <field name="alt" type="float" unit="m"/>
     <field name="ground_alt" type="float" unit="m"/>
  </message>
 

diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
@@ -632,7 +632,7 @@ let ivy_server = fun http ->
   ignore (Ground_Pprz.message_answerer my_id "CONFIG" (send_config http))
 
 
-let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f))
+let cm_of_m = fun f -> Pprz.Int (truncate ((100. *. f) +. 0.5)) (* Convert to cm, with rounding *)
 let dl_id = "ground_dl" (* Hack, should be [my_id] *)
 
 (** Got a ground.MOVE_WAYPOINT and send a datalink.MOVE_WP *)

diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml
@@ -67,7 +67,7 @@ let make_geo_deg = fun lat long ->
   { posn_long = norm_angle ((Deg>>Rad)long); posn_lat = ((Deg>>Rad)lat) }
 
 
-let deg_string_of_rad = fun r -> Printf.sprintf "%.6f" ((Rad>>Deg)r)
+let deg_string_of_rad = fun r -> Printf.sprintf "%.7f" ((Rad>>Deg)r)
 
 let decimal d m s = float d +. float m /. 60. +. s /. 3600.;;
 let dms = fun x ->
@@ -78,7 +78,7 @@ let dms = fun x ->
 
 
 let sprint_degree_of_radian x =
-  Printf.sprintf "%.6f" ((Rad>>Deg) x)
+  Printf.sprintf "%.7f" ((Rad>>Deg) x)
 
 let string_degrees_of_geographic sm =
   Printf.sprintf "%s\t%s"
@@ -584,9 +584,10 @@ let ecef_of_geo = fun geo ->
     and cos_long = cos long in
 
     let chi = sqrt (1. -. e2*.sin_lat*.sin_lat) in
-    let x = (elps.a/.chi +.h)*.cos_lat*.cos_long
-    and y = (elps.a/.chi +.h)*.cos_lat*.sin long
-    and z = (elps.a*.(1.-.e2)/.chi +. h)*.sin_lat in
+    let a_chi = elps.a /. chi in
+    let x = (a_chi +.h)*.cos_lat*.cos_long
+    and y = (a_chi +.h)*.cos_lat*.sin long
+    and z = (a_chi*.(1.-.e2) +. h)*.sin_lat in
     [|x; y; z|]
 
 let geo_of_ecef = fun geo ->

diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
@@ -254,7 +254,7 @@ object (self)
   method moved = moved <> None
   method reset_moved () =
     match moved with
-        None -> ()
+      | None -> ()
       | Some x ->
         Glib.Timeout.remove x;
         item#affine_absolute rotation_45;
@@ -277,11 +277,11 @@ object (self)
 
     let new_pos = ecef_distance current_ecef new_ecef > 2. in
     match moved, new_pos with
-        None, true ->
+      | None, _ ->
           self#move dx dy;
           alt <- alt+.dz;
           if update then updated ()
-      | (None, false) | (Some _, true) -> ()
+      | Some _, true -> ()
       | Some _, false -> self#reset_moved ()
   method set_ground_alt ga = ground_alt <- ga
   method delete () =