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