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 ));
+ 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:
void FGTrim::setupTurn(void){
double g,phi;
phi = fgic->GetRollAngleRadIC();
- if( fabs(phi) > 0.01 && fabs(phi) < 1.56 ) {
+ if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
targetNlf = 1 / cos(phi);
g = fdmex->GetInertial()->gravity();
- psidot = g*tan(phi) / fgic->GetVtrueFpsIC();
+ psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
cout << targetNlf << ", " << psidot << endl;
- }
+ }
+
}
void FGTrim::updateRates(void){
- if(mode == tTurn) {
- double p,q,r,theta,phi;
- theta=fgic->GetPitchAngleRadIC();
- phi=fgic->GetRollAngleRadIC();
- p=-psidot*sin(theta);
- q=psidot*cos(theta)*sin(phi);
- r=psidot*cos(theta)*cos(phi);
+ 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) {