Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

new ir roll and pitch calculation

Browse Source
This commit is contained in:
Pascal Brisset
2006-10-20 15:37:55 +00:00
parent 605418e58d
commit 000c804820
1 changed files with 8 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/sim.ml
+8 -21
View File
@@ -123,7 +123,6 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
and east_label = GMisc.label ~text:"000" ()
and alt_label = GMisc.label ~text:"000" () in
let half_aperture = (Latlong.pi /. 4.) in
let last_gps_state = ref None in
let _run = ref false in
let ir_srtm = ref false in
@@ -151,26 +150,14 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
and ir_task = fun () ->
let phi, theta, _ = FlightModel.get_attitude !state in
let horizon_distance = 1000. in
try
match !last_gps_state with
None -> ()
| Some gps_state ->
let delta_ir =
if !ir_srtm then
let altitude = (int_of_float gps_state.Gps.alt) in
let horizon_right = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course +. Latlong.pi /. 2.) half_aperture horizon_distance in
let horizon_left = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course -. Latlong.pi /. 2.) half_aperture horizon_distance in
horizon_right -. horizon_left
else
0. in
let phi = phi +. FM.roll_neutral_default in
let ir_left = (phi +. delta_ir ) *. !infrared_contrast
and ir_front = (FM.pitch_neutral_default +. theta) *. !infrared_contrast
and ir_top = pi /. 2. *. !infrared_contrast in
Aircraft.infrared ir_left ir_front ir_top
with
x -> Printf.printf "%s\n%!" (Printexc.to_string x)
let phi_sensor = phi +. FM.roll_neutral_default
and theta_sensor = theta +. FM.pitch_neutral_default in
let ir_left = sin phi_sensor *. !infrared_contrast
and ir_front = sin theta_sensor *. !infrared_contrast
and ir_top = cos phi_sensor *. cos theta_sensor *. !infrared_contrast in
Aircraft.infrared ir_left ir_front ir_top
and gps_task = fun () ->
let (x,y,z) = FlightModel.get_xyz !state in