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
  55 /*******************************************************************************/
  56
  57 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
  58
  59   N=Nsub=0;
  60   max_iterations=60;
  61   max_sub_iterations=100;
  62   Tolerance=1E-3;
  63   A_Tolerance = Tolerance / 10;
  64
  65   Debug=0;
  66   fdmex=FDMExec;
  67   fgic=FGIC;
  68   total_its=0;
  69   trimudot=true;
  70   gamma_fallback=true;
  71   axis_count=0;
  72   mode=tt;
  73   xlo=xhi=alo=ahi;
  74   switch(mode) {
  75   case tFull:
  76     cout << "  Full 6-DOF Trim" << endl;
  77     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha,Tolerance));
  78     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle,Tolerance));
  79     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim,A_Tolerance));
  80     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi,Tolerance));
  81     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron,A_Tolerance));
  82     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder,A_Tolerance));
  83     break;
  84   case tLongitudinal:
  85     cout << "  Longitudinal Trim" << endl;
  86     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha,Tolerance));
  87     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle,Tolerance));
  88     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim,A_Tolerance));
  89     break;
  90   case tGround:
  91     cout << "  Ground Trim" << endl;
  92     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL,Tolerance));
  93     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta,A_Tolerance));
  94     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi,A_Tolerance));
  95     break;
  96   }
  97   //cout << "NumAxes: " << TrimAxes.size() << endl;
  98   NumAxes=TrimAxes.size();
  99   sub_iterations=new float[NumAxes];
 100   successful=new float[NumAxes];
 101   solution=new bool[NumAxes];
 102   current_axis=0;
 103 }
 104
 105 /******************************************************************************/
 106
 107 FGTrim::~FGTrim(void) {
 108   for(current_axis=0; current_axis<NumAxes; current_axis++) {
 109     delete TrimAxes[current_axis];
 110   }
 111   delete[] sub_iterations;
 112   delete[] successful;
 113   delete[] solution;
 114 }
 115
 116 /******************************************************************************/
 117
 118 void FGTrim::TrimStats() {
 119   char out[80];
 120   int run_sum=0;
 121   cout << endl << "  Trim Statistics: " << endl;
 122   cout << "    Total Iterations: " << total_its << endl;
 123   if(total_its > 0) {
 124     cout << "    Sub-iterations:" << endl;
 125     for(current_axis=0; current_axis<NumAxes; current_axis++) {
 126       run_sum+=TrimAxes[current_axis]->GetRunCount();
 127       sprintf(out,"   %5s: %3.0f average: %5.2f  successful: %3.0f  stability: %5.2f\n",
 128                   TrimAxes[current_axis]->GetAccelName().c_str(),
 129                   sub_iterations[current_axis],
 130                   sub_iterations[current_axis]/float(total_its),
 131                   successful[current_axis],
 132                   TrimAxes[current_axis]->GetAvgStability() );
 133       cout << out;
 134     }
 135     cout << "    Run Count: " << run_sum << endl;
 136   }
 137 }
 138
 139 /******************************************************************************/
 140
 141 void FGTrim::Report(void) {
 142   cout << "  Trim Results: " << endl;
 143   for(current_axis=0; current_axis<NumAxes; current_axis++)
 144     TrimAxes[current_axis]->AxisReport();
 145
 146 }
 147
 148 /******************************************************************************/
 149
 150 void FGTrim::ReportState(void) {
 151   char out[80], flap[10], gear[10];
 152
 153   cout << endl << "  JSBSim State" << endl;
 154   sprintf(out,"    Weight: %7.0f lbs.  CG: %5.1f, %5.1f, %5.1f inches\n",
 155                    fdmex->GetAircraft()->GetWeight(),
 156                    fdmex->GetAircraft()->GetXYZcg()(1),
 157                    fdmex->GetAircraft()->GetXYZcg()(2),
 158                    fdmex->GetAircraft()->GetXYZcg()(3) );
 159   cout << out;
 160   if( fdmex->GetFCS()->GetDfPos() <= 0.01)
 161     sprintf(flap,"Up");
 162   else
 163     sprintf(flap,"%2.0f",fdmex->GetFCS()->GetDfPos());
 164   if(fdmex->GetAircraft()->GetGearUp() == true)
 165     sprintf(gear,"Up");
 166   else
 167     sprintf(gear,"Down");
 168   sprintf(out, "    Flaps: %3s  Gear: %4s\n",flap,gear);
 169   cout << out;
 170   sprintf(out, "    Speed: %4.0f KCAS  Mach: %5.2f  Altitude: %7.0f ft.\n",
 171                     fdmex->GetAuxiliary()->GetVcalibratedKTS(),
 172                     fdmex->GetState()->GetParameter(FG_MACH),
 173                     fdmex->GetPosition()->Geth() );
 174   cout << out;
 175   sprintf(out, "    Angle of Attack: %6.2f deg  Pitch Angle: %6.2f deg\n",
 176                     fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG,
 177                     fdmex->GetRotation()->Gettht()*RADTODEG );
 178   cout << out;
 179   sprintf(out, "    Flight Path Angle: %6.2f deg  Climb Rate: %5.0f ft/min\n",
 180                     fdmex->GetPosition()->GetGamma()*RADTODEG,
 181                     fdmex->GetPosition()->Gethdot()*60 );
 182   cout << out;
 183   sprintf(out, "    Normal Load Factor: %4.2f g's  Pitch Rate: %5.2f deg/s\n",
 184                     fdmex->GetAircraft()->GetNlf(),
 185                     fdmex->GetState()->GetParameter(FG_PITCHRATE)*RADTODEG );
 186   cout << out;
 187   sprintf(out, "    Heading: %3.0f deg true  Sideslip: %5.2f deg\n",
 188                     fdmex->GetRotation()->Getpsi()*RADTODEG,
 189                     fdmex->GetState()->GetParameter(FG_BETA)*RADTODEG );
 190   cout << out;
 191   sprintf(out, "    Bank Angle: %3.0f deg\n",
 192                     fdmex->GetRotation()->Getphi()*RADTODEG );
 193   cout << out;
 194   sprintf(out, "    Elevator: %5.2f deg  Left Aileron: %5.2f deg  Rudder: %5.2f deg\n",
 195                     fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG,
 196                     fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG,
 197                     fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG );
 198   cout << out;
 199   sprintf(out, "    Throttle: %5.2f%c\n",
 200                     fdmex->GetFCS()->GetThrottlePos(0),'%' );
 201   cout << out;
 202 }
 203
 204 /******************************************************************************/
 205
 206 bool FGTrim::DoTrim(void) {
 207
 208   trim_failed=false;
 209
 210
 211   for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
 212     fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
 213   }
 214
 215   fdmex->GetOutput()->Disable();
 216
 217   //clear the sub iterations counts & zero out the controls
 218   for(current_axis=0;current_axis<NumAxes;current_axis++) {
 219     //cout << current_axis << "  " << TrimAxes[current_axis]->GetAccelName()
 220     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
 221     xlo=TrimAxes[current_axis]->GetControlMin();
 222     xhi=TrimAxes[current_axis]->GetControlMax();
 223     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
 224     TrimAxes[current_axis]->Run();
 225     //TrimAxes[current_axis]->AxisReport();
 226     sub_iterations[current_axis]=0;
 227     successful[current_axis]=0;
 228     solution[current_axis]=false;
 229   }
 230   do {
 231     axis_count=0;
 232     for(current_axis=0;current_axis<NumAxes;current_axis++) {
 233       Nsub=0;
 234       if(!solution[current_axis]) {
 235         if(checkLimits()) {
 236           solution[current_axis]=true;
 237           solve();
 238         }
 239       } else if(findInterval()) {
 240         solve();
 241       } else {
 242         solution[current_axis]=false;
 243       }
 244       sub_iterations[current_axis]+=Nsub;
 245     }
 246     for(current_axis=0;current_axis<NumAxes;current_axis++) {
 247       //these checks need to be done after all the axes have run
 248       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
 249       if(TrimAxes[current_axis]->InTolerance()) {
 250         axis_count++;
 251         successful[current_axis]++;
 252       }
 253     }
 254
 255
 256     if((axis_count == NumAxes-1) && (NumAxes > 1)) {
 257       //cout << NumAxes-1 << " out of " << NumAxes << "!" << endl;
 258       //At this point we can check the input limits of the failed axis
 259       //and declare the trim failed if there is no sign change. If there
 260       //is, keep going until success or max iteration count
 261
 262       //Oh, well: two out of three ain't bad
 263       for(current_axis=0;current_axis<NumAxes;current_axis++) {
 264         //these checks need to be done after all the axes have run
 265         if(!TrimAxes[current_axis]->InTolerance()) {
 266           if(!checkLimits()) {
 267             // special case this for now -- if other cases arise proper
 268             // support can be added to FGTrimAxis
 269             if( (gamma_fallback) &&
 270                 (TrimAxes[current_axis]->GetAccelType() == tUdot) &&
 271                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
 272               cout << "  Can't trim udot with throttle, trying flight"
 273               << " path angle. (" << N << ")" << endl;
 274               if(TrimAxes[current_axis]->GetAccel() > 0)
 275                 TrimAxes[current_axis]->SetControlToMin();
 276               else
 277                 TrimAxes[current_axis]->SetControlToMax();
 278               TrimAxes[current_axis]->Run();
 279               delete TrimAxes[current_axis];
 280               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
 281                                                     tGamma,Tolerance);
 282             } else {
 283               cout << "  Sorry, " << TrimAxes[current_axis]->GetAccelName()
 284               << " doesn't appear to be trimmable" << endl;
 285               //total_its=k;
 286               trim_failed=true; //force the trim to fail
 287             } //gamma_fallback
 288           }
 289         } //solution check
 290       } //for loop
 291     } //all-but-one check
 292     N++;
 293     if(N > max_iterations)
 294       trim_failed=true;
 295   } while((axis_count < NumAxes) && (!trim_failed));
 296   if((!trim_failed) && (axis_count >= NumAxes)) {
 297     total_its=N;
 298     cout << endl << "  Trim successful" << endl;
 299   } else {
 300     total_its=N;
 301     cout << endl << "  Trim failed" << endl;
 302   }
 303   for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
 304     fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
 305   }
 306   fdmex->GetOutput()->Enable();
 307   return !trim_failed;
 308 }
 309
 310 /******************************************************************************/
 311
 312 bool FGTrim::solve(void) {
 313
 314   float x1,x2,x3,f1,f2,f3,d,d0;
 315   const float relax =0.9;
 316   float eps=TrimAxes[current_axis]->GetSolverEps();
 317
 318   x1=x2=x3=0;
 319   d=1;
 320   bool success=false;
 321   //initializations
 322   if( solutionDomain != 0) {
 323    /* if(ahi > alo) { */
 324       x1=xlo;f1=alo;
 325       x3=xhi;f3=ahi;
 326    /* } else {
 327       x1=xhi;f1=ahi;
 328       x3=xlo;f3=alo;
 329     }   */
 330
 331     d0=fabs(x3-x1);
 332     //iterations
 333     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
 334     while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps)
 335               && (Nsub < max_sub_iterations)) {
 336       Nsub++;
 337       d=(x3-x1)/d0;
 338       x2=x1-d*d0*f1/(f3-f1);
 339       TrimAxes[current_axis]->SetControl(x2);
 340       TrimAxes[current_axis]->Run();
 341       f2=TrimAxes[current_axis]->GetAccel();
 342       if(Debug > 1) {
 343         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
 344         << ", " << x2 << ", " << x3 << endl;
 345         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
 346       }
 347       if(f1*f2 <= 0.0) {
 348         x3=x2;
 349         f3=f2;
 350         f1=relax*f1;
 351         //cout << "Solution is between x1 and x2" << endl;
 352       }
 353       else if(f2*f3 <= 0.0) {
 354         x1=x2;
 355         f1=f2;
 356         f3=relax*f3;
 357         //cout << "Solution is between x2 and x3" << endl;
 358
 359       }
 360       //cout << i << endl;
 361
 362
 363     }//end while
 364     if(Nsub < max_sub_iterations) success=true;
 365   }
 366   return success;
 367 }
 368
 369 /******************************************************************************/
 370 /*
 371  produces an interval (xlo..xhi) on one side or the other of the current
 372  control value in which a solution exists.  This domain is, hopefully,
 373  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
 374  to find the solution. This is, hopefully, more efficient than having the
 375  solver start from scratch every time. Maybe it isn't though...
 376  This tries to take advantage of the idea that the changes from iteration to
 377  iteration will be small after the first one or two top-level iterations.
 378
 379  assumes that changing the control will a produce significant change in the
 380  accel i.e. checkLimits() has already been called.
 381
 382  if a solution is found above the current control, the function returns true
 383  and xlo is set to the current control, xhi to the interval max it found, and
 384  solutionDomain is set to 1.
 385  if the solution lies below the current control, then the function returns
 386  true and xlo is set to the interval min it found and xmax to the current
 387  control. if no solution is found, then the function returns false.
 388
 389
 390  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
 391  no assumptions about the state of the sim after this function has run
 392  can be made.
 393 */
 394 bool FGTrim::findInterval(void) {
 395   bool found=false;
 396   float step;
 397   float current_control=TrimAxes[current_axis]->GetControl();
 398   float current_accel=TrimAxes[current_axis]->GetAccel();;
 399   float xmin=TrimAxes[current_axis]->GetControlMin();
 400   float xmax=TrimAxes[current_axis]->GetControlMax();
 401   float lastxlo,lastxhi,lastalo,lastahi;
 402
 403   step=0.025*fabs(xmax);
 404   xlo=xhi=current_control;
 405   alo=ahi=current_accel;
 406   lastxlo=xlo;lastxhi=xhi;
 407   lastalo=alo;lastahi=ahi;
 408   do {
 409
 410     Nsub++;
 411     step*=2;
 412     xlo-=step;
 413     if(xlo < xmin) xlo=xmin;
 414     xhi+=step;
 415     if(xhi > xmax) xhi=xmax;
 416     TrimAxes[current_axis]->SetControl(xlo);
 417     TrimAxes[current_axis]->Run();
 418     alo=TrimAxes[current_axis]->GetAccel();
 419     TrimAxes[current_axis]->SetControl(xhi);
 420     TrimAxes[current_axis]->Run();
 421     ahi=TrimAxes[current_axis]->GetAccel();
 422     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
 423     if(alo*ahi <=0) {  //found interval with root
 424       found=true;
 425       if(alo*current_accel <= 0) { //narrow interval down a bit
 426         solutionDomain=-1;
 427         xhi=lastxlo;
 428         ahi=lastalo;
 429         //xhi=current_control;
 430         //ahi=current_accel;
 431       } else {
 432         solutionDomain=1;
 433         xlo=lastxhi;
 434         alo=lastahi;
 435         //xlo=current_control;
 436         //alo=current_accel;
 437       }
 438     }
 439     lastxlo=xlo;lastxhi=xhi;
 440     lastalo=alo;lastahi=ahi;
 441     if( !found && xlo==xmin && xhi==xmax ) continue;
 442     if(Debug > 1)
 443       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
 444                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
 445   } while(!found && (Nsub <= max_sub_iterations) );
 446   return found;
 447 }
 448
 449 /******************************************************************************/
 450 //checks to see which side of the current control value the solution is on
 451 //and sets solutionDomain accordingly:
 452 //  1 if solution is between the current and max
 453 // -1 if solution is between the min and current
 454 //  0 if there is no solution
 455 //
 456 //if changing the control produces no significant change in the accel then
 457 //solutionDomain is set to zero and the function returns false
 458 //if a solution is found, then xlo and xhi are set so that they bracket
 459 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
 460 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
 461 //xhi=xmax and ahi=accel(xmax)
 462 //in all cases the sim is left such that the control=xmax and accel=ahi
 463
 464 bool FGTrim::checkLimits(void) {
 465   bool solutionExists;
 466   float current_control=TrimAxes[current_axis]->GetControl();
 467   float current_accel=TrimAxes[current_axis]->GetAccel();
 468   xlo=TrimAxes[current_axis]->GetControlMin();
 469   xhi=TrimAxes[current_axis]->GetControlMax();
 470
 471   TrimAxes[current_axis]->SetControl(xlo);
 472   TrimAxes[current_axis]->Run();
 473   alo=TrimAxes[current_axis]->GetAccel();
 474   TrimAxes[current_axis]->SetControl(xhi);
 475   TrimAxes[current_axis]->Run();
 476   ahi=TrimAxes[current_axis]->GetAccel();
 477   if(Debug > 1)
 478     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
 479                                               << alo << ", " << ahi << endl;
 480   solutionDomain=0;
 481   solutionExists=false;
 482   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
 483     if(alo*current_accel < 0) {
 484       solutionExists=true;
 485       solutionDomain=-1;
 486       xhi=current_control;
 487       ahi=current_accel;
 488     } else if(current_accel*ahi < 0){
 489       solutionExists=true;
 490       solutionDomain=1;
 491       xlo=current_control;
 492       alo=current_accel;
 493     }
 494   }
 495   TrimAxes[current_axis]->SetControl(current_control);
 496   TrimAxes[current_axis]->Run();
 497   return solutionExists;
 498 }
 499
 500
 501
 502
 503 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 504