-/*******************************************************************************
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Header: FGTrim.cpp
Author: Tony Peden
// !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
-/*******************************************************************************
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
-*******************************************************************************/
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include <stdlib.h>
#include "FGInitialCondition.h"
#include "FGTrim.h"
#include "FGAircraft.h"
+#include "FGMassBalance.h"
+#include "FGAerodynamics.h"
+#include "FGColumnVector3.h"
+#if _MSC_VER
+#pragma warning (disable : 4786 4788)
+#endif
-/*******************************************************************************/
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_TRIM;
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
Tolerance=1E-3;
A_Tolerance = Tolerance / 10;
- Debug=0;
+ Debug=0;DebugLevel=0;
fdmex=FDMExec;
fgic=FGIC;
total_its=0;
axis_count=0;
mode=tt;
xlo=xhi=alo=ahi;
+ targetNlf=1.0;
+ debug_axis=tAll;
switch(mode) {
case tFull:
- cout << " Full 6-DOF Trim" << endl;
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha,Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle,Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim,A_Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi,Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron,A_Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder,A_Tolerance));
+ cout << " Full Trim" << endl;
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
break;
case tLongitudinal:
cout << " Longitudinal Trim" << endl;
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha,Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle,Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim,A_Tolerance));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
break;
case tGround:
cout << " Ground Trim" << endl;
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL,Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta,A_Tolerance));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi,A_Tolerance));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
+ //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
break;
- }
- //cout << "NumAxes: " << TrimAxes.size() << endl;
- NumAxes=TrimAxes.size();
- sub_iterations=new float[NumAxes];
- successful=new float[NumAxes];
- solution=new bool[NumAxes];
+ case tPullup:
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+ break;
+ case tTurn:
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+ break;
+ case tCustom:
+ case tNone:
+ break;
+}
+ //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+ sub_iterations=new double[TrimAxes.size()];
+ successful=new double[TrimAxes.size()];
+ solution=new bool[TrimAxes.size()];
current_axis=0;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGTrim::~FGTrim(void) {
- for(current_axis=0; current_axis<NumAxes; current_axis++) {
+ for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
delete TrimAxes[current_axis];
}
delete[] sub_iterations;
delete[] successful;
delete[] solution;
+ if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::TrimStats() {
char out[80];
cout << " Total Iterations: " << total_its << endl;
if(total_its > 0) {
cout << " Sub-iterations:" << endl;
- for(current_axis=0; current_axis<NumAxes; current_axis++) {
+ for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
run_sum+=TrimAxes[current_axis]->GetRunCount();
- sprintf(out," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
- TrimAxes[current_axis]->GetAccelName().c_str(),
+ snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
+ TrimAxes[current_axis]->GetStateName().c_str(),
sub_iterations[current_axis],
- sub_iterations[current_axis]/float(total_its),
+ sub_iterations[current_axis]/double(total_its),
successful[current_axis],
TrimAxes[current_axis]->GetAvgStability() );
cout << out;
}
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::Report(void) {
cout << " Trim Results: " << endl;
- for(current_axis=0; current_axis<NumAxes; current_axis++)
+ for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
TrimAxes[current_axis]->AxisReport();
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGTrim::ReportState(void) {
- char out[80], flap[10], gear[10];
-
- cout << endl << " JSBSim State" << endl;
- sprintf(out," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n",
- fdmex->GetAircraft()->GetWeight(),
- fdmex->GetAircraft()->GetXYZcg()(1),
- fdmex->GetAircraft()->GetXYZcg()(2),
- fdmex->GetAircraft()->GetXYZcg()(3) );
- cout << out;
- if( fdmex->GetFCS()->GetDfPos() <= 0.01)
- sprintf(flap,"Up");
- else
- sprintf(flap,"%2.0f",fdmex->GetFCS()->GetDfPos());
- if(fdmex->GetAircraft()->GetGearUp() == true)
- sprintf(gear,"Up");
- else
- sprintf(gear,"Down");
- sprintf(out, " Flaps: %3s Gear: %4s\n",flap,gear);
- cout << out;
- sprintf(out, " Speed: %4.0f KCAS Mach: %5.2f\n",
- fdmex->GetAuxiliary()->GetVcalibratedKTS(),
- fdmex->GetState()->GetParameter(FG_MACH),
- fdmex->GetPosition()->Geth() );
- cout << out;
- sprintf(out, " Altitude: %7.0f ft. AGL Altitude: %7.0f ft.\n",
- fdmex->GetPosition()->Geth(),
- fdmex->GetPosition()->GetDistanceAGL() );
- cout << out;
- sprintf(out, " Angle of Attack: %6.2f deg Pitch Angle: %6.2f deg\n",
- fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG,
- fdmex->GetRotation()->Gettht()*RADTODEG );
- cout << out;
- sprintf(out, " Flight Path Angle: %6.2f deg Climb Rate: %5.0f ft/min\n",
- fdmex->GetPosition()->GetGamma()*RADTODEG,
- fdmex->GetPosition()->Gethdot()*60 );
- cout << out;
- sprintf(out, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n",
- fdmex->GetAircraft()->GetNlf(),
- fdmex->GetState()->GetParameter(FG_PITCHRATE)*RADTODEG );
- cout << out;
- sprintf(out, " Heading: %3.0f deg true Sideslip: %5.2f deg\n",
- fdmex->GetRotation()->Getpsi()*RADTODEG,
- fdmex->GetState()->GetParameter(FG_BETA)*RADTODEG );
- cout << out;
- sprintf(out, " Bank Angle: %3.0f deg\n",
- fdmex->GetRotation()->Getphi()*RADTODEG );
- cout << out;
- sprintf(out, " Elevator: %5.2f deg Left Aileron: %5.2f deg Rudder: %5.2f deg\n",
- fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG,
- fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG,
- fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG );
- cout << out;
- sprintf(out, " Throttle: %5.2f%c\n",
- fdmex->GetFCS()->GetThrottlePos(0),'%' );
- cout << out;
+void FGTrim::ClearStates(void) {
+ FGTrimAxis* ta;
+
+ mode=tCustom;
+ vector<FGTrimAxis*>::iterator iAxes;
+ iAxes = TrimAxes.begin();
+ while (iAxes != TrimAxes.end()) {
+ ta=*iAxes;
+ delete ta;
+ iAxes++;
+ }
+ TrimAxes.clear();
+ cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGTrim::AddState( State state, Control control ) {
+ FGTrimAxis* ta;
+ bool result=true;
+
+ mode = tCustom;
+ vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
+ while (iAxes != TrimAxes.end()) {
+ ta=*iAxes;
+ if( ta->GetStateType() == state )
+ result=false;
+ iAxes++;
+ }
+ if(result) {
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
+ delete[] sub_iterations;
+ delete[] successful;
+ delete[] solution;
+ sub_iterations=new double[TrimAxes.size()];
+ successful=new double[TrimAxes.size()];
+ solution=new bool[TrimAxes.size()];
+ }
+ return result;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGTrim::RemoveState( State state ) {
+ FGTrimAxis* ta;
+ bool result=false;
+
+ mode = tCustom;
+ vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
+ while (iAxes != TrimAxes.end()) {
+ ta=*iAxes;
+ if( ta->GetStateType() == state ) {
+ delete ta;
+ TrimAxes.erase(iAxes);
+ result=true;
+ continue;
+ }
+ iAxes++;
+ }
+ if(result) {
+ delete[] sub_iterations;
+ delete[] successful;
+ delete[] solution;
+ sub_iterations=new double[TrimAxes.size()];
+ successful=new double[TrimAxes.size()];
+ solution=new bool[TrimAxes.size()];
+ }
+ return result;
+}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGTrim::EditState( State state, Control new_control ){
+ FGTrimAxis* ta;
+ bool result=false;
+
+ mode = tCustom;
+ vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
+ while (iAxes != TrimAxes.end()) {
+ ta=*iAxes;
+ if( ta->GetStateType() == state ) {
+ TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
+ delete ta;
+ TrimAxes.erase(iAxes+1);
+ result=true;
+ break;
+ }
+ iAxes++;
+ }
+ return result;
+}
+
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGTrim::DoTrim(void) {
trim_failed=false;
+ int i;
-
- for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
- fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
+ for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
+ fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
}
fdmex->GetOutput()->Disable();
//clear the sub iterations counts & zero out the controls
- for(current_axis=0;current_axis<NumAxes;current_axis++) {
- //cout << current_axis << " " << TrimAxes[current_axis]->GetAccelName()
+ for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
+ //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
//<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
+ if(TrimAxes[current_axis]->GetStateType() == tQdot) {
+ if(mode == tGround)
+ TrimAxes[current_axis]->initTheta();
+ }
xlo=TrimAxes[current_axis]->GetControlMin();
xhi=TrimAxes[current_axis]->GetControlMax();
TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
successful[current_axis]=0;
solution[current_axis]=false;
}
+
+
+ if(mode == tPullup ) {
+ cout << "Setting pitch rate and nlf... " << endl;
+ setupPullup();
+ cout << "pitch rate done ... " << endl;
+ TrimAxes[0]->SetStateTarget(targetNlf);
+ cout << "nlf done" << endl;
+ } else if (mode == tTurn) {
+ setupTurn();
+ //TrimAxes[0]->SetStateTarget(targetNlf);
+ }
+
do {
axis_count=0;
- for(current_axis=0;current_axis<NumAxes;current_axis++) {
+ for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
+ setDebug();
+ updateRates();
Nsub=0;
if(!solution[current_axis]) {
if(checkLimits()) {
}
sub_iterations[current_axis]+=Nsub;
}
- for(current_axis=0;current_axis<NumAxes;current_axis++) {
+ for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(Debug > 0) TrimAxes[current_axis]->AxisReport();
if(TrimAxes[current_axis]->InTolerance()) {
}
- if((axis_count == NumAxes-1) && (NumAxes > 1)) {
- //cout << NumAxes-1 << " out of " << NumAxes << "!" << endl;
+ if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
+ //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
//At this point we can check the input limits of the failed axis
//and declare the trim failed if there is no sign change. If there
//is, keep going until success or max iteration count
//Oh, well: two out of three ain't bad
- for(current_axis=0;current_axis<NumAxes;current_axis++) {
+ for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(!TrimAxes[current_axis]->InTolerance()) {
if(!checkLimits()) {
// special case this for now -- if other cases arise proper
// support can be added to FGTrimAxis
if( (gamma_fallback) &&
- (TrimAxes[current_axis]->GetAccelType() == tUdot) &&
+ (TrimAxes[current_axis]->GetStateType() == tUdot) &&
(TrimAxes[current_axis]->GetControlType() == tThrottle)) {
cout << " Can't trim udot with throttle, trying flight"
<< " path angle. (" << N << ")" << endl;
- if(TrimAxes[current_axis]->GetAccel() > 0)
+ if(TrimAxes[current_axis]->GetState() > 0)
TrimAxes[current_axis]->SetControlToMin();
else
TrimAxes[current_axis]->SetControlToMax();
TrimAxes[current_axis]->Run();
delete TrimAxes[current_axis];
TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
- tGamma,Tolerance);
+ tGamma );
} else {
- cout << " Sorry, " << TrimAxes[current_axis]->GetAccelName()
+ cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
<< " doesn't appear to be trimmable" << endl;
//total_its=k;
trim_failed=true; //force the trim to fail
N++;
if(N > max_iterations)
trim_failed=true;
- } while((axis_count < NumAxes) && (!trim_failed));
- if((!trim_failed) && (axis_count >= NumAxes)) {
+ } while((axis_count < TrimAxes.size()) && (!trim_failed));
+ if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
cout << endl << " Trim successful" << endl;
} else {
total_its=N;
cout << endl << " Trim failed" << endl;
}
- for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
- fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
+ for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
+ fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
fdmex->GetOutput()->Enable();
return !trim_failed;
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGTrim::solve(void) {
- float x1,x2,x3,f1,f2,f3,d,d0;
- const float relax =0.9;
- float eps=TrimAxes[current_axis]->GetSolverEps();
+ double x1,x2,x3,f1,f2,f3,d,d0;
+ const double relax =0.9;
+ double eps=TrimAxes[current_axis]->GetSolverEps();
x1=x2=x3=0;
d=1;
x1=xhi;f1=ahi;
x3=xlo;f3=alo;
} */
-
d0=fabs(x3-x1);
//iterations
//max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
- while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps)
- && (Nsub < max_sub_iterations)) {
+ while ( (TrimAxes[current_axis]->InTolerance() == false )
+ && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
Nsub++;
d=(x3-x1)/d0;
x2=x1-d*d0*f1/(f3-f1);
TrimAxes[current_axis]->SetControl(x2);
TrimAxes[current_axis]->Run();
- f2=TrimAxes[current_axis]->GetAccel();
+ f2=TrimAxes[current_axis]->GetState();
if(Debug > 1) {
cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
<< ", " << x2 << ", " << x3 << endl;
return success;
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
produces an interval (xlo..xhi) on one side or the other of the current
control value in which a solution exists. This domain is, hopefully,
*/
bool FGTrim::findInterval(void) {
bool found=false;
- float step;
- float current_control=TrimAxes[current_axis]->GetControl();
- float current_accel=TrimAxes[current_axis]->GetAccel();;
- float xmin=TrimAxes[current_axis]->GetControlMin();
- float xmax=TrimAxes[current_axis]->GetControlMax();
- float lastxlo,lastxhi,lastalo,lastahi;
+ double step;
+ double current_control=TrimAxes[current_axis]->GetControl();
+ double current_accel=TrimAxes[current_axis]->GetState();;
+ double xmin=TrimAxes[current_axis]->GetControlMin();
+ double xmax=TrimAxes[current_axis]->GetControlMax();
+ double lastxlo,lastxhi,lastalo,lastahi;
step=0.025*fabs(xmax);
xlo=xhi=current_control;
if(xhi > xmax) xhi=xmax;
TrimAxes[current_axis]->SetControl(xlo);
TrimAxes[current_axis]->Run();
- alo=TrimAxes[current_axis]->GetAccel();
+ alo=TrimAxes[current_axis]->GetState();
TrimAxes[current_axis]->SetControl(xhi);
TrimAxes[current_axis]->Run();
- ahi=TrimAxes[current_axis]->GetAccel();
+ ahi=TrimAxes[current_axis]->GetState();
if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
if(alo*ahi <=0) { //found interval with root
found=true;
return found;
}
-/******************************************************************************/
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//checks to see which side of the current control value the solution is on
//and sets solutionDomain accordingly:
// 1 if solution is between the current and max
bool FGTrim::checkLimits(void) {
bool solutionExists;
- float current_control=TrimAxes[current_axis]->GetControl();
- float current_accel=TrimAxes[current_axis]->GetAccel();
+ double current_control=TrimAxes[current_axis]->GetControl();
+ double current_accel=TrimAxes[current_axis]->GetState();
xlo=TrimAxes[current_axis]->GetControlMin();
xhi=TrimAxes[current_axis]->GetControlMax();
TrimAxes[current_axis]->SetControl(xlo);
TrimAxes[current_axis]->Run();
- alo=TrimAxes[current_axis]->GetAccel();
+ alo=TrimAxes[current_axis]->GetState();
TrimAxes[current_axis]->SetControl(xhi);
TrimAxes[current_axis]->Run();
- ahi=TrimAxes[current_axis]->GetAccel();
+ ahi=TrimAxes[current_axis]->GetState();
if(Debug > 1)
cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
<< alo << ", " << ahi << endl;
solutionDomain=0;
solutionExists=false;
if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
- if(alo*current_accel < 0) {
+ if(alo*current_accel <= 0) {
solutionExists=true;
solutionDomain=-1;
xhi=current_control;
return solutionExists;
}
+void FGTrim::setupPullup() {
+ float g,q,cgamma;
+ FGColumnVector3 vPQR;
+ g=fdmex->GetInertial()->gravity();
+ cgamma=cos(fgic->GetFlightPathAngleRadIC());
+ cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
+ << fgic->GetVtrueFpsIC() << endl;
+ q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
+ cout << targetNlf << ", " << q << endl;
+ fdmex->GetRotation()->SetPQR(0,q,0);
+ cout << "setPitchRateInPullup() complete" << endl;
+
+}
+
+void FGTrim::setupTurn(void){
+ double g,phi;
+ phi = fgic->GetRollAngleRadIC();
+ if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
+ targetNlf = 1 / cos(phi);
+ g = fdmex->GetInertial()->gravity();
+ psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
+ cout << targetNlf << ", " << psidot << endl;
+ }
+
+}
+
+void FGTrim::updateRates(void){
+ if( mode == tTurn ) {
+ double phi = fgic->GetRollAngleRadIC();
+ double g = fdmex->GetInertial()->gravity();
+ double p,q,r,theta;
+ if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
+ theta=fgic->GetPitchAngleRadIC();
+ phi=fgic->GetRollAngleRadIC();
+ psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
+ p=-psidot*sin(theta);
+ q=psidot*cos(theta)*sin(phi);
+ r=psidot*cos(theta)*cos(phi);
+ } else {
+ p=q=r=0;
+ }
+ fdmex->GetRotation()->SetPQR(p,q,r);
+ } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
+ float g,q,cgamma;
+ FGColumnVector3 vPQR;
+ g=fdmex->GetInertial()->gravity();
+ cgamma=cos(fgic->GetFlightPathAngleRadIC());
+ q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
+ fdmex->GetRotation()->SetPQR(0,q,0);
+ }
+}
-
-
+void FGTrim::setDebug(void) {
+ if(debug_axis == tAll ||
+ TrimAxes[current_axis]->GetStateType() == debug_axis ) {
+ Debug=DebugLevel;
+ return;
+ } else {
+ Debug=0;
+ return;
+ }
+}
+
//YOU WERE WARNED, BUT YOU DID IT ANYWAY.
projects / flightgear.git / blobdiff