src/FDM/SP/ADA.cxx

   1 // ADA.cxx -- interface to the "External"-ly driven ADA flight model
   2 //
   3 // This program is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU General Public License as
   5 // published by the Free Software Foundation; either version 2 of the
   6 // License, or (at your option) any later version.
   7 //
   8 // This program is distributed in the hope that it will be useful, but
   9 // WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17 // $Id$
  18
  19 // Modified by Cdr. VS Renganthan <vsranga@ada.ernet.in>, 12 Oct 2K
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include <cstring>
  26 #include <cstdio>
  27
  28 #include <simgear/io/iochannel.hxx>
  29 #include <simgear/io/sg_socket.hxx>
  30 #include <simgear/constants.h>
  31
  32 #include <Aircraft/controls.hxx>
  33 #include <Scenery/scenery.hxx> //to pass ground elevation to FDM
  34 #include <Main/globals.hxx>
  35 #include <Main/fg_props.hxx> //to get ID of window (left/right or center)
  36
  37 #include "ADA.hxx"
  38
  39 #define numberofbytes 472 // from FDM to visuals
  40 #define nbytes 8        //from visuals to FDM
  41
  42 static struct {
  43     double number_of_bytes;
  44     double lat_geoc;
  45     double lon_geoc;
  46     double altitude;
  47     double psirad;
  48     double thetrad;
  49     double phirad;
  50     double earth_posn_angle;
  51     double radius_to_vehicle;
  52     double sea_level_radius;
  53     double latitude;
  54     double longitude;
  55     double Vnorth;
  56     double Veast;
  57     double Vdown;
  58     double Vcas_kts;
  59     double prad;
  60     double qrad;
  61     double rrad;
  62     double alpharad;
  63     double betarad;
  64     double latitude_dot;
  65     double longitude_dot;
  66     double radius_dot;
  67     double Gamma_vert_rad;
  68     double Runway_altitude;
  69     double throttle;
  70     double pstick;
  71     double rstick;
  72     double rpedal;
  73     double U_local;
  74     double V_local;
  75     double W_local;
  76     double U_dot_local;
  77     double V_dot_local;
  78     double W_dot_local;
  79     double Machno;
  80     double anxg;
  81     double anyg;
  82     double anzg;
  83     double aux1;
  84     double aux2;
  85     double aux3;
  86     double aux4;
  87     double aux5;
  88     double aux6;
  89     double aux7;
  90     double aux8;
  91     int iaux1;
  92     int iaux2;
  93     int iaux3;
  94     int iaux4;
  95     int iaux5;
  96     int iaux6;
  97     int iaux7;
  98     int iaux8;
  99     int iaux9;
 100     int iaux10;
 101     int iaux11;
 102     int iaux12;
 103     float aux9;
 104     float aux10;
 105     float aux11;
 106     float aux12;
 107     float aux13;
 108     float aux14;
 109     float aux15;
 110     float aux16;
 111     float aux17;
 112     float aux18;
 113 } sixdof_to_visuals;
 114
 115 double view_offset; //if this zero, means center window
 116
 117 static struct {
 118         double ground_elevation;
 119 } visuals_to_sixdof;
 120
 121 #define ground_elevation visuals_to_sixdof.ground_elevation
 122
 123 #define number_of_bytes sixdof_to_visuals.number_of_bytes
 124 #define U_dot_local sixdof_to_visuals.U_dot_local
 125 #define V_dot_local sixdof_to_visuals.V_dot_local
 126 #define W_dot_local sixdof_to_visuals.W_dot_local
 127 #define U_local sixdof_to_visuals.U_local
 128 #define V_local sixdof_to_visuals.V_local
 129 #define W_local sixdof_to_visuals.W_local
 130 #define throttle sixdof_to_visuals.throttle
 131 #define pstick sixdof_to_visuals.pstick
 132 #define rstick sixdof_to_visuals.rstick
 133 #define rpedal sixdof_to_visuals.rpedal
 134 #define V_north sixdof_to_visuals.Vnorth
 135 #define V_east sixdof_to_visuals.Veast
 136 #define V_down sixdof_to_visuals.Vdown
 137 #define V_calibrated_kts sixdof_to_visuals.Vcas_kts
 138 #define P_body sixdof_to_visuals.prad
 139 #define Q_body sixdof_to_visuals.qrad
 140 #define R_body sixdof_to_visuals.rrad
 141 #define Latitude_dot sixdof_to_visuals.latitude_dot
 142 #define Longitude_dot sixdof_to_visuals.longitude_dot
 143 #define Radius_dot sixdof_to_visuals.radius_dot
 144 #define Latitude sixdof_to_visuals.latitude
 145 #define Longitude sixdof_to_visuals.longitude
 146 #define Lat_geocentric sixdof_to_visuals.lat_geoc
 147 #define Lon_geocentric sixdof_to_visuals.lon_geoc
 148 #define Radius_to_vehicle sixdof_to_visuals.radius_to_vehicle
 149 #define Altitude sixdof_to_visuals.altitude
 150 #define Phi sixdof_to_visuals.phirad
 151 #define Theta sixdof_to_visuals.thetrad
 152 #define Psi sixdof_to_visuals.psirad
 153 #define Alpha sixdof_to_visuals.alpharad
 154 #define Beta sixdof_to_visuals.betarad
 155 #define Sea_level_radius sixdof_to_visuals.sea_level_radius
 156 #define Earth_position_angle sixdof_to_visuals.earth_posn_angle
 157 #define Runway_altitude sixdof_to_visuals.Runway_altitude
 158 #define Gamma_vert_rad sixdof_to_visuals.Gamma_vert_rad
 159 #define Machno sixdof_to_visuals.Machno
 160 #define anxg sixdof_to_visuals.anxg
 161 #define anyg sixdof_to_visuals.anyg
 162 #define anzg sixdof_to_visuals.anzg
 163
 164
 165 FGADA::FGADA( double dt ) :
 166     fdmsock(0)
 167 {
 168 //     set_delta_t( dt );
 169 }
 170
 171
 172 FGADA::~FGADA() {
 173     delete fdmsock;
 174 }
 175
 176
 177 // Initialize the ADA flight model, dt is the time increment
 178 // for each subsequent iteration through the EOM
 179 void FGADA::init() {
 180
 181     //do init common to all FDM"s
 182     common_init();
 183
 184     //now do ADA-specific init.
 185
 186     // cout << "FGADA::init()" << endl;
 187
 188     char OutBuffer[nbytes];
 189     copy_to_FGADA();
 190
 191     printf("\nInitialising UDP sockets\n");
 192     // initialise a "udp" socket
 193     fdmsock = new SGSocket( "fdm_pc", "5001", "udp" );
 194
 195     // open as a client
 196     bool result = fdmsock->open(SG_IO_OUT);
 197     if (result == false) {
 198         printf ("Socket Open Error\n");
 199     } else {
 200     copy_to_FGADA();
 201         // Write FGExternal structure from socket to establish connection
 202         int result = fdmsock->write(OutBuffer, nbytes);
 203         printf("Connection established = %d.\n", result);
 204     }
 205 }
 206
 207
 208 // Run an iteration of the EOM.  This is essentially a NOP here
 209 // because these values are getting filled in elsewhere based on
 210 // external input.
 211 void FGADA::update( double dt ) {
 212     // cout << "FGADA::update()" << endl;
 213
 214     if (is_suspended())
 215       return;
 216
 217     char Buffer[numberofbytes];
 218     char OutBuffer[nbytes];
 219
 220     // Read FGExternal structure from socket
 221     while (1) {
 222       int result = fdmsock->read(Buffer, numberofbytes);
 223       if (result == numberofbytes) {
 224          // Copy buffer into FGExternal structure
 225          memcpy (&sixdof_to_visuals, &Buffer, sizeof (Buffer));
 226              // Convert from the FGExternal struct to the FGInterface struct (input)
 227                  copy_from_FGADA();
 228       } else {
 229          break;
 230       }
 231     }
 232
 233     copy_to_FGADA();
 234     fgGetDouble("/sim/view/offset",view_offset);
 235     if ( view_offset == 0.0) {
 236         memcpy (&OutBuffer, &visuals_to_sixdof, sizeof (OutBuffer));
 237         fdmsock->write(OutBuffer, nbytes);
 238     }
 239 }
 240
 241 // Convert from the FGInterface struct to the FGADA struct (output)
 242 bool FGADA::copy_to_FGADA () {
 243     ground_elevation = get_Runway_altitude_m();
 244     return true;
 245 }
 246
 247
 248 // Convert from the FGADA struct to the FGInterface struct (input)
 249 bool FGADA::copy_from_FGADA() {
 250
 251     //Positions and attitudes for The Rendering engine
 252     _set_Geodetic_Position( Latitude, Longitude, Altitude );
 253     _set_Euler_Angles( Phi, Theta, Psi );
 254     _set_Geocentric_Position( Lat_geocentric, Lon_geocentric,
 255                               Radius_to_vehicle );
 256     _set_Sea_level_radius( Sea_level_radius );
 257
 258         _set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot );
 259     _set_Earth_position_angle( Earth_position_angle );
 260
 261         // Velocities and accelerations for the pitch ladder and velocity vector
 262     _set_Accels_Local( U_dot_local, V_dot_local, W_dot_local );
 263     _set_Velocities_Ground( U_local, V_local, W_local );//same as V_NED in mps
 264     _set_Velocities_Local( V_north, V_east, V_down ); //same as UVW_local in fps
 265
 266     //Positions and attitude for ship
 267
 268     fgSetDouble("/fdm-ada/ship-lat", sixdof_to_visuals.aux1);
 269     fgSetDouble("/fdm-ada/ship-lon", sixdof_to_visuals.aux2);
 270     fgSetDouble("/fdm-ada/ship-alt", sixdof_to_visuals.aux3);
 271     fgSetDouble("/fdm-ada/skijump-dist", sixdof_to_visuals.aux4);
 272     fgSetDouble("/fdm-ada/ship-pitch", sixdof_to_visuals.aux9); // faux1
 273     fgSetDouble("/fdm-ada/ship-roll", sixdof_to_visuals.aux10); // faux2
 274     fgSetDouble("/fdm-ada/ship-yaw", sixdof_to_visuals.aux11);  // faux3
 275     fgSetInt("/fdm-ada/draw-ship", sixdof_to_visuals.iaux1);
 276
 277     // controls
 278     globals->get_controls()->set_throttle(0,throttle/131.0);
 279     globals->get_controls()->set_elevator(pstick);
 280     globals->get_controls()->set_aileron(rstick);
 281     globals->get_controls()->set_rudder(rpedal);
 282
 283     // auxilliary parameters for HUD
 284     _set_V_calibrated_kts( V_calibrated_kts );
 285     _set_Alpha( Alpha );
 286     _set_Beta( Beta );
 287     _set_Accels_CG_Body_N( anxg,anyg,anzg);
 288     _set_Mach_number( Machno);
 289     _set_Climb_Rate( W_local*SG_METER_TO_FEET ); //pressure alt in feet for lca(navy)
 290
 291     fgSetInt("/fdm-ada/iaux2", sixdof_to_visuals.iaux2); //control law mode switch posn
 292     fgSetInt("/fdm-ada/iaux3", sixdof_to_visuals.iaux3); //ldg gear posn
 293     fgSetInt("/fdm-ada/iaux4", sixdof_to_visuals.iaux4); // wow nose status
 294     fgSetInt("/fdm-ada/iaux5", sixdof_to_visuals.iaux5); // wow main status
 295     fgSetInt("/fdm-ada/iaux6", sixdof_to_visuals.iaux6); // arrester hook posn
 296     fgSetInt("/fdm-ada/iaux7", sixdof_to_visuals.iaux7);
 297     fgSetInt("/fdm-ada/iaux8", sixdof_to_visuals.iaux8);
 298     fgSetInt("/fdm-ada/iaux9", sixdof_to_visuals.iaux9);
 299     fgSetInt("/fdm-ada/iaux10", sixdof_to_visuals.iaux10);
 300     fgSetInt("/fdm-ada/iaux11", sixdof_to_visuals.iaux11);
 301     fgSetInt("/fdm-ada/iaux12", sixdof_to_visuals.iaux12);
 302
 303     fgSetDouble("/fdm-ada/aux5", sixdof_to_visuals.aux5);
 304     fgSetDouble("/fdm-ada/aux6", sixdof_to_visuals.aux6);
 305     fgSetDouble("/fdm-ada/aux7", sixdof_to_visuals.aux7);
 306     fgSetDouble("/fdm-ada/aux8", sixdof_to_visuals.aux8);
 307
 308     fgSetDouble("/fdm-ada/aux12", sixdof_to_visuals.aux12);
 309     fgSetDouble("/fdm-ada/aux13", sixdof_to_visuals.aux13);
 310     fgSetDouble("/fdm-ada/aux14", sixdof_to_visuals.aux14);
 311     fgSetDouble("/fdm-ada/aux15", sixdof_to_visuals.aux15);
 312     fgSetDouble("/fdm-ada/aux16", sixdof_to_visuals.aux16);
 313     fgSetDouble("/fdm-ada/aux17", sixdof_to_visuals.aux17);
 314     fgSetDouble("/fdm-ada/aux18", sixdof_to_visuals.aux18);
 315
 316     // Angular rates
 317     _set_Omega_Body( P_body, Q_body, R_body );
 318
 319     // Miscellaneous quantities
 320     _set_Gamma_vert_rad( Gamma_vert_rad );
 321     _set_Runway_altitude( Runway_altitude );
 322
 323     //    SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << Longitude
 324     //      << " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude
 325     //      << " alt = " << Altitude << " sl_radius = " << Sea_level_radius
 326     //      << " radius_to_vehicle = " << Radius_to_vehicle );
 327
 328
 329     //    printf("sr=%f\n",Sea_level_radius);
 330     //    printf("psi = %f %f\n",Psi,Psi*SGD_RADIANS_TO_DEGREES);
 331
 332     return true;
 333 }