src/FDM/JSBSim/FGTrim.cpp

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