diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml index 50f5264a12..14a326b028 100644 --- a/sw/simulator/flightModel.ml +++ b/sw/simulator/flightModel.ml @@ -156,8 +156,10 @@ module Make(A:Data.MISSION) = struct 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 (wx, wy) dt -> + let state_update = fun state nominal_airspeed (wx, wy) dt -> let now = state.t +. dt in + if state.air_speed = 0. && state.thrust > 0. then + state.nominal_air_speed <- nominal_airspeed; state.air_speed <- state.nominal_air_speed*.(1.-.sin state.theta); if state.air_speed > 0. then begin let phi_dot_dot = roll_response_factor *. state.delta_a -. state.phi_dot in diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli index 3da310ac5e..aee0a751e2 100644 --- a/sw/simulator/flightModel.mli +++ b/sw/simulator/flightModel.mli @@ -45,7 +45,7 @@ module Make : val nominal_airspeed : float (* m/s *) val roll_neutral_default : float (* rad *) val pitch_neutral_default : float (* rad *) - val state_update : state -> float * float -> float -> unit - (** [state_update state (wind_x, wind_y) dt] With m/s for wind and s for + val state_update : state -> float -> float * float -> float -> unit + (** [state_update nom_airspeed state (wind_x, wind_y) dt] With m/s for wind and s for dt *) end diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml index db38e85ad9..c7aaa0bee0 100644 --- a/sw/simulator/sim.ml +++ b/sw/simulator/sim.ml @@ -154,7 +154,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct let fm_task = fun () -> FM.do_commands !state commands; - FM.state_update !state (!wind_x, !wind_y) fm_period + FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y) fm_period and ir_task = fun () -> let phi, theta, _ = FlightModel.get_attitude !state in