src/FDM/JSBSim/FGTrim.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGTrim.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 <stdlib.h>
  48
  49 #include "FGFDMExec.h"
  50 #include "FGAtmosphere.h"
  51 #include "FGInitialCondition.h"
  52 #include "FGTrim.h"
  53 #include "FGAircraft.h"
  54 #include "FGMassBalance.h"
  55 #include "FGGroundReactions.h"
  56 #include "FGInertial.h"
  57 #include "FGAerodynamics.h"
  58 #include "FGColumnVector3.h"
  59
  60 #if _MSC_VER
  61 #pragma warning (disable : 4786 4788)
  62 #endif
  63
  64 namespace JSBSim {
  65
  66 static const char *IdSrc = "$Id$";
  67 static const char *IdHdr = ID_TRIM;
  68
  69 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  70
  71 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
  72
  73   N=Nsub=0;
  74   max_iterations=60;
  75   max_sub_iterations=100;
  76   Tolerance=1E-3;
  77   A_Tolerance = Tolerance / 10;
  78
  79   Debug=0;DebugLevel=0;
  80   fdmex=FDMExec;
  81   fgic=fdmex->GetIC();
  82   total_its=0;
  83   trimudot=true;
  84   gamma_fallback=true;
  85   axis_count=0;
  86   mode=tt;
  87   xlo=xhi=alo=ahi=0.0;
  88   targetNlf=1.0;
  89   debug_axis=tAll;
  90   SetMode(tt);
  91   if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
  92 }
  93
  94 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  95
  96 FGTrim::~FGTrim(void) {
  97   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
  98     delete TrimAxes[current_axis];
  99   }
 100   delete[] sub_iterations;
 101   delete[] successful;
 102   delete[] solution;
 103   if (debug_lvl & 2) cout << "Destroyed:    FGTrim" << endl;
 104 }
 105
 106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 107
 108 void FGTrim::TrimStats() {
 109   char out[80];
 110   int run_sum=0;
 111   cout << endl << "  Trim Statistics: " << endl;
 112   cout << "    Total Iterations: " << total_its << endl;
 113   if(total_its > 0) {
 114     cout << "    Sub-iterations:" << endl;
 115     for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
 116       run_sum+=TrimAxes[current_axis]->GetRunCount();
 117       snprintf(out,80,"   %5s: %3.0f average: %5.2f  successful: %3.0f  stability: %5.2f\n",
 118                   TrimAxes[current_axis]->GetStateName().c_str(),
 119                   sub_iterations[current_axis],
 120                   sub_iterations[current_axis]/double(total_its),
 121                   successful[current_axis],
 122                   TrimAxes[current_axis]->GetAvgStability() );
 123       cout << out;
 124     }
 125     cout << "    Run Count: " << run_sum << endl;
 126   }
 127 }
 128
 129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 130
 131 void FGTrim::Report(void) {
 132   cout << "  Trim Results: " << endl;
 133   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
 134     TrimAxes[current_axis]->AxisReport();
 135
 136 }
 137
 138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139
 140 void FGTrim::ClearStates(void) {
 141     FGTrimAxis* ta;
 142
 143     mode=tCustom;
 144     vector<FGTrimAxis*>::iterator iAxes;
 145     iAxes = TrimAxes.begin();
 146     while (iAxes != TrimAxes.end()) {
 147       ta=*iAxes;
 148       delete ta;
 149       iAxes++;
 150     }
 151     TrimAxes.clear();
 152     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 153 }
 154
 155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 156
 157 bool FGTrim::AddState( State state, Control control ) {
 158   FGTrimAxis* ta;
 159   bool result=true;
 160
 161   mode = tCustom;
 162   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 163   while (iAxes != TrimAxes.end()) {
 164       ta=*iAxes;
 165       if( ta->GetStateType() == state )
 166         result=false;
 167       iAxes++;
 168   }
 169   if(result) {
 170     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
 171     delete[] sub_iterations;
 172     delete[] successful;
 173     delete[] solution;
 174     sub_iterations=new double[TrimAxes.size()];
 175     successful=new double[TrimAxes.size()];
 176     solution=new bool[TrimAxes.size()];
 177   }
 178   return result;
 179 }
 180
 181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 182
 183 bool FGTrim::RemoveState( State state ) {
 184   FGTrimAxis* ta;
 185   bool result=false;
 186
 187   mode = tCustom;
 188   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 189   while (iAxes != TrimAxes.end()) {
 190       ta=*iAxes;
 191       if( ta->GetStateType() == state ) {
 192         delete ta;
 193         TrimAxes.erase(iAxes);
 194         result=true;
 195         continue;
 196       }
 197       iAxes++;
 198   }
 199   if(result) {
 200     delete[] sub_iterations;
 201     delete[] successful;
 202     delete[] solution;
 203     sub_iterations=new double[TrimAxes.size()];
 204     successful=new double[TrimAxes.size()];
 205     solution=new bool[TrimAxes.size()];
 206   }
 207   return result;
 208 }
 209
 210 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 211
 212 bool FGTrim::EditState( State state, Control new_control ){
 213   FGTrimAxis* ta;
 214   bool result=false;
 215
 216   mode = tCustom;
 217   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 218   while (iAxes != TrimAxes.end()) {
 219       ta=*iAxes;
 220       if( ta->GetStateType() == state ) {
 221         TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
 222         delete ta;
 223         TrimAxes.erase(iAxes+1);
 224         result=true;
 225         break;
 226       }
 227       iAxes++;
 228   }
 229   return result;
 230 }
 231
 232
 233 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 234
 235 bool FGTrim::DoTrim(void) {
 236
 237   trim_failed=false;
 238   int i;
 239
 240   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 241     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
 242   }
 243
 244   fdmex->GetOutput()->Disable();
 245
 246   //clear the sub iterations counts & zero out the controls
 247   for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 248     //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
 249     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
 250     if(TrimAxes[current_axis]->GetStateType() == tQdot) {
 251       if(mode == tGround) {
 252           TrimAxes[current_axis]->initTheta();
 253       }
 254     }
 255     xlo=TrimAxes[current_axis]->GetControlMin();
 256     xhi=TrimAxes[current_axis]->GetControlMax();
 257     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
 258     TrimAxes[current_axis]->Run();
 259     //TrimAxes[current_axis]->AxisReport();
 260     sub_iterations[current_axis]=0;
 261     successful[current_axis]=0;
 262     solution[current_axis]=false;
 263   }
 264
 265
 266   if(mode == tPullup ) {
 267     cout << "Setting pitch rate and nlf... " << endl;
 268     setupPullup();
 269     cout << "pitch rate done ... " << endl;
 270     TrimAxes[0]->SetStateTarget(targetNlf);
 271     cout << "nlf done" << endl;
 272   } else if (mode == tTurn) {
 273     setupTurn();
 274     //TrimAxes[0]->SetStateTarget(targetNlf);
 275   }
 276
 277   do {
 278     axis_count=0;
 279     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 280       setDebug();
 281       updateRates();
 282       Nsub=0;
 283       if(!solution[current_axis]) {
 284         if(checkLimits()) {
 285           solution[current_axis]=true;
 286           solve();
 287         }
 288       } else if(findInterval()) {
 289         solve();
 290       } else {
 291         solution[current_axis]=false;
 292       }
 293       sub_iterations[current_axis]+=Nsub;
 294     }
 295     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 296       //these checks need to be done after all the axes have run
 297       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
 298       if(TrimAxes[current_axis]->InTolerance()) {
 299         axis_count++;
 300         successful[current_axis]++;
 301       }
 302     }
 303
 304
 305     if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
 306       //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
 307       //At this point we can check the input limits of the failed axis
 308       //and declare the trim failed if there is no sign change. If there
 309       //is, keep going until success or max iteration count
 310
 311       //Oh, well: two out of three ain't bad
 312       for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 313         //these checks need to be done after all the axes have run
 314         if(!TrimAxes[current_axis]->InTolerance()) {
 315           if(!checkLimits()) {
 316             // special case this for now -- if other cases arise proper
 317             // support can be added to FGTrimAxis
 318             if( (gamma_fallback) &&
 319                 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
 320                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
 321               cout << "  Can't trim udot with throttle, trying flight"
 322               << " path angle. (" << N << ")" << endl;
 323               if(TrimAxes[current_axis]->GetState() > 0)
 324                 TrimAxes[current_axis]->SetControlToMin();
 325               else
 326                 TrimAxes[current_axis]->SetControlToMax();
 327               TrimAxes[current_axis]->Run();
 328               delete TrimAxes[current_axis];
 329               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
 330                                                     tGamma );
 331             } else {
 332               cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
 333               << " doesn't appear to be trimmable" << endl;
 334               //total_its=k;
 335               trim_failed=true; //force the trim to fail
 336             } //gamma_fallback
 337           }
 338         } //solution check
 339       } //for loop
 340     } //all-but-one check
 341     N++;
 342     if(N > max_iterations)
 343       trim_failed=true;
 344   } while((axis_count < TrimAxes.size()) && (!trim_failed));
 345   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
 346     total_its=N;
 347     if (debug_lvl > 0)
 348         cout << endl << "  Trim successful" << endl;
 349   } else {
 350     total_its=N;
 351     if (debug_lvl > 0)
 352         cout << endl << "  Trim failed" << endl;
 353   }
 354   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 355     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
 356   }
 357   fdmex->GetOutput()->Enable();
 358   return !trim_failed;
 359 }
 360
 361 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 362
 363 bool FGTrim::solve(void) {
 364
 365   double x1,x2,x3,f1,f2,f3,d,d0;
 366   const double relax =0.9;
 367   double eps=TrimAxes[current_axis]->GetSolverEps();
 368
 369   x1=x2=x3=0;
 370   d=1;
 371   bool success=false;
 372   //initializations
 373   if( solutionDomain != 0) {
 374    /* if(ahi > alo) { */
 375       x1=xlo;f1=alo;
 376       x3=xhi;f3=ahi;
 377    /* } else {
 378       x1=xhi;f1=ahi;
 379       x3=xlo;f3=alo;
 380     }   */
 381     d0=fabs(x3-x1);
 382     //iterations
 383     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
 384     while ( (TrimAxes[current_axis]->InTolerance() == false )
 385              && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
 386       Nsub++;
 387       d=(x3-x1)/d0;
 388       x2=x1-d*d0*f1/(f3-f1);
 389       TrimAxes[current_axis]->SetControl(x2);
 390       TrimAxes[current_axis]->Run();
 391       f2=TrimAxes[current_axis]->GetState();
 392       if(Debug > 1) {
 393         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
 394         << ", " << x2 << ", " << x3 << endl;
 395         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
 396       }
 397       if(f1*f2 <= 0.0) {
 398         x3=x2;
 399         f3=f2;
 400         f1=relax*f1;
 401         //cout << "Solution is between x1 and x2" << endl;
 402       }
 403       else if(f2*f3 <= 0.0) {
 404         x1=x2;
 405         f1=f2;
 406         f3=relax*f3;
 407         //cout << "Solution is between x2 and x3" << endl;
 408
 409       }
 410       //cout << i << endl;
 411
 412
 413     }//end while
 414     if(Nsub < max_sub_iterations) success=true;
 415   }
 416   return success;
 417 }
 418
 419 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 420 /*
 421  produces an interval (xlo..xhi) on one side or the other of the current
 422  control value in which a solution exists.  This domain is, hopefully,
 423  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
 424  to find the solution. This is, hopefully, more efficient than having the
 425  solver start from scratch every time. Maybe it isn't though...
 426  This tries to take advantage of the idea that the changes from iteration to
 427  iteration will be small after the first one or two top-level iterations.
 428
 429  assumes that changing the control will a produce significant change in the
 430  accel i.e. checkLimits() has already been called.
 431
 432  if a solution is found above the current control, the function returns true
 433  and xlo is set to the current control, xhi to the interval max it found, and
 434  solutionDomain is set to 1.
 435  if the solution lies below the current control, then the function returns
 436  true and xlo is set to the interval min it found and xmax to the current
 437  control. if no solution is found, then the function returns false.
 438
 439
 440  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
 441  no assumptions about the state of the sim after this function has run
 442  can be made.
 443 */
 444 bool FGTrim::findInterval(void) {
 445   bool found=false;
 446   double step;
 447   double current_control=TrimAxes[current_axis]->GetControl();
 448   double current_accel=TrimAxes[current_axis]->GetState();;
 449   double xmin=TrimAxes[current_axis]->GetControlMin();
 450   double xmax=TrimAxes[current_axis]->GetControlMax();
 451   double lastxlo,lastxhi,lastalo,lastahi;
 452
 453   step=0.025*fabs(xmax);
 454   xlo=xhi=current_control;
 455   alo=ahi=current_accel;
 456   lastxlo=xlo;lastxhi=xhi;
 457   lastalo=alo;lastahi=ahi;
 458   do {
 459
 460     Nsub++;
 461     step*=2;
 462     xlo-=step;
 463     if(xlo < xmin) xlo=xmin;
 464     xhi+=step;
 465     if(xhi > xmax) xhi=xmax;
 466     TrimAxes[current_axis]->SetControl(xlo);
 467     TrimAxes[current_axis]->Run();
 468     alo=TrimAxes[current_axis]->GetState();
 469     TrimAxes[current_axis]->SetControl(xhi);
 470     TrimAxes[current_axis]->Run();
 471     ahi=TrimAxes[current_axis]->GetState();
 472     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
 473     if(alo*ahi <=0) {  //found interval with root
 474       found=true;
 475       if(alo*current_accel <= 0) { //narrow interval down a bit
 476         solutionDomain=-1;
 477         xhi=lastxlo;
 478         ahi=lastalo;
 479         //xhi=current_control;
 480         //ahi=current_accel;
 481       } else {
 482         solutionDomain=1;
 483         xlo=lastxhi;
 484         alo=lastahi;
 485         //xlo=current_control;
 486         //alo=current_accel;
 487       }
 488     }
 489     lastxlo=xlo;lastxhi=xhi;
 490     lastalo=alo;lastahi=ahi;
 491     if( !found && xlo==xmin && xhi==xmax ) continue;
 492     if(Debug > 1)
 493       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
 494                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
 495   } while(!found && (Nsub <= max_sub_iterations) );
 496   return found;
 497 }
 498
 499 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 500 //checks to see which side of the current control value the solution is on
 501 //and sets solutionDomain accordingly:
 502 //  1 if solution is between the current and max
 503 // -1 if solution is between the min and current
 504 //  0 if there is no solution
 505 //
 506 //if changing the control produces no significant change in the accel then
 507 //solutionDomain is set to zero and the function returns false
 508 //if a solution is found, then xlo and xhi are set so that they bracket
 509 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
 510 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
 511 //xhi=xmax and ahi=accel(xmax)
 512 //in all cases the sim is left such that the control=xmax and accel=ahi
 513
 514 bool FGTrim::checkLimits(void) {
 515   bool solutionExists;
 516   double current_control=TrimAxes[current_axis]->GetControl();
 517   double current_accel=TrimAxes[current_axis]->GetState();
 518   xlo=TrimAxes[current_axis]->GetControlMin();
 519   xhi=TrimAxes[current_axis]->GetControlMax();
 520
 521   TrimAxes[current_axis]->SetControl(xlo);
 522   TrimAxes[current_axis]->Run();
 523   alo=TrimAxes[current_axis]->GetState();
 524   TrimAxes[current_axis]->SetControl(xhi);
 525   TrimAxes[current_axis]->Run();
 526   ahi=TrimAxes[current_axis]->GetState();
 527   if(Debug > 1)
 528     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
 529                                               << alo << ", " << ahi << endl;
 530   solutionDomain=0;
 531   solutionExists=false;
 532   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
 533     if(alo*current_accel <= 0) {
 534       solutionExists=true;
 535       solutionDomain=-1;
 536       xhi=current_control;
 537       ahi=current_accel;
 538     } else if(current_accel*ahi < 0){
 539       solutionExists=true;
 540       solutionDomain=1;
 541       xlo=current_control;
 542       alo=current_accel;
 543     }
 544   }
 545   TrimAxes[current_axis]->SetControl(current_control);
 546   TrimAxes[current_axis]->Run();
 547   return solutionExists;
 548 }
 549
 550 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 551
 552 void FGTrim::setupPullup() {
 553   float g,q,cgamma;
 554   g=fdmex->GetInertial()->gravity();
 555   cgamma=cos(fgic->GetFlightPathAngleRadIC());
 556   cout << "setPitchRateInPullup():  " << g << ", " << cgamma << ", "
 557        << fgic->GetVtrueFpsIC() << endl;
 558   q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 559   cout << targetNlf << ", " << q << endl;
 560   fdmex->GetRotation()->SetPQR(0,q,0);
 561   cout << "setPitchRateInPullup() complete" << endl;
 562
 563 }
 564
 565 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 566
 567 void FGTrim::setupTurn(void){
 568   double g,phi;
 569   phi = fgic->GetRollAngleRadIC();
 570   if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
 571     targetNlf = 1 / cos(phi);
 572     g = fdmex->GetInertial()->gravity();
 573     psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
 574     cout << targetNlf << ", " << psidot << endl;
 575   }
 576
 577 }
 578
 579 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 580
 581 void FGTrim::updateRates(void){
 582   if( mode == tTurn ) {
 583     double phi = fgic->GetRollAngleRadIC();
 584     double g = fdmex->GetInertial()->gravity();
 585     double p,q,r,theta;
 586     if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
 587       theta=fgic->GetPitchAngleRadIC();
 588       phi=fgic->GetRollAngleRadIC();
 589       psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
 590       p=-psidot*sin(theta);
 591       q=psidot*cos(theta)*sin(phi);
 592       r=psidot*cos(theta)*cos(phi);
 593     } else {
 594       p=q=r=0;
 595     }
 596     fdmex->GetRotation()->SetPQR(p,q,r);
 597   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
 598       float g,q,cgamma;
 599       g=fdmex->GetInertial()->gravity();
 600       cgamma=cos(fgic->GetFlightPathAngleRadIC());
 601       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 602       fdmex->GetRotation()->SetPQR(0,q,0);
 603   }
 604 }
 605
 606 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 607
 608 void FGTrim::setDebug(void) {
 609   if(debug_axis == tAll ||
 610       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
 611     Debug=DebugLevel;
 612     return;
 613   } else {
 614     Debug=0;
 615     return;
 616   }
 617 }
 618
 619 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 620
 621 void FGTrim::SetMode(TrimMode tt) {
 622     ClearStates();
 623     mode=tt;
 624     switch(tt) {
 625       case tFull:
 626         if (debug_lvl > 0)
 627           cout << "  Full Trim" << endl;
 628         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 629         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 630         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 631         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
 632         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
 633         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 634         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 635         break;
 636       case tLongitudinal:
 637         if (debug_lvl > 0)
 638           cout << "  Longitudinal Trim" << endl;
 639         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 640         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 641         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 642         break;
 643       case tGround:
 644         if (debug_lvl > 0)
 645           cout << "  Ground Trim" << endl;
 646         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
 647         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
 648         //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
 649         break;
 650       case tPullup:
 651         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
 652         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 653         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 654         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
 655         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
 656         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 657         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 658         break;
 659       case tTurn:
 660         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 661         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 662         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 663         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
 664         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 665         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 666         break;
 667       case tCustom:
 668       case tNone:
 669         break;
 670     }
 671     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 672     sub_iterations=new double[TrimAxes.size()];
 673     successful=new double[TrimAxes.size()];
 674     solution=new bool[TrimAxes.size()];
 675     current_axis=0;
 676 }
 677 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 678 }