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