src/FDM/LaRCsimIC.cxx

   1 /*******************************************************************************
   2
   3  Header:       LaRCsimIC.cxx
   4  Author:       Tony Peden
   5  Date started: 10/9/99
   6
   7  ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------
   8
   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
  12  version.
  13
  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
  17  details.
  18
  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.
  22
  23  Further information about the GNU General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25 */
  26
  27
  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>
  35 #include <math.h>
  36 #include <iostream>
  37
  38 LaRCsimIC::LaRCsimIC(void) {
  39   vt=vtg=vw=vc=ve=0;
  40   mach=0;
  41   alpha=beta=gamma=0;
  42   theta=phi=psi=0;
  43   altitude=runway_altitude=hdot=alt_agl=0;
  44   latitude_gd=latitude_gc=longitude=0;
  45   u=v=w=0;
  46   vnorth=veast=vdown=0;
  47   vnorthwind=veastwind=vdownwind=0;
  48   lastSpeedSet=lssetvt;
  49   lastAltSet=lssetasl;
  50   get_atmosphere();
  51   ls_geod_to_geoc( latitude_gd,altitude,&sea_level_radius,&latitude_gc);
  52
  53 }
  54
  55
  56 LaRCsimIC::~LaRCsimIC(void) {}
  57
  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;
  62 }
  63
  64 void LaRCsimIC::SetVcalibratedKtsIC( SCALAR tt) {
  65     vc=tt*KTS_TO_FPS;
  66     lastSpeedSet=lssetvc;
  67     vt=sqrt(1/density_ratio*vc*vc);
  68 }
  69
  70 void LaRCsimIC::SetVtrueFpsIC( SCALAR tt) {
  71   vt=tt;
  72   lastSpeedSet=lssetvt;
  73 }
  74
  75 void LaRCsimIC::SetMachIC( SCALAR tt) {
  76   mach=tt;
  77   vt=mach*soundspeed;
  78   lastSpeedSet=lssetmach;
  79 }
  80
  81 void LaRCsimIC::SetVequivalentKtsIC(SCALAR tt) {
  82   ve=tt*KTS_TO_FPS;
  83   lastSpeedSet=lssetve;
  84   vt=sqrt(SEA_LEVEL_DENSITY/rho)*ve;
  85 }
  86
  87 void LaRCsimIC::SetClimbRateFpmIC( SCALAR tt) {
  88   SetClimbRateFpsIC(tt/60.0);
  89 }
  90
  91 void LaRCsimIC::SetClimbRateFpsIC( SCALAR tt) {
  92   vtg=vt+vw;
  93   cout << "vtg: " << vtg << endl;
  94   if(vtg > 0.1) {
  95     hdot = tt - vdownwind;
  96     gamma=asin(hdot/vtg);
  97     getTheta();
  98     vdown=-1*hdot;
  99     cout << "hdot: " << hdot << endl;
 100     cout << "gamma: " << gamma*RAD_TO_DEG << endl;
 101     cout << "vdown: " << vdown << endl;
 102   }
 103 }
 104
 105 void LaRCsimIC::SetFlightPathAngleRadIC( SCALAR tt) {
 106   gamma=tt;
 107   getTheta();
 108   vtg=vt+vw;
 109   hdot=vtg*sin(tt);
 110   cout << "hdot: " << hdot << endl;
 111   vdown=-1*hdot;
 112 }
 113
 114 void LaRCsimIC::SetPitchAngleRadIC(SCALAR tt) {
 115   if(alt_agl < (DEFAULT_AGL_ALT + 0.1) || vt < 10 )
 116     theta=DEFAULT_PITCH_ON_GROUND;
 117   else
 118     theta=tt;
 119   getAlpha();
 120 }
 121
 122 void LaRCsimIC::SetUBodyFpsIC( SCALAR tt) {
 123   u=tt;
 124   vt=sqrt(u*u+v*v+w*w);
 125   lastSpeedSet=lssetuvw;
 126 }
 127
 128
 129 void LaRCsimIC::SetVBodyFpsIC( SCALAR tt) {
 130   v=tt;
 131   vt=sqrt(u*u+v*v+w*w);
 132   lastSpeedSet=lssetuvw;
 133 }
 134
 135 void LaRCsimIC::SetWBodyFpsIC( SCALAR tt) {
 136   w=tt;
 137   vt=sqrt(u*u+v*v+w*w);
 138   lastSpeedSet=lssetuvw;
 139 }
 140
 141 void LaRCsimIC::SetVNorthAirmassFpsIC(SCALAR tt) {
 142   vnorthwind=tt;
 143   vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
 144 }
 145
 146 void LaRCsimIC::SetVEastAirmassFpsIC(SCALAR tt) {
 147   veastwind=tt;
 148   vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
 149 }
 150
 151 void LaRCsimIC::SetVDownAirmassFpsIC(SCALAR tt){
 152   vdownwind=tt;
 153   vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
 154   vtg=vt+vw;
 155   hdot=-vtg*sin(gamma);
 156 }
 157
 158 void LaRCsimIC::SetAltitudeFtIC( SCALAR tt) {
 159   if(tt > (runway_altitude + DEFAULT_AGL_ALT)) {
 160     altitude=tt;
 161   } else {
 162     altitude=runway_altitude + DEFAULT_AGL_ALT;
 163     alt_agl=DEFAULT_AGL_ALT;
 164     theta=DEFAULT_PITCH_ON_GROUND;
 165   }
 166   lastAltSet=lssetasl;
 167   get_atmosphere();
 168   //lets try to make sure the user gets what they intended
 169
 170   switch(lastSpeedSet) {
 171   case lssetned:
 172     calcVtfromNED();
 173     break;
 174   case lssetuvw:
 175   case lssetvt:
 176     SetVtrueFpsIC(vt);
 177     break;
 178   case lssetvc:
 179     SetVcalibratedKtsIC(vc*V_TO_KNOTS);
 180     break;
 181   case lssetve:
 182     SetVequivalentKtsIC(ve*V_TO_KNOTS);
 183     break;
 184   case lssetmach:
 185     SetMachIC(mach);
 186     break;
 187   }
 188 }
 189
 190 void LaRCsimIC::SetAltitudeAGLFtIC( SCALAR tt) {
 191   alt_agl=tt;
 192   lastAltSet=lssetagl;
 193   altitude=runway_altitude + alt_agl;
 194 }
 195
 196 void LaRCsimIC::SetRunwayAltitudeFtIC( SCALAR tt) {
 197   runway_altitude=tt;
 198   if(lastAltSet == lssetagl)
 199       altitude = runway_altitude + alt_agl;
 200 }
 201
 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);
 216 }
 217
 218 void LaRCsimIC::calcNEDfromVt(void) {
 219   float veastner;
 220
 221   //get the body components of vt
 222   u = GetUBodyFpsIC();
 223   v = GetVBodyFpsIC();
 224   w = GetWBodyFpsIC();
 225
 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)) +
 230            vnorthwind;
 231
 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)) +
 236              veastwind;
 237   veast = veastner;
 238   //veast = veastner + OMEGA_EARTH*sea_level_radius*cos( latitude_gd );
 239
 240   vdown =  u*sin(theta) +
 241            v*sin(phi)*cos(theta) +
 242            w*cos(phi)*cos(theta) +
 243            vdownwind;
 244 }
 245
 246 void LaRCsimIC::SetVnorthFpsIC( SCALAR tt) {
 247   vnorth=tt;
 248   lastSpeedSet=lssetned;
 249   calcVtfromNED();
 250 }
 251
 252 void LaRCsimIC::SetVeastFpsIC( SCALAR tt) {
 253   veast=tt;
 254   lastSpeedSet=lssetned;
 255   calcVtfromNED();
 256 }
 257
 258 void LaRCsimIC::SetVdownFpsIC( SCALAR tt) {
 259   vdown=tt;
 260   calcVtfromNED();
 261   SetClimbRateFpsIC(-1*vdown);
 262   lastSpeedSet=lssetned;
 263 }
 264
 265 void LaRCsimIC::SetLatitudeGDRadIC(SCALAR tt) {
 266   latitude_gd=tt;
 267   ls_geod_to_geoc(latitude_gd,altitude,&sea_level_radius, &latitude_gc);
 268 }
 269
 270 bool LaRCsimIC::getAlpha(void) {
 271   bool result=false;
 272   float guess=theta-gamma;
 273   xlo=xhi=0;
 274   xmin=-89;
 275   xmax=89;
 276   sfunc=&LaRCsimIC::GammaEqOfAlpha;
 277   if(findInterval(0,guess)){
 278     if(solve(&alpha,0)){
 279       result=true;
 280     }
 281   }
 282   return result;
 283 }
 284
 285
 286 bool LaRCsimIC::getTheta(void) {
 287   bool result=false;
 288   float guess=alpha+gamma;
 289   xlo=xhi=0;
 290   xmin=-89;xmax=89;
 291   sfunc=&LaRCsimIC::GammaEqOfTheta;
 292   if(findInterval(0,guess)){
 293     if(solve(&theta,0)){
 294       result=true;
 295     }
 296   }
 297   return result;
 298 }
 299
 300
 301
 302 SCALAR LaRCsimIC::GammaEqOfTheta( SCALAR theta_arg) {
 303   SCALAR a,b,c;
 304
 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);
 309 }
 310
 311 SCALAR LaRCsimIC::GammaEqOfAlpha( SCALAR alpha_arg) {
 312   float a,b,c;
 313
 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);
 318 }
 319
 320
 321
 322
 323
 324
 325 bool LaRCsimIC::findInterval( SCALAR x,SCALAR guess) {
 326   //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
 327
 328   int i=0;
 329   bool found=false;
 330   float flo,fhi,fguess;
 331   float lo,hi,step;
 332   step=0.1;
 333   fguess=(this->*sfunc)(guess)-x;
 334   lo=hi=guess;
 335   do {
 336     step=2*step;
 337     lo-=step;
 338     hi+=step;
 339     if(lo < xmin) lo=xmin;
 340     if(hi > xmax) hi=xmax;
 341     i++;
 342     flo=(this->*sfunc)(lo)-x;
 343     fhi=(this->*sfunc)(hi)-x;
 344     if(flo*fhi <=0) {  //found interval with root
 345       found=true;
 346       if(flo*fguess <= 0) {  //narrow interval down a bit
 347         hi=lo+step;    //to pass solver interval that is as
 348         //small as possible
 349       }
 350       else if(fhi*fguess <= 0) {
 351         lo=hi-step;
 352       }
 353     }
 354     //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
 355   }
 356   while((found == 0) && (i <= 100));
 357   xlo=lo;
 358   xhi=hi;
 359   return found;
 360 }
 361
 362
 363
 364
 365 bool LaRCsimIC::solve( SCALAR *y,SCALAR x) {
 366   float x1,x2,x3,f1,f2,f3,d,d0;
 367   float eps=1E-5;
 368   float const relax =0.9;
 369   int i;
 370   bool success=false;
 371
 372    //initializations
 373   d=1;
 374
 375     x1=xlo;x3=xhi;
 376     f1=(this->*sfunc)(x1)-x;
 377     f3=(this->*sfunc)(x3)-x;
 378     d0=fabs(x3-x1);
 379
 380     //iterations
 381     i=0;
 382     while ((fabs(d) > eps) && (i < 100)) {
 383       d=(x3-x1)/d0;
 384       x2=x1-d*d0*f1/(f3-f1);
 385
 386       f2=(this->*sfunc)(x2)-x;
 387       //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
 388       //cout << "                " << f1 << "," << f2 << "," << f3 << endl;
 389
 390       if(fabs(f2) <= 0.001) {
 391         x1=x3=x2;
 392       } else if(f1*f2 <= 0.0) {
 393         x3=x2;
 394         f3=f2;
 395         f1=relax*f1;
 396       } else if(f2*f3 <= 0) {
 397         x1=x2;
 398         f1=f2;
 399         f3=relax*f3;
 400       }
 401       //cout << i << endl;
 402       i++;
 403     }//end while
 404     if(i < 100) {
 405       success=true;
 406       *y=x2;
 407     }
 408
 409   //cout << "Success= " << success << " Vcas: " << vcas*V_TO_KNOTS << " Mach: " << x2 << endl;
 410   return success;
 411 }