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

some more fg stuffs

Browse Source
This commit is contained in:
Antoine Drouin
2006-03-01 02:36:36 +00:00
parent 32507ad507
commit 6ebb458a0e
5 changed files with 32 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/fg.c
+15 -71
View File
@@ -14,23 +14,26 @@ value fg_sizeof(value unit) {
return Val_int(sizeof(struct FGNetGUI));
}
value fg_msg(value s, value x, value y, value z, value phi) {
#if 0
const double earth_radius = 6372795.;
double lat = 0.656480 + asin(Double_val(x)/earth_radius);
double lon = -2.135537 + asin(Double_val(y)/earth_radius);
value fg_msg_native(value s, value lat, value lon, value z, value phi, value theta, value psi);
value fg_msg_bytecode(value *argv, int argc) {
return fg_msg_native(argv[0], argv[1], argv[2], argv[3], argv[4], argv[5], argv[6]);
}
value fg_msg_native(value s, value lat, value lon, value z, value phi, value theta, value psi) {
struct FGNetGUI msg;
msg.version = FG_NET_GUI_VERSION;
msg.longitude = lon;
msg.latitude = lat;
msg.altitude = Double_val(z);
msg.longitude = Double_val(lon);
msg.latitude = Double_val(lat);
msg.altitude = Double_val(z) + 20;
msg.agl = 0.;
msg.phi = Double_val(phi);
msg.theta = 0.;
msg.psi = 0.;
msg.theta = Double_val(theta);
msg.psi = - Double_val(psi) + M_PI_2;
msg.vcas = 0.;
msg.climb_rate = 0.;
@@ -38,7 +41,7 @@ value fg_msg(value s, value x, value y, value z, value phi) {
msg.num_tanks = 1;
msg.fuel_quantity[0] = 10.;
msg.cur_time = time(NULL);
msg.cur_time = 3213092700ul;//time(NULL);
msg.warp = 0;
msg.ground_elev = 0.;
@@ -47,66 +50,7 @@ value fg_msg(value s, value x, value y, value z, value phi) {
msg.in_range = 1;
msg.course_deviation_deg = 12.;
msg.gs_deviation_deg = 123.;
#endif
static double _x = 0.;
static double _y = 0.;
static double _z = 10;
const double v = 150.;
const double psi = 292.80 * M_PI / 180.;
const double vx = v * cos(psi);
const double vy = v * sin(psi);
const double vz = 1.;
const double dt = 0.01666666;
_x += vx * dt;
_y += vy * dt;
_z += vz * dt;
const double earth_radius = 6372795.;
double lat = 0.656480 + asin(_x/earth_radius);
double lon = -2.135537 + asin(_y/earth_radius);
struct FGNetGUI msg;
// net_gui_init(&gui);
msg.version = FG_NET_GUI_VERSION;
// gui.latitude = 0.656480;
// gui.longitude = -2.135537;
// gui.altitude = 0.807609;
msg.agl = 1.111652;
msg.phi = 0.;
msg.theta = 0.;
msg.psi = 5.20;
msg.vcas = 0.;
msg.climb_rate = 0.;
msg.num_tanks = 1;
msg.fuel_quantity[0] = 0.;
msg.cur_time = 3198060679ul;
msg.warp = 1122474394ul;
msg.ground_elev = 0.;
msg.tuned_freq = 125.65;
msg.nav_radial = 90.;
msg.in_range = 1;
msg.dist_nm = 10.;
msg.course_deviation_deg = 0.;
msg.gs_deviation_deg = 0.;
msg.latitude = lat;
msg.longitude = lon;
msg.altitude = _z;
s = alloc_string(sizeof(msg));
memcpy(String_val(s), (char*)&msg, sizeof(msg));
return Val_unit;
sw/simulator/flightModel.ml
+1
View File
@@ -56,6 +56,7 @@ let init route = {
let get_xyz state = (state.x, state.y, state.z)
let get_time state = state.t
let get_phi state = state.phi
let get_attitude state = (state.phi, 0., state.psi)
let set_air_speed state x = state.air_speed <- x
sw/simulator/flightModel.mli
+1
View File
@@ -35,6 +35,7 @@ val init : radian -> state
val get_xyz : state -> meter * meter * meter
val get_time : state -> float
val get_phi : state -> radian
val get_attitude : state -> radian * radian * radian
val set_air_speed : state -> meter_s -> unit
sw/simulator/sim.ml
+13 -7
View File
@@ -34,7 +34,7 @@ let float_attrib xml a = float_of_string (ExtXml.attrib xml a)
(* Frequencies for perdiodic tasks are expressed in s *)
let ir_period = 1./.20.
let fm_period = 1./.25.
let fg_period = 1./.50.
let fg_period = 1./.25.
module type AIRCRAFT =
@@ -57,7 +57,7 @@ module type AIRCRAFT =
module type AIRCRAFT_ITL = functor (A : Data.MISSION) -> AIRCRAFT
external fg_sizeof : unit -> int = "fg_sizeof"
external fg_msg : string -> float -> float -> float -> float -> unit = "fg_msg"
external fg_msg : string -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native"
let ac_name = ref ""
@@ -178,10 +178,16 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
(** Sending to Flight Gear *)
let fg_task = fun socket buffer () ->
let (x,y,z) = FlightModel.get_xyz !state
and phi = FlightModel.get_phi !state in
fg_msg buffer x y z phi;
(**) for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; (**)
match !last_gps_state with
None -> ()
| Some s ->
let lat = s.Gps.wgs84.Latlong.posn_lat
and lon = s.Gps.wgs84.Latlong.posn_long
and alt = s.Gps.alt
(* and theta_ = s.Gps.course *)
and (phi, theta, psi) = FlightModel.get_attitude !state in
fg_msg buffer lat lon alt phi theta psi;
(** for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
try
ignore (Unix.send socket buffer 0 (String.length buffer) [])
with
@@ -199,7 +205,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
try
let inet_addr = Unix.inet_addr_of_string !fg_client in
let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
Unix.connect socket (Unix.ADDR_INET (inet_addr, 5500));
Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
let buffer = String.create (fg_sizeof ()) in
Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
with
sw/simulator/simsitl.pl
+2
View File
@@ -15,9 +15,11 @@ my $options = {};
GetOptions (
"b=s" => \$options->{ivy_bus},
"a=s" => \$options->{aircraft},
"fg=s" => \$options->{fg},
);
my @args = ();
push @args, "-b", $options->{ivy_bus};
push @args, "-fg", $options->{fg} if exists $options->{fg};
my $sim_binary = Paparazzi::Environment::paparazzi_home()."/var/".$options->{aircraft}."/sim/simsitl";
die "$sim_binary not found. try make AIRCRAFT=$options->{aircraft} ac\n" unless -e $sim_binary;
exec ($sim_binary, @args)