1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Header: FGInitialCondition.h
7 ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
19 You should have received a copy of the GNU General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU General Public License can also be found on
24 the world wide web at http://www.gnu.org.
27 --------------------------------------------------------------------------------
30 FUNCTIONAL DESCRIPTION
31 --------------------------------------------------------------------------------
33 The purpose of this class is to take a set of initial conditions and provide
34 a kinematically consistent set of body axis velocity components, euler
35 angles, and altitude. This class does not attempt to trim the model i.e.
36 the sim will most likely start in a very dynamic state (unless, of course,
37 you have chosen your IC's wisely) even after setting it up with this class.
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 #ifndef FGINITIALCONDITION_H
44 #define FGINITIALCONDITION_H
46 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
48 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
50 #include <FGFDMExec.h>
51 #include <FGJSBBase.h>
52 #include <math/FGColumnVector3.h>
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
58 #define ID_INITIALCONDITION "$Id$"
60 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
66 typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset;
67 typedef enum { setwned, setwmd, setwhc } windset;
69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
73 /** Takes a set of initial conditions and provide a kinematically consistent set
74 of body axis velocity components, euler angles, and altitude. This class
75 does not attempt to trim the model i.e. the sim will most likely start in a
76 very dynamic state (unless, of course, you have chosen your IC's wisely)
77 even after setting it up with this class.
81 With a valid object of FGFDMExec and an aircraft model loaded
82 FGInitialCondition fgic=new FGInitialCondition(FDMExec);
83 fgic->SetVcalibratedKtsIC()
84 fgic->SetAltitudeFtIC();
86 //to directly into Run
87 FDMExec->GetState()->Initialize(fgic)
91 //or to loop the sim w/o integrating
96 Since vc, ve, vt, and mach all represent speed, the remaining
97 three are recalculated each time one of them is set (using the
98 current altitude). The most recent speed set is remembered so
99 that if and when altitude is reset, the last set speed is used
100 to recalculate the remaining three. Setting any of the body
101 components forces a recalculation of vt and vt then becomes the
102 most recent speed set.
104 Alpha,Gamma, and Theta:
106 This class assumes that it will be used to set up the sim for a
107 steady, zero pitch rate condition. Since any two of those angles
108 specifies the third gamma (flight path angle) is favored when setting
109 alpha and theta and alpha is favored when setting gamma. i.e.
111 - set alpha : recalculate theta using gamma as currently set
112 - set theta : recalculate alpha using gamma as currently set
113 - set gamma : recalculate theta using alpha as currently set
115 The idea being that gamma is most interesting to pilots (since it
116 is indicative of climb rate).
118 Setting climb rate is, for the purpose of this discussion,
119 considered equivalent to setting gamma.
121 These are the items that can be set in an initialization file:
123 UBODY <velocity, ft/sec>
124 VBODY <velocity, ft/sec>
125 WBODY <velocity, ft/sec>
126 LATITUDE <position, degrees>
127 LONGITUDE <position, degrees>
128 PHI <orientation, degrees>
129 THETA <orientation, degrees>
130 PSI <orientation, degrees>
131 ALPHA <angle, degrees>
132 BETA <angle, degrees>
133 GAMMA <angle, degrees>
134 ROC <vertical velocity, ft/sec>
135 ALTITUDE <altitude, ft>
136 WINDDIR <wind from-angle, degrees>
137 VWIND <magnitude wind speed, ft/sec>
138 HWIND <headwind speed, knots>
139 XWIND <crosswind speed, knots>
140 VC <calibrated airspeed, ft/sec>
142 VGROUND <ground speed, ft/sec>
150 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
152 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
154 class FGInitialCondition : public FGJSBBase
158 FGInitialCondition(FGFDMExec *fdmex);
160 ~FGInitialCondition();
162 void SetVcalibratedKtsIC(double tt);
163 void SetVequivalentKtsIC(double tt);
164 inline void SetVtrueKtsIC(double tt) { SetVtrueFpsIC(tt*ktstofps); }
165 inline void SetVgroundKtsIC(double tt) { SetVgroundFpsIC(tt*ktstofps); }
166 void SetMachIC(double tt);
168 inline void SetAlphaDegIC(double tt) { SetAlphaRadIC(tt*degtorad); }
169 inline void SetBetaDegIC(double tt) { SetBetaRadIC(tt*degtorad);}
171 inline void SetPitchAngleDegIC(double tt) { SetPitchAngleRadIC(tt*degtorad); }
172 inline void SetRollAngleDegIC(double tt) { SetRollAngleRadIC(tt*degtorad);}
173 inline void SetTrueHeadingDegIC(double tt){ SetTrueHeadingRadIC(tt*degtorad); }
175 void SetClimbRateFpmIC(double tt);
176 inline void SetFlightPathAngleDegIC(double tt) { SetFlightPathAngleRadIC(tt*degtorad); }
178 void SetAltitudeFtIC(double tt);
179 void SetAltitudeAGLFtIC(double tt);
181 void SetSeaLevelRadiusFtIC(double tt);
182 void SetTerrainAltitudeFtIC(double tt);
184 inline void SetLatitudeDegIC(double tt) { latitude=tt*degtorad; }
185 inline void SetLongitudeDegIC(double tt) { longitude=tt*degtorad; }
188 inline double GetVcalibratedKtsIC(void) const { return vc*fpstokts; }
189 inline double GetVequivalentKtsIC(void) const { return ve*fpstokts; }
190 inline double GetVgroundKtsIC(void) const { return vg*fpstokts; }
191 inline double GetVtrueKtsIC(void) const { return vt*fpstokts; }
192 inline double GetMachIC(void) const { return mach; }
194 inline double GetClimbRateFpmIC(void) const { return hdot*60; }
195 inline double GetFlightPathAngleDegIC(void)const { return gamma*radtodeg; }
197 inline double GetAlphaDegIC(void) const { return alpha*radtodeg; }
198 inline double GetBetaDegIC(void) const { return beta*radtodeg; }
200 inline double GetPitchAngleDegIC(void) const { return theta*radtodeg; }
201 inline double GetRollAngleDegIC(void) const { return phi*radtodeg; }
202 inline double GetHeadingDegIC(void) const { return psi*radtodeg; }
204 inline double GetLatitudeDegIC(void) const { return latitude*radtodeg; }
205 inline double GetLongitudeDegIC(void) const { return longitude*radtodeg; }
207 inline double GetAltitudeFtIC(void) const { return altitude; }
208 inline double GetAltitudeAGLFtIC(void) const { return altitude - terrain_altitude; }
210 inline double GetSeaLevelRadiusFtIC(void) const { return sea_level_radius; }
211 inline double GetTerrainAltitudeFtIC(void) const { return terrain_altitude; }
213 void SetVgroundFpsIC(double tt);
214 void SetVtrueFpsIC(double tt);
215 void SetUBodyFpsIC(double tt);
216 void SetVBodyFpsIC(double tt);
217 void SetWBodyFpsIC(double tt);
218 void SetVnorthFpsIC(double tt);
219 void SetVeastFpsIC(double tt);
220 void SetVdownFpsIC(double tt);
221 void SetPRadpsIC(double tt) { p = tt; }
222 void SetQRadpsIC(double tt) { q = tt; }
223 void SetRRadpsIC(double tt) { r = tt; }
225 void SetWindNEDFpsIC(double wN, double wE, double wD);
227 void SetWindMagKtsIC(double mag);
228 void SetWindDirDegIC(double dir);
230 void SetHeadWindKtsIC(double head);
231 void SetCrossWindKtsIC(double cross);// positive from left
233 void SetWindDownKtsIC(double wD);
235 void SetClimbRateFpsIC(double tt);
236 inline double GetVgroundFpsIC(void) const { return vg; }
237 inline double GetVtrueFpsIC(void) const { return vt; }
238 inline double GetWindUFpsIC(void) const { return uw; }
239 inline double GetWindVFpsIC(void) const { return vw; }
240 inline double GetWindWFpsIC(void) const { return ww; }
241 inline double GetWindNFpsIC(void) const { return wnorth; }
242 inline double GetWindEFpsIC(void) const { return weast; }
243 inline double GetWindDFpsIC(void) const { return wdown; }
244 inline double GetWindFpsIC(void) const { return sqrt(wnorth*wnorth + weast*weast); }
245 double GetWindDirDegIC(void);
246 inline double GetClimbRateFpsIC(void) const { return hdot; }
247 double GetUBodyFpsIC(void) const;
248 double GetVBodyFpsIC(void) const;
249 double GetWBodyFpsIC(void) const;
250 double GetPRadpsIC() const { return p; }
251 double GetQRadpsIC() const { return q; }
252 double GetRRadpsIC() const { return r; }
253 void SetFlightPathAngleRadIC(double tt);
254 void SetAlphaRadIC(double tt);
255 void SetPitchAngleRadIC(double tt);
256 void SetBetaRadIC(double tt);
257 void SetRollAngleRadIC(double tt);
258 void SetTrueHeadingRadIC(double tt);
259 inline void SetLatitudeRadIC(double tt) { latitude=tt; }
260 inline void SetLongitudeRadIC(double tt) { longitude=tt; }
261 inline double GetFlightPathAngleRadIC(void) const { return gamma; }
262 inline double GetAlphaRadIC(void) const { return alpha; }
263 inline double GetPitchAngleRadIC(void) const { return theta; }
264 inline double GetBetaRadIC(void) const { return beta; }
265 inline double GetRollAngleRadIC(void) const { return phi; }
266 inline double GetHeadingRadIC(void) const { return psi; }
267 inline double GetLatitudeRadIC(void) const { return latitude; }
268 inline double GetLongitudeRadIC(void) const { return longitude; }
269 inline double GetThetaRadIC(void) const { return theta; }
270 inline double GetPhiRadIC(void) const { return phi; }
271 inline double GetPsiRadIC(void) const { return psi; }
273 inline speedset GetSpeedSet(void) { return lastSpeedSet; }
274 inline windset GetWindSet(void) { return lastWindSet; }
276 bool Load(string rstname, bool useStoredPath = true );
285 double altitude,hdot;
286 double latitude,longitude;
290 double vnorth,veast,vdown;
291 double wnorth,weast,wdown;
292 double whead, wcross, wdir, wmag;
293 double sea_level_radius;
294 double terrain_altitude;
295 double radius_to_vehicle;
297 double alpha, beta, theta, phi, psi, gamma;
298 double salpha,sbeta,stheta,sphi,spsi,sgamma;
299 double calpha,cbeta,ctheta,cphi,cpsi,cgamma;
301 double xlo, xhi,xmin,xmax;
303 typedef double (FGInitialCondition::*fp)(double x);
306 speedset lastSpeedSet;
310 FGPropertyManager *PropertyManager;
314 bool getMachFromVcas(double *Mach,double vcas);
316 double GammaEqOfTheta(double Theta);
317 double GammaEqOfAlpha(double Alpha);
318 double calcVcas(double Mach);
319 void calcUVWfromNED(void);
320 void calcWindUVW(void);
322 bool findInterval(double x,double guess);
323 bool solve(double *y, double x);
324 void Debug(int from);