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 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
208 F_Y_wind = CY * qS * Cos_beta * Cos_beta;
209 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
211 // wind-axis to body-axis transformation
212 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
213 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
214 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
216 // Moment calculations
217 M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
218 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
219 M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
221 // Adding in apparent mass effects
222 if (Mass_appMass_ratio)
223 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
224 if (I_xx_appMass_ratio)
225 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
226 if (I_yy_appMass_ratio)
227 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
228 if (I_zz_appMass_ratio)
229 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
232 F_Z_aero += -Mass_appMass * W_dot_body;
234 M_l_aero += -I_xx_appMass * P_dot_body;
236 M_m_aero += -I_yy_appMass * Q_dot_body;
238 M_n_aero += -I_zz_appMass * R_dot_body;
240 // gyroscopic moments
241 // engineOmega is positive when rotation is ccw when viewed from the front
242 if (gyroForce_Q_body)
243 M_n_aero += polarInertia * engineOmega * Q_body;
244 if (gyroForce_R_body)
245 M_m_aero += -polarInertia * engineOmega * R_body;
247 // ornithopter support
250 // F_X_aero += F_X_aero_flapper;
251 // F_Z_aero += F_Z_aero_flapper;
252 // M_m_aero += flapper_Moment;
263 vis = fgGetDouble("/environment/visibility-m");
268 fgSetDouble("/environment/visibility-m", vis);
272 /* Send data on the network to the Glass Cockpit */
276 // input += " stick_right " + ftoa(Lat_control);
277 // input += " rudder_left " + ftoa(-Rudder_pedal);
278 // input += " stick_forward " + ftoa(Long_control);
279 // input += " stick_trim_forward " + ftoa(Long_trim);
280 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
281 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
282 // input += " vehicle_speed " + ftoa(V_rel_wind);
283 // input += " throttle_forward " + ftoa(Throttle_pct);
284 // input += " altitude " + ftoa(Altitude);
285 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
287 // testarray.getHello();
288 // testarray.sendData(input);
290 /* End of Networking */
294 void uiuc_engine_routine()
299 void uiuc_gear_routine ()
304 void uiuc_record_routine(double dt)
306 if (Simtime >= recordStartTime)
310 //void uiuc_network_routine()
314 //end uiuc_wrapper.cpp