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\n",
 171                     fdmex->GetAuxiliary()->GetVcalibratedKTS(),
 172                     fdmex->GetState()->GetParameter(FG_MACH),
 173                     fdmex->GetPosition()->Geth() );
 174   cout << out;
 175   sprintf(out, "    Altitude: %7.0f ft.  AGL Altitude: %7.0f ft.\n",
 176                     fdmex->GetPosition()->Geth(),
 177                     fdmex->GetPosition()->GetDistanceAGL() );
 178   cout << out;
 179   sprintf(out, "    Angle of Attack: %6.2f deg  Pitch Angle: %6.2f deg\n",
 180                     fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG,
 181                     fdmex->GetRotation()->Gettht()*RADTODEG );
 182   cout << out;
 183   sprintf(out, "    Flight Path Angle: %6.2f deg  Climb Rate: %5.0f ft/min\n",
 184                     fdmex->GetPosition()->GetGamma()*RADTODEG,
 185                     fdmex->GetPosition()->Gethdot()*60 );
 186   cout << out;
 187   sprintf(out, "    Normal Load Factor: %4.2f g's  Pitch Rate: %5.2f deg/s\n",
 188                     fdmex->GetAircraft()->GetNlf(),
 189                     fdmex->GetState()->GetParameter(FG_PITCHRATE)*RADTODEG );
 190   cout << out;
 191   sprintf(out, "    Heading: %3.0f deg true  Sideslip: %5.2f deg\n",
 192                     fdmex->GetRotation()->Getpsi()*RADTODEG,
 193                     fdmex->GetState()->GetParameter(FG_BETA)*RADTODEG );
 194   cout << out;
 195   sprintf(out, "    Bank Angle: %3.0f deg\n",
 196                     fdmex->GetRotation()->Getphi()*RADTODEG );
 197   cout << out;
 198   sprintf(out, "    Elevator: %5.2f deg  Left Aileron: %5.2f deg  Rudder: %5.2f deg\n",
 199                     fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG,
 200                     fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG,
 201                     fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG );
 202   cout << out;
 203   sprintf(out, "    Throttle: %5.2f%c\n",
 204                     fdmex->GetFCS()->GetThrottlePos(0),'%' );
 205   cout << out;
 206 }
 207
 208 /******************************************************************************/
 209
 210 bool FGTrim::DoTrim(void) {
 211
 212   trim_failed=false;
 213
 214
 215   for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
 216     fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
 217   }
 218
 219   fdmex->GetOutput()->Disable();
 220
 221   //clear the sub iterations counts & zero out the controls
 222   for(current_axis=0;current_axis<NumAxes;current_axis++) {
 223     //cout << current_axis << "  " << TrimAxes[current_axis]->GetAccelName()
 224     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
 225     xlo=TrimAxes[current_axis]->GetControlMin();
 226     xhi=TrimAxes[current_axis]->GetControlMax();
 227     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
 228     TrimAxes[current_axis]->Run();
 229     //TrimAxes[current_axis]->AxisReport();
 230     sub_iterations[current_axis]=0;
 231     successful[current_axis]=0;
 232     solution[current_axis]=false;
 233   }
 234   do {
 235     axis_count=0;
 236     for(current_axis=0;current_axis<NumAxes;current_axis++) {
 237       Nsub=0;
 238       if(!solution[current_axis]) {
 239         if(checkLimits()) {
 240           solution[current_axis]=true;
 241           solve();
 242         }
 243       } else if(findInterval()) {
 244         solve();
 245       } else {
 246         solution[current_axis]=false;
 247       }
 248       sub_iterations[current_axis]+=Nsub;
 249     }
 250     for(current_axis=0;current_axis<NumAxes;current_axis++) {
 251       //these checks need to be done after all the axes have run
 252       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
 253       if(TrimAxes[current_axis]->InTolerance()) {
 254         axis_count++;
 255         successful[current_axis]++;
 256       }
 257     }
 258
 259
 260     if((axis_count == NumAxes-1) && (NumAxes > 1)) {
 261       //cout << NumAxes-1 << " out of " << NumAxes << "!" << endl;
 262       //At this point we can check the input limits of the failed axis
 263       //and declare the trim failed if there is no sign change. If there
 264       //is, keep going until success or max iteration count
 265
 266       //Oh, well: two out of three ain't bad
 267       for(current_axis=0;current_axis<NumAxes;current_axis++) {
 268         //these checks need to be done after all the axes have run
 269         if(!TrimAxes[current_axis]->InTolerance()) {
 270           if(!checkLimits()) {
 271             // special case this for now -- if other cases arise proper
 272             // support can be added to FGTrimAxis
 273             if( (gamma_fallback) &&
 274                 (TrimAxes[current_axis]->GetAccelType() == tUdot) &&
 275                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
 276               cout << "  Can't trim udot with throttle, trying flight"
 277               << " path angle. (" << N << ")" << endl;
 278               if(TrimAxes[current_axis]->GetAccel() > 0)
 279                 TrimAxes[current_axis]->SetControlToMin();
 280               else
 281                 TrimAxes[current_axis]->SetControlToMax();
 282               TrimAxes[current_axis]->Run();
 283               delete TrimAxes[current_axis];
 284               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
 285                                                     tGamma,Tolerance);
 286             } else {
 287               cout << "  Sorry, " << TrimAxes[current_axis]->GetAccelName()
 288               << " doesn't appear to be trimmable" << endl;
 289               //total_its=k;
 290               trim_failed=true; //force the trim to fail
 291             } //gamma_fallback
 292           }
 293         } //solution check
 294       } //for loop
 295     } //all-but-one check
 296     N++;
 297     if(N > max_iterations)
 298       trim_failed=true;
 299   } while((axis_count < NumAxes) && (!trim_failed));
 300   if((!trim_failed) && (axis_count >= NumAxes)) {
 301     total_its=N;
 302     cout << endl << "  Trim successful" << endl;
 303   } else {
 304     total_its=N;
 305     cout << endl << "  Trim failed" << endl;
 306   }
 307   for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
 308     fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
 309   }
 310   fdmex->GetOutput()->Enable();
 311   return !trim_failed;
 312 }
 313
 314 /******************************************************************************/
 315
 316 bool FGTrim::solve(void) {
 317
 318   float x1,x2,x3,f1,f2,f3,d,d0;
 319   const float relax =0.9;
 320   float eps=TrimAxes[current_axis]->GetSolverEps();
 321
 322   x1=x2=x3=0;
 323   d=1;
 324   bool success=false;
 325   //initializations
 326   if( solutionDomain != 0) {
 327    /* if(ahi > alo) { */
 328       x1=xlo;f1=alo;
 329       x3=xhi;f3=ahi;
 330    /* } else {
 331       x1=xhi;f1=ahi;
 332       x3=xlo;f3=alo;
 333     }   */
 334
 335     d0=fabs(x3-x1);
 336     //iterations
 337     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
 338     while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps)
 339               && (Nsub < max_sub_iterations)) {
 340       Nsub++;
 341       d=(x3-x1)/d0;
 342       x2=x1-d*d0*f1/(f3-f1);
 343       TrimAxes[current_axis]->SetControl(x2);
 344       TrimAxes[current_axis]->Run();
 345       f2=TrimAxes[current_axis]->GetAccel();
 346       if(Debug > 1) {
 347         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
 348         << ", " << x2 << ", " << x3 << endl;
 349         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
 350       }
 351       if(f1*f2 <= 0.0) {
 352         x3=x2;
 353         f3=f2;
 354         f1=relax*f1;
 355         //cout << "Solution is between x1 and x2" << endl;
 356       }
 357       else if(f2*f3 <= 0.0) {
 358         x1=x2;
 359         f1=f2;
 360         f3=relax*f3;
 361         //cout << "Solution is between x2 and x3" << endl;
 362
 363       }
 364       //cout << i << endl;
 365
 366
 367     }//end while
 368     if(Nsub < max_sub_iterations) success=true;
 369   }
 370   return success;
 371 }
 372
 373 /******************************************************************************/
 374 /*
 375  produces an interval (xlo..xhi) on one side or the other of the current
 376  control value in which a solution exists.  This domain is, hopefully,
 377  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
 378  to find the solution. This is, hopefully, more efficient than having the
 379  solver start from scratch every time. Maybe it isn't though...
 380  This tries to take advantage of the idea that the changes from iteration to
 381  iteration will be small after the first one or two top-level iterations.
 382
 383  assumes that changing the control will a produce significant change in the
 384  accel i.e. checkLimits() has already been called.
 385
 386  if a solution is found above the current control, the function returns true
 387  and xlo is set to the current control, xhi to the interval max it found, and
 388  solutionDomain is set to 1.
 389  if the solution lies below the current control, then the function returns
 390  true and xlo is set to the interval min it found and xmax to the current
 391  control. if no solution is found, then the function returns false.
 392
 393
 394  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
 395  no assumptions about the state of the sim after this function has run
 396  can be made.
 397 */
 398 bool FGTrim::findInterval(void) {
 399   bool found=false;
 400   float step;
 401   float current_control=TrimAxes[current_axis]->GetControl();
 402   float current_accel=TrimAxes[current_axis]->GetAccel();;
 403   float xmin=TrimAxes[current_axis]->GetControlMin();
 404   float xmax=TrimAxes[current_axis]->GetControlMax();
 405   float lastxlo,lastxhi,lastalo,lastahi;
 406
 407   step=0.025*fabs(xmax);
 408   xlo=xhi=current_control;
 409   alo=ahi=current_accel;
 410   lastxlo=xlo;lastxhi=xhi;
 411   lastalo=alo;lastahi=ahi;
 412   do {
 413
 414     Nsub++;
 415     step*=2;
 416     xlo-=step;
 417     if(xlo < xmin) xlo=xmin;
 418     xhi+=step;
 419     if(xhi > xmax) xhi=xmax;
 420     TrimAxes[current_axis]->SetControl(xlo);
 421     TrimAxes[current_axis]->Run();
 422     alo=TrimAxes[current_axis]->GetAccel();
 423     TrimAxes[current_axis]->SetControl(xhi);
 424     TrimAxes[current_axis]->Run();
 425     ahi=TrimAxes[current_axis]->GetAccel();
 426     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
 427     if(alo*ahi <=0) {  //found interval with root
 428       found=true;
 429       if(alo*current_accel <= 0) { //narrow interval down a bit
 430         solutionDomain=-1;
 431         xhi=lastxlo;
 432         ahi=lastalo;
 433         //xhi=current_control;
 434         //ahi=current_accel;
 435       } else {
 436         solutionDomain=1;
 437         xlo=lastxhi;
 438         alo=lastahi;
 439         //xlo=current_control;
 440         //alo=current_accel;
 441       }
 442     }
 443     lastxlo=xlo;lastxhi=xhi;
 444     lastalo=alo;lastahi=ahi;
 445     if( !found && xlo==xmin && xhi==xmax ) continue;
 446     if(Debug > 1)
 447       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
 448                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
 449   } while(!found && (Nsub <= max_sub_iterations) );
 450   return found;
 451 }
 452
 453 /******************************************************************************/
 454 //checks to see which side of the current control value the solution is on
 455 //and sets solutionDomain accordingly:
 456 //  1 if solution is between the current and max
 457 // -1 if solution is between the min and current
 458 //  0 if there is no solution
 459 //
 460 //if changing the control produces no significant change in the accel then
 461 //solutionDomain is set to zero and the function returns false
 462 //if a solution is found, then xlo and xhi are set so that they bracket
 463 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
 464 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
 465 //xhi=xmax and ahi=accel(xmax)
 466 //in all cases the sim is left such that the control=xmax and accel=ahi
 467
 468 bool FGTrim::checkLimits(void) {
 469   bool solutionExists;
 470   float current_control=TrimAxes[current_axis]->GetControl();
 471   float current_accel=TrimAxes[current_axis]->GetAccel();
 472   xlo=TrimAxes[current_axis]->GetControlMin();
 473   xhi=TrimAxes[current_axis]->GetControlMax();
 474
 475   TrimAxes[current_axis]->SetControl(xlo);
 476   TrimAxes[current_axis]->Run();
 477   alo=TrimAxes[current_axis]->GetAccel();
 478   TrimAxes[current_axis]->SetControl(xhi);
 479   TrimAxes[current_axis]->Run();
 480   ahi=TrimAxes[current_axis]->GetAccel();
 481   if(Debug > 1)
 482     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
 483                                               << alo << ", " << ahi << endl;
 484   solutionDomain=0;
 485   solutionExists=false;
 486   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
 487     if(alo*current_accel < 0) {
 488       solutionExists=true;
 489       solutionDomain=-1;
 490       xhi=current_control;
 491       ahi=current_accel;
 492     } else if(current_accel*ahi < 0){
 493       solutionExists=true;
 494       solutionDomain=1;
 495       xlo=current_control;
 496       alo=current_accel;
 497     }
 498   }
 499   TrimAxes[current_axis]->SetControl(current_control);
 500   TrimAxes[current_axis]->Run();
 501   return solutionExists;
 502 }
 503
 504
 505
 506
 507 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 508