From: ehofman Date: Tue, 19 Oct 2004 11:10:20 +0000 (+0000) Subject: A first stab at an ACMS (Aircraft Condition Monitoring System) Special Purpose suppor... X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=8b53b86aa0f9c5a25ff5490689417ccce42e406e;p=flightgear.git A first stab at an ACMS (Aircraft Condition Monitoring System) Special Purpose support FDM. Move the ADA FDM into the Special Purpose directory and make the SP FDM's a configure option. --- diff --git a/configure.ac b/configure.ac index 28281184b..a2c48a0fb 100644 --- a/configure.ac +++ b/configure.ac @@ -68,6 +68,10 @@ else fi AM_CONDITIONAL(ENABLE_MPLAYER_AS, test "x$with_multiplayer" != "xno") +AC_ARG_ENABLE(sp_fdms, [ --enable-sp-fdms Include special purpose Flight Models], [enable_sp_fdms="$enableval"] ) +AC_DEFINE([ENABLE_SP_FMDS], test "x$enable_sp_fdms" = "xyes", [Define to include special purpose FDMs]) +AM_CONDITIONAL(ENABLE_SP_FDM, test "x$enable_sp_fdms" != "xno") + dnl Thread related checks # defaults to yes @@ -219,7 +223,7 @@ AC_SEARCH_LIBS(dlclose, dl) base_LIBS="$LIBS" dnl Check for SDL if enabled. -AC_ARG_ENABLE(sdl, [ --enable-sdl Configure to use SDL instead of GLUT], enable_sdl="yes", enable_sdl="") +AC_ARG_ENABLE(sdl, [ --enable-sdl Configure to use SDL instead of GLUT], [enable_sdl="$enableval"]) AM_CONDITIONAL(USE_SDL, test "x$enable_sdl" = "xyes") if test "x$enable_sdl" = "xyes"; then AC_DEFINE([PU_USE_SDL], 1, [Define to use SDL]) @@ -545,6 +549,7 @@ AC_CONFIG_FILES([ \ src/FDM/JSBSim/Makefile \ src/FDM/JSBSim/filtersjb/Makefile \ src/FDM/LaRCsim/Makefile \ + src/FDM/SP/Makefile \ src/FDM/UIUCModel/Makefile \ src/FDM/YASim/Makefile \ src/FDM/Makefile \ @@ -601,3 +606,10 @@ if test "x$with_threads" = "xyes"; then else echo "threads: no" fi + +if test "x$enable_sp_fdms" != "xno"; then + echo "Include special purpose flight models: yes" +else + echo "Include special purpose flight models: no" +fi + diff --git a/src/Cockpit/cockpit.cxx b/src/Cockpit/cockpit.cxx index 70366cf40..75e765ecc 100644 --- a/src/Cockpit/cockpit.cxx +++ b/src/Cockpit/cockpit.cxx @@ -43,7 +43,9 @@ #include #include -#include +#ifdef ENABLE_SP_FMDS +#include +#endif #include
#include
#include
@@ -337,6 +339,7 @@ float get_anzg ( void ) return anzg; } +#ifdef ENABLE_SP_FMDS int get_iaux1 (void) { FGADA *fdm = (FGADA *)current_aircraft.fdm_state; @@ -516,7 +519,7 @@ float get_aux18 (void) FGADA *fdm = (FGADA *)current_aircraft.fdm_state; return fdm->get_faux(10); } -// #endif +#endif // $$$ end - added, VS Renganathan 13 Oct 2K diff --git a/src/FDM/ADA.cxx b/src/FDM/ADA.cxx deleted file mode 100644 index 11728b7bf..000000000 --- a/src/FDM/ADA.cxx +++ /dev/null @@ -1,331 +0,0 @@ -// ADA.cxx -- interface to the "External"-ly driven ADA flight model -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License as -// published by the Free Software Foundation; either version 2 of the -// License, or (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, but -// WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -// -// $Id$ - -// Modified by Cdr. VS Renganthan , 12 Oct 2K - -#ifdef HAVE_CONFIG_H -# include -#endif - -#include -#include -#include - -#include -#include
- -#include
//to get ID of window (left/right or center) -#include //to pass ground elevation to FDM - -#include "ADA.hxx" - -#define numberofbytes 472 // from FDM to visuals -#define nbytes 8 //from visuals to FDM - -struct { - double number_of_bytes; - double lat_geoc; - double lon_geoc; - double altitude; - double psirad; - double thetrad; - double phirad; - double earth_posn_angle; - double radius_to_vehicle; - double sea_level_radius; - double latitude; - double longitude; - double Vnorth; - double Veast; - double Vdown; - double Vcas_kts; - double prad; - double qrad; - double rrad; - double alpharad; - double betarad; - double latitude_dot; - double longitude_dot; - double radius_dot; - double Gamma_vert_rad; - double Runway_altitude; - double throttle; - double pstick; - double rstick; - double rpedal; - double U_local; - double V_local; - double W_local; - double U_dot_local; - double V_dot_local; - double W_dot_local; - double Machno; - double anxg; - double anyg; - double anzg; - double aux1; - double aux2; - double aux3; - double aux4; - double aux5; - double aux6; - double aux7; - double aux8; - int iaux1; - int iaux2; - int iaux3; - int iaux4; - int iaux5; - int iaux6; - int iaux7; - int iaux8; - int iaux9; - int iaux10; - int iaux11; - int iaux12; - float aux9; - float aux10; - float aux11; - float aux12; - float aux13; - float aux14; - float aux15; - float aux16; - float aux17; - float aux18; -} sixdof_to_visuals; - -double view_offset; //if this zero, means center window - -struct { - double ground_elevation; -} visuals_to_sixdof; - -#define ground_elevation visuals_to_sixdof.ground_elevation - -#define number_of_bytes sixdof_to_visuals.number_of_bytes -#define U_dot_local sixdof_to_visuals.U_dot_local -#define V_dot_local sixdof_to_visuals.V_dot_local -#define W_dot_local sixdof_to_visuals.W_dot_local -#define U_local sixdof_to_visuals.U_local -#define V_local sixdof_to_visuals.V_local -#define W_local sixdof_to_visuals.W_local -#define throttle sixdof_to_visuals.throttle -#define pstick sixdof_to_visuals.pstick -#define rstick sixdof_to_visuals.rstick -#define rpedal sixdof_to_visuals.rpedal -#define V_north sixdof_to_visuals.Vnorth -#define V_east sixdof_to_visuals.Veast -#define V_down sixdof_to_visuals.Vdown -#define V_calibrated_kts sixdof_to_visuals.Vcas_kts -#define P_body sixdof_to_visuals.prad -#define Q_body sixdof_to_visuals.qrad -#define R_body sixdof_to_visuals.rrad -#define Latitude_dot sixdof_to_visuals.latitude_dot -#define Longitude_dot sixdof_to_visuals.longitude_dot -#define Radius_dot sixdof_to_visuals.radius_dot -#define Latitude sixdof_to_visuals.latitude -#define Longitude sixdof_to_visuals.longitude -#define Lat_geocentric sixdof_to_visuals.lat_geoc -#define Lon_geocentric sixdof_to_visuals.lon_geoc -#define Radius_to_vehicle sixdof_to_visuals.radius_to_vehicle -#define Altitude sixdof_to_visuals.altitude -#define Phi sixdof_to_visuals.phirad -#define Theta sixdof_to_visuals.thetrad -#define Psi sixdof_to_visuals.psirad -#define Alpha sixdof_to_visuals.alpharad -#define Beta sixdof_to_visuals.betarad -#define Sea_level_radius sixdof_to_visuals.sea_level_radius -#define Earth_position_angle sixdof_to_visuals.earth_posn_angle -#define Runway_altitude sixdof_to_visuals.Runway_altitude -#define Gamma_vert_rad sixdof_to_visuals.Gamma_vert_rad -#define Machno sixdof_to_visuals.Machno -#define anxg sixdof_to_visuals.anxg -#define anyg sixdof_to_visuals.anyg -#define anzg sixdof_to_visuals.anzg - - -FGADA::FGADA( double dt ) { -// set_delta_t( dt ); -} - - -FGADA::~FGADA() { -} - - -// Initialize the ADA flight model, dt is the time increment -// for each subsequent iteration through the EOM -void FGADA::init() { - - //do init common to all FDM"s - common_init(); - - //now do ADA-specific init. - - // cout << "FGADA::init()" << endl; - - char OutBuffer[nbytes]; - copy_to_FGADA(); - - printf("\nInitialising UDP sockets\n"); - // initialise a "udp" socket - fdmsock = new SGSocket( "fdm_pc", "5001", "udp" ); - - // open as a client - bool result = fdmsock->open(SG_IO_OUT); - if (result == false) { - printf ("Socket Open Error\n"); - } else { - copy_to_FGADA(); - // Write FGExternal structure from socket to establish connection - int result = fdmsock->write(OutBuffer, nbytes); - printf("Connection established = %d.\n", result); - } -} - - -// Run an iteration of the EOM. This is essentially a NOP here -// because these values are getting filled in elsewhere based on -// external input. -void FGADA::update( double dt ) { - // cout << "FGADA::update()" << endl; - - if (is_suspended()) - return; - - char Buffer[numberofbytes]; - char OutBuffer[nbytes]; - - // Read FGExternal structure from socket - while (1) { - int result = fdmsock->read(Buffer, numberofbytes); - if (result == numberofbytes) { - // Copy buffer into FGExternal structure - memcpy (&sixdof_to_visuals, &Buffer, sizeof (Buffer)); - // Convert from the FGExternal struct to the FGInterface struct (input) - copy_from_FGADA(); - } else { - break; - } - } - - copy_to_FGADA(); - fgGetDouble("/sim/view/offset",view_offset); - if ( view_offset == 0.0) { - memcpy (&OutBuffer, &visuals_to_sixdof, sizeof (OutBuffer)); - fdmsock->write(OutBuffer, nbytes); - } -} - -// Convert from the FGInterface struct to the FGADA struct (output) -bool FGADA::copy_to_FGADA () { - ground_elevation = globals->get_scenery()->get_cur_elev(); - return true; -} - - -// Convert from the FGADA struct to the FGInterface struct (input) -bool FGADA::copy_from_FGADA() { - - //Positions and attitudes for The Rendering engine - _set_Geodetic_Position( Latitude, Longitude, Altitude ); - _set_Euler_Angles( Phi, Theta, Psi ); - _set_Geocentric_Position( Lat_geocentric, Lon_geocentric, - Radius_to_vehicle ); - _set_Sea_level_radius( Sea_level_radius ); - - _set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot ); - _set_Earth_position_angle( Earth_position_angle ); - _set_sin_lat_geocentric(Lat_geocentric); - _set_cos_lat_geocentric(Lat_geocentric); - _set_sin_cos_longitude(Longitude); - _set_sin_cos_latitude(Latitude); - - // Velocities and accelerations for the pitch ladder and velocity vector - _set_Accels_Local( U_dot_local, V_dot_local, W_dot_local ); - _set_Velocities_Ground( U_local, V_local, W_local );//same as V_NED in mps - _set_Velocities_Local( V_north, V_east, V_down ); //same as UVW_local in fps - - //Positions and attitude for ship - _set_daux(1,sixdof_to_visuals.aux1);//ship lat - _set_daux(2,sixdof_to_visuals.aux2);//ship lon - _set_daux(3,sixdof_to_visuals.aux3);//ship alt+heave - _set_daux(4,sixdof_to_visuals.aux4);//distance of a/c from ski-jump exit - _set_faux(1,sixdof_to_visuals.aux9);//ship pitch - _set_faux(2,sixdof_to_visuals.aux10);//ship roll - _set_faux(3,sixdof_to_visuals.aux11);//ship yaw - _set_iaux(1,sixdof_to_visuals.iaux1);//flag for drawing ship - - // controls - globals->get_controls()->set_throttle(0,throttle/131.0); - globals->get_controls()->set_elevator(pstick); - globals->get_controls()->set_aileron(rstick); - globals->get_controls()->set_rudder(rpedal); - - // auxilliary parameters for HUD - _set_V_calibrated_kts( V_calibrated_kts ); - _set_Alpha( Alpha ); - _set_Beta( Beta ); - _set_Accels_CG_Body_N( anxg,anyg,anzg); - _set_Mach_number( Machno); - _set_Climb_Rate( W_local*SG_METER_TO_FEET ); //pressure alt in feet for lca(navy) - - _set_iaux(2,sixdof_to_visuals.iaux2);//control law mode switch posn - _set_iaux(3,sixdof_to_visuals.iaux3);//ldg gear posn - _set_iaux(4,sixdof_to_visuals.iaux4);// wow nose status - _set_iaux(5,sixdof_to_visuals.iaux5);// wow main status - _set_iaux(6,sixdof_to_visuals.iaux6);// arrester hook posn - _set_iaux(7,sixdof_to_visuals.iaux7); - _set_iaux(8,sixdof_to_visuals.iaux8); - _set_iaux(9,sixdof_to_visuals.iaux9); - _set_iaux(10,sixdof_to_visuals.iaux10); - _set_iaux(11,sixdof_to_visuals.iaux11); - _set_iaux(12,sixdof_to_visuals.iaux12); - - _set_daux(5,sixdof_to_visuals.aux5); - _set_daux(6,sixdof_to_visuals.aux6); - _set_daux(7,sixdof_to_visuals.aux7); - _set_daux(8,sixdof_to_visuals.aux8); - - _set_faux(4,sixdof_to_visuals.aux12); - _set_faux(5,sixdof_to_visuals.aux13); - _set_faux(6,sixdof_to_visuals.aux14); - _set_faux(7,sixdof_to_visuals.aux15); - _set_faux(8,sixdof_to_visuals.aux16); - _set_faux(9,sixdof_to_visuals.aux17); - _set_faux(10,sixdof_to_visuals.aux18); - - // Angular rates - _set_Omega_Body( P_body, Q_body, R_body ); - - // Miscellaneous quantities - _set_Gamma_vert_rad( Gamma_vert_rad ); - _set_Runway_altitude( Runway_altitude ); - - // SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << Longitude - // << " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude - // << " alt = " << Altitude << " sl_radius = " << Sea_level_radius - // << " radius_to_vehicle = " << Radius_to_vehicle ); - - - // printf("sr=%f\n",Sea_level_radius); - // printf("psi = %f %f\n",Psi,Psi*SGD_RADIANS_TO_DEGREES); - - return true; -} diff --git a/src/FDM/ADA.hxx b/src/FDM/ADA.hxx deleted file mode 100644 index b3830c72a..000000000 --- a/src/FDM/ADA.hxx +++ /dev/null @@ -1,87 +0,0 @@ -// ADA.hxx -- interface to the "External"-ly driven ADA flight model -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License as -// published by the Free Software Foundation; either version 2 of the -// License, or (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, but -// WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -// -// $Id$ - - -#ifndef _ADA_HXX -#define _ADA_HXX - - -class SGSocket; - -#include - - -class FGADA: public FGInterface { - -private: - - SGSocket *fdmsock; - - // Auxilliary Flight Model parameters, basically for HUD - double aux1; // auxilliary flag - double aux2; // auxilliary flag - double aux3; // auxilliary flag - double aux4; // auxilliary flag - double aux5; // auxilliary flag - double aux6; // auxilliary flag - double aux7; // auxilliary flag - double aux8; // auxilliary flag - float aux9; // auxilliary flag - float aux10; // auxilliary flag - float aux11; // auxilliary flag - float aux12; // auxilliary flag - float aux13; // auxilliary flag - float aux14; // auxilliary flag - float aux15; // auxilliary flag - float aux16; // auxilliary flag - float aux17; // auxilliary flag - float aux18; // auxilliary flag - int iaux1; // auxilliary flag - int iaux2; // auxilliary flag - int iaux3; // auxilliary flag - int iaux4; // auxilliary flag - int iaux5; // auxilliary flag - int iaux6; // auxilliary flag - int iaux7; // auxilliary flag - int iaux8; // auxilliary flag - int iaux9; // auxilliary flag - int iaux10; // auxilliary flag - int iaux11; // auxilliary flag - int iaux12; // auxilliary flag - - // copy FDM state to FGADA structures - bool copy_to_FGADA(); - - // copy FDM state from FGADA structures - bool copy_from_FGADA(); - -public: - - FGADA( double dt ); - ~FGADA(); - - // reset flight params to a specific position - void init(); - - // update position based on inputs, positions, velocities, etc. - void update(double dt); - -}; - - -#endif // _ADA_HXX diff --git a/src/FDM/Makefile.am b/src/FDM/Makefile.am index 6d87e9372..e27dfc431 100644 --- a/src/FDM/Makefile.am +++ b/src/FDM/Makefile.am @@ -1,9 +1,15 @@ -SUBDIRS = Balloon ExternalNet ExternalPipe JSBSim LaRCsim UIUCModel YASim +if ENABLE_SP_FDM +SP_DIR = SP +else +SP_DIR = +endif + +SUBDIRS = Balloon JSBSim LaRCsim UIUCModel YASim \ + $(SP_DIR) ExternalNet ExternalPipe noinst_LIBRARIES = libFlight.a libFlight_a_SOURCES = \ - ADA.cxx ADA.hxx \ Balloon.cxx Balloon.h \ flight.cxx flight.hxx \ MagicCarpet.cxx MagicCarpet.hxx \ diff --git a/src/FDM/SP/ACMS.cxx b/src/FDM/SP/ACMS.cxx new file mode 100644 index 000000000..44e6c99e6 --- /dev/null +++ b/src/FDM/SP/ACMS.cxx @@ -0,0 +1,95 @@ +// ACMS.cxx -- interface to the ACMS FDM +// +// Written by Erik Hofman, started October 2004 +// +// Copyright (C) 2004 Erik Hofman +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// + +#include +#include
+ +#include "ACMS.hxx" + +FGACMS::FGACMS( double dt ) +{ +// set_delta_t( dt ); +} + + +FGACMS::~FGACMS() { +} + + +// Initialize the ACMSFDM flight model, dt is the time increment +// for each subsequent iteration through the EOM +void FGACMS::init() { + common_init(); +} + + +// Run an iteration of the EOM (equations of motion) +void FGACMS::update( double dt ) { + + double pitch = get_Theta(); + double roll = get_Phi(); + double heading = get_Psi() * SG_DEGREES_TO_RADIANS; + double alt = get_Altitude(); + + double sl_radius, lat_geoc; + sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc ); + + double lon_acc = get_V_north(); + double lat_acc = get_V_east(); + double vert_acc = get_V_down(); + + double accel_heading = atan( lon_acc/lat_acc ); + double accel_pitch = atan( vert_acc/accel_heading ); + + double accel = sqrt(sqrt(lon_acc*lon_acc + lat_acc*lat_acc) + + vert_acc*vert_acc); + + double velocity = get_V_true_kts() * accel / (SG_METER_TO_NM * 3600.0); + double speed = cos (pitch) * velocity; // meters/sec + double dist = speed * dt; + double kts = velocity * 6076.11549 * 3600.0; + + double climb_rate = fgGetDouble("/velocities/climb-rate", 0); + double climb = climb_rate * dt; + + _set_Alpha( pitch - accel_pitch); + _set_Beta( heading - accel_heading); + _set_Climb_Rate( climb_rate ); + _set_V_equiv_kts( kts ); + _set_V_calibrated_kts( kts ); + _set_V_ground_speed( kts ); + _set_Altitude( get_Altitude() + climb ); + + // update (lon/lat) position + double lat2, lon2, az2; + geo_direct_wgs_84 ( get_Altitude(), + get_Latitude() * SGD_RADIANS_TO_DEGREES, + get_Longitude() * SGD_RADIANS_TO_DEGREES, + get_Psi() * SGD_RADIANS_TO_DEGREES, + dist, &lat2, &lon2, &az2 ); + _set_Geocentric_Position( lat_geoc, get_Longitude(), + sl_radius + get_Altitude() + climb ); + _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET); + + _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS ); + _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS ); + +} diff --git a/src/FDM/SP/ACMS.hxx b/src/FDM/SP/ACMS.hxx new file mode 100644 index 000000000..bdfb769f7 --- /dev/null +++ b/src/FDM/SP/ACMS.hxx @@ -0,0 +1,46 @@ +// ACMS.hxx -- interface to the AIAircraft FDM +// +// Written by Erik Hofman, started October 2004 +// +// Copyright (C) 2004 Erik Hofman +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// + + +#ifndef _ACMS_HXX +#define _ACMS_HXX + +#include + +#include + + +class FGACMS: public FGInterface +{ +public: + FGACMS( double dt ); + ~FGACMS(); + + // reset flight params to a specific position + void init(); + + // update position based on properties + void update( double dt ); + +}; + + +#endif // _ACMS_HXX diff --git a/src/FDM/SP/ADA.cxx b/src/FDM/SP/ADA.cxx new file mode 100644 index 000000000..11728b7bf --- /dev/null +++ b/src/FDM/SP/ADA.cxx @@ -0,0 +1,331 @@ +// ADA.cxx -- interface to the "External"-ly driven ADA flight model +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + +// Modified by Cdr. VS Renganthan , 12 Oct 2K + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include +#include +#include + +#include +#include
+ +#include
//to get ID of window (left/right or center) +#include //to pass ground elevation to FDM + +#include "ADA.hxx" + +#define numberofbytes 472 // from FDM to visuals +#define nbytes 8 //from visuals to FDM + +struct { + double number_of_bytes; + double lat_geoc; + double lon_geoc; + double altitude; + double psirad; + double thetrad; + double phirad; + double earth_posn_angle; + double radius_to_vehicle; + double sea_level_radius; + double latitude; + double longitude; + double Vnorth; + double Veast; + double Vdown; + double Vcas_kts; + double prad; + double qrad; + double rrad; + double alpharad; + double betarad; + double latitude_dot; + double longitude_dot; + double radius_dot; + double Gamma_vert_rad; + double Runway_altitude; + double throttle; + double pstick; + double rstick; + double rpedal; + double U_local; + double V_local; + double W_local; + double U_dot_local; + double V_dot_local; + double W_dot_local; + double Machno; + double anxg; + double anyg; + double anzg; + double aux1; + double aux2; + double aux3; + double aux4; + double aux5; + double aux6; + double aux7; + double aux8; + int iaux1; + int iaux2; + int iaux3; + int iaux4; + int iaux5; + int iaux6; + int iaux7; + int iaux8; + int iaux9; + int iaux10; + int iaux11; + int iaux12; + float aux9; + float aux10; + float aux11; + float aux12; + float aux13; + float aux14; + float aux15; + float aux16; + float aux17; + float aux18; +} sixdof_to_visuals; + +double view_offset; //if this zero, means center window + +struct { + double ground_elevation; +} visuals_to_sixdof; + +#define ground_elevation visuals_to_sixdof.ground_elevation + +#define number_of_bytes sixdof_to_visuals.number_of_bytes +#define U_dot_local sixdof_to_visuals.U_dot_local +#define V_dot_local sixdof_to_visuals.V_dot_local +#define W_dot_local sixdof_to_visuals.W_dot_local +#define U_local sixdof_to_visuals.U_local +#define V_local sixdof_to_visuals.V_local +#define W_local sixdof_to_visuals.W_local +#define throttle sixdof_to_visuals.throttle +#define pstick sixdof_to_visuals.pstick +#define rstick sixdof_to_visuals.rstick +#define rpedal sixdof_to_visuals.rpedal +#define V_north sixdof_to_visuals.Vnorth +#define V_east sixdof_to_visuals.Veast +#define V_down sixdof_to_visuals.Vdown +#define V_calibrated_kts sixdof_to_visuals.Vcas_kts +#define P_body sixdof_to_visuals.prad +#define Q_body sixdof_to_visuals.qrad +#define R_body sixdof_to_visuals.rrad +#define Latitude_dot sixdof_to_visuals.latitude_dot +#define Longitude_dot sixdof_to_visuals.longitude_dot +#define Radius_dot sixdof_to_visuals.radius_dot +#define Latitude sixdof_to_visuals.latitude +#define Longitude sixdof_to_visuals.longitude +#define Lat_geocentric sixdof_to_visuals.lat_geoc +#define Lon_geocentric sixdof_to_visuals.lon_geoc +#define Radius_to_vehicle sixdof_to_visuals.radius_to_vehicle +#define Altitude sixdof_to_visuals.altitude +#define Phi sixdof_to_visuals.phirad +#define Theta sixdof_to_visuals.thetrad +#define Psi sixdof_to_visuals.psirad +#define Alpha sixdof_to_visuals.alpharad +#define Beta sixdof_to_visuals.betarad +#define Sea_level_radius sixdof_to_visuals.sea_level_radius +#define Earth_position_angle sixdof_to_visuals.earth_posn_angle +#define Runway_altitude sixdof_to_visuals.Runway_altitude +#define Gamma_vert_rad sixdof_to_visuals.Gamma_vert_rad +#define Machno sixdof_to_visuals.Machno +#define anxg sixdof_to_visuals.anxg +#define anyg sixdof_to_visuals.anyg +#define anzg sixdof_to_visuals.anzg + + +FGADA::FGADA( double dt ) { +// set_delta_t( dt ); +} + + +FGADA::~FGADA() { +} + + +// Initialize the ADA flight model, dt is the time increment +// for each subsequent iteration through the EOM +void FGADA::init() { + + //do init common to all FDM"s + common_init(); + + //now do ADA-specific init. + + // cout << "FGADA::init()" << endl; + + char OutBuffer[nbytes]; + copy_to_FGADA(); + + printf("\nInitialising UDP sockets\n"); + // initialise a "udp" socket + fdmsock = new SGSocket( "fdm_pc", "5001", "udp" ); + + // open as a client + bool result = fdmsock->open(SG_IO_OUT); + if (result == false) { + printf ("Socket Open Error\n"); + } else { + copy_to_FGADA(); + // Write FGExternal structure from socket to establish connection + int result = fdmsock->write(OutBuffer, nbytes); + printf("Connection established = %d.\n", result); + } +} + + +// Run an iteration of the EOM. This is essentially a NOP here +// because these values are getting filled in elsewhere based on +// external input. +void FGADA::update( double dt ) { + // cout << "FGADA::update()" << endl; + + if (is_suspended()) + return; + + char Buffer[numberofbytes]; + char OutBuffer[nbytes]; + + // Read FGExternal structure from socket + while (1) { + int result = fdmsock->read(Buffer, numberofbytes); + if (result == numberofbytes) { + // Copy buffer into FGExternal structure + memcpy (&sixdof_to_visuals, &Buffer, sizeof (Buffer)); + // Convert from the FGExternal struct to the FGInterface struct (input) + copy_from_FGADA(); + } else { + break; + } + } + + copy_to_FGADA(); + fgGetDouble("/sim/view/offset",view_offset); + if ( view_offset == 0.0) { + memcpy (&OutBuffer, &visuals_to_sixdof, sizeof (OutBuffer)); + fdmsock->write(OutBuffer, nbytes); + } +} + +// Convert from the FGInterface struct to the FGADA struct (output) +bool FGADA::copy_to_FGADA () { + ground_elevation = globals->get_scenery()->get_cur_elev(); + return true; +} + + +// Convert from the FGADA struct to the FGInterface struct (input) +bool FGADA::copy_from_FGADA() { + + //Positions and attitudes for The Rendering engine + _set_Geodetic_Position( Latitude, Longitude, Altitude ); + _set_Euler_Angles( Phi, Theta, Psi ); + _set_Geocentric_Position( Lat_geocentric, Lon_geocentric, + Radius_to_vehicle ); + _set_Sea_level_radius( Sea_level_radius ); + + _set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot ); + _set_Earth_position_angle( Earth_position_angle ); + _set_sin_lat_geocentric(Lat_geocentric); + _set_cos_lat_geocentric(Lat_geocentric); + _set_sin_cos_longitude(Longitude); + _set_sin_cos_latitude(Latitude); + + // Velocities and accelerations for the pitch ladder and velocity vector + _set_Accels_Local( U_dot_local, V_dot_local, W_dot_local ); + _set_Velocities_Ground( U_local, V_local, W_local );//same as V_NED in mps + _set_Velocities_Local( V_north, V_east, V_down ); //same as UVW_local in fps + + //Positions and attitude for ship + _set_daux(1,sixdof_to_visuals.aux1);//ship lat + _set_daux(2,sixdof_to_visuals.aux2);//ship lon + _set_daux(3,sixdof_to_visuals.aux3);//ship alt+heave + _set_daux(4,sixdof_to_visuals.aux4);//distance of a/c from ski-jump exit + _set_faux(1,sixdof_to_visuals.aux9);//ship pitch + _set_faux(2,sixdof_to_visuals.aux10);//ship roll + _set_faux(3,sixdof_to_visuals.aux11);//ship yaw + _set_iaux(1,sixdof_to_visuals.iaux1);//flag for drawing ship + + // controls + globals->get_controls()->set_throttle(0,throttle/131.0); + globals->get_controls()->set_elevator(pstick); + globals->get_controls()->set_aileron(rstick); + globals->get_controls()->set_rudder(rpedal); + + // auxilliary parameters for HUD + _set_V_calibrated_kts( V_calibrated_kts ); + _set_Alpha( Alpha ); + _set_Beta( Beta ); + _set_Accels_CG_Body_N( anxg,anyg,anzg); + _set_Mach_number( Machno); + _set_Climb_Rate( W_local*SG_METER_TO_FEET ); //pressure alt in feet for lca(navy) + + _set_iaux(2,sixdof_to_visuals.iaux2);//control law mode switch posn + _set_iaux(3,sixdof_to_visuals.iaux3);//ldg gear posn + _set_iaux(4,sixdof_to_visuals.iaux4);// wow nose status + _set_iaux(5,sixdof_to_visuals.iaux5);// wow main status + _set_iaux(6,sixdof_to_visuals.iaux6);// arrester hook posn + _set_iaux(7,sixdof_to_visuals.iaux7); + _set_iaux(8,sixdof_to_visuals.iaux8); + _set_iaux(9,sixdof_to_visuals.iaux9); + _set_iaux(10,sixdof_to_visuals.iaux10); + _set_iaux(11,sixdof_to_visuals.iaux11); + _set_iaux(12,sixdof_to_visuals.iaux12); + + _set_daux(5,sixdof_to_visuals.aux5); + _set_daux(6,sixdof_to_visuals.aux6); + _set_daux(7,sixdof_to_visuals.aux7); + _set_daux(8,sixdof_to_visuals.aux8); + + _set_faux(4,sixdof_to_visuals.aux12); + _set_faux(5,sixdof_to_visuals.aux13); + _set_faux(6,sixdof_to_visuals.aux14); + _set_faux(7,sixdof_to_visuals.aux15); + _set_faux(8,sixdof_to_visuals.aux16); + _set_faux(9,sixdof_to_visuals.aux17); + _set_faux(10,sixdof_to_visuals.aux18); + + // Angular rates + _set_Omega_Body( P_body, Q_body, R_body ); + + // Miscellaneous quantities + _set_Gamma_vert_rad( Gamma_vert_rad ); + _set_Runway_altitude( Runway_altitude ); + + // SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << Longitude + // << " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude + // << " alt = " << Altitude << " sl_radius = " << Sea_level_radius + // << " radius_to_vehicle = " << Radius_to_vehicle ); + + + // printf("sr=%f\n",Sea_level_radius); + // printf("psi = %f %f\n",Psi,Psi*SGD_RADIANS_TO_DEGREES); + + return true; +} diff --git a/src/FDM/SP/ADA.hxx b/src/FDM/SP/ADA.hxx new file mode 100644 index 000000000..b3830c72a --- /dev/null +++ b/src/FDM/SP/ADA.hxx @@ -0,0 +1,87 @@ +// ADA.hxx -- interface to the "External"-ly driven ADA flight model +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#ifndef _ADA_HXX +#define _ADA_HXX + + +class SGSocket; + +#include + + +class FGADA: public FGInterface { + +private: + + SGSocket *fdmsock; + + // Auxilliary Flight Model parameters, basically for HUD + double aux1; // auxilliary flag + double aux2; // auxilliary flag + double aux3; // auxilliary flag + double aux4; // auxilliary flag + double aux5; // auxilliary flag + double aux6; // auxilliary flag + double aux7; // auxilliary flag + double aux8; // auxilliary flag + float aux9; // auxilliary flag + float aux10; // auxilliary flag + float aux11; // auxilliary flag + float aux12; // auxilliary flag + float aux13; // auxilliary flag + float aux14; // auxilliary flag + float aux15; // auxilliary flag + float aux16; // auxilliary flag + float aux17; // auxilliary flag + float aux18; // auxilliary flag + int iaux1; // auxilliary flag + int iaux2; // auxilliary flag + int iaux3; // auxilliary flag + int iaux4; // auxilliary flag + int iaux5; // auxilliary flag + int iaux6; // auxilliary flag + int iaux7; // auxilliary flag + int iaux8; // auxilliary flag + int iaux9; // auxilliary flag + int iaux10; // auxilliary flag + int iaux11; // auxilliary flag + int iaux12; // auxilliary flag + + // copy FDM state to FGADA structures + bool copy_to_FGADA(); + + // copy FDM state from FGADA structures + bool copy_from_FGADA(); + +public: + + FGADA( double dt ); + ~FGADA(); + + // reset flight params to a specific position + void init(); + + // update position based on inputs, positions, velocities, etc. + void update(double dt); + +}; + + +#endif // _ADA_HXX diff --git a/src/FDM/SP/Makefile.am b/src/FDM/SP/Makefile.am new file mode 100644 index 000000000..f0295e518 --- /dev/null +++ b/src/FDM/SP/Makefile.am @@ -0,0 +1,8 @@ + +noinst_LIBRARIES = libSPFDM.a + +libSPFDM_a_SOURCES = \ + ADA.cxx ADA.hxx \ + ACMS.cxx ACMS.hxx + +INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src diff --git a/src/FDM/SP/README b/src/FDM/SP/README new file mode 100644 index 000000000..920672223 --- /dev/null +++ b/src/FDM/SP/README @@ -0,0 +1,5 @@ +This directory contains special purpose FDM's that should not be of +interest to the common user. At this time there is a special FDM to +support ACMS files which are generated from flight recorder data +(http://www.sasflightops.com/dlk/acms.htm). + diff --git a/src/Main/Makefile.am b/src/Main/Makefile.am index 87fc984aa..13a3b43c0 100644 --- a/src/Main/Makefile.am +++ b/src/Main/Makefile.am @@ -7,6 +7,12 @@ else MPLAYER_LIBS = endif +if ENABLE_SP_FDM +SP_FDM_LIBS = $(top_builddir)/src/FDM/SP/libSPFDM.a +else +SP_FDM_LIBS = +endif + if WITH_THREADS THREAD_LIBS = -lsgthreads $(thread_LIBS) else @@ -69,6 +75,7 @@ fgfs_LDADD = \ $(top_builddir)/src/FDM/JSBSim/filtersjb/libfiltersjb.a \ $(top_builddir)/src/FDM/LaRCsim/libLaRCsim.a \ $(top_builddir)/src/FDM/UIUCModel/libUIUCModel.a \ + $(SP_FDM_LIBS) \ $(top_builddir)/src/GUI/libGUI.a \ $(top_builddir)/src/Autopilot/libAutopilot.a \ $(top_builddir)/src/Input/libInput.a \ diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index eeb545951..1d5ec766d 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -82,7 +82,10 @@ #include #include #include -#include +#ifdef ENABLE_SP_FMDS +#include +#include +#endif #include #include #include @@ -1252,8 +1255,12 @@ void fgInitFDM() { cur_fdm_state = new FGLaRCsim( dt ); } else if ( model == "jsb" ) { cur_fdm_state = new FGJSBsim( dt ); +#ifdef ENABLE_SP_FMDS } else if ( model == "ada" ) { cur_fdm_state = new FGADA( dt ); + } else if ( model == "acms" ) { + cur_fdm_state = new FGACMS( dt ); +#endif } else if ( model == "balloon" ) { cur_fdm_state = new FGBalloonSim( dt ); } else if ( model == "magic" ) { @@ -1708,7 +1715,6 @@ bool fgInitSubsystems() { //////////////////////////////////////////////////////////////////// // Initialize the radio stack subsystem. //////////////////////////////////////////////////////////////////// - current_radiostack = new FGRadioStack; current_radiostack->init(); current_radiostack->bind(); @@ -1717,7 +1723,6 @@ bool fgInitSubsystems() { //////////////////////////////////////////////////////////////////// // Initialize the cockpit subsystem //////////////////////////////////////////////////////////////////// - if( fgCockpitInit( ¤t_aircraft )) { // Cockpit initialized ok. } else { diff --git a/src/Main/main.cxx b/src/Main/main.cxx index 42fef6d59..276892f32 100644 --- a/src/Main/main.cxx +++ b/src/Main/main.cxx @@ -68,7 +68,7 @@ #include #include #include -#include +// #include #include #include #include