/*******************************************************************************
-
+
Header: FGInitialCondition.cpp
Author: Tony Peden
Date started: 7/1/99
-
+
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
-
+
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
-
+
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
-
+
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
-
+
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
-
-
+
+
HISTORY
--------------------------------------------------------------------------------
7/1/99 TP Created
-
-
+
+
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
-
+
The purpose of this class is to take a set of initial conditions and provide
a kinematically consistent set of body axis velocity components, euler
angles, and altitude. This class does not attempt to trim the model i.e.
the sim will most likely start in a very dynamic state (unless, of course,
you have chosen your IC's wisely) even after setting it up with this class.
-
+
CAVEAT: This class makes use of alpha=theta-gamma. This means that setting
any of the three with this class is only valid for steady state
(all accels zero) and zero pitch rate. One example where this
would produce invalid results is setting up for a trim in a pull-up
or pushover (both have nonzero pitch rate). Maybe someday...
-
+
********************************************************************************
INCLUDES
*******************************************************************************/
#include "FGOutput.h"
#include "FGDefs.h"
-
-
-
-
-FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec)
-{
- vt=vc=0;
- mach=0;
- alpha=beta=gamma=0;
- theta=phi=psi=0;
- altitude=hdot=0;
- latitude=longitude=0;
+FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) {
+ vt=vc=ve=0;
+ mach=0;
+ alpha=beta=gamma=0;
+ theta=phi=psi=0;
+ altitude=hdot=0;
+ latitude=longitude=0;
+ u=v=w=0;
+ lastSpeedSet=setvt;
+ if(FDMExec != NULL ) {
fdmex=FDMExec;
fdmex->GetPosition()->Seth(altitude);
fdmex->GetAtmosphere()->Run();
+ } else {
+ cout << "FGInitialCondition: This class requires a pointer to an valid FGFDMExec object" << endl;
+ }
}
-FGInitialCondition::~FGInitialCondition(void) {};
-
-
-void FGInitialCondition::SetVcalibratedKtsIC(float tt)
-{
- vc=tt*KTSTOFPS;
- if(getMachFromVcas(&mach,vc)) {
- vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
- }
-}
-
-
+FGInitialCondition::~FGInitialCondition(void) {}
-void FGInitialCondition::SetVtrueKtsIC(float tt)
-{
- vt=tt*KTSTOFPS;
- mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
- vc=calcVcas(mach);
-}
+void FGInitialCondition::SetVcalibratedKtsIC(float tt) {
-void FGInitialCondition::SetMachIC(float tt)
-{
- mach=tt;
+ if(getMachFromVcas(&mach,tt*KTSTOFPS)) {
+ //cout << "Mach: " << mach << endl;
+ lastSpeedSet=setvc;
+ vc=tt*KTSTOFPS;
vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
- vc=calcVcas(mach);
+ ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
//cout << "Vt: " << vt*FPSTOKTS << " Vc: " << vc*FPSTOKTS << endl;
+ }
+ else {
+ cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl;
+ cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl;
+ }
}
-
-
-void FGInitialCondition::SetAltitudeFtIC(float tt)
-{
- altitude=tt;
- fdmex->GetPosition()->Seth(altitude);
- fdmex->GetAtmosphere()->Run();
- mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
- vc=calcVcas(mach);
+void FGInitialCondition::SetVequivalentKtsIC(float tt) {
+ ve=tt*KTSTOFPS;
+ lastSpeedSet=setve;
+ vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
+ mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
+ vc=calcVcas(mach);
}
-
-void FGInitialCondition::SetFlightPathAngleDegIC(float tt)
-{
- gamma=tt*DEGTORAD;
- theta=alpha+gamma;
+void FGInitialCondition::SetVtrueKtsIC(float tt) {
+ vt=tt*KTSTOFPS;
+ lastSpeedSet=setvt;
+ mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed();
+ vc=calcVcas(mach);
+ ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
}
-
-void FGInitialCondition::SetAlphaDegIC(float tt)
-{
- alpha=tt*DEGTORAD;
- theta=alpha+gamma;
+void FGInitialCondition::SetMachIC(float tt) {
+ mach=tt;
+ lastSpeedSet=setmach;
+ vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed();
+ vc=calcVcas(mach);
+ ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio());
+ //cout << "Vt: " << vt*FPSTOKTS << " Vc: " << vc*FPSTOKTS << endl;
}
-void FGInitialCondition::SetBetaDegIC(float tt)
-{
- beta=tt*DEGTORAD;
-}
+void FGInitialCondition::SetClimbRateFpmIC(float tt) {
-void FGInitialCondition::SetRollAngleDegIC(float tt)
-{
- phi=tt*DEGTORAD;
+ if(vt > 0.1) {
+ hdot=tt/60;
+ gamma=asin(hdot/vt);
+ }
}
-
-void FGInitialCondition::SetPitchAngleDegIC(float tt)
-{
- theta=tt*DEGTORAD;
- alpha=theta-gamma;
+void FGInitialCondition::SetUBodyFpsIC(float tt) {
+ u=tt;
+ vt=sqrt(u*u+v*v+w*w);
+ lastSpeedSet=setvt;
}
-
-void FGInitialCondition::SetHeadingDegIC(float tt)
-{
- psi=tt*DEGTORAD;
+void FGInitialCondition::SetVBodyFpsIC(float tt) {
+ v=tt;
+ vt=sqrt(u*u+v*v+w*w);
+ lastSpeedSet=setvt;
}
-
-void FGInitialCondition::SetLatitudeDegIC(float tt)
-{
- latitude=tt*DEGTORAD;
+void FGInitialCondition::SetWBodyFpsIC(float tt) {
+ w=tt;
+ vt=sqrt(u*u+v*v+w*w);
+ lastSpeedSet=setvt;
}
-void FGInitialCondition::SetLongitudeDegIC(float tt)
-{
- longitude=tt*DEGTORAD;
-}
-
-
-float FGInitialCondition::GetUBodyFpsIC(void)
-{
- return vt*cos(alpha)*cos(beta);
-}
-
-
-float FGInitialCondition::GetVBodyFpsIC(void)
-{
- return vt*sin(beta);
-}
-
-
-float FGInitialCondition::GetWBodyFpsIC(void)
-{
- return vt*sin(alpha)*cos(beta);
-}
-
-
-float FGInitialCondition::GetThetaRadIC(void)
-{
- return theta;
-}
-
-
-float FGInitialCondition::GetPhiRadIC(void)
-{
- return phi;
-}
-
-
-float FGInitialCondition::GetPsiRadIC(void)
-{
- return psi;
-}
-
-
-float FGInitialCondition::GetLatitudeRadIC(void)
-{
- return latitude;
-}
-
-
-float FGInitialCondition::GetLongitudeRadIC(void)
-{
- return longitude;
-}
-
-
-float FGInitialCondition::GetAltitudeFtIC(void)
-{
- return altitude;
+void FGInitialCondition::SetAltitudeFtIC(float tt) {
+ altitude=tt;
+ fdmex->GetPosition()->Seth(altitude);
+ fdmex->GetAtmosphere()->Run();
+
+ //lets try to make sure the user gets what they intended
+
+ switch(lastSpeedSet) {
+ case setvt:
+ SetVtrueKtsIC(vt*FPSTOKTS);
+ break;
+ case setvc:
+ SetVcalibratedKtsIC(vc*FPSTOKTS);
+ break;
+ case setve:
+ SetVequivalentKtsIC(ve*FPSTOKTS);
+ break;
+ case setmach:
+ SetMachIC(mach);
+ break;
+ }
}
float FGInitialCondition::calcVcas(float Mach) {
- float p=fdmex->GetAtmosphere()->GetPressure();
- float psl=fdmex->GetAtmosphere()->GetPressureSL();
- float rhosl=fdmex->GetAtmosphere()->GetDensitySL();
- float pt,A,B,D,vcas;
-
- if(Mach < 1) //calculate total pressure assuming isentropic flow
- pt=p*pow((1 + 0.2*Mach*Mach),3.5);
- else
- {
- // shock in front of pitot tube, we'll assume its normal and use
- // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
- // pressure behind the shock to the static pressure in front
-
-
- //the normal shock assumption should not be a bad one -- most supersonic
- //aircraft place the pitot probe out front so that it is the forward
- //most point on the aircraft. The real shock would, of course, take
- //on something like the shape of a rounded-off cone but, here again,
- //the assumption should be good since the opening of the pitot probe
- //is very small and, therefore, the effects of the shock curvature
- //should be small as well. AFAIK, this approach is fairly well accepted
- //within the aerospace community
-
- B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
-
- // The denominator above is zero for Mach ~ 0.38, for which
- // we'll never be here, so we're safe
-
- D = (2.8*Mach*Mach-0.4)*0.4167;
- pt = p*pow(B,3.5)*D;
- }
-
- A = pow(((pt-p)/psl+1),0.28571);
- vcas = sqrt(7*psl/rhosl*(A-1));
- //cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << p << endl;
- return vcas;
+ float p=fdmex->GetAtmosphere()->GetPressure();
+ float psl=fdmex->GetAtmosphere()->GetPressureSL();
+ float rhosl=fdmex->GetAtmosphere()->GetDensitySL();
+ float pt,A,B,D,vcas;
+
+ if(Mach < 1) //calculate total pressure assuming isentropic flow
+ pt=p*pow((1 + 0.2*Mach*Mach),3.5);
+ else {
+ // shock in front of pitot tube, we'll assume its normal and use
+ // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
+ // pressure behind the shock to the static pressure in front
+
+
+ //the normal shock assumption should not be a bad one -- most supersonic
+ //aircraft place the pitot probe out front so that it is the forward
+ //most point on the aircraft. The real shock would, of course, take
+ //on something like the shape of a rounded-off cone but, here again,
+ //the assumption should be good since the opening of the pitot probe
+ //is very small and, therefore, the effects of the shock curvature
+ //should be small as well. AFAIK, this approach is fairly well accepted
+ //within the aerospace community
+
+ B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8);
+
+ // The denominator above is zero for Mach ~ 0.38, for which
+ // we'll never be here, so we're safe
+
+ D = (2.8*Mach*Mach-0.4)*0.4167;
+ pt = p*pow(B,3.5)*D;
+ }
+
+ A = pow(((pt-p)/psl+1),0.28571);
+ vcas = sqrt(7*psl/rhosl*(A-1));
+ //cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << pt << endl;
+ return vcas;
}
bool FGInitialCondition::findMachInterval(float *mlo, float *mhi, float vcas) {
- //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
-
- int i=0;
- bool found=false;
- float flo,fhi,fguess;
- float lo,hi,guess,step;
- step=0.1;
- guess=1.5;
- fguess=calcVcas(guess)-vcas;
- lo=hi=guess;
- do{
- step=2*step;
- lo-=step;
- if(lo < 0)
- lo=0;
- hi+=step;
- i++;
- flo=calcVcas(lo)-vcas;
- fhi=calcVcas(hi)-vcas;
- if(flo*fhi <=0){ //found interval with root
- found=true;
- if(flo*fguess <= 0){ //narrow interval down a bit
- hi=lo+step; //to pass solver interval that is as
- //small as possible
- }
- else if(fhi*fguess <= 0){
- lo=hi-step;
- }
- }
- //cout << "findMachInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
+ //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
+
+ int i=0;
+ bool found=false;
+ float flo,fhi,fguess;
+ float lo,hi,guess,step;
+ step=0.1;
+ guess=1.5;
+ fguess=calcVcas(guess)-vcas;
+ lo=hi=guess;
+ do {
+ step=2*step;
+ lo-=step;
+ if(lo < 0)
+ lo=0;
+ hi+=step;
+ i++;
+ flo=calcVcas(lo)-vcas;
+ fhi=calcVcas(hi)-vcas;
+ if(flo*fhi <=0) { //found interval with root
+ found=true;
+ if(flo*fguess <= 0) { //narrow interval down a bit
+ hi=lo+step; //to pass solver interval that is as
+ //small as possible
+ }
+ else if(fhi*fguess <= 0) {
+ lo=hi-step;
+ }
}
- while((found == 0) && (i <= 100));
- *mlo=lo;
- *mhi=hi;
- return found;
+ //cout << "findMachInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
+ }
+ while((found == 0) && (i <= 100));
+ *mlo=lo;
+ *mhi=hi;
+ return found;
}
bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) {
- float x1,x2,x3,f1,f2,f3,d,d0;
- float eps=1E-3;
- float const relax =0.9;
- int i;
- bool success=false;
- //initializations
- if(findMachInterval(&x1,&x3,vcas)) {
-
- f1=calcVcas(x1)-vcas;
- f3=calcVcas(x3)-vcas;
- d0=fabs(x3-x1);
-
- //iterations
- i=0;
- while ((fabs(d) > eps) && (i < 100)){
-
- d=(x3-x1)/d0;
- x2=x1-d*d0*f1/(f3-f1);
- f2=calcVcas(x2)-vcas;
- if(f1*f2 <= 0.0){
- x3=x2;
- f3=f2;
- f1=relax*f1;
- }
- else if(f2*f3 <= 0){
- x1=x2;
- f1=f2;
- f3=relax*f3;
- }
- //cout << i << endl;
- i++;
- }//end while
- if(i < 100) {
- success=true;
- *Mach=x2;
- }
-
- }
- //cout << "Success= " << success << " Vcas: " << vcas*FPSTOKTS << " Mach: " << *Mach << endl;
+ float x1,x2,x3,f1,f2,f3,d,d0;
+ float eps=1E-3;
+ float const relax =0.9;
+ int i;
+ bool success=false;
+
+ if(vcas < 0.1) {
+ Mach=0;
+ success=true;
return success;
+ }
+ //initializations
+ d=1;
+ if(findMachInterval(&x1,&x3,vcas)) {
+
+
+ f1=calcVcas(x1)-vcas;
+ f3=calcVcas(x3)-vcas;
+ d0=fabs(x3-x1);
+
+ //iterations
+ i=0;
+ while ((fabs(d) > eps) && (i < 100)) {
+ //cout << "getMachFromVcas x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
+ d=(x3-x1)/d0;
+ x2=x1-d*d0*f1/(f3-f1);
+
+ f2=calcVcas(x2)-vcas;
+ if(f1*f2 <= 0.0) {
+ x3=x2;
+ f3=f2;
+ f1=relax*f1;
+ } else if(f2*f3 <= 0) {
+ x1=x2;
+ f1=f2;
+ f3=relax*f3;
+ }
+ //cout << i << endl;
+ i++;
+ }//end while
+ if(i < 100) {
+ success=true;
+ *Mach=x2;
+ }
+
+ }
+ //cout << "Success= " << success << " Vcas: " << vcas*FPSTOKTS << " Mach: " << x2 << endl;
+ return success;
}
projects / flightgear.git / blobdiff