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