fly/flightgear
1
0
Fork 0
Code Activity

LaRCsim FDM - detabbing of all files.

Browse source 
All '\t' have been replaced with 8 spaces, as most of the code is indented with spaces.
This commit is contained in:
Edward d'Auvergne 2015-12-10 10:07:21 +01:00
parent dc04fc2294
commit 9b7db929f2
42 changed files with 3976 additions and 3976 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

74
src/FDM/LaRCsim/Cherokee.txt
View file

@ -1,42 +1,42 @@
I will explain assumptions I have made:
All global vars, such as Alpha, Alpha_dot, Density, Altitude, so on,
have correct values in the moment cherokee functions are called.
All global vars, such as Alpha, Alpha_dot, Density, Altitude, so on,
have correct values in the moment cherokee functions are called.
Body coord system is defined as follows:
X_body -> from cg to nose
Y_body -> along right wing
Z_body -> "down" (to make right coord system)
All forces and moments act in CG.
Body coord system is defined as follows:
X_body -> from cg to nose
Y_body -> along right wing
Z_body -> "down" (to make right coord system)
All forces and moments act in CG.
If strange behaviour is experienced, (like impossibility of level
flight), it is probably misused coord system. Let me know and fix
the error.
If strange behaviour is experienced, (like impossibility of level
flight), it is probably misused coord system. Let me know and fix
the error.
All controls are in the range [-1.0, 1.0] If it is not so, values in
cherokee_aero.c on lines 119-121 should be changed acordingly. If
commands appear to be oposite, this is the place to change sign
convention.
All controls are in the range [-1.0, 1.0] If it is not so, values in
cherokee_aero.c on lines 119-121 should be changed acordingly. If
commands appear to be oposite, this is the place to change sign
convention.
Engine controls are in range [0.0, 1.0] (I found out later that lower
bound is -0.2, so I added fabs(). I know, it is dirty and phisiclly
wrong, but it was simply the fastest way to deal with it).
Engine controls are in range [0.0, 1.0] (I found out later that lower
bound is -0.2, so I added fabs(). I know, it is dirty and phisiclly
wrong, but it was simply the fastest way to deal with it).
All initialization files are *.ic:
mass = 74.53 slugs
Ixx = 1070 slugs
Iyy = 1249 slugs
Izz = 2312 slugs
Ixz = 0.0 slugs
above changes are writen in *.ic included in cherokee.zip. However,
other data in *.ic files are not changed in any way.
All initialization files are *.ic:
mass = 74.53 slugs
Ixx = 1070 slugs
Iyy = 1249 slugs
Izz = 2312 slugs
Ixz = 0.0 slugs
above changes are writen in *.ic included in cherokee.zip. However,
other data in *.ic files are not changed in any way.
aditional properties: (if needed)
S = 157.5 ft^2 -> wing area
b = 30.0 ft -> wing span
Ar = 5.71 -> aspect ratio
c = 5.25 ft -> midspan chord
aditional properties: (if needed)
S = 157.5 ft^2 -> wing area
b = 30.0 ft -> wing span
Ar = 5.71 -> aspect ratio
c = 5.25 ft -> midspan chord
Once more: Source are TOTALY UNCHECKED. I hope it will work, but I do not
@ -50,11 +50,11 @@ gsikic@public.srce.hr
PS
Work to be done (for myself):
Landing gear (it is just navion_gear.c copied for now, these are similar
class of aircraft, so it should work),
Alpha_max is still undone,
Spin (if I find any references concerning simulating spin),
Efect of ground,
Flaps,
.......
Landing gear (it is just navion_gear.c copied for now, these are similar
class of aircraft, so it should work),
Alpha_max is still undone,
Spin (if I find any references concerning simulating spin),
Efect of ground,
Flaps,
.......

282
src/FDM/LaRCsim/IO360.cxx
View file

@ -45,14 +45,14 @@ void FGNewEngine::init(double dt) {
CONVERT_HP_TO_WATTS = 745.6999;
// Properties of working fluids
Cp_air = 1005; // J/KgK
Cp_fuel = 1700; // J/KgK
Cp_air = 1005; // J/KgK
Cp_fuel = 1700; // J/KgK
calorific_value_fuel = 47.3e6; // W/Kg Note that this is only an approximate value
rho_fuel = 800; // kg/m^3 - an estimate for now
rho_fuel = 800; // kg/m^3 - an estimate for now
R_air = 287.3;
// environment inputs
p_amb_sea_level = 101325; // Pascals
p_amb_sea_level = 101325; // Pascals
// Control inputs - ARE THESE NEEDED HERE???
Throttle_Lever_Pos = 75;
@ -67,7 +67,7 @@ void FGNewEngine::init(double dt) {
MaxHP = 200; //Lycoming IO360 -A-C-D series
// MaxHP = 180; //Current Lycoming IO360 ?
// displacement = 520; //Continental IO520-M
displacement = 360; //Lycoming IO360
displacement = 360; //Lycoming IO360
displacement_SI = displacement * CONVERT_CUBIC_INCHES_TO_METERS_CUBED;
engine_inertia = 0.2; //kgm^2 - value taken from a popular family saloon car engine - need to find an aeroengine value !!!!!
prop_inertia = 0.05; //kgm^2 - this value is a total guess - dcl
@ -77,7 +77,7 @@ void FGNewEngine::init(double dt) {
Max_Manifold_Pressure = 28.50; //Inches Hg. An approximation - should be able to find it in the engine performance data
Min_Manifold_Pressure = 6.5; //Inches Hg. This is a guess corresponding to approx 0.24 bar MAP (7 in Hg) - need to find some proper data for this
Max_RPM = 2700;
Min_RPM = 600; //Recommended idle from Continental data sheet
Min_RPM = 600; //Recommended idle from Continental data sheet
Mag_Derate_Percent = 5;
Gear_Ratio = 1;
n_R = 2; // Number of crank revolutions per power cycle - 2 for a 4 stroke engine.
@ -90,20 +90,20 @@ void FGNewEngine::init(double dt) {
// Initialise Engine Variables used by this instance
if(running)
RPM = 600;
RPM = 600;
else
RPM = 0;
RPM = 0;
Percentage_Power = 0;
Manifold_Pressure = 29.96; // Inches
Fuel_Flow_gals_hr = 0;
// Torque = 0;
Torque_SI = 0;
CHT = 298.0; //deg Kelvin
CHT_degF = (CHT * 1.8) - 459.67; //deg Fahrenheit
CHT = 298.0; //deg Kelvin
CHT_degF = (CHT * 1.8) - 459.67; //deg Fahrenheit
Mixture = 14;
Oil_Pressure = 0; // PSI
Oil_Temp = 85; // Deg C
current_oil_temp = 298.0; //deg Kelvin
Oil_Pressure = 0; // PSI
Oil_Temp = 85; // Deg C
current_oil_temp = 298.0; //deg Kelvin
/**** one of these is superfluous !!!!***/
HP = 0;
RPS = 0;
@ -124,24 +124,24 @@ void FGNewEngine::update() {
// Hack for testing - should output every 5 seconds
static int count1 = 0;
if(count1 == 0) {
// cout << "P_atmos = " << p_amb << " T_atmos = " << T_amb << '\n';
// cout << "Manifold pressure = " << Manifold_Pressure << " True_Manifold_Pressure = " << True_Manifold_Pressure << '\n';
// cout << "p_amb_sea_level = " << p_amb_sea_level << '\n';
// cout << "equivalence_ratio = " << equivalence_ratio << '\n';
// cout << "combustion_efficiency = " << combustion_efficiency << '\n';
// cout << "AFR = " << 14.7 / equivalence_ratio << '\n';
// cout << "Mixture lever = " << Mixture_Lever_Pos << '\n';
// cout << "n = " << RPM << " rpm\n";
// cout << "P_atmos = " << p_amb << " T_atmos = " << T_amb << '\n';
// cout << "Manifold pressure = " << Manifold_Pressure << " True_Manifold_Pressure = " << True_Manifold_Pressure << '\n';
// cout << "p_amb_sea_level = " << p_amb_sea_level << '\n';
// cout << "equivalence_ratio = " << equivalence_ratio << '\n';
// cout << "combustion_efficiency = " << combustion_efficiency << '\n';
// cout << "AFR = " << 14.7 / equivalence_ratio << '\n';
// cout << "Mixture lever = " << Mixture_Lever_Pos << '\n';
// cout << "n = " << RPM << " rpm\n";
// cout << "T_amb = " << T_amb << '\n';
// cout << "running = " << running << '\n';
// cout << "fuel = " << fgGetFloat("/consumables/fuel/tank[0]/level-gal_us") << '\n';
// cout << "Percentage_Power = " << Percentage_Power << '\n';
// cout << "current_oil_temp = " << current_oil_temp << '\n';
// cout << "EGT = " << EGT << '\n';
// cout << "running = " << running << '\n';
// cout << "fuel = " << fgGetFloat("/consumables/fuel/tank[0]/level-gal_us") << '\n';
// cout << "Percentage_Power = " << Percentage_Power << '\n';
// cout << "current_oil_temp = " << current_oil_temp << '\n';
// cout << "EGT = " << EGT << '\n';
}
count1++;
if(count1 == 100)
count1 = 0;
count1 = 0;
*/
// Check parameters that may alter the operating state of the engine.
@ -156,59 +156,59 @@ void FGNewEngine::update() {
// 2 -> right only
// 3 -> both
if(mag_pos != 0) {
spark = true;
spark = true;
} else {
spark = false;
spark = false;
} // neglects battery voltage, master on switch, etc for now.
if((mag_pos == 1) || (mag_pos > 2))
Magneto_Left = true;
Magneto_Left = true;
if(mag_pos > 1)
Magneto_Right = true;
Magneto_Right = true;
// crude check for fuel
if((fgGetFloat("/consumables/fuel/tank[0]/level-gal_us") > 0) || (fgGetFloat("/consumables/fuel/tank[1]/level-gal_us") > 0)) {
fuel = true;
fuel = true;
} else {
fuel = false;
fuel = false;
} // Need to make this better, eg position of fuel selector switch.
// Check if we are turning the starter motor
if(cranking != starter) {
// This check saves .../cranking from getting updated every loop - they only update when changed.
cranking = starter;
crank_counter = 0;
// This check saves .../cranking from getting updated every loop - they only update when changed.
cranking = starter;
crank_counter = 0;
}
// Note that although /engines/engine[0]/starter and /engines/engine[0]/cranking might appear to be duplication it is
// not since the starter may be engaged with the battery voltage too low for cranking to occur (or perhaps the master
// switch just left off) and the sound manager will read .../cranking to determine wether to play a cranking sound.
// For now though none of that is implemented so cranking can be set equal to .../starter without further checks.
// int Alternate_Air_Pos =0; // Off = 0. Reduces power by 3 % for same throttle setting
// int Alternate_Air_Pos =0; // Off = 0. Reduces power by 3 % for same throttle setting
// DCL - don't know what this Alternate_Air_Pos is - this is a leftover from the Schubert code.
//Check mode of engine operation
if(cranking) {
crank_counter++;
if(RPM <= 480) {
RPM += 100;
if(RPM > 480)
RPM = 480;
} else {
// consider making a horrible noise if the starter is engaged with the engine running
}
crank_counter++;
if(RPM <= 480) {
RPM += 100;
if(RPM > 480)
RPM = 480;
} else {
// consider making a horrible noise if the starter is engaged with the engine running
}
}
if((!running) && (spark) && (fuel) && (crank_counter > 120)) {
// start the engine if revs high enough
if(RPM > 450) {
// For now just instantaneously start but later we should maybe crank for a bit
running = true;
// RPM = 600;
}
// start the engine if revs high enough
if(RPM > 450) {
// For now just instantaneously start but later we should maybe crank for a bit
running = true;
// RPM = 600;
}
}
if( (running) && ((!spark)||(!fuel)) ) {
// Cut the engine
// note that we only cut the power - the engine may continue to spin if the prop is in a moving airstream
running = false;
// Cut the engine
// note that we only cut the power - the engine may continue to spin if the prop is in a moving airstream
running = false;
}
// Now we've ascertained whether the engine is running or not we can start to do the engine calculations 'proper'
@ -250,14 +250,14 @@ void FGNewEngine::update() {
}
else {
Torque_SI = ((Power_SI * 60.0) / (2.0 * LS_PI * RPM)) - Torque_FMEP; //Torque = power / angular velocity
// cout << Torque << " Nm\n";
// cout << Torque << " Nm\n";
}
//Calculate Exhaust gas temperature
if(running)
Calc_EGT();
Calc_EGT();
else
EGT = 298.0;
EGT = 298.0;
// Calculate Cylinder Head Temperature
Calc_CHT();
@ -286,14 +286,14 @@ void FGNewEngine::update() {
// And finally a last check on the engine state after doing the torque balance with the prop - have we stalled?
if(running) {
//Check if we have stalled the engine
if (RPM == 0) {
running = false;
} else if((RPM <= 480) && (cranking)) {
//Make sure the engine noise dosn't play if the engine won't start due to eg mixture lever pulled out.
running = false;
EGT = 298.0;
}
//Check if we have stalled the engine
if (RPM == 0) {
running = false;
} else if((RPM <= 480) && (cranking)) {
//Make sure the engine noise dosn't play if the engine won't start due to eg mixture lever pulled out.
running = false;
EGT = 298.0;
}
}
// And finally, do any unit conversions from internal units to output units
@ -324,22 +324,22 @@ float FGNewEngine::Lookup_Combustion_Efficiency(float thi_actual)
for(i=0;i<j;i++)
{
if(i == (j-1)) {
// Assume linear extrapolation of the slope between the last two points beyond the last point
float dydx = (neta_comb[i] - neta_comb[i-1]) / (thi[i] - thi[i-1]);
neta_comb_actual = neta_comb[i] + dydx * (thi_actual - thi[i]);
return neta_comb_actual;
}
if(thi_actual == thi[i]) {
neta_comb_actual = neta_comb[i];
return neta_comb_actual;
}
if((thi_actual > thi[i]) && (thi_actual < thi[i + 1])) {
//do linear interpolation between the two points
factor = (thi_actual - thi[i]) / (thi[i+1] - thi[i]);
neta_comb_actual = (factor * (neta_comb[i+1] - neta_comb[i])) + neta_comb[i];
return neta_comb_actual;
}
if(i == (j-1)) {
// Assume linear extrapolation of the slope between the last two points beyond the last point
float dydx = (neta_comb[i] - neta_comb[i-1]) / (thi[i] - thi[i-1]);
neta_comb_actual = neta_comb[i] + dydx * (thi_actual - thi[i]);
return neta_comb_actual;
}
if(thi_actual == thi[i]) {
neta_comb_actual = neta_comb[i];
return neta_comb_actual;
}
if((thi_actual > thi[i]) && (thi_actual < thi[i + 1])) {
//do linear interpolation between the two points
factor = (thi_actual - thi[i]) / (thi[i+1] - thi[i]);
neta_comb_actual = (factor * (neta_comb[i+1] - neta_comb[i])) + neta_comb[i];
return neta_comb_actual;
}
}
//if we get here something has gone badly wrong
@ -372,28 +372,28 @@ float FGNewEngine::Power_Mixture_Correlation(float thi_actual)
for(i=0;i<j;i++)
{
if(i == (j-1)) {
// Assume linear extrapolation of the slope between the last two points beyond the last point
dydx = (mixPerPow[i] - mixPerPow[i-1]) / (AFR[i] - AFR[i-1]);
mixPerPow_actual = mixPerPow[i] + dydx * (AFR_actual - AFR[i]);
return mixPerPow_actual;
}
if((i == 0) && (AFR_actual < AFR[i])) {
// Assume linear extrapolation of the slope between the first two points for points before the first point
dydx = (mixPerPow[1] - mixPerPow[0]) / (AFR[1] - AFR[0]);
mixPerPow_actual = mixPerPow[0] + dydx * (AFR_actual - AFR[0]);
return mixPerPow_actual;
}
if(AFR_actual == AFR[i]) {
mixPerPow_actual = mixPerPow[i];
return mixPerPow_actual;
}
if((AFR_actual > AFR[i]) && (AFR_actual < AFR[i + 1])) {
//do linear interpolation between the two points
factor = (AFR_actual - AFR[i]) / (AFR[i+1] - AFR[i]);
mixPerPow_actual = (factor * (mixPerPow[i+1] - mixPerPow[i])) + mixPerPow[i];
return mixPerPow_actual;
}
if(i == (j-1)) {
// Assume linear extrapolation of the slope between the last two points beyond the last point
dydx = (mixPerPow[i] - mixPerPow[i-1]) / (AFR[i] - AFR[i-1]);
mixPerPow_actual = mixPerPow[i] + dydx * (AFR_actual - AFR[i]);
return mixPerPow_actual;
}
if((i == 0) && (AFR_actual < AFR[i])) {
// Assume linear extrapolation of the slope between the first two points for points before the first point
dydx = (mixPerPow[1] - mixPerPow[0]) / (AFR[1] - AFR[0]);
mixPerPow_actual = mixPerPow[0] + dydx * (AFR_actual - AFR[0]);
return mixPerPow_actual;
}
if(AFR_actual == AFR[i]) {
mixPerPow_actual = mixPerPow[i];
return mixPerPow_actual;
}
if((AFR_actual > AFR[i]) && (AFR_actual < AFR[i + 1])) {
//do linear interpolation between the two points
factor = (AFR_actual - AFR[i]) / (AFR[i+1] - AFR[i]);
mixPerPow_actual = (factor * (mixPerPow[i+1] - mixPerPow[i])) + mixPerPow[i];
return mixPerPow_actual;
}
}
//if we get here something has gone badly wrong
@ -408,16 +408,16 @@ void FGNewEngine::Calc_CHT()
{
float h1 = -95.0; //co-efficient for free convection
float h2 = -3.95; //co-efficient for forced convection
float h3 = -0.05; //co-efficient for forced convection due to prop backwash
float v_apparent; //air velocity over cylinder head in m/s
float h3 = -0.05; //co-efficient for forced convection due to prop backwash
float v_apparent; //air velocity over cylinder head in m/s
float v_dot_cooling_air;
float m_dot_cooling_air;
float temperature_difference;
float arbitary_area = 1.0;
float dqdt_from_combustion;
float dqdt_forced; //Rate of energy transfer to/from cylinder head due to forced convection (Joules) (sign convention: to cylinder head is +ve)
float dqdt_free; //Rate of energy transfer to/from cylinder head due to free convection (Joules) (sign convention: to cylinder head is +ve)
float dqdt_cylinder_head; //Overall energy change in cylinder head
float dqdt_forced; //Rate of energy transfer to/from cylinder head due to forced convection (Joules) (sign convention: to cylinder head is +ve)
float dqdt_free; //Rate of energy transfer to/from cylinder head due to free convection (Joules) (sign convention: to cylinder head is +ve)
float dqdt_cylinder_head; //Overall energy change in cylinder head
float CpCylinderHead = 800.0; //FIXME - this is a guess - I need to look up the correct value
float MassCylinderHead = 8.0; //Kg - this is a guess - it dosn't have to be absolutely accurate but can be varied to alter the warm-up rate
float HeatCapacityCylinderHead;
@ -484,11 +484,11 @@ float FGNewEngine::Calc_Manifold_Pressure ( float LeverPosn, float MaxMan, float
//allow for idle bypass valve or slightly open throttle stop
if(Inches < MinMan)
Inches = MinMan;
Inches = MinMan;
//Check for stopped engine (crudest way of compensating for engine speed)
if(RPM == 0)
Inches = 29.92;
Inches = 29.92;
return Inches;
}
@ -566,14 +566,14 @@ void FGNewEngine::Calc_Percentage_Power(bool mag_left, bool mag_right)
// Now Derate engine for the effects of Bad/Switched off magnetos
//if (Magneto_Left == 0 && Magneto_Right == 0) {
if(!running) {
// cout << "Both OFF\n";
Percentage_Power = 0;
// cout << "Both OFF\n";
Percentage_Power = 0;
} else if (mag_left && mag_right) {
// cout << "Both On ";
// cout << "Both On ";
} else if (mag_left == 0 || mag_right== 0) {
// cout << "1 Magneto Failed ";
Percentage_Power = Percentage_Power * ((100.0 - Mag_Derate_Percent)/100.0);
// cout << FGEng1_Percentage_Power << "%" << "\t";
// cout << "1 Magneto Failed ";
Percentage_Power = Percentage_Power * ((100.0 - Mag_Derate_Percent)/100.0);
// cout << FGEng1_Percentage_Power << "%" << "\t";
}
/*
//DCL - stall the engine if RPM drops below 450 - this is possible if mixture lever is pulled right out
@ -582,24 +582,24 @@ void FGNewEngine::Calc_Percentage_Power(bool mag_left, bool mag_right)
Percentage_Power = 0;
*/
if(Percentage_Power < 0)
Percentage_Power = 0;
Percentage_Power = 0;
}
// Calculate Oil Temperature in degrees Kelvin
float FGNewEngine::Calc_Oil_Temp (float oil_temp)
{
float idle_percentage_power = 2.3; // approximately
float target_oil_temp; // Steady state oil temp at the current engine conditions
float time_constant; // The time constant for the differential equation
float idle_percentage_power = 2.3; // approximately
float target_oil_temp; // Steady state oil temp at the current engine conditions
float time_constant; // The time constant for the differential equation
if(running) {
target_oil_temp = 363;
time_constant = 500; // Time constant for engine-on idling.
if(Percentage_Power > idle_percentage_power) {
time_constant /= ((Percentage_Power / idle_percentage_power) / 10.0); // adjust for power
}
target_oil_temp = 363;
time_constant = 500; // Time constant for engine-on idling.
if(Percentage_Power > idle_percentage_power) {
time_constant /= ((Percentage_Power / idle_percentage_power) / 10.0); // adjust for power
}
} else {
target_oil_temp = 298;
time_constant = 1000; // Time constant for engine-off; reflects the fact that oil is no longer getting circulated
target_oil_temp = 298;
time_constant = 1000; // Time constant for engine-off; reflects the fact that oil is no longer getting circulated
}
float dOilTempdt = (target_oil_temp - oil_temp) / time_constant;
@ -612,18 +612,18 @@ float FGNewEngine::Calc_Oil_Temp (float oil_temp)
// Calculate Oil Pressure
float FGNewEngine::Calc_Oil_Press (float Oil_Temp, float Engine_RPM)
{
float Oil_Pressure = 0; //PSI
float Oil_Press_Relief_Valve = 60; //PSI
float Oil_Pressure = 0; //PSI
float Oil_Press_Relief_Valve = 60; //PSI
float Oil_Press_RPM_Max = 1800;
float Design_Oil_Temp = 85; //Celsius
float Oil_Viscosity_Index = 0.25; // PSI/Deg C
// float Temp_Deviation = 0; // Deg C
float Design_Oil_Temp = 85; //Celsius
float Oil_Viscosity_Index = 0.25; // PSI/Deg C
// float Temp_Deviation = 0; // Deg C
Oil_Pressure = (Oil_Press_Relief_Valve / Oil_Press_RPM_Max) * Engine_RPM;
// Pressure relief valve opens at Oil_Press_Relief_Valve PSI setting
if (Oil_Pressure >= Oil_Press_Relief_Valve) {
Oil_Pressure = Oil_Press_Relief_Valve;
Oil_Pressure = Oil_Press_Relief_Valve;
}
// Now adjust pressure according to Temp which affects the viscosity
@ -640,7 +640,7 @@ void FGNewEngine::Do_Prop_Calcs()
float Gear_Ratio = 1.0;
float forward_velocity; // m/s
float prop_power_consumed_SI; // Watts
double J; // advance ratio - dimensionless
double J; // advance ratio - dimensionless
double Cp_20; // coefficient of power for 20 degree blade angle
double Cp_25; // coefficient of power for 25 degree blade angle
double Cp; // Our actual coefficient of power
@ -675,8 +675,8 @@ void FGNewEngine::Do_Prop_Calcs()
//cout << "prop HP consumed = " << prop_power_consumed_SI / 745.699 << '\n';
if(angular_velocity_SI == 0)
prop_torque = 0;
// However this can give problems - if rpm == 0 but forward velocity increases the prop should be able to generate a torque to start the engine spinning
// Unlikely to happen in practice - but I suppose someone could move the lever of a stopped large piston engine from feathered to windmilling.
// However this can give problems - if rpm == 0 but forward velocity increases the prop should be able to generate a torque to start the engine spinning
// Unlikely to happen in practice - but I suppose someone could move the lever of a stopped large piston engine from feathered to windmilling.
// This *does* give problems - if the plane is put into a steep climb whilst windmilling the engine friction will eventually stop it spinning.
// When put back into a dive it never starts re-spinning again. Although it is unlikely that anyone would do this in real life, they might well do it in a sim!!!
else
@ -690,11 +690,11 @@ void FGNewEngine::Do_Prop_Calcs()
// Check for zero forward velocity to avoid divide by zero
if(forward_velocity < 0.0001)
prop_thrust = 0.0;
// I don't see how this works - how can the plane possibly start from rest ???
// Hmmmm - it works because the forward_velocity at present never drops below about 0.03 even at rest
// We can't really rely on this in the future - needs fixing !!!!
// The problem is that we're doing this calculation backwards - we're working out the thrust from the power consumed and the velocity, which becomes invalid as velocity goes to zero.
// It would be far more natural to work out the power consumed from the thrust - FIXME!!!!!.
// I don't see how this works - how can the plane possibly start from rest ???
// Hmmmm - it works because the forward_velocity at present never drops below about 0.03 even at rest
// We can't really rely on this in the future - needs fixing !!!!
// The problem is that we're doing this calculation backwards - we're working out the thrust from the power consumed and the velocity, which becomes invalid as velocity goes to zero.
// It would be far more natural to work out the power consumed from the thrust - FIXME!!!!!.
else
prop_thrust = neta_prop * prop_power_consumed_SI / forward_velocity; //TODO - rename forward_velocity to IAS_SI
//cout << "prop_thrust = " << prop_thrust << '\n';

138
src/FDM/LaRCsim/IO360.hxx
View file

@ -36,22 +36,22 @@ private:
float CONVERT_HP_TO_WATTS;
// Properties of working fluids
float Cp_air; // J/KgK
float Cp_fuel; // J/KgK
float Cp_air; // J/KgK
float Cp_fuel; // J/KgK
float calorific_value_fuel; // W/Kg
float rho_fuel; // kg/m^3
float rho_air; // kg/m^3
float rho_fuel; // kg/m^3
float rho_air; // kg/m^3
// environment inputs
float p_amb_sea_level; // Sea level ambient pressure in Pascals
float p_amb; // Ambient pressure at current altitude in Pascals
float T_amb; // ditto deg Kelvin
float p_amb_sea_level; // Sea level ambient pressure in Pascals
float p_amb; // Ambient pressure at current altitude in Pascals
float T_amb; // ditto deg Kelvin
// Control inputs
float Throttle_Lever_Pos; // 0 = Closed, 100 = Fully Open
float Propeller_Lever_Pos; // 0 = Full Course 100 = Full Fine
float Mixture_Lever_Pos; // 0 = Idle Cut Off 100 = Full Rich
int mag_pos; // 0=off, 1=left, 2=right, 3=both.
float Throttle_Lever_Pos; // 0 = Closed, 100 = Fully Open
float Propeller_Lever_Pos; // 0 = Full Course 100 = Full Fine
float Mixture_Lever_Pos; // 0 = Idle Cut Off 100 = Full Rich
int mag_pos; // 0=off, 1=left, 2=right, 3=both.
bool starter;
//misc
@ -59,75 +59,75 @@ private:
double time_step;
// Engine Specific Variables that should be read in from a config file
float MaxHP; // Horsepower
float displacement; // Cubic inches
float displacement_SI; //m^3 (derived from above rather than read in)
float engine_inertia; //kg.m^2
float prop_inertia; //kg.m^2
float Max_Fuel_Flow; // Units??? Do we need this variable any more??
float MaxHP; // Horsepower
float displacement; // Cubic inches
float displacement_SI; //m^3 (derived from above rather than read in)
float engine_inertia; //kg.m^2
float prop_inertia; //kg.m^2
float Max_Fuel_Flow; // Units??? Do we need this variable any more??
// Engine specific variables that maybe should be read in from config but are pretty generic and won't vary much for a naturally aspirated piston engine.
float Max_Manifold_Pressure; // inches Hg - typical manifold pressure at full throttle and typical max rpm
float Min_Manifold_Pressure; // inches Hg - typical manifold pressure at idle (closed throttle)
float Max_RPM; // rpm - this is really a bit of a hack and could be make redundant if the prop model works properly and takes tips at speed of sound into account.
float Min_RPM; // rpm - possibly redundant ???
float Mag_Derate_Percent; // Percentage reduction in power when mags are switched from 'both' to either 'L' or 'R'
float Gear_Ratio; // Gearing between engine and propellor
float Max_RPM; // rpm - this is really a bit of a hack and could be make redundant if the prop model works properly and takes tips at speed of sound into account.
float Min_RPM; // rpm - possibly redundant ???
float Mag_Derate_Percent; // Percentage reduction in power when mags are switched from 'both' to either 'L' or 'R'
float Gear_Ratio; // Gearing between engine and propellor
float n_R; // Number of cycles per power stroke - 2 for a 4 stroke engine.
// Engine Variables not read in from config file
float RPM; // rpm
float Percentage_Power; // Power output as percentage of maximum power output
float Manifold_Pressure; // Inches Hg
float Fuel_Flow_gals_hr; // USgals/hour
float Torque_lbft; // lb-ft
float Torque_SI; // Nm
float CHT; // Cylinder head temperature deg K
float CHT_degF; // Ditto in deg Fahrenheit
float RPM; // rpm
float Percentage_Power; // Power output as percentage of maximum power output
float Manifold_Pressure; // Inches Hg
float Fuel_Flow_gals_hr; // USgals/hour
float Torque_lbft; // lb-ft
float Torque_SI; // Nm
float CHT; // Cylinder head temperature deg K
float CHT_degF; // Ditto in deg Fahrenheit
float Mixture;
float Oil_Pressure; // PSI
float Oil_Temp; // Deg C
float current_oil_temp; // deg K
float Oil_Pressure; // PSI
float Oil_Temp; // Deg C
float current_oil_temp; // deg K
/**** one of these is superfluous !!!!***/
float HP; // Current power output in HP
float Power_SI; // Current power output in Watts
float HP; // Current power output in HP
float Power_SI; // Current power output in Watts
float RPS;
float angular_velocity_SI; // rad/s
float Torque_FMEP; // The component of Engine torque due to FMEP (Nm)
float Torque_Imbalance; // difference between engine and prop torque
float EGT; // Exhaust gas temperature deg K
float EGT_degF; // Exhaust gas temperature deg Fahrenheit
float Torque_Imbalance; // difference between engine and prop torque
float EGT; // Exhaust gas temperature deg K
float EGT_degF; // Exhaust gas temperature deg Fahrenheit
float volumetric_efficiency;
float combustion_efficiency;
float equivalence_ratio; // ratio of stoichiometric AFR over actual AFR
float thi_sea_level; // the equivalence ratio we would be running at assuming sea level air denisity in the manifold
float v_dot_air; // volume flow rate of air into engine - m^3/s
float m_dot_air; // mass flow rate of air into engine - kg/s
float m_dot_fuel; // mass flow rate of fuel into engine - kg/s
float swept_volume; // total engine swept volume - m^3
float equivalence_ratio; // ratio of stoichiometric AFR over actual AFR
float thi_sea_level; // the equivalence ratio we would be running at assuming sea level air denisity in the manifold
float v_dot_air; // volume flow rate of air into engine - m^3/s
float m_dot_air; // mass flow rate of air into engine - kg/s
float m_dot_fuel; // mass flow rate of fuel into engine - kg/s
float swept_volume; // total engine swept volume - m^3
/********* swept volume or the geometry used to calculate it should be in the config read section surely ??? ******/
float True_Manifold_Pressure; //in Hg - actual manifold gauge pressure
float rho_air_manifold; // denisty of air in the manifold - kg/m^3
float R_air; // Gas constant of air (287) UNITS???
float delta_T_exhaust; // Temperature change of exhaust this time step - degK
float rho_air_manifold; // denisty of air in the manifold - kg/m^3
float R_air; // Gas constant of air (287) UNITS???
float delta_T_exhaust; // Temperature change of exhaust this time step - degK
float heat_capacity_exhaust; // exhaust gas specific heat capacity - J/kgK
float enthalpy_exhaust; // Enthalpy at current exhaust gas conditions - UNITS???
float enthalpy_exhaust; // Enthalpy at current exhaust gas conditions - UNITS???
float Percentage_of_best_power_mixture_power; // Current power as a percentage of what power we would have at the same conditions but at best power mixture
float abstract_mixture; //temporary hack
float angular_acceleration; //rad/s^2
float abstract_mixture; //temporary hack
float angular_acceleration; //rad/s^2
float FMEP; //Friction Mean Effective Pressure (Pa)
// Various bits of housekeeping describing the engines state.
bool running; // flag to indicate the engine is running self sustaining
bool cranking; // flag to indicate the engine is being cranked
int crank_counter; // Number of iterations that the engine has been cranked non-stop
bool spark; // flag to indicate a spark is available
bool fuel; // flag to indicate fuel is available
bool running; // flag to indicate the engine is running self sustaining
bool cranking; // flag to indicate the engine is being cranked
int crank_counter; // Number of iterations that the engine has been cranked non-stop
bool spark; // flag to indicate a spark is available
bool fuel; // flag to indicate fuel is available
// Propellor Variables
float FGProp1_RPS; // rps
float prop_torque; // Nm
float prop_thrust; // Newtons
float FGProp1_RPS; // rps
float prop_torque; // Nm
float prop_thrust; // Newtons
double prop_diameter; // meters
double blade_angle; // degrees
@ -177,12 +177,12 @@ public:
//constructor
FGNewEngine() {
// outfile.open("FGNewEngine.dat", ios::out|ios::trunc);
// outfile.open("FGNewEngine.dat", ios::out|ios::trunc);
}
//destructor
~FGNewEngine() {
// outfile.close();
// outfile.close();
}
// set initial default values
@ -193,30 +193,30 @@ public:
inline void set_IAS( float value ) { IAS = value; }
inline void set_Throttle_Lever_Pos( float value ) {
Throttle_Lever_Pos = value;
Throttle_Lever_Pos = value;
}
inline void set_Propeller_Lever_Pos( float value ) {
Propeller_Lever_Pos = value;
Propeller_Lever_Pos = value;
}
inline void set_Mixture_Lever_Pos( float value ) {
Mixture_Lever_Pos = value;
Mixture_Lever_Pos = value;
}
// set the magneto switch position
inline void set_Magneto_Switch_Pos( int value ) {
mag_pos = value;
mag_pos = value;
}
inline void setStarterFlag( bool flag ) {
starter = flag;
starter = flag;
}
// set ambient pressure - takes pounds per square foot
inline void set_p_amb( float value ) {
p_amb = value * 47.88026;
// Convert to Pascals
p_amb = value * 47.88026;
// Convert to Pascals
}
// set ambient temperature - takes degrees Rankine
inline void set_T_amb( float value ) {
T_amb = value * 0.555555555556;
// Convert to degrees Kelvin
T_amb = value * 0.555555555556;
// Convert to degrees Kelvin
}
// accessors
@ -225,7 +225,7 @@ public:
// inline float get_Rho() const { return Rho; }
inline float get_MaxHP() const { return MaxHP; }
inline float get_Percentage_Power() const { return Percentage_Power; }
inline float get_EGT() const { return EGT_degF; } // Returns EGT in Fahrenheit
inline float get_EGT() const { return EGT_degF; } // Returns EGT in Fahrenheit
inline float get_CHT() const { return CHT_degF; } // Note this returns CHT in Fahrenheit
inline float get_prop_thrust_SI() const { return prop_thrust; }
inline float get_prop_thrust_lbs() const { return (prop_thrust * 0.2248); }

414
src/FDM/LaRCsim/LaRCsim.cxx
View file

@ -25,7 +25,7 @@
#endif
#include <math.h>
#include <string.h> // strcmp()
#include <string.h> // strcmp()
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
@ -74,16 +74,16 @@ FGLaRCsim::FGLaRCsim( double dt ) {
copy_to_LaRCsim(); // initialize all of LaRCsim's vars
//this should go away someday -- formerly done in fg_init.cxx
Mass = 2./32.174;
I_xx = 0.0454;
I_yy = 0.0191;
I_zz = 0.0721;
I_xz = 0;
// Mass = 8.547270E+01;
// I_xx = 1.048000E+03;
// I_yy = 3.000000E+03;
// I_zz = 3.530000E+03;
// I_xz = 0.000000E+00;
Mass = 2./32.174;
I_xx = 0.0454;
I_yy = 0.0191;
I_zz = 0.0721;
I_xz = 0;
// Mass = 8.547270E+01;
// I_xx = 1.048000E+03;
// I_yy = 3.000000E+03;
// I_zz = 3.530000E+03;
// I_xz = 0.000000E+00;
}
ls_set_model_dt(dt);
@ -121,142 +121,142 @@ void FGLaRCsim::update( double dt ) {
if ( !strcmp(aero->getStringValue(), "c172") ) {
// set control inputs
// cout << "V_calibrated_kts = " << V_calibrated_kts << '\n';
eng.set_IAS( V_calibrated_kts );
eng.set_Throttle_Lever_Pos( globals->get_controls()->get_throttle( 0 )
* 100.0 );
eng.set_Propeller_Lever_Pos( 100 );
eng.set_IAS( V_calibrated_kts );
eng.set_Throttle_Lever_Pos( globals->get_controls()->get_throttle( 0 )
* 100.0 );
eng.set_Propeller_Lever_Pos( 100 );
eng.set_Mixture_Lever_Pos( globals->get_controls()->get_mixture( 0 )
* 100.0 );
eng.set_Magneto_Switch_Pos( globals->get_controls()->get_magnetos(0) );
eng.setStarterFlag( globals->get_controls()->get_starter(0) );
eng.set_p_amb( Static_pressure );
eng.set_T_amb( Static_temperature );
* 100.0 );
eng.set_Magneto_Switch_Pos( globals->get_controls()->get_magnetos(0) );
eng.setStarterFlag( globals->get_controls()->get_starter(0) );
eng.set_p_amb( Static_pressure );
eng.set_T_amb( Static_temperature );
// update engine model
eng.update();
// update engine model
eng.update();
// Fake control-surface positions
fgSetDouble("/surface-positions/flap-pos-norm",
fgGetDouble("/controls/flight/flaps"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/elevator-pos-norm",
fgGetDouble("/controls/flight/elevator"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/left-aileron-pos-norm",
fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/right-aileron-pos-norm",
-1 * fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/rudder-pos-norm",
fgGetDouble("/controls/flight/rudder"));
// Fake control-surface positions
fgSetDouble("/surface-positions/flap-pos-norm",
fgGetDouble("/controls/flight/flaps"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/elevator-pos-norm",
fgGetDouble("/controls/flight/elevator"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/left-aileron-pos-norm",
fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/right-aileron-pos-norm",
-1 * fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/rudder-pos-norm",
fgGetDouble("/controls/flight/rudder"));
// copy engine state values onto "bus"
fgSetDouble("/engines/engine/rpm", eng.get_RPM());
fgSetDouble("/engines/engine/mp-osi", eng.get_Manifold_Pressure());
fgSetDouble("/engines/engine/max-hp", eng.get_MaxHP());
fgSetDouble("/engines/engine/power-pct", eng.get_Percentage_Power());
fgSetDouble("/engines/engine/egt-degf", eng.get_EGT());
fgSetDouble("/engines/engine/cht-degf", eng.get_CHT());
fgSetDouble("/engines/engine/prop-thrust", eng.get_prop_thrust_SI());
fgSetDouble("/engines/engine/fuel-flow-gph",
eng.get_fuel_flow_gals_hr());
fgSetDouble("/engines/engine/oil-temperature-degf",
eng.get_oil_temp());
fgSetDouble("/engines/engine/running", eng.getRunningFlag());
fgSetDouble("/engines/engine/cranking", eng.getCrankingFlag());
// copy engine state values onto "bus"
fgSetDouble("/engines/engine/rpm", eng.get_RPM());
fgSetDouble("/engines/engine/mp-osi", eng.get_Manifold_Pressure());
fgSetDouble("/engines/engine/max-hp", eng.get_MaxHP());
fgSetDouble("/engines/engine/power-pct", eng.get_Percentage_Power());
fgSetDouble("/engines/engine/egt-degf", eng.get_EGT());
fgSetDouble("/engines/engine/cht-degf", eng.get_CHT());
fgSetDouble("/engines/engine/prop-thrust", eng.get_prop_thrust_SI());
fgSetDouble("/engines/engine/fuel-flow-gph",
eng.get_fuel_flow_gals_hr());
fgSetDouble("/engines/engine/oil-temperature-degf",
eng.get_oil_temp());
fgSetDouble("/engines/engine/running", eng.getRunningFlag());
fgSetDouble("/engines/engine/cranking", eng.getCrankingFlag());
const SGPropertyNode *fuel_freeze
= fgGetNode("/sim/freeze/fuel");
const SGPropertyNode *fuel_freeze
= fgGetNode("/sim/freeze/fuel");
if ( ! fuel_freeze->getBoolValue() ) {
//Assume we are using both tanks equally for now
fgSetDouble("/consumables/fuel/tank[0]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[0]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
fgSetDouble("/consumables/fuel/tank[1]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[1]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
}
if ( ! fuel_freeze->getBoolValue() ) {
//Assume we are using both tanks equally for now
fgSetDouble("/consumables/fuel/tank[0]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[0]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
fgSetDouble("/consumables/fuel/tank[1]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[1]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
}
// Apparently the IO360 thrust model is not working.
// F_X_engine is zero here.
F_X_engine = eng.get_prop_thrust_lbs();
// Apparently the IO360 thrust model is not working.
// F_X_engine is zero here.
F_X_engine = eng.get_prop_thrust_lbs();
Flap_handle = 30.0 * globals->get_controls()->get_flaps();
Flap_handle = 30.0 * globals->get_controls()->get_flaps();
}
// done with c172-larcsim if-block
if ( !strcmp(aero->getStringValue(), "basic") ) {
// set control inputs
// cout << "V_calibrated_kts = " << V_calibrated_kts << '\n';
eng.set_IAS( V_calibrated_kts );
eng.set_Throttle_Lever_Pos( globals->get_controls()->get_throttle( 0 )
* 100.0 );
eng.set_Propeller_Lever_Pos( 100 );
eng.set_IAS( V_calibrated_kts );
eng.set_Throttle_Lever_Pos( globals->get_controls()->get_throttle( 0 )
* 100.0 );
eng.set_Propeller_Lever_Pos( 100 );
eng.set_Mixture_Lever_Pos( globals->get_controls()->get_mixture( 0 )
* 100.0 );
eng.set_Magneto_Switch_Pos( globals->get_controls()->get_magnetos(0) );
eng.setStarterFlag( globals->get_controls()->get_starter(0) );
eng.set_p_amb( Static_pressure );
eng.set_T_amb( Static_temperature );
* 100.0 );
eng.set_Magneto_Switch_Pos( globals->get_controls()->get_magnetos(0) );
eng.setStarterFlag( globals->get_controls()->get_starter(0) );
eng.set_p_amb( Static_pressure );
eng.set_T_amb( Static_temperature );
// update engine model
eng.update();
// update engine model
eng.update();
// Fake control-surface positions
fgSetDouble("/surface-positions/flap-pos-norm",
fgGetDouble("/controls/flight/flaps"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/elevator-pos-norm",
fgGetDouble("/controls/flight/elevator"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/left-aileron-pos-norm",
fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/right-aileron-pos-norm",
-1 * fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/rudder-pos-norm",
fgGetDouble("/controls/flight/rudder"));
// Fake control-surface positions
fgSetDouble("/surface-positions/flap-pos-norm",
fgGetDouble("/controls/flight/flaps"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/elevator-pos-norm",
fgGetDouble("/controls/flight/elevator"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/left-aileron-pos-norm",
fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/right-aileron-pos-norm",
-1 * fgGetDouble("/controls/flight/aileron"));
// FIXME: ignoring trim
fgSetDouble("/surface-positions/rudder-pos-norm",
fgGetDouble("/controls/flight/rudder"));
// copy engine state values onto "bus"
fgSetDouble("/engines/engine/rpm", eng.get_RPM());
fgSetDouble("/engines/engine/mp-osi", eng.get_Manifold_Pressure());
fgSetDouble("/engines/engine/max-hp", eng.get_MaxHP());
fgSetDouble("/engines/engine/power-pct", eng.get_Percentage_Power());
fgSetDouble("/engines/engine/egt-degf", eng.get_EGT());
fgSetDouble("/engines/engine/cht-degf", eng.get_CHT());
fgSetDouble("/engines/engine/prop-thrust", eng.get_prop_thrust_SI());
fgSetDouble("/engines/engine/fuel-flow-gph",
eng.get_fuel_flow_gals_hr());
fgSetDouble("/engines/engine/oil-temperature-degf",
eng.get_oil_temp());
fgSetDouble("/engines/engine/running", eng.getRunningFlag());
fgSetDouble("/engines/engine/cranking", eng.getCrankingFlag());
// copy engine state values onto "bus"
fgSetDouble("/engines/engine/rpm", eng.get_RPM());
fgSetDouble("/engines/engine/mp-osi", eng.get_Manifold_Pressure());
fgSetDouble("/engines/engine/max-hp", eng.get_MaxHP());
fgSetDouble("/engines/engine/power-pct", eng.get_Percentage_Power());
fgSetDouble("/engines/engine/egt-degf", eng.get_EGT());
fgSetDouble("/engines/engine/cht-degf", eng.get_CHT());
fgSetDouble("/engines/engine/prop-thrust", eng.get_prop_thrust_SI());
fgSetDouble("/engines/engine/fuel-flow-gph",
eng.get_fuel_flow_gals_hr());
fgSetDouble("/engines/engine/oil-temperature-degf",
eng.get_oil_temp());
fgSetDouble("/engines/engine/running", eng.getRunningFlag());
fgSetDouble("/engines/engine/cranking", eng.getCrankingFlag());
const SGPropertyNode *fuel_freeze
= fgGetNode("/sim/freeze/fuel");
const SGPropertyNode *fuel_freeze
= fgGetNode("/sim/freeze/fuel");
if ( ! fuel_freeze->getBoolValue() ) {
//Assume we are using both tanks equally for now
fgSetDouble("/consumables/fuel/tank[0]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[0]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
fgSetDouble("/consumables/fuel/tank[1]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[1]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
}
if ( ! fuel_freeze->getBoolValue() ) {
//Assume we are using both tanks equally for now
fgSetDouble("/consumables/fuel/tank[0]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[0]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
fgSetDouble("/consumables/fuel/tank[1]/level-gal_us",
fgGetDouble("/consumables/fuel/tank[1]/level-gal_us")
- (eng.get_fuel_flow_gals_hr() / (2 * 3600))
* dt);
}
// Apparently the IO360 thrust model is not working.
// F_X_engine is zero here.
F_X_engine = eng.get_prop_thrust_lbs();
// Apparently the IO360 thrust model is not working.
// F_X_engine is zero here.
F_X_engine = eng.get_prop_thrust_lbs();
Flap_handle = 30.0 * globals->get_controls()->get_flaps();
Flap_handle = 30.0 * globals->get_controls()->get_flaps();
}
// done with basic-larcsim if-block
@ -264,8 +264,8 @@ void FGLaRCsim::update( double dt ) {
// lets try to avoid really screwing up the LaRCsim model
if ( get_Altitude() < -9000.0 ) {
save_alt = get_Altitude();
set_Altitude( 0.0 );
save_alt = get_Altitude();
set_Altitude( 0.0 );
}
// copy control positions into the LaRCsim structure
@ -336,34 +336,34 @@ void FGLaRCsim::update( double dt ) {
// spoilers with transition occurring in uiuc_aerodeflections.cpp
if(use_spoilers) {
Spoiler_handle = spoiler_max * fgGetDouble("/controls/flight/spoilers");
Spoiler_handle = spoiler_max * fgGetDouble("/controls/flight/spoilers");
}
// gear with transition occurring here in LaRCsim.cxx
if (use_gear) {
if(fgGetBool("/controls/gear/gear-down")) {
Gear_handle = 1.0;
}
else {
Gear_handle = 0.;
}
// commanded gear is 0 or 1
gear_cmd_norm = Gear_handle;
// amount gear moves per time step [relative to 1]
gear_increment_per_timestep = gear_rate * dt;
// determine gear position with respect to gear command
if (gear_pos_norm < gear_cmd_norm) {
gear_pos_norm += gear_increment_per_timestep;
if (gear_pos_norm > gear_cmd_norm)
gear_pos_norm = gear_cmd_norm;
} else if (gear_pos_norm > gear_cmd_norm) {
gear_pos_norm -= gear_increment_per_timestep;
if (gear_pos_norm < gear_cmd_norm)
gear_pos_norm = gear_cmd_norm;
}
// set the gear position
fgSetDouble("/gear/gear[0]/position-norm", gear_pos_norm);
fgSetDouble("/gear/gear[1]/position-norm", gear_pos_norm);
fgSetDouble("/gear/gear[2]/position-norm", gear_pos_norm);
if(fgGetBool("/controls/gear/gear-down")) {
Gear_handle = 1.0;
}
else {
Gear_handle = 0.;
}
// commanded gear is 0 or 1
gear_cmd_norm = Gear_handle;
// amount gear moves per time step [relative to 1]
gear_increment_per_timestep = gear_rate * dt;
// determine gear position with respect to gear command
if (gear_pos_norm < gear_cmd_norm) {
gear_pos_norm += gear_increment_per_timestep;
if (gear_pos_norm > gear_cmd_norm)
gear_pos_norm = gear_cmd_norm;
} else if (gear_pos_norm > gear_cmd_norm) {
gear_pos_norm -= gear_increment_per_timestep;
if (gear_pos_norm < gear_cmd_norm)
gear_pos_norm = gear_cmd_norm;
}
// set the gear position
fgSetDouble("/gear/gear[0]/position-norm", gear_pos_norm);
fgSetDouble("/gear/gear[1]/position-norm", gear_pos_norm);
fgSetDouble("/gear/gear[2]/position-norm", gear_pos_norm);
}
@ -376,34 +376,34 @@ void FGLaRCsim::update( double dt ) {
fgSetDouble("/engines/engine/cranking", 1);
fgSetDouble("/engines/engine/running", 1);
if ( !strcmp(uiuc_type->getStringValue(), "uiuc-prop")) {
// uiuc prop driven airplane, e.g. Wright Flyer
// uiuc prop driven airplane, e.g. Wright Flyer
}
else if ( !strcmp(uiuc_type->getStringValue(), "uiuc-jet")) {
// uiuc jet aircraft, e.g. a4d
// used for setting the sound
fgSetDouble("/engines/engine/n1", (75 + (globals->get_controls()->get_throttle( 0 ) * 100.0 )/400));
fgSetDouble("/engines/engine/prop-thrust", (4000 + F_X_engine/2));
// used for setting the instruments
fgSetDouble("/engines/engine[0]/n1", (50 + (globals->get_controls()->get_throttle( 0 ) * 50)));
// uiuc jet aircraft, e.g. a4d
// used for setting the sound
fgSetDouble("/engines/engine/n1", (75 + (globals->get_controls()->get_throttle( 0 ) * 100.0 )/400));
fgSetDouble("/engines/engine/prop-thrust", (4000 + F_X_engine/2));
// used for setting the instruments
fgSetDouble("/engines/engine[0]/n1", (50 + (globals->get_controls()->get_throttle( 0 ) * 50)));
}
else if ( !strcmp(uiuc_type->getStringValue(), "uiuc-sailplane")) {
// uiuc sailplane, e.g. asw20
fgSetDouble("/engines/engine/cranking", 0);
// uiuc sailplane, e.g. asw20
fgSetDouble("/engines/engine/cranking", 0);
}
else if ( !strcmp(uiuc_type->getStringValue(), "uiuc-hangglider")) {
// uiuc hang glider, e.g. airwave
fgSetDouble("/engines/engine/cranking", 0);
// uiuc hang glider, e.g. airwave
fgSetDouble("/engines/engine/cranking", 0);
}
else if ( !strcmp(uiuc_type->getStringValue(), "uiuc-ornithopter")) {
// flapping wings
fgSetDouble("/canopy/position-norm", 0);
fgSetDouble("/wing-phase/position-norm", sin(flapper_phi - 3 * LS_PI / 2));
fgSetDouble("/wing-phase/position-deg", flapper_phi*RAD_TO_DEG);
fgSetDouble("/wing-phase/position-one", 1);
fgSetDouble("/thorax/volume", 0);
fgSetDouble("/thorax/rpm", 0);
// fgSetDouble("/thorax/volume", ((1+sin(2*(flapper_phi+LS_PI)))/2));
// fgSetDouble("/thorax/rpm", ((1+sin(2*(flapper_phi+LS_PI)))/2));
// flapping wings
fgSetDouble("/canopy/position-norm", 0);
fgSetDouble("/wing-phase/position-norm", sin(flapper_phi - 3 * LS_PI / 2));
fgSetDouble("/wing-phase/position-deg", flapper_phi*RAD_TO_DEG);
fgSetDouble("/wing-phase/position-one", 1);
fgSetDouble("/thorax/volume", 0);
fgSetDouble("/thorax/rpm", 0);
// fgSetDouble("/thorax/volume", ((1+sin(2*(flapper_phi+LS_PI)))/2));
// fgSetDouble("/thorax/rpm", ((1+sin(2*(flapper_phi+LS_PI)))/2));
}
}
@ -411,10 +411,10 @@ void FGLaRCsim::update( double dt ) {
// add Gamma_horiz_deg to properties, mss 021213
if (use_gamma_horiz_on_speed) {
if (V_rel_wind > gamma_horiz_on_speed) {
fgSetDouble("/orientation/gamma-horiz-deg", (Gamma_horiz_rad * RAD_TO_DEG));
fgSetDouble("/orientation/gamma-horiz-deg", (Gamma_horiz_rad * RAD_TO_DEG));
}
else {
fgSetDouble("/orientation/gamma-horiz-deg", fgGetDouble("/orientation/heading-deg"));
fgSetDouble("/orientation/gamma-horiz-deg", fgGetDouble("/orientation/heading-deg"));
}
}
else {
@ -432,7 +432,7 @@ void FGLaRCsim::update( double dt ) {
// but lets restore our original bogus altitude when we are done
if ( save_alt < -9000.0 ) {
set_Altitude( save_alt );
set_Altitude( save_alt );
}
}
@ -647,9 +647,9 @@ bool FGLaRCsim::copy_from_LaRCsim() {
// Velocities
_set_Velocities_Local( V_north, V_east, V_down );
// set_Velocities_Ground( V_north_rel_ground, V_east_rel_ground,
// V_down_rel_ground );
// V_down_rel_ground );
_set_Velocities_Local_Airmass( V_north_airmass, V_east_airmass,
V_down_airmass );
V_down_airmass );
// set_Velocities_Local_Rel_Airmass( V_north_rel_airmass,
// V_east_rel_airmass, V_down_rel_airmass );
// set_Velocities_Gust( U_gust, V_gust, W_gust );
@ -675,9 +675,9 @@ bool FGLaRCsim::copy_from_LaRCsim() {
_set_Mach_number( Mach_number );
SG_LOG( SG_FLIGHT, SG_DEBUG, "lon = " << Longitude
<< " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude
<< " alt = " << Altitude << " sl_radius = " << Sea_level_radius
<< " radius_to_vehicle = " << Radius_to_vehicle );
<< " lat_geoc = " << Lat_geocentric << " lat_geod = " << Latitude
<< " alt = " << Altitude << " sl_radius = " << Sea_level_radius
<< " radius_to_vehicle = " << Radius_to_vehicle );
double tmp_lon_geoc = Lon_geocentric;
while ( tmp_lon_geoc < -SGD_PI ) { tmp_lon_geoc += SGD_2PI; }
@ -689,7 +689,7 @@ bool FGLaRCsim::copy_from_LaRCsim() {
// Positions
_set_Geocentric_Position( Lat_geocentric, tmp_lon_geoc,
Radius_to_vehicle );
Radius_to_vehicle );
_set_Geodetic_Position( Latitude, tmp_lon, Altitude );
_set_Euler_Angles( Phi, Theta, Psi );
@ -757,23 +757,23 @@ bool FGLaRCsim::copy_from_LaRCsim() {
// cout << "climb rate = " << -V_down * 60 << endl;
if ( !strcmp(aero->getStringValue(), "uiuc") ) {
if (pilot_elev_no) {
globals->get_controls()->set_elevator(Long_control);
globals->get_controls()->set_elevator_trim(Long_trim);
// controls.set_elevator(Long_control);
// controls.set_elevator_trim(Long_trim);
if (pilot_elev_no) {
globals->get_controls()->set_elevator(Long_control);
globals->get_controls()->set_elevator_trim(Long_trim);
// controls.set_elevator(Long_control);
// controls.set_elevator_trim(Long_trim);
}
if (pilot_ail_no) {
if (pilot_ail_no) {
globals->get_controls()->set_aileron(Lat_control);
// controls.set_aileron(Lat_control);
// controls.set_aileron(Lat_control);
}
if (pilot_rud_no) {
if (pilot_rud_no) {
globals->get_controls()->set_rudder(Rudder_pedal);
// controls.set_rudder(Rudder_pedal);
// controls.set_rudder(Rudder_pedal);
}
if (pilot_throttle_no) {
if (pilot_throttle_no) {
globals->get_controls()->set_throttle(0,Throttle_pct);
// controls.set_throttle(0,Throttle_pct);
// controls.set_throttle(0,Throttle_pct);
}
}
@ -810,9 +810,9 @@ void FGLaRCsim::snap_shot(void) {
lsic->SetHeadingRadIC( get_Psi() );
lsic->SetClimbRateFpsIC( get_Climb_Rate() );
lsic->SetVNEDAirmassFpsIC( get_V_north_airmass(),
get_V_east_airmass(),
get_V_down_airmass() );
}
get_V_east_airmass(),
get_V_down_airmass() );
}
//Positions
void FGLaRCsim::set_Latitude(double lat) {
@ -841,7 +841,7 @@ void FGLaRCsim::set_Altitude(double alt) {
void FGLaRCsim::set_V_calibrated_kts(double vc) {
SG_LOG( SG_FLIGHT, SG_INFO,
"FGLaRCsim::set_V_calibrated_kts: " << vc );
"FGLaRCsim::set_V_calibrated_kts: " << vc );
snap_shot();
lsic->SetVcalibratedKtsIC(vc);
set_ls();
@ -858,7 +858,7 @@ void FGLaRCsim::set_Mach_number(double mach) {
void FGLaRCsim::set_Velocities_Local( double north, double east, double down ){
SG_LOG( SG_FLIGHT, SG_INFO, "FGLaRCsim::set_Velocities_local: "
<< north << " " << east << " " << down );
<< north << " " << east << " " << down );
snap_shot();
lsic->SetVNEDFpsIC(north, east, down);
set_ls();
@ -867,7 +867,7 @@ void FGLaRCsim::set_Velocities_Local( double north, double east, double down ){
void FGLaRCsim::set_Velocities_Body( double u, double v, double w){
SG_LOG( SG_FLIGHT, SG_INFO, "FGLaRCsim::set_Velocities_Body: "
<< u << " " << v << " " << w );
<< u << " " << v << " " << w );
snap_shot();
lsic->SetUVWFpsIC(u,v,w);
set_ls();
@ -877,7 +877,7 @@ void FGLaRCsim::set_Velocities_Body( double u, double v, double w){
//Euler angles
void FGLaRCsim::set_Euler_Angles( double phi, double theta, double psi ) {
SG_LOG( SG_FLIGHT, SG_INFO, "FGLaRCsim::set_Euler_angles: "
<< phi << " " << theta << " " << psi );
<< phi << " " << theta << " " << psi );
snap_shot();
lsic->SetPitchAngleRadIC(theta);
@ -914,10 +914,10 @@ void FGLaRCsim::set_AltitudeAGL(double altagl) {
/* getting a namespace conflict...
void FGLaRCsim::set_Velocities_Local_Airmass (double wnorth,
double weast,
double wdown ) {
double weast,
double wdown ) {
SG_LOG( SG_FLIGHT, SG_INFO, "FGLaRCsim::set_Velocities_Local_Airmass: "
<< wnorth << " " << weast << " " << wdown );
<< wnorth << " " << weast << " " << wdown );
snap_shot();
lsic->SetVNEDAirmassFpsIC( wnorth, weast, wdown );
set_ls();
@ -927,23 +927,23 @@ void FGLaRCsim::set_Velocities_Local_Airmass (double wnorth,
void FGLaRCsim::set_Static_pressure(double p) {
SG_LOG( SG_FLIGHT, SG_INFO,
"FGLaRCsim::set_Static_pressure: " << p );
"FGLaRCsim::set_Static_pressure: " << p );
SG_LOG( SG_FLIGHT, SG_INFO,
"LaRCsim does not support externally supplied atmospheric data" );
"LaRCsim does not support externally supplied atmospheric data" );
}
void FGLaRCsim::set_Static_temperature(double T) {
SG_LOG( SG_FLIGHT, SG_INFO,
"FGLaRCsim::set_Static_temperature: " << T );
"FGLaRCsim::set_Static_temperature: " << T );
SG_LOG( SG_FLIGHT, SG_INFO,
"LaRCsim does not support externally supplied atmospheric data" );
"LaRCsim does not support externally supplied atmospheric data" );
}
void FGLaRCsim::set_Density(double rho) {
SG_LOG( SG_FLIGHT, SG_INFO, "FGLaRCsim::set_Density: " << rho );
SG_LOG( SG_FLIGHT, SG_INFO,
"LaRCsim does not support externally supplied atmospheric data" );
"LaRCsim does not support externally supplied atmospheric data" );
}

10
src/FDM/LaRCsim/LaRCsim.hxx
View file

@ -96,11 +96,11 @@ public:
void _set_Inertias( double m, double xx, double yy, double zz, double xz)
{
mass = m;
i_xx = xx;
i_yy = yy;
i_zz = zz;
i_xz = xz;
mass = m;
i_xx = xx;
i_yy = yy;
i_zz = zz;
i_xz = xz;
}
};

328
src/FDM/LaRCsim/atmos_62.c
View file

@ -1,43 +1,43 @@
/***************************************************************************
TITLE: atmos_62
TITLE: atmos_62
----------------------------------------------------------------------------
FUNCTION: 1962 atmosphere table interpolation routine
FUNCTION: 1962 atmosphere table interpolation routine
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 920827 by Bruce Jackson as part of the C-castle
development effort.
GENEALOGY: Created 920827 by Bruce Jackson as part of the C-castle
development effort.
----------------------------------------------------------------------------
DESIGNED BY: B. Jackson
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: B. Jackson
MAINTAINED BY: B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
931220 Added ambient pressure and temperature as outputs. EBJ
940111 Changed includes from "ls_eom.h" to "ls_types.h" and
"ls_constants.h"; changed DATA to SCALAR types. EBJ
DATE PURPOSE BY
931220 Added ambient pressure and temperature as outputs. EBJ
940111 Changed includes from "ls_eom.h" to "ls_types.h" and
"ls_constants.h"; changed DATA to SCALAR types. EBJ
----------------------------------------------------------------------------
REFERENCES:
[ 1] Hornbeck, Robert W.: "Numerical Methods", Prentice-Hall,
1975. ISBN 0-13-626614-2
[ 1] Hornbeck, Robert W.: "Numerical Methods", Prentice-Hall,
1975. ISBN 0-13-626614-2
----------------------------------------------------------------------------
@ -64,16 +64,16 @@
#include <math.h>
#define alt_0 d_a_table[index ][0]
#define alt_1 d_a_table[index+1][0]
#define den_0 d_a_table[index ][1]
#define den_1 d_a_table[index+1][1]
#define sps_0 d_a_table[index ][2]
#define sps_1 d_a_table[index+1][2]
#define gden_0 d_a_table[index ][3]
#define gden_1 d_a_table[index+1][3]
#define gsps_0 d_a_table[index ][4]
#define gsps_1 d_a_table[index+1][4]
#define alt_0 d_a_table[index ][0]
#define alt_1 d_a_table[index+1][0]
#define den_0 d_a_table[index ][1]
#define den_1 d_a_table[index+1][1]
#define sps_0 d_a_table[index ][2]
#define sps_1 d_a_table[index+1][2]
#define gden_0 d_a_table[index ][3]
#define gden_1 d_a_table[index+1][3]
#define gsps_0 d_a_table[index ][4]
#define gsps_1 d_a_table[index+1][4]
#define MAX_ALT_INDEX 121
#define DELT_ALT 2000.
@ -83,137 +83,137 @@
#define MAX_ALTITUDE 240000.
void ls_atmos( SCALAR altitude, SCALAR * sigma, SCALAR * v_sound,
SCALAR * t_amb, SCALAR * p_amb )
SCALAR * t_amb, SCALAR * p_amb )
{
int index;
SCALAR daltp, daltn, daltp3, daltn3, density;
SCALAR t_amb_r, p_amb_r;
int index;
SCALAR daltp, daltn, daltp3, daltn3, density;
SCALAR t_amb_r, p_amb_r;
SCALAR tmp;
static SCALAR d_a_table[MAX_ALT_INDEX][5] =
static SCALAR d_a_table[MAX_ALT_INDEX][5] =
{
{ 0., 2.37701E-03, 1.11642E+03, 0.00000E+00, 0.00000E+00 },
{ 2000., 2.24098E-03, 1.10872E+03, 1.92857E-12, -1.75815E-08 },
{ 4000., 2.11099E-03, 1.10097E+03, 1.34570E-12, -1.21740E-08 },
{ 6000., 1.98684E-03, 1.09315E+03, 1.44862E-12, -1.47225E-08 },
{ 8000., 1.86836E-03, 1.08529E+03, 1.36481E-12, -1.44359E-08 },
{ 10000., 1.75537E-03, 1.07736E+03, 1.32716E-12, -1.45340E-08 },
{ 12000., 1.64768E-03, 1.06938E+03, 1.27657E-12, -1.44280E-08 },
{ 14000., 1.54511E-03, 1.06133E+03, 1.24656E-12, -1.62540E-08 },
{ 16000., 1.44751E-03, 1.05323E+03, 1.19220E-12, -1.50560E-08 },
{ 18000., 1.35469E-03, 1.04506E+03, 1.15463E-12, -1.65220E-08 },
{ 20000., 1.26649E-03, 1.03683E+03, 1.11926E-12, -1.63562E-08 },
{ 22000., 1.18276E-03, 1.02853E+03, 1.07333E-12, -1.70533E-08 },
{ 24000., 1.10333E-03, 1.02016E+03, 1.03743E-12, -1.59305E-08 },
{ 26000., 1.02805E-03, 1.01173E+03, 1.00195E-12, -2.27248E-08 },
{ 28000., 9.56760E-04, 1.00322E+03, 9.39764E-13, 3.29851E-10 },
{ 30000., 8.89320E-04, 9.94641E+02, 1.01399E-12, -8.80946E-08 },
{ 32000., 8.25570E-04, 9.85988E+02, 5.39268E-13, 2.41048E-07 },
{ 34000., 7.65380E-04, 9.77258E+02, 2.16894E-12, -9.91599E-07 },
{ 36000., 7.08600E-04, 9.68448E+02, -4.10001E-12, 3.60535E-06 },
{ 38000., 6.44190E-04, 9.68053E+02, 2.78612E-12, -8.07290E-07 },
{ 40000., 5.85146E-04, 9.68053E+02, 1.00455E-12, 2.16313E-07 },
{ 42000., 5.31517E-04, 9.68053E+02, 1.31819E-12, -5.79609E-08 },
{ 44000., 4.82801E-04, 9.68053E+02, 1.09217E-12, 1.55309E-08 },
{ 46000., 4.38554E-04, 9.68053E+02, 1.01661E-12, -4.16262E-09 },
{ 48000., 3.98359E-04, 9.68053E+02, 9.19375E-13, 1.11961E-09 },
{ 50000., 3.61850E-04, 9.68053E+02, 8.34886E-13, -3.15801E-10 },
{ 52000., 3.28686E-04, 9.68053E+02, 7.58579E-13, 1.43600E-10 },
{ 54000., 2.98561E-04, 9.68053E+02, 6.89297E-13, -2.58597E-10 },
{ 56000., 2.71197E-04, 9.68053E+02, 6.25735E-13, 8.90788E-10 },
{ 58000., 2.46341E-04, 9.68053E+02, 5.69765E-13, -3.30456E-09 },
{ 60000., 2.23765E-04, 9.68053E+02, 5.15206E-13, 1.23274E-08 },
{ 62000., 2.03256E-04, 9.68053E+02, 4.69912E-13, -4.60052E-08 },
{ 64000., 1.84627E-04, 9.68053E+02, 4.25146E-13, 1.71693E-07 },
{ 66000., 1.67616E-04, 9.68314E+02, 2.56502E-13, -2.49268E-07 },
{ 68000., 1.51855E-04, 9.68676E+02, 4.23844E-13, 9.76878E-07 },
{ 70000., 1.37615E-04, 9.71034E+02, 3.29621E-13, -6.64245E-07 },
{ 72000., 1.24744E-04, 9.72390E+02, 3.11170E-13, 1.77102E-07 },
{ 74000., 1.13107E-04, 9.73745E+02, 2.76697E-13, -4.56627E-08 },
{ 76000., 1.02584E-04, 9.75099E+02, 2.53043E-13, 4.04902E-09 },
{ 78000., 9.30660E-05, 9.76450E+02, 2.18633E-13, 2.49667E-08 },
{ 80000., 8.44530E-05, 9.77799E+02, 2.29927E-13, -1.06916E-07 },
{ 82000., 7.67140E-05, 9.78950E+02, 1.72660E-13, 1.05696E-07 },
{ 84000., 6.97010E-05, 9.80290E+02, 1.68432E-13, -3.23682E-08 },
{ 86000., 6.33490E-05, 9.81620E+02, 1.45113E-13, 8.77690E-09 },
{ 88000., 5.75880E-05, 9.82950E+02, 1.37617E-13, -2.73938E-09 },
{ 90000., 5.23700E-05, 9.84280E+02, 1.18918E-13, 2.18061E-09 },
{ 92000., 4.76350E-05, 9.85610E+02, 1.11210E-13, -5.98306E-09 },
{ 94000., 4.33410E-05, 9.86930E+02, 9.77408E-14, 6.75162E-09 },
{ 96000., 3.94430E-05, 9.88260E+02, 9.18264E-14, -6.02343E-09 },
{ 98000., 3.59080E-05, 9.89580E+02, 7.94534E-14, 2.34210E-09 },
{ 100000., 3.26960E-05, 9.90900E+02, 7.48600E-14, -3.34498E-09 },
{ 102000., 2.97810E-05, 9.92210E+02, 6.66067E-14, -3.96219E-09 },
{ 104000., 2.71320E-05, 9.93530E+02, 5.77131E-14, 3.41937E-08 },
{ 106000., 2.46980E-05, 9.95410E+02, 2.50410E-14, 7.07187E-07 },
{ 108000., 2.24140E-05, 9.99070E+02, 6.71229E-14, -1.92943E-07 },
{ 110000., 2.03570E-05, 1.00272E+03, 4.69675E-14, 4.95832E-08 },
{ 112000., 1.85010E-05, 1.00636E+03, 4.65069E-14, -2.03903E-08 },
{ 114000., 1.68270E-05, 1.00998E+03, 4.00047E-14, 1.97789E-09 },
{ 116000., 1.53150E-05, 1.01359E+03, 3.64744E-14, -2.52130E-09 },
{ 118000., 1.39480E-05, 1.01719E+03, 3.15976E-14, -6.89271E-09 },
{ 120000., 1.27100E-05, 1.02077E+03, 3.06351E-14, 9.21465E-11 },
{ 122000., 1.15920E-05, 1.02434E+03, 2.58618E-14, -8.47587E-09 },
{ 124000., 1.05790E-05, 1.02789E+03, 2.34176E-14, 3.81135E-09 },
{ 126000., 9.66010E-06, 1.03144E+03, 2.16178E-14, -6.76951E-09 },
{ 128000., 8.82710E-06, 1.03497E+03, 1.89611E-14, -6.73330E-09 },
{ 130000., 8.07070E-06, 1.03848E+03, 1.74377E-14, 3.70270E-09 },
{ 132000., 7.38380E-06, 1.04199E+03, 1.55382E-14, -8.07752E-09 },
{ 134000., 6.75940E-06, 1.04548E+03, 1.41595E-14, -1.39263E-09 },
{ 136000., 6.19160E-06, 1.04896E+03, 1.27239E-14, -1.35196E-09 },
{ 138000., 5.67490E-06, 1.05243E+03, 1.15951E-14, -8.19953E-09 },
{ 140000., 5.20450E-06, 1.05588E+03, 1.03459E-14, 4.15010E-09 },
{ 142000., 4.77570E-06, 1.05933E+03, 9.42149E-15, -8.40086E-09 },
{ 144000., 4.38470E-06, 1.06276E+03, 8.66820E-15, -5.46671E-10 },
{ 146000., 4.02820E-06, 1.06618E+03, 7.65573E-15, -4.41246E-09 },
{ 148000., 3.70260E-06, 1.06959E+03, 7.05890E-15, 3.19650E-09 },
{ 150000., 3.40520E-06, 1.07299E+03, 6.40867E-15, -2.33736E-08 },
{ 152000., 3.13330E-06, 1.07637E+03, 5.55641E-15, 6.02977E-08 },
{ 154000., 2.88480E-06, 1.07975E+03, 6.46568E-15, -2.17817E-07 },
{ 156000., 2.66270E-06, 1.08202E+03, 8.18087E-15, -8.54029E-07 },
{ 158000., 2.46830E-06, 1.08202E+03, 2.36086E-15, 2.28931E-07 },
{ 160000., 2.28810E-06, 1.08202E+03, 3.67571E-15, -6.16972E-08 },
{ 162000., 2.12120E-06, 1.08202E+03, 2.88632E-15, 1.78573E-08 },
{ 164000., 1.96640E-06, 1.08202E+03, 2.92903E-15, -9.73206E-09 },
{ 166000., 1.82300E-06, 1.08202E+03, 2.49757E-15, 2.10709E-08 },
{ 168000., 1.69000E-06, 1.08202E+03, 2.68069E-15, -7.45517E-08 },
{ 170000., 1.56680E-06, 1.08202E+03, 1.47966E-15, 2.77136E-07 },
{ 172000., 1.45250E-06, 1.08202E+03, 4.75068E-15, -1.03399E-06 },
{ 174000., 1.35240E-06, 1.07963E+03, 8.17622E-16, 2.73830E-07 },
{ 176000., 1.25880E-06, 1.07723E+03, 1.72883E-15, -7.63301E-08 },
{ 178000., 1.17130E-06, 1.07482E+03, 1.41704E-15, 1.64901E-08 },
{ 180000., 1.08960E-06, 1.07240E+03, 1.30299E-15, -4.63038E-09 },
{ 182000., 1.01320E-06, 1.06998E+03, 1.32100E-15, 2.03140E-09 },
{ 184000., 9.41950E-07, 1.06756E+03, 1.13799E-15, -3.49522E-09 },
{ 186000., 8.75370E-07, 1.06513E+03, 1.13202E-15, -3.05052E-09 },
{ 188000., 8.13260E-07, 1.06269E+03, 1.03892E-15, 6.97283E-10 },
{ 190000., 7.55320E-07, 1.06025E+03, 9.67290E-16, 2.61383E-10 },
{ 192000., 7.01260E-07, 1.05781E+03, 9.11920E-16, -1.74281E-09 },
{ 194000., 6.50850E-07, 1.05536E+03, 8.60032E-16, -8.29013E-09 },
{ 196000., 6.03870E-07, 1.05290E+03, 7.92951E-16, 1.99033E-08 },
{ 198000., 5.60110E-07, 1.05044E+03, 7.98164E-16, -7.13232E-08 },
{ 200000., 5.19320E-07, 1.04798E+03, 4.69394E-16, 2.65389E-07 },
{ 202000., 4.81340E-07, 1.04550E+03, 1.53926E-15, -1.02023E-06 },
{ 204000., 4.47960E-07, 1.04063E+03, 2.73571E-16, 2.30547E-07 },
{ 206000., 4.16690E-07, 1.03565E+03, 5.31456E-16, -6.69551E-08 },
{ 208000., 3.87320E-07, 1.03065E+03, 4.50605E-16, 7.27308E-09 },
{ 210000., 3.59790E-07, 1.02562E+03, 4.26126E-16, -7.13720E-09 },
{ 212000., 3.33970E-07, 1.02057E+03, 4.09893E-16, -8.72426E-09 },
{ 214000., 3.09780E-07, 1.01549E+03, 3.79301E-16, -2.96576E-09 },
{ 216000., 2.87120E-07, 1.01039E+03, 3.67902E-16, -9.41272E-09 },
{ 218000., 2.65920E-07, 1.00526E+03, 3.39092E-16, -4.38337E-09 },
{ 220000., 2.46090E-07, 1.00011E+03, 3.30732E-16, -3.05378E-09 },
{ 222000., 2.27570E-07, 9.94940E+02, 3.02981E-16, -1.34015E-08 },
{ 224000., 2.10270E-07, 9.89730E+02, 2.87343E-16, -3.34027E-09 },
{ 226000., 1.94120E-07, 9.84500E+02, 2.72646E-16, -3.23743E-09 },
{ 228000., 1.79060E-07, 9.79250E+02, 2.57074E-16, -1.37100E-08 },
{ 230000., 1.65030E-07, 9.73960E+02, 2.44060E-16, -1.92258E-09 },
{ 232000., 1.51970E-07, 9.68650E+02, 2.21687E-16, -8.59969E-09 },
{ 234000., 1.39810E-07, 9.63310E+02, 2.19191E-16, -8.67865E-09 },
{ 236000., 1.28510E-07, 9.57940E+02, 1.91549E-16, -1.68569E-09 },
{ 238000., 1.18020E-07, 9.52550E+02, 2.29613E-16, -1.45786E-08 },
{ 240000., 1.08270E-07, 9.47120E+02, 0.00000E+00, 0.00000E+00 }
{ 0., 2.37701E-03, 1.11642E+03, 0.00000E+00, 0.00000E+00 },
{ 2000., 2.24098E-03, 1.10872E+03, 1.92857E-12, -1.75815E-08 },
{ 4000., 2.11099E-03, 1.10097E+03, 1.34570E-12, -1.21740E-08 },
{ 6000., 1.98684E-03, 1.09315E+03, 1.44862E-12, -1.47225E-08 },
{ 8000., 1.86836E-03, 1.08529E+03, 1.36481E-12, -1.44359E-08 },
{ 10000., 1.75537E-03, 1.07736E+03, 1.32716E-12, -1.45340E-08 },
{ 12000., 1.64768E-03, 1.06938E+03, 1.27657E-12, -1.44280E-08 },
{ 14000., 1.54511E-03, 1.06133E+03, 1.24656E-12, -1.62540E-08 },
{ 16000., 1.44751E-03, 1.05323E+03, 1.19220E-12, -1.50560E-08 },
{ 18000., 1.35469E-03, 1.04506E+03, 1.15463E-12, -1.65220E-08 },
{ 20000., 1.26649E-03, 1.03683E+03, 1.11926E-12, -1.63562E-08 },
{ 22000., 1.18276E-03, 1.02853E+03, 1.07333E-12, -1.70533E-08 },
{ 24000., 1.10333E-03, 1.02016E+03, 1.03743E-12, -1.59305E-08 },
{ 26000., 1.02805E-03, 1.01173E+03, 1.00195E-12, -2.27248E-08 },
{ 28000., 9.56760E-04, 1.00322E+03, 9.39764E-13, 3.29851E-10 },
{ 30000., 8.89320E-04, 9.94641E+02, 1.01399E-12, -8.80946E-08 },
{ 32000., 8.25570E-04, 9.85988E+02, 5.39268E-13, 2.41048E-07 },
{ 34000., 7.65380E-04, 9.77258E+02, 2.16894E-12, -9.91599E-07 },
{ 36000., 7.08600E-04, 9.68448E+02, -4.10001E-12, 3.60535E-06 },
{ 38000., 6.44190E-04, 9.68053E+02, 2.78612E-12, -8.07290E-07 },
{ 40000., 5.85146E-04, 9.68053E+02, 1.00455E-12, 2.16313E-07 },
{ 42000., 5.31517E-04, 9.68053E+02, 1.31819E-12, -5.79609E-08 },
{ 44000., 4.82801E-04, 9.68053E+02, 1.09217E-12, 1.55309E-08 },
{ 46000., 4.38554E-04, 9.68053E+02, 1.01661E-12, -4.16262E-09 },
{ 48000., 3.98359E-04, 9.68053E+02, 9.19375E-13, 1.11961E-09 },
{ 50000., 3.61850E-04, 9.68053E+02, 8.34886E-13, -3.15801E-10 },
{ 52000., 3.28686E-04, 9.68053E+02, 7.58579E-13, 1.43600E-10 },
{ 54000., 2.98561E-04, 9.68053E+02, 6.89297E-13, -2.58597E-10 },
{ 56000., 2.71197E-04, 9.68053E+02, 6.25735E-13, 8.90788E-10 },
{ 58000., 2.46341E-04, 9.68053E+02, 5.69765E-13, -3.30456E-09 },
{ 60000., 2.23765E-04, 9.68053E+02, 5.15206E-13, 1.23274E-08 },
{ 62000., 2.03256E-04, 9.68053E+02, 4.69912E-13, -4.60052E-08 },
{ 64000., 1.84627E-04, 9.68053E+02, 4.25146E-13, 1.71693E-07 },
{ 66000., 1.67616E-04, 9.68314E+02, 2.56502E-13, -2.49268E-07 },
{ 68000., 1.51855E-04, 9.68676E+02, 4.23844E-13, 9.76878E-07 },
{ 70000., 1.37615E-04, 9.71034E+02, 3.29621E-13, -6.64245E-07 },
{ 72000., 1.24744E-04, 9.72390E+02, 3.11170E-13, 1.77102E-07 },
{ 74000., 1.13107E-04, 9.73745E+02, 2.76697E-13, -4.56627E-08 },
{ 76000., 1.02584E-04, 9.75099E+02, 2.53043E-13, 4.04902E-09 },
{ 78000., 9.30660E-05, 9.76450E+02, 2.18633E-13, 2.49667E-08 },
{ 80000., 8.44530E-05, 9.77799E+02, 2.29927E-13, -1.06916E-07 },
{ 82000., 7.67140E-05, 9.78950E+02, 1.72660E-13, 1.05696E-07 },
{ 84000., 6.97010E-05, 9.80290E+02, 1.68432E-13, -3.23682E-08 },
{ 86000., 6.33490E-05, 9.81620E+02, 1.45113E-13, 8.77690E-09 },
{ 88000., 5.75880E-05, 9.82950E+02, 1.37617E-13, -2.73938E-09 },
{ 90000., 5.23700E-05, 9.84280E+02, 1.18918E-13, 2.18061E-09 },
{ 92000., 4.76350E-05, 9.85610E+02, 1.11210E-13, -5.98306E-09 },
{ 94000., 4.33410E-05, 9.86930E+02, 9.77408E-14, 6.75162E-09 },
{ 96000., 3.94430E-05, 9.88260E+02, 9.18264E-14, -6.02343E-09 },
{ 98000., 3.59080E-05, 9.89580E+02, 7.94534E-14, 2.34210E-09 },
{ 100000., 3.26960E-05, 9.90900E+02, 7.48600E-14, -3.34498E-09 },
{ 102000., 2.97810E-05, 9.92210E+02, 6.66067E-14, -3.96219E-09 },
{ 104000., 2.71320E-05, 9.93530E+02, 5.77131E-14, 3.41937E-08 },
{ 106000., 2.46980E-05, 9.95410E+02, 2.50410E-14, 7.07187E-07 },
{ 108000., 2.24140E-05, 9.99070E+02, 6.71229E-14, -1.92943E-07 },
{ 110000., 2.03570E-05, 1.00272E+03, 4.69675E-14, 4.95832E-08 },
{ 112000., 1.85010E-05, 1.00636E+03, 4.65069E-14, -2.03903E-08 },
{ 114000., 1.68270E-05, 1.00998E+03, 4.00047E-14, 1.97789E-09 },
{ 116000., 1.53150E-05, 1.01359E+03, 3.64744E-14, -2.52130E-09 },
{ 118000., 1.39480E-05, 1.01719E+03, 3.15976E-14, -6.89271E-09 },
{ 120000., 1.27100E-05, 1.02077E+03, 3.06351E-14, 9.21465E-11 },
{ 122000., 1.15920E-05, 1.02434E+03, 2.58618E-14, -8.47587E-09 },
{ 124000., 1.05790E-05, 1.02789E+03, 2.34176E-14, 3.81135E-09 },
{ 126000., 9.66010E-06, 1.03144E+03, 2.16178E-14, -6.76951E-09 },
{ 128000., 8.82710E-06, 1.03497E+03, 1.89611E-14, -6.73330E-09 },
{ 130000., 8.07070E-06, 1.03848E+03, 1.74377E-14, 3.70270E-09 },
{ 132000., 7.38380E-06, 1.04199E+03, 1.55382E-14, -8.07752E-09 },
{ 134000., 6.75940E-06, 1.04548E+03, 1.41595E-14, -1.39263E-09 },
{ 136000., 6.19160E-06, 1.04896E+03, 1.27239E-14, -1.35196E-09 },
{ 138000., 5.67490E-06, 1.05243E+03, 1.15951E-14, -8.19953E-09 },
{ 140000., 5.20450E-06, 1.05588E+03, 1.03459E-14, 4.15010E-09 },
{ 142000., 4.77570E-06, 1.05933E+03, 9.42149E-15, -8.40086E-09 },
{ 144000., 4.38470E-06, 1.06276E+03, 8.66820E-15, -5.46671E-10 },
{ 146000., 4.02820E-06, 1.06618E+03, 7.65573E-15, -4.41246E-09 },
{ 148000., 3.70260E-06, 1.06959E+03, 7.05890E-15, 3.19650E-09 },
{ 150000., 3.40520E-06, 1.07299E+03, 6.40867E-15, -2.33736E-08 },
{ 152000., 3.13330E-06, 1.07637E+03, 5.55641E-15, 6.02977E-08 },
{ 154000., 2.88480E-06, 1.07975E+03, 6.46568E-15, -2.17817E-07 },
{ 156000., 2.66270E-06, 1.08202E+03, 8.18087E-15, -8.54029E-07 },
{ 158000., 2.46830E-06, 1.08202E+03, 2.36086E-15, 2.28931E-07 },
{ 160000., 2.28810E-06, 1.08202E+03, 3.67571E-15, -6.16972E-08 },
{ 162000., 2.12120E-06, 1.08202E+03, 2.88632E-15, 1.78573E-08 },
{ 164000., 1.96640E-06, 1.08202E+03, 2.92903E-15, -9.73206E-09 },
{ 166000., 1.82300E-06, 1.08202E+03, 2.49757E-15, 2.10709E-08 },
{ 168000., 1.69000E-06, 1.08202E+03, 2.68069E-15, -7.45517E-08 },
{ 170000., 1.56680E-06, 1.08202E+03, 1.47966E-15, 2.77136E-07 },
{ 172000., 1.45250E-06, 1.08202E+03, 4.75068E-15, -1.03399E-06 },
{ 174000., 1.35240E-06, 1.07963E+03, 8.17622E-16, 2.73830E-07 },
{ 176000., 1.25880E-06, 1.07723E+03, 1.72883E-15, -7.63301E-08 },
{ 178000., 1.17130E-06, 1.07482E+03, 1.41704E-15, 1.64901E-08 },
{ 180000., 1.08960E-06, 1.07240E+03, 1.30299E-15, -4.63038E-09 },
{ 182000., 1.01320E-06, 1.06998E+03, 1.32100E-15, 2.03140E-09 },
{ 184000., 9.41950E-07, 1.06756E+03, 1.13799E-15, -3.49522E-09 },
{ 186000., 8.75370E-07, 1.06513E+03, 1.13202E-15, -3.05052E-09 },
{ 188000., 8.13260E-07, 1.06269E+03, 1.03892E-15, 6.97283E-10 },
{ 190000., 7.55320E-07, 1.06025E+03, 9.67290E-16, 2.61383E-10 },
{ 192000., 7.01260E-07, 1.05781E+03, 9.11920E-16, -1.74281E-09 },
{ 194000., 6.50850E-07, 1.05536E+03, 8.60032E-16, -8.29013E-09 },
{ 196000., 6.03870E-07, 1.05290E+03, 7.92951E-16, 1.99033E-08 },
{ 198000., 5.60110E-07, 1.05044E+03, 7.98164E-16, -7.13232E-08 },
{ 200000., 5.19320E-07, 1.04798E+03, 4.69394E-16, 2.65389E-07 },
{ 202000., 4.81340E-07, 1.04550E+03, 1.53926E-15, -1.02023E-06 },
{ 204000., 4.47960E-07, 1.04063E+03, 2.73571E-16, 2.30547E-07 },
{ 206000., 4.16690E-07, 1.03565E+03, 5.31456E-16, -6.69551E-08 },
{ 208000., 3.87320E-07, 1.03065E+03, 4.50605E-16, 7.27308E-09 },
{ 210000., 3.59790E-07, 1.02562E+03, 4.26126E-16, -7.13720E-09 },
{ 212000., 3.33970E-07, 1.02057E+03, 4.09893E-16, -8.72426E-09 },
{ 214000., 3.09780E-07, 1.01549E+03, 3.79301E-16, -2.96576E-09 },
{ 216000., 2.87120E-07, 1.01039E+03, 3.67902E-16, -9.41272E-09 },
{ 218000., 2.65920E-07, 1.00526E+03, 3.39092E-16, -4.38337E-09 },
{ 220000., 2.46090E-07, 1.00011E+03, 3.30732E-16, -3.05378E-09 },
{ 222000., 2.27570E-07, 9.94940E+02, 3.02981E-16, -1.34015E-08 },
{ 224000., 2.10270E-07, 9.89730E+02, 2.87343E-16, -3.34027E-09 },
{ 226000., 1.94120E-07, 9.84500E+02, 2.72646E-16, -3.23743E-09 },
{ 228000., 1.79060E-07, 9.79250E+02, 2.57074E-16, -1.37100E-08 },
{ 230000., 1.65030E-07, 9.73960E+02, 2.44060E-16, -1.92258E-09 },
{ 232000., 1.51970E-07, 9.68650E+02, 2.21687E-16, -8.59969E-09 },
{ 234000., 1.39810E-07, 9.63310E+02, 2.19191E-16, -8.67865E-09 },
{ 236000., 1.28510E-07, 9.57940E+02, 1.91549E-16, -1.68569E-09 },
{ 238000., 1.18020E-07, 9.52550E+02, 2.29613E-16, -1.45786E-08 },
{ 240000., 1.08270E-07, 9.47120E+02, 0.00000E+00, 0.00000E+00 }
};
/* for purposes of doing the table lookup, force the incoming
@ -222,7 +222,7 @@ void ls_atmos( SCALAR altitude, SCALAR * sigma, SCALAR * v_sound,
// printf("altitude = %.2f\n", altitude);
if ( altitude < 0.0 ) {
altitude = 0.0;
altitude = 0.0;
}
// printf("altitude = %.2f\n", altitude);
@ -240,28 +240,28 @@ void ls_atmos( SCALAR altitude, SCALAR * sigma, SCALAR * v_sound,
daltn3 = daltn*daltn*daltn;
density = (gden_0/6)*((daltp3/2000) - 2000*daltp)
+ (gden_1/6)*((daltn3/2000) - 2000*daltn)
+ den_0*daltp/2000 + den_1*daltn/2000;
+ (gden_1/6)*((daltn3/2000) - 2000*daltn)
+ den_0*daltp/2000 + den_1*daltn/2000;
*v_sound = (gsps_0/6)*((daltp3/2000) - 2000*daltp)
+ (gsps_1/6)*((daltn3/2000) - 2000*daltn)
+ sps_0*daltp/2000 + sps_1*daltn/2000;
+ (gsps_1/6)*((daltn3/2000) - 2000*daltn)
+ sps_0*daltp/2000 + sps_1*daltn/2000;
*sigma = density/SEA_LEVEL_DENSITY;
if (altitude < HLEV) /* BUG - these curve fits are only good to about 75000 ft */
{
t_amb_r = 1. - 6.875e-6*altitude;
// printf("index = %d t_amb_r = %.2f\n", index, t_amb_r);
// p_amb_r = pow( t_amb_r, 5.256 );
tmp = 5.256; // avoid a segfault (?)
p_amb_r = pow( t_amb_r, tmp );
// printf("p_amb_r = %.2f\n", p_amb_r);
t_amb_r = 1. - 6.875e-6*altitude;
// printf("index = %d t_amb_r = %.2f\n", index, t_amb_r);
// p_amb_r = pow( t_amb_r, 5.256 );
tmp = 5.256; // avoid a segfault (?)
p_amb_r = pow( t_amb_r, tmp );
// printf("p_amb_r = %.2f\n", p_amb_r);
}
else
{
t_amb_r = 0.751895;
p_amb_r = 0.2234*exp( -4.806e-5 * (altitude - HLEV));
t_amb_r = 0.751895;
p_amb_r = 0.2234*exp( -4.806e-5 * (altitude - HLEV));
}
*p_amb = p_amb_r * PAMB0;

2
src/FDM/LaRCsim/atmos_62.h
View file

@ -18,7 +18,7 @@ extern "C" {
void ls_atmos( SCALAR altitude, SCALAR * sigma, SCALAR * v_sound,
SCALAR * t_amb, SCALAR * p_amb );
SCALAR * t_amb, SCALAR * p_amb );
#ifdef __cplusplus
}

354
src/FDM/LaRCsim/c172_aero.c
View file

@ -1,37 +1,37 @@
/***************************************************************************
TITLE: c172_aero
TITLE: c172_aero
----------------------------------------------------------------------------
FUNCTION: aerodynamics model based on constant stability derivatives
FUNCTION: aerodynamics model based on constant stability derivatives
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Based on data from:
Part 1 of Roskam's S&C text
The FAA type certificate data sheet for the 172
Various sources on the net
John D. Anderson's Intro to Flight text (NACA 2412 data)
UIUC's airfoil data web site
GENEALOGY: Based on data from:
Part 1 of Roskam's S&C text
The FAA type certificate data sheet for the 172
Various sources on the net
John D. Anderson's Intro to Flight text (NACA 2412 data)
UIUC's airfoil data web site
----------------------------------------------------------------------------
DESIGNED BY: Tony Peden
CODED BY: Tony Peden
MAINTAINED BY: Tony Peden
DESIGNED BY: Tony Peden
CODED BY: Tony Peden
MAINTAINED BY: Tony Peden
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
DATE PURPOSE BY
6/10/99 Initial test release
@ -40,28 +40,28 @@
REFERENCES:
Aero Coeffs:
CL lift
Cd drag
Cm pitching moment
Cy sideforce
Cn yawing moment
Croll,Cl rolling moment (yeah, I know. Shoot me.)
CL lift
Cd drag
Cm pitching moment
Cy sideforce
Cn yawing moment
Croll,Cl rolling moment (yeah, I know. Shoot me.)
Subscripts
o constant i.e. not a function of alpha or beta
a alpha
adot d(alpha)/dt
q pitch rate
qdot d(q)/dt
beta sideslip angle
p roll rate
r yaw rate
da aileron deflection
de elevator deflection
dr rudder deflection
s stability axes
o constant i.e. not a function of alpha or beta
a alpha
adot d(alpha)/dt
q pitch rate
qdot d(q)/dt
beta sideslip angle
p roll rate
r yaw rate
da aileron deflection
de elevator deflection
dr rudder deflection
s stability axes
----------------------------------------------------------------------------
@ -74,7 +74,7 @@
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
@ -83,7 +83,7 @@
--------------------------------------------------------------------------*/
#include "ls_generic.h"
#include "ls_cockpit.h"
#include "ls_constants.h"
@ -100,10 +100,10 @@
#ifdef USENZ
#define NZ generic_.n_cg_body_v[2]
#define NZ generic_.n_cg_body_v[2]
#else
#define NZ 1
#endif
#define NZ 1
#endif
extern COCKPIT cockpit_;
@ -174,35 +174,35 @@ extern COCKPIT cockpit_;
static SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
{
SCALAR slope;
int i=1;
float y;
/* if x is outside the table, return value at x[0] or x[Ntable-1]*/
if(x <= x_table[0])
{
y=y_table[0];
/* printf("x smaller than x_table[0]: %g %g\n",x,x_table[0]); */
}
else if(x >= x_table[Ntable-1])
{
slope=(y_table[Ntable-1]-y_table[Ntable-2])/(x_table[Ntable-1]-x_table[Ntable-2]);
y=slope*(x-x_table[Ntable-1]) +y_table[Ntable-1];
/* printf("x larger than x_table[N]: %g %g %d\n",x,x_table[NCL-1],Ntable-1);
*/ }
else /*x is within the table, interpolate linearly to find y value*/
{
while(x_table[i] <= x) {i++;}
slope=(y_table[i]-y_table[i-1])/(x_table[i]-x_table[i-1]);
/* printf("x: %g, i: %d, cl[i]: %g, cl[i-1]: %g, slope: %g\n",x,i,y_table[i],y_table[i-1],slope); */
y=slope*(x-x_table[i-1]) +y_table[i-1];
}
return y;
}
SCALAR slope;
int i=1;
float y;
/* if x is outside the table, return value at x[0] or x[Ntable-1]*/
if(x <= x_table[0])
{
y=y_table[0];
/* printf("x smaller than x_table[0]: %g %g\n",x,x_table[0]); */
}
else if(x >= x_table[Ntable-1])
{
slope=(y_table[Ntable-1]-y_table[Ntable-2])/(x_table[Ntable-1]-x_table[Ntable-2]);
y=slope*(x-x_table[Ntable-1]) +y_table[Ntable-1];
/* printf("x larger than x_table[N]: %g %g %d\n",x,x_table[NCL-1],Ntable-1);
*/ }
else /*x is within the table, interpolate linearly to find y value*/
{
while(x_table[i] <= x) {i++;}
slope=(y_table[i]-y_table[i-1])/(x_table[i]-x_table[i-1]);
/* printf("x: %g, i: %d, cl[i]: %g, cl[i-1]: %g, slope: %g\n",x,i,y_table[i],y_table[i-1],slope); */
y=slope*(x-x_table[i-1]) +y_table[i-1];
}
return y;
}
void c172_aero( SCALAR dt, int Initialize ) {
@ -214,7 +214,7 @@ void c172_aero( SCALAR dt, int Initialize ) {
static SCALAR trim_inc = 0.0002;
static SCALAR alpha_ind[NCL]={-0.087,0,0.14,0.21,0.24,0.26,0.28,0.31,0.35};
static SCALAR alpha_ind[NCL]={-0.087,0,0.14,0.21,0.24,0.26,0.28,0.31,0.35};
static SCALAR CLtable[NCL]={-0.22,0.25,1.02,1.252,1.354,1.44,1.466,1.298,0.97};
static SCALAR flap_ind[Ndf]={0,10,20,30};
@ -230,68 +230,68 @@ void c172_aero( SCALAR dt, int Initialize ) {
/* printf("Initialize= %d\n",Initialize); */
/* printf("Initializing aero model...Initialize= %d\n", Initialize);
*/
/* printf("Initializing aero model...Initialize= %d\n", Initialize);
*/
/*nondimensionalization quantities*/
/*units here are ft and lbs */
cbar=4.9; /*mean aero chord ft*/
b=35.8; /*wing span ft */
Sw=174; /*wing planform surface area ft^2*/
rPiARe=0.054; /*reciprocal of Pi*AR*e*/
lbare=3.682; /*elevator moment arm / MAC*/
CLadot=1.7;
CLq=3.9;
CLob=0;
/*units here are ft and lbs */
cbar=4.9; /*mean aero chord ft*/
b=35.8; /*wing span ft */
Sw=174; /*wing planform surface area ft^2*/
rPiARe=0.054; /*reciprocal of Pi*AR*e*/
lbare=3.682; /*elevator moment arm / MAC*/
CLadot=1.7;
CLq=3.9;
CLob=0;
Ai=1.24;
Cdob=0.036;
Cda=0.13; /*Not used*/
Cdde=0.06;
Cdob=0.036;
Cda=0.13; /*Not used*/
Cdde=0.06;
Cma=-1.8;
Cmadot=-5.2;
Cmq=-12.4;
Cmob=-0.02;
Cmde=-1.28;
CLde=-Cmde / lbare; /* kinda backwards, huh? */
Cma=-1.8;
Cmadot=-5.2;
Cmq=-12.4;
Cmob=-0.02;
Cmde=-1.28;
CLde=-Cmde / lbare; /* kinda backwards, huh? */
Clbeta=-0.089;
Clp=-0.47;
Clr=0.096;
Clda=-0.09;
Cldr=0.0147;
Clbeta=-0.089;
Clp=-0.47;
Clr=0.096;
Clda=-0.09;
Cldr=0.0147;
Cnbeta=0.065;
Cnp=-0.03;
Cnr=-0.099;
Cnda=-0.0053;
Cndr=-0.0657;
Cnbeta=0.065;
Cnp=-0.03;
Cnr=-0.099;
Cnda=-0.0053;
Cndr=-0.0657;
Cybeta=-0.31;
Cyp=-0.037;
Cyr=0.21;
Cyda=0.0;
Cydr=0.187;
Cybeta=-0.31;
Cyp=-0.037;
Cyr=0.21;
Cyda=0.0;
Cydr=0.187;
MaxTakeoffWeight=2550;
EmptyWeight=1500;
MaxTakeoffWeight=2550;
EmptyWeight=1500;
Zcg=0.51;
Zcg=0.51;
/*
LaRCsim uses:
Cm > 0 => ANU
Cl > 0 => Right wing down
Cn > 0 => ANL
so:
Cl > 0 => Right wing down
Cn > 0 => ANL
so:
elevator > 0 => AND -- aircraft nose down
aileron > 0 => right wing up
rudder > 0 => ANL
aileron > 0 => right wing up
rudder > 0 => ANL
*/
/*do weight & balance here since there is no better place*/
@ -316,58 +316,58 @@ void c172_aero( SCALAR dt, int Initialize ) {
if(Flap_handle < flap_ind[0])
{
fi=0;
Flap_handle=flap_ind[0];
lastFlapHandle=Flap_handle;
Flap_Position=flap_ind[0];
fi=0;
Flap_handle=flap_ind[0];
lastFlapHandle=Flap_handle;
Flap_Position=flap_ind[0];
}
else if(Flap_handle > flap_ind[Ndf-1])
{
fi=Ndf-1;
Flap_handle=flap_ind[fi];
lastFlapHandle=Flap_handle;
Flap_Position=flap_ind[fi];
fi=Ndf-1;
Flap_handle=flap_ind[fi];
lastFlapHandle=Flap_handle;
Flap_Position=flap_ind[fi];
}
else
{
if(dt <= 0)
Flap_Position=Flap_handle;
else
{
if(Flap_handle != lastFlapHandle)
{
Flaps_In_Transit=1;
}
if(Flaps_In_Transit)
{
fi=0;
while(flap_ind[fi] < Flap_handle) { fi++; }
if(Flap_Position < Flap_handle)
{
else
{
if(dt <= 0)
Flap_Position=Flap_handle;
else
{
if(Flap_handle != lastFlapHandle)
{
Flaps_In_Transit=1;
}
if(Flaps_In_Transit)
{
fi=0;
while(flap_ind[fi] < Flap_handle) { fi++; }
if(Flap_Position < Flap_handle)
{
if(flap_times[fi] > 0)
flap_transit_rate=(flap_ind[fi] - flap_ind[fi-1])/flap_times[fi];
else
flap_transit_rate=(flap_ind[fi] - flap_ind[fi-1])/5;
}
else
{
if(flap_times[fi+1] > 0)
flap_transit_rate=(flap_ind[fi] - flap_ind[fi+1])/flap_times[fi+1];
else
flap_transit_rate=(flap_ind[fi] - flap_ind[fi+1])/5;
}
if(fabs(Flap_Position - Flap_handle) > dt*flap_transit_rate)
Flap_Position+=flap_transit_rate*dt;
else
{
Flaps_In_Transit=0;
Flap_Position=Flap_handle;
}
}
}
lastFlapHandle=Flap_handle;
}
flap_transit_rate=(flap_ind[fi] - flap_ind[fi-1])/flap_times[fi];
else
flap_transit_rate=(flap_ind[fi] - flap_ind[fi-1])/5;
}
else
{
if(flap_times[fi+1] > 0)
flap_transit_rate=(flap_ind[fi] - flap_ind[fi+1])/flap_times[fi+1];
else
flap_transit_rate=(flap_ind[fi] - flap_ind[fi+1])/5;
}
if(fabs(Flap_Position - Flap_handle) > dt*flap_transit_rate)
Flap_Position+=flap_transit_rate*dt;
else
{
Flaps_In_Transit=0;
Flap_Position=Flap_handle;
}
}
}
lastFlapHandle=Flap_handle;
}
if(Aft_trim) long_trim = long_trim - trim_inc;
if(Fwd_trim) long_trim = long_trim + trim_inc;
@ -375,17 +375,17 @@ void c172_aero( SCALAR dt, int Initialize ) {
/* printf("Long_control: %7.4f, long_trim: %7.4f,DEG_TO_RAD: %7.4f, RAD_TO_DEG: %7.4f\n",Long_control,long_trim,DEG_TO_RAD,RAD_TO_DEG);
*/ /*scale pct control to degrees deflection*/
if ((Long_control+Long_trim) <= 0)
elevator=(Long_control+Long_trim)*28*DEG_TO_RAD;
elevator=(Long_control+Long_trim)*28*DEG_TO_RAD;
else
elevator=(Long_control+Long_trim)*23*DEG_TO_RAD;
elevator=(Long_control+Long_trim)*23*DEG_TO_RAD;
aileron = -1*Lat_control*17.5*DEG_TO_RAD;
rudder = -1*Rudder_pedal*16*DEG_TO_RAD;
/*
The aileron travel limits are 20 deg. TEU and 15 deg TED
but since we don't distinguish between left and right we'll
use the average here (17.5 deg)
*/
use the average here (17.5 deg)
*/
/*calculate rate derivative nondimensionalization (is that a word?) factors */
@ -396,14 +396,14 @@ void c172_aero( SCALAR dt, int Initialize ) {
*/
if(V_rel_wind > DYN_ON_SPEED)
{
cbar_2V=cbar/(2*V_rel_wind);
b_2V=b/(2*V_rel_wind);
cbar_2V=cbar/(2*V_rel_wind);
b_2V=b/(2*V_rel_wind);
}
else
{
cbar_2V=0;
b_2V=0;
}
cbar_2V=0;
b_2V=0;
}
/*calcuate the qS nondimensionalization factors*/
@ -430,7 +430,7 @@ void c172_aero( SCALAR dt, int Initialize ) {
/* printf("FP: %g\n",Flap_Position);
printf("CLo: %g\n",CLo);
printf("Cdo: %g\n",Cdo);
printf("Cmo: %g\n",Cmo); */
printf("Cmo: %g\n",Cmo); */

70
src/FDM/LaRCsim/c172_engine.c
View file

@ -1,36 +1,36 @@
/***************************************************************************
TITLE: engine.c
TITLE: engine.c
----------------------------------------------------------------------------
FUNCTION: dummy engine routine
FUNCTION: dummy engine routine
----------------------------------------------------------------------------
MODULE STATUS: incomplete
MODULE STATUS: incomplete
----------------------------------------------------------------------------
GENEALOGY: This is a renamed navion_engine.c originall written by E. Bruce
Jackson
GENEALOGY: This is a renamed navion_engine.c originall written by E. Bruce
Jackson
----------------------------------------------------------------------------
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER INFO:
CURRENT RCS HEADER INFO:
$Header$
@ -40,23 +40,23 @@ $Header$
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY: ls_model();
CALLED BY: ls_model();
----------------------------------------------------------------------------
CALLS TO: none
CALLS TO: none
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
@ -67,31 +67,31 @@ $Header$
#include "ls_cockpit.h"
#include "c172_aero.h"
extern SIM_CONTROL sim_control_;
extern SIM_CONTROL sim_control_;
void c172_engine( SCALAR dt, int init ) {
float v,h,pa;
float bhp=160;
Throttle[3] = Throttle_pct;
if ( ! Use_External_Engine ) {
/* do a crude engine power calc based on throttle position */
v=V_rel_wind;
h=Altitude;
if(V_rel_wind < 10)
v=10;
if(Altitude < 0)
h=0;
pa=(0.00144*v + 0.546)*(1 - 1.6E-5*h)*bhp;
if(pa < 0)
pa=0;
/* do a crude engine power calc based on throttle position */
v=V_rel_wind;
h=Altitude;
if(V_rel_wind < 10)
v=10;
if(Altitude < 0)
h=0;
pa=(0.00144*v + 0.546)*(1 - 1.6E-5*h)*bhp;
if(pa < 0)
pa=0;
F_X_engine = Throttle[3]*(pa*550)/v;
F_X_engine = Throttle[3]*(pa*550)/v;
} else {
/* accept external settings */
/* accept external settings */
}
/* printf("F_X_engine = %.3f\n", F_X_engine); */

316
src/FDM/LaRCsim/c172_gear.c
View file

@ -1,38 +1,38 @@
/***************************************************************************
TITLE: gear
TITLE: gear
----------------------------------------------------------------------------
FUNCTION: Landing gear model for example simulation
FUNCTION: Landing gear model for example simulation
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 931012 by E. B. Jackson
GENEALOGY: Created 931012 by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
$Header$
$Log$
@ -79,23 +79,23 @@ Updates from Tony.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
@ -157,24 +157,24 @@ void c172_gear()
*/
static int num_wheels = NUM_WHEELS; /* number of wheels */
static int num_wheels = NUM_WHEELS; /* number of wheels */
static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
{
{ 3.91, 0., 6.67 }, /*nose*/ /* in feet */
{ -1.47, 3.58, 6.71 }, /*right main*/
{ -1.47, -3.58, 6.71 }, /*left main*/
{ -15.67, 0, 2.42 } /*tail skid */
{ 3.91, 0., 6.67 }, /*nose*/ /* in feet */
{ -1.47, 3.58, 6.71 }, /*right main*/
{ -1.47, -3.58, 6.71 }, /*left main*/
{ -15.67, 0, 2.42 } /*tail skid */
};
// static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
// { -0.5, 2.5, 2.5, 0};
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1200., 900., 900., 10000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 200., 300., 300., 400. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0., 0}; /* radians, +CW */
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1200., 900., 900., 10000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 200., 300., 300., 400. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0., 0}; /* radians, +CW */
/*
* End of aircraft specific code
*/
@ -187,58 +187,58 @@ void c172_gear()
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
* V V
*/
static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
// DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
// DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
DATA temp3a[3];
// DATA temp3b[3];
DATA tempF[3];
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
@ -248,152 +248,152 @@ void c172_gear()
percent_brake[2] = Brake_pct[1];
caster_angle_rad[0] =
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
{
/* printf("%s:\n",gear_strings[i]); */
/* printf("%s:\n",gear_strings[i]); */
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
/* plus contribution due to cg velocities */
/* plus contribution due to cg velocities */
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
clear3(f_wheel_local_v);
reaction_normal_force=0;
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
clear3(f_wheel_local_v);
reaction_normal_force=0;
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
reaction_normal_force = 0.;
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
/* Calculate friction coefficients */
/* Calculate friction coefficients */
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
/* Calculate foreward and sideward reaction forces */
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
*/
/* Rotate into local (N-E-D) axes */
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
/*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
}

48
src/FDM/LaRCsim/c172_init.c
View file

@ -1,59 +1,59 @@
/***************************************************************************
TITLE: navion_init.c
TITLE: navion_init.c
----------------------------------------------------------------------------
FUNCTION: Initializes navion math model
FUNCTION: Initializes navion math model
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Renamed navion_init.c originally created on 930111 by Bruce Jackson
GENEALOGY: Renamed navion_init.c originally created on 930111 by Bruce Jackson
----------------------------------------------------------------------------
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
950314 Removed initialization of state variables, since this is
now done (version 1.4b1) in ls_init. EBJ
950406 Removed #include of "shmdefs.h"; shmdefs.h is a duplicate
of "navion.h". EBJ
CURRENT RCS HEADER:
950314 Removed initialization of state variables, since this is
now done (version 1.4b1) in ls_init. EBJ
950406 Removed #include of "shmdefs.h"; shmdefs.h is a duplicate
of "navion.h". EBJ
CURRENT RCS HEADER:
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_types.h"

954
src/FDM/LaRCsim/c172_main.c
View file

File diff suppressed because it is too large Load diff

162
src/FDM/LaRCsim/cherokee_aero.c
View file

@ -1,14 +1,14 @@
/***************************************************************************
TITLE: Cherokee_aero
TITLE: Cherokee_aero
----------------------------------------------------------------------------
FUNCTION: Linear aerodynamics model
FUNCTION: Linear aerodynamics model
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
@ -17,20 +17,20 @@
----------------------------------------------------------------------------
MODIFICATION HISTORY:
----------------------------------------------------------------------------
REFERENCES:
Based upon book:
Barnes W. McCormick,
"Aerodynamics, Aeronautics and Flight Mechanics",
John Wiley & Sons,1995, ISBN 0-471-11087-6
Based upon book:
Barnes W. McCormick,
"Aerodynamics, Aeronautics and Flight Mechanics",
John Wiley & Sons,1995, ISBN 0-471-11087-6
any suggestions, corrections, aditional data, flames, everything to
Gordan Sikic
gsikic@public.srce.hr
any suggestions, corrections, aditional data, flames, everything to
Gordan Sikic
gsikic@public.srce.hr
This source is not checked in this configuration in any way.
@ -70,101 +70,101 @@ This source is not checked in this configuration in any way.
void cherokee_aero()
/*float ** Cherokee (float t, VectorStanja &X, float *U)*/
{
static float
Cza = -19149.0/(146.69*146.69*157.5/2.0*0.00238),
Czat = -73.4*4*146.69/0.00238/157.5/5.25,
Czq = -2.655*4*2400.0/32.2/0.00238/157.5/146.69/5.25,
Cma = -21662.0 *2/146.69/0.00238/157.5/146.69/5.25,
Cmat = -892.4 *4/146.69/0.00238/157.5/146.69/5.25,
Cmq = -2405.1 *4/0.00238/157.5/146.69/5.25/5.25,
Czde = -1050.49 *2/0.00238/157.5/146.69/146.69,
Cmde = -12771.9 *2/0.00238/157.5/146.69/146.69/5.25,
Clb = -12891.0/(146.69*146.69*157.5/2.0*0.00238)/30.0,
Clp = -0.4704,
Clr = 0.1665,
Cyb = -1169.8/(146.69*146.69*157.5/2.0*0.00238),
// Cyp = -0.0342, (unused)
Cnb = 11127.2/(146.69*146.69*157.5/2.0*0.00238)/30.0,
Cnp = -0.0691,
Cnr = -0.0930,
// Cyf = -14.072/(146.69*146.69*157.5/2.0*0.00238), (unused)
// Cyps = 89.229/(146.69*146.69*157.5/2.0*0.00238), (unused)
// Clf = -5812.4/(146.69*146.69*157.5/2.0*0.00238)/30.0, //%Clda ? (unused)
// Cnf = -853.93/(146.69*146.69*157.5/2.0*0.00238)/30.0, //%Cnda ? (unused)
// Cnps = -1149.0/(146.69*146.69*157.5/2.0*0.00238)/30.0, //%Cndr ? (unused)
Cyr = 1.923/(146.69*146.69*157.5/2.0*0.00238),
static float
Cza = -19149.0/(146.69*146.69*157.5/2.0*0.00238),
Czat = -73.4*4*146.69/0.00238/157.5/5.25,
Czq = -2.655*4*2400.0/32.2/0.00238/157.5/146.69/5.25,
Cma = -21662.0 *2/146.69/0.00238/157.5/146.69/5.25,
Cmat = -892.4 *4/146.69/0.00238/157.5/146.69/5.25,
Cmq = -2405.1 *4/0.00238/157.5/146.69/5.25/5.25,
Czde = -1050.49 *2/0.00238/157.5/146.69/146.69,
Cmde = -12771.9 *2/0.00238/157.5/146.69/146.69/5.25,
Clb = -12891.0/(146.69*146.69*157.5/2.0*0.00238)/30.0,
Clp = -0.4704,
Clr = 0.1665,
Cyb = -1169.8/(146.69*146.69*157.5/2.0*0.00238),
// Cyp = -0.0342, (unused)
Cnb = 11127.2/(146.69*146.69*157.5/2.0*0.00238)/30.0,
Cnp = -0.0691,
Cnr = -0.0930,
// Cyf = -14.072/(146.69*146.69*157.5/2.0*0.00238), (unused)
// Cyps = 89.229/(146.69*146.69*157.5/2.0*0.00238), (unused)
// Clf = -5812.4/(146.69*146.69*157.5/2.0*0.00238)/30.0, //%Clda ? (unused)
// Cnf = -853.93/(146.69*146.69*157.5/2.0*0.00238)/30.0, //%Cnda ? (unused)
// Cnps = -1149.0/(146.69*146.69*157.5/2.0*0.00238)/30.0, //%Cndr ? (unused)
Cyr = 1.923/(146.69*146.69*157.5/2.0*0.00238),
Cx0 = -0.4645/(157.5*0.3048*0.3048),
Cx0 = -0.4645/(157.5*0.3048*0.3048),
Cz0 = -0.11875,
Cm0 = 0.0959,
Cz0 = -0.11875,
Cm0 = 0.0959,
Clda = -5812.4/(146.69*146.69*157.5/2.0*0.00238)/30.0, // Clf
// Cnda = -853.93/(146.69*146.69*157.5/2.0*0.00238)/30.0, // Cnf (unused)
Cndr = -1149.0/(146.69*146.69*157.5/2.0*0.00238)/30.0, // Cnps
Clda = -5812.4/(146.69*146.69*157.5/2.0*0.00238)/30.0, // Clf
// Cnda = -853.93/(146.69*146.69*157.5/2.0*0.00238)/30.0, // Cnf (unused)
Cndr = -1149.0/(146.69*146.69*157.5/2.0*0.00238)/30.0, // Cnps
/*
Possible problems: convention about positive control surfaces offset
Possible problems: convention about positive control surfaces offset
*/
elevator = 0.0, // 20.0 * 180.0/57.3 * Long_control
aileron = 0.0, // 30.0 * 180.0/57.3 * Lat_control
rudder = 0.0, // 30.0 * 180.0/57.3 * Rudder_pedal,
elevator = 0.0, // 20.0 * 180.0/57.3 * Long_control
aileron = 0.0, // 30.0 * 180.0/57.3 * Lat_control
rudder = 0.0, // 30.0 * 180.0/57.3 * Rudder_pedal,
// m = 2400/32.2, // mass
S = 157.5, // wing area
b = 30.0, // wing span
c = 5.25, // main aerodynamic chrod
// m = 2400/32.2, // mass
S = 157.5, // wing area
b = 30.0, // wing span
c = 5.25, // main aerodynamic chrod
// Ixyz[3] = {1070.0*14.59*0.3048*0.3048, 1249.0*14.59*0.3048*0.3048, 2312.0*14.59*0.3048*0.3048},
// Fa[3],
// Ma[3],
// *RetVal[4] = {&m, Ixyz, Fa, Ma};
// Ixyz[3] = {1070.0*14.59*0.3048*0.3048, 1249.0*14.59*0.3048*0.3048, 2312.0*14.59*0.3048*0.3048},
// Fa[3],
// Ma[3],
// *RetVal[4] = {&m, Ixyz, Fa, Ma};
// float
V = 0.0, // V_rel_wind
qd = 0.0, // Density*V*V/2.0, //dinamicki tlak
// float
V = 0.0, // V_rel_wind
qd = 0.0, // Density*V*V/2.0, //dinamicki tlak
Cx,Cy,Cz,
Cl,Cm,Cn,
p,q,r;
Cx,Cy,Cz,
Cl,Cm,Cn,
p,q,r;
/* derivatives are defined in "wind" axes so... */
p = P_body*Cos_alpha + R_body*Sin_alpha;
q = Q_body;
r = -P_body*Sin_alpha + R_body*Cos_alpha;
p = P_body*Cos_alpha + R_body*Sin_alpha;
q = Q_body;
r = -P_body*Sin_alpha + R_body*Cos_alpha;
Cz = Cz0 + Cza*Std_Alpha + Czat*(Std_Alpha_dot*c/2.0/V) + Czq*(q*c/2.0/V) + Czde * elevator;
Cm = Cm0 + Cma*Std_Alpha + Cmat*(Std_Alpha_dot*c/2.0/V) + Cmq*(q*c/2.0/V) + Cmde * elevator;
Cz = Cz0 + Cza*Std_Alpha + Czat*(Std_Alpha_dot*c/2.0/V) + Czq*(q*c/2.0/V) + Czde * elevator;
Cm = Cm0 + Cma*Std_Alpha + Cmat*(Std_Alpha_dot*c/2.0/V) + Cmq*(q*c/2.0/V) + Cmde * elevator;
Cx = Cx0 - (Cza*Std_Alpha)*(Cza*Std_Alpha)/(LS_PI*5.71*0.6);
Cl = Clb*Std_Beta + Clp*(p*b/2.0/V) + Clr*(r*b/2.0/V) + Clda * aileron;
Cx = Cx0 - (Cza*Std_Alpha)*(Cza*Std_Alpha)/(LS_PI*5.71*0.6);
Cl = Clb*Std_Beta + Clp*(p*b/2.0/V) + Clr*(r*b/2.0/V) + Clda * aileron;
Cy = Cyb*Std_Beta + Cyr*(r*b/2.0/V);
Cn = Cnb*Std_Beta + Cnp*(p*b/2.0/V) + Cnr*(r*b/2.0/V) + Cndr * rudder;
Cy = Cyb*Std_Beta + Cyr*(r*b/2.0/V);
Cn = Cnb*Std_Beta + Cnp*(p*b/2.0/V) + Cnr*(r*b/2.0/V) + Cndr * rudder;
/* back to body axes */
{
float
CD = Cx,
CL = Cz;
{
float
CD = Cx,
CL = Cz;
Cx = CD - CL*Sin_alpha;
Cz = CL;
}
Cx = CD - CL*Sin_alpha;
Cz = CL;
}
/* AD forces and moments */
F_X_aero = Cx*qd*S;
F_Y_aero = Cy*qd*S;
F_Z_aero = Cz*qd*S;
F_X_aero = Cx*qd*S;
F_Y_aero = Cy*qd*S;
F_Z_aero = Cz*qd*S;
M_l_aero = (Cl*Cos_alpha - Cn*Sin_alpha)*b*qd*S;
M_m_aero = Cm*c*qd*S;
M_n_aero = (Cl*Sin_alpha + Cn*Cos_alpha)*b*qd*S;
M_l_aero = (Cl*Cos_alpha - Cn*Sin_alpha)*b*qd*S;
M_m_aero = Cm*c*qd*S;
M_n_aero = (Cl*Sin_alpha + Cn*Cos_alpha)*b*qd*S;
}

74
src/FDM/LaRCsim/cherokee_engine.c
View file

@ -7,14 +7,14 @@ meaning that there is no time delay, engine acts as gain only.
Based upon book:
Barnes W. McCormick,
"Aerodynamics, Aeronautics and Flight Mechanics",
John Wiley & Sons,1995, ISBN 0-471-11087-6
Based upon book:
Barnes W. McCormick,
"Aerodynamics, Aeronautics and Flight Mechanics",
John Wiley & Sons,1995, ISBN 0-471-11087-6
any suggestions, corrections, aditional data, flames, everything to
Gordan Sikic
gsikic@public.srce.hr
any suggestions, corrections, aditional data, flames, everything to
Gordan Sikic
gsikic@public.srce.hr
This source is not checked in this configuration in any way.
@ -39,62 +39,62 @@ This source is not checked in this configuration in any way.
void cherokee_engine( SCALAR dt, int init )
{
static float
dP = (180.0-117.0)*745.7, // in Wats
dn = (2700.0-2350.0)/60.0, // d_rpm (I mean d_rps, in seconds)
D = 6.17*0.3048, // propeller diameter
dPh = (58.0-180.0)*745.7, // change of power as f-cn of height
dH = 25000.0*0.3048;
static float
dP = (180.0-117.0)*745.7, // in Wats
dn = (2700.0-2350.0)/60.0, // d_rpm (I mean d_rps, in seconds)
D = 6.17*0.3048, // propeller diameter
dPh = (58.0-180.0)*745.7, // change of power as f-cn of height
dH = 25000.0*0.3048;
float n, // rps
H,
J, // advance ratio (ratio of horizontal speed to speed of propeller's tip)
eta, // iskoristivost elise
T,
V,
P;
float n, // rps
H,
J, // advance ratio (ratio of horizontal speed to speed of propeller's tip)
eta, // iskoristivost elise
T,
V,
P;
/* copied from navion_engine.c */
if (init || sim_control_.sim_type != cockpit)
Throttle[3] = Throttle_pct;
Throttle[3] = Throttle_pct;
/*assumption -> 0.0 <= Throttle[3] <=1.0 */
P = fabs(Throttle[3])*180.0*745.7; /*180.0*745.5 ->max avail power in W */
n = dn/dP*(P-117.0*745.7) + 2350.0/60.0;
/*assumption -> 0.0 <= Throttle[3] <=1.0 */
P = fabs(Throttle[3])*180.0*745.7; /*180.0*745.5 ->max avail power in W */
n = dn/dP*(P-117.0*745.7) + 2350.0/60.0;
/* V [m/s] */
V = (V_rel_wind < 10.0*0.3048 ? 10.0 : V_rel_wind*0.3048);
V = (V_rel_wind < 10.0*0.3048 ? 10.0 : V_rel_wind*0.3048);
J = V/n/D;
J = V/n/D;
/*
propeller efficiency
propeller efficiency
if J >0.7 & J < .85
eta = 0.8;
eta = 0.8;
elseif J < 0.7
eta = (0.8-0.55)/(.7-.3)*(J-0.3) + 0.55;
eta = (0.8-0.55)/(.7-.3)*(J-0.3) + 0.55;
else
eta = (0.6-0.8)/(1.0-0.85)*(J-0.85) + 0.8;
eta = (0.6-0.8)/(1.0-0.85)*(J-0.85) + 0.8;
end
*/
eta = (J < 0.7 ? ((0.8-0.55)/(.7-.3)*(J-0.3) + 0.55) :
(J > 0.85 ? ((0.6-0.8)/(1.0-0.85)*(J-0.85) + 0.8) : 0.8));
eta = (J < 0.7 ? ((0.8-0.55)/(.7-.3)*(J-0.3) + 0.55) :
(J > 0.85 ? ((0.6-0.8)/(1.0-0.85)*(J-0.85) + 0.8) : 0.8));
/* power on Altitude (I mean Altitude, not Attitude...)*/
H = Altitude/0.3048; /* H == Altitude in [m] */
P *= (dPh/dH*H + 180.0*745.7)/(180.0*745.7);
H = Altitude/0.3048; /* H == Altitude in [m] */
P *= (dPh/dH*H + 180.0*745.7)/(180.0*745.7);
T = eta*P/V; /* T in N (Thrust in Newtons) */
T = eta*P/V; /* T in N (Thrust in Newtons) */
/*assumption: Engine's line of thrust passes through cg */
F_X_engine = T*0.2248; /* F_X_engine in lb */
F_X_engine = T*0.2248; /* F_X_engine in lb */
F_Y_engine = 0.0;
F_Z_engine = 0.0;

328
src/FDM/LaRCsim/cherokee_gear.c
View file

@ -1,38 +1,38 @@
/***************************************************************************
TITLE: gear
TITLE: gear
----------------------------------------------------------------------------
FUNCTION: Landing gear model for example simulation
FUNCTION: Landing gear model for example simulation
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 931012 by E. B. Jackson
GENEALOGY: Created 931012 by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
$Header$
$Log$
@ -67,23 +67,23 @@ Start of 0.6.x branch.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
@ -143,21 +143,21 @@ void cherokee_gear()
#define NUM_WHEELS 3
static int num_wheels = NUM_WHEELS; /* number of wheels */
static int num_wheels = NUM_WHEELS; /* number of wheels */
static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
{
{ 10., 0., 4. }, /* in feet */
{ -1., 3., 4. },
{ -1., -3., 4. }
{ 10., 0., 4. }, /* in feet */
{ -1., 3., 4. },
{ -1., -3., 4. }
};
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1500., 5000., 5000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 100., 150., 150. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0.}; /* radians, +CW */
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1500., 5000., 5000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 100., 150., 150. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0.}; /* radians, +CW */
/*
* End of aircraft specific code
*/
@ -170,51 +170,51 @@ void cherokee_gear()
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
* V V
*/
static DATA sliding_mu = 0.5;
static DATA rolling_mu = 0.01;
static DATA max_brake_mu = 0.6;
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA sliding_mu = 0.5;
static DATA rolling_mu = 0.01;
static DATA max_brake_mu = 0.6;
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
@ -225,115 +225,115 @@ void cherokee_gear()
caster_angle_rad[0] = 0.03*Rudder_pedal;
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
{
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
/* plus contribution due to cg velocities */
/* plus contribution due to cg velocities */
add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
if( d_wheel_rwy_local_v[2] < 0. )
{
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
}
/* Calculate friction coefficients */
forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu;
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
if( d_wheel_rwy_local_v[2] < 0. )
{
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
}
/* Calculate friction coefficients */
forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu;
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}

26
src/FDM/LaRCsim/cherokee_init.c
View file

@ -1,47 +1,47 @@
/***************************************************************************
TITLE: cherokee_init.c
TITLE: cherokee_init.c
----------------------------------------------------------------------------
FUNCTION: Initializes cherokee math model
FUNCTION: Initializes cherokee math model
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY:
GENEALOGY:
----------------------------------------------------------------------------
----------------------------------------------------------------------------
CURRENT RCS HEADER:
CURRENT RCS HEADER:
Well,
I do not have vorking RCS here (Sorry)
Well,
I do not have vorking RCS here (Sorry)
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_types.h"

46
src/FDM/LaRCsim/default_model_routines.c
View file

@ -1,34 +1,34 @@
/***************************************************************************
TITLE: engine.c
TITLE: engine.c
----------------------------------------------------------------------------
FUNCTION: dummy engine routine
FUNCTION: dummy engine routine
----------------------------------------------------------------------------
MODULE STATUS: incomplete
MODULE STATUS: incomplete
----------------------------------------------------------------------------
GENEALOGY: Created 15 OCT 92 as dummy routine for checkout. EBJ
GENEALOGY: Created 15 OCT 92 as dummy routine for checkout. EBJ
----------------------------------------------------------------------------
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER INFO:
CURRENT RCS HEADER INFO:
$Header$
@ -67,23 +67,23 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY: ls_model();
CALLED BY: ls_model();
----------------------------------------------------------------------------
CALLS TO: none
CALLS TO: none
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
@ -91,10 +91,10 @@ Initial Flight Gear revision.
#include "ls_types.h"
#include "default_model_routines.h"
void inertias( SCALAR dt, int Initialize ) {}
void subsystems( SCALAR dt, int Initialize ) {}
/* void engine() {} */
/* void gear() {} */
void inertias( SCALAR dt, int Initialize ) {}
void subsystems( SCALAR dt, int Initialize ) {}
/* void engine() {} */
/* void gear() {} */

64
src/FDM/LaRCsim/ls_accel.c
View file

@ -1,41 +1,41 @@
/***************************************************************************
TITLE: ls_Accel
TITLE: ls_Accel
----------------------------------------------------------------------------
FUNCTION: Sums forces and moments and calculates accelerations
FUNCTION: Sums forces and moments and calculates accelerations
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Written 920731 by Bruce Jackson. Based upon equations
GENEALOGY: Written 920731 by Bruce Jackson. Based upon equations
given in reference [1] and a Matrix-X/System Build block
diagram model of equations of motion coded by David Raney
at NASA-Langley in June of 1992.
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY:
MAINTAINED BY:
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE
DATE PURPOSE
931007 Moved calculations of auxiliary accelerations here from ls_aux.c BY
and corrected minus sign in front of A_Y_Pilot
contribution from Q_body*P_body*D_X_pilot term.
931007 Moved calculations of auxiliary accelerations here from ls_aux.c BY
and corrected minus sign in front of A_Y_Pilot
contribution from Q_body*P_body*D_X_pilot term.
940111 Changed DATA to SCALAR; updated header files
$Header$
$Log$
Revision 1.1 2002/09/10 01:14:01 curt
@ -82,7 +82,7 @@ Initial Flight Gear revision.
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
@ -100,7 +100,7 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
OUTPUTS: State derivatives
OUTPUTS: State derivatives
-------------------------------------------------------------------------*/
#include "ls_types.h"
@ -111,10 +111,10 @@ Initial Flight Gear revision.
void ls_accel( void ) {
SCALAR inv_Mass, inv_Radius;
SCALAR ixz2, c0, c1, c2, c3, c4, c5, c6, c7, c8, c9, c10;
SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
SCALAR tan_Lat_geocentric;
SCALAR inv_Mass, inv_Radius;
SCALAR ixz2, c0, c1, c2, c3, c4, c5, c6, c7, c8, c9, c10;
SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
SCALAR tan_Lat_geocentric;
/* Sum forces and moments at reference point */
@ -178,7 +178,7 @@ void ls_accel( void ) {
Q_dot_body = c5*R_body*P_body + c6*(R_body*R_body - P_body*P_body) + c7*M_m_cg;
R_dot_body = (c8*P_body + c9*R_body)*Q_body + c4*M_l_cg + c10*M_n_cg;
/* Calculate body axis accelerations (move to ls_accel?) */
/* Calculate body axis accelerations (move to ls_accel?) */
inv_Mass = 1/Mass;
@ -190,16 +190,16 @@ void ls_accel( void ) {
dy_pilot_from_cg = Dy_pilot - Dy_cg;
dz_pilot_from_cg = Dz_pilot - Dz_cg;
A_X_pilot = A_X_cg + (-R_body*R_body - Q_body*Q_body)*dx_pilot_from_cg
+ ( P_body*Q_body - R_dot_body )*dy_pilot_from_cg
+ ( P_body*R_body + Q_dot_body )*dz_pilot_from_cg;
A_Y_pilot = A_Y_cg + ( P_body*Q_body + R_dot_body )*dx_pilot_from_cg
+ (-P_body*P_body - R_body*R_body)*dy_pilot_from_cg
+ ( Q_body*R_body - P_dot_body )*dz_pilot_from_cg;
A_Z_pilot = A_Z_cg + ( P_body*R_body - Q_dot_body )*dx_pilot_from_cg
+ ( Q_body*R_body + P_dot_body )*dy_pilot_from_cg
+ (-Q_body*Q_body - P_body*P_body)*dz_pilot_from_cg;
A_X_pilot = A_X_cg + (-R_body*R_body - Q_body*Q_body)*dx_pilot_from_cg
+ ( P_body*Q_body - R_dot_body )*dy_pilot_from_cg
+ ( P_body*R_body + Q_dot_body )*dz_pilot_from_cg;
A_Y_pilot = A_Y_cg + ( P_body*Q_body + R_dot_body )*dx_pilot_from_cg
+ (-P_body*P_body - R_body*R_body)*dy_pilot_from_cg
+ ( Q_body*R_body - P_dot_body )*dz_pilot_from_cg;
A_Z_pilot = A_Z_cg + ( P_body*R_body - Q_dot_body )*dx_pilot_from_cg
+ ( Q_body*R_body + P_dot_body )*dy_pilot_from_cg
+ (-Q_body*Q_body - P_body*P_body)*dz_pilot_from_cg;
N_X_cg = INVG*A_X_cg;
N_Y_cg = INVG*A_Y_cg;
N_Z_cg = INVG*A_Z_cg;
@ -210,11 +210,11 @@ void ls_accel( void ) {
U_dot_body = T_local_to_body_11*V_dot_north + T_local_to_body_12*V_dot_east
+ T_local_to_body_13*V_dot_down - Q_total*W_body + R_total*V_body;
+ T_local_to_body_13*V_dot_down - Q_total*W_body + R_total*V_body;
V_dot_body = T_local_to_body_21*V_dot_north + T_local_to_body_22*V_dot_east
+ T_local_to_body_23*V_dot_down - R_total*U_body + P_total*W_body;
+ T_local_to_body_23*V_dot_down - R_total*U_body + P_total*W_body;
W_dot_body = T_local_to_body_31*V_dot_north + T_local_to_body_32*V_dot_east
+ T_local_to_body_33*V_dot_down - P_total*V_body + Q_total*U_body;
+ T_local_to_body_33*V_dot_down - P_total*V_body + Q_total*U_body;
/* End of ls_accel */
}

396
src/FDM/LaRCsim/ls_aux.c
View file

@ -1,47 +1,47 @@
/***************************************************************************
TITLE: ls_aux
TITLE: ls_aux
----------------------------------------------------------------------------
FUNCTION: Atmospheric and auxilary relationships for LaRCSim EOM
FUNCTION: Atmospheric and auxilary relationships for LaRCSim EOM
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 9208026 as part of C-castle simulation project.
GENEALOGY: Created 9208026 as part of C-castle simulation project.
----------------------------------------------------------------------------
DESIGNED BY: B. Jackson
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: B. Jackson
MAINTAINED BY: B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE
DATE PURPOSE
931006 Moved calculations of auxiliary accelerations from here
to ls_accel.c and corrected minus sign in front of A_Y_Pilot
contribution from Q_body*P_body*D_X_pilot term. EBJ
to ls_accel.c and corrected minus sign in front of A_Y_Pilot
contribution from Q_body*P_body*D_X_pilot term. EBJ
931014 Changed calculation of Alpha from atan to atan2 so sign is correct.
EBJ
EBJ
931220 Added calculations for static and total temperatures & pressures,
as well as dynamic and impact pressures and calibrated airspeed.
EBJ
as well as dynamic and impact pressures and calibrated airspeed.
EBJ
940111 Changed #included header files from old "ls_eom.h" to newer
"ls_types.h", "ls_constants.h" and "ls_generic.h". EBJ
"ls_types.h", "ls_constants.h" and "ls_generic.h". EBJ
950207 Changed use of "abs" to "fabs" in calculation of signU. EBJ
950228 Fixed bug in calculation of beta_dot. EBJ
950228 Fixed bug in calculation of beta_dot. EBJ
CURRENT RCS HEADER INFO:
@ -260,7 +260,7 @@ Initial Flight Gear revision.
*
* Revision 1.7 1993/10/14 11:25:38 bjax
* Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas
* larger than +/- 90 degrees are calculated correctly. EBJ
* larger than +/- 90 degrees are calculated correctly. EBJ
*
* Revision 1.6 1993/10/07 18:45:56 bjax
* A little cleanup; no significant changes. EBJ
@ -282,25 +282,25 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
of Compressible Fluid Flow", Volume I, The Ronald
Press, 1953.
REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
of Compressible Fluid Flow", Volume I, The Ronald
Press, 1953.
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_types.h"
@ -317,210 +317,210 @@ Initial Flight Gear revision.
void ls_aux( void ) {
SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
/* SCALAR inv_Mass; */
SCALAR v_XZ_plane_2, signU, v_tangential;
/* SCALAR inv_radius_ratio; */
SCALAR cos_rwy_hdg, sin_rwy_hdg;
SCALAR mach2, temp_ratio, pres_ratio;
SCALAR tmp;
SCALAR dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
/* SCALAR inv_Mass; */
SCALAR v_XZ_plane_2, signU, v_tangential;
/* SCALAR inv_radius_ratio; */
SCALAR cos_rwy_hdg, sin_rwy_hdg;
SCALAR mach2, temp_ratio, pres_ratio;
SCALAR tmp;
/* update geodetic position */
ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
&Latitude, &Altitude, &Sea_level_radius );
Longitude = Lon_geocentric - Earth_position_angle;
ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
&Latitude, &Altitude, &Sea_level_radius );
Longitude = Lon_geocentric - Earth_position_angle;
/* Calculate body axis velocities */
/* Form relative velocity vector */
/* Form relative velocity vector */
V_north_rel_ground = V_north;
V_east_rel_ground = V_east
- OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
V_down_rel_ground = V_down;
V_north_rel_ground = V_north;
V_east_rel_ground = V_east
- OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
V_down_rel_ground = V_down;
V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
V_east_rel_airmass = V_east_rel_ground - V_east_airmass;
V_down_rel_airmass = V_down_rel_ground - V_down_airmass;
U_body = T_local_to_body_11*V_north_rel_airmass
+ T_local_to_body_12*V_east_rel_airmass
+ T_local_to_body_13*V_down_rel_airmass + U_gust;
V_body = T_local_to_body_21*V_north_rel_airmass
+ T_local_to_body_22*V_east_rel_airmass
+ T_local_to_body_23*V_down_rel_airmass + V_gust;
W_body = T_local_to_body_31*V_north_rel_airmass
+ T_local_to_body_32*V_east_rel_airmass
+ T_local_to_body_33*V_down_rel_airmass + W_gust;
V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
V_east_rel_airmass = V_east_rel_ground - V_east_airmass;
V_down_rel_airmass = V_down_rel_ground - V_down_airmass;
U_body = T_local_to_body_11*V_north_rel_airmass
+ T_local_to_body_12*V_east_rel_airmass
+ T_local_to_body_13*V_down_rel_airmass + U_gust;
V_body = T_local_to_body_21*V_north_rel_airmass
+ T_local_to_body_22*V_east_rel_airmass
+ T_local_to_body_23*V_down_rel_airmass + V_gust;
W_body = T_local_to_body_31*V_north_rel_airmass
+ T_local_to_body_32*V_east_rel_airmass
+ T_local_to_body_33*V_down_rel_airmass + W_gust;
V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
/* Calculate alpha and beta rates */
/* Calculate alpha and beta rates */
v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
if (U_body == 0)
signU = 1;
else
signU = U_body/fabs(U_body);
if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
{
Std_Alpha_dot = 0;
Std_Beta_dot = 0;
}
else
{
Std_Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
v_XZ_plane_2;
Std_Beta_dot = (signU*v_XZ_plane_2*V_dot_body
- V_body*(U_body*U_dot_body + W_body*W_dot_body))
/(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
}
v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
if (U_body == 0)
signU = 1;
else
signU = U_body/fabs(U_body);
if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
{
Std_Alpha_dot = 0;
Std_Beta_dot = 0;
}
else
{
Std_Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
v_XZ_plane_2;
Std_Beta_dot = (signU*v_XZ_plane_2*V_dot_body
- V_body*(U_body*U_dot_body + W_body*W_dot_body))
/(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
}
/* Calculate flight path and other flight condition values */
if (U_body == 0)
Std_Alpha = 0;
else
Std_Alpha = atan2( W_body, U_body );
Cos_alpha = cos(Std_Alpha);
Sin_alpha = sin(Std_Alpha);
if (V_rel_wind == 0)
Std_Beta = 0;
else
Std_Beta = asin( V_body/ V_rel_wind );
Cos_beta = cos(Std_Beta);
Sin_beta = sin(Std_Beta);
V_true_kts = V_rel_wind * V_TO_KNOTS;
V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
+ V_east_rel_ground*V_east_rel_ground );
if (U_body == 0)
Std_Alpha = 0;
else
Std_Alpha = atan2( W_body, U_body );
Cos_alpha = cos(Std_Alpha);
Sin_alpha = sin(Std_Alpha);
if (V_rel_wind == 0)
Std_Beta = 0;
else
Std_Beta = asin( V_body/ V_rel_wind );
Cos_beta = cos(Std_Beta);
Sin_beta = sin(Std_Beta);
V_true_kts = V_rel_wind * V_TO_KNOTS;
V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
+ V_east_rel_ground*V_east_rel_ground );
V_rel_ground = sqrt(V_ground_speed*V_ground_speed
+ V_down_rel_ground*V_down_rel_ground );
v_tangential = sqrt(V_north*V_north + V_east*V_east);
V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
if( (V_ground_speed == 0) && (V_down == 0) )
Gamma_vert_rad = 0;
else
Gamma_vert_rad = atan2( -V_down, V_ground_speed );
if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
Gamma_horiz_rad = 0;
else
Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
if (Gamma_horiz_rad < 0)
Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
/* Calculate local gravity */
ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
V_rel_ground = sqrt(V_ground_speed*V_ground_speed
+ V_down_rel_ground*V_down_rel_ground );
v_tangential = sqrt(V_north*V_north + V_east*V_east);
V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
if( (V_ground_speed == 0) && (V_down == 0) )
Gamma_vert_rad = 0;
else
Gamma_vert_rad = atan2( -V_down, V_ground_speed );
if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
Gamma_horiz_rad = 0;
else
Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
if (Gamma_horiz_rad < 0)
Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
/* Calculate local gravity */
ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
/* call function for (smoothed) density ratio, sonic velocity, and
ambient pressure */
ambient pressure */
ls_atmos(Altitude, &Sigma, &V_sound,
&Static_temperature, &Static_pressure);
Density = Sigma*SEA_LEVEL_DENSITY;
Mach_number = V_rel_wind / V_sound;
V_equiv = V_rel_wind*sqrt(Sigma);
V_equiv_kts = V_equiv*V_TO_KNOTS;
ls_atmos(Altitude, &Sigma, &V_sound,
&Static_temperature, &Static_pressure);
Density = Sigma*SEA_LEVEL_DENSITY;
Mach_number = V_rel_wind / V_sound;
V_equiv = V_rel_wind*sqrt(Sigma);
V_equiv_kts = V_equiv*V_TO_KNOTS;
/* calculate temperature and pressure ratios (from [1]) */
mach2 = Mach_number*Mach_number;
temp_ratio = 1.0 + 0.2*mach2;
tmp = 3.5;
pres_ratio = pow( temp_ratio, tmp );
mach2 = Mach_number*Mach_number;
temp_ratio = 1.0 + 0.2*mach2;
tmp = 3.5;
pres_ratio = pow( temp_ratio, tmp );
Total_temperature = temp_ratio*Static_temperature;
Total_pressure = pres_ratio*Static_pressure;
Total_temperature = temp_ratio*Static_temperature;
Total_pressure = pres_ratio*Static_pressure;
/* calculate impact and dynamic pressures */
Impact_pressure = Total_pressure - Static_pressure;
Impact_pressure = Total_pressure - Static_pressure;
Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
/* calculate calibrated airspeed indication */
V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
V_calibrated_kts = V_calibrated*V_TO_KNOTS;
Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
V_calibrated_kts = V_calibrated*V_TO_KNOTS;
Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
/* Determine location in runway coordinates */
Radius_to_rwy = Sea_level_radius + Runway_altitude;
cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
*(Longitude - Runway_longitude);
D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy;
X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
+ D_cg_east_of_rwy*sin_rwy_hdg;
Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
+ D_cg_east_of_rwy*cos_rwy_hdg;
H_cg_rwy = D_cg_above_rwy;
dx_pilot_from_cg = Dx_pilot - Dx_cg;
dy_pilot_from_cg = Dy_pilot - Dy_cg;
dz_pilot_from_cg = Dz_pilot - Dz_cg;
Radius_to_rwy = Sea_level_radius + Runway_altitude;
cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
*(Longitude - Runway_longitude);
D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy;
X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
+ D_cg_east_of_rwy*sin_rwy_hdg;
Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
+ D_cg_east_of_rwy*cos_rwy_hdg;
H_cg_rwy = D_cg_above_rwy;
dx_pilot_from_cg = Dx_pilot - Dx_cg;
dy_pilot_from_cg = Dy_pilot - Dy_cg;
dz_pilot_from_cg = Dz_pilot - Dz_cg;
D_pilot_north_of_rwy = D_cg_north_of_rwy
+ T_local_to_body_11*dx_pilot_from_cg
+ T_local_to_body_21*dy_pilot_from_cg
+ T_local_to_body_31*dz_pilot_from_cg;
D_pilot_east_of_rwy = D_cg_east_of_rwy
+ T_local_to_body_12*dx_pilot_from_cg
+ T_local_to_body_22*dy_pilot_from_cg
+ T_local_to_body_32*dz_pilot_from_cg;
D_pilot_above_rwy = D_cg_above_rwy
- T_local_to_body_13*dx_pilot_from_cg
- T_local_to_body_23*dy_pilot_from_cg
- T_local_to_body_33*dz_pilot_from_cg;
X_pilot_rwy = D_pilot_north_of_rwy*cos_rwy_hdg
+ D_pilot_east_of_rwy*sin_rwy_hdg;
Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
+ D_pilot_east_of_rwy*cos_rwy_hdg;
H_pilot_rwy = D_pilot_above_rwy;
D_pilot_north_of_rwy = D_cg_north_of_rwy
+ T_local_to_body_11*dx_pilot_from_cg
+ T_local_to_body_21*dy_pilot_from_cg
+ T_local_to_body_31*dz_pilot_from_cg;
D_pilot_east_of_rwy = D_cg_east_of_rwy
+ T_local_to_body_12*dx_pilot_from_cg
+ T_local_to_body_22*dy_pilot_from_cg
+ T_local_to_body_32*dz_pilot_from_cg;
D_pilot_above_rwy = D_cg_above_rwy
- T_local_to_body_13*dx_pilot_from_cg
- T_local_to_body_23*dy_pilot_from_cg
- T_local_to_body_33*dz_pilot_from_cg;
X_pilot_rwy = D_pilot_north_of_rwy*cos_rwy_hdg
+ D_pilot_east_of_rwy*sin_rwy_hdg;
Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
+ D_pilot_east_of_rwy*cos_rwy_hdg;
H_pilot_rwy = D_pilot_above_rwy;
/* Calculate Euler rates */
Sin_phi = sin(Phi);
Cos_phi = cos(Phi);
Sin_theta = sin(Theta);
Cos_theta = cos(Theta);
Sin_psi = sin(Psi);
Cos_psi = cos(Psi);
if( Cos_theta == 0 )
Psi_dot = 0;
else
Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
Phi_dot = P_total + Psi_dot*Sin_theta;
Sin_phi = sin(Phi);
Cos_phi = cos(Phi);
Sin_theta = sin(Theta);
Cos_theta = cos(Theta);
Sin_psi = sin(Psi);
Cos_psi = cos(Psi);
if( Cos_theta == 0 )
Psi_dot = 0;
else
Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
Phi_dot = P_total + Psi_dot*Sin_theta;
/* end of ls_aux */
}

68
src/FDM/LaRCsim/ls_cockpit.h
View file

@ -1,37 +1,37 @@
/***************************************************************************
TITLE: ls_cockpit.h
TITLE: ls_cockpit.h
----------------------------------------------------------------------------
FUNCTION: Header for cockpit IO
FUNCTION: Header for cockpit IO
----------------------------------------------------------------------------
MODULE STATUS: Developmental
MODULE STATUS: Developmental
----------------------------------------------------------------------------
GENEALOGY: Created 20 DEC 93 by E. B. Jackson
GENEALOGY: Created 20 DEC 93 by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
950314 Added "throttle_pct" field to cockpit header for both
display and trim purposes. EBJ
CURRENT RCS HEADER:
950314 Added "throttle_pct" field to cockpit header for both
display and trim purposes. EBJ
CURRENT RCS HEADER:
$Header$
$Log$
@ -103,24 +103,24 @@ typedef struct {
extern COCKPIT cockpit_;
#define Left_button cockpit_.left_pb_on_stick
#define Right_button cockpit_.right_pb_on_stick
#define Rudder_pedal cockpit_.rudder_pedal
#define Flap_handle cockpit_.flap_handle
#define Throttle cockpit_.throttle
#define Throttle_pct cockpit_.throttle_pct
#define First_trigger cockpit_.trig_pos_1
#define Second_trigger cockpit_.trig_pos_2
#define Long_control cockpit_.long_stick
#define Left_button cockpit_.left_pb_on_stick
#define Right_button cockpit_.right_pb_on_stick
#define Rudder_pedal cockpit_.rudder_pedal
#define Flap_handle cockpit_.flap_handle
#define Throttle cockpit_.throttle
#define Throttle_pct cockpit_.throttle_pct
#define First_trigger cockpit_.trig_pos_1
#define Second_trigger cockpit_.trig_pos_2
#define Long_control cockpit_.long_stick
#define Long_trim cockpit_.long_trim
#define Lat_control cockpit_.lat_stick
#define Fwd_trim cockpit_.forward_trim
#define Aft_trim cockpit_.aft_trim
#define Left_trim cockpit_.left_trim
#define Right_trim cockpit_.right_trim
#define SB_extend cockpit_.sb_extend
#define SB_retract cockpit_.sb_retract
#define Gear_sel_up cockpit_.gear_sel_up
#define Lat_control cockpit_.lat_stick
#define Fwd_trim cockpit_.forward_trim
#define Aft_trim cockpit_.aft_trim
#define Left_trim cockpit_.left_trim
#define Right_trim cockpit_.right_trim
#define SB_extend cockpit_.sb_extend
#define SB_retract cockpit_.sb_retract
#define Gear_sel_up cockpit_.gear_sel_up
#define Brake_pct cockpit_.brake_pct

134
src/FDM/LaRCsim/ls_constants.h
View file

@ -1,67 +1,67 @@
/***************************************************************************
TITLE: ls_constants.h
TITLE: ls_constants.h
----------------------------------------------------------------------------
FUNCTION: LaRCSim constants definition header file
FUNCTION: LaRCSim constants definition header file
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 15 DEC 1993 by Bruce Jackson; was part of
old ls_eom.h header file
GENEALOGY: Created 15 DEC 1993 by Bruce Jackson; was part of
old ls_eom.h header file
----------------------------------------------------------------------------
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
----------------------------------------------------------------------------
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
[ 3] Beyer, William H., editor: "CRC Standard Mathematical
Tables, 28th edition", CRC Press, Boca Raton, FL, 1987,
ISBN 0-8493-0628-0
[ 4] Dowdy, M. C.; Jackson, E. B.; and Nichols, J. H.:
"Controls Analysis and Simulation Test Loop Environ-
ment (CASTLE) Programmer's Guide, Version 1.3",
NATC TM 89-11, 30 March 1989.
[ 5] Halliday, David; and Resnick, Robert: "Fundamentals
of Physics, Revised Printing", Wiley and Sons, 1974.
ISBN 0-471-34431-1
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
[ 3] Beyer, William H., editor: "CRC Standard Mathematical
Tables, 28th edition", CRC Press, Boca Raton, FL, 1987,
ISBN 0-8493-0628-0
[ 4] Dowdy, M. C.; Jackson, E. B.; and Nichols, J. H.:
"Controls Analysis and Simulation Test Loop Environ-
ment (CASTLE) Programmer's Guide, Version 1.3",
NATC TM 89-11, 30 March 1989.
[ 5] Halliday, David; and Resnick, Robert: "Fundamentals
of Physics, Revised Printing", Wiley and Sons, 1974.
ISBN 0-471-34431-1
[ 6] Anon: "U. S. Standard Atmosphere, 1962"
[ 7] Anon: "Aeronautical Vest Pocket Handbook, 17th edition",
Pratt & Whitney Aircraft Group, Dec. 1977
[ 8] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
[ 6] Anon: "U. S. Standard Atmosphere, 1962"
[ 7] Anon: "Aeronautical Vest Pocket Handbook, 17th edition",
Pratt & Whitney Aircraft Group, Dec. 1977
[ 8] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
--------------------------------------------------------------------------*/
@ -81,22 +81,22 @@
#endif
/* Define constants (note: many factors will need to change for other
systems of measure) */
systems of measure) */
/* Value of Pi from ref [3] */
#define LS_PI 3.14159265358979323846264338327950288419716939967511
#define LS_PI 3.14159265358979323846264338327950288419716939967511
/* Value of earth radius from [8], ft */
#define EQUATORIAL_RADIUS 20925650.
#define EQUATORIAL_RADIUS 20925650.
#define RESQ 437882827922500.
/* Value of earth flattening parameter from ref [8]
Note: FP = f
E = 1-f
EPS = sqrt(1-(1-f)^2) */
#define FP .003352813178
/* Value of earth flattening parameter from ref [8]
Note: FP = f
E = 1-f
EPS = sqrt(1-(1-f)^2) */
#define FP .003352813178
#define E .996647186
#define EPS .081819221
#define INVG .031080997
@ -105,21 +105,21 @@
#define OMEGA_EARTH .00007272205217
/* miscellaneous units conversions (ref [7]) */
#define V_TO_KNOTS 0.5921
#define DEG_TO_RAD 0.017453292
#define RAD_TO_DEG 57.29577951
#define FT_TO_METERS 0.3048
#define METERS_TO_FT 3.2808
#define K_TO_R 1.8
#define R_TO_K 0.55555556
#define NSM_TO_PSF 0.02088547
#define PSF_TO_NSM 47.8801826
#define KCM_TO_SCF 0.00194106
#define SCF_TO_KCM 515.183616
#define V_TO_KNOTS 0.5921
#define DEG_TO_RAD 0.017453292
#define RAD_TO_DEG 57.29577951
#define FT_TO_METERS 0.3048
#define METERS_TO_FT 3.2808
#define K_TO_R 1.8
#define R_TO_K 0.55555556
#define NSM_TO_PSF 0.02088547
#define PSF_TO_NSM 47.8801826
#define KCM_TO_SCF 0.00194106
#define SCF_TO_KCM 515.183616
/* ENGLISH Atmospheric reference properties [6] */
#define SEA_LEVEL_DENSITY 0.002376888
#define SEA_LEVEL_DENSITY 0.002376888
#endif

568
src/FDM/LaRCsim/ls_generic.h
View file

@ -1,68 +1,68 @@
/***************************************************************************
TITLE: ls_generic.h
TITLE: ls_generic.h
----------------------------------------------------------------------------
FUNCTION: LaRCSim generic parameters header file
FUNCTION: LaRCSim generic parameters header file
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 15 DEC 1993 by Bruce Jackson;
was part of old ls_eom.h header
GENEALOGY: Created 15 DEC 1993 by Bruce Jackson;
was part of old ls_eom.h header
----------------------------------------------------------------------------
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
----------------------------------------------------------------------------
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
[ 3] Beyer, William H., editor: "CRC Standard Mathematical
Tables, 28th edition", CRC Press, Boca Raton, FL, 1987,
ISBN 0-8493-0628-0
[ 4] Dowdy, M. C.; Jackson, E. B.; and Nichols, J. H.:
"Controls Analysis and Simulation Test Loop Environ-
ment (CASTLE) Programmer's Guide, Version 1.3",
NATC TM 89-11, 30 March 1989.
[ 5] Halliday, David; and Resnick, Robert: "Fundamentals
of Physics, Revised Printing", Wiley and Sons, 1974.
ISBN 0-471-34431-1
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
[ 3] Beyer, William H., editor: "CRC Standard Mathematical
Tables, 28th edition", CRC Press, Boca Raton, FL, 1987,
ISBN 0-8493-0628-0
[ 4] Dowdy, M. C.; Jackson, E. B.; and Nichols, J. H.:
"Controls Analysis and Simulation Test Loop Environ-
ment (CASTLE) Programmer's Guide, Version 1.3",
NATC TM 89-11, 30 March 1989.
[ 5] Halliday, David; and Resnick, Robert: "Fundamentals
of Physics, Revised Printing", Wiley and Sons, 1974.
ISBN 0-471-34431-1
[ 6] Anon: "U. S. Standard Atmosphere, 1962"
[ 7] Anon: "Aeronautical Vest Pocket Handbook, 17th edition",
Pratt & Whitney Aircraft Group, Dec. 1977
[ 8] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
[ 6] Anon: "U. S. Standard Atmosphere, 1962"
[ 7] Anon: "Aeronautical Vest Pocket Handbook, 17th edition",
Pratt & Whitney Aircraft Group, Dec. 1977
[ 8] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
--------------------------------------------------------------------------*/
@ -82,340 +82,340 @@ extern "C" {
typedef struct {
/*================== Mass properties and geometry values ==================*/
DATA mass, i_xx, i_yy, i_zz, i_xz; /* Inertias */
#define Mass generic_.mass
#define I_xx generic_.i_xx
#define I_yy generic_.i_yy
#define I_zz generic_.i_zz
#define I_xz generic_.i_xz
VECTOR_3 d_pilot_rp_body_v; /* Pilot location rel to ref pt */
#define D_pilot_rp_body_v generic_.d_pilot_rp_body_v
#define Dx_pilot generic_.d_pilot_rp_body_v[0]
#define Dy_pilot generic_.d_pilot_rp_body_v[1]
#define Dz_pilot generic_.d_pilot_rp_body_v[2]
DATA mass, i_xx, i_yy, i_zz, i_xz; /* Inertias */
#define Mass generic_.mass
#define I_xx generic_.i_xx
#define I_yy generic_.i_yy
#define I_zz generic_.i_zz
#define I_xz generic_.i_xz
VECTOR_3 d_pilot_rp_body_v; /* Pilot location rel to ref pt */
#define D_pilot_rp_body_v generic_.d_pilot_rp_body_v
#define Dx_pilot generic_.d_pilot_rp_body_v[0]
#define Dy_pilot generic_.d_pilot_rp_body_v[1]
#define Dz_pilot generic_.d_pilot_rp_body_v[2]
VECTOR_3 d_cg_rp_body_v; /* CG position w.r.t. ref. point */
#define D_cg_rp_body_v generic_.d_cg_rp_body_v
#define Dx_cg generic_.d_cg_rp_body_v[0]
#define Dy_cg generic_.d_cg_rp_body_v[1]
#define Dz_cg generic_.d_cg_rp_body_v[2]
VECTOR_3 d_cg_rp_body_v; /* CG position w.r.t. ref. point */
#define D_cg_rp_body_v generic_.d_cg_rp_body_v
#define Dx_cg generic_.d_cg_rp_body_v[0]
#define Dy_cg generic_.d_cg_rp_body_v[1]
#define Dz_cg generic_.d_cg_rp_body_v[2]
/*================================ Forces =================================*/
VECTOR_3 f_body_total_v;
#define F_body_total_v generic_.f_body_total_v
#define F_X generic_.f_body_total_v[0]
#define F_Y generic_.f_body_total_v[1]
#define F_Z generic_.f_body_total_v[2]
#define F_body_total_v generic_.f_body_total_v
#define F_X generic_.f_body_total_v[0]
#define F_Y generic_.f_body_total_v[1]
#define F_Z generic_.f_body_total_v[2]
VECTOR_3 f_local_total_v;
#define F_local_total_v generic_.f_local_total_v
#define F_north generic_.f_local_total_v[0]
#define F_east generic_.f_local_total_v[1]
#define F_down generic_.f_local_total_v[2]
#define F_local_total_v generic_.f_local_total_v
#define F_north generic_.f_local_total_v[0]
#define F_east generic_.f_local_total_v[1]
#define F_down generic_.f_local_total_v[2]
VECTOR_3 f_aero_v;
#define F_aero_v generic_.f_aero_v
#define F_X_aero generic_.f_aero_v[0]
#define F_Y_aero generic_.f_aero_v[1]
#define F_Z_aero generic_.f_aero_v[2]
#define F_aero_v generic_.f_aero_v
#define F_X_aero generic_.f_aero_v[0]
#define F_Y_aero generic_.f_aero_v[1]
#define F_Z_aero generic_.f_aero_v[2]
VECTOR_3 f_engine_v;
#define F_engine_v generic_.f_engine_v
#define F_X_engine generic_.f_engine_v[0]
#define F_Y_engine generic_.f_engine_v[1]
#define F_Z_engine generic_.f_engine_v[2]
#define F_engine_v generic_.f_engine_v
#define F_X_engine generic_.f_engine_v[0]
#define F_Y_engine generic_.f_engine_v[1]
#define F_Z_engine generic_.f_engine_v[2]
int use_external_engine;
#define Use_External_Engine generic_.use_external_engine
VECTOR_3 f_gear_v;
#define F_gear_v generic_.f_gear_v
#define F_X_gear generic_.f_gear_v[0]
#define F_Y_gear generic_.f_gear_v[1]
#define F_Z_gear generic_.f_gear_v[2]
#define F_gear_v generic_.f_gear_v
#define F_X_gear generic_.f_gear_v[0]
#define F_Y_gear generic_.f_gear_v[1]
#define F_Z_gear generic_.f_gear_v[2]
/*================================ Moments ================================*/
VECTOR_3 m_total_rp_v;
#define M_total_rp_v generic_.m_total_rp_v
#define M_l_rp generic_.m_total_rp_v[0]
#define M_m_rp generic_.m_total_rp_v[1]
#define M_n_rp generic_.m_total_rp_v[2]
#define M_total_rp_v generic_.m_total_rp_v
#define M_l_rp generic_.m_total_rp_v[0]
#define M_m_rp generic_.m_total_rp_v[1]
#define M_n_rp generic_.m_total_rp_v[2]
VECTOR_3 m_total_cg_v;
#define M_total_cg_v generic_.m_total_cg_v
#define M_l_cg generic_.m_total_cg_v[0]
#define M_m_cg generic_.m_total_cg_v[1]
#define M_n_cg generic_.m_total_cg_v[2]
#define M_total_cg_v generic_.m_total_cg_v
#define M_l_cg generic_.m_total_cg_v[0]
#define M_m_cg generic_.m_total_cg_v[1]
#define M_n_cg generic_.m_total_cg_v[2]
VECTOR_3 m_aero_v;
#define M_aero_v generic_.m_aero_v
#define M_l_aero generic_.m_aero_v[0]
#define M_m_aero generic_.m_aero_v[1]
#define M_n_aero generic_.m_aero_v[2]
#define M_aero_v generic_.m_aero_v
#define M_l_aero generic_.m_aero_v[0]
#define M_m_aero generic_.m_aero_v[1]
#define M_n_aero generic_.m_aero_v[2]
VECTOR_3 m_engine_v;
#define M_engine_v generic_.m_engine_v
#define M_l_engine generic_.m_engine_v[0]
#define M_m_engine generic_.m_engine_v[1]
#define M_n_engine generic_.m_engine_v[2]
#define M_engine_v generic_.m_engine_v
#define M_l_engine generic_.m_engine_v[0]
#define M_m_engine generic_.m_engine_v[1]
#define M_n_engine generic_.m_engine_v[2]
VECTOR_3 m_gear_v;
#define M_gear_v generic_.m_gear_v
#define M_l_gear generic_.m_gear_v[0]
#define M_m_gear generic_.m_gear_v[1]
#define M_n_gear generic_.m_gear_v[2]
#define M_gear_v generic_.m_gear_v
#define M_l_gear generic_.m_gear_v[0]
#define M_m_gear generic_.m_gear_v[1]
#define M_n_gear generic_.m_gear_v[2]
/*============================== Accelerations ============================*/
VECTOR_3 v_dot_local_v;
#define V_dot_local_v generic_.v_dot_local_v
#define V_dot_north generic_.v_dot_local_v[0]
#define V_dot_east generic_.v_dot_local_v[1]
#define V_dot_down generic_.v_dot_local_v[2]
#define V_dot_local_v generic_.v_dot_local_v
#define V_dot_north generic_.v_dot_local_v[0]
#define V_dot_east generic_.v_dot_local_v[1]
#define V_dot_down generic_.v_dot_local_v[2]
VECTOR_3 v_dot_body_v;
#define V_dot_body_v generic_.v_dot_body_v
#define U_dot_body generic_.v_dot_body_v[0]
#define V_dot_body generic_.v_dot_body_v[1]
#define W_dot_body generic_.v_dot_body_v[2]
#define V_dot_body_v generic_.v_dot_body_v
#define U_dot_body generic_.v_dot_body_v[0]
#define V_dot_body generic_.v_dot_body_v[1]
#define W_dot_body generic_.v_dot_body_v[2]
VECTOR_3 a_cg_body_v;
#define A_cg_body_v generic_.a_cg_body_v
#define A_X_cg generic_.a_cg_body_v[0]
#define A_Y_cg generic_.a_cg_body_v[1]
#define A_Z_cg generic_.a_cg_body_v[2]
#define A_cg_body_v generic_.a_cg_body_v
#define A_X_cg generic_.a_cg_body_v[0]
#define A_Y_cg generic_.a_cg_body_v[1]
#define A_Z_cg generic_.a_cg_body_v[2]
VECTOR_3 a_pilot_body_v;
#define A_pilot_body_v generic_.a_pilot_body_v
#define A_X_pilot generic_.a_pilot_body_v[0]
#define A_Y_pilot generic_.a_pilot_body_v[1]
#define A_Z_pilot generic_.a_pilot_body_v[2]
#define A_pilot_body_v generic_.a_pilot_body_v
#define A_X_pilot generic_.a_pilot_body_v[0]
#define A_Y_pilot generic_.a_pilot_body_v[1]
#define A_Z_pilot generic_.a_pilot_body_v[2]
VECTOR_3 n_cg_body_v;
#define N_cg_body_v generic_.n_cg_body_v
#define N_X_cg generic_.n_cg_body_v[0]
#define N_Y_cg generic_.n_cg_body_v[1]
#define N_Z_cg generic_.n_cg_body_v[2]
#define N_cg_body_v generic_.n_cg_body_v
#define N_X_cg generic_.n_cg_body_v[0]
#define N_Y_cg generic_.n_cg_body_v[1]
#define N_Z_cg generic_.n_cg_body_v[2]
VECTOR_3 n_pilot_body_v;
#define N_pilot_body_v generic_.n_pilot_body_v
#define N_X_pilot generic_.n_pilot_body_v[0]
#define N_Y_pilot generic_.n_pilot_body_v[1]
#define N_Z_pilot generic_.n_pilot_body_v[2]
#define N_pilot_body_v generic_.n_pilot_body_v
#define N_X_pilot generic_.n_pilot_body_v[0]
#define N_Y_pilot generic_.n_pilot_body_v[1]
#define N_Z_pilot generic_.n_pilot_body_v[2]
VECTOR_3 omega_dot_body_v;
#define Omega_dot_body_v generic_.omega_dot_body_v
#define P_dot_body generic_.omega_dot_body_v[0]
#define Q_dot_body generic_.omega_dot_body_v[1]
#define R_dot_body generic_.omega_dot_body_v[2]
#define Omega_dot_body_v generic_.omega_dot_body_v
#define P_dot_body generic_.omega_dot_body_v[0]
#define Q_dot_body generic_.omega_dot_body_v[1]
#define R_dot_body generic_.omega_dot_body_v[2]
/*============================== Velocities ===============================*/
VECTOR_3 v_local_v;
#define V_local_v generic_.v_local_v
#define V_north generic_.v_local_v[0]
#define V_east generic_.v_local_v[1]
#define V_down generic_.v_local_v[2]
#define V_local_v generic_.v_local_v
#define V_north generic_.v_local_v[0]
#define V_east generic_.v_local_v[1]
#define V_down generic_.v_local_v[2]
VECTOR_3 v_local_rel_ground_v; /* V rel w.r.t. earth surface */
#define V_local_rel_ground_v generic_.v_local_rel_ground_v
#define V_north_rel_ground generic_.v_local_rel_ground_v[0]
#define V_east_rel_ground generic_.v_local_rel_ground_v[1]
#define V_down_rel_ground generic_.v_local_rel_ground_v[2]
VECTOR_3 v_local_rel_ground_v; /* V rel w.r.t. earth surface */
#define V_local_rel_ground_v generic_.v_local_rel_ground_v
#define V_north_rel_ground generic_.v_local_rel_ground_v[0]
#define V_east_rel_ground generic_.v_local_rel_ground_v[1]
#define V_down_rel_ground generic_.v_local_rel_ground_v[2]
VECTOR_3 v_local_airmass_v; /* velocity of airmass (steady winds) */
#define V_local_airmass_v generic_.v_local_airmass_v
#define V_north_airmass generic_.v_local_airmass_v[0]
#define V_east_airmass generic_.v_local_airmass_v[1]
#define V_down_airmass generic_.v_local_airmass_v[2]
VECTOR_3 v_local_airmass_v; /* velocity of airmass (steady winds) */
#define V_local_airmass_v generic_.v_local_airmass_v
#define V_north_airmass generic_.v_local_airmass_v[0]
#define V_east_airmass generic_.v_local_airmass_v[1]
#define V_down_airmass generic_.v_local_airmass_v[2]
VECTOR_3 v_local_rel_airmass_v; /* velocity of veh. relative to airmass */
#define V_local_rel_airmass_v generic_.v_local_rel_airmass_v
#define V_north_rel_airmass generic_.v_local_rel_airmass_v[0]
#define V_east_rel_airmass generic_.v_local_rel_airmass_v[1]
#define V_down_rel_airmass generic_.v_local_rel_airmass_v[2]
VECTOR_3 v_local_rel_airmass_v; /* velocity of veh. relative to airmass */
#define V_local_rel_airmass_v generic_.v_local_rel_airmass_v
#define V_north_rel_airmass generic_.v_local_rel_airmass_v[0]
#define V_east_rel_airmass generic_.v_local_rel_airmass_v[1]
#define V_down_rel_airmass generic_.v_local_rel_airmass_v[2]
VECTOR_3 v_local_gust_v; /* linear turbulence components, L frame */
#define V_local_gust_v generic_.v_local_gust_v
#define U_gust generic_.v_local_gust_v[0]
#define V_gust generic_.v_local_gust_v[1]
#define W_gust generic_.v_local_gust_v[2]
#define V_local_gust_v generic_.v_local_gust_v
#define U_gust generic_.v_local_gust_v[0]
#define V_gust generic_.v_local_gust_v[1]
#define W_gust generic_.v_local_gust_v[2]
VECTOR_3 v_wind_body_v; /* Wind-relative velocities in body axis */
#define V_wind_body_v generic_.v_wind_body_v
#define U_body generic_.v_wind_body_v[0]
#define V_body generic_.v_wind_body_v[1]
#define W_body generic_.v_wind_body_v[2]
VECTOR_3 v_wind_body_v; /* Wind-relative velocities in body axis */
#define V_wind_body_v generic_.v_wind_body_v
#define U_body generic_.v_wind_body_v[0]
#define V_body generic_.v_wind_body_v[1]
#define W_body generic_.v_wind_body_v[2]
DATA v_rel_wind, v_true_kts, v_rel_ground, v_inertial;
DATA v_ground_speed, v_equiv, v_equiv_kts;
DATA v_calibrated, v_calibrated_kts;
#define V_rel_wind generic_.v_rel_wind
#define V_true_kts generic_.v_true_kts
#define V_rel_ground generic_.v_rel_ground
#define V_inertial generic_.v_inertial
#define V_ground_speed generic_.v_ground_speed
#define V_equiv generic_.v_equiv
#define V_equiv_kts generic_.v_equiv_kts
#define V_calibrated generic_.v_calibrated
#define V_calibrated_kts generic_.v_calibrated_kts
#define V_rel_wind generic_.v_rel_wind
#define V_true_kts generic_.v_true_kts
#define V_rel_ground generic_.v_rel_ground
#define V_inertial generic_.v_inertial
#define V_ground_speed generic_.v_ground_speed
#define V_equiv generic_.v_equiv
#define V_equiv_kts generic_.v_equiv_kts
#define V_calibrated generic_.v_calibrated
#define V_calibrated_kts generic_.v_calibrated_kts
VECTOR_3 omega_body_v; /* Angular B rates */
#define Omega_body_v generic_.omega_body_v
#define P_body generic_.omega_body_v[0]
#define Q_body generic_.omega_body_v[1]
#define R_body generic_.omega_body_v[2]
VECTOR_3 omega_local_v; /* Angular L rates */
#define Omega_local_v generic_.omega_local_v
#define P_local generic_.omega_local_v[0]
#define Q_local generic_.omega_local_v[1]
#define R_local generic_.omega_local_v[2]
VECTOR_3 omega_body_v; /* Angular B rates */
#define Omega_body_v generic_.omega_body_v
#define P_body generic_.omega_body_v[0]
#define Q_body generic_.omega_body_v[1]
#define R_body generic_.omega_body_v[2]
VECTOR_3 omega_local_v; /* Angular L rates */
#define Omega_local_v generic_.omega_local_v
#define P_local generic_.omega_local_v[0]
#define Q_local generic_.omega_local_v[1]
#define R_local generic_.omega_local_v[2]
VECTOR_3 omega_total_v; /* Diff btw B & L */
#define Omega_total_v generic_.omega_total_v
#define P_total generic_.omega_total_v[0]
#define Q_total generic_.omega_total_v[1]
#define R_total generic_.omega_total_v[2]
VECTOR_3 omega_total_v; /* Diff btw B & L */
#define Omega_total_v generic_.omega_total_v
#define P_total generic_.omega_total_v[0]
#define Q_total generic_.omega_total_v[1]
#define R_total generic_.omega_total_v[2]
VECTOR_3 euler_rates_v;
#define Euler_rates_v generic_.euler_rates_v
#define Phi_dot generic_.euler_rates_v[0]
#define Theta_dot generic_.euler_rates_v[1]
#define Psi_dot generic_.euler_rates_v[2]
#define Euler_rates_v generic_.euler_rates_v
#define Phi_dot generic_.euler_rates_v[0]
#define Theta_dot generic_.euler_rates_v[1]
#define Psi_dot generic_.euler_rates_v[2]
VECTOR_3 geocentric_rates_v; /* Geocentric linear velocities */
#define Geocentric_rates_v generic_.geocentric_rates_v
#define Latitude_dot generic_.geocentric_rates_v[0]
#define Longitude_dot generic_.geocentric_rates_v[1]
#define Radius_dot generic_.geocentric_rates_v[2]
VECTOR_3 geocentric_rates_v; /* Geocentric linear velocities */
#define Geocentric_rates_v generic_.geocentric_rates_v
#define Latitude_dot generic_.geocentric_rates_v[0]
#define Longitude_dot generic_.geocentric_rates_v[1]
#define Radius_dot generic_.geocentric_rates_v[2]
/*=============================== Positions ===============================*/
VECTOR_3 geocentric_position_v;
#define Geocentric_position_v generic_.geocentric_position_v
#define Lat_geocentric generic_.geocentric_position_v[0]
#define Lon_geocentric generic_.geocentric_position_v[1]
#define Radius_to_vehicle generic_.geocentric_position_v[2]
#define Geocentric_position_v generic_.geocentric_position_v
#define Lat_geocentric generic_.geocentric_position_v[0]
#define Lon_geocentric generic_.geocentric_position_v[1]
#define Radius_to_vehicle generic_.geocentric_position_v[2]
VECTOR_3 geodetic_position_v;
#define Geodetic_position_v generic_.geodetic_position_v
#define Latitude generic_.geodetic_position_v[0]
#define Longitude generic_.geodetic_position_v[1]
#define Altitude generic_.geodetic_position_v[2]
#define Geodetic_position_v generic_.geodetic_position_v
#define Latitude generic_.geodetic_position_v[0]
#define Longitude generic_.geodetic_position_v[1]
#define Altitude generic_.geodetic_position_v[2]
VECTOR_3 euler_angles_v;
#define Euler_angles_v generic_.euler_angles_v
#define Phi generic_.euler_angles_v[0]
#define Theta generic_.euler_angles_v[1]
#define Psi generic_.euler_angles_v[2]
#define Euler_angles_v generic_.euler_angles_v
#define Phi generic_.euler_angles_v[0]
#define Theta generic_.euler_angles_v[1]
#define Psi generic_.euler_angles_v[2]
/*======================= Miscellaneous quantities ========================*/
DATA t_local_to_body_m[3][3]; /* Transformation matrix L to B */
#define T_local_to_body_m generic_.t_local_to_body_m
#define T_local_to_body_11 generic_.t_local_to_body_m[0][0]
#define T_local_to_body_12 generic_.t_local_to_body_m[0][1]
#define T_local_to_body_13 generic_.t_local_to_body_m[0][2]
#define T_local_to_body_21 generic_.t_local_to_body_m[1][0]
#define T_local_to_body_22 generic_.t_local_to_body_m[1][1]
#define T_local_to_body_23 generic_.t_local_to_body_m[1][2]
#define T_local_to_body_31 generic_.t_local_to_body_m[2][0]
#define T_local_to_body_32 generic_.t_local_to_body_m[2][1]
#define T_local_to_body_33 generic_.t_local_to_body_m[2][2]
DATA t_local_to_body_m[3][3]; /* Transformation matrix L to B */
#define T_local_to_body_m generic_.t_local_to_body_m
#define T_local_to_body_11 generic_.t_local_to_body_m[0][0]
#define T_local_to_body_12 generic_.t_local_to_body_m[0][1]
#define T_local_to_body_13 generic_.t_local_to_body_m[0][2]
#define T_local_to_body_21 generic_.t_local_to_body_m[1][0]
#define T_local_to_body_22 generic_.t_local_to_body_m[1][1]
#define T_local_to_body_23 generic_.t_local_to_body_m[1][2]
#define T_local_to_body_31 generic_.t_local_to_body_m[2][0]
#define T_local_to_body_32 generic_.t_local_to_body_m[2][1]
#define T_local_to_body_33 generic_.t_local_to_body_m[2][2]
DATA gravity; /* Local acceleration due to G */
#define Gravity generic_.gravity
DATA gravity; /* Local acceleration due to G */
#define Gravity generic_.gravity
DATA centrifugal_relief; /* load factor reduction due to speed */
#define Centrifugal_relief generic_.centrifugal_relief
DATA centrifugal_relief; /* load factor reduction due to speed */
#define Centrifugal_relief generic_.centrifugal_relief
DATA alpha, beta, alpha_dot, beta_dot; /* in radians */
#define Std_Alpha generic_.alpha
#define Std_Beta generic_.beta
#define Std_Alpha_dot generic_.alpha_dot
#define Std_Beta_dot generic_.beta_dot
DATA alpha, beta, alpha_dot, beta_dot; /* in radians */
#define Std_Alpha generic_.alpha
#define Std_Beta generic_.beta
#define Std_Alpha_dot generic_.alpha_dot
#define Std_Beta_dot generic_.beta_dot
DATA cos_alpha, sin_alpha, cos_beta, sin_beta;
#define Cos_alpha generic_.cos_alpha
#define Sin_alpha generic_.sin_alpha
#define Cos_beta generic_.cos_beta
#define Sin_beta generic_.sin_beta
#define Cos_alpha generic_.cos_alpha
#define Sin_alpha generic_.sin_alpha
#define Cos_beta generic_.cos_beta
#define Sin_beta generic_.sin_beta
DATA cos_phi, sin_phi, cos_theta, sin_theta, cos_psi, sin_psi;
#define Cos_phi generic_.cos_phi
#define Sin_phi generic_.sin_phi
#define Cos_theta generic_.cos_theta
#define Sin_theta generic_.sin_theta
#define Cos_psi generic_.cos_psi
#define Sin_psi generic_.sin_psi
DATA gamma_vert_rad, gamma_horiz_rad; /* Flight path angles */
#define Gamma_vert_rad generic_.gamma_vert_rad
#define Gamma_horiz_rad generic_.gamma_horiz_rad
#define Cos_phi generic_.cos_phi
#define Sin_phi generic_.sin_phi
#define Cos_theta generic_.cos_theta
#define Sin_theta generic_.sin_theta
#define Cos_psi generic_.cos_psi
#define Sin_psi generic_.sin_psi
DATA gamma_vert_rad, gamma_horiz_rad; /* Flight path angles */
#define Gamma_vert_rad generic_.gamma_vert_rad
#define Gamma_horiz_rad generic_.gamma_horiz_rad
DATA sigma, density, v_sound, mach_number;
#define Sigma generic_.sigma
#define Density generic_.density
#define V_sound generic_.v_sound
#define Mach_number generic_.mach_number
#define Sigma generic_.sigma
#define Density generic_.density
#define V_sound generic_.v_sound
#define Mach_number generic_.mach_number
DATA static_pressure, total_pressure, impact_pressure, dynamic_pressure;
#define Static_pressure generic_.static_pressure
#define Total_pressure generic_.total_pressure
#define Impact_pressure generic_.impact_pressure
#define Dynamic_pressure generic_.dynamic_pressure
#define Static_pressure generic_.static_pressure
#define Total_pressure generic_.total_pressure
#define Impact_pressure generic_.impact_pressure
#define Dynamic_pressure generic_.dynamic_pressure
DATA static_temperature, total_temperature;
#define Static_temperature generic_.static_temperature
#define Total_temperature generic_.total_temperature
#define Static_temperature generic_.static_temperature
#define Total_temperature generic_.total_temperature
DATA sea_level_radius, earth_position_angle;
#define Sea_level_radius generic_.sea_level_radius
#define Earth_position_angle generic_.earth_position_angle
#define Sea_level_radius generic_.sea_level_radius
#define Earth_position_angle generic_.earth_position_angle
DATA runway_altitude, runway_latitude, runway_longitude, runway_heading;
#define Runway_altitude generic_.runway_altitude
#define Runway_latitude generic_.runway_latitude
#define Runway_longitude generic_.runway_longitude
#define Runway_heading generic_.runway_heading
#define Runway_altitude generic_.runway_altitude
#define Runway_latitude generic_.runway_latitude
#define Runway_longitude generic_.runway_longitude
#define Runway_heading generic_.runway_heading
DATA radius_to_rwy;
#define Radius_to_rwy generic_.radius_to_rwy
#define Radius_to_rwy generic_.radius_to_rwy
VECTOR_3 d_cg_rwy_local_v; /* CG rel. to rwy in local coords */
#define D_cg_rwy_local_v generic_.d_cg_rwy_local_v
#define D_cg_north_of_rwy generic_.d_cg_rwy_local_v[0]
#define D_cg_east_of_rwy generic_.d_cg_rwy_local_v[1]
#define D_cg_above_rwy generic_.d_cg_rwy_local_v[2]
#define D_cg_rwy_local_v generic_.d_cg_rwy_local_v
#define D_cg_north_of_rwy generic_.d_cg_rwy_local_v[0]
#define D_cg_east_of_rwy generic_.d_cg_rwy_local_v[1]
#define D_cg_above_rwy generic_.d_cg_rwy_local_v[2]
VECTOR_3 d_cg_rwy_rwy_v; /* CG relative to runway, in rwy coordinates */
#define D_cg_rwy_rwy_v generic_.d_cg_rwy_rwy_v
#define X_cg_rwy generic_.d_cg_rwy_rwy_v[0]
#define Y_cg_rwy generic_.d_cg_rwy_rwy_v[1]
#define H_cg_rwy generic_.d_cg_rwy_rwy_v[2]
VECTOR_3 d_cg_rwy_rwy_v; /* CG relative to runway, in rwy coordinates */
#define D_cg_rwy_rwy_v generic_.d_cg_rwy_rwy_v
#define X_cg_rwy generic_.d_cg_rwy_rwy_v[0]
#define Y_cg_rwy generic_.d_cg_rwy_rwy_v[1]
#define H_cg_rwy generic_.d_cg_rwy_rwy_v[2]
VECTOR_3 d_pilot_rwy_local_v; /* pilot rel. to rwy in local coords */
#define D_pilot_rwy_local_v generic_.d_pilot_rwy_local_v
#define D_pilot_north_of_rwy generic_.d_pilot_rwy_local_v[0]
#define D_pilot_east_of_rwy generic_.d_pilot_rwy_local_v[1]
#define D_pilot_above_rwy generic_.d_pilot_rwy_local_v[2]
VECTOR_3 d_pilot_rwy_local_v; /* pilot rel. to rwy in local coords */
#define D_pilot_rwy_local_v generic_.d_pilot_rwy_local_v
#define D_pilot_north_of_rwy generic_.d_pilot_rwy_local_v[0]
#define D_pilot_east_of_rwy generic_.d_pilot_rwy_local_v[1]
#define D_pilot_above_rwy generic_.d_pilot_rwy_local_v[2]
VECTOR_3 d_pilot_rwy_rwy_v; /* pilot rel. to rwy, in rwy coords. */
#define D_pilot_rwy_rwy_v generic_.d_pilot_rwy_rwy_v
#define X_pilot_rwy generic_.d_pilot_rwy_rwy_v[0]
#define Y_pilot_rwy generic_.d_pilot_rwy_rwy_v[1]
#define H_pilot_rwy generic_.d_pilot_rwy_rwy_v[2]
VECTOR_3 d_pilot_rwy_rwy_v; /* pilot rel. to rwy, in rwy coords. */
#define D_pilot_rwy_rwy_v generic_.d_pilot_rwy_rwy_v
#define X_pilot_rwy generic_.d_pilot_rwy_rwy_v[0]
#define Y_pilot_rwy generic_.d_pilot_rwy_rwy_v[1]
#define H_pilot_rwy generic_.d_pilot_rwy_rwy_v[2]
} GENERIC;
extern GENERIC generic_; /* usually defined in ls_main.c */
extern GENERIC generic_; /* usually defined in ls_main.c */
#ifdef __cplusplus

140
src/FDM/LaRCsim/ls_geodesy.c
View file

@ -1,42 +1,42 @@
/***************************************************************************
TITLE: ls_geodesy
TITLE: ls_geodesy
----------------------------------------------------------------------------
FUNCTION: Converts geocentric coordinates to geodetic positions
FUNCTION: Converts geocentric coordinates to geodetic positions
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Written as part of LaRCSim project by E. B. Jackson
GENEALOGY: Written as part of LaRCSim project by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
930208 Modified to avoid singularity near polar region. EBJ
930602 Moved backwards calcs here from ls_step. EBJ
931214 Changed erroneous Latitude and Altitude variables to
*lat_geod and *alt in routine ls_geoc_to_geod. EBJ
940111 Changed header files from old ls_eom.h style to ls_types,
and ls_constants. Also replaced old DATA type with new
SCALAR type. EBJ
MODIFICATION HISTORY:
DATE PURPOSE BY
930208 Modified to avoid singularity near polar region. EBJ
930602 Moved backwards calcs here from ls_step. EBJ
931214 Changed erroneous Latitude and Altitude variables to
*lat_geod and *alt in routine ls_geoc_to_geod. EBJ
940111 Changed header files from old ls_eom.h style to ls_types,
and ls_constants. Also replaced old DATA type with new
SCALAR type. EBJ
CURRENT RCS HEADER:
CURRENT RCS HEADER:
$Header$
$Log$
@ -224,34 +224,34 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
[ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
[ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
----------------------------------------------------------------------------
CALLED BY: ls_aux
CALLED BY: ls_aux
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
lat_geoc Geocentric latitude, radians, + = North
radius C.G. radius to earth center, ft
INPUTS:
lat_geoc Geocentric latitude, radians, + = North
radius C.G. radius to earth center, ft
----------------------------------------------------------------------------
OUTPUTS:
lat_geod Geodetic latitude, radians, + = North
alt C.G. altitude above mean sea level, ft
sea_level_r radius from earth center to sea level at
local vertical (surface normal) of C.G.
OUTPUTS:
lat_geod Geodetic latitude, radians, + = North
alt C.G. altitude above mean sea level, ft
sea_level_r radius from earth center to sea level at
local vertical (surface normal) of C.G.
--------------------------------------------------------------------------*/
@ -266,54 +266,54 @@ Initial Flight Gear revision.
void ls_geoc_to_geod( SCALAR lat_geoc, SCALAR radius, SCALAR *lat_geod,
SCALAR *alt, SCALAR *sea_level_r )
SCALAR *alt, SCALAR *sea_level_r )
{
SCALAR t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
SCALAR sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
SCALAR t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
SCALAR sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
if( ( (HALF_PI - lat_geoc) < ONE_SECOND ) /* near North pole */
|| ( (HALF_PI + lat_geoc) < ONE_SECOND ) ) /* near South pole */
{
*lat_geod = lat_geoc;
*sea_level_r = EQUATORIAL_RADIUS*E;
*alt = radius - *sea_level_r;
}
else
{
t_lat = tan(lat_geoc);
x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E);
mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha);
if (lat_geoc < 0) mu_alpha = - mu_alpha;
sin_mu_a = sin(mu_alpha);
delt_lambda = mu_alpha - lat_geoc;
r_alpha = x_alpha/cos(lat_geoc);
l_point = radius - r_alpha;
*alt = l_point*cos(delt_lambda);
denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/
(denom*denom*denom);
delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt);
*lat_geod = mu_alpha - delt_mu;
lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */
sin_lambda_sl = sin( lambda_sl );
*sea_level_r = sqrt(RESQ
/(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
}
if( ( (HALF_PI - lat_geoc) < ONE_SECOND ) /* near North pole */
|| ( (HALF_PI + lat_geoc) < ONE_SECOND ) ) /* near South pole */
{
*lat_geod = lat_geoc;
*sea_level_r = EQUATORIAL_RADIUS*E;
*alt = radius - *sea_level_r;
}
else
{
t_lat = tan(lat_geoc);
x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E);
mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha);
if (lat_geoc < 0) mu_alpha = - mu_alpha;
sin_mu_a = sin(mu_alpha);
delt_lambda = mu_alpha - lat_geoc;
r_alpha = x_alpha/cos(lat_geoc);
l_point = radius - r_alpha;
*alt = l_point*cos(delt_lambda);
denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/
(denom*denom*denom);
delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt);
*lat_geod = mu_alpha - delt_mu;
lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */
sin_lambda_sl = sin( lambda_sl );
*sea_level_r = sqrt(RESQ
/(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
}
}
void ls_geod_to_geoc( SCALAR lat_geod, SCALAR alt,
SCALAR *sl_radius, SCALAR *lat_geoc )
SCALAR *sl_radius, SCALAR *lat_geoc )
{
SCALAR lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py;
lambda_sl = atan( E*E * tan(lat_geod) ); /* sea level geocentric latitude */
sin_lambda_sl = sin( lambda_sl );
cos_lambda_sl = cos( lambda_sl );
sin_mu = sin(lat_geod); /* Geodetic (map makers') latitude */
sin_mu = sin(lat_geod); /* Geodetic (map makers') latitude */
cos_mu = cos(lat_geod);
*sl_radius = sqrt(RESQ
/(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
/(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
py = *sl_radius*sin_lambda_sl + alt*sin_mu;
px = *sl_radius*cos_lambda_sl + alt*cos_mu;
*lat_geoc = atan2( py, px );

4
src/FDM/LaRCsim/ls_geodesy.h
View file

@ -9,10 +9,10 @@ extern "C" {
#endif
void ls_geoc_to_geod( SCALAR lat_geoc, SCALAR radius,
SCALAR *lat_geod, SCALAR *alt, SCALAR *sea_level_r );
SCALAR *lat_geod, SCALAR *alt, SCALAR *sea_level_r );
void ls_geod_to_geoc( SCALAR lat_geod, SCALAR alt, SCALAR *sl_radius,
SCALAR *lat_geoc );
SCALAR *lat_geoc );
#ifdef __cplusplus
}

74
src/FDM/LaRCsim/ls_gravity.c
View file

@ -1,38 +1,38 @@
/***************************************************************************
TITLE: ls_gravity
TITLE: ls_gravity
----------------------------------------------------------------------------
FUNCTION: Gravity model for LaRCsim
FUNCTION: Gravity model for LaRCsim
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created by Bruce Jackson on September 25, 1992.
GENEALOGY: Created by Bruce Jackson on September 25, 1992.
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY: Bruce Jackson
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY: Bruce Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
940111 Changed include files to "ls_types.h" and
"ls_constants.h" from "ls_eom.h"; also changed DATA types
to SCALAR types. EBJ
MODIFICATION HISTORY:
DATE PURPOSE BY
940111 Changed include files to "ls_types.h" and
"ls_constants.h" from "ls_eom.h"; also changed DATA types
to SCALAR types. EBJ
$Header$
$Log$
Revision 1.1 2002/09/10 01:14:02 curt
@ -60,29 +60,29 @@ Initial Flight Gear revision.
*
* Revision 1.1 1992/12/30 13:18:46 bjax
* Initial revision
*
*
----------------------------------------------------------------------------
REFERENCES: Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-
REFERENCES: Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_types.h"
@ -90,19 +90,19 @@ Initial Flight Gear revision.
#include "ls_gravity.h"
#include <math.h>
#define GM 1.4076431E16
#define J2 1.08263E-3
#define GM 1.4076431E16
#define J2 1.08263E-3
void ls_gravity( SCALAR radius, SCALAR lat, SCALAR *gravity )
{
SCALAR radius_ratio, rrsq, sinsqlat;
radius_ratio = radius/EQUATORIAL_RADIUS;
rrsq = radius_ratio*radius_ratio;
sinsqlat = sin(lat)*sin(lat);
*gravity = (GM/(radius*radius))
*sqrt(2.25*rrsq*rrsq*J2*J2*(5*sinsqlat*sinsqlat -2*sinsqlat + 1)
+ 3*rrsq*J2*(1 - 3*sinsqlat) + 1);
SCALAR radius_ratio, rrsq, sinsqlat;
radius_ratio = radius/EQUATORIAL_RADIUS;
rrsq = radius_ratio*radius_ratio;
sinsqlat = sin(lat)*sin(lat);
*gravity = (GM/(radius*radius))
*sqrt(2.25*rrsq*rrsq*J2*J2*(5*sinsqlat*sinsqlat -2*sinsqlat + 1)
+ 3*rrsq*J2*(1 - 3*sinsqlat) + 1);
}

296
src/FDM/LaRCsim/ls_init.c
View file

@ -1,36 +1,36 @@
/***************************************************************************
TITLE: ls_init.c
TITLE: ls_init.c
----------------------------------------------------------------------------
FUNCTION: Initializes simulation
FUNCTION: Initializes simulation
----------------------------------------------------------------------------
MODULE STATUS: incomplete
MODULE STATUS: incomplete
----------------------------------------------------------------------------
GENEALOGY: Written 921230 by Bruce Jackson
GENEALOGY: Written 921230 by Bruce Jackson
----------------------------------------------------------------------------
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
950314 Added get_set, put_set, and init routines. EBJ
CURRENT RCS HEADER:
950314 Added get_set, put_set, and init routines. EBJ
CURRENT RCS HEADER:
$Header$
$Log$
@ -109,23 +109,23 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
// static char rcsid[] = "$Id$";
@ -156,14 +156,14 @@ void basic_init( void );
typedef struct
{
symbol_rec Symbol;
double value;
symbol_rec Symbol;
double value;
} cont_state_rec;
typedef struct
{
symbol_rec Symbol;
long value;
symbol_rec Symbol;
long value;
} disc_state_rec;
@ -172,8 +172,8 @@ extern SCALAR Simtime;
/* static int Symbols_loaded = 0; */
static int Number_of_Continuous_States = 0;
static int Number_of_Discrete_States = 0;
static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
void ls_init_init( void ) {
@ -181,52 +181,52 @@ void ls_init_init( void ) {
/* int error; */
if (Number_of_Continuous_States == 0)
{
Number_of_Continuous_States = HARDWIRED;
{
Number_of_Continuous_States = HARDWIRED;
for (i=0;i<HARDWIRED;i++)
strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
for (i=0;i<HARDWIRED;i++)
strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
strcpy( Continuous_States[ 0].Symbol.Par_Name,
"generic_.geodetic_position_v[0]");
strcpy( Continuous_States[ 1].Symbol.Par_Name,
"generic_.geodetic_position_v[1]");
strcpy( Continuous_States[ 2].Symbol.Par_Name,
"generic_.geodetic_position_v[2]");
strcpy( Continuous_States[ 3].Symbol.Par_Name,
"generic_.v_local_v[0]");
strcpy( Continuous_States[ 4].Symbol.Par_Name,
"generic_.v_local_v[1]");
strcpy( Continuous_States[ 5].Symbol.Par_Name,
"generic_.v_local_v[2]");
strcpy( Continuous_States[ 6].Symbol.Par_Name,
"generic_.euler_angles_v[0]");
strcpy( Continuous_States[ 7].Symbol.Par_Name,
"generic_.euler_angles_v[1]");
strcpy( Continuous_States[ 8].Symbol.Par_Name,
"generic_.euler_angles_v[2]");
strcpy( Continuous_States[ 9].Symbol.Par_Name,
"generic_.omega_body_v[0]");
strcpy( Continuous_States[10].Symbol.Par_Name,
"generic_.omega_body_v[1]");
strcpy( Continuous_States[11].Symbol.Par_Name,
"generic_.omega_body_v[2]");
strcpy( Continuous_States[12].Symbol.Par_Name,
"generic_.earth_position_angle");
}
strcpy( Continuous_States[ 0].Symbol.Par_Name,
"generic_.geodetic_position_v[0]");
strcpy( Continuous_States[ 1].Symbol.Par_Name,
"generic_.geodetic_position_v[1]");
strcpy( Continuous_States[ 2].Symbol.Par_Name,
"generic_.geodetic_position_v[2]");
strcpy( Continuous_States[ 3].Symbol.Par_Name,
"generic_.v_local_v[0]");
strcpy( Continuous_States[ 4].Symbol.Par_Name,
"generic_.v_local_v[1]");
strcpy( Continuous_States[ 5].Symbol.Par_Name,
"generic_.v_local_v[2]");
strcpy( Continuous_States[ 6].Symbol.Par_Name,
"generic_.euler_angles_v[0]");
strcpy( Continuous_States[ 7].Symbol.Par_Name,
"generic_.euler_angles_v[1]");
strcpy( Continuous_States[ 8].Symbol.Par_Name,
"generic_.euler_angles_v[2]");
strcpy( Continuous_States[ 9].Symbol.Par_Name,
"generic_.omega_body_v[0]");
strcpy( Continuous_States[10].Symbol.Par_Name,
"generic_.omega_body_v[1]");
strcpy( Continuous_States[11].Symbol.Par_Name,
"generic_.omega_body_v[2]");
strcpy( Continuous_States[12].Symbol.Par_Name,
"generic_.earth_position_angle");
}
/* commented out by CLO
for (i=0;i<Number_of_Continuous_States;i++)
{
(void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
}
{
(void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
}
for (i=0;i<Number_of_Discrete_States;i++)
{
(void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
}
{
(void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
}
*/
}
@ -270,14 +270,14 @@ void ls_init( char * aircraft ) {
/* commented out by CLO
for(i=0;i<Number_of_Continuous_States;i++)
if (Continuous_States[i].Symbol.Addr)
ls_set_sym_val( &Continuous_States[i].Symbol,
Continuous_States[i].value );
if (Continuous_States[i].Symbol.Addr)
ls_set_sym_val( &Continuous_States[i].Symbol,
Continuous_States[i].value );
for(i=0;i<Number_of_Discrete_States;i++)
if (Discrete_States[i].Symbol.Addr)
ls_set_sym_val( &Discrete_States[i].Symbol,
(double) Discrete_States[i].value );
if (Discrete_States[i].Symbol.Addr)
ls_set_sym_val( &Discrete_States[i].Symbol,
(double) Discrete_States[i].value );
*/
switch (current_model) {
@ -336,70 +336,70 @@ char *ls_init_get_set(char *buffer, char *eob)
while( !abrt && (eob > bufptr))
{
bufptr = strtok( 0L, "\n");
if (bufptr == 0) return 0L;
if (strncasecmp( bufptr, "end", 3) == 0) break;
bufptr = strtok( 0L, "\n");
if (bufptr == 0) return 0L;
if (strncasecmp( bufptr, "end", 3) == 0) break;
sscanf( bufptr, "%s", line );
if (line[0] != '#') /* ignore comments */
{
switch (looking_for)
{
case cont_states_header:
{
if (strncasecmp( line, "continuous_states", 17) == 0)
{
n = sscanf( bufptr, "%s%d", line,
&Number_of_Continuous_States );
if (n != 2) abrt = 1;
looking_for = cont_states;
i = 0;
}
break;
}
case cont_states:
{
n = sscanf( bufptr, "%s%s%le",
Continuous_States[i].Symbol.Mod_Name,
Continuous_States[i].Symbol.Par_Name,
&Continuous_States[i].value );
if (n != 3) abrt = 1;
Continuous_States[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Continuous_States)
looking_for = disc_states_header;
break;
}
case disc_states_header:
{
if (strncasecmp( line, "discrete_states", 15) == 0)
{
n = sscanf( bufptr, "%s%d", line,
&Number_of_Discrete_States );
if (n != 2) abrt = 1;
looking_for = disc_states;
i = 0;
}
break;
}
case disc_states:
{
n = sscanf( bufptr, "%s%s%ld",
Discrete_States[i].Symbol.Mod_Name,
Discrete_States[i].Symbol.Par_Name,
&Discrete_States[i].value );
if (n != 3) abrt = 1;
Discrete_States[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Discrete_States) looking_for = done;
}
case done:
{
break;
}
}
sscanf( bufptr, "%s", line );
if (line[0] != '#') /* ignore comments */
{
switch (looking_for)
{
case cont_states_header:
{
if (strncasecmp( line, "continuous_states", 17) == 0)
{
n = sscanf( bufptr, "%s%d", line,
&Number_of_Continuous_States );
if (n != 2) abrt = 1;
looking_for = cont_states;
i = 0;
}
break;
}
case cont_states:
{
n = sscanf( bufptr, "%s%s%le",
Continuous_States[i].Symbol.Mod_Name,
Continuous_States[i].Symbol.Par_Name,
&Continuous_States[i].value );
if (n != 3) abrt = 1;
Continuous_States[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Continuous_States)
looking_for = disc_states_header;
break;
}
case disc_states_header:
{
if (strncasecmp( line, "discrete_states", 15) == 0)
{
n = sscanf( bufptr, "%s%d", line,
&Number_of_Discrete_States );
if (n != 2) abrt = 1;
looking_for = disc_states;
i = 0;
}
break;
}
case disc_states:
{
n = sscanf( bufptr, "%s%s%ld",
Discrete_States[i].Symbol.Mod_Name,
Discrete_States[i].Symbol.Par_Name,
&Discrete_States[i].value );
if (n != 3) abrt = 1;
Discrete_States[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Discrete_States) looking_for = done;
}
case done:
{
break;
}
}
}
}
}
Symbols_loaded = !abrt;
@ -423,18 +423,18 @@ void ls_init_put_set( FILE *fp )
fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
fprintf(fp, "# module parameter value\n");
for (i=0; i<Number_of_Continuous_States; i++)
fprintf(fp, " %s\t%s\t%E\n",
Continuous_States[i].Symbol.Mod_Name,
Continuous_States[i].Symbol.Par_Name,
Continuous_States[i].value );
fprintf(fp, " %s\t%s\t%E\n",
Continuous_States[i].Symbol.Mod_Name,
Continuous_States[i].Symbol.Par_Name,
Continuous_States[i].value );
fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
fprintf(fp, "# module parameter value\n");
for (i=0;i<Number_of_Discrete_States;i++)
fprintf(fp, " %s\t%s\t%ld\n",
Discrete_States[i].Symbol.Mod_Name,
Discrete_States[i].Symbol.Par_Name,
Discrete_States[i].value );
fprintf(fp, " %s\t%s\t%ld\n",
Discrete_States[i].Symbol.Mod_Name,
Discrete_States[i].Symbol.Par_Name,
Discrete_States[i].value );
fprintf(fp, "end\n");
return;
}
@ -446,13 +446,13 @@ void ls_save_current_as_ic( void ) {
/* commented out by CLO
for(i=0;i<Number_of_Continuous_States;i++)
if (Continuous_States[i].Symbol.Addr)
Continuous_States[i].value =
ls_get_sym_val( &Continuous_States[i].Symbol, &error );
if (Continuous_States[i].Symbol.Addr)
Continuous_States[i].value =
ls_get_sym_val( &Continuous_States[i].Symbol, &error );
for(i=0;i<Number_of_Discrete_States;i++)
if (Discrete_States[i].Symbol.Addr)
Discrete_States[i].value = (long)
ls_get_sym_val( &Discrete_States[i].Symbol, &error );
if (Discrete_States[i].Symbol.Addr)
Discrete_States[i].value = (long)
ls_get_sym_val( &Discrete_States[i].Symbol, &error );
*/
}

390
src/FDM/LaRCsim/ls_interface.c
View file

@ -28,66 +28,66 @@
/***************************************************************************
TITLE: LaRCsim.c
TITLE: LaRCsim.c
----------------------------------------------------------------------------
FUNCTION: Top level routine for LaRCSIM. Includes
global variable declarations.
FUNCTION: Top level routine for LaRCSIM. Includes
global variable declarations.
----------------------------------------------------------------------------
MODULE STATUS: Developmental
MODULE STATUS: Developmental
----------------------------------------------------------------------------
GENEALOGY: Written 921230 by Bruce Jackson
GENEALOGY: Written 921230 by Bruce Jackson
----------------------------------------------------------------------------
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
930111 Added "progname" variable to keep name of invoking command.
EBJ
931012 Removed altitude < 0. test to support gear development. EBJ
931214 Added various pressures (Impact, Dynamic, Static, etc.) EBJ
931215 Adopted new generic variable structure. EBJ
931218 Added command line options decoding. EBJ
940110 Changed file type of matrix file to ".m" EBJ
940513 Renamed this routine "LaRCsim.c" from "ls_main.c" EBJ
940513 Added time_stamp routine, t_stamp. EBJ
950225 Added options flag, 'i', to set I/O output rate. EBJ
950306 Added calls to ls_get_settings() and ls_put_settings() EBJ
950314 Options flag 'i' now reads IC file; 'o' is output rate EBJ
950406 Many changes: added definition of default value macros;
removed local variables term_update_hz, model_dt, endtime,
substituted sim_control_ globals for these; removed
initialization of sim_control_.tape_channels; moved optarg
to generic extern; added mod_end_time & mod_buf_size flags
and temporary buffer_time and data_rate locals to
ls_checkopts(); added additional command line switches '-s'
and '-b'; made psuedo-mandatory file names for data output
switches; considerable rewrite of logic for setting data
buffer length and interleave parameters; updated '-h' help
output message; added protection logic to calculations of
these parameters; added check of return value on first call
to ls_cockpit() so <esc> abort works from initial pause
state; added call to ls_unsync() immediately following
first ls_sync() call, if paused (to avoid alarm clock
timeout); moved call to ls_record() into non-paused
multiloop path (was filling buffer with identical data
during pause); put check of paused flag before calling sync
routine ls_pause(); and added call to exit() on termination.
930111 Added "progname" variable to keep name of invoking command.
EBJ
931012 Removed altitude < 0. test to support gear development. EBJ
931214 Added various pressures (Impact, Dynamic, Static, etc.) EBJ
931215 Adopted new generic variable structure. EBJ
931218 Added command line options decoding. EBJ
940110 Changed file type of matrix file to ".m" EBJ
940513 Renamed this routine "LaRCsim.c" from "ls_main.c" EBJ
940513 Added time_stamp routine, t_stamp. EBJ
950225 Added options flag, 'i', to set I/O output rate. EBJ
950306 Added calls to ls_get_settings() and ls_put_settings() EBJ
950314 Options flag 'i' now reads IC file; 'o' is output rate EBJ
950406 Many changes: added definition of default value macros;
removed local variables term_update_hz, model_dt, endtime,
substituted sim_control_ globals for these; removed
initialization of sim_control_.tape_channels; moved optarg
to generic extern; added mod_end_time & mod_buf_size flags
and temporary buffer_time and data_rate locals to
ls_checkopts(); added additional command line switches '-s'
and '-b'; made psuedo-mandatory file names for data output
switches; considerable rewrite of logic for setting data
buffer length and interleave parameters; updated '-h' help
output message; added protection logic to calculations of
these parameters; added check of return value on first call
to ls_cockpit() so <esc> abort works from initial pause
state; added call to ls_unsync() immediately following
first ls_sync() call, if paused (to avoid alarm clock
timeout); moved call to ls_record() into non-paused
multiloop path (was filling buffer with identical data
during pause); put check of paused flag before calling sync
routine ls_pause(); and added call to exit() on termination.
$Header$
@ -202,23 +202,23 @@ $Original log: LaRCsim.c,v $
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
@ -249,12 +249,12 @@ $Original log: LaRCsim.c,v $
/* global variable declarations */
/* TAPE *Tape; */
GENERIC generic_;
SIM_CONTROL sim_control_;
/* TAPE *Tape; */
GENERIC generic_;
SIM_CONTROL sim_control_;
COCKPIT cockpit_;
SCALAR Simtime;
SCALAR Simtime;
#define DEFAULT_TERM_UPDATE_HZ 20
#define DEFAULT_MODEL_HZ 120
@ -266,7 +266,7 @@ SCALAR Simtime;
/* global variables */
char *progname;
char *fullname;
char *fullname;
/* file variables - default simulation settings */
@ -289,13 +289,13 @@ void ls_stamp( void ) {
nowtime_t = time( 0 );
nowtime = localtime( &nowtime_t ); /* set fields to correct time values */
date = (nowtime->tm_year % 100)*10000
+ (nowtime->tm_mon + 1)*100
+ (nowtime->tm_mday);
+ (nowtime->tm_mon + 1)*100
+ (nowtime->tm_mday);
sprintf(sim_control_.date_string, "%06ld", date);
sprintf(sim_control_.time_stamp, "%02d:%02d:%02d",
nowtime->tm_hour, nowtime->tm_min, nowtime->tm_sec);
nowtime->tm_hour, nowtime->tm_min, nowtime->tm_sec);
#ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
cuserid( sim_control_.userid ); /* set up user id */
cuserid( sim_control_.userid ); /* set up user id */
#endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
return;
}
@ -303,21 +303,21 @@ void ls_stamp( void ) {
void ls_setdefopts( void ) {
/* set default values for most options */
sim_control_.debug = 0; /* change to non-zero if in dbx! */
sim_control_.debug = 0; /* change to non-zero if in dbx! */
sim_control_.vision = 0;
sim_control_.write_av = 0; /* write Agile-Vu '.flt' file */
sim_control_.write_mat = 0; /* write matrix-x/matlab script */
sim_control_.write_tab = 0; /* write tab delim. history file */
sim_control_.write_asc1 = 0; /* write GetData file */
sim_control_.save_spacing = DEFAULT_SAVE_SPACING;
/* interpolation on recording */
sim_control_.write_spacing = DEFAULT_WRITE_SPACING;
/* interpolation on output */
sim_control_.write_av = 0; /* write Agile-Vu '.flt' file */
sim_control_.write_mat = 0; /* write matrix-x/matlab script */
sim_control_.write_tab = 0; /* write tab delim. history file */
sim_control_.write_asc1 = 0; /* write GetData file */
sim_control_.save_spacing = DEFAULT_SAVE_SPACING;
/* interpolation on recording */
sim_control_.write_spacing = DEFAULT_WRITE_SPACING;
/* interpolation on output */
sim_control_.end_time = DEFAULT_END_TIME;
sim_control_.model_hz = DEFAULT_MODEL_HZ;
sim_control_.term_update_hz = DEFAULT_TERM_UPDATE_HZ;
sim_control_.time_slices = (long int)(DEFAULT_END_TIME * DEFAULT_MODEL_HZ /
DEFAULT_SAVE_SPACING);
DEFAULT_SAVE_SPACING);
sim_control_.paused = 0;
speedup = 1.0;
@ -334,7 +334,7 @@ void ls_setdefopts( void ) {
extern char *optarg;
extern int optind;
int ls_checkopts(argc, argv) /* check and set options flags */
int ls_checkopts(argc, argv) /* check and set options flags */
int argc;
char *argv[];
{
@ -351,126 +351,126 @@ int ls_checkopts(argc, argv) /* check and set options flags */
/* set default values */
buffer_time = sim_control_.time_slices * sim_control_.save_spacing /
sim_control_.model_hz;
sim_control_.model_hz;
data_rate = sim_control_.model_hz / sim_control_.save_spacing;
while ((c = getopt(argc, argv, "Aa:b:de:f:hi:kmo:r:s:t:x:")) != EOF)
switch (c) {
case 'A':
if (sim_control_.sim_type == GLmouse)
{
fprintf(stderr, "Cannot specify both keyboard (k) and ACES (A) cockpits option\n");
fprintf(stderr, "Keyboard operation assumed.\n");
break;
}
sim_control_.sim_type = cockpit;
break;
case 'a':
sim_control_.write_av = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(fltname, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
case 'b':
buffer_time = atof(optarg);
if (buffer_time <= 0.) opt_err = -1;
mod_buf_size++;
break;
case 'd':
sim_control_.debug = 1;
break;
case 'e':
sim_control_.end_time = atof(optarg);
mod_end_time++;
break;
case 'f':
sim_control_.model_hz = atof(optarg);
break;
case 'h':
opt_err = 1;
break;
case 'i':
/* ls_get_settings( optarg ); */
break;
case 'k':
sim_control_.sim_type = GLmouse;
break;
case 'm':
sim_control_.vision = 1;
break;
case 'o':
sim_control_.term_update_hz = atof(optarg);
if (sim_control_.term_update_hz <= 0.) opt_err = 1;
break;
case 'r':
sim_control_.write_mat = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(matname, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
case 's':
data_rate = atof(optarg);
if (data_rate <= 0.) opt_err = -1;
break;
case 't':
sim_control_.write_tab = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(tabname, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
case 'x':
sim_control_.write_asc1 = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(asc1name, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
default:
opt_err = 1;
}
switch (c) {
case 'A':
if (sim_control_.sim_type == GLmouse)
{
fprintf(stderr, "Cannot specify both keyboard (k) and ACES (A) cockpits option\n");
fprintf(stderr, "Keyboard operation assumed.\n");
break;
}
sim_control_.sim_type = cockpit;
break;
case 'a':
sim_control_.write_av = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(fltname, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
case 'b':
buffer_time = atof(optarg);
if (buffer_time <= 0.) opt_err = -1;
mod_buf_size++;
break;
case 'd':
sim_control_.debug = 1;
break;
case 'e':
sim_control_.end_time = atof(optarg);
mod_end_time++;
break;
case 'f':
sim_control_.model_hz = atof(optarg);
break;
case 'h':
opt_err = 1;
break;
case 'i':
/* ls_get_settings( optarg ); */
break;
case 'k':
sim_control_.sim_type = GLmouse;
break;
case 'm':
sim_control_.vision = 1;
break;
case 'o':
sim_control_.term_update_hz = atof(optarg);
if (sim_control_.term_update_hz <= 0.) opt_err = 1;
break;
case 'r':
sim_control_.write_mat = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(matname, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
case 's':
data_rate = atof(optarg);
if (data_rate <= 0.) opt_err = -1;
break;
case 't':
sim_control_.write_tab = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(tabname, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
case 'x':
sim_control_.write_asc1 = 1;
if (optarg != NULL)
if (*optarg != '-')
strncpy(asc1name, optarg, MAX_FILE_NAME_LENGTH);
else
optind--;
break;
default:
opt_err = 1;
}
if (opt_err)
{
fprintf(stderr, "Usage: %s [-options]\n", progname);
fprintf(stderr, "\n");
fprintf(stderr, " where [-options] is zero or more of the following:\n");
fprintf(stderr, "\n");
fprintf(stderr, " [A|k] Run mode: [A]CES cockpit [default]\n");
fprintf(stderr, " or [k]eyboard\n");
fprintf(stderr, "\n");
fprintf(stderr, " [i <filename>] [i]nitial conditions filename\n");
fprintf(stderr, "\n");
fprintf(stderr, " [f <value>] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n",
sim_control_.model_hz);
fprintf(stderr, "\n");
fprintf(stderr, " [o <value>] Display [o]utput frequency, Hz (default is %5.2f Hz)\n",
sim_control_.term_update_hz);
fprintf(stderr, "\n");
fprintf(stderr, " [s <value>] Data storage frequency, Hz (default is %5.2f Hz)\n",
data_rate);
fprintf(stderr, "\n");
fprintf(stderr, " [e <value>] [e]nd time in seconds (default %5.1f seconds)\n",
sim_control_.end_time);
fprintf(stderr, "\n");
fprintf(stderr, " [b <value>] circular time history storage [b]uffer size, in seconds \n");
fprintf(stderr, " (default %5.1f seconds) (normally same as end time)\n",
sim_control_.time_slices*sim_control_.save_spacing/
sim_control_.model_hz);
fprintf(stderr, "\n");
fprintf(stderr, " [atxr [<filename>]] Output: [a]gile-vu (default name: %s )\n", fltname);
fprintf(stderr, " and/or [t]ab delimited ( '' name: %s )\n", tabname);
fprintf(stderr, " and/or [x]plot (default name: %s)\n", asc1name);
fprintf(stderr, " and/or mat[r]ix script ( '' name: %s )\n", matname);
fprintf(stderr, "\n");
return OPT_ERR;
fprintf(stderr, "Usage: %s [-options]\n", progname);
fprintf(stderr, "\n");
fprintf(stderr, " where [-options] is zero or more of the following:\n");
fprintf(stderr, "\n");
fprintf(stderr, " [A|k] Run mode: [A]CES cockpit [default]\n");
fprintf(stderr, " or [k]eyboard\n");
fprintf(stderr, "\n");
fprintf(stderr, " [i <filename>] [i]nitial conditions filename\n");
fprintf(stderr, "\n");
fprintf(stderr, " [f <value>] Iteration rate [f]requency, Hz (default is %5.2f Hz)\n",
sim_control_.model_hz);
fprintf(stderr, "\n");
fprintf(stderr, " [o <value>] Display [o]utput frequency, Hz (default is %5.2f Hz)\n",
sim_control_.term_update_hz);
fprintf(stderr, "\n");
fprintf(stderr, " [s <value>] Data storage frequency, Hz (default is %5.2f Hz)\n",
data_rate);
fprintf(stderr, "\n");
fprintf(stderr, " [e <value>] [e]nd time in seconds (default %5.1f seconds)\n",
sim_control_.end_time);
fprintf(stderr, "\n");
fprintf(stderr, " [b <value>] circular time history storage [b]uffer size, in seconds \n");
fprintf(stderr, " (default %5.1f seconds) (normally same as end time)\n",
sim_control_.time_slices*sim_control_.save_spacing/
sim_control_.model_hz);
fprintf(stderr, "\n");
fprintf(stderr, " [atxr [<filename>]] Output: [a]gile-vu (default name: %s )\n", fltname);
fprintf(stderr, " and/or [t]ab delimited ( '' name: %s )\n", tabname);
fprintf(stderr, " and/or [x]plot (default name: %s)\n", asc1name);
fprintf(stderr, " and/or mat[r]ix script ( '' name: %s )\n", matname);
fprintf(stderr, "\n");
return OPT_ERR;
}
/* calculate additional controls */
@ -479,9 +479,9 @@ int ls_checkopts(argc, argv) /* check and set options flags */
if (sim_control_.save_spacing < 1) sim_control_.save_spacing = 1;
sim_control_.time_slices = buffer_time * sim_control_.model_hz /
sim_control_.save_spacing;
sim_control_.save_spacing;
if (sim_control_.time_slices < 2) sim_control_.time_slices = 2;
return OPT_OK;
}
#endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
@ -527,13 +527,13 @@ int ls_cockpit( void ) {
int ls_toplevel_init(double dt, char * aircraft) {
model_dt = dt;
ls_setdefopts(); /* set default options */
ls_setdefopts(); /* set default options */
ls_stamp(); /* ID stamp; record time and date of run */
if (speedup == 0.0) {
fprintf(stderr, "%s: Cannot run with speedup of 0.\n", progname);
return 1;
fprintf(stderr, "%s: Cannot run with speedup of 0.\n", progname);
return 1;
}
/* printf("LS pre Init pos = %.2f\n", Latitude); */
@ -543,8 +543,8 @@ int ls_toplevel_init(double dt, char * aircraft) {
/* printf("LS post Init pos = %.2f\n", Latitude); */
if (speedup > 0) {
/* Initialize (get) cockpit (controls) settings */
ls_cockpit();
/* Initialize (get) cockpit (controls) settings */
ls_cockpit();
}
return(1);
@ -553,14 +553,14 @@ int ls_toplevel_init(double dt, char * aircraft) {
/* Run an iteration of the EOM (equations of motion) */
int ls_update(int multiloop) {
int i;
int i;
if (speedup > 0) {
ls_cockpit();
ls_cockpit();
}
for ( i = 0; i < multiloop; i++ ) {
ls_loop( model_dt, 0);
ls_loop( model_dt, 0);
}
return 1;

296
src/FDM/LaRCsim/ls_matrix.c
View file

@ -1,48 +1,48 @@
/***************************************************************************
TITLE: ls_matrix.c
TITLE: ls_matrix.c
----------------------------------------------------------------------------
FUNCTION: general real matrix routines; includes
gaussj() for matrix inversion using
Gauss-Jordan method with full pivoting.
The routines in this module have come more or less from ref [1].
Note that, probably due to the heritage of ref [1] (which has a
FORTRAN version that was probably written first), the use of 1 as
the first element of an array (or vector) is used. This is accomplished
in memory by allocating, but not using, the 0 elements in each dimension.
While this wastes some memory, it allows the routines to be ported more
easily from FORTRAN (I suspect) as well as adhering to conventional
matrix notation. As a result, however, traditional ANSI C convention
(0-base indexing) is not followed; as the authors of ref [1] point out,
there is some question of the portability of the resulting routines
which sometimes access negative indexes. See ref [1] for more details.
FUNCTION: general real matrix routines; includes
gaussj() for matrix inversion using
Gauss-Jordan method with full pivoting.
The routines in this module have come more or less from ref [1].
Note that, probably due to the heritage of ref [1] (which has a
FORTRAN version that was probably written first), the use of 1 as
the first element of an array (or vector) is used. This is accomplished
in memory by allocating, but not using, the 0 elements in each dimension.
While this wastes some memory, it allows the routines to be ported more
easily from FORTRAN (I suspect) as well as adhering to conventional
matrix notation. As a result, however, traditional ANSI C convention
(0-base indexing) is not followed; as the authors of ref [1] point out,
there is some question of the portability of the resulting routines
which sometimes access negative indexes. See ref [1] for more details.
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 950222 E. B. Jackson
GENEALOGY: Created 950222 E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: from Numerical Recipes in C, by Press, et. al.
CODED BY: Bruce Jackson
MAINTAINED BY:
DESIGNED BY: from Numerical Recipes in C, by Press, et. al.
CODED BY: Bruce Jackson
MAINTAINED BY:
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER:
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER:
$Header$
$Log$
@ -68,24 +68,24 @@ Initial revision.
----------------------------------------------------------------------------
REFERENCES: [1] Press, William H., et. al, Numerical Recipes in
C, 2nd edition, Cambridge University Press, 1992
REFERENCES: [1] Press, William H., et. al, Numerical Recipes in
C, 2nd edition, Cambridge University Press, 1992
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
@ -129,10 +129,10 @@ double **nr_matrix(long nrl, long nrh, long ncl, long nch)
m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
if (!m)
{
fprintf(stderr, "Memory failure in routine 'nr_matrix'.\n");
exit(1);
}
{
fprintf(stderr, "Memory failure in routine 'nr_matrix'.\n");
exit(1);
}
m += NR_END;
m -= nrl;
@ -140,10 +140,10 @@ double **nr_matrix(long nrl, long nrh, long ncl, long nch)
/* allocate rows and set pointers to them */
m[nrl] = (double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
if (!m[nrl])
{
fprintf(stderr, "Memory failure in routine 'matrix'\n");
exit(1);
}
{
fprintf(stderr, "Memory failure in routine 'matrix'\n");
exit(1);
}
m[nrl] += NR_END;
m[nrl] -= ncl;
@ -179,38 +179,38 @@ int nr_gaussj(double **a, int n, double **b, int m)
/* Note: this routine modified by EBJ to make b optional, if m == 0 */
{
int *indxc, *indxr, *ipiv;
int i, icol = 0, irow = 0, j, k, l, ll;
int *indxc, *indxr, *ipiv;
int i, icol = 0, irow = 0, j, k, l, ll;
double big, dum, pivinv, temp;
int bexists = ((m != 0) || (b == 0));
int bexists = ((m != 0) || (b == 0));
indxc = nr_ivector(1,n); /* The integer arrays ipiv, indxr, and */
indxr = nr_ivector(1,n); /* indxc are used for pivot bookkeeping */
indxc = nr_ivector(1,n); /* The integer arrays ipiv, indxr, and */
indxr = nr_ivector(1,n); /* indxc are used for pivot bookkeeping */
ipiv = nr_ivector(1,n);
for (j=1;j<=n;j++) ipiv[j] = 0;
for (i=1;i<=n;i++) /* This is the main loop over columns */
{
big = 0.0;
for (j=1;j<=n;j++) /* This is outer loop of pivot search */
if (ipiv[j] != 1)
for (k=1;k<=n;k++)
{
if (ipiv[k] == 0)
{
if (fabs(a[j][k]) >= big)
{
big = fabs(a[j][k]);
irow = j;
icol = k;
}
}
else
if (ipiv[k] > 1) return -1;
}
++(ipiv[icol]);
for (i=1;i<=n;i++) /* This is the main loop over columns */
{
big = 0.0;
for (j=1;j<=n;j++) /* This is outer loop of pivot search */
if (ipiv[j] != 1)
for (k=1;k<=n;k++)
{
if (ipiv[k] == 0)
{
if (fabs(a[j][k]) >= big)
{
big = fabs(a[j][k]);
irow = j;
icol = k;
}
}
else
if (ipiv[k] > 1) return -1;
}
++(ipiv[icol]);
/* We now have the pivot element, so we interchange rows, if needed, */
/* to put the pivot element on the diagonal. The columns are not */
@ -221,45 +221,45 @@ int nr_gaussj(double **a, int n, double **b, int m)
/* this form of bookkeeping, the solution b's will end up in the correct */
/* order, and the inverse matrix will be scrambed by columns. */
if (irow != icol)
{
/* for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */
for (l=1;l<=n;l++)
{
temp=a[irow][l];
a[irow][l]=a[icol][l];
a[icol][l]=temp;
}
if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
}
indxr[i] = irow; /* We are now ready to divide the pivot row */
indxc[i] = icol; /* by the pivot element, a[irow][icol] */
if (a[icol][icol] == 0.0) return -1;
pivinv = 1.0/a[icol][icol];
a[icol][icol] = 1.0;
for (l=1;l<=n;l++) a[icol][l] *= pivinv;
if (bexists) for (l=1;l<=m;l++) b[icol][l] *= pivinv;
for (ll=1;ll<=n;ll++) /* Next, we reduce the rows... */
if (ll != icol ) /* .. except for the pivot one */
{
dum = a[ll][icol];
a[ll][icol] = 0.0;
for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
if (bexists) for (l=1;l<=m;l++) b[ll][i] -= b[icol][l]*dum;
}
}
if (irow != icol)
{
/* for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */
for (l=1;l<=n;l++)
{
temp=a[irow][l];
a[irow][l]=a[icol][l];
a[icol][l]=temp;
}
if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
}
indxr[i] = irow; /* We are now ready to divide the pivot row */
indxc[i] = icol; /* by the pivot element, a[irow][icol] */
if (a[icol][icol] == 0.0) return -1;
pivinv = 1.0/a[icol][icol];
a[icol][icol] = 1.0;
for (l=1;l<=n;l++) a[icol][l] *= pivinv;
if (bexists) for (l=1;l<=m;l++) b[icol][l] *= pivinv;
for (ll=1;ll<=n;ll++) /* Next, we reduce the rows... */
if (ll != icol ) /* .. except for the pivot one */
{
dum = a[ll][icol];
a[ll][icol] = 0.0;
for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
if (bexists) for (l=1;l<=m;l++) b[ll][i] -= b[icol][l]*dum;
}
}
/* This is the end of the mail loop over columns of the reduction. It
only remains to unscrambled the solution in view of the column
interchanges. We do this by interchanging pairs of columns in
the reverse order that the permutation was built up. */
for (l=n;l>=1;l--)
{
if (indxr[l] != indxc[l])
for (k=1;k<=n;k++)
SWAP(a[k][indxr[l]],a[k][indxc[l]])
}
{
if (indxr[l] != indxc[l])
for (k=1;k<=n;k++)
SWAP(a[k][indxr[l]],a[k][indxc[l]])
}
/* and we are done */
@ -267,35 +267,35 @@ int nr_gaussj(double **a, int n, double **b, int m)
nr_free_ivector(indxr,1 /*,n*/ );
nr_free_ivector(indxc,1 /*,n*/ );
return 0; /* indicate success */
return 0; /* indicate success */
}
void nr_copymat(double **orig, int n, double **copy)
/* overwrites matrix 'copy' with copy of matrix 'orig' */
{
long i, j;
if ((orig==0)||(copy==0)||(n==0)) return;
for (i=1;i<=n;i++)
for (j=1;j<=n;j++)
copy[i][j] = orig[i][j];
long i, j;
if ((orig==0)||(copy==0)||(n==0)) return;
for (i=1;i<=n;i++)
for (j=1;j<=n;j++)
copy[i][j] = orig[i][j];
}
void nr_multmat(double **m1, int n, double **m2, double **prod)
{
long i, j, k;
if ((m1==0)||(m2==0)||(prod==0)||(n==0)) return;
for (i=1;i<=n;i++)
for (j=1;j<=n;j++)
{
prod[i][j] = 0.0;
for(k=1;k<=n;k++) prod[i][j] += m1[i][k]*m2[k][j];
}
long i, j, k;
if ((m1==0)||(m2==0)||(prod==0)||(n==0)) return;
for (i=1;i<=n;i++)
for (j=1;j<=n;j++)
{
prod[i][j] = 0.0;
for(k=1;k<=n;k++) prod[i][j] += m1[i][k]*m2[k][j];
}
}
void nr_printmat(double **a, int n)
@ -305,9 +305,9 @@ void nr_printmat(double **a, int n)
printf("\n");
for(i=1;i<=n;i++)
{
for(j=1;j<=n;j++)
printf("% 9.4f ", a[i][j]);
printf("\n");
for(j=1;j<=n;j++)
printf("% 9.4f ", a[i][j]);
printf("\n");
}
printf("\n");
@ -329,32 +329,32 @@ void testmat( void ) /* main() for test purposes */
/* for(i=1;i<=n;i++) mat1[i][i]= 5.0; */
for(loop=0;loop<maxloop;loop++)
{
if (loop != 0)
for(i=1;i<=n;i++)
for(j=1;j<=n;j++)
mat1[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
for(loop=0;loop<maxloop;loop++)
{
if (loop != 0)
for(i=1;i<=n;i++)
for(j=1;j<=n;j++)
mat1[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
printf("Original matrix:\n");
nr_printmat( mat1, n );
nr_copymat( mat1, n, mat2 );
i = nr_gaussj( mat2, n, 0, 0 );
if (i) printf("Singular matrix.\n");
printf("Inverted matrix:\n");
nr_printmat( mat2, n );
nr_multmat( mat1, n, mat2, mat3 );
printf("Original multiplied by inverse:\n");
nr_printmat( mat3, n );
if (loop < maxloop-1) /* sleep(1) */;
}
printf("Original matrix:\n");
nr_printmat( mat1, n );
nr_copymat( mat1, n, mat2 );
i = nr_gaussj( mat2, n, 0, 0 );
if (i) printf("Singular matrix.\n");
printf("Inverted matrix:\n");
nr_printmat( mat2, n );
nr_multmat( mat1, n, mat2, mat3 );
printf("Original multiplied by inverse:\n");
nr_printmat( mat3, n );
if (loop < maxloop-1) /* sleep(1) */;
}
nr_free_matrix( mat1, 1, n, 1, n );
nr_free_matrix( mat2, 1, n, 1, n );

66
src/FDM/LaRCsim/ls_matrix.h
View file

@ -1,46 +1,46 @@
/***************************************************************************
TITLE: ls_matrix.h
TITLE: ls_matrix.h
----------------------------------------------------------------------------
FUNCTION: Header file for general real matrix routines.
The routines in this module have come more or less from ref [1].
Note that, probably due to the heritage of ref [1] (which has a
FORTRAN version that was probably written first), the use of 1 as
the first element of an array (or vector) is used. This is accomplished
in memory by allocating, but not using, the 0 elements in each dimension.
While this wastes some memory, it allows the routines to be ported more
easily from FORTRAN (I suspect) as well as adhering to conventional
matrix notation. As a result, however, traditional ANSI C convention
(0-base indexing) is not followed; as the authors of ref [1] point out,
there is some question of the portability of the resulting routines
which sometimes access negative indexes. See ref [1] for more details.
FUNCTION: Header file for general real matrix routines.
The routines in this module have come more or less from ref [1].
Note that, probably due to the heritage of ref [1] (which has a
FORTRAN version that was probably written first), the use of 1 as
the first element of an array (or vector) is used. This is accomplished
in memory by allocating, but not using, the 0 elements in each dimension.
While this wastes some memory, it allows the routines to be ported more
easily from FORTRAN (I suspect) as well as adhering to conventional
matrix notation. As a result, however, traditional ANSI C convention
(0-base indexing) is not followed; as the authors of ref [1] point out,
there is some question of the portability of the resulting routines
which sometimes access negative indexes. See ref [1] for more details.
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 950222 E. B. Jackson
GENEALOGY: Created 950222 E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: from Numerical Recipes in C, by Press, et. al.
CODED BY: Bruce Jackson
MAINTAINED BY:
DESIGNED BY: from Numerical Recipes in C, by Press, et. al.
CODED BY: Bruce Jackson
MAINTAINED BY:
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER:
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER:
$Header$
$Log$
@ -62,24 +62,24 @@ Initial revision.
----------------------------------------------------------------------------
REFERENCES: [1] Press, William H., et. al, Numerical Recipes in
C, 2nd edition, Cambridge University Press, 1992
REFERENCES: [1] Press, William H., et. al, Numerical Recipes in
C, 2nd edition, Cambridge University Press, 1992
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <stdlib.h>

48
src/FDM/LaRCsim/ls_model.c
View file

@ -1,40 +1,40 @@
/***************************************************************************
TITLE: ls_model()
TITLE: ls_model()
----------------------------------------------------------------------------
FUNCTION: Model loop executive
FUNCTION: Model loop executive
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 15 October 1992 as part of LaRCSIM project
by Bruce Jackson.
GENEALOGY: Created 15 October 1992 as part of LaRCSIM project
by Bruce Jackson.
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY: maintainer
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY: maintainer
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
950306 Added parameters to call: dt, which is the step size
to be taken this loop (caution: may vary from call to call)
and Initialize, which if non-zero, implies an initialization
is requested. EBJ
950306 Added parameters to call: dt, which is the step size
to be taken this loop (caution: may vary from call to call)
and Initialize, which if non-zero, implies an initialization
is requested. EBJ
CURRENT RCS HEADER INFO:
CURRENT RCS HEADER INFO:
$Header$
$Log$
Revision 1.5 2005/12/19 12:53:21 ehofman
@ -140,23 +140,23 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY: ls_step (in initialization), ls_loop (planned)
CALLED BY: ls_step (in initialization), ls_loop (planned)
----------------------------------------------------------------------------
CALLS TO: aero(), engine(), gear()
CALLS TO: aero(), engine(), gear()
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <stdio.h>

104
src/FDM/LaRCsim/ls_sim_control.h
View file

@ -1,42 +1,42 @@
/***************************************************************************
TITLE: ls_sim_control.h
TITLE: ls_sim_control.h
----------------------------------------------------------------------------
FUNCTION: LaRCSim simulation control parameters header file
FUNCTION: LaRCSim simulation control parameters header file
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 18 DEC 1993 by Bruce Jackson
GENEALOGY: Created 18 DEC 1993 by Bruce Jackson
----------------------------------------------------------------------------
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
940204 Added "overrun" flag to indicate non-real-time frame.
940210 Added "vision" flag to indicate use of shared memory.
940513 Added "max_tape_channels" and "max_time_slices" EBJ
950308 Increased size of time_stamp and date_string to include
terminating null char. EBJ
950314 Addedf "paused" flag to make this global (was local to
ls_cockpit routine). EBJ
950406 Removed tape_channels parameter, and added end_time, model_hz,
and term_update_hz parameters. EBJ
MODIFICATION HISTORY:
DATE PURPOSE BY
940204 Added "overrun" flag to indicate non-real-time frame.
940210 Added "vision" flag to indicate use of shared memory.
940513 Added "max_tape_channels" and "max_time_slices" EBJ
950308 Increased size of time_stamp and date_string to include
terminating null char. EBJ
950314 Addedf "paused" flag to make this global (was local to
ls_cockpit routine). EBJ
950406 Removed tape_channels parameter, and added end_time, model_hz,
and term_update_hz parameters. EBJ
$Header$
$Log$
@ -79,7 +79,7 @@ Initial Flight Gear revision.
* Modified write_cmp2 flag to write_asc1 flag, since XPLOT 4.00 doesn't
* support cmp2. Also added RCS header and log entries in header.
*
--------------------------------------------------------------------------*/
@ -95,37 +95,37 @@ Initial Flight Gear revision.
typedef struct {
enum { batch, terminal, GLmouse, cockpit } sim_type;
char simname[64]; /* name of simulation */
int run_number; /* run number of this session */
char date_string[7]; /* like "931220" */
char time_stamp[9]; /* like "13:00:00" */
char simname[64]; /* name of simulation */
int run_number; /* run number of this session */
char date_string[7]; /* like "931220" */
char time_stamp[9]; /* like "13:00:00" */
#ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
char userid[L_cuserid]; /* who is running this sim */
#endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
long time_slices; /* number of points that can be recorded (circ buff) */
int write_av; /* will be writing out an Agile_VU file after run */
int write_mat; /* will be writing out a matrix script of session */
int write_tab; /* will be writing out a tab-delimited time history */
int write_asc1; /* will be writing out a GetData ASCII 1 file */
int save_spacing; /* spacing between data points when recording
data to memory; 0 = every point, 1 = every
other point; 2 = every fourth point, etc. */
int write_spacing; /* spacing between data points when writing
output files; 0 = every point, 1 = every
other point; 2 = every fourth point, etc. */
int overrun; /* indicates, if non-zero, a frame overrun
occurred in the previous frame. Suitable for
setting a display flag or writing an error
message. */
int vision; /* indicates, if non-zero, marriage to LaRC VISION
software (developed A. Dare and J. Burley of the
former Cockpit Technologies Branch) */
int debug; /* indicates, if non-zero, to operate in debug mode
which implies disable double-buffering and synch.
attempts to avoid errors */
int paused; /* indicates simulation is paused */
float end_time; /* end of simulation run value */
float model_hz; /* current inner loop frame rate */
long time_slices; /* number of points that can be recorded (circ buff) */
int write_av; /* will be writing out an Agile_VU file after run */
int write_mat; /* will be writing out a matrix script of session */
int write_tab; /* will be writing out a tab-delimited time history */
int write_asc1; /* will be writing out a GetData ASCII 1 file */
int save_spacing; /* spacing between data points when recording
data to memory; 0 = every point, 1 = every
other point; 2 = every fourth point, etc. */
int write_spacing; /* spacing between data points when writing
output files; 0 = every point, 1 = every
other point; 2 = every fourth point, etc. */
int overrun; /* indicates, if non-zero, a frame overrun
occurred in the previous frame. Suitable for
setting a display flag or writing an error
message. */
int vision; /* indicates, if non-zero, marriage to LaRC VISION
software (developed A. Dare and J. Burley of the
former Cockpit Technologies Branch) */
int debug; /* indicates, if non-zero, to operate in debug mode
which implies disable double-buffering and synch.
attempts to avoid errors */
int paused; /* indicates simulation is paused */
float end_time; /* end of simulation run value */
float model_hz; /* current inner loop frame rate */
float term_update_hz; /* current terminal refresh frequency */
} SIM_CONTROL;

326
src/FDM/LaRCsim/ls_step.c
View file

@ -1,52 +1,52 @@
/***************************************************************************
TITLE: ls_step
TITLE: ls_step
----------------------------------------------------------------------------
FUNCTION: Integration routine for equations of motion
(vehicle states)
FUNCTION: Integration routine for equations of motion
(vehicle states)
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Written 920802 by Bruce Jackson. Based upon equations
given in reference [1] and a Matrix-X/System Build block
diagram model of equations of motion coded by David Raney
at NASA-Langley in June of 1992.
GENEALOGY: Written 920802 by Bruce Jackson. Based upon equations
given in reference [1] and a Matrix-X/System Build block
diagram model of equations of motion coded by David Raney
at NASA-Langley in June of 1992.
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY:
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY:
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
921223 Modified calculation of Phi and Psi to use the "atan2" routine
rather than the "atan" to allow full circular angles.
"atan" limits to +/- pi/2. EBJ
940111 Changed from oldstyle include file ls_eom.h; also changed
from DATA to SCALAR type. EBJ
921223 Modified calculation of Phi and Psi to use the "atan2" routine
rather than the "atan" to allow full circular angles.
"atan" limits to +/- pi/2. EBJ
940111 Changed from oldstyle include file ls_eom.h; also changed
from DATA to SCALAR type. EBJ
950207 Initialized Alpha_dot and Beta_dot to zero on first pass; calculated
thereafter. EBJ
950207 Initialized Alpha_dot and Beta_dot to zero on first pass; calculated
thereafter. EBJ
950224 Added logic to avoid adding additional increment to V_east
in case V_east already accounts for rotating earth.
EBJ
950224 Added logic to avoid adding additional increment to V_east
in case V_east already accounts for rotating earth.
EBJ
CURRENT RCS HEADER:
CURRENT RCS HEADER:
$Header$
$Log$
@ -268,32 +268,32 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO: None.
CALLS TO: None.
----------------------------------------------------------------------------
INPUTS: State derivatives
INPUTS: State derivatives
----------------------------------------------------------------------------
OUTPUTS: States
OUTPUTS: States
--------------------------------------------------------------------------*/
@ -312,8 +312,8 @@ Initial Flight Gear revision.
/* #include "ls_sim_control.h" */
#include <math.h>
extern Model current_model; /* defined in ls_model.c */
extern SCALAR Simtime; /* defined in ls_main.c */
extern Model current_model; /* defined in ls_model.c */
extern SCALAR Simtime; /* defined in ls_main.c */
void uiuc_init_vars() {
static int init = 0;
@ -328,102 +328,102 @@ void uiuc_init_vars() {
void ls_step( SCALAR dt, int Initialize ) {
static int inited = 0;
SCALAR dth;
static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
SCALAR epsilon, inv_eps, local_gnd_veast;
SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
static SCALAR e_0, e_1, e_2, e_3;
static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
static int inited = 0;
SCALAR dth;
static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
SCALAR epsilon, inv_eps, local_gnd_veast;
SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
static SCALAR e_0, e_1, e_2, e_3;
static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
/* I N I T I A L I Z A T I O N */
if ( (inited == 0) || (Initialize != 0) )
{
if ( (inited == 0) || (Initialize != 0) )
{
/* Set past values to zero */
v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
/* Initialize geocentric position from geodetic latitude and altitude */
ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
Earth_position_angle = 0;
Lon_geocentric = Longitude;
Radius_to_vehicle = Altitude + Sea_level_radius;
ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
Earth_position_angle = 0;
Lon_geocentric = Longitude;
Radius_to_vehicle = Altitude + Sea_level_radius;
/* Correct eastward velocity to account for earths' rotation, if necessary */
local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
V_east = V_east + local_gnd_veast;
local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
V_east = V_east + local_gnd_veast;
/* Initialize quaternions and transformation matrix from Euler angles */
// Initialize UIUC aircraft model
if (current_model == UIUC) {
uiuc_init_2_wrapper();
// Initialize UIUC aircraft model
if (current_model == UIUC) {
uiuc_init_2_wrapper();
}
e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
+ sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
- sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
+ sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
+ sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
+ sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
- sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
+ sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
+ sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
// Initialize local velocities (V_north, V_east, V_down)
// based on transformation matrix calculated above
if (current_model == UIUC) {
uiuc_local_vel_init();
}
// Initialize local velocities (V_north, V_east, V_down)
// based on transformation matrix calculated above
if (current_model == UIUC) {
uiuc_local_vel_init();
}
/* Calculate local gravitation acceleration */
/* Calculate local gravitation acceleration */
ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
/* Initialize vehicle model */
/* Initialize vehicle model */
ls_aux();
ls_model(0.0, 0);
ls_aux();
ls_model(0.0, 0);
/* Calculate initial accelerations */
/* Calculate initial accelerations */
ls_accel();
ls_accel();
/* Initialize auxiliary variables */
ls_aux();
Std_Alpha_dot = 0.;
Std_Beta_dot = 0.;
ls_aux();
Std_Alpha_dot = 0.;
Std_Beta_dot = 0.;
/* set flag; disable integrators */
inited = -1;
dt = 0.0;
}
inited = -1;
dt = 0.0;
}
/* Update time */
dth = 0.5*dt;
Simtime = Simtime + dt;
dth = 0.5*dt;
Simtime = Simtime + dt;
/* L I N E A R V E L O C I T I E S */
@ -446,12 +446,12 @@ void ls_step( SCALAR dt, int Initialize ) {
cos_Lat_geocentric = cos(Lat_geocentric);
if ( cos_Lat_geocentric != 0) {
Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
}
Latitude_dot = V_north*inv_Radius_to_vehicle;
Radius_dot = -V_down;
/* A N G U L A R V E L O C I T I E S A N D P O S I T I O N S */
/* Integrate rotational accelerations to get velocities */
@ -493,74 +493,74 @@ void ls_step( SCALAR dt, int Initialize ) {
/* Integrate using trapezoidal as before */
e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
/* calculate orthagonality correction - scale quaternion to unity length */
epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
inv_eps = 1/epsilon;
e_0 = inv_eps*e_0;
e_1 = inv_eps*e_1;
e_2 = inv_eps*e_2;
e_3 = inv_eps*e_3;
epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
inv_eps = 1/epsilon;
e_0 = inv_eps*e_0;
e_1 = inv_eps*e_1;
e_2 = inv_eps*e_2;
e_3 = inv_eps*e_3;
/* Save past values */
e_dot_0_past = e_dot_0;
e_dot_1_past = e_dot_1;
e_dot_2_past = e_dot_2;
e_dot_3_past = e_dot_3;
e_dot_0_past = e_dot_0;
e_dot_1_past = e_dot_1;
e_dot_2_past = e_dot_2;
e_dot_3_past = e_dot_3;
/* Update local to body transformation matrix */
T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
/* Calculate Euler angles */
Theta = asin( -T_local_to_body_13 );
Theta = asin( -T_local_to_body_13 );
if( T_local_to_body_11 == 0 )
Psi = 0;
else
Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
if( T_local_to_body_11 == 0 )
Psi = 0;
else
Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
if( T_local_to_body_33 == 0 )
Phi = 0;
else
Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
if( T_local_to_body_33 == 0 )
Phi = 0;
else
Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
/* Resolve Psi to 0 - 359.9999 */
if (Psi < 0 ) Psi = Psi + 2*LS_PI;
if (Psi < 0 ) Psi = Psi + 2*LS_PI;
/* L I N E A R P O S I T I O N S */
/* Trapezoidal acceleration for position */
Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
/* Save past values */
latitude_dot_past = Latitude_dot;
longitude_dot_past = Longitude_dot;
radius_dot_past = Radius_dot;
latitude_dot_past = Latitude_dot;
longitude_dot_past = Longitude_dot;
radius_dot_past = Radius_dot;
/* end of ls_step */
}
/*************************************************************************/

2
src/FDM/LaRCsim/ls_step.h
View file

@ -5,7 +5,7 @@
#define _LS_STEP_H
void ls_step( SCALAR dt, int Initialize );
void ls_step( SCALAR dt, int Initialize );
#endif /* _LS_STEP_H */

120
src/FDM/LaRCsim/ls_sym.h
View file

@ -1,42 +1,42 @@
/***************************************************************************
TITLE: ls_sym.h
TITLE: ls_sym.h
----------------------------------------------------------------------------
FUNCTION: Header file for symbol table routines
FUNCTION: Header file for symbol table routines
----------------------------------------------------------------------------
MODULE STATUS: production
MODULE STATUS: production
----------------------------------------------------------------------------
GENEALOGY: Created 930629 by E. B. Jackson
GENEALOGY: Created 930629 by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
CODED BY: same
MAINTAINED BY:
DESIGNED BY: Bruce Jackson
CODED BY: same
MAINTAINED BY:
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
950227 Added header and declarations for ls_print_findsym_error(),
ls_get_double(), and ls_get_double() routines. EBJ
950227 Added header and declarations for ls_print_findsym_error(),
ls_get_double(), and ls_get_double() routines. EBJ
950302 Added structure for symbol description. EBJ
950306 Added ls_get_sym_val() and ls_set_sym_val() routines.
This is now the production version. EBJ
CURRENT RCS HEADER:
950302 Added structure for symbol description. EBJ
950306 Added ls_get_sym_val() and ls_set_sym_val() routines.
This is now the production version. EBJ
CURRENT RCS HEADER:
$Header$
$Log$
@ -76,23 +76,23 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
@ -118,56 +118,56 @@ Initial Flight Gear revision.
typedef enum {Unknown, Char, UChar, SHint, USHint, Sint, Uint, Slng, Ulng, flt, dbl} vartype;
typedef char SYMBOL_NAME[64];
typedef vartype SYMBOL_TYPE;
typedef char SYMBOL_NAME[64];
typedef vartype SYMBOL_TYPE;
typedef struct
{
SYMBOL_NAME Mod_Name;
SYMBOL_NAME Par_Name;
SYMBOL_NAME Mod_Name;
SYMBOL_NAME Par_Name;
SYMBOL_TYPE Par_Type;
SYMBOL_NAME Alias;
char *Addr;
} symbol_rec;
char *Addr;
} symbol_rec;
extern int ls_findsym( const char *modname, const char *varname,
char **addr, vartype *vtype );
extern int ls_findsym( const char *modname, const char *varname,
char **addr, vartype *vtype );
extern void ls_print_findsym_error(int result,
char *mod_name,
char *var_name);
extern void ls_print_findsym_error(int result,
char *mod_name,
char *var_name);
extern double ls_get_double(vartype sym_type, void *addr );
extern double ls_get_double(vartype sym_type, void *addr );
extern void ls_set_double(vartype sym_type, void *addr, double value );
extern void ls_set_double(vartype sym_type, void *addr, double value );
extern double ls_get_sym_val( symbol_rec *symrec, int *error );
extern double ls_get_sym_val( symbol_rec *symrec, int *error );
/* This routine attempts to return the present value of the symbol
described in symbol_rec. If Addr is non-zero, the value of that
location, interpreted as type double, will be returned. If Addr
is zero, and Mod_Name and Par_Name are both not null strings,
the ls_findsym() routine is used to try to obtain the address
by looking at debugger symbol tables in the executable image, and
the value of the double contained at that address is returned,
and the symbol record is updated to contain the address of that
symbol. If an error is discovered, 'error' will be non-zero and
and error message is printed on stderr. */
/* This routine attempts to return the present value of the symbol
described in symbol_rec. If Addr is non-zero, the value of that
location, interpreted as type double, will be returned. If Addr
is zero, and Mod_Name and Par_Name are both not null strings,
the ls_findsym() routine is used to try to obtain the address
by looking at debugger symbol tables in the executable image, and
the value of the double contained at that address is returned,
and the symbol record is updated to contain the address of that
symbol. If an error is discovered, 'error' will be non-zero and
and error message is printed on stderr. */
extern void ls_set_sym_val( symbol_rec *symrec, double value );
extern void ls_set_sym_val( symbol_rec *symrec, double value );
/* This routine sets the value of a double at the location pointed
to by the symbol_rec's Addr field, if Addr is non-zero. If Addr
is zero, and Mod_Name and Par_Name are both not null strings,
the ls_findsym() routine is used to try to obtain the address
by looking at debugger symbol tables in the executable image, and
the value of the double contained at that address is returned,
and the symbol record is updated to contain the address of that
symbol. If an error is discovered, 'error' will be non-zero and
and error message is printed on stderr. */
/* This routine sets the value of a double at the location pointed
to by the symbol_rec's Addr field, if Addr is non-zero. If Addr
is zero, and Mod_Name and Par_Name are both not null strings,
the ls_findsym() routine is used to try to obtain the address
by looking at debugger symbol tables in the executable image, and
the value of the double contained at that address is returned,
and the symbol record is updated to contain the address of that
symbol. If an error is discovered, 'error' will be non-zero and
and error message is printed on stderr. */
#endif /* _LS_SYM_H */

642
src/FDM/LaRCsim/ls_trim.c
View file

@ -1,90 +1,90 @@
/***************************************************************************
TITLE: ls_trim.c
TITLE: ls_trim.c
----------------------------------------------------------------------------
FUNCTION: Trims the simulated aircraft by using certain
controls to null out a similar number of outputs.
FUNCTION: Trims the simulated aircraft by using certain
controls to null out a similar number of outputs.
This routine used modified Newton-Raphson method to find the vector
of control corrections, delta_U, to drive a similar-sized vector of
output errors, Y, to near-zero. Nearness to zero is to within a
tolerance specified by the Criteria vector. An optional Weight
vector can be used to improve the numerical properties of the
Jacobian matrix (called H_Partials).
of control corrections, delta_U, to drive a similar-sized vector of
output errors, Y, to near-zero. Nearness to zero is to within a
tolerance specified by the Criteria vector. An optional Weight
vector can be used to improve the numerical properties of the
Jacobian matrix (called H_Partials).
Using a single-sided difference method, each control is
independently perturbed and the change in each output of
interest is calculated, forming a Jacobian matrix H (variable
name is H_Partials):
Using a single-sided difference method, each control is
independently perturbed and the change in each output of
interest is calculated, forming a Jacobian matrix H (variable
name is H_Partials):
dY = H dU
dY = H dU
The columns of H correspond to the control effect; the rows of
H correspond to the outputs affected.
The columns of H correspond to the control effect; the rows of
H correspond to the outputs affected.
We wish to find dU such that for U = U0 + dU,
Y = Y0 + dY = 0
or dY = -Y0
We wish to find dU such that for U = U0 + dU,
Y = Y0 + dY = 0
or dY = -Y0
One solution is dU = inv(H)*(-Y0); however, inverting H
directly is not numerically sound, since it may be singular
(especially if one of the controls is on a limit, or the
problem is poorly posed). An alternative is to either weight
the elements of dU to make them more normalized; another is to
multiply both sides by the transpose of H and invert the
resulting [H' H]. This routine does both:
One solution is dU = inv(H)*(-Y0); however, inverting H
directly is not numerically sound, since it may be singular
(especially if one of the controls is on a limit, or the
problem is poorly posed). An alternative is to either weight
the elements of dU to make them more normalized; another is to
multiply both sides by the transpose of H and invert the
resulting [H' H]. This routine does both:
-Y0 = H dU
W (-Y0) = W H dU premultiply by W
H' W (-Y0) = H' W H dU premultiply by H'
W (-Y0) = W H dU premultiply by W
H' W (-Y0) = H' W H dU premultiply by H'
dU = [inv(H' W H)][ H' W (-Y0)] Solve for dU
As a further refinement, dU is limited to a smallish magnitude
so that Y approaches 0 in small steps (to avoid an overshoot
if the problem is inherently non-linear).
As a further refinement, dU is limited to a smallish magnitude
so that Y approaches 0 in small steps (to avoid an overshoot
if the problem is inherently non-linear).
Lastly, this routine can be easily fooled by "local minima",
or depressions in the solution space that don't lead to a Y =
0 solution. The only advice we can offer is to "go somewheres
else and try again"; often approaching a trim solution from a
different (non-trimmed) starting point will prove beneficial.
Lastly, this routine can be easily fooled by "local minima",
or depressions in the solution space that don't lead to a Y =
0 solution. The only advice we can offer is to "go somewheres
else and try again"; often approaching a trim solution from a
different (non-trimmed) starting point will prove beneficial.
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created from old CASTLE SHELL$TRIM.PAS
on 6 FEB 95, which was based upon an Ames
CASPRE routine called BQUIET.
GENEALOGY: Created from old CASTLE SHELL$TRIM.PAS
on 6 FEB 95, which was based upon an Ames
CASPRE routine called BQUIET.
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
DESIGNED BY: E. B. Jackson
CODED BY: same
CODED BY: same
MAINTAINED BY: same
MAINTAINED BY: same
----------------------------------------------------------------------------
MODIFICATION HISTORY:
MODIFICATION HISTORY:
DATE PURPOSE BY
DATE PURPOSE BY
950307 Modified to make use of ls_get_sym_val and ls_put_sym_val
routines. EBJ
950329 Fixed bug in making use of more than 3 controls;
removed call by ls_trim_get_set() to ls_trim_init(). EBJ
950307 Modified to make use of ls_get_sym_val and ls_put_sym_val
routines. EBJ
950329 Fixed bug in making use of more than 3 controls;
removed call by ls_trim_get_set() to ls_trim_init(). EBJ
CURRENT RCS HEADER:
CURRENT RCS HEADER:
$Header$
$Log$
@ -142,23 +142,23 @@ Start of 0.6.x branch.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
@ -192,38 +192,38 @@ static char rcsid[] = "$Id$";
typedef struct
{
symbol_rec Symbol;
double Min_Val, Max_Val, Curr_Val, Authority;
double Percent, Requested_Percent, Pert_Size;
int Ineffective, At_Limit;
symbol_rec Symbol;
double Min_Val, Max_Val, Curr_Val, Authority;
double Percent, Requested_Percent, Pert_Size;
int Ineffective, At_Limit;
} control_rec;
typedef struct
{
symbol_rec Symbol;
double Curr_Val, Weighting, Trim_Criteria;
int Uncontrollable;
symbol_rec Symbol;
double Curr_Val, Weighting, Trim_Criteria;
int Uncontrollable;
} output_rec;
static int Symbols_loaded = 0;
static int Index;
static int Trim_Cycles;
static int First;
static int Trimmed;
static double Gain;
static int Symbols_loaded = 0;
static int Index;
static int Trim_Cycles;
static int First;
static int Trimmed;
static double Gain;
static int Number_of_Controls;
static int Number_of_Outputs;
static control_rec Controls[ MAX_NUMBER_OF_CONTROLS ];
static output_rec Outputs[ MAX_NUMBER_OF_OUTPUTS ];
static int Number_of_Controls;
static int Number_of_Outputs;
static control_rec Controls[ MAX_NUMBER_OF_CONTROLS ];
static output_rec Outputs[ MAX_NUMBER_OF_OUTPUTS ];
static double **H_Partials;
static double **H_Partials;
static double Baseline_Output[ MAX_NUMBER_OF_OUTPUTS ];
static double Saved_Control, Saved_Control_Percent;
static double Baseline_Output[ MAX_NUMBER_OF_OUTPUTS ];
static double Saved_Control, Saved_Control_Percent;
static double Cost, Previous_Cost;
static double Cost, Previous_Cost;
@ -242,13 +242,13 @@ int ls_trim_init()
Trimmed = 0;
for (i=0;i<Number_of_Controls;i++)
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error) Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Requested_Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
}
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error) Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Requested_Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
}
H_Partials = nr_matrix( 1, Number_of_Controls, 1, Number_of_Controls );
if (H_Partials == 0) return -1;
@ -262,20 +262,20 @@ void ls_trim_get_vals()
int i, error;
for (i=0;i<Number_of_Outputs;i++)
{
Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
}
{
Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
}
Cost = 0.0;
for (i=0;i<Number_of_Controls;i++)
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error) Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
}
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error) Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
}
}
@ -286,22 +286,22 @@ void ls_trim_move_controls()
int i;
for(i=0;i<Number_of_Controls;i++)
{
Controls[i].At_Limit = 0;
if (Controls[i].Requested_Percent <= 0.0)
{
Controls[i].Requested_Percent = 0.0;
Controls[i].At_Limit = 1;
}
if (Controls[i].Requested_Percent >= 1.0)
{
Controls[i].Requested_Percent = 1.0;
Controls[i].At_Limit = 1;
}
Controls[i].Curr_Val = Controls[i].Min_Val +
(Controls[i].Max_Val - Controls[i].Min_Val) *
Controls[i].Requested_Percent;
}
{
Controls[i].At_Limit = 0;
if (Controls[i].Requested_Percent <= 0.0)
{
Controls[i].Requested_Percent = 0.0;
Controls[i].At_Limit = 1;
}
if (Controls[i].Requested_Percent >= 1.0)
{
Controls[i].Requested_Percent = 1.0;
Controls[i].At_Limit = 1;
}
Controls[i].Curr_Val = Controls[i].Min_Val +
(Controls[i].Max_Val - Controls[i].Min_Val) *
Controls[i].Requested_Percent;
}
}
void ls_trim_put_controls()
@ -310,8 +310,8 @@ void ls_trim_put_controls()
int i;
for (i=0;i<Number_of_Controls;i++)
if (Controls[i].Symbol.Addr)
ls_set_sym_val( &Controls[i].Symbol, Controls[i].Curr_Val );
if (Controls[i].Symbol.Addr)
ls_set_sym_val( &Controls[i].Symbol, Controls[i].Curr_Val );
}
void ls_trim_calc_cost()
@ -321,7 +321,7 @@ void ls_trim_calc_cost()
Cost = 0.0;
for(i=0;i<Number_of_Outputs;i++)
Cost += pow((Outputs[i].Curr_Val/Outputs[i].Trim_Criteria),2.0);
Cost += pow((Outputs[i].Curr_Val/Outputs[i].Trim_Criteria),2.0);
}
void ls_trim_save_baseline_outputs()
@ -329,7 +329,7 @@ void ls_trim_save_baseline_outputs()
int i, error;
for (i=0;i<Number_of_Outputs;i++)
Baseline_Output[i] = ls_get_sym_val( &Outputs[i].Symbol, &error );
Baseline_Output[i] = ls_get_sym_val( &Outputs[i].Symbol, &error );
}
int ls_trim_eval_outputs()
@ -338,7 +338,7 @@ int ls_trim_eval_outputs()
trimmed = 1;
for(i=0;i<Number_of_Outputs;i++)
if( fabs(Outputs[i].Curr_Val) > Outputs[i].Trim_Criteria) trimmed = 0;
if( fabs(Outputs[i].Curr_Val) > Outputs[i].Trim_Criteria) trimmed = 0;
return trimmed;
}
@ -350,54 +350,54 @@ void ls_trim_calc_h_column()
delta_control = (Controls[Index].Curr_Val - Saved_Control)/Controls[Index].Authority;
for(i=0;i<Number_of_Outputs;i++)
{
delta_output = Outputs[i].Curr_Val - Baseline_Output[i];
H_Partials[i+1][Index+1] = delta_output/delta_control;
}
{
delta_output = Outputs[i].Curr_Val - Baseline_Output[i];
H_Partials[i+1][Index+1] = delta_output/delta_control;
}
}
void ls_trim_do_step()
{
int i, j, l, singular;
double **h_trans_w_h;
double delta_req_mag, scaling;
double delta_U_requested[ MAX_NUMBER_OF_CONTROLS ];
double temp[ MAX_NUMBER_OF_CONTROLS ];
double **h_trans_w_h;
double delta_req_mag, scaling;
double delta_U_requested[ MAX_NUMBER_OF_CONTROLS ];
double temp[ MAX_NUMBER_OF_CONTROLS ];
/* Identify ineffective controls (whose partials are all near zero) */
for (j=0;j<Number_of_Controls;j++)
{
Controls[j].Ineffective = 1;
for(i=0;i<Number_of_Outputs;i++)
if (fabs(H_Partials[i+1][j+1]) > EPS) Controls[j].Ineffective = 0;
}
{
Controls[j].Ineffective = 1;
for(i=0;i<Number_of_Outputs;i++)
if (fabs(H_Partials[i+1][j+1]) > EPS) Controls[j].Ineffective = 0;
}
/* Identify uncontrollable outputs */
for (j=0;j<Number_of_Outputs;j++)
{
Outputs[j].Uncontrollable = 1;
for(i=0;i<Number_of_Controls;i++)
if (fabs(H_Partials[j+1][i+1]) > EPS) Outputs[j].Uncontrollable = 0;
}
{
Outputs[j].Uncontrollable = 1;
for(i=0;i<Number_of_Controls;i++)
if (fabs(H_Partials[j+1][i+1]) > EPS) Outputs[j].Uncontrollable = 0;
}
/* Calculate well-conditioned partials matrix [ H' W H ] */
h_trans_w_h = nr_matrix(1, Number_of_Controls, 1, Number_of_Controls);
if (h_trans_w_h == 0)
{
fprintf(stderr, "Memory error in ls_trim().\n");
exit(1);
}
{
fprintf(stderr, "Memory error in ls_trim().\n");
exit(1);
}
for (l=1;l<=Number_of_Controls;l++)
for (j=1;j<=Number_of_Controls;j++)
{
h_trans_w_h[l][j] = 0.0;
for (i=1;i<=Number_of_Outputs;i++)
h_trans_w_h[l][j] +=
H_Partials[i][l]*H_Partials[i][j]*Outputs[i-1].Weighting;
}
for (j=1;j<=Number_of_Controls;j++)
{
h_trans_w_h[l][j] = 0.0;
for (i=1;i<=Number_of_Outputs;i++)
h_trans_w_h[l][j] +=
H_Partials[i][l]*H_Partials[i][j]*Outputs[i-1].Weighting;
}
/* Invert the partials array [ inv( H' W H ) ]; note: h_trans_w_h is replaced
with its inverse during this function call */
@ -405,50 +405,50 @@ void ls_trim_do_step()
singular = nr_gaussj( h_trans_w_h, Number_of_Controls, 0, 0 );
if (singular) /* Can't invert successfully */
{
nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
1, Number_of_Controls );
fprintf(stderr, "Singular matrix in ls_trim().\n");
return;
}
{
nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
1, Number_of_Controls );
fprintf(stderr, "Singular matrix in ls_trim().\n");
return;
}
/* Form right hand side of equality: temp = [ H' W (-Y) ] */
for(i=0;i<Number_of_Controls;i++)
{
temp[i] = 0.0;
for(j=0;j<Number_of_Outputs;j++)
temp[i] -= H_Partials[j+1][i+1]*Baseline_Output[j]*Outputs[j].Weighting;
}
{
temp[i] = 0.0;
for(j=0;j<Number_of_Outputs;j++)
temp[i] -= H_Partials[j+1][i+1]*Baseline_Output[j]*Outputs[j].Weighting;
}
/* Solve for dU = [inv( H' W H )][ H' W (-Y)] */
for(i=0;i<Number_of_Controls;i++)
{
delta_U_requested[i] = 0.0;
for(j=0;j<Number_of_Controls;j++)
delta_U_requested[i] += h_trans_w_h[i+1][j+1]*temp[j];
}
{
delta_U_requested[i] = 0.0;
for(j=0;j<Number_of_Controls;j++)
delta_U_requested[i] += h_trans_w_h[i+1][j+1]*temp[j];
}
/* limit step magnitude to certain size, but not direction */
delta_req_mag = 0.0;
for(i=0;i<Number_of_Controls;i++)
delta_req_mag += delta_U_requested[i]*delta_U_requested[i];
delta_req_mag += delta_U_requested[i]*delta_U_requested[i];
delta_req_mag = sqrt(delta_req_mag);
scaling = STEP_LIMIT/delta_req_mag;
if (scaling < 1.0)
for(i=0;i<Number_of_Controls;i++)
delta_U_requested[i] *= scaling;
for(i=0;i<Number_of_Controls;i++)
delta_U_requested[i] *= scaling;
/* Convert deltas to percent of authority */
for(i=0;i<Number_of_Controls;i++)
Controls[i].Requested_Percent = Controls[i].Percent + delta_U_requested[i];
Controls[i].Requested_Percent = Controls[i].Percent + delta_U_requested[i];
/* free up temporary matrix */
nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
1, Number_of_Controls );
1, Number_of_Controls );
}
@ -462,70 +462,70 @@ int ls_trim()
Trimmed = 0;
if (Symbols_loaded) {
ls_trim_init(); /* Initialize Outputs & controls */
ls_trim_get_vals(); /* Limit the current control settings */
Baseline = TRUE;
ls_trim_move_controls(); /* Write out the new values of controls */
ls_trim_put_controls();
ls_loop( 0.0, -1 ); /* Cycle the simulation once with new limited
controls */
ls_trim_init(); /* Initialize Outputs & controls */
ls_trim_get_vals(); /* Limit the current control settings */
Baseline = TRUE;
ls_trim_move_controls(); /* Write out the new values of controls */
ls_trim_put_controls();
ls_loop( 0.0, -1 ); /* Cycle the simulation once with new limited
controls */
/* Main trim cycle loop follows */
/* Main trim cycle loop follows */
while((!Trimmed) && (Trim_Cycles < Max_Cycles))
{
ls_trim_get_vals();
if (Index == -1)
{
ls_trim_calc_cost();
/*Adjust_Gain(); */
ls_trim_save_baseline_outputs();
Trimmed = ls_trim_eval_outputs();
}
else
{
ls_trim_calc_h_column();
Controls[Index].Curr_Val = Saved_Control;
Controls[Index].Percent = Saved_Control_Percent;
Controls[Index].Requested_Percent = Saved_Control_Percent;
}
Index++;
if (!Trimmed)
{
if (Index >= Number_of_Controls)
{
Baseline = TRUE;
Index = -1;
ls_trim_do_step();
}
else
{ /* Save the current value & pert next control */
Baseline = FALSE;
Saved_Control = Controls[Index].Curr_Val;
Saved_Control_Percent = Controls[Index].Percent;
while((!Trimmed) && (Trim_Cycles < Max_Cycles))
{
ls_trim_get_vals();
if (Index == -1)
{
ls_trim_calc_cost();
/*Adjust_Gain(); */
ls_trim_save_baseline_outputs();
Trimmed = ls_trim_eval_outputs();
}
else
{
ls_trim_calc_h_column();
Controls[Index].Curr_Val = Saved_Control;
Controls[Index].Percent = Saved_Control_Percent;
Controls[Index].Requested_Percent = Saved_Control_Percent;
}
Index++;
if (!Trimmed)
{
if (Index >= Number_of_Controls)
{
Baseline = TRUE;
Index = -1;
ls_trim_do_step();
}
else
{ /* Save the current value & pert next control */
Baseline = FALSE;
Saved_Control = Controls[Index].Curr_Val;
Saved_Control_Percent = Controls[Index].Percent;
if (Controls[Index].Percent <
(1.0 - Controls[Index].Pert_Size) )
{
Controls[Index].Requested_Percent =
Controls[Index].Percent +
Controls[Index].Pert_Size ;
}
else
{
Controls[Index].Requested_Percent =
Controls[Index].Percent -
Controls[Index].Pert_Size;
}
}
ls_trim_move_controls();
ls_trim_put_controls();
ls_loop( 0.0, -1 );
Trim_Cycles++;
}
}
if (Controls[Index].Percent <
(1.0 - Controls[Index].Pert_Size) )
{
Controls[Index].Requested_Percent =
Controls[Index].Percent +
Controls[Index].Pert_Size ;
}
else
{
Controls[Index].Requested_Percent =
Controls[Index].Percent -
Controls[Index].Pert_Size;
}
}
ls_trim_move_controls();
ls_trim_put_controls();
ls_loop( 0.0, -1 );
Trim_Cycles++;
}
}
nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls );
nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls );
}
if (!Trimmed) fprintf(stderr, "Trim unsuccessful.\n");
@ -562,99 +562,99 @@ char *ls_trim_get_set(char *buffer, char *eob)
while( !abrt && (eob > bufptr))
{
bufptr = strtok_r( 0L, "\n", lasts );
if (bufptr == 0) return 0L;
if (strncasecmp( bufptr, "end", 3) == 0) break;
bufptr = strtok_r( 0L, "\n", lasts );
if (bufptr == 0) return 0L;
if (strncasecmp( bufptr, "end", 3) == 0) break;
sscanf( bufptr, "%s", line );
if (line[0] != '#') /* ignore comments */
{
switch (looking_for)
{
case controls_header:
{
if (strncasecmp( line, "controls", 8) == 0)
{
n = sscanf( bufptr, "%s%d", line, &Number_of_Controls );
if (n != 2) abrt = 1;
looking_for = controls;
i = 0;
}
break;
}
case controls:
{
n = sscanf( bufptr, "%s%s%le%le%le",
Controls[i].Symbol.Mod_Name,
Controls[i].Symbol.Par_Name,
&Controls[i].Min_Val,
&Controls[i].Max_Val,
&Controls[i].Pert_Size);
if (n != 5) abrt = 1;
Controls[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Controls) looking_for = outputs_header;
break;
}
case outputs_header:
{
if (strncasecmp( line, "outputs", 7) == 0)
{
n = sscanf( bufptr, "%s%d", line, &Number_of_Outputs );
if (n != 2) abrt = 1;
looking_for = outputs;
i = 0;
}
break;
}
case outputs:
{
n = sscanf( bufptr, "%s%s%le",
Outputs[i].Symbol.Mod_Name,
Outputs[i].Symbol.Par_Name,
&Outputs[i].Trim_Criteria );
if (n != 3) abrt = 1;
Outputs[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Outputs) looking_for = done;
}
case done:
{
break;
}
}
sscanf( bufptr, "%s", line );
if (line[0] != '#') /* ignore comments */
{
switch (looking_for)
{
case controls_header:
{
if (strncasecmp( line, "controls", 8) == 0)
{
n = sscanf( bufptr, "%s%d", line, &Number_of_Controls );
if (n != 2) abrt = 1;
looking_for = controls;
i = 0;
}
break;
}
case controls:
{
n = sscanf( bufptr, "%s%s%le%le%le",
Controls[i].Symbol.Mod_Name,
Controls[i].Symbol.Par_Name,
&Controls[i].Min_Val,
&Controls[i].Max_Val,
&Controls[i].Pert_Size);
if (n != 5) abrt = 1;
Controls[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Controls) looking_for = outputs_header;
break;
}
case outputs_header:
{
if (strncasecmp( line, "outputs", 7) == 0)
{
n = sscanf( bufptr, "%s%d", line, &Number_of_Outputs );
if (n != 2) abrt = 1;
looking_for = outputs;
i = 0;
}
break;
}
case outputs:
{
n = sscanf( bufptr, "%s%s%le",
Outputs[i].Symbol.Mod_Name,
Outputs[i].Symbol.Par_Name,
&Outputs[i].Trim_Criteria );
if (n != 3) abrt = 1;
Outputs[i].Symbol.Addr = NIL_POINTER;
i++;
if (i >= Number_of_Outputs) looking_for = done;
}
case done:
{
break;
}
}
}
}
}
if ((!abrt) &&
(Number_of_Controls > 0) &&
(Number_of_Outputs == Number_of_Controls))
{
Symbols_loaded = 1;
(Number_of_Controls > 0) &&
(Number_of_Outputs == Number_of_Controls))
{
Symbols_loaded = 1;
for(i=0;i<Number_of_Controls;i++) /* Initialize fields in Controls data */
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error)
Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Authority = Controls[i].Max_Val - Controls[i].Min_Val;
if (Controls[i].Authority == 0.0)
Controls[i].Authority = 1.0;
Controls[i].Requested_Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
Controls[i].Pert_Size = Controls[i].Pert_Size/Controls[i].Authority;
}
for(i=0;i<Number_of_Controls;i++) /* Initialize fields in Controls data */
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error)
Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Authority = Controls[i].Max_Val - Controls[i].Min_Val;
if (Controls[i].Authority == 0.0)
Controls[i].Authority = 1.0;
Controls[i].Requested_Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
Controls[i].Pert_Size = Controls[i].Pert_Size/Controls[i].Authority;
}
for (i=0;i<Number_of_Outputs;i++) /* Initialize fields in Outputs data */
{
Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
Outputs[i].Weighting =
Outputs[0].Trim_Criteria/Outputs[i].Trim_Criteria;
}
}
for (i=0;i<Number_of_Outputs;i++) /* Initialize fields in Outputs data */
{
Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
Outputs[i].Weighting =
Outputs[0].Trim_Criteria/Outputs[i].Trim_Criteria;
}
}
bufptr = *lasts;
return bufptr;
@ -676,19 +676,19 @@ void ls_trim_put_set( FILE *fp )
fprintf(fp, " controls: %d\n", Number_of_Controls);
fprintf(fp, "# module parameter min_val max_val pert_size\n");
for (i=0; i<Number_of_Controls; i++)
fprintf(fp, " %s\t%s\t%E\t%E\t%E\n",
Controls[i].Symbol.Mod_Name,
Controls[i].Symbol.Par_Name,
Controls[i].Min_Val,
Controls[i].Max_Val,
Controls[i].Pert_Size*Controls[i].Authority);
fprintf(fp, " %s\t%s\t%E\t%E\t%E\n",
Controls[i].Symbol.Mod_Name,
Controls[i].Symbol.Par_Name,
Controls[i].Min_Val,
Controls[i].Max_Val,
Controls[i].Pert_Size*Controls[i].Authority);
fprintf(fp, " outputs: %d\n", Number_of_Outputs);
fprintf(fp, "# module parameter trim_criteria\n");
for (i=0;i<Number_of_Outputs;i++)
fprintf(fp, " %s\t%s\t%E\n",
Outputs[i].Symbol.Mod_Name,
Outputs[i].Symbol.Par_Name,
Outputs[i].Trim_Criteria );
fprintf(fp, " %s\t%s\t%E\n",
Outputs[i].Symbol.Mod_Name,
Outputs[i].Symbol.Par_Name,
Outputs[i].Trim_Criteria );
fprintf(fp, "end\n");
return;
}

30
src/FDM/LaRCsim/ls_types.h
View file

@ -1,34 +1,34 @@
/***************************************************************************
TITLE: ls_types.h
TITLE: ls_types.h
----------------------------------------------------------------------------
FUNCTION: LaRCSim type definitions header file
FUNCTION: LaRCSim type definitions header file
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 15 DEC 1993 by Bruce Jackson from old
ls_eom.h header
GENEALOGY: Created 15 DEC 1993 by Bruce Jackson from old
ls_eom.h header
----------------------------------------------------------------------------
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
DESIGNED BY: B. Jackson
CODED BY: B. Jackson
MAINTAINED BY: guess who
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
19 MAY 2001 Reduce MSVC6 warnings Geoff R. McLane
MODIFICATION HISTORY:
DATE PURPOSE BY
19 MAY 2001 Reduce MSVC6 warnings Geoff R. McLane
--------------------------------------------------------------------------*/
#ifndef _LS_TYPES_H

70
src/FDM/LaRCsim/navion_aero.c
View file

@ -1,41 +1,41 @@
/***************************************************************************
TITLE: Navion_aero
TITLE: Navion_aero
----------------------------------------------------------------------------
FUNCTION: Linear aerodynamics model
FUNCTION: Linear aerodynamics model
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Based upon class notes from AA271, Stanford University,
GENEALOGY: Based upon class notes from AA271, Stanford University,
Spring 1988. Dr. Robert Cannon, instructor.
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY: Bruce Jackson
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY: Bruce Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
DATE PURPOSE BY
921229 Changed Alpha, Beta into radians; added Alpha bias.
EBJ
EBJ
930105 Modified to support linear airframe simulation by
adding shared memory initialization routine. EBJ
adding shared memory initialization routine. EBJ
931013 Added scaling by airspeed, to allow for low-airspeed
ground operations. EBJ
ground operations. EBJ
940216 Scaled long, lat stick and rudder to more appropriate values
of elevator and aileron. EBJ
of elevator and aileron. EBJ
----------------------------------------------------------------------------
@ -65,24 +65,24 @@ control surface deflections. For example, L_beta is an estimate of how
much roll moment varies per degree of sideslip increase. A decoding
ring is given below:
X Aerodynamic force, lbs, in X-axis (+ forward)
Y Aerodynamic force, lbs, in Y-axis (+ right)
Z Aerodynamic force, lbs, in Z-axis (+ down)
L Aero. moment about X-axis (+ roll right), ft-lbs
M Aero. moment about Y-axis (+ pitch up), ft-lbs
N Aero. moment about Z-axis (+ nose right), ft-lbs
X Aerodynamic force, lbs, in X-axis (+ forward)
Y Aerodynamic force, lbs, in Y-axis (+ right)
Z Aerodynamic force, lbs, in Z-axis (+ down)
L Aero. moment about X-axis (+ roll right), ft-lbs
M Aero. moment about Y-axis (+ pitch up), ft-lbs
N Aero. moment about Z-axis (+ nose right), ft-lbs
0 Subscript implying initial, or nominal, value
u X-axis component of airspeed (ft/sec) (+ forward)
v Y-axis component of airspeed (ft/sec) (+ right)
w Z-axis component of airspeed (ft/sec) (+ down)
p X-axis ang. rate (rad/sec) (+ roll right), rad/sec
q Y-axis ang. rate (rad/sec) (+ pitch up), rad/sec
r Z-axis ang. rate (rad/sec) (+ yaw right), rad/sec
beta Angle of sideslip, degrees (+ wind in RIGHT ear)
da Aileron deflection, degrees (+ left ail. TE down)
de Elevator deflection, degrees (+ trailing edge down)
dr Rudder deflection, degrees (+ trailing edge LEFT)
0 Subscript implying initial, or nominal, value
u X-axis component of airspeed (ft/sec) (+ forward)
v Y-axis component of airspeed (ft/sec) (+ right)
w Z-axis component of airspeed (ft/sec) (+ down)
p X-axis ang. rate (rad/sec) (+ roll right), rad/sec
q Y-axis ang. rate (rad/sec) (+ pitch up), rad/sec
r Z-axis ang. rate (rad/sec) (+ yaw right), rad/sec
beta Angle of sideslip, degrees (+ wind in RIGHT ear)
da Aileron deflection, degrees (+ left ail. TE down)
de Elevator deflection, degrees (+ trailing edge down)
dr Rudder deflection, degrees (+ trailing edge LEFT)
----------------------------------------------------------------------------
@ -203,10 +203,10 @@ void navion_aero( SCALAR dt, int Initialize ) {
F_Z_aero = scale*(Z_0 + Mass*(Z_u*u + Z_w*w + Z_de*elevator));
M_l_aero = scale*(I_xx*(L_beta*Std_Beta + L_p*P_body + L_r*R_body
+ L_da*aileron + L_dr*rudder));
+ L_da*aileron + L_dr*rudder));
M_m_aero = scale*(M_0 + I_yy*(M_w*w + M_q*Q_body + M_de*(elevator + Long_trim)));
M_n_aero = scale*(I_zz*(N_beta*Std_Beta + N_p*P_body + N_r*R_body
+ N_da*aileron + N_dr*rudder));
+ N_da*aileron + N_dr*rudder));
}

40
src/FDM/LaRCsim/navion_engine.c
View file

@ -1,34 +1,34 @@
/***************************************************************************
TITLE: engine.c
TITLE: engine.c
----------------------------------------------------------------------------
FUNCTION: dummy engine routine
FUNCTION: dummy engine routine
----------------------------------------------------------------------------
MODULE STATUS: incomplete
MODULE STATUS: incomplete
----------------------------------------------------------------------------
GENEALOGY: Created 15 OCT 92 as dummy routine for checkout. EBJ
GENEALOGY: Created 15 OCT 92 as dummy routine for checkout. EBJ
----------------------------------------------------------------------------
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
DESIGNED BY: designer
CODED BY: programmer
MAINTAINED BY: maintainer
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
CURRENT RCS HEADER INFO:
CURRENT RCS HEADER INFO:
$Header$
@ -38,23 +38,23 @@ $Header$
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY: ls_model();
CALLED BY: ls_model();
----------------------------------------------------------------------------
CALLS TO: none
CALLS TO: none
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
@ -64,7 +64,7 @@ $Header$
#include "ls_sim_control.h"
#include "ls_cockpit.h"
extern SIM_CONTROL sim_control_;
extern SIM_CONTROL sim_control_;
void navion_engine( SCALAR dt, int init ) {
/* if (init) { */

328
src/FDM/LaRCsim/navion_gear.c
View file

@ -1,38 +1,38 @@
/***************************************************************************
TITLE: gear
TITLE: gear
----------------------------------------------------------------------------
FUNCTION: Landing gear model for example simulation
FUNCTION: Landing gear model for example simulation
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Created 931012 by E. B. Jackson
GENEALOGY: Created 931012 by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
931218 Added navion.h header to allow connection with
aileron displacement for nosewheel steering. EBJ
940511 Connected nosewheel to rudder pedal; adjusted gain.
CURRENT RCS HEADER:
$Header$
$Log$
@ -92,23 +92,23 @@ Initial Flight Gear revision.
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
@ -167,21 +167,21 @@ void navion_gear( SCALAR dt, int Initialize ) {
#define NUM_WHEELS 3
static int num_wheels = NUM_WHEELS; /* number of wheels */
static int num_wheels = NUM_WHEELS; /* number of wheels */
static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations */
{
{ 10., 0., 4. }, /* in feet */
{ -1., 3., 4. },
{ -1., -3., 4. }
{ 10., 0., 4. }, /* in feet */
{ -1., 3., 4. },
{ -1., -3., 4. }
};
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1500., 5000., 5000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 100., 150., 150. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0.}; /* radians, +CW */
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 1500., 5000., 5000. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 100., 150., 150. };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0.}; /* radians, +CW */
/*
* End of aircraft specific code
*/
@ -194,51 +194,51 @@ void navion_gear( SCALAR dt, int Initialize ) {
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
* V V
*/
static DATA sliding_mu = 0.5;
static DATA rolling_mu = 0.01;
static DATA max_brake_mu = 0.6;
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA sliding_mu = 0.5;
static DATA rolling_mu = 0.01;
static DATA max_brake_mu = 0.6;
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
DATA temp3a[3], temp3b[3], tempF[3], tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
@ -249,115 +249,115 @@ void navion_gear( SCALAR dt, int Initialize ) {
caster_angle_rad[0] = 0.03*Rudder_pedal;
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
{
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );
/* plus contribution due to cg velocities */
/* plus contribution due to cg velocities */
add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
if( d_wheel_rwy_local_v[2] < 0. )
{
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
}
/* Calculate friction coefficients */
forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu;
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
if( d_wheel_rwy_local_v[2] < 0. )
{
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
}
/* Calculate friction coefficients */
forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu;
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}

48
src/FDM/LaRCsim/navion_init.c
View file

@ -1,59 +1,59 @@
/***************************************************************************
TITLE: navion_init.c
TITLE: navion_init.c
----------------------------------------------------------------------------
FUNCTION: Initializes navion math model
FUNCTION: Initializes navion math model
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Added to navion package 930111 by Bruce Jackson
GENEALOGY: Added to navion package 930111 by Bruce Jackson
----------------------------------------------------------------------------
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
DESIGNED BY: EBJ
CODED BY: EBJ
MAINTAINED BY: EBJ
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
MODIFICATION HISTORY:
DATE PURPOSE BY
950314 Removed initialization of state variables, since this is
now done (version 1.4b1) in ls_init. EBJ
950406 Removed #include of "shmdefs.h"; shmdefs.h is a duplicate
of "navion.h". EBJ
CURRENT RCS HEADER:
950314 Removed initialization of state variables, since this is
now done (version 1.4b1) in ls_init. EBJ
950406 Removed #include of "shmdefs.h"; shmdefs.h is a duplicate
of "navion.h". EBJ
CURRENT RCS HEADER:
----------------------------------------------------------------------------
REFERENCES:
REFERENCES:
----------------------------------------------------------------------------
CALLED BY:
CALLED BY:
----------------------------------------------------------------------------
CALLS TO:
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
OUTPUTS:
OUTPUTS:
--------------------------------------------------------------------------*/
#include "ls_types.h"

44
src/FDM/LaRCsim/on_ground.ic
View file

@ -2,28 +2,28 @@ init
0010
continuous_states: 22
# module parameter value
* generic_.geodetic_position_v[0] 2.793445E-05
* generic_.geodetic_position_v[1] 3.262070E-07
* generic_.geodetic_position_v[2] 3.758099E+00
* generic_.v_local_v[0] 7.287719E+00
* generic_.v_local_v[1] 1.521770E+03
* generic_.v_local_v[2] -1.265722E-05
* generic_.euler_angles_v[0] -2.658474E-06
* generic_.euler_angles_v[1] 7.401790E-03
* generic_.euler_angles_v[2] 1.391358E-03
* generic_.omega_body_v[0] 7.206685E-05
* generic_.omega_body_v[1] 0.000000E+00
* generic_.omega_body_v[2] 9.492658E-05
* generic_.earth_position_angle 0.000000E+00
* generic_.mass 8.547270E+01
* generic_.i_xx 1.048000E+03
* generic_.i_yy 3.000000E+03
* generic_.i_zz 3.530000E+03
* generic_.i_xz 0.000000E+00
* generic_.d_cg_rp_body_v[0] 0.000000E+00
* generic_.d_cg_rp_body_v[1] 0.000000E+00
* generic_.d_cg_rp_body_v[2] 0.000000E+00
aero long_trim 1.969572E-03
* generic_.geodetic_position_v[0] 2.793445E-05
* generic_.geodetic_position_v[1] 3.262070E-07
* generic_.geodetic_position_v[2] 3.758099E+00
* generic_.v_local_v[0] 7.287719E+00
* generic_.v_local_v[1] 1.521770E+03
* generic_.v_local_v[2] -1.265722E-05
* generic_.euler_angles_v[0] -2.658474E-06
* generic_.euler_angles_v[1] 7.401790E-03
* generic_.euler_angles_v[2] 1.391358E-03
* generic_.omega_body_v[0] 7.206685E-05
* generic_.omega_body_v[1] 0.000000E+00
* generic_.omega_body_v[2] 9.492658E-05
* generic_.earth_position_angle 0.000000E+00
* generic_.mass 8.547270E+01
* generic_.i_xx 1.048000E+03
* generic_.i_yy 3.000000E+03
* generic_.i_zz 3.530000E+03
* generic_.i_xz 0.000000E+00
* generic_.d_cg_rp_body_v[0] 0.000000E+00
* generic_.d_cg_rp_body_v[1] 0.000000E+00
* generic_.d_cg_rp_body_v[2] 0.000000E+00
aero long_trim 1.969572E-03
discrete_states: 0
# module parameter value
end

28
src/FDM/LaRCsim/uiuc_aero.c
View file

@ -1,32 +1,32 @@
/***************************************************************************
TITLE: uiuc_aero
TITLE: uiuc_aero
----------------------------------------------------------------------------
FUNCTION: aerodynamics, engine and gear model
FUNCTION: aerodynamics, engine and gear model
----------------------------------------------------------------------------
MODULE STATUS: developmental
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Equations based on Part 1 of Roskam's S&C text
GENEALOGY: Equations based on Part 1 of Roskam's S&C text
----------------------------------------------------------------------------
DESIGNED BY: Bipin Sehgal
CODED BY: Bipin Sehgal
MAINTAINED BY: Rob Deters and Glen Dimock
DESIGNED BY: Bipin Sehgal
CODED BY: Bipin Sehgal
MAINTAINED BY: Rob Deters and Glen Dimock
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
DATE PURPOSE BY
3/17/00 Initial test release
3/09/01 Added callout to UIUC gear function. (DPM)
6/18/01 Added call out to UIUC record routine (RD)
@ -45,7 +45,7 @@
----------------------------------------------------------------------------
INPUTS:
INPUTS:
----------------------------------------------------------------------------
@ -69,7 +69,7 @@ void uiuc_init_2_wrapper()
// On first time through initialize UIUC aircraft model
if (init==0) {
init=-1;
uiuc_defaults_inits();
uiuc_defaults_inits();
uiuc_init_aeromodel();
}