1 /**********************************************************************
3 FILENAME: uiuc_wrapper.cpp
5 ----------------------------------------------------------------------
7 DESCRIPTION: A wrapper(interface) between the UIUC Aeromodel (C++ files)
8 and the LaRCsim FDM (C files)
10 ----------------------------------------------------------------------
14 ----------------------------------------------------------------------
18 ----------------------------------------------------------------------
20 HISTORY: 01/26/2000 initial release
21 03/09/2001 (DPM) added support for gear
22 06/18/2001 (RD) Made uiuc_recorder its own routine.
23 07/19/2001 (RD) Added uiuc_vel_init() to initialize
25 08/27/2001 (RD) Added uiuc_initial_init() to help
26 in starting an A/C at an initial condition
27 02/24/2002 (GD) Added uiuc_network_routine()
28 03/27/2002 (RD) Changed how forces are calculated when
31 ----------------------------------------------------------------------
33 AUTHOR(S): Bipin Sehgal <bsehgal@uiuc.edu>
34 Robert Deters <rdeters@uiuc.edu>
35 Glen Dimock <dimock@uiuc.edu>
36 David Megginson <david@megginson.com>
38 ----------------------------------------------------------------------
42 ----------------------------------------------------------------------
46 ----------------------------------------------------------------------
50 ----------------------------------------------------------------------
54 ----------------------------------------------------------------------
58 ----------------------------------------------------------------------
60 COPYRIGHT: (C) 2000 by Michael Selig
62 This program is free software; you can redistribute it and/or
63 modify it under the terms of the GNU General Public License
64 as published by the Free Software Foundation.
66 This program is distributed in the hope that it will be useful,
67 but WITHOUT ANY WARRANTY; without even the implied warranty of
68 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
69 GNU General Public License for more details.
71 You should have received a copy of the GNU General Public License
72 along with this program; if not, write to the Free Software
73 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
74 USA or view http://www.gnu.org/copyleft/gpl.html.
76 **********************************************************************/
78 #include <simgear/compiler.h>
79 #include <simgear/misc/sg_path.hxx>
80 #include <Aircraft/aircraft.hxx>
81 #include <Main/fg_props.hxx>
83 #include "uiuc_aircraft.h"
84 #include "uiuc_aircraftdir.h"
85 #include "uiuc_coefficients.h"
86 #include "uiuc_engine.h"
87 #include "uiuc_gear.h"
88 #include "uiuc_aerodeflections.h"
89 #include "uiuc_recorder.h"
90 #include "uiuc_menu.h"
91 #include "uiuc_betaprobe.h"
92 #include <FDM/LaRCsim/ls_generic.h>
93 //#include "Main/simple_udp.h"
94 #include "uiuc_fog.h" //321654
95 //#include "uiuc_network.h"
96 //#include "uiuc_get_flapper.h"
98 #if !defined (SG_HAVE_NATIVE_SGI_COMPILERS)
103 extern "C" void uiuc_init_aeromodel ();
104 extern "C" void uiuc_force_moment(double dt);
105 extern "C" void uiuc_engine_routine();
106 extern "C" void uiuc_gear_routine();
107 extern "C" void uiuc_record_routine(double dt);
108 //extern "C" void uiuc_network_routine();
109 extern "C" void uiuc_vel_init ();
110 extern "C" void uiuc_initial_init ();
112 AIRCRAFT *aircraft_ = new AIRCRAFT;
113 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
115 // SendArray testarray(4950);
117 /* Convert float to string */
118 //string ftoa(double in)
120 // static char temp[20];
121 // sprintf(temp,"%g",in);
122 // return (string)temp;
125 void uiuc_initial_init ()
127 if (P_body_init_true)
128 P_body = P_body_init;
129 if (Q_body_init_true)
130 Q_body = Q_body_init;
131 if (R_body_init_true)
132 R_body = R_body_init;
141 if (U_body_init_true)
142 U_body = U_body_init;
143 if (V_body_init_true)
144 V_body = V_body_init;
145 if (W_body_init_true)
146 W_body = W_body_init;
150 void uiuc_vel_init ()
152 if (U_body_init_true && V_body_init_true && W_body_init_true)
154 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
156 det_T_l_to_b = T_local_to_body_11*(T_local_to_body_22*T_local_to_body_33-T_local_to_body_23*T_local_to_body_32) - T_local_to_body_12*(T_local_to_body_21*T_local_to_body_33-T_local_to_body_23*T_local_to_body_31) + T_local_to_body_13*(T_local_to_body_21*T_local_to_body_32-T_local_to_body_22*T_local_to_body_31);
157 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
158 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
159 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
160 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
161 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
162 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
163 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
164 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
165 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
167 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
168 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
169 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
171 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
175 void uiuc_init_aeromodel ()
177 SGPath path(globals->get_fg_root());
178 path.append(aircraft_dir);
179 path.append("aircraft.dat");
180 cout << "We are using "<< path.str() << endl;
181 uiuc_initializemaps(); // Initialize the <string,int> maps
182 uiuc_menu(path.str()); // Read the specified aircraft file
185 void uiuc_force_moment(double dt)
187 double qS = Dynamic_pressure * Sw;
188 double qScbar = qS * cbar;
189 double qSb = qS * bw;
191 uiuc_aerodeflections(dt);
192 uiuc_coefficients(dt);
195 // uiuc_get_flapper(dt);
198 /* Calculate the forces */
207 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
208 // hence Cos_beta * Cos_beta term included.
209 // Same thing is done w/ moments below.
210 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
211 // that should not be the case. See FGFS notes 021105
212 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
214 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
215 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
216 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
217 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
219 // wind-axis to body-axis transformation
220 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
221 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
222 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
224 // Moment calculations
225 M_l_aero = Cl * qSb ;
226 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
227 M_n_aero = Cn * qSb ;
228 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
229 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
230 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
232 // Adding in apparent mass effects
233 if (Mass_appMass_ratio)
234 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
235 if (I_xx_appMass_ratio)
236 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
237 if (I_yy_appMass_ratio)
238 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
239 if (I_zz_appMass_ratio)
240 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
243 F_Z_aero += -Mass_appMass * W_dot_body;
245 M_l_aero += -I_xx_appMass * P_dot_body;
247 M_m_aero += -I_yy_appMass * Q_dot_body;
249 M_n_aero += -I_zz_appMass * R_dot_body;
251 // gyroscopic moments
252 // engineOmega is positive when rotation is ccw when viewed from the front
253 if (gyroForce_Q_body)
254 M_n_aero += polarInertia * engineOmega * Q_body;
255 if (gyroForce_R_body)
256 M_m_aero += -polarInertia * engineOmega * R_body;
258 // ornithopter support
261 // F_X_aero += F_X_aero_flapper;
262 // F_Z_aero += F_Z_aero_flapper;
263 // M_m_aero += flapper_Moment;
274 vis = fgGetDouble("/environment/visibility-m");
279 fgSetDouble("/environment/visibility-m", vis);
283 /* Send data on the network to the Glass Cockpit */
287 // input += " stick_right " + ftoa(Lat_control);
288 // input += " rudder_left " + ftoa(-Rudder_pedal);
289 // input += " stick_forward " + ftoa(Long_control);
290 // input += " stick_trim_forward " + ftoa(Long_trim);
291 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
292 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
293 // input += " vehicle_speed " + ftoa(V_rel_wind);
294 // input += " throttle_forward " + ftoa(Throttle_pct);
295 // input += " altitude " + ftoa(Altitude);
296 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
298 // testarray.getHello();
299 // testarray.sendData(input);
301 /* End of Networking */
305 void uiuc_engine_routine()
310 void uiuc_gear_routine ()
315 void uiuc_record_routine(double dt)
317 if (Simtime >= recordStartTime)
321 //void uiuc_network_routine()
323 // if (use_uiuc_network)
324 // uiuc_network(2); //send data
326 //end uiuc_wrapper.cpp