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