+
+void FGAIShip::setWPPos() {
+
+ if (curr->name == "END" || curr->name == "WAIT" || curr->name == "WAITUNTIL"){
+ cout<< curr->name << endl;
+ return;
+ }
+
+ double elevation_m = 0;
+ wppos.setLatitudeDeg(curr->latitude);
+ wppos.setLongitudeDeg(curr->longitude);
+ wppos.setElevationFt(0);
+
+ if (curr->on_ground){
+
+ if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(wppos, 10000),
+ elevation_m, &_material)){
+ wppos.setElevationM(elevation_m);
+ }
+
+ } else {
+ wppos.setElevationFt(curr->altitude);
+ }
+
+}
+
+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;
+
+ //if (_wp_range > _sp_turn_radius_ft / (2 * 6076.1155)){
+ 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;
+ _xtrack_error = xtrack_error_nm * 6076.1155;
+
+ if (_lead_angle<= -_lead_angle_limit)
+ _lead_angle = -_lead_angle_limit;
+ else if (_lead_angle >= _lead_angle_limit)
+ _lead_angle = _lead_angle_limit;
+
+}