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