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     xlo=TrimAxes[current_axis]->GetControlMin();
 252     xhi=TrimAxes[current_axis]->GetControlMax();
 253     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
 254     TrimAxes[current_axis]->Run();
 255     //TrimAxes[current_axis]->AxisReport();
 256     sub_iterations[current_axis]=0;
 257     successful[current_axis]=0;
 258     solution[current_axis]=false;
 259   }
 260
 261
 262   if(mode == tPullup ) {
 263     cout << "Setting pitch rate and nlf... " << endl;
 264     setupPullup();
 265     cout << "pitch rate done ... " << endl;
 266     TrimAxes[0]->SetStateTarget(targetNlf);
 267     cout << "nlf done" << endl;
 268   } else if (mode == tTurn) {
 269     setupTurn();
 270     //TrimAxes[0]->SetStateTarget(targetNlf);
 271   }
 272
 273   do {
 274     axis_count=0;
 275     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 276       setDebug();
 277       updateRates();
 278       Nsub=0;
 279       if(!solution[current_axis]) {
 280         if(checkLimits()) {
 281           solution[current_axis]=true;
 282           solve();
 283         }
 284       } else if(findInterval()) {
 285         solve();
 286       } else {
 287         solution[current_axis]=false;
 288       }
 289       sub_iterations[current_axis]+=Nsub;
 290     }
 291     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 292       //these checks need to be done after all the axes have run
 293       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
 294       if(TrimAxes[current_axis]->InTolerance()) {
 295         axis_count++;
 296         successful[current_axis]++;
 297       }
 298     }
 299
 300
 301     if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
 302       //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
 303       //At this point we can check the input limits of the failed axis
 304       //and declare the trim failed if there is no sign change. If there
 305       //is, keep going until success or max iteration count
 306
 307       //Oh, well: two out of three ain't bad
 308       for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 309         //these checks need to be done after all the axes have run
 310         if(!TrimAxes[current_axis]->InTolerance()) {
 311           if(!checkLimits()) {
 312             // special case this for now -- if other cases arise proper
 313             // support can be added to FGTrimAxis
 314             if( (gamma_fallback) &&
 315                 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
 316                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
 317               cout << "  Can't trim udot with throttle, trying flight"
 318               << " path angle. (" << N << ")" << endl;
 319               if(TrimAxes[current_axis]->GetState() > 0)
 320                 TrimAxes[current_axis]->SetControlToMin();
 321               else
 322                 TrimAxes[current_axis]->SetControlToMax();
 323               TrimAxes[current_axis]->Run();
 324               delete TrimAxes[current_axis];
 325               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
 326                                                     tGamma );
 327             } else {
 328               cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
 329               << " doesn't appear to be trimmable" << endl;
 330               //total_its=k;
 331               trim_failed=true; //force the trim to fail
 332             } //gamma_fallback
 333           }
 334         } //solution check
 335       } //for loop
 336     } //all-but-one check
 337     N++;
 338     if(N > max_iterations)
 339       trim_failed=true;
 340   } while((axis_count < TrimAxes.size()) && (!trim_failed));
 341   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
 342     total_its=N;
 343     cout << endl << "  Trim successful" << endl;
 344   } else {
 345     total_its=N;
 346     cout << endl << "  Trim failed" << endl;
 347   }
 348   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 349     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
 350   }
 351   fdmex->GetOutput()->Enable();
 352   return !trim_failed;
 353 }
 354
 355 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 356
 357 bool FGTrim::solve(void) {
 358
 359   double x1,x2,x3,f1,f2,f3,d,d0;
 360   const double relax =0.9;
 361   double eps=TrimAxes[current_axis]->GetSolverEps();
 362
 363   x1=x2=x3=0;
 364   d=1;
 365   bool success=false;
 366   //initializations
 367   if( solutionDomain != 0) {
 368    /* if(ahi > alo) { */
 369       x1=xlo;f1=alo;
 370       x3=xhi;f3=ahi;
 371    /* } else {
 372       x1=xhi;f1=ahi;
 373       x3=xlo;f3=alo;
 374     }   */
 375     d0=fabs(x3-x1);
 376     //iterations
 377     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
 378     while ( (TrimAxes[current_axis]->InTolerance() == false )
 379              && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
 380       Nsub++;
 381       d=(x3-x1)/d0;
 382       x2=x1-d*d0*f1/(f3-f1);
 383       TrimAxes[current_axis]->SetControl(x2);
 384       TrimAxes[current_axis]->Run();
 385       f2=TrimAxes[current_axis]->GetState();
 386       if(Debug > 1) {
 387         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
 388         << ", " << x2 << ", " << x3 << endl;
 389         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
 390       }
 391       if(f1*f2 <= 0.0) {
 392         x3=x2;
 393         f3=f2;
 394         f1=relax*f1;
 395         //cout << "Solution is between x1 and x2" << endl;
 396       }
 397       else if(f2*f3 <= 0.0) {
 398         x1=x2;
 399         f1=f2;
 400         f3=relax*f3;
 401         //cout << "Solution is between x2 and x3" << endl;
 402
 403       }
 404       //cout << i << endl;
 405
 406
 407     }//end while
 408     if(Nsub < max_sub_iterations) success=true;
 409   }
 410   return success;
 411 }
 412
 413 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 414 /*
 415  produces an interval (xlo..xhi) on one side or the other of the current
 416  control value in which a solution exists.  This domain is, hopefully,
 417  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
 418  to find the solution. This is, hopefully, more efficient than having the
 419  solver start from scratch every time. Maybe it isn't though...
 420  This tries to take advantage of the idea that the changes from iteration to
 421  iteration will be small after the first one or two top-level iterations.
 422
 423  assumes that changing the control will a produce significant change in the
 424  accel i.e. checkLimits() has already been called.
 425
 426  if a solution is found above the current control, the function returns true
 427  and xlo is set to the current control, xhi to the interval max it found, and
 428  solutionDomain is set to 1.
 429  if the solution lies below the current control, then the function returns
 430  true and xlo is set to the interval min it found and xmax to the current
 431  control. if no solution is found, then the function returns false.
 432
 433
 434  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
 435  no assumptions about the state of the sim after this function has run
 436  can be made.
 437 */
 438 bool FGTrim::findInterval(void) {
 439   bool found=false;
 440   double step;
 441   double current_control=TrimAxes[current_axis]->GetControl();
 442   double current_accel=TrimAxes[current_axis]->GetState();;
 443   double xmin=TrimAxes[current_axis]->GetControlMin();
 444   double xmax=TrimAxes[current_axis]->GetControlMax();
 445   double lastxlo,lastxhi,lastalo,lastahi;
 446
 447   step=0.025*fabs(xmax);
 448   xlo=xhi=current_control;
 449   alo=ahi=current_accel;
 450   lastxlo=xlo;lastxhi=xhi;
 451   lastalo=alo;lastahi=ahi;
 452   do {
 453
 454     Nsub++;
 455     step*=2;
 456     xlo-=step;
 457     if(xlo < xmin) xlo=xmin;
 458     xhi+=step;
 459     if(xhi > xmax) xhi=xmax;
 460     TrimAxes[current_axis]->SetControl(xlo);
 461     TrimAxes[current_axis]->Run();
 462     alo=TrimAxes[current_axis]->GetState();
 463     TrimAxes[current_axis]->SetControl(xhi);
 464     TrimAxes[current_axis]->Run();
 465     ahi=TrimAxes[current_axis]->GetState();
 466     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
 467     if(alo*ahi <=0) {  //found interval with root
 468       found=true;
 469       if(alo*current_accel <= 0) { //narrow interval down a bit
 470         solutionDomain=-1;
 471         xhi=lastxlo;
 472         ahi=lastalo;
 473         //xhi=current_control;
 474         //ahi=current_accel;
 475       } else {
 476         solutionDomain=1;
 477         xlo=lastxhi;
 478         alo=lastahi;
 479         //xlo=current_control;
 480         //alo=current_accel;
 481       }
 482     }
 483     lastxlo=xlo;lastxhi=xhi;
 484     lastalo=alo;lastahi=ahi;
 485     if( !found && xlo==xmin && xhi==xmax ) continue;
 486     if(Debug > 1)
 487       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
 488                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
 489   } while(!found && (Nsub <= max_sub_iterations) );
 490   return found;
 491 }
 492
 493 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 494 //checks to see which side of the current control value the solution is on
 495 //and sets solutionDomain accordingly:
 496 //  1 if solution is between the current and max
 497 // -1 if solution is between the min and current
 498 //  0 if there is no solution
 499 //
 500 //if changing the control produces no significant change in the accel then
 501 //solutionDomain is set to zero and the function returns false
 502 //if a solution is found, then xlo and xhi are set so that they bracket
 503 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
 504 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
 505 //xhi=xmax and ahi=accel(xmax)
 506 //in all cases the sim is left such that the control=xmax and accel=ahi
 507
 508 bool FGTrim::checkLimits(void) {
 509   bool solutionExists;
 510   double current_control=TrimAxes[current_axis]->GetControl();
 511   double current_accel=TrimAxes[current_axis]->GetState();
 512   xlo=TrimAxes[current_axis]->GetControlMin();
 513   xhi=TrimAxes[current_axis]->GetControlMax();
 514
 515   TrimAxes[current_axis]->SetControl(xlo);
 516   TrimAxes[current_axis]->Run();
 517   alo=TrimAxes[current_axis]->GetState();
 518   TrimAxes[current_axis]->SetControl(xhi);
 519   TrimAxes[current_axis]->Run();
 520   ahi=TrimAxes[current_axis]->GetState();
 521   if(Debug > 1)
 522     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
 523                                               << alo << ", " << ahi << endl;
 524   solutionDomain=0;
 525   solutionExists=false;
 526   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
 527     if(alo*current_accel <= 0) {
 528       solutionExists=true;
 529       solutionDomain=-1;
 530       xhi=current_control;
 531       ahi=current_accel;
 532     } else if(current_accel*ahi < 0){
 533       solutionExists=true;
 534       solutionDomain=1;
 535       xlo=current_control;
 536       alo=current_accel;
 537     }
 538   }
 539   TrimAxes[current_axis]->SetControl(current_control);
 540   TrimAxes[current_axis]->Run();
 541   return solutionExists;
 542 }
 543
 544 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 545
 546 void FGTrim::setupPullup() {
 547   float g,q,cgamma;
 548   FGColumnVector3 vPQR;
 549   g=fdmex->GetInertial()->gravity();
 550   cgamma=cos(fgic->GetFlightPathAngleRadIC());
 551   cout << "setPitchRateInPullup():  " << g << ", " << cgamma << ", "
 552        << fgic->GetVtrueFpsIC() << endl;
 553   q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 554   cout << targetNlf << ", " << q << endl;
 555   fdmex->GetRotation()->SetPQR(0,q,0);
 556   cout << "setPitchRateInPullup() complete" << endl;
 557
 558 }
 559
 560 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 561
 562 void FGTrim::setupTurn(void){
 563   double g,phi;
 564   phi = fgic->GetRollAngleRadIC();
 565   if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
 566     targetNlf = 1 / cos(phi);
 567     g = fdmex->GetInertial()->gravity();
 568     psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
 569     cout << targetNlf << ", " << psidot << endl;
 570   }
 571
 572 }
 573
 574 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 575
 576 void FGTrim::updateRates(void){
 577   if( mode == tTurn ) {
 578     double phi = fgic->GetRollAngleRadIC();
 579     double g = fdmex->GetInertial()->gravity();
 580     double p,q,r,theta;
 581     if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
 582       theta=fgic->GetPitchAngleRadIC();
 583       phi=fgic->GetRollAngleRadIC();
 584       psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
 585       p=-psidot*sin(theta);
 586       q=psidot*cos(theta)*sin(phi);
 587       r=psidot*cos(theta)*cos(phi);
 588     } else {
 589       p=q=r=0;
 590     }
 591     fdmex->GetRotation()->SetPQR(p,q,r);
 592   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
 593       float g,q,cgamma;
 594       FGColumnVector3 vPQR;
 595       g=fdmex->GetInertial()->gravity();
 596       cgamma=cos(fgic->GetFlightPathAngleRadIC());
 597       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 598       fdmex->GetRotation()->SetPQR(0,q,0);
 599   }
 600 }
 601
 602 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 603
 604 void FGTrim::setDebug(void) {
 605   if(debug_axis == tAll ||
 606       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
 607     Debug=DebugLevel;
 608     return;
 609   } else {
 610     Debug=0;
 611     return;
 612   }
 613 }
 614
 615 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 616
 617 void FGTrim::SetMode(TrimMode tt) {
 618     ClearStates();
 619     switch(tt) {
 620       case tFull:
 621         cout << "  Full Trim" << endl;
 622         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 623         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 624         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 625         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
 626         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
 627         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 628         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 629         break;
 630       case tLongitudinal:
 631         cout << "  Longitudinal Trim" << endl;
 632         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 633         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 634         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 635         break;
 636       case tGround:
 637         cout << "  Ground Trim" << endl;
 638         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
 639         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
 640         //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
 641         break;
 642       case tPullup:
 643         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
 644         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 645         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 646         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
 647         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
 648         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 649         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 650         break;
 651       case tTurn:
 652         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 653         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 654         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 655         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
 656         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 657         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 658         break;
 659       case tCustom:
 660       case tNone:
 661         break;
 662     }
 663     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 664     sub_iterations=new double[TrimAxes.size()];
 665     successful=new double[TrimAxes.size()];
 666     solution=new bool[TrimAxes.size()];
 667     current_axis=0;
 668 }
 669 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 670 }