src/FDM/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., 675 Mass Ave, Cambridge, MA 02139, USA.
  16 //
  17 // $Id$
  18
  19 // Modified by Cdr. VS Renganthan <vsranga@ada.ernet.in>, 12 Oct 2K
  20
  21 #include <simgear/io/iochannel.hxx>
  22
  23 #include <Controls/controls.hxx>
  24 #include <GUI/gui.h>
  25 #include <Main/globals.hxx>
  26
  27 #include "ADA.hxx"
  28
  29 #define numberofbytes 472
  30
  31 struct {
  32     double number_of_bytes;
  33     double lat_geoc;
  34     double lon_geoc;
  35     double altitude;
  36     double psirad;
  37     double thetrad;
  38     double phirad;
  39     double earth_posn_angle;
  40     double radius_to_vehicle;
  41     double sea_level_radius;
  42     double latitude;
  43     double longitude;
  44     double Vnorth;
  45     double Veast;
  46     double Vdown;
  47     double Vcas_kts;
  48     double prad;
  49     double qrad;
  50     double rrad;
  51     double alpharad;
  52     double betarad;
  53     double latitude_dot;
  54     double longitude_dot;
  55     double radius_dot;
  56     double Gamma_vert_rad;
  57     double Runway_altitude;
  58     double throttle;
  59     double pstick;
  60     double rstick;
  61     double rpedal;
  62     double U_local;
  63     double V_local;
  64     double W_local;
  65     double U_dot_local;
  66     double V_dot_local;
  67     double W_dot_local;
  68     double Machno;
  69     double anxg;
  70     double anyg;
  71     double anzg;
  72     double aux1;
  73     double aux2;
  74     double aux3;
  75     double aux4;
  76     double aux5;
  77     double aux6;
  78     double aux7;
  79     double aux8;
  80     int iaux1;
  81     int iaux2;
  82     int iaux3;
  83     int iaux4;
  84     int iaux5;
  85     int iaux6;
  86     int iaux7;
  87     int iaux8;
  88     int iaux9;
  89     int iaux10;
  90     int iaux11;
  91     int iaux12;
  92     float aux9;
  93     float aux10;
  94     float aux11;
  95     float aux12;
  96     float aux13;
  97     float aux14;
  98     float aux15;
  99     float aux16;
 100     float aux17;
 101     float aux18;
 102 } sixdof_to_visuals;
 103
 104 #define number_of_bytes sixdof_to_visuals.number_of_bytes
 105 #define U_dot_local sixdof_to_visuals.U_dot_local
 106 #define V_dot_local sixdof_to_visuals.V_dot_local
 107 #define W_dot_local sixdof_to_visuals.W_dot_local
 108 #define U_local sixdof_to_visuals.U_local
 109 #define V_local sixdof_to_visuals.V_local
 110 #define W_local sixdof_to_visuals.W_local
 111 #define throttle sixdof_to_visuals.throttle
 112 #define pstick sixdof_to_visuals.pstick
 113 #define rstick sixdof_to_visuals.rstick
 114 #define rpedal sixdof_to_visuals.rpedal
 115 #define V_north sixdof_to_visuals.Vnorth
 116 #define V_east sixdof_to_visuals.Veast
 117 #define V_down sixdof_to_visuals.Vdown
 118 #define V_calibrated_kts sixdof_to_visuals.Vcas_kts
 119 #define P_body sixdof_to_visuals.prad
 120 #define Q_body sixdof_to_visuals.qrad
 121 #define R_body sixdof_to_visuals.rrad
 122 #define Latitude_dot sixdof_to_visuals.latitude_dot
 123 #define Longitude_dot sixdof_to_visuals.longitude_dot
 124 #define Radius_dot sixdof_to_visuals.radius_dot
 125 #define Latitude sixdof_to_visuals.latitude
 126 #define Longitude sixdof_to_visuals.longitude
 127 #define Lat_geocentric sixdof_to_visuals.lat_geoc
 128 #define Lon_geocentric sixdof_to_visuals.lon_geoc
 129 #define Radius_to_vehicle sixdof_to_visuals.radius_to_vehicle
 130 #define Altitude sixdof_to_visuals.altitude
 131 #define Phi sixdof_to_visuals.phirad
 132 #define Theta sixdof_to_visuals.thetrad
 133 #define Psi sixdof_to_visuals.psirad
 134 #define Alpha sixdof_to_visuals.alpharad
 135 #define Beta sixdof_to_visuals.betarad
 136 #define Sea_level_radius sixdof_to_visuals.sea_level_radius
 137 #define Earth_position_angle sixdof_to_visuals.earth_posn_angle
 138 #define Runway_altitude sixdof_to_visuals.Runway_altitude
 139 #define Gamma_vert_rad sixdof_to_visuals.Gamma_vert_rad
 140 #define Machno sixdof_to_visuals.Machno
 141 #define anxg sixdof_to_visuals.anxg
 142 #define anyg sixdof_to_visuals.anyg
 143 #define anzg sixdof_to_visuals.anzg
 144
 145
 146 FGADA::FGADA( double dt ) {
 147     set_delta_t( dt );
 148 }
 149
 150
 151 FGADA::~FGADA() {
 152 }
 153
 154
 155 // Initialize the ADA flight model, dt is the time increment
 156 // for each subsequent iteration through the EOM
 157 void FGADA::init() {
 158
 159                                 // explicitly call the superclass's
 160                                 // init() method first.
 161     FGInterface::init();
 162
 163     // cout << "FGADA::init()" << endl;
 164
 165     char Buffer[numberofbytes];
 166
 167     printf("\nInitialising UDP sockets\n");
 168     // initialise a "udp" socket
 169     fdmsock = new SGSocket( "reddy_pc", "5001", "udp" );
 170
 171     // open as a client
 172     bool result = fdmsock->open(SG_IO_OUT);
 173     if (result == false) {
 174         printf ("Socket Open Error\n");
 175     } else {
 176         // Dummy Write FGExternal structure from socket to establish connection
 177         int result = fdmsock->write(Buffer, numberofbytes);
 178         printf("Connection established.\n");
 179     }
 180 }
 181
 182
 183 // Run an iteration of the EOM.  This is essentially a NOP here
 184 // because these values are getting filled in elsewhere based on
 185 // external input.
 186 bool FGADA::update( int multiloop ) {
 187     // cout << "FGADA::update()" << endl;
 188
 189     char Buffer[numberofbytes];
 190
 191     // Read FGExternal structure from socket
 192     while (1) {
 193       int result = fdmsock->read(Buffer, numberofbytes);
 194       if (result == numberofbytes) {
 195          // Copy buffer into FGExternal structure
 196          memcpy (&sixdof_to_visuals, &Buffer, sizeof (Buffer));
 197       } else {
 198          break;
 199       }
 200     }
 201
 202     //cout << endl << sixdof_to_visuals.aux18 << endl;
 203     // Close Visuals through message/flag from Flight model
 204     if (sixdof_to_visuals.aux18 == 1) {
 205         fdmsock->close();
 206         ConfirmExitDialog();
 207     }
 208     //cout << endl << sixdof_to_visuals.aux18 << endl;
 209
 210     // Convert from the FGExternal struct to the FGInterface struct (input)
 211     copy_from_FGADA();
 212
 213     return true;
 214 }
 215
 216 // Convert from the FGInterface struct to the FGADA struct (output)
 217 bool FGADA::copy_to_FGADA () {
 218
 219     return true;
 220 }
 221
 222
 223 // Convert from the FGADA struct to the FGInterface struct (input)
 224 bool FGADA::copy_from_FGADA() {
 225
 226     // Velocities
 227     _set_Velocities_Local( V_north, V_east, V_down );
 228     _set_V_calibrated_kts( V_calibrated_kts );
 229
 230     // Angular rates
 231     _set_Omega_Body( P_body, Q_body, R_body );
 232     _set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot );
 233
 234     //    SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << Longitude
 235     //      << " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude
 236     //      << " alt = " << Altitude << " sl_radius = " << Sea_level_radius
 237     //      << " radius_to_vehicle = " << Radius_to_vehicle );
 238
 239     // Positions
 240     _set_Geocentric_Position( Lat_geocentric, Lon_geocentric,
 241                               Radius_to_vehicle );
 242     _set_Geodetic_Position( Latitude, Longitude, Altitude );
 243     _set_Euler_Angles( Phi, Theta, Psi );
 244
 245     // Miscellaneous quantities
 246     _set_Alpha( Alpha );
 247     _set_Beta( Beta );
 248     _set_Gamma_vert_rad( Gamma_vert_rad );
 249     _set_Sea_level_radius( Sea_level_radius );
 250     _set_Earth_position_angle( Earth_position_angle );
 251     _set_Runway_altitude( Runway_altitude );
 252     _set_sin_lat_geocentric(Lat_geocentric);
 253     _set_cos_lat_geocentric(Lat_geocentric);
 254     _set_sin_cos_longitude(Longitude);
 255     _set_sin_cos_latitude(Latitude);
 256     _set_Accels_Local( U_dot_local, V_dot_local, W_dot_local );
 257     _set_Velocities_Ground( U_local, V_local, W_local );
 258     _set_Accels_CG_Body_N( anxg,anyg,anzg);
 259     _set_Mach_number( Machno);
 260
 261     //    printf("sr=%f\n",Sea_level_radius);
 262     //    printf("psi = %f %f\n",Psi,Psi*SGD_RADIANS_TO_DEGREES);
 263
 264     // controls
 265     globals->get_controls()->set_throttle(0,throttle/131.0);
 266     globals->get_controls()->set_elevator(pstick);
 267     globals->get_controls()->set_aileron(rstick);
 268     globals->get_controls()->set_rudder(rpedal);
 269
 270     // auxilliary parameters for HUD
 271     set_iaux1(sixdof_to_visuals.iaux1);
 272     set_iaux2(sixdof_to_visuals.iaux2);
 273     set_iaux3(sixdof_to_visuals.iaux3);
 274     set_iaux4(sixdof_to_visuals.iaux4);
 275     set_iaux5(sixdof_to_visuals.iaux5);
 276     set_iaux6(sixdof_to_visuals.iaux6);
 277     set_iaux7(sixdof_to_visuals.iaux7);
 278     set_iaux8(sixdof_to_visuals.iaux8);
 279     set_iaux9(sixdof_to_visuals.iaux9);
 280     set_iaux10(sixdof_to_visuals.iaux10);
 281     set_iaux11(sixdof_to_visuals.iaux11);
 282     set_iaux12(sixdof_to_visuals.iaux12);
 283     set_aux1(sixdof_to_visuals.aux1);
 284     set_aux2(sixdof_to_visuals.aux2);
 285     set_aux3(sixdof_to_visuals.aux3);
 286     set_aux4(sixdof_to_visuals.aux4);
 287     set_aux5(sixdof_to_visuals.aux5);
 288     set_aux6(sixdof_to_visuals.aux6);
 289     set_aux7(sixdof_to_visuals.aux7);
 290     set_aux8(sixdof_to_visuals.aux8);
 291     set_aux9(sixdof_to_visuals.aux9);
 292     set_aux10(sixdof_to_visuals.aux10);
 293     set_aux11(sixdof_to_visuals.aux11);
 294     set_aux12(sixdof_to_visuals.aux12);
 295     set_aux13(sixdof_to_visuals.aux13);
 296     set_aux14(sixdof_to_visuals.aux14);
 297     set_aux15(sixdof_to_visuals.aux15);
 298     set_aux16(sixdof_to_visuals.aux16);
 299     set_aux17(sixdof_to_visuals.aux17);
 300     set_aux18(sixdof_to_visuals.aux18);
 301
 302     cout << endl << sixdof_to_visuals.aux18 << endl;
 303
 304     return true;
 305 }