Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

taking into account roll neutral

Browse Source
This commit is contained in:
Pascal Brisset
2005-10-18 20:22:16 +00:00
parent 771ae0382e
commit d3ea2cb0f1
5 changed files with 32 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/twinstar3.xml
+1 -1
View File
@@ -35,7 +35,7 @@
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<linear name="RollOfIrs" arity="2" coeff1="-1" coeff2="-1"/>
<linear name="PitchOfIrs" arity="2" coeff1="-1" coeff2="1"/>
<define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
<define name="RAD_OF_IR_MAX_VALUE" value="0.01"/>
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
<define name="ADC_ROLL_NEUTRAL" value="-1026"/>
<define name="ADC_PITCH_NEUTRAL" value="5"/>
sw/simulator/flightModel.ml
+27 -27
View File
@@ -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 *)
sw/simulator/flightModel.mli
+1
View File
@@ -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 *)
sw/simulator/sim.ml
+2 -1
View File
@@ -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
sw/simulator/sitl.ml
+1
View File
@@ -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"