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 **********************************************************************/
82 #include <simgear/compiler.h>
83 #include <simgear/misc/sg_path.hxx>
84 #include <Aircraft/aircraft.hxx>
85 #include <Main/fg_props.hxx>
87 #include "uiuc_aircraft.h"
88 #include "uiuc_aircraftdir.h"
89 #include "uiuc_coefficients.h"
90 #include "uiuc_engine.h"
91 #include "uiuc_gear.h"
92 #include "uiuc_aerodeflections.h"
93 #include "uiuc_recorder.h"
94 #include "uiuc_menu.h"
95 #include "uiuc_betaprobe.h"
96 #include <FDM/LaRCsim/ls_generic.h>
97 //#include "Main/simple_udp.h"
98 #include "uiuc_fog.h" //321654
99 //#include "uiuc_network.h"
100 //#include "uiuc_get_flapper.h"
105 extern "C" void uiuc_init_aeromodel ();
106 extern "C" void uiuc_force_moment(double dt);
107 extern "C" void uiuc_engine_routine();
108 extern "C" void uiuc_gear_routine();
109 extern "C" void uiuc_record_routine(double dt);
110 //extern "C" void uiuc_network_routine();
111 extern "C" void uiuc_vel_init ();
112 extern "C" void uiuc_initial_init ();
114 AIRCRAFT *aircraft_ = new AIRCRAFT;
115 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
117 // SendArray testarray(4950);
119 /* Convert float to string */
120 //string ftoa(double in)
122 // static char temp[20];
123 // sprintf(temp,"%g",in);
124 // return (string)temp;
127 void uiuc_initial_init ()
129 if (P_body_init_true)
130 P_body = P_body_init;
131 if (Q_body_init_true)
132 Q_body = Q_body_init;
133 if (R_body_init_true)
134 R_body = R_body_init;
143 if (U_body_init_true)
144 U_body = U_body_init;
145 if (V_body_init_true)
146 V_body = V_body_init;
147 if (W_body_init_true)
148 W_body = W_body_init;
152 void uiuc_vel_init ()
154 if (U_body_init_true && V_body_init_true && W_body_init_true)
156 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
158 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);
159 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
160 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
161 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
162 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
163 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
164 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
165 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
166 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
167 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
169 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
170 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
171 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
173 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
177 void uiuc_init_aeromodel ()
179 SGPath path(globals->get_fg_root());
180 path.append(aircraft_dir);
181 path.append("aircraft.dat");
182 cout << "We are using "<< path.str() << endl;
183 uiuc_initializemaps(); // Initialize the <string,int> maps
184 uiuc_menu(path.str()); // Read the specified aircraft file
187 void uiuc_force_moment(double dt)
189 double qS = Dynamic_pressure * Sw;
190 double qScbar = qS * cbar;
191 double qSb = qS * bw;
193 uiuc_aerodeflections(dt);
194 uiuc_coefficients(dt);
197 // uiuc_get_flapper(dt);
200 /* Calculate the forces */
209 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
210 // hence Cos_beta * Cos_beta term included.
211 // Same thing is done w/ moments below.
212 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
213 // that should not be the case. See FGFS notes 021105
214 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
216 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
217 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
218 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
219 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
221 // wind-axis to body-axis transformation
222 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
223 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
224 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
226 // Moment calculations
227 M_l_aero = Cl * qSb ;
228 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
229 M_n_aero = Cn * qSb ;
230 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
231 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
232 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
234 // Adding in apparent mass effects
235 if (Mass_appMass_ratio)
236 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
237 if (I_xx_appMass_ratio)
238 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
239 if (I_yy_appMass_ratio)
240 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
241 if (I_zz_appMass_ratio)
242 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
245 F_Z_aero += -Mass_appMass * W_dot_body;
247 M_l_aero += -I_xx_appMass * P_dot_body;
249 M_m_aero += -I_yy_appMass * Q_dot_body;
251 M_n_aero += -I_zz_appMass * R_dot_body;
253 // gyroscopic moments
254 // engineOmega is positive when rotation is ccw when viewed from the front
255 if (gyroForce_Q_body)
256 M_n_aero += polarInertia * engineOmega * Q_body;
257 if (gyroForce_R_body)
258 M_m_aero += -polarInertia * engineOmega * R_body;
260 // ornithopter support
263 // F_X_aero += F_X_aero_flapper;
264 // F_Z_aero += F_Z_aero_flapper;
265 // M_m_aero += flapper_Moment;
276 vis = fgGetDouble("/environment/visibility-m");
281 fgSetDouble("/environment/visibility-m", vis);
285 /* Send data on the network to the Glass Cockpit */
289 // input += " stick_right " + ftoa(Lat_control);
290 // input += " rudder_left " + ftoa(-Rudder_pedal);
291 // input += " stick_forward " + ftoa(Long_control);
292 // input += " stick_trim_forward " + ftoa(Long_trim);
293 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
294 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
295 // input += " vehicle_speed " + ftoa(V_rel_wind);
296 // input += " throttle_forward " + ftoa(Throttle_pct);
297 // input += " altitude " + ftoa(Altitude);
298 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
300 // testarray.getHello();
301 // testarray.sendData(input);
303 /* End of Networking */
307 void uiuc_engine_routine()
312 void uiuc_gear_routine ()
317 void uiuc_record_routine(double dt)
319 if (Simtime >= recordStartTime)
323 //void uiuc_network_routine()
325 // if (use_uiuc_network)
326 // uiuc_network(2); //send data
328 //end uiuc_wrapper.cpp