+ return true;
+}
+
+
+/**
+ * Check difference between target bearing and current heading and correct if necessary.
+ *
+ * @param curr
+ */
+void FGAIAircraft::controlHeading(FGAIFlightPlan::waypoint* curr) {
+ double calc_bearing = fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
+ //cerr << "Bearing = " << calc_bearing << endl;
+ if (speed < 0) {
+ calc_bearing +=180;
+ if (calc_bearing > 360)
+ calc_bearing -= 360;
+ }
+
+ if (finite(calc_bearing)) {
+ double hdg_error = calc_bearing - tgt_heading;
+ if (fabs(hdg_error) > 0.01) {
+ TurnTo( calc_bearing );
+ }
+
+ } else {
+ cerr << "calc_bearing is not a finite number : "
+ << "Speed " << speed
+ << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
+ << "waypoint " << curr->latitude << ", " << curr->longitude << endl;
+ cerr << "waypoint name " << curr->name;
+ exit(1); // FIXME
+ }
+}
+
+
+/**
+ * Update the lead distance calculation if speed has changed sufficiently
+ * to prevent spinning (hopefully);
+ *
+ * @param curr
+ * @param next
+ */
+void FGAIAircraft::controlSpeed(FGAIFlightPlan::waypoint* curr, FGAIFlightPlan::waypoint* next) {
+ double speed_diff = speed - prevSpeed;
+
+ if (fabs(speed_diff) > 10) {
+ prevSpeed = speed;
+ if (next) {
+ fp->setLeadDistance(speed, tgt_heading, curr, next);
+ }
+ }
+}
+
+
+/**
+ * Update target values (heading, alt, speed) depending on flight plan or control properties
+ */
+void FGAIAircraft::updatePrimaryTargetValues(bool& flightplanActive, bool& aiOutOfSight) {
+ if (fp) // AI object has a flightplan
+ {
+ //TODO make this a function of AIBase
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
+ //cerr << "UpateTArgetValues() " << endl;
+ ProcessFlightPlan(dt, now);
+
+ // Do execute Ground elev for inactive aircraft, so they
+ // Are repositioned to the correct ground altitude when the user flies within visibility range.
+ // In addition, check whether we are out of user range, so this aircraft
+ // can be deleted.
+ if (onGround()) {
+ Transform(); // make sure aip is initialized.
+ getGroundElev(dt);
+ doGroundAltitude();
+ // Transform();
+ pos.setElevationFt(altitude_ft);
+ }
+ if (trafficRef) {
+ //cerr << trafficRef->getRegistration() << " Setting altitude to " << altitude_ft;
+ aiOutOfSight = !aiTrafficVisible();
+ if (aiOutOfSight) {
+ setDie(true);
+ //cerr << trafficRef->getRegistration() << " is set to die " << endl;
+ aiOutOfSight = true;
+ return;
+ }
+ }
+ timeElapsed = now - fp->getStartTime();
+ flightplanActive = fp->isActive(now);
+ } else {
+ // no flight plan, update target heading, speed, and altitude
+ // from control properties. These default to the initial
+ // settings in the config file, but can be changed "on the
+ // fly".
+ string lat_mode = props->getStringValue("controls/flight/lateral-mode");
+ if ( lat_mode == "roll" ) {
+ double angle
+ = props->getDoubleValue("controls/flight/target-roll" );
+ RollTo( angle );
+ } else {
+ double angle
+ = props->getDoubleValue("controls/flight/target-hdg" );
+ TurnTo( angle );
+ }
+
+ string lon_mode
+ = props->getStringValue("controls/flight/longitude-mode");
+ if ( lon_mode == "alt" ) {
+ double alt = props->getDoubleValue("controls/flight/target-alt" );
+ ClimbTo( alt );
+ } else {
+ double angle
+ = props->getDoubleValue("controls/flight/target-pitch" );
+ PitchTo( angle );
+ }
+
+ AccelTo( props->getDoubleValue("controls/flight/target-spd" ) );
+ }
+}
+
+void FGAIAircraft::updatePosition() {
+ // convert speed to degrees per second
+ double speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS )
+ * speed * 1.686 / ft_per_deg_lat;
+ double speed_east_deg_sec = sin( hdg * SGD_DEGREES_TO_RADIANS )
+ * speed * 1.686 / ft_per_deg_lon;
+
+ // set new position
+ pos.setLatitudeDeg( pos.getLatitudeDeg() + speed_north_deg_sec * dt);
+ pos.setLongitudeDeg( pos.getLongitudeDeg() + speed_east_deg_sec * dt);
+}
+
+
+void FGAIAircraft::updateHeading() {
+ // adjust heading based on current bank angle
+ if (roll == 0.0)
+ roll = 0.01;
+
+ if (roll != 0.0) {
+ // double turnConstant;
+ //if (no_roll)
+ // turnConstant = 0.0088362;
+ //else
+ // turnConstant = 0.088362;
+ // If on ground, calculate heading change directly
+ if (onGround()) {
+ double headingDiff = fabs(hdg-tgt_heading);
+ double bank_sense = 0.0;
+ /*
+ double diff = fabs(hdg - tgt_heading);
+ if (diff > 180)
+ diff = fabs(diff - 360);
+
+ double sum = hdg + diff;
+ if (sum > 360.0)
+ sum -= 360.0;
+ if (fabs(sum - tgt_heading) < 1.0) {
+ bank_sense = 1.0; // right turn
+ } else {
+ bank_sense = -1.0; // left turn
+ }*/
+ if (headingDiff > 180)
+ headingDiff = fabs(headingDiff - 360);
+ double sum = hdg + headingDiff;
+ if (sum > 360.0)
+ sum -= 360.0;
+ if (fabs(sum - tgt_heading) > 0.0001) {
+ bank_sense = -1.0;
+ } else {
+ bank_sense = 1.0;
+ }
+ //if (trafficRef)
+ //cerr << trafficRef->getCallSign() << " Heading "
+ // << hdg << ". Target " << tgt_heading << ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << endl;
+ //if (headingDiff > 60) {
+ groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS);
+ //groundTargetSpeed = tgt_speed - tgt_speed * (headingDiff/180);
+ //} else {
+ // groundTargetSpeed = tgt_speed;
+ //}
+ if (sign(groundTargetSpeed) != sign(tgt_speed))
+ groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode
+
+ if (headingDiff > 30.0) {
+ // invert if pushed backward
+ headingChangeRate += 10.0 * dt * sign(roll);
+
+ // Clamp the maximum steering rate to 30 degrees per second,
+ // But only do this when the heading error is decreasing.
+ if ((headingDiff < headingError)) {
+ if (headingChangeRate > 30)
+ headingChangeRate = 30;
+ else if (headingChangeRate < -30)
+ headingChangeRate = -30;
+ }
+ } else {
+ if (fabs(headingChangeRate) > headingDiff)
+ headingChangeRate = headingDiff*sign(roll);
+ else
+ headingChangeRate += dt * sign(roll);
+ }
+
+ hdg += headingChangeRate * dt * (fabs(speed) / 15);
+ headingError = headingDiff;
+ } else {
+ if (fabs(speed) > 1.0) {
+ turn_radius_ft = 0.088362 * speed * speed
+ / tan( fabs(roll) / SG_RADIANS_TO_DEGREES );
+ } else {
+ // Check if turn_radius_ft == 0; this might lead to a division by 0.
+ turn_radius_ft = 1.0;
+ }
+ double turn_circum_ft = SGD_2PI * turn_radius_ft;
+ double dist_covered_ft = speed * 1.686 * dt;
+ double alpha = dist_covered_ft / turn_circum_ft * 360.0;
+ hdg += alpha * sign(roll);
+ }
+ while ( hdg > 360.0 ) {
+ hdg -= 360.0;
+ spinCounter++;
+ }
+ while ( hdg < 0.0) {
+ hdg += 360.0;
+ spinCounter--;
+ }
+ }
+}
+
+
+void FGAIAircraft::updateBankAngleTarget() {
+ // adjust target bank angle if heading lock engaged
+ if (hdg_lock) {
+ double bank_sense = 0.0;
+ double diff = fabs(hdg - tgt_heading);
+ if (diff > 180)
+ diff = fabs(diff - 360);
+
+ double sum = hdg + diff;
+ if (sum > 360.0)
+ sum -= 360.0;
+ if (fabs(sum - tgt_heading) < 1.0) {
+ bank_sense = 1.0; // right turn
+ } else {
+ bank_sense = -1.0; // left turn
+ }
+ if (diff < _performance->maximumBankAngle()) {
+ tgt_roll = diff * bank_sense;
+ } else {
+ tgt_roll = _performance->maximumBankAngle() * bank_sense;
+ }
+ if ((fabs((double) spinCounter) > 1) && (diff > _performance->maximumBankAngle())) {
+ tgt_speed *= 0.999; // Ugly hack: If aircraft get stuck, they will continually spin around.
+ // The only way to resolve this is to make them slow down.
+ }
+ }
+}
+
+
+void FGAIAircraft::updateVerticalSpeedTarget() {
+ // adjust target Altitude, based on ground elevation when on ground
+ if (onGround()) {
+ getGroundElev(dt);
+ doGroundAltitude();
+ } else if (alt_lock) {
+ // find target vertical speed
+ if (use_perf_vs) {
+ if (altitude_ft < tgt_altitude_ft) {
+ tgt_vs = tgt_altitude_ft - altitude_ft;
+ if (tgt_vs > _performance->climbRate())
+ tgt_vs = _performance->climbRate();
+ } else {
+ tgt_vs = tgt_altitude_ft - altitude_ft;
+ if (tgt_vs < (-_performance->descentRate()))
+ tgt_vs = -_performance->descentRate();
+ }
+ } else {
+ double max_vs = 4*(tgt_altitude_ft - altitude_ft);
+ double min_vs = 100;
+ if (tgt_altitude_ft < altitude_ft)
+ min_vs = -100.0;
+ if ((fabs(tgt_altitude_ft - altitude_ft) < 1500.0)
+ && (fabs(max_vs) < fabs(tgt_vs)))
+ tgt_vs = max_vs;
+
+ if (fabs(tgt_vs) < fabs(min_vs))
+ tgt_vs = min_vs;
+ }
+ } //else
+ // tgt_vs = 0.0;
+}
+
+void FGAIAircraft::updatePitchAngleTarget() {
+ // if on ground and above vRotate -> initial rotation
+ if (onGround() && (speed > _performance->vRotate()))
+ tgt_pitch = 8.0; // some rough B737 value
+
+ //TODO pitch angle on approach and landing
+
+ // match pitch angle to vertical speed
+ else if (tgt_vs > 0) {
+ tgt_pitch = tgt_vs * 0.005;
+ } else {
+ tgt_pitch = tgt_vs * 0.002;
+ }
+}
+
+string FGAIAircraft::atGate() {
+ string tmp("");
+ if (fp->getLeg() < 3) {
+ if (trafficRef) {
+ if (fp->getGate() > 0) {
+ FGParking *park =
+ trafficRef->getDepartureAirport()->getDynamics()->getParking(fp->getGate());
+ tmp = park->getName();
+ }
+ }
+ }
+ return tmp;
+}
+
+void FGAIAircraft::handleATCRequests() {
+ //TODO implement NullController for having no ATC to save the conditionals
+ if (controller) {
+ controller->update(getID(),
+ pos.getLatitudeDeg(),
+ pos.getLongitudeDeg(),
+ hdg,
+ speed,
+ altitude_ft, dt);
+ processATC(controller->getInstruction(getID()));
+ }
+}
+
+void FGAIAircraft::updateActualState() {
+ //update current state
+ //TODO have a single tgt_speed and check speed limit on ground on setting tgt_speed
+ updatePosition();
+
+ if (onGround())
+ speed = _performance->actualSpeed(this, groundTargetSpeed, dt);
+ else
+ speed = _performance->actualSpeed(this, tgt_speed, dt);
+
+ updateHeading();
+ roll = _performance->actualBankAngle(this, tgt_roll, dt);
+
+ // adjust altitude (meters) based on current vertical speed (fpm)
+ altitude_ft += vs / 60.0 * dt;
+ pos.setElevationFt(altitude_ft);
+
+ vs = _performance->actualVerticalSpeed(this, tgt_vs, dt);
+ pitch = _performance->actualPitch(this, tgt_pitch, dt);
+}
+
+void FGAIAircraft::updateSecondaryTargetValues() {
+ // derived target state values
+ updateBankAngleTarget();
+ updateVerticalSpeedTarget();
+ updatePitchAngleTarget();
+
+ //TODO calculate wind correction angle (tgt_yaw)