gistfile1.java

  public void holdYaw(double target,double factor) throws FgException
  {
    // get current Heading degree
    // calculate the deviation in degree
    double aircraftHdg = a.getHeadingDeg();
    double degreeToTurn = calcDegreeToTurn();


    // Limit the potential for an integrator to accumulate ridicuously high values when
    // we cannot reach its target yaw for a period of time.
    updateYawDifferenceIntegral(degreeToTurn);

    double rudderPos = degreeToTurn / factor + yawDifferenceIntegral/(factor*40);

    // How much are we moving a rudder by? (NEGATIVE_INFINITY is used to mean that the previous rudder value is not known)
    double difference = prevRudder != Double.NEGATIVE_INFINITY? rudderPos-prevRudder:0;

    final double rudderMaxDelta = 0.05; //  why the hell is this declared here?

    double newRudder = calcNewRudder(rudderPos, difference);

    // set
    a.setRudder(newRudder);
    prevRudder = newRudder;
  }

  private double calcDegreeToTurn(double target, double aircraftHdg) {
    degreeToTurn = (target-aircraftHdg);
    // normalise the degree to a value between -180 and 180 degrees
    if (degreeToTurn < -180) {
      degreeToTurn +=360;
    } else if (degreeToTurn > 180) {
      degreeToTurn-=360;
    }
    return degreeToTurn;
  }

  private double updateYawDifferenceIntegral(double degreeToTurn) {
    yawDifferenceIntegral += degreeToTurn;
    if (yawDifferenceIntegral > 100) {
      yawDifferenceIntegral = 100;
    } else if (yawDifferenceIntegral < -100) {
      yawDifferenceIntegral = -100;
    }
  }

  private double calcNewRudder(double rudderPos, double difference) {
    double newRudder = rudderPos;
    if (difference > rudderMaxDelta) {
      newRudder = prevRudder+rudderMaxDelta;
    } else if (difference < -rudderMaxDelta) {
      newRudder = prevRudder-rudderMaxDelta;
    }

    //if the absolute value of the calculated rudder position is too large, set it to 
    //the greatest/smallest possible value
    if (newRudder > maxRudder) {
      newRudder = maxRudder;
    } else if (newRudder < -maxRudder) {
      newRudder = -maxRudder;
    }

    return newRudder;
  }