src/FDM/JSBSim/FGTrim.cpp

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