Esempio n. 1
0
bool FGTrim::DoTrim(void) {
  bool trim_failed=false;
  unsigned int N = 0;
  unsigned int axis_count = 0;
  FGFCS *FCS = fdmex->GetFCS();
  vector<double> throttle0 = FCS->GetThrottleCmd();
  double elevator0 = FCS->GetDeCmd();
  double aileron0 = FCS->GetDaCmd();
  double rudder0 = FCS->GetDrCmd();
  double PitchTrim0 = FCS->GetPitchTrimCmd();
  fgic = *fdmex->GetIC();

  for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
    fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
  }

  fdmex->SetTrimStatus(true);
  fdmex->SuspendIntegration();

  fgic.SetPRadpsIC(0.0);
  fgic.SetQRadpsIC(0.0);
  fgic.SetRRadpsIC(0.0);

  if (mode == tGround) {
    trimOnGround();
    double theta = fgic.GetThetaRadIC();
    double phi = fgic.GetPhiRadIC();
    // Take opportunity of the first approx. found by trimOnGround() to
    // refine the control limits.
    TrimAxes[0].SetControlLimits(0., fgic.GetAltitudeAGLFtIC());
    TrimAxes[1].SetControlLimits(theta - 5.0 * degtorad, theta + 5.0 * degtorad);
    TrimAxes[2].SetControlLimits(phi - 30.0 * degtorad, phi + 30.0 * degtorad);
  }

  //clear the sub iterations counts & zero out the controls
  for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
    //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
    //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
    xlo=TrimAxes[current_axis].GetControlMin();
    xhi=TrimAxes[current_axis].GetControlMax();
    TrimAxes[current_axis].SetControl((xlo+xhi)/2);
    TrimAxes[current_axis].Run();
    //TrimAxes[current_axis].AxisReport();
    sub_iterations[current_axis]=0;
    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(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
      setDebug(TrimAxes[current_axis]);
      updateRates();
      Nsub=0;
      if(!solution[current_axis]) {
        if(checkLimits(TrimAxes[current_axis])) {
          solution[current_axis]=true;
          solve(TrimAxes[current_axis]);
        }
      } else if(findInterval(TrimAxes[current_axis])) {
        solve(TrimAxes[current_axis]);
      } else {
        solution[current_axis]=false;
      }
      sub_iterations[current_axis]+=Nsub;
    }
    for(unsigned int 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()) {
        axis_count++;
        successful[current_axis]++;
      }
    }

    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(unsigned int 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(TrimAxes[current_axis])) {
            // special case this for now -- if other cases arise proper
            // support can be added to FGTrimAxis
            if( (gamma_fallback) &&
                (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].GetState() > 0)
                TrimAxes[current_axis].SetControlToMin();
              else
                TrimAxes[current_axis].SetControlToMax();
              TrimAxes[current_axis].Run();
              TrimAxes[current_axis]=FGTrimAxis(fdmex,&fgic,tUdot,tGamma);
            } else {
              cout << "  Sorry, " << TrimAxes[current_axis].GetStateName()
              << " doesn't appear to be trimmable" << endl;
              //total_its=k;
              trim_failed=true; //force the trim to fail
            } //gamma_fallback
          }
        } //solution check
      } //for loop
    } //all-but-one check
    N++;
    if(N > max_iterations)
      trim_failed=true;
  } while((axis_count < TrimAxes.size()) && (!trim_failed));

  if((!trim_failed) && (axis_count >= TrimAxes.size())) {
    total_its=N;
    if (debug_lvl > 0)
        cout << endl << "  Trim successful" << endl;
  } else { // The trim has failed
    total_its=N;

    // Restore the aircraft parameters to their initial values
    fgic = *fdmex->GetIC();
    FCS->SetDeCmd(elevator0);
    FCS->SetDaCmd(aileron0);
    FCS->SetDrCmd(rudder0);
    FCS->SetPitchTrimCmd(PitchTrim0);
    for (unsigned int i=0; i < throttle0.size(); i++)
      FCS->SetThrottleCmd(i, throttle0[i]);

    fdmex->Initialize(&fgic);
    fdmex->Run();

    // If WOW is true we must make sure there are no gears into the ground.
    if (fdmex->GetGroundReactions()->GetWOW())
      trimOnGround();

    if (debug_lvl > 0)
        cout << endl << "  Trim failed" << endl;
  }

  fdmex->ResumeIntegration();
  fdmex->SetTrimStatus(false);

  for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
    fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
  }

  return !trim_failed;
}
Esempio n. 2
0
bool FGTrim::DoTrim(void) {

  trim_failed=false;
  int i;

  for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
    fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
  }

  fdmex->DisableOutput();

  fdmex->SetTrimStatus(true);
  fdmex->SuspendIntegration();

  fgic->SetPRadpsIC(0.0);
  fgic->SetQRadpsIC(0.0);
  fgic->SetRRadpsIC(0.0);

  //clear the sub iterations counts & zero out the controls
  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);
    TrimAxes[current_axis]->Run();
    //TrimAxes[current_axis]->AxisReport();
    sub_iterations[current_axis]=0;
    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<TrimAxes.size();current_axis++) {
      setDebug();
      updateRates();
      Nsub=0;
      if(!solution[current_axis]) {
        if(checkLimits()) {
          solution[current_axis]=true;
          solve();
        }
      } else if(findInterval()) {
        solve();
      } else {
        solution[current_axis]=false;
      }
      sub_iterations[current_axis]+=Nsub;
    }
    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()) {
        axis_count++;
        successful[current_axis]++;
      }
    }


    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<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]->GetStateType() == tUdot) &&
                (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
              cout << "  Can't trim udot with throttle, trying flight"
              << " path angle. (" << N << ")" << endl;
              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 );
            } else {
              cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
              << " doesn't appear to be trimmable" << endl;
              //total_its=k;
              trim_failed=true; //force the trim to fail
            } //gamma_fallback
          }
        } //solution check
      } //for loop
    } //all-but-one check
    N++;
    if(N > max_iterations)
      trim_failed=true;
  } while((axis_count < TrimAxes.size()) && (!trim_failed));
  if((!trim_failed) && (axis_count >= TrimAxes.size())) {
    total_its=N;
    if (debug_lvl > 0)
        cout << endl << "  Trim successful" << endl;
  } else {
    total_its=N;
    if (debug_lvl > 0)
        cout << endl << "  Trim failed" << endl;
  }
  for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
    fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
  }
  fdmex->SetTrimStatus(false);
  fdmex->ResumeIntegration();
  fdmex->EnableOutput();
  return !trim_failed;
}