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