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"