diff --git a/conf/airframes/twinstar3.xml b/conf/airframes/twinstar3.xml
index 6623a4eafc..affdded8e9 100755
--- a/conf/airframes/twinstar3.xml
+++ b/conf/airframes/twinstar3.xml
@@ -35,7 +35,7 @@
-
+
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index 92bd34be87..1d3eda25ed 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -66,33 +66,33 @@ let g = 9.81
module Make(A:Data.MISSION) = struct
open Data
-let section = fun name ->
- try
- ExtXml.child A.ac.airframe ~select:(fun x -> ExtXml.attrib x "name" = name) "section"
- with
- Not_found ->
- failwith (Printf.sprintf "Child 'section' with 'name=%s' expected in '%s'\n" name (Xml.to_string A.ac.airframe))
+ let section = fun name ->
+ try
+ ExtXml.child A.ac.airframe ~select:(fun x -> ExtXml.attrib x "name" = name) "section"
+ with
+ Not_found ->
+ failwith (Printf.sprintf "Child 'section' with 'name=%s' expected in '%s'\n" name (Xml.to_string A.ac.airframe))
-let simu_section = section "SIMU"
+ let simu_section = section "SIMU"
-let defined_value = fun sect name ->
- try
- (Xml.attrib (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value")
- with
- Not_found ->
- failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect))
+ let defined_value = fun sect name ->
+ try
+ (Xml.attrib (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value")
+ with
+ Not_found ->
+ failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect))
-let float_value = fun section s -> float_of_string (defined_value section s)
+ let float_value = fun section s -> float_of_string (defined_value section s)
-let roll_response_factor = float_value simu_section "ROLL_RESPONSE_FACTOR"
+ let roll_response_factor = float_value simu_section "ROLL_RESPONSE_FACTOR"
-let yaw_response_factor = float_value simu_section "YAW_RESPONSE_FACTOR"
+ let yaw_response_factor = float_value simu_section "YAW_RESPONSE_FACTOR"
-let weight = float_value simu_section "WEIGHT"
+ let weight = float_value simu_section "WEIGHT"
-let max_phi = 0.7 (* rad *)
-let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x
+ let max_phi = 0.7 (* rad *)
+ let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x
@@ -129,14 +129,14 @@ let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x
Not_found ->
failwith (Printf.sprintf "Child 'servos' expected in '%s'\n" (Xml.to_string A.ac.airframe))
-let misc_section = section "MISC"
+ let misc_section = section "MISC"
-let infrared_section = section "INFRARED"
+ let infrared_section = section "INFRARED"
+ let nominal_airspeed = float_of_string (defined_value misc_section "NOMINAL_AIRSPEED")
-let nominal_airspeed = float_of_string (defined_value misc_section "NOMINAL_AIRSPEED")
-
-let ir_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
+ let adc_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
+ let roll_neutral_default = rad_of_deg (float_value infrared_section "ROLL_NEUTRAL_DEFAULT")
let get_servo name =
try
@@ -181,9 +181,9 @@ let ir_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
and no_aileron_left = int_attrib aileron_left "no" in
fun state servo ->
let left = - sign_aileron_left * (servo.(no_aileron_left) - n_delta_a) in
- (** if left <> 0 then Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout; **)
- state.delta_a <- c_lda *. float left;
- do_thrust state servo
+ (** if left <> 0 then Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout; **)
+ state.delta_a <- c_lda *. float left;
+ do_thrust state servo
| None, Some ailevon_left, Some ailevon_right ->
let c_lda = 2.5e-4 in (* phi_dot_dot from aileron *)
diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli
index 117c48eeb4..70dd9b3d80 100644
--- a/sw/simulator/flightModel.mli
+++ b/sw/simulator/flightModel.mli
@@ -44,6 +44,7 @@ module Make :
val do_servos : state -> Stdlib.us array -> unit
val nb_servos : int
val nominal_airspeed : float (* m/s *)
+ val roll_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
dt *)
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index f3ef0a2e82..c583c716ab 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -149,7 +149,8 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
horizon_right -. horizon_left
else
0. in
- let ir_left = ( (phi +. delta_ir ) *. !infrared_contrast)
+ let phi = phi +. FM.roll_neutral_default in
+ let ir_left = (phi +. delta_ir ) *. !infrared_contrast
and ir_front = 0. in
Aircraft.infrared ir_left ir_front
with
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 1eccd51a9b..be90d9f192 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -139,6 +139,7 @@ module Make(A:Data.MISSION) = struct
external set_ir_roll : int -> unit = "set_ir_roll"
let infrared = fun ir_left ir_front ->
+ (** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
set_ir_roll (truncate ir_left)
external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos"