1 // ADA.cxx -- interface to the "External"-ly driven ADA flight model
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.
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.
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.
19 // Modified by Cdr. VS Renganthan <vsranga@ada.ernet.in>, 12 Oct 2K
28 #include <simgear/io/iochannel.hxx>
29 #include <simgear/io/sg_socket.hxx>
30 #include <simgear/constants.h>
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)
39 #define numberofbytes 472 // from FDM to visuals
40 #define nbytes 8 //from visuals to FDM
43 double number_of_bytes;
50 double earth_posn_angle;
51 double radius_to_vehicle;
52 double sea_level_radius;
67 double Gamma_vert_rad;
68 double Runway_altitude;
115 double view_offset; //if this zero, means center window
118 double ground_elevation;
121 #define ground_elevation visuals_to_sixdof.ground_elevation
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
165 FGADA::FGADA( double dt ) :
168 // set_delta_t( dt );
177 // Initialize the ADA flight model, dt is the time increment
178 // for each subsequent iteration through the EOM
181 //do init common to all FDM"s
184 //now do ADA-specific init.
186 // cout << "FGADA::init()" << endl;
188 char OutBuffer[nbytes];
191 printf("\nInitialising UDP sockets\n");
192 // initialise a "udp" socket
193 fdmsock = new SGSocket( "fdm_pc", "5001", "udp" );
196 bool result = fdmsock->open(SG_IO_OUT);
197 if (result == false) {
198 printf ("Socket Open Error\n");
201 // Write FGExternal structure from socket to establish connection
202 int result = fdmsock->write(OutBuffer, nbytes);
203 printf("Connection established = %d.\n", result);
208 // Run an iteration of the EOM. This is essentially a NOP here
209 // because these values are getting filled in elsewhere based on
211 void FGADA::update( double dt ) {
212 // cout << "FGADA::update()" << endl;
217 char Buffer[numberofbytes];
218 char OutBuffer[nbytes];
220 // Read FGExternal structure from socket
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)
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);
241 // Convert from the FGInterface struct to the FGADA struct (output)
242 bool FGADA::copy_to_FGADA () {
243 ground_elevation = get_Runway_altitude_m();
248 // Convert from the FGADA struct to the FGInterface struct (input)
249 bool FGADA::copy_from_FGADA() {
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,
256 _set_Sea_level_radius( Sea_level_radius );
258 _set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot );
259 _set_Earth_position_angle( Earth_position_angle );
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
266 //Positions and attitude for ship
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);
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);
283 // auxilliary parameters for HUD
284 _set_V_calibrated_kts( V_calibrated_kts );
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)
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);
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);
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);
317 _set_Omega_Body( P_body, Q_body, R_body );
319 // Miscellaneous quantities
320 _set_Gamma_vert_rad( Gamma_vert_rad );
321 _set_Runway_altitude( Runway_altitude );
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 );
329 // printf("sr=%f\n",Sea_level_radius);
330 // printf("psi = %f %f\n",Psi,Psi*SGD_RADIANS_TO_DEGREES);