src/FDM/JSBSim/initialization/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 Lesser 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 Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26  HISTORY
  27 --------------------------------------------------------------------------------
  28 9/8/99   TP   Created
  29
  30 FUNCTIONAL DESCRIPTION
  31 --------------------------------------------------------------------------------
  32
  33 This class takes the given set of IC's and finds the angle of attack, elevator,
  34 and throttle setting required to fly steady level. This is currently for in-air
  35 conditions only.  It is implemented using an iterative, one-axis-at-a-time
  36 scheme. */
  37
  38 //  !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
  39
  40 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  41 INCLUDES
  42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  43
  44 #include <cstdlib>
  45 #include <iomanip>
  46 #include "FGTrim.h"
  47 #include <models/FGAtmosphere.h>
  48 #include "FGInitialCondition.h"
  49 #include <models/FGAircraft.h>
  50 #include <models/FGMassBalance.h>
  51 #include <models/FGGroundReactions.h>
  52 #include <models/FGInertial.h>
  53 #include <models/FGAerodynamics.h>
  54 #include <math/FGColumnVector3.h>
  55
  56 #if _MSC_VER
  57 #pragma warning (disable : 4786 4788)
  58 #endif
  59
  60 namespace JSBSim {
  61
  62 static const char *IdSrc = "$Id$";
  63 static const char *IdHdr = ID_TRIM;
  64
  65 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  66
  67 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
  68
  69   N=Nsub=0;
  70   max_iterations=60;
  71   max_sub_iterations=100;
  72   Tolerance=1E-3;
  73   A_Tolerance = Tolerance / 10;
  74
  75   Debug=0;DebugLevel=0;
  76   fdmex=FDMExec;
  77   fgic=fdmex->GetIC();
  78   total_its=0;
  79   trimudot=true;
  80   gamma_fallback=false;
  81   axis_count=0;
  82   mode=tt;
  83   xlo=xhi=alo=ahi=0.0;
  84   targetNlf=1.0;
  85   debug_axis=tAll;
  86   SetMode(tt);
  87   if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
  88 }
  89
  90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  91
  92 FGTrim::~FGTrim(void) {
  93   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
  94     delete TrimAxes[current_axis];
  95   }
  96   delete[] sub_iterations;
  97   delete[] successful;
  98   delete[] solution;
  99   if (debug_lvl & 2) cout << "Destroyed:    FGTrim" << endl;
 100 }
 101
 102 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 103
 104 void FGTrim::TrimStats() {
 105   int run_sum=0;
 106   cout << endl << "  Trim Statistics: " << endl;
 107   cout << "    Total Iterations: " << total_its << endl;
 108   if( total_its > 0) {
 109     cout << "    Sub-iterations:" << endl;
 110     for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
 111       run_sum += TrimAxes[current_axis]->GetRunCount();
 112       cout << "   " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
 113            << ": " << setprecision(3) << sub_iterations[current_axis]
 114            << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
 115            << "  successful:  " << setprecision(3) << successful[current_axis]
 116            << "  stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
 117            << endl;
 118     }
 119     cout << "    Run Count: " << run_sum << endl;
 120   }
 121 }
 122
 123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 124
 125 void FGTrim::Report(void) {
 126   cout << "  Trim Results: " << endl;
 127   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
 128     TrimAxes[current_axis]->AxisReport();
 129
 130 }
 131
 132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 133
 134 void FGTrim::ClearStates(void) {
 135     FGTrimAxis* ta;
 136
 137     mode=tCustom;
 138     vector<FGTrimAxis*>::iterator iAxes;
 139     iAxes = TrimAxes.begin();
 140     while (iAxes != TrimAxes.end()) {
 141       ta=*iAxes;
 142       delete ta;
 143       iAxes++;
 144     }
 145     TrimAxes.clear();
 146     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 147 }
 148
 149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 150
 151 bool FGTrim::AddState( State state, Control control ) {
 152   FGTrimAxis* ta;
 153   bool result=true;
 154
 155   mode = tCustom;
 156   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 157   while (iAxes != TrimAxes.end()) {
 158       ta=*iAxes;
 159       if( ta->GetStateType() == state )
 160         result=false;
 161       iAxes++;
 162   }
 163   if(result) {
 164     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
 165     delete[] sub_iterations;
 166     delete[] successful;
 167     delete[] solution;
 168     sub_iterations=new double[TrimAxes.size()];
 169     successful=new double[TrimAxes.size()];
 170     solution=new bool[TrimAxes.size()];
 171   }
 172   return result;
 173 }
 174
 175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 176
 177 bool FGTrim::RemoveState( State state ) {
 178   FGTrimAxis* ta;
 179   bool result=false;
 180
 181   mode = tCustom;
 182   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 183   while (iAxes != TrimAxes.end()) {
 184       ta=*iAxes;
 185       if( ta->GetStateType() == state ) {
 186         delete ta;
 187         iAxes = TrimAxes.erase(iAxes);
 188         result=true;
 189         continue;
 190       }
 191       iAxes++;
 192   }
 193   if(result) {
 194     delete[] sub_iterations;
 195     delete[] successful;
 196     delete[] solution;
 197     sub_iterations=new double[TrimAxes.size()];
 198     successful=new double[TrimAxes.size()];
 199     solution=new bool[TrimAxes.size()];
 200   }
 201   return result;
 202 }
 203
 204 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 205
 206 bool FGTrim::EditState( State state, Control new_control ){
 207   FGTrimAxis* ta;
 208   bool result=false;
 209
 210   mode = tCustom;
 211   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 212   while (iAxes != TrimAxes.end()) {
 213       ta=*iAxes;
 214       if( ta->GetStateType() == state ) {
 215         TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
 216         delete ta;
 217         TrimAxes.erase(iAxes+1);
 218         result=true;
 219         break;
 220       }
 221       iAxes++;
 222   }
 223   return result;
 224 }
 225
 226
 227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 228
 229 bool FGTrim::DoTrim(void) {
 230
 231   trim_failed=false;
 232   int i;
 233
 234   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 235     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
 236   }
 237
 238   fdmex->DisableOutput();
 239
 240   fgic->SetPRadpsIC(0.0);
 241   fgic->SetQRadpsIC(0.0);
 242   fgic->SetRRadpsIC(0.0);
 243
 244   //clear the sub iterations counts & zero out the controls
 245   for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 246     //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
 247     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
 248     if(TrimAxes[current_axis]->GetStateType() == tQdot) {
 249       if(mode == tGround) {
 250           TrimAxes[current_axis]->initTheta();
 251       }
 252     }
 253     xlo=TrimAxes[current_axis]->GetControlMin();
 254     xhi=TrimAxes[current_axis]->GetControlMax();
 255     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
 256     TrimAxes[current_axis]->Run();
 257     //TrimAxes[current_axis]->AxisReport();
 258     sub_iterations[current_axis]=0;
 259     successful[current_axis]=0;
 260     solution[current_axis]=false;
 261   }
 262
 263
 264   if(mode == tPullup ) {
 265     cout << "Setting pitch rate and nlf... " << endl;
 266     setupPullup();
 267     cout << "pitch rate done ... " << endl;
 268     TrimAxes[0]->SetStateTarget(targetNlf);
 269     cout << "nlf done" << endl;
 270   } else if (mode == tTurn) {
 271     setupTurn();
 272     //TrimAxes[0]->SetStateTarget(targetNlf);
 273   }
 274
 275   do {
 276     axis_count=0;
 277     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 278       setDebug();
 279       updateRates();
 280       Nsub=0;
 281       if(!solution[current_axis]) {
 282         if(checkLimits()) {
 283           solution[current_axis]=true;
 284           solve();
 285         }
 286       } else if(findInterval()) {
 287         solve();
 288       } else {
 289         solution[current_axis]=false;
 290       }
 291       sub_iterations[current_axis]+=Nsub;
 292     }
 293     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 294       //these checks need to be done after all the axes have run
 295       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
 296       if(TrimAxes[current_axis]->InTolerance()) {
 297         axis_count++;
 298         successful[current_axis]++;
 299       }
 300     }
 301
 302
 303     if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
 304       //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
 305       //At this point we can check the input limits of the failed axis
 306       //and declare the trim failed if there is no sign change. If there
 307       //is, keep going until success or max iteration count
 308
 309       //Oh, well: two out of three ain't bad
 310       for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 311         //these checks need to be done after all the axes have run
 312         if(!TrimAxes[current_axis]->InTolerance()) {
 313           if(!checkLimits()) {
 314             // special case this for now -- if other cases arise proper
 315             // support can be added to FGTrimAxis
 316             if( (gamma_fallback) &&
 317                 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
 318                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
 319               cout << "  Can't trim udot with throttle, trying flight"
 320               << " path angle. (" << N << ")" << endl;
 321               if(TrimAxes[current_axis]->GetState() > 0)
 322                 TrimAxes[current_axis]->SetControlToMin();
 323               else
 324                 TrimAxes[current_axis]->SetControlToMax();
 325               TrimAxes[current_axis]->Run();
 326               delete TrimAxes[current_axis];
 327               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
 328                                                     tGamma );
 329             } else {
 330               cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
 331               << " doesn't appear to be trimmable" << endl;
 332               //total_its=k;
 333               trim_failed=true; //force the trim to fail
 334             } //gamma_fallback
 335           }
 336         } //solution check
 337       } //for loop
 338     } //all-but-one check
 339     N++;
 340     if(N > max_iterations)
 341       trim_failed=true;
 342   } while((axis_count < TrimAxes.size()) && (!trim_failed));
 343   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
 344     total_its=N;
 345     if (debug_lvl > 0)
 346         cout << endl << "  Trim successful" << endl;
 347   } else {
 348     total_its=N;
 349     if (debug_lvl > 0)
 350         cout << endl << "  Trim failed" << endl;
 351   }
 352   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 353     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
 354   }
 355   fdmex->EnableOutput();
 356   return !trim_failed;
 357 }
 358
 359 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 360
 361 bool FGTrim::solve(void) {
 362
 363   double x1,x2,x3,f1,f2,f3,d,d0;
 364   const double relax =0.9;
 365   double eps=TrimAxes[current_axis]->GetSolverEps();
 366
 367   x1=x2=x3=0;
 368   d=1;
 369   bool success=false;
 370   //initializations
 371   if( solutionDomain != 0) {
 372    /* if(ahi > alo) { */
 373       x1=xlo;f1=alo;
 374       x3=xhi;f3=ahi;
 375    /* } else {
 376       x1=xhi;f1=ahi;
 377       x3=xlo;f3=alo;
 378     }   */
 379     d0=fabs(x3-x1);
 380     //iterations
 381     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
 382     while ( (TrimAxes[current_axis]->InTolerance() == false )
 383              && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
 384       Nsub++;
 385       d=(x3-x1)/d0;
 386       x2=x1-d*d0*f1/(f3-f1);
 387       TrimAxes[current_axis]->SetControl(x2);
 388       TrimAxes[current_axis]->Run();
 389       f2=TrimAxes[current_axis]->GetState();
 390       if(Debug > 1) {
 391         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
 392         << ", " << x2 << ", " << x3 << endl;
 393         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
 394       }
 395       if(f1*f2 <= 0.0) {
 396         x3=x2;
 397         f3=f2;
 398         f1=relax*f1;
 399         //cout << "Solution is between x1 and x2" << endl;
 400       }
 401       else if(f2*f3 <= 0.0) {
 402         x1=x2;
 403         f1=f2;
 404         f3=relax*f3;
 405         //cout << "Solution is between x2 and x3" << endl;
 406
 407       }
 408       //cout << i << endl;
 409
 410
 411     }//end while
 412     if(Nsub < max_sub_iterations) success=true;
 413   }
 414   return success;
 415 }
 416
 417 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 418 /*
 419  produces an interval (xlo..xhi) on one side or the other of the current
 420  control value in which a solution exists.  This domain is, hopefully,
 421  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
 422  to find the solution. This is, hopefully, more efficient than having the
 423  solver start from scratch every time. Maybe it isn't though...
 424  This tries to take advantage of the idea that the changes from iteration to
 425  iteration will be small after the first one or two top-level iterations.
 426
 427  assumes that changing the control will a produce significant change in the
 428  accel i.e. checkLimits() has already been called.
 429
 430  if a solution is found above the current control, the function returns true
 431  and xlo is set to the current control, xhi to the interval max it found, and
 432  solutionDomain is set to 1.
 433  if the solution lies below the current control, then the function returns
 434  true and xlo is set to the interval min it found and xmax to the current
 435  control. if no solution is found, then the function returns false.
 436
 437
 438  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
 439  no assumptions about the state of the sim after this function has run
 440  can be made.
 441 */
 442 bool FGTrim::findInterval(void) {
 443   bool found=false;
 444   double step;
 445   double current_control=TrimAxes[current_axis]->GetControl();
 446   double current_accel=TrimAxes[current_axis]->GetState();;
 447   double xmin=TrimAxes[current_axis]->GetControlMin();
 448   double xmax=TrimAxes[current_axis]->GetControlMax();
 449   double lastxlo,lastxhi,lastalo,lastahi;
 450
 451   step=0.025*fabs(xmax);
 452   xlo=xhi=current_control;
 453   alo=ahi=current_accel;
 454   lastxlo=xlo;lastxhi=xhi;
 455   lastalo=alo;lastahi=ahi;
 456   do {
 457
 458     Nsub++;
 459     step*=2;
 460     xlo-=step;
 461     if(xlo < xmin) xlo=xmin;
 462     xhi+=step;
 463     if(xhi > xmax) xhi=xmax;
 464     TrimAxes[current_axis]->SetControl(xlo);
 465     TrimAxes[current_axis]->Run();
 466     alo=TrimAxes[current_axis]->GetState();
 467     TrimAxes[current_axis]->SetControl(xhi);
 468     TrimAxes[current_axis]->Run();
 469     ahi=TrimAxes[current_axis]->GetState();
 470     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
 471     if(alo*ahi <=0) {  //found interval with root
 472       found=true;
 473       if(alo*current_accel <= 0) { //narrow interval down a bit
 474         solutionDomain=-1;
 475         xhi=lastxlo;
 476         ahi=lastalo;
 477         //xhi=current_control;
 478         //ahi=current_accel;
 479       } else {
 480         solutionDomain=1;
 481         xlo=lastxhi;
 482         alo=lastahi;
 483         //xlo=current_control;
 484         //alo=current_accel;
 485       }
 486     }
 487     lastxlo=xlo;lastxhi=xhi;
 488     lastalo=alo;lastahi=ahi;
 489     if( !found && xlo==xmin && xhi==xmax ) continue;
 490     if(Debug > 1)
 491       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
 492                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
 493   } while(!found && (Nsub <= max_sub_iterations) );
 494   return found;
 495 }
 496
 497 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 498 //checks to see which side of the current control value the solution is on
 499 //and sets solutionDomain accordingly:
 500 //  1 if solution is between the current and max
 501 // -1 if solution is between the min and current
 502 //  0 if there is no solution
 503 //
 504 //if changing the control produces no significant change in the accel then
 505 //solutionDomain is set to zero and the function returns false
 506 //if a solution is found, then xlo and xhi are set so that they bracket
 507 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
 508 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
 509 //xhi=xmax and ahi=accel(xmax)
 510 //in all cases the sim is left such that the control=xmax and accel=ahi
 511
 512 bool FGTrim::checkLimits(void) {
 513   bool solutionExists;
 514   double current_control=TrimAxes[current_axis]->GetControl();
 515   double current_accel=TrimAxes[current_axis]->GetState();
 516   xlo=TrimAxes[current_axis]->GetControlMin();
 517   xhi=TrimAxes[current_axis]->GetControlMax();
 518
 519   TrimAxes[current_axis]->SetControl(xlo);
 520   TrimAxes[current_axis]->Run();
 521   alo=TrimAxes[current_axis]->GetState();
 522   TrimAxes[current_axis]->SetControl(xhi);
 523   TrimAxes[current_axis]->Run();
 524   ahi=TrimAxes[current_axis]->GetState();
 525   if(Debug > 1)
 526     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
 527                                               << alo << ", " << ahi << endl;
 528   solutionDomain=0;
 529   solutionExists=false;
 530   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
 531     if(alo*current_accel <= 0) {
 532       solutionExists=true;
 533       solutionDomain=-1;
 534       xhi=current_control;
 535       ahi=current_accel;
 536     } else if(current_accel*ahi < 0){
 537       solutionExists=true;
 538       solutionDomain=1;
 539       xlo=current_control;
 540       alo=current_accel;
 541     }
 542   }
 543   TrimAxes[current_axis]->SetControl(current_control);
 544   TrimAxes[current_axis]->Run();
 545   return solutionExists;
 546 }
 547
 548 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 549
 550 void FGTrim::setupPullup() {
 551   double g,q,cgamma;
 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   fgic->SetQRadpsIC(q);
 559   cout << "setPitchRateInPullup() complete" << endl;
 560
 561 }
 562
 563 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 564
 565 void FGTrim::setupTurn(void){
 566   double g,phi;
 567   phi = fgic->GetPhiRadIC();
 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->GetPhiRadIC();
 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->GetThetaRadIC();
 586       phi=fgic->GetPhiRadIC();
 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     fgic->SetPRadpsIC(p);
 595     fgic->SetQRadpsIC(q);
 596     fgic->SetRRadpsIC(r);
 597   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
 598       double g,q,cgamma;
 599       g=fdmex->GetInertial()->gravity();
 600       cgamma=cos(fgic->GetFlightPathAngleRadIC());
 601       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 602       fgic->SetQRadpsIC(q);
 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 }