[paparazzi-commits] [4382] fix bug when dt=0

[Pascal Brisset]

Revision: 4382
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4382
Author:   hecto
Date:     2009-12-22 16:12:56 +0000 (Tue, 22 Dec 2009)
Log Message:
-----------
 fix  bug when dt=0

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/gps.ml

Modified: paparazzi3/trunk/sw/simulator/gps.ml
===================================================================
--- paparazzi3/trunk/sw/simulator/gps.ml        2009-12-22 16:12:17 UTC (rev 
4381)
+++ paparazzi3/trunk/sw/simulator/gps.ml        2009-12-22 16:12:56 UTC (rev 
4382)
@@ -43,18 +43,22 @@
   fun c -> c +. ng ()
 
 let state = fun pos0 alt0 ->
-  let last_x = ref 0. and last_y = ref 0. 
-  and last_t = ref 0. and last_z = ref 0. in
-  let tow = float (Latlong.get_gps_tow ()) in
+  let last_x = ref 0. and last_y = ref 0. and last_z = ref 0.
+  and last_gspeed = ref 0. and last_course = ref 0. and last_climb = ref 0.
+  and last_t = ref 0.
+  and tow = float (Latlong.get_gps_tow ()) in
 
   fun (x, y, z) t ->
-   let dx = x -. !last_x
-    and dy = y -. !last_y
-    and dt = t -. !last_t in
-    let gspeed = sqrt (dx*.dx +. dy*.dy) /. dt
-    and course = norm_angle (pi/.2. -. atan2 dy dx)
-    and climb = (z -. !last_z) /. dt in
+    let dt = t -. !last_t in
 
+    if dt > 0. then begin (** Compute derivatives *)
+      let dx = x -. !last_x
+      and dy = y -. !last_y in
+      last_gspeed := sqrt (dx*.dx +. dy*.dy) /. dt;
+      last_course := norm_angle (pi/.2. -. atan2 dy dx);
+      last_climb := (z -. !last_z) /. dt
+    end; (** Else use previous derivatives *)
+    
     let utm0 = utm_of WGS84 !pos0 in
     let utm = utm_add utm0 (x, y) in
     let wgs84 = of_utm WGS84 utm
@@ -65,14 +69,14 @@
     last_z := z;
     last_t := t;
 
-    let course = if course < 0. then course +. 2. *. pi else course in
+    let course = if !last_course < 0. then !last_course +. 2. *. pi else 
!last_course in
     
     {
      wgs84 = wgs84;
      alt = alt;
      time = t +. tow;
-     climb = climb_noise climb;
-     gspeed = gspeed;
+     climb = climb_noise !last_climb;
+     gspeed = !last_gspeed;
      course = course;
      availability = true;
    }