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 <simgear/compiler.h>
  29
  30 #include <math.h>
  31 #include STL_IOSTREAM
  32
  33 #include "FDM/LaRCsimIC.hxx"
  34 #include <FDM/LaRCsim/ls_cockpit.h>
  35 #include <FDM/LaRCsim/ls_generic.h>
  36 #include <FDM/LaRCsim/ls_interface.h>
  37 #include <FDM/LaRCsim/atmos_62.h>
  38 #include <FDM/LaRCsim/ls_constants.h>
  39 #include <FDM/LaRCsim/ls_geodesy.h>
  40
  41 #if !defined(SG_HAVE_NATIVE_SGI_COMPILERS)
  42 SG_USING_STD(cout);
  43 SG_USING_STD(endl);
  44 #endif
  45
  46
  47 LaRCsimIC::LaRCsimIC(void) {
  48   vt=vtg=vw=vc=ve=0;
  49   mach=0;
  50   alpha=beta=gamma=0;
  51   theta=phi=psi=0;
  52   altitude=runway_altitude=hdot=alt_agl=0;
  53   latitude_gd=latitude_gc=longitude=0;
  54   u=v=w=0;
  55   vnorth=veast=vdown=0;
  56   vnorthwind=veastwind=vdownwind=0;
  57   lastSpeedSet=lssetvt;
  58   lastAltSet=lssetasl;
  59   get_atmosphere();
  60   ls_geod_to_geoc( latitude_gd,altitude,&sea_level_radius,&latitude_gc);
  61
  62 }
  63
  64
  65 LaRCsimIC::~LaRCsimIC(void) {}
  66
  67 void LaRCsimIC::get_atmosphere(void) {
  68   Altitude=altitude; //set LaRCsim variable
  69   ls_atmos(Altitude, &density_ratio, &soundspeed, &T, &p);
  70   rho=density_ratio*SEA_LEVEL_DENSITY;
  71 }
  72
  73 void LaRCsimIC::SetVcalibratedKtsIC( SCALAR tt) {
  74     vc=tt*KTS_TO_FPS;
  75     lastSpeedSet=lssetvc;
  76     vt=sqrt(1/density_ratio*vc*vc);
  77 }
  78
  79 void LaRCsimIC::SetVtrueFpsIC( SCALAR tt) {
  80   vt=tt;
  81   lastSpeedSet=lssetvt;
  82 }
  83
  84 void LaRCsimIC::SetMachIC( SCALAR tt) {
  85   mach=tt;
  86   vt=mach*soundspeed;
  87   lastSpeedSet=lssetmach;
  88 }
  89
  90 void LaRCsimIC::SetVequivalentKtsIC(SCALAR tt) {
  91   ve=tt*KTS_TO_FPS;
  92   lastSpeedSet=lssetve;
  93   vt=sqrt(SEA_LEVEL_DENSITY/rho)*ve;
  94 }
  95
  96 void LaRCsimIC::SetClimbRateFpmIC( SCALAR tt) {
  97   SetClimbRateFpsIC(tt/60.0);
  98 }
  99
 100 void LaRCsimIC::SetClimbRateFpsIC( SCALAR tt) {
 101   vtg=vt+vw;
 102   cout << "vtg: " << vtg << endl;
 103   if(vtg > 0.1) {
 104     hdot = tt - vdownwind;
 105     gamma=asin(hdot/vtg);
 106     getTheta();
 107     vdown=-1*hdot;
 108     cout << "hdot: " << hdot << endl;
 109     cout << "gamma: " << gamma*SGD_RADIANS_TO_DEGREES << endl;
 110     cout << "vdown: " << vdown << endl;
 111   }
 112 }
 113
 114 void LaRCsimIC::SetFlightPathAngleRadIC( SCALAR tt) {
 115   gamma=tt;
 116   getTheta();
 117   vtg=vt+vw;
 118   hdot=vtg*sin(tt);
 119   cout << "hdot: " << hdot << endl;
 120   vdown=-1*hdot;
 121 }
 122
 123 void LaRCsimIC::SetPitchAngleRadIC(SCALAR tt) {
 124   if(alt_agl < (DEFAULT_AGL_ALT + 0.1) || vt < 10 )
 125     theta=DEFAULT_PITCH_ON_GROUND;
 126   else
 127     theta=tt;
 128   getAlpha();
 129 }
 130
 131 void LaRCsimIC::SetUVWFpsIC(SCALAR vu, SCALAR vv, SCALAR vw) {
 132   u=vu; v=vv; w=vw;
 133   vt=sqrt(u*u+v*v+w*w);
 134   lastSpeedSet=lssetuvw;
 135 }
 136
 137
 138 void LaRCsimIC::SetVNEDAirmassFpsIC(SCALAR north, SCALAR east, SCALAR down ) {
 139   vnorthwind=north; veastwind=east; vdownwind=down;
 140   vw=sqrt(vnorthwind*vnorthwind + veastwind*veastwind + vdownwind*vdownwind);
 141   vtg=vt+vw;
 142   SetClimbRateFpsIC(-1*(vdown+vdownwind));
 143 }
 144
 145 void LaRCsimIC::SetAltitudeFtIC( SCALAR tt) {
 146   if(tt > (runway_altitude + DEFAULT_AGL_ALT)) {
 147     altitude=tt;
 148   } else {
 149     altitude=runway_altitude + DEFAULT_AGL_ALT;
 150     alt_agl=DEFAULT_AGL_ALT;
 151     theta=DEFAULT_PITCH_ON_GROUND;
 152   }
 153   lastAltSet=lssetasl;
 154   get_atmosphere();
 155   //lets try to make sure the user gets what they intended
 156
 157   switch(lastSpeedSet) {
 158   case lssetned:
 159     calcVtfromNED();
 160     break;
 161   case lssetuvw:
 162   case lssetvt:
 163     SetVtrueFpsIC(vt);
 164     break;
 165   case lssetvc:
 166     SetVcalibratedKtsIC(vc*V_TO_KNOTS);
 167     break;
 168   case lssetve:
 169     SetVequivalentKtsIC(ve*V_TO_KNOTS);
 170     break;
 171   case lssetmach:
 172     SetMachIC(mach);
 173     break;
 174   }
 175 }
 176
 177 void LaRCsimIC::SetAltitudeAGLFtIC( SCALAR tt) {
 178   alt_agl=tt;
 179   lastAltSet=lssetagl;
 180   altitude=runway_altitude + alt_agl;
 181 }
 182
 183 void LaRCsimIC::SetRunwayAltitudeFtIC( SCALAR tt) {
 184   runway_altitude=tt;
 185   if(lastAltSet == lssetagl)
 186       altitude = runway_altitude + alt_agl;
 187 }
 188
 189 void LaRCsimIC::calcVtfromNED(void) {
 190   //take the earth's rotation out of veast first
 191   //float veastner = veast- OMEGA_EARTH*sea_level_radius*cos( latitude_gd );
 192   float veastner=veast;
 193   u = (vnorth - vnorthwind)*cos(theta)*cos(psi) +
 194       (veastner - veastwind)*cos(theta)*sin(psi) -
 195       (vdown - vdownwind)*sin(theta);
 196   v = (vnorth - vnorthwind)*(sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) +
 197       (veastner - veastwind)*(sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi)) +
 198       (vdown - vdownwind)*sin(phi)*cos(theta);
 199   w = (vnorth - vnorthwind)*(cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) +
 200       (veastner - veastwind)*(cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) +
 201       (vdown - vdownwind)*cos(phi)*cos(theta);
 202   vt = sqrt(u*u + v*v + w*w);
 203 }
 204
 205 void LaRCsimIC::calcNEDfromVt(void) {
 206   float veastner;
 207
 208   //get the body components of vt
 209   u = GetUBodyFpsIC();
 210   v = GetVBodyFpsIC();
 211   w = GetWBodyFpsIC();
 212
 213   //transform them to local axes and add on the wind components
 214   vnorth = u*cos(theta)*cos(psi) +
 215            v*(sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) +
 216            w*(cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) +
 217            vnorthwind;
 218
 219   //need to account for the earth's rotation here
 220   veastner = u*cos(theta)*sin(psi) +
 221              v*(sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi)) +
 222              w*(cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) +
 223              veastwind;
 224   veast = veastner;
 225   //veast = veastner + OMEGA_EARTH*sea_level_radius*cos( latitude_gd );
 226
 227   vdown =  u*sin(theta) +
 228            v*sin(phi)*cos(theta) +
 229            w*cos(phi)*cos(theta) +
 230            vdownwind;
 231 }
 232
 233 void LaRCsimIC::SetVNEDFpsIC( SCALAR north, SCALAR east, SCALAR down) {
 234   vnorth=north;
 235   veast=east;
 236   vdown=down;
 237   SetClimbRateFpsIC(-1*vdown);
 238   lastSpeedSet=lssetned;
 239   calcVtfromNED();
 240 }
 241
 242 void LaRCsimIC::SetLatitudeGDRadIC(SCALAR tt) {
 243   latitude_gd=tt;
 244   ls_geod_to_geoc(latitude_gd,altitude,&sea_level_radius, &latitude_gc);
 245 }
 246
 247 bool LaRCsimIC::getAlpha(void) {
 248   bool result=false;
 249   float guess=theta-gamma;
 250   xlo=xhi=0;
 251   xmin=-89;
 252   xmax=89;
 253   sfunc=&LaRCsimIC::GammaEqOfAlpha;
 254   if(findInterval(0,guess)){
 255     if(solve(&alpha,0)){
 256       result=true;
 257     }
 258   }
 259   return result;
 260 }
 261
 262
 263 bool LaRCsimIC::getTheta(void) {
 264   bool result=false;
 265   float guess=alpha+gamma;
 266   xlo=xhi=0;
 267   xmin=-89;xmax=89;
 268   sfunc=&LaRCsimIC::GammaEqOfTheta;
 269   if(findInterval(0,guess)){
 270     if(solve(&theta,0)){
 271       result=true;
 272     }
 273   }
 274   return result;
 275 }
 276
 277
 278
 279 SCALAR LaRCsimIC::GammaEqOfTheta( SCALAR theta_arg) {
 280   SCALAR a,b,c;
 281
 282   a=cos(alpha)*cos(beta)*sin(theta_arg);
 283   b=sin(beta)*sin(phi);
 284   c=sin(alpha)*cos(beta)*cos(phi);
 285   return sin(gamma)-a+(b+c)*cos(theta_arg);
 286 }
 287
 288 SCALAR LaRCsimIC::GammaEqOfAlpha( SCALAR alpha_arg) {
 289   float a,b,c;
 290
 291   a=cos(alpha_arg)*cos(beta)*sin(theta);
 292   b=sin(beta)*sin(phi);
 293   c=sin(alpha_arg)*cos(beta)*cos(phi);
 294   return sin(gamma)-a+(b+c)*cos(theta);
 295 }
 296
 297
 298
 299
 300
 301
 302 bool LaRCsimIC::findInterval( SCALAR x,SCALAR guess) {
 303   //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){
 304
 305   int i=0;
 306   bool found=false;
 307   float flo,fhi,fguess;
 308   float lo,hi,step;
 309   step=0.1;
 310   fguess=(this->*sfunc)(guess)-x;
 311   lo=hi=guess;
 312   do {
 313     step=2*step;
 314     lo-=step;
 315     hi+=step;
 316     if(lo < xmin) lo=xmin;
 317     if(hi > xmax) hi=xmax;
 318     i++;
 319     flo=(this->*sfunc)(lo)-x;
 320     fhi=(this->*sfunc)(hi)-x;
 321     if(flo*fhi <=0) {  //found interval with root
 322       found=true;
 323       if(flo*fguess <= 0) {  //narrow interval down a bit
 324         hi=lo+step;    //to pass solver interval that is as
 325         //small as possible
 326       }
 327       else if(fhi*fguess <= 0) {
 328         lo=hi-step;
 329       }
 330     }
 331     //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl;
 332   }
 333   while((found == 0) && (i <= 100));
 334   xlo=lo;
 335   xhi=hi;
 336   return found;
 337 }
 338
 339
 340
 341
 342 bool LaRCsimIC::solve( SCALAR *y,SCALAR x) {
 343   float x1,x2,x3,f1,f2,f3,d,d0;
 344   float eps=1E-5;
 345   float const relax =0.9;
 346   int i;
 347   bool success=false;
 348
 349    //initializations
 350   d=1;
 351
 352     x1=xlo;x3=xhi;
 353     f1=(this->*sfunc)(x1)-x;
 354     f3=(this->*sfunc)(x3)-x;
 355     d0=fabs(x3-x1);
 356
 357     //iterations
 358     i=0;
 359     while ((fabs(d) > eps) && (i < 100)) {
 360       d=(x3-x1)/d0;
 361       x2=x1-d*d0*f1/(f3-f1);
 362
 363       f2=(this->*sfunc)(x2)-x;
 364       //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
 365       //cout << "                " << f1 << "," << f2 << "," << f3 << endl;
 366
 367       if(fabs(f2) <= 0.001) {
 368         x1=x3=x2;
 369       } else if(f1*f2 <= 0.0) {
 370         x3=x2;
 371         f3=f2;
 372         f1=relax*f1;
 373       } else if(f2*f3 <= 0) {
 374         x1=x2;
 375         f1=f2;
 376         f3=relax*f3;
 377       }
 378       //cout << i << endl;
 379       i++;
 380     }//end while
 381     if(i < 100) {
 382       success=true;
 383       *y=x2;
 384     }
 385
 386   //cout << "Success= " << success << " Vcas: " << vcas*V_TO_KNOTS << " Mach: " << x2 << endl;
 387   return success;
 388 }