src/FDM/JSBSim/FGTrimLong.cpp

   1 /*******************************************************************************
   2
   3  Header:       FGTrimLong.cpp
   4  Author:       Tony Peden
   5  Date started: 9/8/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  HISTORY
  28 --------------------------------------------------------------------------------
  29 9/8/99   TP   Created
  30
  31
  32 FUNCTIONAL DESCRIPTION
  33 --------------------------------------------------------------------------------
  34
  35 This class takes the given set of IC's and finds the angle of attack, elevator,
  36 and throttle setting required to fly steady level. This is currently for in-air
  37 conditions only.  It is implemented using an iterative, one-axis-at-a-time
  38 scheme. */
  39
  40 //  !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
  41
  42
  43 /*******************************************************************************
  44 INCLUDES
  45 *******************************************************************************/
  46
  47 #include "FGFDMExec.h"
  48 #include "FGAtmosphere.h"
  49 #include "FGInitialCondition.h"
  50 #include "FGTrimLong.h"
  51 #include "FGAircraft.h"
  52
  53 /*******************************************************************************
  54 CLASS DECLARATION
  55 *******************************************************************************/
  56
  57 FGTrimLong::FGTrimLong(FGFDMExec *FDMExec,FGInitialCondition *FGIC ) {
  58
  59   Ncycles=40;
  60   Naxis=10;
  61   Tolerance=1E-3;
  62   A_Tolerance = Tolerance / 10;
  63
  64   Debug=0;
  65   fdmex=FDMExec;
  66   fgic=FGIC;
  67   alphaMin=fdmex->GetAircraft()->GetAlphaCLMin()*RADTODEG;
  68   alphaMax=fdmex->GetAircraft()->GetAlphaCLMax()*RADTODEG;
  69   if(alphaMax <= alphaMin) {
  70     alphaMax=20;
  71     alphaMin=-5;
  72   }
  73   udotf=&FGTrimLong::udot_func;
  74   wdotf=&FGTrimLong::wdot_func;
  75   qdotf=&FGTrimLong::qdot_func;
  76   total_its=0;
  77   udot_subits=wdot_subits=qdot_subits=0;
  78   trimudot=true;
  79   axis_count=0;
  80
  81 }
  82
  83 /******************************************************************************/
  84
  85 FGTrimLong::~FGTrimLong(void) {}
  86
  87 /******************************************************************************/
  88
  89 void FGTrimLong::TrimStats() {
  90   cout << endl << "  Trim Statistics: " << endl;
  91   cout << "    Total Iterations: " << total_its << endl;
  92   if(total_its > 0) {
  93     cout << "    Sub-iterations:" << endl;
  94     cout << "      wdot: " << wdot_subits << " average: " << wdot_subits/total_its << endl;
  95     cout << "      udot: " << udot_subits << " average: " << udot_subits/total_its << endl;
  96     cout << "      qdot: " << qdot_subits << " average: " << qdot_subits/total_its << endl;
  97   }
  98 }
  99
 100 /******************************************************************************/
 101
 102 void FGTrimLong::Report(void) {
 103   cout << endl << "  Trim Results" << endl;
 104   cout << "  Alpha: " << fdmex->GetTranslation()->Getalpha()*RADTODEG
 105   << " wdot: " << fdmex->GetTranslation()->GetUVWdot()(3)
 106   << " Tolerance " << Tolerance << endl;
 107
 108   cout << "  Throttle: " << fdmex->GetFCS()->GetThrottlePos(0)
 109   << " udot: " << fdmex->GetTranslation()->GetUVWdot()(1)
 110   << " Tolerance " << Tolerance << endl;
 111
 112   cout << "  Elevator: " << fdmex->GetFCS()->GetDePos()*RADTODEG
 113   << " qdot: " << fdmex->GetRotation()->GetPQRdot()(2)
 114   << " Tolerance " << A_Tolerance << endl;
 115 }
 116
 117 /******************************************************************************/
 118
 119 void FGTrimLong::ReportState(void) {
 120   cout << endl << "  JSBSim Trim Report" << endl;
 121   cout << "    Weight: " << fdmex->GetAircraft()->GetWeight()
 122   << " lbs.  CG x,y,z: " << fdmex->GetAircraft()->GetXYZcg()
 123   << " inches " << endl;
 124
 125   cout << "    Flaps: ";
 126   float flaps=fdmex->GetFCS()->GetDfPos();
 127   if(flaps <= 0.01)
 128     cout << "Up";
 129   else
 130     cout << flaps;
 131
 132   cout << "  Gear: ";
 133   if(fdmex->GetAircraft()->GetGearUp() == true)
 134     cout << "Up" << endl;
 135   else
 136     cout << "Down" << endl;
 137
 138   cout << "    Speed: " << fdmex->GetAuxiliary()->GetVcalibratedKTS()
 139   << " KCAS  Mach: " << fdmex->GetState()->GetParameter(FG_MACH)
 140   << endl;
 141
 142   cout << "    Altitude: " << fdmex->GetPosition()->Geth() << " ft" << endl;
 143
 144
 145   cout << "    Pitch Angle: " << fdmex->GetRotation()->Gettht()*RADTODEG
 146   << " deg  Angle of Attack: " << fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG
 147   << " deg" << endl;
 148
 149
 150   cout << "    Flight Path Angle: "
 151   << fdmex->GetPosition()->GetGamma()*RADTODEG
 152   << " deg" << endl;
 153
 154
 155   cout << "    Normal Load Factor: " << fdmex->GetAircraft()->GetNlf() << endl;
 156
 157   cout << "    Pitch Rate: " << fdmex->GetState()->GetParameter(FG_PITCHRATE)*RADTODEG
 158   << " deg/s" << endl;
 159
 160   cout << "    Roll Angle: " << fdmex->GetRotation()->Getphi()*RADTODEG
 161   << " deg  Roll Rate: " << fdmex->GetState()->GetParameter(FG_ROLLRATE)
 162   << " deg/s"
 163   << endl ;
 164
 165   cout << "    Sideslip: " << fdmex->GetState()->GetParameter(FG_BETA) *RADTODEG
 166   << " deg  Yaw Rate: " << fdmex->GetState()->GetParameter(FG_YAWRATE)*RADTODEG
 167   << " deg/s " << endl;
 168
 169   cout << "    Elevator: " << fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG
 170   << " deg  Left Aileron: " << fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG
 171   << " deg  Rudder: " << fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG
 172   << " deg" << endl;
 173
 174   cout << "    Throttle: " << fdmex->GetFCS()->GetThrottlePos(0)/100 << endl;
 175 }
 176
 177 /******************************************************************************/
 178
 179 void FGTrimLong::setThrottlesPct(float tt) {
 180
 181   float tMin,tMax;
 182   for(int i=0;i<fdmex->GetAircraft()->GetNumEngines();i++) {
 183     tMin=fdmex->GetAircraft()->GetEngine(i)->GetThrottleMin();
 184     tMax=fdmex->GetAircraft()->GetEngine(i)->GetThrottleMax();
 185     dth=tt;
 186     //cout << "setThrottlespct: " << i << ", " << tMin << ", " << tMax << ", " << dth << endl;
 187     fdmex -> GetFCS() -> SetThrottleCmd(i,tMin+dth*(tMax-tMin));
 188   }
 189 }
 190
 191 /******************************************************************************/
 192
 193 int FGTrimLong::checkLimits(trimfp fp, float current, float min, float max) {
 194   float lo,hi;
 195   int result=0;
 196   //cout << "Min: " << min << " Max: " << max << endl;
 197   lo=(this->*fp)(min);
 198   hi=(this->*fp)(max);
 199
 200   if(lo*hi >= 0) {
 201     //cout << "Lo: " << lo << " Hi: " << hi << endl;
 202     result=0;
 203   } else {
 204     lo=(this->*fp)(0);
 205     if(lo*hi >= 0)
 206       result=-1;
 207     else
 208       result=1;
 209   }
 210
 211   return result;
 212 }
 213
 214 /******************************************************************************/
 215
 216 bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result,
 217          float eps, float min, float max, int max_iterations, int *actual_its) {
 218
 219   float x1,x2,x3,f1,f2,f3,d,d0;
 220   float const relax =0.9;
 221   int i;
 222   x1 = x3 = x2 = 0;
 223   d=1;
 224   bool success=false;
 225   //initializations
 226   int side=checkLimits(fp,guess,min,max);
 227   if(side != 0) {
 228     if (side < 0)
 229       x3=min;
 230     else
 231       x1=max;
 232
 233     f1=(this->*fp)(x1)-desired;
 234     f3=(this->*fp)(x3)-desired;
 235     d0=fabs(x3-x1);
 236
 237     //iterations
 238     i=0;
 239     while ((fabs(d) > eps) && (i < max_iterations)) {
 240       if(Debug > 1)
 241         cout << "FGTrimLong::solve i,x1,x2,x3: " << i << ", " << x1
 242                                             << ", " << x2 << ", " << x3 << endl;
 243       d=(x3-x1)/d0;
 244       x2=x1-d*d0*f1/(f3-f1);
 245       // if(x2 < min)
 246       //         x2=min;
 247       //       else if(x2 > max)
 248       //         x2=max;
 249       f2=(this->*fp)(x2)-desired;
 250       if(f1*f2 <= 0.0) {
 251         x3=x2;
 252         f3=f2;
 253         f1=relax*f1;
 254       } else if(f2*f3 <= 0) {
 255         x1=x2;
 256         f1=f2;
 257         f3=relax*f3;
 258       }
 259       //cout << i << endl;
 260       i++;
 261     }//end while
 262     if(i < max_iterations) {
 263       success=true;
 264       *result=x2;
 265     }
 266     *actual_its=i;
 267   } else {
 268     *actual_its=0;
 269   }
 270   return success;
 271 }
 272
 273 /******************************************************************************/
 274
 275 bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess,
 276                                              float desired,int max_iterations) {
 277   int i=0;
 278   bool found=false;
 279   float flo,fhi,fguess;
 280   float xlo,xhi,step;
 281   step=0.1*guess;
 282   fguess=(this->*fp)(guess)-desired;
 283   xlo=xhi=guess;
 284   do {
 285     step=2*step;
 286     xlo-=step;
 287     xhi+=step;
 288     i++;
 289     flo=(this->*fp)(xlo)-desired;
 290     fhi=(this->*fp)(xhi)-desired;
 291     if(flo*fhi <=0) {  //found interval with root
 292       found=true;
 293       if(flo*fguess <= 0) {  //narrow interval down a bit
 294         xhi=xlo+step;    //to pass solver interval that is as
 295         //small as possible
 296       }
 297       else if(fhi*fguess <= 0) {
 298         xlo=xhi-step;
 299       }
 300     }
 301     if(Debug > 1)
 302       cout << "FGTrimLong::findInterval: i=" << i << " Lo= " << xlo
 303                            << " Hi= " << xhi << " flo*fhi: " << flo*fhi << endl;
 304   } while((found == 0) && (i <= max_iterations));
 305   *lo=xlo;
 306   *hi=xhi;
 307   return found;
 308 }
 309
 310 /******************************************************************************/
 311
 312 float FGTrimLong::udot_func(float x) {
 313   setThrottlesPct(x);
 314   fdmex->RunIC(fgic);
 315   return fdmex->GetTranslation()->GetUVWdot()(1);
 316 }
 317
 318 /******************************************************************************/
 319
 320 float FGTrimLong::wdot_func(float x) {
 321   fgic->SetAlphaDegIC(x);
 322   fdmex->RunIC(fgic);
 323   return fdmex->GetTranslation()->GetUVWdot()(3);
 324 }
 325
 326 /******************************************************************************/
 327
 328 float FGTrimLong::qdot_func(float x) {
 329   fdmex->GetFCS()->SetPitchTrimCmd(x);
 330   fdmex->RunIC(fgic);
 331   return fdmex->GetRotation()->GetPQRdot()(2);
 332 }
 333
 334 /******************************************************************************/
 335
 336 bool FGTrimLong::DoTrim(void) {
 337   int k=0;
 338   int its;
 339
 340
 341   if(fgic->GetVtrueKtsIC() < 1) {
 342     cout << "Trim failed, on-ground trimming not yet implemented." << endl;
 343     cout << "Or did you *really* mean to start in-air"
 344          << " with less than 1 knot airspeed?" << endl;
 345     return false;
 346   }
 347
 348
 349   fgic -> SetAlphaDegIC((alphaMin+alphaMax)/2);
 350   fdmex -> GetFCS() -> SetDeCmd(0);
 351   fdmex -> GetFCS() -> SetPitchTrimCmd(0);
 352   setThrottlesPct(0.5);
 353   fdmex -> RunIC(fgic);
 354
 355   if(trimudot == false)
 356     udot=0;
 357   do {
 358     axis_count=0;
 359     solve(wdotf,fgic->GetAlphaDegIC(),0,&wdot,Tolerance,alphaMin, alphaMax,Naxis,&its);
 360     wdot_subits+=its;
 361     if(Debug > 0) {
 362       cout << "Alpha: " << fdmex->GetTranslation()->Getalpha()*RADTODEG
 363       << " wdot: " << fdmex->GetTranslation()->GetUVWdot()(3)
 364       << endl;
 365     }
 366     solve(udotf,dth,0,&udot,Tolerance,0,1,Naxis,&its);
 367     udot_subits+=its;
 368     if(Debug > 0) {
 369       cout << "Throttle: " << fdmex->GetFCS()->GetThrottlePos(0)
 370       << " udot: " << fdmex->GetTranslation()->GetUVWdot()(1)
 371       << endl;
 372     }
 373     solve(qdotf,fdmex->GetFCS()->GetPitchTrimCmd(),0,&qdot,A_Tolerance,-1,1,Naxis,&its);
 374     qdot_subits+=its;
 375     if(Debug > 0) {
 376       cout << "Elevator: " << fdmex->GetFCS()->GetDePos()*RADTODEG
 377       << " qdot: " << fdmex->GetRotation()->GetPQRdot()(2)
 378       << endl;
 379     }
 380     wdot=fabs(fdmex->GetTranslation()->GetUVWdot()(3));
 381     qdot=fabs(fdmex->GetRotation()->GetPQRdot()(2));
 382     udot=fabs(fdmex->GetTranslation()->GetUVWdot()(1));
 383
 384     //these checks need to be done after all the axes have run
 385     if(udot < Tolerance)
 386       axis_count++;
 387     if(wdot < Tolerance)
 388       axis_count++;
 389     if(qdot < A_Tolerance)
 390       axis_count++;
 391     if(axis_count == 2) {
 392
 393       //At this point we can check the input limits of the failed axis
 394       //and declare the trim failed if there is no sign change. If there
 395       //is, keep going until success or max iteration count
 396
 397       //Oh, well: two out of three ain't bad
 398       if(wdot > Tolerance) {
 399         if(checkLimits(wdotf,fgic->GetAlphaDegIC(),alphaMin,alphaMax) == false) {
 400           cout << "    Sorry, wdot doesn't appear to be trimmable" << endl;
 401           total_its=k;
 402           k=Ncycles; //force the trim to fail
 403         }
 404      }
 405       if( udot > Tolerance ) {
 406         if(checkLimits(udotf,dth,0,1) == false) {
 407           cout << "    Sorry, udot doesn't appear to be trimmable" << endl;
 408           cout << "    Resetting throttles to zero" << endl;
 409           setThrottlesPct(0);
 410           fdmex->GetFCS()->SetThrottleCmd(-1,0);
 411           fdmex->RunIC(fgic);
 412           total_its=k;
 413           k=Ncycles; //force the trim to fail
 414         }
 415      }
 416       if(qdot > A_Tolerance) {
 417
 418         if(checkLimits(qdotf,fdmex->GetFCS()->GetPitchTrimCmd(),-1,1) == false) {
 419           cout << "    Sorry, qdot doesn't appear to be trimmable" << endl;
 420           total_its=k;
 421           k=Ncycles; //force the trim to fail
 422         }
 423       }
 424     }
 425     k++;
 426   } while((axis_count < 3) && (k < Ncycles));
 427   if(axis_count >= 3) {
 428     total_its=k;
 429     cout << endl << "  Trim successful" << endl;
 430     return true;
 431   } else {
 432     total_its=k;
 433     cout << endl << "  Trim failed" << endl;
 434     return false;
 435   }
 436
 437 }
 438
 439 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 440