[paparazzi-commits] [4383] handle vertical wind

[Pascal Brisset]

Revision: 4383
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4383
Author:   hecto
Date:     2009-12-22 16:13:39 +0000 (Tue, 22 Dec 2009)
Log Message:
-----------
 handle vertical wind

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

Modified: paparazzi3/trunk/sw/simulator/flightModel.ml
===================================================================
--- paparazzi3/trunk/sw/simulator/flightModel.ml        2009-12-22 16:12:56 UTC 
(rev 4382)
+++ paparazzi3/trunk/sw/simulator/flightModel.ml        2009-12-22 16:13:39 UTC 
(rev 4383)
@@ -61,8 +61,8 @@
     val max_bat_level : float (* V *)
     val roll_neutral_default : float (* rad *)
     val pitch_neutral_default : float (* rad *)
-    val state_update : state -> float -> float * float -> float -> float -> 
unit
-       (** [state_update nom_airspeed state (wind_x, wind_y) on_ground dt] 
With m/s for wind and s for
+    val state_update : state -> float -> float * float * float -> float -> 
float -> unit
+       (** [state_update nom_airspeed state (wind_x, wind_y, wind_z) on_ground 
dt] With m/s for wind and s for
            dt *)
   end
 
@@ -179,7 +179,7 @@
    http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf
    http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf
  *)
-  let state_update = fun state nominal_airspeed (wx, wy) agl dt ->
+  let state_update = fun state nominal_airspeed (wx, wy, wz) agl dt ->
     let now = state.t +. dt in
     if state.air_speed = 0. && state.thrust > 0. then
       state.air_speed <- nominal_airspeed;
@@ -227,7 +227,8 @@
       let x_dot = state.air_speed *. cos state.psi +. wx
       and y_dot = state.air_speed *. sin state.psi +. wy in
       state.x <- state.x +. x_dot *. dt;
-      state.y <- state.y +. y_dot *. dt
+      state.y <- state.y +. y_dot *. dt;
+      state.z <- state.z +. wz *. dt
     end;
     state.t <- now
 end (* Make functor *)