1 /*******************************************************************************
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.
28 #include "FDM/LaRCsimIC.hxx"
29 #include <FDM/LaRCsim/ls_cockpit.h>
30 #include <FDM/LaRCsim/ls_generic.h>
31 #include <FDM/LaRCsim/ls_interface.h>
32 #include <FDM/LaRCsim/atmos_62.h>
33 #include <FDM/LaRCsim/ls_constants.h>
34 #include <FDM/LaRCsim/ls_geodesy.h>
38 LaRCsimIC::LaRCsimIC(void) {
43 altitude=runway_altitude=hdot=alt_agl=0;
44 latitude_gd=latitude_gc=longitude=0;
47 vnorthwind=veastwind=vdownwind=0;
51 ls_geod_to_geoc( latitude_gd,altitude,&sea_level_radius,&latitude_gc);
56 LaRCsimIC::~LaRCsimIC(void) {}
58 void LaRCsimIC::get_atmosphere(void) {
59 Altitude=altitude; //set LaRCsim variable
60 ls_atmos(Altitude, &density_ratio, &soundspeed, &T, &p);
61 rho=density_ratio*SEA_LEVEL_DENSITY;
64 void LaRCsimIC::SetVcalibratedKtsIC( SCALAR tt) {
67 vt=sqrt(1/density_ratio*vc*vc);
70 void LaRCsimIC::SetVtrueFpsIC( SCALAR tt) {
75 void LaRCsimIC::SetMachIC( SCALAR tt) {
78 lastSpeedSet=lssetmach;
81 void LaRCsimIC::SetVequivalentKtsIC(SCALAR tt) {
84 vt=sqrt(SEA_LEVEL_DENSITY/rho)*ve;
87 void LaRCsimIC::SetClimbRateFpmIC( SCALAR tt) {
88 SetClimbRateFpsIC(tt/60.0);
91 void LaRCsimIC::SetClimbRateFpsIC( SCALAR tt) {
93 cout << "vtg: " << vtg << endl;
95 hdot = tt - vdownwind;
99 cout << "hdot: " << hdot << endl;
100 cout << "gamma: " << gamma*RAD_TO_DEG << endl;
101 cout << "vdown: " << vdown << endl;
105 void LaRCsimIC::SetFlightPathAngleRadIC( SCALAR tt) {
110 cout << "hdot: " << hdot << endl;
114 void LaRCsimIC::SetPitchAngleRadIC(SCALAR tt) {
115 if(alt_agl < (DEFAULT_AGL_ALT + 0.1) || vt < 10 )
116 theta=DEFAULT_PITCH_ON_GROUND;
122 void LaRCsimIC::SetUBodyFpsIC( SCALAR tt) {
124 vt=sqrt(u*u+v*v+w*w);
125 lastSpeedSet=lssetuvw;
129 void LaRCsimIC::SetVBodyFpsIC( SCALAR tt) {
131 vt=sqrt(u*u+v*v+w*w);
132 lastSpeedSet=lssetuvw;
135 void LaRCsimIC::SetWBodyFpsIC( SCALAR tt) {
137 vt=sqrt(u*u+v*v+w*w);
138 lastSpeedSet=lssetuvw;
141 void LaRCsimIC::SetVNorthAirmassFpsIC(SCALAR tt) {
143 vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
146 void LaRCsimIC::SetVEastAirmassFpsIC(SCALAR tt) {
148 vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
151 void LaRCsimIC::SetVDownAirmassFpsIC(SCALAR tt){
153 vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
155 hdot=-vtg*sin(gamma);
158 void LaRCsimIC::SetAltitudeFtIC( SCALAR tt) {
159 if(tt > (runway_altitude + DEFAULT_AGL_ALT)) {
162 altitude=runway_altitude + DEFAULT_AGL_ALT;
163 alt_agl=DEFAULT_AGL_ALT;
164 theta=DEFAULT_PITCH_ON_GROUND;
168 //lets try to make sure the user gets what they intended
170 switch(lastSpeedSet) {
179 SetVcalibratedKtsIC(vc*V_TO_KNOTS);
182 SetVequivalentKtsIC(ve*V_TO_KNOTS);
190 void LaRCsimIC::SetAltitudeAGLFtIC( SCALAR tt) {
193 altitude=runway_altitude + alt_agl;
196 void LaRCsimIC::SetRunwayAltitudeFtIC( SCALAR tt) {
198 if(lastAltSet == lssetagl)
199 altitude = runway_altitude + alt_agl;
202 void LaRCsimIC::calcVtfromNED(void) {
203 //take the earth's rotation out of veast first
204 //float veastner = veast- OMEGA_EARTH*sea_level_radius*cos( latitude_gd );
205 float veastner=veast;
206 u = (vnorth - vnorthwind)*cos(theta)*cos(psi) +
207 (veastner - veastwind)*cos(theta)*sin(psi) -
208 (vdown - vdownwind)*sin(theta);
209 v = (vnorth - vnorthwind)*(sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) +
210 (veastner - veastwind)*(sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi)) +
211 (vdown - vdownwind)*sin(phi)*cos(theta);
212 w = (vnorth - vnorthwind)*(cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) +
213 (veastner - veastwind)*(cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) +
214 (vdown - vdownwind)*cos(phi)*cos(theta);
215 vt = sqrt(u*u + v*v + w*w);
218 void LaRCsimIC::calcNEDfromVt(void) {
221 //get the body components of vt
226 //transform them to local axes and add on the wind components
227 vnorth = u*cos(theta)*cos(psi) +
228 v*(sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) +
229 w*(cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) +
232 //need to account for the earth's rotation here
233 veastner = u*cos(theta)*sin(psi) +
234 v*(sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi)) +
235 w*(cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) +
238 //veast = veastner + OMEGA_EARTH*sea_level_radius*cos( latitude_gd );
240 vdown = u*sin(theta) +
241 v*sin(phi)*cos(theta) +
242 w*cos(phi)*cos(theta) +
246 void LaRCsimIC::SetVnorthFpsIC( SCALAR tt) {
248 lastSpeedSet=lssetned;
252 void LaRCsimIC::SetVeastFpsIC( SCALAR tt) {
254 lastSpeedSet=lssetned;
258 void LaRCsimIC::SetVdownFpsIC( SCALAR tt) {
261 SetClimbRateFpsIC(-1*vdown);
262 lastSpeedSet=lssetned;
265 void LaRCsimIC::SetLatitudeGDRadIC(SCALAR tt) {
267 ls_geod_to_geoc(latitude_gd,altitude,&sea_level_radius, &latitude_gc);
270 bool LaRCsimIC::getAlpha(void) {
272 float guess=theta-gamma;
276 sfunc=&LaRCsimIC::GammaEqOfAlpha;
277 if(findInterval(0,guess)){
286 bool LaRCsimIC::getTheta(void) {
288 float guess=alpha+gamma;
291 sfunc=&LaRCsimIC::GammaEqOfTheta;
292 if(findInterval(0,guess)){
302 SCALAR LaRCsimIC::GammaEqOfTheta( SCALAR theta_arg) {
305 a=cos(alpha)*cos(beta)*sin(theta_arg);
306 b=sin(beta)*sin(phi);
307 c=sin(alpha)*cos(beta)*cos(phi);
308 return sin(gamma)-a+(b+c)*cos(theta_arg);
311 SCALAR LaRCsimIC::GammaEqOfAlpha( SCALAR alpha_arg) {
314 a=cos(alpha_arg)*cos(beta)*sin(theta);
315 b=sin(beta)*sin(phi);
316 c=sin(alpha_arg)*cos(beta)*cos(phi);
317 return sin(gamma)-a+(b+c)*cos(theta);
325 bool LaRCsimIC::findInterval( SCALAR x,SCALAR guess) {
326 //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
330 float flo,fhi,fguess;
333 fguess=(this->*sfunc)(guess)-x;
339 if(lo < xmin) lo=xmin;
340 if(hi > xmax) hi=xmax;
342 flo=(this->*sfunc)(lo)-x;
343 fhi=(this->*sfunc)(hi)-x;
344 if(flo*fhi <=0) { //found interval with root
346 if(flo*fguess <= 0) { //narrow interval down a bit
347 hi=lo+step; //to pass solver interval that is as
350 else if(fhi*fguess <= 0) {
354 //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
356 while((found == 0) && (i <= 100));
365 bool LaRCsimIC::solve( SCALAR *y,SCALAR x) {
366 float x1,x2,x3,f1,f2,f3,d,d0;
368 float const relax =0.9;
376 f1=(this->*sfunc)(x1)-x;
377 f3=(this->*sfunc)(x3)-x;
382 while ((fabs(d) > eps) && (i < 100)) {
384 x2=x1-d*d0*f1/(f3-f1);
386 f2=(this->*sfunc)(x2)-x;
387 //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
388 //cout << " " << f1 << "," << f2 << "," << f3 << endl;
390 if(fabs(f2) <= 0.001) {
392 } else if(f1*f2 <= 0.0) {
396 } else if(f2*f3 <= 0) {
409 //cout << "Success= " << success << " Vcas: " << vcas*V_TO_KNOTS << " Mach: " << x2 << endl;