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