fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/FDM/UIUCModel/uiuc_wrapper.cpp

330 lines
10 KiB
C++
Raw Blame History

/**********************************************************************
FILENAME: uiuc_wrapper.cpp
----------------------------------------------------------------------
DESCRIPTION: A wrapper(interface) between the UIUC Aeromodel (C++ files)
and the LaRCsim FDM (C files)
----------------------------------------------------------------------
STATUS: alpha version
----------------------------------------------------------------------
REFERENCES:
----------------------------------------------------------------------
HISTORY: 01/26/2000 initial release
03/09/2001 (DPM) added support for gear
06/18/2001 (RD) Made uiuc_recorder its own routine.
07/19/2001 (RD) Added uiuc_vel_init() to initialize
the velocities.
08/27/2001 (RD) Added uiuc_initial_init() to help
in starting an A/C at an initial condition
02/24/2002 (GD) Added uiuc_network_routine()
03/27/2002 (RD) Changed how forces are calculated when
body-axis is used
----------------------------------------------------------------------
AUTHOR(S): Bipin Sehgal <bsehgal@uiuc.edu>
Robert Deters <rdeters@uiuc.edu>
Glen Dimock <dimock@uiuc.edu>
David Megginson <david@megginson.com>
----------------------------------------------------------------------
VARIABLES:
----------------------------------------------------------------------
INPUTS: *
----------------------------------------------------------------------
OUTPUTS: *
----------------------------------------------------------------------
CALLED BY: *
----------------------------------------------------------------------
CALLS TO: *
----------------------------------------------------------------------
COPYRIGHT: (C) 2000 by Michael Selig
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
USA or view http://www.gnu.org/copyleft/gpl.html.
**********************************************************************/
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include <simgear/compiler.h>
#include <simgear/misc/sg_path.hxx>
#include <Aircraft/aircraft.hxx>
#include <Main/fg_props.hxx>
#include "uiuc_aircraft.h"
#include "uiuc_aircraftdir.h"
#include "uiuc_coefficients.h"
#include "uiuc_engine.h"
#include "uiuc_gear.h"
#include "uiuc_aerodeflections.h"
#include "uiuc_recorder.h"
#include "uiuc_menu.h"
#include "uiuc_betaprobe.h"
#include <FDM/LaRCsim/ls_generic.h>
//#include "Main/simple_udp.h"
#include "uiuc_fog.h" //321654
//#include "uiuc_network.h"
//#include "uiuc_get_flapper.h"
#if !defined (SG_HAVE_NATIVE_SGI_COMPILERS)
SG_USING_STD(cout);
SG_USING_STD(endl);
#endif
extern "C" void uiuc_init_aeromodel ();
extern "C" void uiuc_force_moment(double dt);
extern "C" void uiuc_engine_routine();
extern "C" void uiuc_gear_routine();
extern "C" void uiuc_record_routine(double dt);
//extern "C" void uiuc_network_routine();
extern "C" void uiuc_vel_init ();
extern "C" void uiuc_initial_init ();
AIRCRAFT *aircraft_ = new AIRCRAFT;
AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
// SendArray testarray(4950);
/* Convert float to string */
//string ftoa(double in)
//{
// static char temp[20];
// sprintf(temp,"%g",in);
// return (string)temp;
//}
void uiuc_initial_init ()
{
if (P_body_init_true)
P_body = P_body_init;
if (Q_body_init_true)
Q_body = Q_body_init;
if (R_body_init_true)
R_body = R_body_init;
if (Phi_init_true)
Phi = Phi_init;
if (Theta_init_true)
Theta = Theta_init;
if (Psi_init_true)
Psi = Psi_init;
if (U_body_init_true)
U_body = U_body_init;
if (V_body_init_true)
V_body = V_body_init;
if (W_body_init_true)
W_body = W_body_init;
}
void uiuc_vel_init ()
{
if (U_body_init_true && V_body_init_true && W_body_init_true)
{
double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
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);
cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
}
}
void uiuc_init_aeromodel ()
{
SGPath path(globals->get_fg_root());
path.append(aircraft_dir);
path.append("aircraft.dat");
cout << "We are using "<< path.str() << endl;
uiuc_initializemaps(); // Initialize the <string,int> maps
uiuc_menu(path.str()); // Read the specified aircraft file
}
void uiuc_force_moment(double dt)
{
double qS = Dynamic_pressure * Sw;
double qScbar = qS * cbar;
double qSb = qS * bw;
uiuc_aerodeflections(dt);
uiuc_coefficients(dt);
//if (flapper_model)
// {
// uiuc_get_flapper(dt);
// }
/* Calculate the forces */
if (CX && CZ)
{
F_X_aero = CX * qS;
F_Y_aero = CY * qS;
F_Z_aero = CZ * qS;
}
else
{
// Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
// hence Cos_beta * Cos_beta term included.
// Same thing is done w/ moments below.
// Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
// that should not be the case. See FGFS notes 021105
F_X_wind = -CD * qS * Cos_beta * Cos_beta;
F_Y_wind = CY * qS;
F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
// F_X_wind = -CD * qS * Cos_beta * Cos_beta;
// F_Y_wind = CY * qS * Cos_beta * Cos_beta;
// F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
// wind-axis to body-axis transformation
F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
}
// Moment calculations
M_l_aero = Cl * qSb ;
M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
M_n_aero = Cn * qSb ;
// M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
// M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
// M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
// Adding in apparent mass effects
if (Mass_appMass_ratio)
F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
if (I_xx_appMass_ratio)
M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
if (I_yy_appMass_ratio)
M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
if (I_zz_appMass_ratio)
M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
if (Mass_appMass)
F_Z_aero += -Mass_appMass * W_dot_body;
if (I_xx_appMass)
M_l_aero += -I_xx_appMass * P_dot_body;
if (I_yy_appMass)
M_m_aero += -I_yy_appMass * Q_dot_body;
if (I_zz_appMass)
M_n_aero += -I_zz_appMass * R_dot_body;
// gyroscopic moments
// engineOmega is positive when rotation is ccw when viewed from the front
if (gyroForce_Q_body)
M_n_aero += polarInertia * engineOmega * Q_body;
if (gyroForce_R_body)
M_m_aero += -polarInertia * engineOmega * R_body;
// ornithopter support
//if (flapper_model)
// {
// F_X_aero += F_X_aero_flapper;
// F_Z_aero += F_Z_aero_flapper;
// M_m_aero += flapper_Moment;
// }
// fog field update
Fog = 0;
if (fog_field)
uiuc_fog();
double vis;
if (Fog != 0)
{
vis = fgGetDouble("/environment/visibility-m");
if (Fog > 0)
vis /= 1.01;
else
vis *= 1.01;
fgSetDouble("/environment/visibility-m", vis);
}
/* Send data on the network to the Glass Cockpit */
// string input="";
// input += " stick_right " + ftoa(Lat_control);
// input += " rudder_left " + ftoa(-Rudder_pedal);
// input += " stick_forward " + ftoa(Long_control);
// input += " stick_trim_forward " + ftoa(Long_trim);
// input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
// input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
// input += " vehicle_speed " + ftoa(V_rel_wind);
// input += " throttle_forward " + ftoa(Throttle_pct);
// input += " altitude " + ftoa(Altitude);
// input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
// testarray.getHello();
// testarray.sendData(input);
/* End of Networking */
}
void uiuc_engine_routine()
{
uiuc_engine();
}
void uiuc_gear_routine ()
{
uiuc_gear();
}
void uiuc_record_routine(double dt)
{
if (Simtime >= recordStartTime)
uiuc_recorder(dt);
}
//void uiuc_network_routine()
//{
// if (use_uiuc_network)
// uiuc_network(2); //send data
//}
//end uiuc_wrapper.cpp
View git blame Copy permalink