TurbGain = 0.0;
TurbRate = 1.0;
+ T_dev_sl = T_dev = delta_T = 0.0;
+
bind();
Debug(0);
}
}
+ T_dev = 0.0;
+ if (delta_T != 0.0) {
+ T_dev = delta_T;
+ } else {
+ if ((h < 36089.239) && (T_dev_sl != 0.0)) {
+ T_dev = T_dev_sl * ( 1.0 - (h/36089.239));
+ }
+ }
+ density_altitude = h + T_dev * 66.7;
+
+ reftemp+=T_dev;
if (slope == 0) {
intTemperature = reftemp;
intPressure = refpress*exp(-Inertial->SLgravity()/(reftemp*Reng)*(altitude-htab[i]));
&FGAtmosphere::GetSoundSpeedRatio);
PropertyManager->Tie("atmosphere/psiw-rad", this,
&FGAtmosphere::GetWindPsi);
+ PropertyManager->Tie("atmosphere/delta-T", this,
+ &FGAtmosphere::GetDeltaT, &FGAtmosphere::SetDeltaT);
+ PropertyManager->Tie("atmosphere/T-sl-dev-F", this,
+ &FGAtmosphere::GetSLTempDev, &FGAtmosphere::SetSLTempDev);
+ PropertyManager->Tie("atmosphere/density-altitude", this,
+ &FGAtmosphere::GetDensityAltitude);
PropertyManager->Tie("atmosphere/p-turb-rad_sec", this,1,
(PMF)&FGAtmosphere::GetTurbPQR);
PropertyManager->Tie("atmosphere/q-turb-rad_sec", this,2,
PropertyManager->Untie("atmosphere/rho-sl-slugs_ft3");
PropertyManager->Untie("atmosphere/P-sl-psf");
PropertyManager->Untie("atmosphere/a-sl-fps");
+ PropertyManager->Untie("atmosphere/delta-T");
+ PropertyManager->Untie("atmosphere/T-sl-dev-F");
+ PropertyManager->Untie("atmosphere/density-altitude");
PropertyManager->Untie("atmosphere/theta-norm");
PropertyManager->Untie("atmosphere/sigma-norm");
PropertyManager->Untie("atmosphere/delta-norm");
/// Provides the external atmosphere model with an interface to set the pressure.
inline void SetExPressure(double p) { exPressure=p; }
+ /// Sets the temperature deviation at sea-level in degrees Fahrenheit
+ inline void SetSLTempDev(double d) { T_dev_sl = d; }
+ /// Gets the temperature deviation at sea-level in degrees Fahrenheit
+ inline double GetSLTempDev(void) const { return T_dev_sl; }
+ /// Sets the current delta-T in degrees Fahrenheit
+ inline void SetDeltaT(double d) { delta_T = d; }
+ /// Gets the current delta-T in degrees Fahrenheit
+ inline double GetDeltaT(void) const { return delta_T; }
+ /// Gets the at-altitude temperature deviation in degrees Fahrenheit
+ inline double GetTempDev(void) const { return T_dev; }
+ /// Gets the density altitude in feet
+ inline double GetDensityAltitude(void) const { return density_altitude; }
+
/// Sets the wind components in NED frame.
inline void SetWindNED(double wN, double wE, double wD) { vWindNED(1)=wN; vWindNED(2)=wE; vWindNED(3)=wD;}
bool useExternal;
double exTemperature,exDensity,exPressure;
double intTemperature, intDensity, intPressure;
+ double T_dev_sl, T_dev, delta_T, density_altitude;
double MagnitudedAccelDt, MagnitudeAccel, Magnitude;
double TurbGain;
// Rotation
- double cTht = Propagate->GetCostht();
- double cPhi = Propagate->GetCosphi();
- double sPhi = Propagate->GetSinphi();
+ double cTht = Propagate->GetCosEuler(eTht);
+ double cPhi = Propagate->GetCosEuler(ePhi);
+ double sPhi = Propagate->GetSinEuler(ePhi);
vEulerRates(eTht) = vPQR(eQ)*cPhi - vPQR(eR)*sPhi;
if (cTht != 0.0) {
psiw = Atmosphere->GetWindPsi();
vw = Atmosphere->GetWindNED().Magnitude();
- return vw*cos(psiw - Propagate->Getpsi());
+ return vw*cos(psiw - Propagate->GetEuler(ePsi));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
psiw = Atmosphere->GetWindPsi();
vw = Atmosphere->GetWindNED().Magnitude();
- return vw*sin(psiw - Propagate->Getpsi());
+ return vw*sin(psiw - Propagate->GetEuler(ePsi));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*/
double Magnitude(void) const;
+ /** Length of the vector in a coordinate axis plane.
+
+ Compute and return the euclidean norm of this vector projected into
+ the coordinate axis plane idx1-idx2.
+ */
+ double Magnitude(int idx1, int idx2) const {
+ return sqrt( Entry(idx1)*Entry(idx1) + Entry(idx2)*Entry(idx2) );
+ }
+
/** Normalize.
Normalize the vector to have the Magnitude() == 1.0. If the vector
thrType = Cfg_ptr->GetValue();
if (thrType == "FG_PROPELLER") {
- Thruster = new FGPropeller(FDMExec, Cfg_ptr);
+ Thruster = new FGPropeller(FDMExec, Cfg_ptr, EngineNumber);
} else if (thrType == "FG_NOZZLE") {
- Thruster = new FGNozzle(FDMExec, Cfg_ptr);
+ Thruster = new FGNozzle(FDMExec, Cfg_ptr, EngineNumber);
} else if (thrType == "FG_DIRECT") {
- Thruster = new FGThruster( FDMExec, Cfg_ptr);
+ Thruster = new FGThruster( FDMExec, Cfg_ptr, EngineNumber);
}
AC_cfg->GetNextConfigLine();
MixturePos.clear();
PropAdvanceCmd.clear();
PropAdvance.clear();
+ SteerPosDeg.clear();
unsigned int i;
if (FGModel::Run()) return true; // fast exit if nothing to do
+ // Set the default engine commands
for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
+ // Set the default steering angle
+ for (i=0; i<SteerPosDeg.size(); i++) {
+ FGLGear* gear = GroundReactions->GetGearUnit(i);
+ SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() );
+ }
+
for (i=0; i<APComponents.size(); i++) APComponents[i]->Run(); // cycle AP components
for (i=0; i<FCSComponents.size(); i++) FCSComponents[i]->Run(); // cycle FCS components
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+void FGFCS::AddGear(void)
+{
+ SteerPosDeg.push_back(0.0);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGFCS::Normalize(void) {
//not all of these are guaranteed to be defined for every model
&FGFCS::GetDrCmd,
&FGFCS::SetDrCmd,
true);
+ PropertyManager->Tie("fcs/steer-cmd-norm", this,
+ &FGFCS::GetDsCmd,
+ &FGFCS::SetDsCmd,
+ true);
PropertyManager->Tie("fcs/flap-cmd-norm", this,
&FGFCS::GetDfCmd,
&FGFCS::SetDfCmd,
true );
}
}
+
+ for (i=0; i<SteerPosDeg.size(); i++) {
+ if (GroundReactions->GetGearUnit(i)->GetSteerable()) {
+ snprintf(tmp,80,"fcs/steer-pos-deg[%u]",i);
+ PropertyManager->Tie( tmp, this, i,
+ &FGFCS::GetSteerPosDeg,
+ &FGFCS::SetSteerPosDeg,
+ true );
+ }
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@return rudder command in percent */
inline double GetDrCmd(void) const { return DrCmd; }
+ /** Gets the steering command.
+ @return steering command in percent */
+ inline double GetDsCmd(void) const { return DsCmd; }
+
/** Gets the flaps command.
@return flaps command in percent */
inline double GetDfCmd(void) const { return DfCmd; }
@return mixture position for the given engine in percent ( 0 - 100)*/
inline double GetMixturePos(int engine) const { return MixturePos[engine]; }
+ /** Gets the steering position.
+ @return steering position in degrees */
+ double GetSteerPosDeg(int gear) const { return SteerPosDeg[gear]; }
+
/** Gets the gear position (0 up, 1 down), defaults to down
@return gear position (0 up, 1 down) */
inline double GetGearPos(void) const { return GearPos; }
@param cmd rudder command in percent*/
inline void SetDrCmd(double cmd) { DrCmd = cmd; }
+ /** Sets the steering command
+ @param cmd steering command in percent*/
+ inline void SetDsCmd(double cmd) { DsCmd = cmd; }
+
/** Sets the flaps command
@param cmd flaps command in percent*/
inline void SetDfCmd(double cmd) { DfCmd = cmd; }
@param cmd mixture setting in percent (0 - 100)*/
void SetMixturePos(int engine, double cmd);
+ /** Sets the steering position
+ @param cmd steering position in degrees*/
+ void SetSteerPosDeg(int gear, double pos) { SteerPosDeg[gear] = pos; }
+
/** Set the gear extend/retract position, defaults to down
@param gear position 0 up, 1 down */
void SetGearPos(double gearpos) { GearPos = gearpos; }
bool Load(FGConfigFile* AC_cfg);
void AddThrottle(void);
+ void AddGear(void);
FGPropertyManager* GetPropertyManager(void) { return PropertyManager; }
void unbind(FGPropertyManager *node);
private:
- double DaCmd, DeCmd, DrCmd, DfCmd, DsbCmd, DspCmd;
+ double DaCmd, DeCmd, DrCmd, DsCmd, DfCmd, DsbCmd, DspCmd;
double AP_DaCmd, AP_DeCmd, AP_DrCmd, AP_ThrottleCmd;
double DePos[NForms], DaLPos[NForms], DaRPos[NForms], DrPos[NForms];
double DfPos[NForms], DsbPos[NForms], DspPos[NForms];
vector <double> MixturePos;
vector <double> PropAdvanceCmd;
vector <double> PropAdvance;
+ vector <double> SteerPosDeg;
double LeftBrake, RightBrake, CenterBrake; // Brake settings
double GearCmd,GearPos;
AC_cfg->GetNextConfigLine();
while ((token = AC_cfg->GetValue()) != string("/UNDERCARRIAGE")) {
- lGear.push_back(FGLGear(AC_cfg, FDMExec));
+ int num = lGear.size();
+ lGear.push_back(FGLGear(AC_cfg, FDMExec, num));
+ FCS->AddGear();
}
return true;
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
+FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex, int number) : Exec(fdmex)
{
string tmp;
+ GearNumber = number;
+
*AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3)
>> kSpring >> bDamp>> dynamicFCoeff >> staticFCoeff
>> rollingFCoeff >> sSteerType >> sBrakeGroup
FGLGear::FGLGear(const FGLGear& lgear)
{
+ GearNumber = lgear.GearNumber;
+
State = lgear.State;
Aircraft = lgear.Aircraft;
Propagate = lgear.Propagate;
FGColumnVector3& FGLGear::Force(void)
{
double SinWheel, CosWheel;
- double deltaSlip;
double deltaT = State->Getdt()*Aircraft->GetRate();
vForce.InitMatrix();
GearDown = true;
}
+ // Compute the steering angle in any case.
+ // Will make shure that animations will look right.
+ switch (eSteerType) {
+ case stSteer:
+ SteerAngle = degtorad * FCS->GetSteerPosDeg(GearNumber);
+ break;
+ case stFixed:
+ SteerAngle = 0.0;
+ break;
+ case stCaster:
+ // Note to Jon: This is not correct for castering gear. I'll fix it later.
+ SteerAngle = 0.0;
+ break;
+ default:
+ cerr << "Improper steering type membership detected for this gear." << endl;
+ break;
+ }
+
if (GearDown) {
vWhlBodyVec = MassBalance->StructuralToBody(vXYZ);
break;
}
- switch (eSteerType) {
- case stSteer:
- SteerAngle = -maxSteerAngle * FCS->GetDrCmd() * 0.01745;
- break;
- case stFixed:
- SteerAngle = 0.0;
- break;
- case stCaster:
-// Note to Jon: This is not correct for castering gear. I'll fix it later.
- SteerAngle = 0.0;
- break;
- default:
- cerr << "Improper steering type membership detected for this gear." << endl;
- break;
- }
-
// Transform the wheel velocities from the local axis system to the wheel axis system.
// For now, steering angle is assumed to happen in the Local Z axis,
// not the strut axis as it should be. Will fix this later.
- SinWheel = sin(Propagate->Getpsi() + SteerAngle);
- CosWheel = cos(Propagate->Getpsi() + SteerAngle);
+ SinWheel = sin(Propagate->GetEuler(ePsi) + SteerAngle);
+ CosWheel = cos(Propagate->GetEuler(ePsi) + SteerAngle);
RollingWhlVel = vWhlVelVec(eX)*CosWheel + vWhlVelVec(eY)*SinWheel;
SideWhlVel = vWhlVelVec(eY)*CosWheel - vWhlVelVec(eX)*SinWheel;
WOW = false;
+ // Return to neutral position between 1.0 and 0.8 gear pos.
+ SteerAngle *= max(FCS->GetGearPos()-0.8, 0.0)/0.2;
+
if (Propagate->GetDistanceAGL() > 200.0) {
FirstContact = false;
StartedGroundRun = false;
/** Constructor
@param Executive a pointer to the parent executive object
@param File a pointer to the config file instance */
- FGLGear(FGConfigFile* File, FGFDMExec* Executive);
+ FGLGear(FGConfigFile* File, FGFDMExec* Executive, int number);
/** Constructor
@param lgear a reference to an existing FGLGear object */
FGLGear(const FGLGear& lgear);
/** Get the console touchdown reporting feature
@return true if reporting is turned on */
inline bool GetReport(void) { return ReportEnable; }
- inline double GetSteerAngle(void) { return SteerAngle;}
- inline double GetstaticFCoeff(void) { return staticFCoeff;}
+ double GetSteerNorm(void) const { return radtodeg/maxSteerAngle*SteerAngle; }
+ double GetDefaultSteerAngle(double cmd) const { return cmd*maxSteerAngle; }
+ double GetstaticFCoeff(void) { return staticFCoeff; }
inline int GetBrakeGroup(void) { return (int)eBrakeGrp; }
inline int GetSteerType(void) { return (int)eSteerType; }
+ bool GetSteerable(void) const { return eSteerType != stFixed; }
inline bool GetRetractable(void) { return isRetractable; }
inline bool GetGearUnitUp(void) { return GearUp; }
inline bool GetGearUnitDown(void) { return GearDown; }
double GetWheelVel(int axis) { return vWhlVelVec(axis);}
private:
+ int GearNumber;
FGColumnVector3 vXYZ;
FGColumnVector3 vMoment;
FGColumnVector3 vWhlBodyVec;
void FGLocation::SetLongitude(double longitude)
{
- double rtmp = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
+ double rtmp = mECLoc.Magnitude(eX, eY);
+ // Check if we have zero radius.
+ // If so set it to 1, so that we can set a position
+ if (0.0 == mECLoc.Magnitude())
+ rtmp = 1.0;
+
// Fast return if we are on the north or south pole ...
if (rtmp == 0.0)
return;
mCacheValid = false;
- mECLoc(eX) = rtmp*sin(longitude);
- mECLoc(eY) = rtmp*cos(longitude);
+ mECLoc(eX) = rtmp*cos(longitude);
+ mECLoc(eY) = rtmp*sin(longitude);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = 1.0;
}
- double rtmp = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
+ double rtmp = mECLoc.Magnitude(eX, eY);
if (rtmp != 0.0) {
double fac = r/rtmp*cos(latitude);
mECLoc(eX) *= fac;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg) : FGThruster(FDMExec)
+FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg, int num) : FGThruster(FDMExec)
{
string token;
Area2 = (Diameter*Diameter/4.0)*M_PI;
AreaT = Area2/ExpR;
+ char property_name[80];
+ snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum);
+ PropertyManager->Tie( property_name, &ThrustCoeff );
+
Debug(0);
}
FGNozzle::~FGNozzle()
{
+ char property_name[80];
+ snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum);
+ PropertyManager->Untie( property_name );
+
Debug(1);
}
Thrust = max((double)0.0, (CfPc * AreaT + (PE - pAtm)*Area2) * nzlEff);
vFn(1) = Thrust * cos(ReverserAngle);
+ ThrustCoeff = CfPc / ((pAtm - PE) * Area2);
+
return Thrust;
}
public:
/// Constructor
- FGNozzle(FGFDMExec* exec, FGConfigFile* AC_cfg);
+ FGNozzle(FGFDMExec* exec, FGConfigFile* AC_cfg, int num = 0);
/// Destructor
~FGNozzle();
socket->Clear();
socket->Append(State->Getsim_time());
socket->Append(Propagate->Geth());
- socket->Append(Propagate->Getphi());
- socket->Append(Propagate->Gettht());
- socket->Append(Propagate->Getpsi());
+ socket->Append(Propagate->GetEuler(ePhi));
+ socket->Append(Propagate->GetEuler(eTht));
+ socket->Append(Propagate->GetEuler(ePsi));
socket->Append(Atmosphere->GetDensity());
socket->Append(Auxiliary->GetVt());
socket->Append(Propagate->GetUVW(eU));
// Compute some derived values.
vVel = VState.vQtrn.GetTInv()*VState.vUVW;
+
+ // Finaly make shure that the quaternion stays normalized.
+ VState.vQtrn.Normalize();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius, &FGPropagate::SetRunwayRadius);
- PropertyManager->Tie("attitude/phi-rad", this, &FGPropagate::Getphi);
- PropertyManager->Tie("attitude/theta-rad", this, &FGPropagate::Gettht);
- PropertyManager->Tie("attitude/psi-rad", this, &FGPropagate::Getpsi);
+ PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
+ PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
+ PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
- PropertyManager->Tie("attitude/roll-rad", this, &FGPropagate::Getphi);
- PropertyManager->Tie("attitude/pitch-rad", this, &FGPropagate::Gettht);
- PropertyManager->Tie("attitude/heading-true-rad", this, &FGPropagate::Getpsi);
+ PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
+ PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
+ PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double Geth(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
double GetPQR(int axis) const {return VState.vPQR(axis);}
double GetPQRdot(int idx) const {return vPQRdot(idx);}
- double GetEuler(int axis) const { return VState.vQtrn.GetEuler()(axis); }
+ double GetEuler(int axis) const { return VState.vQtrn.GetEuler(axis); }
+ double GetCosEuler(int idx) const { return VState.vQtrn.GetCosEuler(idx); }
+ double GetSinEuler(int idx) const { return VState.vQtrn.GetSinEuler(idx); }
double Gethdot(void) const { return -vVel(eDown); }
/** Returns the "constant" RunwayRadius.
double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
const FGLocation& GetLocation(void) const { return VState.vLocation; }
- double Getphi(void) const { return VState.vQtrn.GetEulerPhi(); }
- double Gettht(void) const { return VState.vQtrn.GetEulerTheta(); }
- double Getpsi(void) const { return VState.vQtrn.GetEulerPsi(); }
-
- double GetCosphi(void) const { return VState.vQtrn.GetCosEulerPhi(); }
- double GetCostht(void) const { return VState.vQtrn.GetCosEulerTheta(); }
- double GetCospsi(void) const { return VState.vQtrn.GetCosEulerPsi(); }
-
- double GetSinphi(void) const { return VState.vQtrn.GetSinEulerPhi(); }
- double GetSintht(void) const { return VState.vQtrn.GetSinEulerTheta(); }
- double GetSinpsi(void) const { return VState.vQtrn.GetSinEulerPsi(); }
-
/** Retrieves the local-to-body transformation matrix.
@return a reference to the local-to-body transformation matrix. */
const FGMatrix33& GetTl2b(void) const { return VState.vQtrn.GetT(); }
// not just the X-axis of the engine/propeller. This may or may not work for a
// helicopter.
-FGPropeller::FGPropeller(FGFDMExec* exec, FGConfigFile* Prop_cfg) : FGThruster(exec)
+FGPropeller::FGPropeller(FGFDMExec* exec, FGConfigFile* Prop_cfg, int num) : FGThruster(exec)
{
string token;
int rows, cols;
RPM = 0;
vTorque.InitMatrix();
+ char property_name[80];
+ snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum);
+ PropertyManager->Tie( property_name, &ThrustCoeff );
+
Debug(0);
}
{
if (cThrust) delete cThrust;
if (cPower) delete cPower;
+
+ char property_name[80];
+ snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum);
+ PropertyManager->Untie( property_name );
+
Debug(1);
}
double FGPropeller::Calculate(double PowerAvailable)
{
- double J, C_Thrust, omega;
+ double J, omega;
double Vel = fdmex->GetAuxiliary()->GetAeroUVW(eU);
double rho = fdmex->GetAtmosphere()->GetDensity();
double RPS = RPM/60.0;
}
if (MaxPitch == MinPitch) { // Fixed pitch prop
- C_Thrust = cThrust->GetValue(J);
+ ThrustCoeff = cThrust->GetValue(J);
} else { // Variable pitch prop
- C_Thrust = cThrust->GetValue(J, Pitch);
+ ThrustCoeff = cThrust->GetValue(J, Pitch);
}
if (P_Factor > 0.0001) {
cerr << "P-Factor value in config file must be greater than zero" << endl;
}
- Thrust = C_Thrust*RPS*RPS*Diameter*Diameter*Diameter*Diameter*rho;
+ Thrust = ThrustCoeff*RPS*RPS*Diameter*Diameter*Diameter*Diameter*rho;
omega = RPS*2.0*M_PI;
vFn(1) = Thrust;
/** Constructor for FGPropeller.
@param exec a pointer to the main executive object
@param AC_cfg a pointer to the main aircraft config file object */
- FGPropeller(FGFDMExec* exec, FGConfigFile* AC_cfg);
+ FGPropeller(FGFDMExec* exec, FGConfigFile* AC_cfg, int num = 0);
/// Destructor for FGPropeller - deletes the FGTable objects
~FGPropeller();
angular velocities PQR.
*/
FGQuaternion FGQuaternion::GetQDot(const FGColumnVector3& PQR) const {
+ double norm = Magnitude();
+ if (norm == 0.0)
+ return FGQuaternion::zero();
+ double rnorm = 1.0/norm;
+
FGQuaternion QDot;
QDot(1) = -0.5*(Entry(2)*PQR(eP) + Entry(3)*PQR(eQ) + Entry(4)*PQR(eR));
QDot(2) = 0.5*(Entry(1)*PQR(eP) + Entry(3)*PQR(eR) - Entry(4)*PQR(eQ));
QDot(3) = 0.5*(Entry(1)*PQR(eQ) + Entry(4)*PQR(eP) - Entry(2)*PQR(eR));
QDot(4) = 0.5*(Entry(1)*PQR(eR) + Entry(2)*PQR(eQ) - Entry(3)*PQR(eP));
- return QDot;
+ return rnorm*QDot;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@param psi The euler Z axis (heading) angle in radians */
FGQuaternion(double phi, double tht, double psi);
+ /** Initializer by one euler angle.
+ Initialize the quaternion with the single euler angle where its index
+ is given in the first argument.
+ @param idx Index of the euler angle to initialize
+ @param angle The euler angle in radians */
+ FGQuaternion(int idx, double angle)
+ : mCacheValid(false) {
+ double angle2 = 0.5*angle;
+
+ double Sangle2 = sin(angle2);
+ double Cangle2 = cos(angle2);
+
+ if (idx == ePhi) {
+ Entry(1) = Cangle2;
+ Entry(2) = Sangle2;
+ Entry(3) = 0.0;
+ Entry(4) = 0.0;
+
+ } else if (idx == eTht) {
+ Entry(1) = Cangle2;
+ Entry(2) = 0.0;
+ Entry(3) = Sangle2;
+ Entry(4) = 0.0;
+
+ } else {
+ Entry(1) = Cangle2;
+ Entry(2) = 0.0;
+ Entry(3) = 0.0;
+ Entry(4) = Sangle2;
+
+ }
+ }
+
/// Destructor.
~FGQuaternion() {}
/** Transformation matrix.
@return a reference to the transformation/rotation matrix
corresponding to this quaternion rotation. */
- const FGMatrix33& GetT() const { ComputeDerived(); return mT; }
+ const FGMatrix33& GetT(void) const { ComputeDerived(); return mT; }
/** Backward transformation matrix.
@return a reference to the inverse transformation/rotation matrix
corresponding to this quaternion rotation. */
- const FGMatrix33& GetTInv() const { ComputeDerived(); return mTInv; }
+ const FGMatrix33& GetTInv(void) const { ComputeDerived(); return mTInv; }
/** Retrieves the Euler angles.
@return a reference to the triad of euler angles corresponding
to this quaternion rotation.
@units radians */
- const FGColumnVector3& GetEuler() const {
+ const FGColumnVector3& GetEuler(void) const {
ComputeDerived();
return mEulerAngles;
}
- /** Euler angle theta.
- @return the euler angle theta (pitch attitude) corresponding to this
- quaternion rotation.
- @units radians */
- double GetEulerTheta() const {
- ComputeDerived();
- return mEulerAngles(eTht);
- }
-
- /** Euler angle theta.
- @return the euler angle theta (pitch attitude) corresponding to
- this quaternion rotation.
- @units degrees */
- double GetEulerThetaDeg() const {
- ComputeDerived();
- return radtodeg*mEulerAngles(eTht);
- }
-
- /** Euler angle psi.
- @return the heading euler angle (psi) corresponding to this quaternion
- rotation.
- @units radians */
- double GetEulerPsi() const {
- ComputeDerived();
- return mEulerAngles(ePsi);
- }
-
- /** Retrieves the heading angle.
- @return the Euler angle psi (heading) corresponding to this quaternion
- rotation.
- @units degrees */
- double GetEulerPsiDeg() const {
- ComputeDerived();
- return radtodeg*mEulerAngles(ePsi);
- }
-
- /** Retrieves the roll angle.
- @return the euler angle phi (roll) corresponding to this quaternion
- rotation.
- @units radians */
- double GetEulerPhi() const {
+ /** Retrieves the Euler angles.
+ @param i the euler angle index.
+ @return a reference to the i-th euler angles corresponding
+ to this quaternion rotation.
+ @units radians */
+ double GetEuler(int i) const {
ComputeDerived();
- return mEulerAngles(ePhi);
+ return mEulerAngles(i);
}
- /** Retrieves the roll angle.
- Returns the Euler angle phi (roll) corresponding to this quaternion rotation.
- @units degrees */
- double GetEulerPhiDeg() const {
+ /** Retrieves the Euler angles.
+ @param i the euler angle index.
+ @return a reference to the i-th euler angles corresponding
+ to this quaternion rotation.
+ @units degrees */
+ double GetEulerDeg(int i) const {
ComputeDerived();
- return radtodeg*mEulerAngles(ePhi);
+ return radtodeg*mEulerAngles(i);
}
- /** Retrieves sine theta.
+ /** Retrieves sine of the given euler angle.
@return the sine of the Euler angle theta (pitch attitude) corresponding
to this quaternion rotation. */
- double GetSinEulerTheta() const {
- ComputeDerived();
- return mEulerSines(eTht);
- }
-
- /** Retrieves sine psi.
- @return the sine of the Euler angle psi (heading) corresponding to this
- quaternion rotation. */
- double GetSinEulerPsi() const {
- ComputeDerived();
- return mEulerSines(ePsi);
- }
-
- /** Sine of euler angle phi.
- @return the sine of the Euler angle phi (roll) corresponding to this
- quaternion rotation. */
- double GetSinEulerPhi() const {
- ComputeDerived();
- return mEulerSines(ePhi);
- }
-
- /** Cosine of euler angle theta.
- @return the cosine of the Euler angle theta (pitch) corresponding to this
- quaternion rotation. */
- double GetCosEulerTheta() const {
+ double GetSinEuler(int i) const {
ComputeDerived();
- return mEulerCosines(eTht);
+ return mEulerSines(i);
}
- /** Cosine of euler angle psi.
- @return the cosine of the Euler angle psi (heading) corresponding to this
- quaternion rotation. */
- double GetCosEulerPsi() const {
- ComputeDerived();
- return mEulerCosines(ePsi);
- }
-
- /** Cosine of euler angle phi.
- @return the cosine of the Euler angle phi (roll) corresponding to this
- quaternion rotation. */
- double GetCosEulerPhi() const {
+ /** Retrieves cosine of the given euler angle.
+ @return the sine of the Euler angle theta (pitch attitude) corresponding
+ to this quaternion rotation. */
+ double GetCosEuler(int i) const {
ComputeDerived();
- return mEulerCosines(ePhi);
+ return mEulerCosines(i);
}
/** Read access the entries of the vector.
-
+
@param idx the component index.
-
+
Return the value of the matrix entry at the given index.
Indices are counted starting with 1.
-
+
Note that the index given in the argument is unchecked.
*/
double operator()(unsigned int idx) const { return Entry(idx); }
/** Write access the entries of the vector.
-
+
@param idx the component index.
-
+
Return a reference to the vector entry at the given index.
Indices are counted starting with 1.
-
+
Note that the index given in the argument is unchecked.
*/
double& operator()(unsigned int idx) { return Entry(idx); }
/** Read access the entries of the vector.
-
+
@param idx the component index.
-
+
Return the value of the matrix entry at the given index.
Indices are counted starting with 1.
-
+
This function is just a shortcut for the @ref double
operator()(unsigned int idx) const function. It is
used internally to access the elements in a more convenient way.
-
+
Note that the index given in the argument is unchecked.
*/
double Entry(unsigned int idx) const { return mData[idx-1]; }
/** Write access the entries of the vector.
-
+
@param idx the component index.
-
+
Return a reference to the vector entry at the given index.
Indices are counted starting with 1.
-
+
This function is just a shortcut for the @ref double&
operator()(unsigned int idx) function. It is
used internally to access the elements in a more convenient way.
-
+
Note that the index given in the argument is unchecked.
*/
double& Entry(unsigned int idx) { mCacheValid = false; return mData[idx-1]; }
return *this;
}
+ /** Inverse of the quaternion.
+
+ Compute and return the inverse of the quaternion so that the orientation
+ represented with *this multiplied with the returned value is equal to
+ the identity orientation.
+ */
+ FGQuaternion Inverse(void) const {
+ double norm = Magnitude();
+ if (norm == 0.0)
+ return *this;
+ double rNorm = 1.0/norm;
+ return FGQuaternion( Entry(1)*rNorm, -Entry(2)*rNorm,
+ -Entry(3)*rNorm, -Entry(4)*rNorm );
+ }
+
+ /** Conjugate of the quaternion.
+
+ Compute and return the conjugate of the quaternion. This one is equal
+ to the inverse iff the quaternion is normalized.
+ */
+ FGQuaternion Conjugate(void) const {
+ return FGQuaternion( Entry(1), -Entry(2), -Entry(3), -Entry(4) );
+ }
+
friend FGQuaternion operator*(double, const FGQuaternion&);
-
+
/** Length of the vector.
-
+
Compute and return the euclidean norm of this vector.
*/
- double Magnitude() const { return sqrt(SqrMagnitude()); }
+ double Magnitude(void) const { return sqrt(SqrMagnitude()); }
/** Square of the length of the vector.
-
+
Compute and return the square of the euclidean norm of this vector.
*/
- double SqrMagnitude() const {
+ double SqrMagnitude(void) const {
return Entry(1)*Entry(1)+Entry(2)*Entry(2)
+Entry(3)*Entry(3)+Entry(4)*Entry(4);
}
/** Normialze.
-
+
Normalize the vector to have the Magnitude() == 1.0. If the vector
is equal to zero it is left untouched.
*/
- void Normalize();
+ void Normalize(void);
/** Zero quaternion vector. Does not represent any orientation.
Useful for initialization of increments */
/** Computation of derived values.
This function checks if the derived values like euler angles and
transformation matrices are already computed. If so, it
- returns. If they need to be computed this is done here. */
+ returns. If they need to be computed the real worker routine
+ \ref FGQuaternion::ComputeDerivedUnconditional(void) const
+ is called.
+ This function is inlined to avoid function calls in the fast path. */
void ComputeDerived(void) const {
if (!mCacheValid)
ComputeDerivedUnconditional();
Type = ttDirect;
SetTransformType(FGForce::tCustom);
+ EngineNum = 0;
+ PropertyManager = FDMExec->GetPropertyManager();
+
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGThruster::FGThruster(FGFDMExec *FDMExec,
- FGConfigFile *Eng_cfg ): FGForce(FDMExec)
+ FGConfigFile *Eng_cfg, int num ): FGForce(FDMExec)
{
Type = ttDirect;
SetTransformType(FGForce::tCustom);
Name = Eng_cfg->GetValue();
GearRatio = 1.0;
+
+ EngineNum = num;
+ ThrustCoeff = 0.0;
+ PropertyManager = FDMExec->GetPropertyManager();
+
+ char property_name[80];
+ snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum);
+ PropertyManager->Tie( property_name, &ThrustCoeff );
+
Debug(0);
}
FGThruster::~FGThruster()
{
+ char property_name[80];
+ snprintf(property_name, 80, "propulsion/c-thrust[%u]", EngineNum);
+ PropertyManager->Untie( property_name );
+
Debug(1);
}
#include "FGForce.h"
#include "FGConfigFile.h"
+#include "FGPropertyManager.h"
#include <string>
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
public:
/// Constructor
FGThruster(FGFDMExec *FDMExec);
- FGThruster(FGFDMExec *FDMExec, FGConfigFile *Eng_cfg );
+ FGThruster(FGFDMExec *FDMExec, FGConfigFile *Eng_cfg, int num );
/// Destructor
virtual ~FGThruster();
enum eType {ttNozzle, ttRotor, ttPropeller, ttDirect};
- virtual double Calculate(double tt) { Thrust = tt; vFn(1) = Thrust; return 0.0; }
+ virtual double Calculate(double tt) {
+ Thrust = tt; vFn(1) = Thrust;
+ return 0.0;
+ }
void SetName(string name) {Name = name;}
virtual void SetRPM(double rpm) {};
virtual double GetPowerRequired(void) {return 0.0;}
double PowerRequired;
double deltaT;
double GearRatio;
+ double ThrustCoeff;
+ int EngineNum;
+ FGPropertyManager* PropertyManager;
virtual void Debug(int from);
};
}
solver_eps=tolerance/100;
break;
case tTheta:
- control_min=fdmex->GetPropagate()->Gettht() - 5*degtorad;
- control_max=fdmex->GetPropagate()->Gettht() + 5*degtorad;
+ control_min=fdmex->GetPropagate()->GetEuler(eTht) - 5*degtorad;
+ control_max=fdmex->GetPropagate()->GetEuler(eTht) + 5*degtorad;
state_convert=radtodeg;
break;
case tPhi:
- control_min=fdmex->GetPropagate()->Getphi() - 30*degtorad;
- control_max=fdmex->GetPropagate()->Getphi() + 30*degtorad;
+ control_min=fdmex->GetPropagate()->GetEuler(ePhi) - 30*degtorad;
+ control_max=fdmex->GetPropagate()->GetEuler(ePhi) + 30*degtorad;
state_convert=radtodeg;
control_convert=radtodeg;
break;
control_convert=radtodeg;
break;
case tHeading:
- control_min=fdmex->GetPropagate()->Getpsi() - 30*degtorad;
- control_max=fdmex->GetPropagate()->Getpsi() + 30*degtorad;
+ control_min=fdmex->GetPropagate()->GetEuler(ePsi) - 30*degtorad;
+ control_max=fdmex->GetPropagate()->GetEuler(ePsi) + 30*degtorad;
state_convert=radtodeg;
break;
}
case tYawTrim:
case tRudder: control_value=fdmex->GetFCS() -> GetDrCmd(); break;
case tAltAGL: control_value=fdmex->GetPropagate()->GetDistanceAGL();break;
- case tTheta: control_value=fdmex->GetPropagate()->Gettht(); break;
- case tPhi: control_value=fdmex->GetPropagate()->Getphi(); break;
+ case tTheta: control_value=fdmex->GetPropagate()->GetEuler(eTht); break;
+ case tPhi: control_value=fdmex->GetPropagate()->GetEuler(ePhi); break;
case tGamma: control_value=fdmex->GetAuxiliary()->GetGamma();break;
- case tHeading: control_value=fdmex->GetPropagate()->Getpsi(); break;
+ case tHeading: control_value=fdmex->GetPropagate()->GetEuler(ePsi); break;
}
}
double FGTrimAxis::computeHmgt(void) {
double diff;
- diff = fdmex->GetPropagate()->Getpsi() -
+ diff = fdmex->GetPropagate()->GetEuler(ePsi) -
fdmex->GetAuxiliary()->GetGroundTrack();
if( diff < -M_PI ) {
}
cout << "SetThetaOnGround ref gear: " << ref << endl;
if(ref >= 0) {
- double sp = fdmex->GetPropagate()->GetSinphi();
- double cp = fdmex->GetPropagate()->GetCosphi();
+ double sp = fdmex->GetPropagate()->GetSinEuler(ePhi);
+ double cp = fdmex->GetPropagate()->GetCosEuler(ePhi);
double lx = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(1);
double ly = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(2);
double lz = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(3);
}
//cout << i << endl;
if (debug_lvl > 0) {
- cout << " Initial Theta: " << fdmex->GetPropagate()->Gettht()*radtodeg << endl;
+ cout << " Initial Theta: " << fdmex->GetPropagate()->GetEuler(eTht)*radtodeg << endl;
cout << " Used gear unit " << iAft << " as aft and " << iForward << " as forward" << endl;
}
control_min=(theta+5)*degtorad;
i++;
}
if (ref >= 0) {
- double st = sin(fdmex->GetPropagate()->Gettht());
- double ct = cos(fdmex->GetPropagate()->Gettht());
+ double st = fdmex->GetPropagate()->GetSinEuler(eTht);
+ double ct = fdmex->GetPropagate()->GetCosEuler(eTht);
double lx = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(1);
double ly = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(2);
double lz = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(3);
void FGTrimAxis::AxisReport(void) {
char out[80];
- sprintf(out," %20s: %6.2f %5s: %9.2e Tolerance: %3.0e\n",
+
+ sprintf(out," %20s: %6.2f %5s: %9.2e Tolerance: %3.0e",
GetControlName().c_str(), GetControl()*control_convert,
GetStateName().c_str(), GetState()+state_target, GetTolerance());
cout << out;
+ if( abs(GetState()+state_target) < abs(GetTolerance()) )
+ cout << " Passed" << endl;
+ else
+ cout << " Failed" << endl;
}
/*****************************************************************************/
ThrustTables.back()->Load(Eng_cfg);
}
else cerr << "Unhandled token in Engine config file: " << token << endl;
+ if (token == "EOF") return false;
}
// Pre-calculations and initializations
stall_warning->setDoubleValue(0);
SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: "
- << Propagate->Getphi()*RADTODEG << " deg" );
+ << Propagate->GetEuler(ePhi)*RADTODEG << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: "
- << Propagate->Gettht()*RADTODEG << " deg" );
+ << Propagate->GetEuler(eTht)*RADTODEG << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: "
- << Propagate->Getpsi()*RADTODEG << " deg" );
+ << Propagate->GetEuler(ePsi)*RADTODEG << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: "
<< Propagate->GetLocation().GetLatitudeDeg() << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: "
FCS->SetPitchTrimCmd( globals->get_controls()->get_elevator_trim() );
FCS->SetDrCmd( -globals->get_controls()->get_rudder() );
FCS->SetYawTrimCmd( -globals->get_controls()->get_rudder_trim() );
- FCS->SetDfCmd( globals->get_controls()->get_flaps() );
+ // FIXME: make that get_steering work
+// FCS->SetDsCmd( globals->get_controls()->get_steering()/80.0 );
+ FCS->SetDsCmd( globals->get_controls()->get_rudder() );
+ FCS->SetDfCmd( globals->get_controls()->get_flaps() );
FCS->SetDsbCmd( globals->get_controls()->get_speedbrake() );
FCS->SetDspCmd( globals->get_controls()->get_spoilers() );
_set_Altitude_AGL( Propagate->GetDistanceAGL() );
- _set_Euler_Angles( Propagate->Getphi(),
- Propagate->Gettht(),
- Propagate->Getpsi() );
+ _set_Euler_Angles( Propagate->GetEuler(ePhi),
+ Propagate->GetEuler(eTht),
+ Propagate->GetEuler(ePsi) );
_set_Alpha( Auxiliary->Getalpha() );
_set_Beta( Auxiliary->Getbeta() );
FGGroundReactions* gr=fdmex->GetGroundReactions();
int Ngear=GroundReactions->GetNumGearUnits();
for (int i=0;i<Ngear;i++) {
+ FGLGear *gear = gr->GetGearUnit(i);
SGPropertyNode * node = fgGetNode("gear/gear", i, true);
- node->setDoubleValue("xoffset-in",
- gr->GetGearUnit(i)->GetBodyLocation()(1));
- node->setDoubleValue("yoffset-in",
- gr->GetGearUnit(i)->GetBodyLocation()(2));
- node->setDoubleValue("zoffset-in",
- gr->GetGearUnit(i)->GetBodyLocation()(3));
- node->setBoolValue("wow", gr->GetGearUnit(i)->GetWOW());
- node->setBoolValue("has-brake", gr->GetGearUnit(i)->GetBrakeGroup() > 0);
+ node->setDoubleValue("xoffset-in", gear->GetBodyLocation()(1));
+ node->setDoubleValue("yoffset-in", gear->GetBodyLocation()(2));
+ node->setDoubleValue("zoffset-in", gear->GetBodyLocation()(3));
+ node->setBoolValue("wow", gear->GetWOW());
+ node->setBoolValue("has-brake", gear->GetBrakeGroup() > 0);
node->setDoubleValue("position-norm", FCS->GetGearPos());
- node->setDoubleValue("tire-pressure-norm", gr->GetGearUnit(i)->GetTirePressure());
+ node->setDoubleValue("tire-pressure-norm", gear->GetTirePressure());
+ if ( gear->GetSteerable() )
+ node->setDoubleValue("steering-norm", gear->GetSteerNorm());
}
}
FGGroundReactions* gr=fdmex->GetGroundReactions();
int Ngear=GroundReactions->GetNumGearUnits();
for (int i=0;i<Ngear;i++) {
+ FGLGear *gear = gr->GetGearUnit(i);
SGPropertyNode * node = fgGetNode("gear/gear", i, true);
- node->getChild("wow", 0, true)->setBoolValue(gr->GetGearUnit(i)->GetWOW());
+ node->getChild("wow", 0, true)->setBoolValue( gear->GetWOW());
node->getChild("position-norm", 0, true)->setDoubleValue(FCS->GetGearPos());
- gr->GetGearUnit(i)->SetTirePressure(node->getDoubleValue("tire-pressure-norm"));
+ gear->SetTirePressure(node->getDoubleValue("tire-pressure-norm"));
+ if ( gear->GetSteerable() )
+ node->setDoubleValue("steering-norm", gear->GetSteerNorm());
}
}
projects / flightgear.git / commitdiff