+
+void FGAIShip::setWPPos() {
+
+ if (curr->name == "END" || curr->name == "WAIT"
+ || curr->name == "WAITUNTIL" || curr->name == "TUNNEL"){
+ //cout << curr->name << " returning" << endl;
+ return;
+ }
+
+ double elevation_m = 0;
+ wppos.setLatitudeDeg(curr->latitude);
+ wppos.setLongitudeDeg(curr->longitude);
+ wppos.setElevationM(0);
+
+ if (curr->on_ground){
+
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(wppos, 3000),
+ elevation_m, &_material, 0)){
+ wppos.setElevationM(elevation_m);
+ }
+
+ //cout << curr->name << " setting measured elev " << elevation_m << endl;
+
+ } else {
+ wppos.setElevationM(curr->altitude);
+ //cout << curr->name << " setting FP elev " << elevation_m << endl;
+ }
+
+ curr->altitude = wppos.getElevationM();
+
+}
+
+void FGAIShip::setXTrackError() {
+
+ double course = getCourse(prev->latitude, prev->longitude,
+ curr->latitude, curr->longitude);
+ double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
+ curr->latitude, curr->longitude);
+ double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range;
+ double factor = -0.0045 * speed + 1;
+ double limit = _lead_angle_limit * factor;
+
+ if (_wp_range > 0){
+ _lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES;
+ } else
+ _lead_angle = 0;
+
+ _lead_angle *= _lead_angle_gain * factor;
+ _xtrack_error = xtrack_error_nm * 6076.1155;
+
+ SG_CLAMP_RANGE(_lead_angle, -limit, limit);
+
+}