All '\t' have been replaced with 8 spaces, as most of the code is indented with spaces.
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.
-
- 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.
-
- 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).
-
- 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
+ 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.
+
+ 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.
+
+ 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.
+
+ 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
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,
+ .......
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;
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
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.
// 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;
// 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.
// 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'
}
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();
// 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
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
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
{
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;
//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;
}
// 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
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;
// 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
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
//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
// 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';
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
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
//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
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
// 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); }
#endif
#include <math.h>
-#include <string.h> // strcmp()
+#include <string.h> // strcmp()
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
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);
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 );
-
- // 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"));
-
- // 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");
-
- 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();
-
- Flap_handle = 30.0 * globals->get_controls()->get_flaps();
+ * 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();
+
+ // 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());
+
+ 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);
+ }
+
+ // 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();
}
// 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 );
-
- // 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"));
-
- // 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");
-
- 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();
-
- Flap_handle = 30.0 * globals->get_controls()->get_flaps();
+ * 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();
+
+ // 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());
+
+ 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);
+ }
+
+ // 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();
}
// done with basic-larcsim if-block
// 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
// 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);
}
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));
}
}
// 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 {
// but lets restore our original bogus altitude when we are done
if ( save_alt < -9000.0 ) {
- set_Altitude( save_alt );
+ set_Altitude( save_alt );
}
}
// 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 );
_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; }
// 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 );
// 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);
}
}
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) {
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();
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();
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();
//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);
/* 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();
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" );
}
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;
}
};
/***************************************************************************
- 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
----------------------------------------------------------------------------
#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.
#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
// printf("altitude = %.2f\n", altitude);
if ( altitude < 0.0 ) {
- altitude = 0.0;
+ altitude = 0.0;
}
// printf("altitude = %.2f\n", altitude);
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;
void ls_atmos( SCALAR altitude, SCALAR * sigma, SCALAR * v_sound,
- SCALAR * t_amb, SCALAR * p_amb );
+ SCALAR * t_amb, SCALAR * p_amb );
#ifdef __cplusplus
}
/***************************************************************************
- 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
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
+
----------------------------------------------------------------------------
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
--------------------------------------------------------------------------*/
-
+
#include "ls_generic.h"
#include "ls_cockpit.h"
#include "ls_constants.h"
#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_;
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 ) {
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};
/* 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;
-
- 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;
-
- 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;
-
-
-
- MaxTakeoffWeight=2550;
- EmptyWeight=1500;
+ 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? */
+
+ 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;
+
+ Cybeta=-0.31;
+ Cyp=-0.037;
+ Cyr=0.21;
+ Cyda=0.0;
+ Cydr=0.187;
+
+
+
+ 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*/
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;
/* 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 */
*/
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*/
/* 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); */
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY: ls_model();
+ CALLED BY: ls_model();
----------------------------------------------------------------------------
- CALLS TO: none
+ CALLS TO: none
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
#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;
-
- F_X_engine = Throttle[3]*(pa*550)/v;
+ /* 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;
} else {
- /* accept external settings */
+ /* accept external settings */
}
/* printf("F_X_engine = %.3f\n", F_X_engine); */
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
*/
- 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
*/
*
* 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
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); */
-
- if (reaction_normal_force > 0.) reaction_normal_force = 0.;
- /* to prevent damping component from swamping spring component */
-
-
- /* 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];
- }
-
- /* 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);
+ 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 */
+
+
+ /* 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];
+ }
+
+ /* 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);
*/
- /* 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); */
}
/***************************************************************************
- 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"
void do_trims(int kmax,FILE *out,InitialConditions IC)
{
- int bad_trim=0,i,j;
- double speed,elevator,cmcl,maxspeed;
- out=fopen("trims.out","w");
- speed=55;
-
- for(j=0;j<=0;j+=10)
- {
- IC.flap_handle=j;
- for(i=4;i<=4;i++)
- {
- switch(i)
- {
- case 1: IC.weight=1500;IC.cg=0.155;break;
- case 2: IC.weight=1500;IC.cg=0.364;break;
- case 3: IC.weight=1950;IC.cg=0.155;break;
- case 4: IC.weight=2400;IC.cg=0.257;break;
- case 5: IC.weight=2550;IC.cg=0.364;break;
- }
-
- speed=40;
- if(j > 0) { maxspeed = 90; }
- else { maxspeed = 170; }
- while(speed <= maxspeed)
- {
- IC.vc=speed;
- Long_control=0;Theta=0;Throttle_pct=0.0;
-
- bad_trim=trim_long(kmax,IC);
- if(Long_control <= 0)
- elevator=Long_control*28;
- else
- elevator=Long_control*23;
- if(fabs(CL) > 1E-3)
- {
- cmcl=cm / CL;
- }
- if(!bad_trim)
- {
- fprintf(out,"%g,%g,%g,%g,%g",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position);
- fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
- /* printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg);
- */ }
- else
- {
- printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_true_kts,Weight,Cg,Flap_Position);
- printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
- printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
- }
- speed+=10;
- }
- }
- }
- fclose(out);
+ int bad_trim=0,i,j;
+ double speed,elevator,cmcl,maxspeed;
+ out=fopen("trims.out","w");
+ speed=55;
+
+ for(j=0;j<=0;j+=10)
+ {
+ IC.flap_handle=j;
+ for(i=4;i<=4;i++)
+ {
+ switch(i)
+ {
+ case 1: IC.weight=1500;IC.cg=0.155;break;
+ case 2: IC.weight=1500;IC.cg=0.364;break;
+ case 3: IC.weight=1950;IC.cg=0.155;break;
+ case 4: IC.weight=2400;IC.cg=0.257;break;
+ case 5: IC.weight=2550;IC.cg=0.364;break;
+ }
+
+ speed=40;
+ if(j > 0) { maxspeed = 90; }
+ else { maxspeed = 170; }
+ while(speed <= maxspeed)
+ {
+ IC.vc=speed;
+ Long_control=0;Theta=0;Throttle_pct=0.0;
+
+ bad_trim=trim_long(kmax,IC);
+ if(Long_control <= 0)
+ elevator=Long_control*28;
+ else
+ elevator=Long_control*23;
+ if(fabs(CL) > 1E-3)
+ {
+ cmcl=cm / CL;
+ }
+ if(!bad_trim)
+ {
+ fprintf(out,"%g,%g,%g,%g,%g",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position);
+ fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
+ /* printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg);
+ */ }
+ else
+ {
+ printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_true_kts,Weight,Cg,Flap_Position);
+ printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
+ printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
+ }
+ speed+=10;
+ }
+ }
+ }
+ fclose(out);
}
find_max_alt(int kmax,InitialConditions IC)
{
- int bad_trim=0,i=0;
- float min=0,max=30000;
- IC.use_gamma_tmg=1;
- IC.gamma=0;
- IC.vc=73;
- IC.altitude==1000;
- while(!bad_trim)
- {
- bad_trim=trim_long(200,IC);
- IC.altitude+=1000;
- }
- while((fabs(max-min) > 100) && (i < 50))
- {
-
- IC.altitude=(max-min)/2 + min;
- printf("\nIC.altitude: %g, max: %g, min: %g, bad_trim: %d\n",IC.altitude,max,min,bad_trim);
- printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
-
- bad_trim=trim_long(200,IC);
-
- if(bad_trim == 1 )
- max=IC.altitude;
- else
- min=IC.altitude;
- i++;
- }
-}
-
+ int bad_trim=0,i=0;
+ float min=0,max=30000;
+ IC.use_gamma_tmg=1;
+ IC.gamma=0;
+ IC.vc=73;
+ IC.altitude==1000;
+ while(!bad_trim)
+ {
+ bad_trim=trim_long(200,IC);
+ IC.altitude+=1000;
+ }
+ while((fabs(max-min) > 100) && (i < 50))
+ {
+
+ IC.altitude=(max-min)/2 + min;
+ printf("\nIC.altitude: %g, max: %g, min: %g, bad_trim: %d\n",IC.altitude,max,min,bad_trim);
+ printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
+
+ bad_trim=trim_long(200,IC);
+
+ if(bad_trim == 1 )
+ max=IC.altitude;
+ else
+ min=IC.altitude;
+ i++;
+ }
+}
+
void find_trim_stall(int kmax,FILE *out,InitialConditions IC)
{
- int k=0,i,j;
- int failf;
- char axis[10];
- double speed,elevator,cmcl,speed_inc,lastgood;
- out=fopen("trim_stall.summary","w");
- speed=90;
- speed_inc=10;
- //failf=malloc(sizeof(int));
-
- for(j=0;j<=30;j+=10)
- {
- IC.flap_handle=j;
- for(i=1;i<=6;i++)
- {
- switch(i)
- {
- case 1: IC.weight=1500;IC.cg=0.155;break;
- case 2: IC.weight=1500;IC.cg=0.364;break;
- case 3: IC.weight=2400;IC.cg=0.155;break;
- case 4: IC.weight=2400;IC.cg=0.364;break;
- case 5: IC.weight=2550;IC.cg=0.257;break;
- case 6: IC.weight=2550;IC.cg=0.364;break;
- }
-
- speed=90;
- speed_inc=10;
- while(speed_inc >= 0.5)
- {
- IC.vc=speed;
- Long_control=0;Theta=0;Throttle_pct=0.0;
- failf=trim_longfr(kmax,IC);
- if(Long_control <= 0)
- elevator=Long_control*28;
- else
- elevator=Long_control*23;
- if(fabs(CL) > 1E-3)
- {
- cmcl=cm / CL;
- }
- if(failf == 0)
- {
- lastgood=speed;
- axis[0]='\0';
- //fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position,k);
- //fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
- /* printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg);
- */ }
- else
- {
- printf("trim failed at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_kts,Weight,Cg,Flap_Position);
- printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
- printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
- printf("Speed increment: %g\n",speed_inc);
- speed+=speed_inc;
- speed_inc/=2;
- }
- speed-=speed_inc;
-
-
- }
- printf("failf %d\n",failf);
- if(failf == 1)
- strcpy(axis,"lift");
- else if(failf == 2)
- strcpy(axis,"thrust");
- else if(failf == 3)
- strcpy(axis,"pitch");
- fprintf(out,"Last good speed: %g, Flaps: %g, Weight: %g, CG: %g, failed axis: %s\n",lastgood,Flap_handle,Weight,Cg,axis);
-
-
- }
- }
- fclose(out);
- //free(failf);
-}
+ int k=0,i,j;
+ int failf;
+ char axis[10];
+ double speed,elevator,cmcl,speed_inc,lastgood;
+ out=fopen("trim_stall.summary","w");
+ speed=90;
+ speed_inc=10;
+ //failf=malloc(sizeof(int));
+
+ for(j=0;j<=30;j+=10)
+ {
+ IC.flap_handle=j;
+ for(i=1;i<=6;i++)
+ {
+ switch(i)
+ {
+ case 1: IC.weight=1500;IC.cg=0.155;break;
+ case 2: IC.weight=1500;IC.cg=0.364;break;
+ case 3: IC.weight=2400;IC.cg=0.155;break;
+ case 4: IC.weight=2400;IC.cg=0.364;break;
+ case 5: IC.weight=2550;IC.cg=0.257;break;
+ case 6: IC.weight=2550;IC.cg=0.364;break;
+ }
+
+ speed=90;
+ speed_inc=10;
+ while(speed_inc >= 0.5)
+ {
+ IC.vc=speed;
+ Long_control=0;Theta=0;Throttle_pct=0.0;
+ failf=trim_longfr(kmax,IC);
+ if(Long_control <= 0)
+ elevator=Long_control*28;
+ else
+ elevator=Long_control*23;
+ if(fabs(CL) > 1E-3)
+ {
+ cmcl=cm / CL;
+ }
+ if(failf == 0)
+ {
+ lastgood=speed;
+ axis[0]='\0';
+ //fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position,k);
+ //fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
+ /* printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg);
+ */ }
+ else
+ {
+ printf("trim failed at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_kts,Weight,Cg,Flap_Position);
+ printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
+ printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
+ printf("Speed increment: %g\n",speed_inc);
+ speed+=speed_inc;
+ speed_inc/=2;
+ }
+ speed-=speed_inc;
+
+
+ }
+ printf("failf %d\n",failf);
+ if(failf == 1)
+ strcpy(axis,"lift");
+ else if(failf == 2)
+ strcpy(axis,"thrust");
+ else if(failf == 3)
+ strcpy(axis,"pitch");
+ fprintf(out,"Last good speed: %g, Flaps: %g, Weight: %g, CG: %g, failed axis: %s\n",lastgood,Flap_handle,Weight,Cg,axis);
+
+
+ }
+ }
+ fclose(out);
+ //free(failf);
+}
// Initialize the LaRCsim flight model, dt is the time increment for
}
int wave_stats(float *var,float *var_rate,int N,FILE *out)
-{
- int Nc,i,Nmaxima;
- float varmax,slope,intercept,time,ld,zeta,omegad,omegan;
- float varmaxima[100],vm_times[100];
- /*adjust N so that any constant slope region at the end is cut off */
- i=N;
- while((fabs(var_rate[N]-var_rate[i]) < 0.1) && (i >= 0))
- {
+{
+ int Nc,i,Nmaxima;
+ float varmax,slope,intercept,time,ld,zeta,omegad,omegan;
+ float varmaxima[100],vm_times[100];
+ /*adjust N so that any constant slope region at the end is cut off */
+ i=N;
+ while((fabs(var_rate[N]-var_rate[i]) < 0.1) && (i >= 0))
+ {
i--;
- }
- Nc=N-i;
- slope=(var[N]-var[Nc])/(N*0.01 - Nc*0.01);
- intercept=var[N]-slope*N*0.01;
- printf("\tRotating constant decay out of data using:\n");
- printf("\tslope: %g, intercept: %g\n",slope,intercept);
- printf("\tUsing first %d points for dynamic response analysis\n",Nc);
- varmax=0;
- Nmaxima=0;i=0;
- while((i <= Nc) && (i <= 801))
- {
-
- fprintf(out,"%g\t%g",i*0.01,var[i]);
- var[i]-=slope*i*0.01+intercept;
- /* printf("%g\n",var[i]); */
+ }
+ Nc=N-i;
+ slope=(var[N]-var[Nc])/(N*0.01 - Nc*0.01);
+ intercept=var[N]-slope*N*0.01;
+ printf("\tRotating constant decay out of data using:\n");
+ printf("\tslope: %g, intercept: %g\n",slope,intercept);
+ printf("\tUsing first %d points for dynamic response analysis\n",Nc);
+ varmax=0;
+ Nmaxima=0;i=0;
+ while((i <= Nc) && (i <= 801))
+ {
+
+ fprintf(out,"%g\t%g",i*0.01,var[i]);
+ var[i]-=slope*i*0.01+intercept;
+ /* printf("%g\n",var[i]); */
fprintf(out,"\t%g\n",var[i]);
- if(var[i] > varmax)
- {
- varmax=var[i];
- time=i*0.01;
-
- }
- if((var[i-1]*var[i] < 0) && (var[i] > 0))
- {
- varmaxima[Nmaxima]=varmax;
- vm_times[Nmaxima]=time;
- printf("\t%6.2f: %8.4f\n",vm_times[Nmaxima],varmaxima[Nmaxima]);
- varmax=0;Nmaxima++;
-
- }
-
- i++;
- }
- varmaxima[Nmaxima]=varmax;
+ if(var[i] > varmax)
+ {
+ varmax=var[i];
+ time=i*0.01;
+
+ }
+ if((var[i-1]*var[i] < 0) && (var[i] > 0))
+ {
+ varmaxima[Nmaxima]=varmax;
+ vm_times[Nmaxima]=time;
+ printf("\t%6.2f: %8.4f\n",vm_times[Nmaxima],varmaxima[Nmaxima]);
+ varmax=0;Nmaxima++;
+
+ }
+
+ i++;
+ }
+ varmaxima[Nmaxima]=varmax;
vm_times[Nmaxima]=time;
Nmaxima++;
- if(Nmaxima > 2)
- {
- ld=log(varmaxima[1]/varmaxima[2]); //logarithmic decrement
- zeta=ld/sqrt(4*LS_PI*LS_PI +ld*ld); //damping ratio
- omegad=1/(vm_times[2]-vm_times[1]); //damped natural frequency Hz
- if(zeta < 1)
- {
- omegan=omegad/sqrt(1-zeta*zeta); //natural frequency Hz
- }
- printf("\tDamping Ratio: %g\n",zeta);
- printf("\tDamped Freqency: %g Hz\n\tNatural Freqency: %g Hz\n",omegad,omegan);
- }
- else
- printf("\tNot enough points to take log decrement\n");
-/* printf("w: %g, u: %g, q: %g\n",W_body,U_body,Q_body);
+ if(Nmaxima > 2)
+ {
+ ld=log(varmaxima[1]/varmaxima[2]); //logarithmic decrement
+ zeta=ld/sqrt(4*LS_PI*LS_PI +ld*ld); //damping ratio
+ omegad=1/(vm_times[2]-vm_times[1]); //damped natural frequency Hz
+ if(zeta < 1)
+ {
+ omegan=omegad/sqrt(1-zeta*zeta); //natural frequency Hz
+ }
+ printf("\tDamping Ratio: %g\n",zeta);
+ printf("\tDamped Freqency: %g Hz\n\tNatural Freqency: %g Hz\n",omegad,omegan);
+ }
+ else
+ printf("\tNot enough points to take log decrement\n");
+/* printf("w: %g, u: %g, q: %g\n",W_body,U_body,Q_body);
*/
- return 1;
-}
+ return 1;
+}
// Run an iteration of the EOM (equations of motion)
int main(int argc, char *argv[]) {
-
- double save_alt = 0.0;
+
+ double save_alt = 0.0;
int multiloop=1,k=0,i,j,touchdown,N;
- double time=0,elev_trim,elev_trim_save,elevator,speed,cmcl;
- FILE *out;
- double hgain,hdiffgain,herr,herrprev,herr_diff,htarget;
- double lastVt,vtdots,vtdott;
- InitialConditions IC;
+ double time=0,elev_trim,elev_trim_save,elevator,speed,cmcl;
+ FILE *out;
+ double hgain,hdiffgain,herr,herrprev,herr_diff,htarget;
+ double lastVt,vtdots,vtdott;
+ InitialConditions IC;
SCALAR *control[7];
- SCALAR *state[7];
- float old_state,effectiveness,tol,delta_state,lctrim;
- float newcm,lastcm,cmalpha,td_vspeed,td_time,stop_time;
- float h[801],hdot[801],altmin,lastAlt,theta[800],theta_dot[800];
-
+ SCALAR *state[7];
+ float old_state,effectiveness,tol,delta_state,lctrim;
+ float newcm,lastcm,cmalpha,td_vspeed,td_time,stop_time;
+ float h[801],hdot[801],altmin,lastAlt,theta[800],theta_dot[800];
+
if(argc < 6)
- {
- printf("Need args: $c172 speed alt alpha elev throttle\n");
- exit(1);
- }
- initIC(&IC);
-
- IC.latitude=47.5299892; //BFI
- IC.longitude=122.3019561;
- Runway_altitude = 18.0;
-
- IC.altitude=strtod(argv[2],NULL);
- printf("h: %g, argv[2]: %s\n",IC.altitude,argv[2]);
- IC.vc=strtod(argv[1],NULL);
- IC.alpha=0;
- IC.beta=0;
- IC.theta=strtod(argv[3],NULL);
- IC.use_gamma_tmg=0;
- IC.phi=0;
- IC.psi=0;
- IC.weight=2400;
- IC.cg=0.25;
- IC.flap_handle=10;
- IC.long_control=0;
- IC.rudder_pedal=0;
+ {
+ printf("Need args: $c172 speed alt alpha elev throttle\n");
+ exit(1);
+ }
+ initIC(&IC);
+
+ IC.latitude=47.5299892; //BFI
+ IC.longitude=122.3019561;
+ Runway_altitude = 18.0;
+
+ IC.altitude=strtod(argv[2],NULL);
+ printf("h: %g, argv[2]: %s\n",IC.altitude,argv[2]);
+ IC.vc=strtod(argv[1],NULL);
+ IC.alpha=0;
+ IC.beta=0;
+ IC.theta=strtod(argv[3],NULL);
+ IC.use_gamma_tmg=0;
+ IC.phi=0;
+ IC.psi=0;
+ IC.weight=2400;
+ IC.cg=0.25;
+ IC.flap_handle=10;
+ IC.long_control=0;
+ IC.rudder_pedal=0;
-
- ls_ForceAltitude(IC.altitude);
+
+ ls_ForceAltitude(IC.altitude);
fgLaRCsimInit(0.01);
- setIC(IC);
- printf("Dx_cg: %g\n",Dx_cg);
- V_down=strtod(argv[4],NULL);;
- ls_loop(0,-1);
- i=0;time=0;
- IC.long_control=0;
- altmin=Altitude;
+ setIC(IC);
+ printf("Dx_cg: %g\n",Dx_cg);
+ V_down=strtod(argv[4],NULL);;
+ ls_loop(0,-1);
+ i=0;time=0;
+ IC.long_control=0;
+ altmin=Altitude;
printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",Altitude,Theta*RAD_TO_DEG,V_down);
- while(time < 5.0)
- {
- printf("Time: %g, Flap_handle: %g, Flap_position: %g, Transit: %d\n",time,Flap_handle,Flap_Position,Flaps_In_Transit);
- if(time > 2.5)
- Flap_handle=20;
- else if (time > 0.5)
- Flap_handle=20;
- ls_update(1);
- time+=0.01;
+ while(time < 5.0)
+ {
+ printf("Time: %g, Flap_handle: %g, Flap_position: %g, Transit: %d\n",time,Flap_handle,Flap_Position,Flaps_In_Transit);
+ if(time > 2.5)
+ Flap_handle=20;
+ else if (time > 0.5)
+ Flap_handle=20;
+ ls_update(1);
+ time+=0.01;
}
-
-
-
- /*out=fopen("drop.out","w");
- N=800;touchdown=0;
-
- while(i <= N)
- {
- ls_update(1);
- printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",D_cg_above_rwy,Theta*RAD_TO_DEG,V_down);
- fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
- h[i]=D_cg_above_rwy;hdot[i]=V_down;
- theta[i]=Theta; theta_dot[i]=Theta_dot;
- if(D_cg_above_rwy < altmin)
- altmin=D_cg_above_rwy;
- if((F_Z_gear < -10) && (! touchdown))
- {
- touchdown=1;
- td_vspeed=V_down;
- td_time=time;
- }
- time+=0.01;
- i++;
- }
- while(V_rel_ground > 1)
- {
- if(Brake_pct < 1)
- {
- Brake_pct+=0.02;
- }
- ls_update(1);
- time=i*0.01;
- fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
- i++;
+
+
+
+ /*out=fopen("drop.out","w");
+ N=800;touchdown=0;
+
+ while(i <= N)
+ {
+ ls_update(1);
+ printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",D_cg_above_rwy,Theta*RAD_TO_DEG,V_down);
+ fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
+ h[i]=D_cg_above_rwy;hdot[i]=V_down;
+ theta[i]=Theta; theta_dot[i]=Theta_dot;
+ if(D_cg_above_rwy < altmin)
+ altmin=D_cg_above_rwy;
+ if((F_Z_gear < -10) && (! touchdown))
+ {
+ touchdown=1;
+ td_vspeed=V_down;
+ td_time=time;
+ }
+ time+=0.01;
+ i++;
+ }
+ while(V_rel_ground > 1)
+ {
+ if(Brake_pct < 1)
+ {
+ Brake_pct+=0.02;
+ }
+ ls_update(1);
+ time=i*0.01;
+ fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
+ i++;
}
- stop_time=time;
+ stop_time=time;
while((time-stop_time) < 5.0)
- {
- ls_update(1);
- time=i*0.01;
- fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
- i++;
- }
- fclose(out);
-
- printf("Min Altitude: %g, Final Alitutde: %g, Delta: %g\n",altmin, h[N], D_cg_above_rwy-altmin);
- printf("Vertical Speed at touchdown: %g, Time at touchdown: %g\n",td_vspeed,td_time);
+ {
+ ls_update(1);
+ time=i*0.01;
+ fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
+ i++;
+ }
+ fclose(out);
+
+ printf("Min Altitude: %g, Final Alitutde: %g, Delta: %g\n",altmin, h[N], D_cg_above_rwy-altmin);
+ printf("Vertical Speed at touchdown: %g, Time at touchdown: %g\n",td_vspeed,td_time);
printf("\nAltitude response:\n");
- out=fopen("alt.out","w");
- wave_stats(h,hdot,N,out);
- fclose(out);
- out=fopen("theta.out","w");
- printf("\nPitch Attitude response:\n");
- wave_stats(theta,theta_dot,N,out);
+ out=fopen("alt.out","w");
+ wave_stats(h,hdot,N,out);
+ fclose(out);
+ out=fopen("theta.out","w");
+ printf("\nPitch Attitude response:\n");
+ wave_stats(theta,theta_dot,N,out);
fclose(out);*/
- /*printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position);
- printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);
- printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
+ /*printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position);
+ printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);
+ printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control);
- printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde);
- */
-
-
-
-
-
-
-
- /* ls_loop(0.0,-1);
-
- control[1]=&IC.long_control;
- control[2]=&IC.throttle;
- control[3]=&IC.alpha;
- control[4]=&IC.beta;
- control[5]=&IC.phi;
- control[6]=&IC.lat_control;
-
- state[1]=&Q_dot_body;state[2]=&U_dot_body;state[3]=&W_dot_body;
- state[4]=&R_dot_body;state[5]=&V_dot_body;state[6]=&P_dot_body;
-
-
- for(i=1;i<=6;i++)
- {
- old_state=*state[i];
- tol=1E-4;
- for(j=1;j<=6;j++)
- {
- *control[j]+=0.1;
- setIC(IC);
- ls_loop(0.0,-1);
- delta_state=*state[i]-old_state;
- effectiveness=(delta_state)/ 0.1;
- if(delta_state < tol)
- effectiveness = 0;
- printf("%8.4f,",delta_state);
- *control[j]-=0.1;
-
- }
- printf("\n");
- setIC(IC);
- ls_loop(0.0,-1);
- } */
-
- return 1;
+ printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde);
+ */
+
+
+
+
+
+
+
+ /* ls_loop(0.0,-1);
+
+ control[1]=&IC.long_control;
+ control[2]=&IC.throttle;
+ control[3]=&IC.alpha;
+ control[4]=&IC.beta;
+ control[5]=&IC.phi;
+ control[6]=&IC.lat_control;
+
+ state[1]=&Q_dot_body;state[2]=&U_dot_body;state[3]=&W_dot_body;
+ state[4]=&R_dot_body;state[5]=&V_dot_body;state[6]=&P_dot_body;
+
+
+ for(i=1;i<=6;i++)
+ {
+ old_state=*state[i];
+ tol=1E-4;
+ for(j=1;j<=6;j++)
+ {
+ *control[j]+=0.1;
+ setIC(IC);
+ ls_loop(0.0,-1);
+ delta_state=*state[i]-old_state;
+ effectiveness=(delta_state)/ 0.1;
+ if(delta_state < tol)
+ effectiveness = 0;
+ printf("%8.4f,",delta_state);
+ *control[j]-=0.1;
+
+ }
+ printf("\n");
+ setIC(IC);
+ ls_loop(0.0,-1);
+ } */
+
+ return 1;
}
/*
void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC)
{
-
- SCALAR htarget,hgain,hdiffgain,herr,herr_diff,herrprev;
- SCALAR theta_trim,elev_trim,time;
- int k;
- k=trim_long(kmax,IC);
- printf("Trim:\n\tAlpha: %10.6f, elev: %10.6f, Throttle: %10.6f\n\twdot: %10.6f, qdot: %10.6f, udot: %10.6f\n",Alpha*RAD_TO_DEG,Long_control,Throttle_pct,W_dot_body,U_dot_body,Q_dot_body);
-
-
- htarget=0;
-
- hgain=1;
- hdiffgain=1;
- elev_trim=Long_control;
- out=fopen("stick_pull.out","w");
- herr=Q_body-htarget;
-
- //fly steady-level for 2 seconds, well, zero pitch rate anyway
- while(time < 2.0)
- {
- herrprev=herr;
- ls_update(1);
- herr=Q_body-htarget;
- herr_diff=herr-herrprev;
- Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
- time+=0.01;
- //printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi);
+
+ SCALAR htarget,hgain,hdiffgain,herr,herr_diff,herrprev;
+ SCALAR theta_trim,elev_trim,time;
+ int k;
+ k=trim_long(kmax,IC);
+ printf("Trim:\n\tAlpha: %10.6f, elev: %10.6f, Throttle: %10.6f\n\twdot: %10.6f, qdot: %10.6f, udot: %10.6f\n",Alpha*RAD_TO_DEG,Long_control,Throttle_pct,W_dot_body,U_dot_body,Q_dot_body);
+
+
+ htarget=0;
+
+ hgain=1;
+ hdiffgain=1;
+ elev_trim=Long_control;
+ out=fopen("stick_pull.out","w");
+ herr=Q_body-htarget;
+
+ //fly steady-level for 2 seconds, well, zero pitch rate anyway
+ while(time < 2.0)
+ {
+ herrprev=herr;
+ ls_update(1);
+ herr=Q_body-htarget;
+ herr_diff=herr-herrprev;
+ Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
+ time+=0.01;
+ //printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi);
//printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
- }
-
- //begin untrimmed climb at theta_trim + 2 degrees
- hgain=4;
- hdiffgain=2;
- theta_trim=Theta;
- htarget=theta_trim;
- herr=Theta-htarget;
- while(time < tmax)
- {
- //ramp in the target theta
- if(htarget < (theta_trim + 2*DEG_TO_RAD))
- {
- htarget+= 0.01*DEG_TO_RAD;
- }
- herrprev=herr;
- ls_update(1);
- herr=Theta-htarget;
- herr_diff=herr-herrprev;
- Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
- time+=0.01;
- //printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
+ }
+
+ //begin untrimmed climb at theta_trim + 2 degrees
+ hgain=4;
+ hdiffgain=2;
+ theta_trim=Theta;
+ htarget=theta_trim;
+ herr=Theta-htarget;
+ while(time < tmax)
+ {
+ //ramp in the target theta
+ if(htarget < (theta_trim + 2*DEG_TO_RAD))
+ {
+ htarget+= 0.01*DEG_TO_RAD;
+ }
+ herrprev=herr;
+ ls_update(1);
+ herr=Theta-htarget;
+ herr_diff=herr-herrprev;
+ Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
+ time+=0.01;
+ //printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi);
//printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
- }
- printf("%g,%g\n",theta_trim*RAD_TO_DEG,htarget*RAD_TO_DEG);
- fclose(out);
-}
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
+ }
+ printf("%g,%g\n",theta_trim*RAD_TO_DEG,htarget*RAD_TO_DEG);
+ fclose(out);
+}
void do_takeoff(FILE *out)
{
- SCALAR htarget,hgain,hdiffgain,elev_trim,elev_trim_save,herr;
- SCALAR time,herrprev,herr_diff;
-
- htarget=0;
-
- hgain=1;
- hdiffgain=1;
- elev_trim=Long_control;
- elev_trim_save=elev_trim;
-
-
- out=fopen("takeoff.out","w");
- herr=Q_body-htarget;
-
- // attempt to maintain zero pitch rate during the roll
- while((V_calibrated_kts < 61) && (time < 30.0))
- {
- // herrprev=herr
- ls_update(1);
- // herr=Q_body-htarget;
- // herr_diff=herr-herrprev;
- // Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
- time+=0.01;
- printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,U_body,W_body);
-// printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
-// fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
-
- }
- //At Vr, ramp in 10% nose up elevator in 0.5 seconds
- elev_trim_save=0;
- printf("At Vr, rotate...\n");
- while((Q_body < 3.0*RAD_TO_DEG) && (time < 30.0))
- {
- Long_control-=0.01;
- ls_update(1);
- printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, cm: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,cm,U_body,W_body);
-
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
- time +=0.01;
-
- }
- //Maintain 15 degrees theta for the climbout
- htarget=15*DEG_TO_RAD;
- herr=Theta-htarget;
- hgain=10;
- hdiffgain=1;
- elev_trim=Long_control;
- while(time < 30.0)
- {
- herrprev=herr;
- ls_update(1);
- herr=Theta-htarget;
- herr_diff=herr-herrprev;
- Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
- time+=0.01;
- printf("Time: %7.4f, Alt: %7.4f, Speed: %7.4f, Theta: %7.4f\n",time,Altitude,V_calibrated_kts,Theta*RAD_TO_DEG);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
- fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
- }
- fclose(out);
- printf("Speed: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, udot: %7.4f\n",V_true_kts,Altitude,Alpha*RAD_TO_DEG,Long_control,Q_body*RAD_TO_DEG,U_dot_body);
- printf("F_down_total: %7.4f, F_Z_aero: %7.4f, F_X: %7.4f, M_m_cg: %7.4f\n\n",F_down+Mass*Gravity,F_Z_aero,F_X,M_m_cg);
+ SCALAR htarget,hgain,hdiffgain,elev_trim,elev_trim_save,herr;
+ SCALAR time,herrprev,herr_diff;
+
+ htarget=0;
+
+ hgain=1;
+ hdiffgain=1;
+ elev_trim=Long_control;
+ elev_trim_save=elev_trim;
+
+
+ out=fopen("takeoff.out","w");
+ herr=Q_body-htarget;
+
+ // attempt to maintain zero pitch rate during the roll
+ while((V_calibrated_kts < 61) && (time < 30.0))
+ {
+ // herrprev=herr
+ ls_update(1);
+ // herr=Q_body-htarget;
+ // herr_diff=herr-herrprev;
+ // Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
+ time+=0.01;
+ printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,U_body,W_body);
+// printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
+// fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
+
+ }
+ //At Vr, ramp in 10% nose up elevator in 0.5 seconds
+ elev_trim_save=0;
+ printf("At Vr, rotate...\n");
+ while((Q_body < 3.0*RAD_TO_DEG) && (time < 30.0))
+ {
+ Long_control-=0.01;
+ ls_update(1);
+ printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, cm: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,cm,U_body,W_body);
+
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
+ time +=0.01;
+
+ }
+ //Maintain 15 degrees theta for the climbout
+ htarget=15*DEG_TO_RAD;
+ herr=Theta-htarget;
+ hgain=10;
+ hdiffgain=1;
+ elev_trim=Long_control;
+ while(time < 30.0)
+ {
+ herrprev=herr;
+ ls_update(1);
+ herr=Theta-htarget;
+ herr_diff=herr-herrprev;
+ Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
+ time+=0.01;
+ printf("Time: %7.4f, Alt: %7.4f, Speed: %7.4f, Theta: %7.4f\n",time,Altitude,V_calibrated_kts,Theta*RAD_TO_DEG);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
+ fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
+ }
+ fclose(out);
+ printf("Speed: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, udot: %7.4f\n",V_true_kts,Altitude,Alpha*RAD_TO_DEG,Long_control,Q_body*RAD_TO_DEG,U_dot_body);
+ printf("F_down_total: %7.4f, F_Z_aero: %7.4f, F_X: %7.4f, M_m_cg: %7.4f\n\n",F_down+Mass*Gravity,F_Z_aero,F_X,M_m_cg);
/***************************************************************************
- TITLE: Cherokee_aero
-
+ TITLE: Cherokee_aero
+
----------------------------------------------------------------------------
- FUNCTION: Linear aerodynamics model
+ FUNCTION: Linear aerodynamics model
----------------------------------------------------------------------------
- MODULE STATUS: developmental
+ MODULE STATUS: developmental
----------------------------------------------------------------------------
----------------------------------------------------------------------------
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.
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),
-
- Cx0 = -0.4645/(157.5*0.3048*0.3048),
-
- 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
+ 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),
+
+ 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
/*
- 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;
}
- 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.
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;
-
- float n, // rps
- H,
- J, // advance ratio (ratio of horizontal speed to speed of propeller's tip)
- eta, // iskoristivost elise
- T,
- V,
- P;
+ 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;
/* 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;
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
#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
*/
*
* 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
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 );
-
- /* 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;
-
- /* 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 */
-
- 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 );
-
+ /*========================================*/
+ /* 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 */
+
+ 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 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 );
+
}
}
/***************************************************************************
- 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"
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY: ls_model();
+ CALLED BY: ls_model();
----------------------------------------------------------------------------
- CALLS TO: none
+ CALLS TO: none
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
#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() {} */
/***************************************************************************
- 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
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
----------------------------------------------------------------------------
- OUTPUTS: State derivatives
+ OUTPUTS: State derivatives
-------------------------------------------------------------------------*/
#include "ls_types.h"
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 */
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;
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;
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 */
}
/***************************************************************************
- 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:
*
* 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
----------------------------------------------------------------------------
- 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"
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 */
-
- 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);
-
-
- /* 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));
- }
+ /* 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_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 */
+
+ 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 );
-
- 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 );
-
+ 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 );
+
/* call function for (smoothed) density ratio, sonic velocity, and
- 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;
+ 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;
/* 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;
-
- 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;
-
+ 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;
+
/* 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 */
}
/***************************************************************************
- 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$
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
/***************************************************************************
- 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
-
- [ 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
+ 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
--------------------------------------------------------------------------*/
#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
#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
/***************************************************************************
- 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
-
- [ 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
+ 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
--------------------------------------------------------------------------*/
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]
-
- 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]
-
+
+ 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]
+
/*================================ 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]
-
- 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_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]
+#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_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_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
-
- 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]
+#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_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 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 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 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 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 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]
-
- 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_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]
+#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_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]
} GENERIC;
-extern GENERIC generic_; /* usually defined in ls_main.c */
+extern GENERIC generic_; /* usually defined in ls_main.c */
#ifdef __cplusplus
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- 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.
--------------------------------------------------------------------------*/
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;
-
- 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));
- }
+ 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));
+ }
}
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 );
#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
}
/***************************************************************************
- 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
*
* 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"
#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);
}
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
// static char rcsid[] = "$Id$";
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;
/* 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 ) {
/* int error; */
if (Number_of_Continuous_States == 0)
- {
- Number_of_Continuous_States = HARDWIRED;
-
- 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");
- }
+ {
+ Number_of_Continuous_States = HARDWIRED;
+
+ 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");
+ }
/* 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;
+ }
*/
}
/* 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) {
while( !abrt && (eob > bufptr))
{
- 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;
- }
- }
-
- }
+ 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;
+ }
+ }
+
+ }
}
Symbols_loaded = !abrt;
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;
}
/* 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 );
*/
}
/***************************************************************************
- 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
-
- 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.
+ 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.
$Header$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
/* 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
/* global variables */
char *progname;
-char *fullname;
+char *fullname;
/* file variables - default simulation settings */
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;
}
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;
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[];
{
/* 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 */
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 */
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); */
/* 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);
/* 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;
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- 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:
--------------------------------------------------------------------------*/
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;
/* 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;
/* 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 */
/* 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 */
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)
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");
/* 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();
-
- 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) */;
- }
+ 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) */;
+ }
nr_free_matrix( mat1, 1, n, 1, n );
nr_free_matrix( mat2, 1, n, 1, n );
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- 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>
/***************************************************************************
- 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
----------------------------------------------------------------------------
- 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>
/***************************************************************************
- 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$
* 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.
*
-
+
--------------------------------------------------------------------------*/
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;
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- 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
--------------------------------------------------------------------------*/
/* #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;
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;
-
- // 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 */
-
- ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
-
-/* Initialize vehicle model */
-
- ls_aux();
- ls_model(0.0, 0);
-
-/* Calculate initial accelerations */
-
- ls_accel();
-
+ 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();
+ }
+
+/* Calculate local gravitation acceleration */
+
+ ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
+
+/* Initialize vehicle model */
+
+ ls_aux();
+ ls_model(0.0, 0);
+
+/* Calculate initial accelerations */
+
+ 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 */
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 */
/* 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 */
}
/*************************************************************************/
-
+
#define _LS_STEP_H
-void ls_step( SCALAR dt, int Initialize );
+void ls_step( SCALAR dt, int Initialize );
#endif /* _LS_STEP_H */
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
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 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. */
-
-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. */
+extern void ls_set_double(vartype sym_type, void *addr, double value );
+
+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. */
+
+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. */
#endif /* _LS_SYM_H */
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
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;
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;
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;
+ }
}
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()
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()
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()
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()
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;
}
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 */
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 );
}
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 */
-
- /* 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;
-
- 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 );
+ 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 */
+
+ 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++;
+ }
+ }
+
+ nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls );
}
if (!Trimmed) fprintf(stderr, "Trim unsuccessful.\n");
while( !abrt && (eob > bufptr))
{
- 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;
- }
- }
-
- }
+ 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;
+ }
+ }
+
+ }
}
if ((!abrt) &&
- (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_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;
- }
- }
+ (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_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;
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;
}
/***************************************************************************
- 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
/***************************************************************************
- 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
----------------------------------------------------------------------------
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
-
- 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)
+ 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)
----------------------------------------------------------------------------
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));
}
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY: ls_model();
+ CALLED BY: ls_model();
----------------------------------------------------------------------------
- CALLS TO: none
+ CALLS TO: none
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
#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) { */
/***************************************************************************
- 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$
----------------------------------------------------------------------------
- REFERENCES:
+ REFERENCES:
----------------------------------------------------------------------------
- CALLED BY:
+ CALLED BY:
----------------------------------------------------------------------------
- CALLS TO:
+ CALLS TO:
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
- OUTPUTS:
+ OUTPUTS:
--------------------------------------------------------------------------*/
#include <math.h>
#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
*/
*
* 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
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 );
-
- /* 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;
-
- /* 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 */
-
- 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 );
-
+ /*========================================*/
+ /* 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 */
+
+ 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 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 );
+
}
}
/***************************************************************************
- 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"
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
/***************************************************************************
- 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)
----------------------------------------------------------------------------
- INPUTS:
+ INPUTS:
----------------------------------------------------------------------------
// On first time through initialize UIUC aircraft model
if (init==0) {
init=-1;
- uiuc_defaults_inits();
+ uiuc_defaults_inits();
uiuc_init_aeromodel();
}
projects / flightgear.git / commitdiff