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