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