src/AIModel/AIGroundVehicle.cxx

   1 // FGAIGroundVehicle - FGAIShip-derived class creates an AI Ground Vehicle
   2 // by adding a ground following utility
   3 //
   4 // Written by Vivian Meazza, started August 2009.
   5 // - vivian.meazza at lineone.net
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include <simgear/sg_inlines.h>
  26
  27 #include <Main/viewer.hxx>
  28 #include <Scenery/scenery.hxx>
  29 #include <Scenery/tilemgr.hxx>
  30 #include <Airports/dynamics.hxx>
  31
  32 #include "AIGroundVehicle.hxx"
  33
  34 FGAIGroundVehicle::FGAIGroundVehicle() :
  35 FGAIShip(otGroundVehicle),
  36
  37 _pitch(0),
  38 _pitch_deg(0),
  39 _speed_kt(0),
  40 _selected_ac(0),
  41 _range_ft(0),
  42 _relbrg (0),
  43 _parent_speed(0),
  44 _dt_count(0),
  45 _next_run(0),
  46 _parent_x_offset(0),
  47 _parent_y_offset(0),
  48 _parent_z_offset(0),
  49 _parent(""),
  50 _break_count(0)
  51
  52 {
  53     invisible = false;
  54 }
  55
  56 FGAIGroundVehicle::~FGAIGroundVehicle() {}
  57
  58 void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
  59     if (!scFileNode)
  60         return;
  61
  62     FGAIShip::readFromScenario(scFileNode);
  63
  64     setName(scFileNode->getStringValue("name", "groundvehicle"));
  65     setParentName(scFileNode->getStringValue("parent", ""));
  66     setNoRoll(scFileNode->getBoolValue("no-roll", true));
  67     setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0));
  68     setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0));
  69     setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 35.0));
  70     setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0));
  71     setZOffset(scFileNode->getDoubleValue("hitch-z-offset", 0.0));
  72     setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
  73     setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
  74     setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
  75     setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1));
  76     setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
  77     setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 1.0));
  78     setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
  79     setInitialTunnel(scFileNode->getBoolValue("tunnel", false));
  80     //we may need these later for towed vehicles
  81     //    setSubID(scFileNode->getIntValue("SubID", 0));
  82     //    setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
  83     //    setFormate(scFileNode->getBoolValue("formate", true));
  84 }
  85
  86 void FGAIGroundVehicle::bind() {
  87     FGAIShip::bind();
  88
  89     props->tie("controls/constants/elevation-coeff",
  90         SGRawValuePointer<double>(&_elevation_coeff));
  91     props->tie("controls/constants/pitch-coeff",
  92         SGRawValuePointer<double>(&_pitch_coeff));
  93     props->tie("position/ht-AGL-ft",
  94         SGRawValuePointer<double>(&_ht_agl_ft));
  95     props->tie("hitch/rel-bearing-deg",
  96          SGRawValuePointer<double>(&_relbrg));
  97     props->tie("hitch/tow-angle-deg",
  98          SGRawValuePointer<double>(&_tow_angle));
  99     props->tie("hitch/range-ft",
 100         SGRawValuePointer<double>(&_range_ft));
 101     props->tie("hitch/x-offset-ft",
 102         SGRawValuePointer<double>(&_x_offset));
 103     props->tie("hitch/y-offset-ft",
 104         SGRawValuePointer<double>(&_y_offset));
 105     props->tie("hitch/z-offset-ft",
 106         SGRawValuePointer<double>(&_z_offset));
 107     props->tie("hitch/parent-x-offset-ft",
 108         SGRawValuePointer<double>(&_parent_x_offset));
 109     props->tie("hitch/parent-y-offset-ft",
 110         SGRawValuePointer<double>(&_parent_y_offset));
 111     props->tie("hitch/parent-z-offset-ft",
 112         SGRawValuePointer<double>(&_parent_z_offset));
 113     props->tie("controls/constants/tow-angle/gain",
 114         SGRawValuePointer<double>(&_tow_angle_gain));
 115     props->tie("controls/constants/tow-angle/limit-deg",
 116         SGRawValuePointer<double>(&_tow_angle_limit));
 117     props->tie("controls/contact-x1-offset-ft",
 118         SGRawValuePointer<double>(&_contact_x1_offset));
 119     props->tie("controls/contact-x2-offset-ft",
 120         SGRawValuePointer<double>(&_contact_x2_offset));
 121 }
 122
 123 void FGAIGroundVehicle::unbind() {
 124     FGAIShip::unbind();
 125
 126     props->untie("controls/constants/elevation-coeff");
 127     props->untie("controls/constants/pitch-coeff");
 128     props->untie("position/ht-AGL-ft");
 129     props->untie("hitch/rel-bearing-deg");
 130     props->untie("hitch/tow-angle-deg");
 131     props->untie("hitch/range-ft");
 132     props->untie("hitch/x-offset-ft");
 133     props->untie("hitch/y-offset-ft");
 134     props->untie("hitch/z-offset-ft");
 135     props->untie("hitch/parent-x-offset-ft");
 136     props->untie("hitch/parent-y-offset-ft");
 137     props->untie("hitch/parent-y-offset-ft");
 138     props->untie("controls/constants/tow-angle/gain");
 139     props->untie("controls/constants/tow-angle/limit-deg");
 140     props->untie("controls/contact-x1-offset-ft");
 141     props->untie("controls/contact-x2-offset-ft");
 142 }
 143
 144 bool FGAIGroundVehicle::init(bool search_in_AI_path) {
 145     if (!FGAIShip::init(search_in_AI_path))
 146         return false;
 147
 148     invisible = false;
 149     _limit = 200;
 150     no_roll = true;
 151 \r
 152     props->setStringValue("controls/parent-name", _parent.c_str());\r
 153     setParentNode();
 154
 155     return true;
 156 }
 157
 158 void FGAIGroundVehicle::update(double dt) {
 159     //    SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
 160     FGAIShip::update(dt);
 161
 162     RunGroundVehicle(dt);
 163 }
 164
 165 void FGAIGroundVehicle::setNoRoll(bool nr) {
 166     no_roll = nr;
 167 }
 168
 169 void FGAIGroundVehicle::setContactX1offset(double x1) {
 170     _contact_x1_offset = x1;
 171 }
 172
 173 void FGAIGroundVehicle::setContactX2offset(double x2) {
 174     _contact_x2_offset = x2;
 175 }
 176
 177 void FGAIGroundVehicle::setXOffset(double x) {
 178     _x_offset = x;
 179 }
 180
 181 void FGAIGroundVehicle::setYOffset(double y) {
 182     _y_offset = y;
 183 }
 184
 185 void FGAIGroundVehicle::setZOffset(double z) {
 186     _z_offset = z;
 187 }
 188
 189 void FGAIGroundVehicle::setPitchCoeff(double pc) {
 190     _pitch_coeff = pc;
 191 }
 192
 193 void FGAIGroundVehicle::setElevCoeff(double ec) {
 194     _elevation_coeff = ec;
 195 }
 196
 197 void FGAIGroundVehicle::setTowAngleGain(double g) {
 198     _tow_angle_gain = g;
 199 }
 200
 201 void FGAIGroundVehicle::setTowAngleLimit(double l) {
 202     _tow_angle_limit = l;
 203 }
 204
 205 void FGAIGroundVehicle::setElevation(double h, double dt, double coeff){
 206     double c = dt / (coeff + dt);
 207     _elevation_ft = (h * c) + (_elevation_ft * (1 - c));
 208 }
 209
 210 void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){
 211     double c = dt / (coeff + dt);
 212     _pitch_deg = (p * c) + (_pitch_deg * (1 - c));
 213 }
 214
 215 void FGAIGroundVehicle::setParentName(const string& p) {
 216     _parent = p;
 217 }
 218
 219 void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
 220     ta *= _tow_angle_gain;
 221     double factor = -0.0045 * speed + 1;
 222     double limit = _tow_angle_limit * factor;
 223 //      cout << "speed "<< speed << " _factor " << _factor<<" " <<_tow_angle_limit<< endl;
 224      _tow_angle = pow(ta,2) * sign(ta) * factor;
 225     SG_CLAMP_RANGE(_tow_angle, -limit, limit);
 226 }
 227
 228 bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
 229
 230     double height_m ;
 231
 232     if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){
 233             _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
 234
 235             if (_material) {
 236                 const vector<string>& names = _material->get_names();
 237
 238                 _solid = _material->get_solid();
 239
 240                 if (!names.empty())
 241                     props->setStringValue("material/name", names[0].c_str());
 242                 else
 243                     props->setStringValue("material/name", "");
 244
 245                 //cout << "material " << names[0].c_str()
 246                 //    << " _elevation_m " << _elevation_m
 247                 //    << " solid " << _solid
 248                 //    << " load " << _load_resistance
 249                 //    << " frictionFactor " << _frictionFactor
 250                 //    << endl;
 251
 252             }
 253
 254             return true;
 255     } else {
 256         return false;
 257     }
 258
 259 }
 260
 261 bool FGAIGroundVehicle::getPitch() {
 262
 263     if (!_tunnel){
 264         double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
 265         double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
 266         double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
 267         double _z_offset_m = _parent_z_offset * SG_FEET_TO_METER;
 268
 269         SGVec3d front(-contact_offset_x1_m, 0, 0);
 270         SGVec3d rear(-contact_offset_x2_m, 0, 0);
 271         SGVec3d Front = getCartPosAt(front);
 272         SGVec3d Rear = getCartPosAt(rear);
 273
 274         SGGeod geodFront = SGGeod::fromCart(Front);
 275         SGGeod geodRear = SGGeod::fromCart(Rear);
 276
 277         double front_elev_m = 0;
 278         double rear_elev_m = 0;
 279         double elev_front = 0;
 280         double elev_rear = 0;
 281         double max_alt = 10000;
 282
 283         if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 3000),
 284             elev_front, &_material, 0)){
 285                 front_elev_m = elev_front + _z_offset_m;
 286         } else
 287             return false;
 288
 289         if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 3000),
 290             elev_rear, &_material, 0)){
 291                 rear_elev_m = elev_rear;
 292         } else
 293             return false;
 294
 295         if (vel >= 0){
 296             double diff = front_elev_m - rear_elev_m;
 297             _pitch = atan2 (diff,
 298                 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
 299             _elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
 300         } else {
 301             double diff = rear_elev_m - front_elev_m;
 302             _pitch = atan2 (diff,
 303                 fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
 304             _elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
 305             _pitch = -_pitch;
 306         }
 307
 308     } else {
 309
 310         if (prev->altitude == 0 || curr->altitude == 0) return false;
 311
 312         static double distance;
 313         static double d_alt;
 314         static double curr_alt;
 315         static double prev_alt;
 316
 317         if (_new_waypoint){
 318             //cout << "new waypoint, calculating pitch " << endl;
 319             curr_alt = curr->altitude;
 320             prev_alt = prev->altitude;
 321             //cout << "prev_alt" <<prev_alt << endl;
 322             d_alt = (curr_alt - prev_alt) * SG_METER_TO_FEET;
 323             //_elevation = prev->altitude;
 324             distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude),
 325             SGGeod::fromDeg(curr->longitude, curr->latitude));
 326             _pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES;
 327             //cout << "new waypoint, calculating pitch " <<  _pitch <<
 328             //    " " << _pitch_offset << " " << _elevation <<endl;
 329         }
 330
 331         double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()),
 332             SGGeod::fromDeg(curr->longitude, curr->latitude));
 333
 334         /*cout << "tunnel " << _tunnel
 335              << " distance prev & curr " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET
 336              << " distance to go " << distance_to_go * SG_METER_TO_FEET
 337              << " d_alt ft " << d_alt
 338              << endl;*/
 339
 340         if (distance_to_go > distance)
 341             _elevation = prev_alt;
 342         else
 343             _elevation = curr_alt - (tan(_pitch * SG_DEGREES_TO_RADIANS) * distance_to_go * SG_METER_TO_FEET);
 344
 345     }
 346
 347     //getGroundElev(pos);
 348
 349     return true;
 350 }
 351
 352 void FGAIGroundVehicle::setParentNode() {
 353
 354     if(_parent == "")
 355         return;
 356
 357     const SGPropertyNode_ptr ai = fgGetNode("/ai/models", true);
 358
 359     for (int i = ai->nChildren() - 1; i >= -1; i--) {
 360         SGPropertyNode_ptr model;
 361
 362         if (i < 0) { // last iteration: selected model
 363             model = _selected_ac;
 364         } else {
 365             model = ai->getChild(i);
 366             string path = ai->getPath();
 367             const string name = model->getStringValue("name");
 368
 369             if (!model->nChildren()){
 370                 continue;
 371             }
 372             if (name == _parent) {
 373                 _selected_ac = model;  // save selected model for last iteration
 374                 break;
 375             }
 376
 377         }
 378         if (!model)
 379             continue;
 380
 381     }// end for loop
 382
 383     if (_selected_ac != 0){
 384         const string name = _selected_ac->getStringValue("name");
 385         _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
 386         _parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft");
 387         _parent_z_offset = _selected_ac->getDoubleValue("hitch/z-offset-ft");
 388         _hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft")
 389             * SG_FEET_TO_METER;
 390         _hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
 391             * SG_FEET_TO_METER;
 392         _hitch_z_offset_m = _selected_ac->getDoubleValue("hitch/z-offset-ft")
 393             * SG_FEET_TO_METER;
 394         setParent();
 395     } else {
 396         SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
 397                 << " parent not found: dying ");
 398         setDie(true);
 399     }
 400
 401 }
 402
 403 void FGAIGroundVehicle::setParent(){
 404
 405     double lat = _selected_ac->getDoubleValue("position/latitude-deg");
 406     double lon = _selected_ac->getDoubleValue("position/longitude-deg");
 407     double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
 408
 409     _selectedpos.setLatitudeDeg(lat);
 410     _selectedpos.setLongitudeDeg(lon);
 411     _selectedpos.setElevationFt(elevation);
 412
 413     _parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
 414
 415     SGVec3d rear_hitch(-_hitch_x_offset_m, _hitch_y_offset_m, 0);
 416     SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
 417
 418     SGGeod rearpos = SGGeod::fromCart(RearHitch);
 419
 420     double user_lat = rearpos.getLatitudeDeg();
 421     double user_lon = rearpos.getLongitudeDeg();
 422
 423     double range, bearing;
 424
 425     calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
 426         user_lat, user_lon, range, bearing);
 427     _range_ft = range * 6076.11549;
 428     _relbrg = calcRelBearingDeg(bearing, hdg);
 429 }
 430
 431 void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2,
 432                             double &range, double &bearing) const
 433 {
 434     // calculate the bearing and range of the second pos from the first
 435     double az2, distance;
 436     geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
 437     range = distance * SG_METER_TO_NM;
 438 }
 439
 440 double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading)
 441 {
 442     double angle = bearing - heading;
 443     SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
 444     return angle;
 445 }
 446
 447 SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const {
 448     double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
 449     double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
 450     double roll = _selected_ac->getDoubleValue("orientation/roll-deg");
 451
 452     // Transform that one to the horizontal local coordinate system.
 453     SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos);
 454
 455     // and postrotate the orientation of the AIModel wrt the horizontal
 456     // local frame
 457     hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
 458
 459     // The offset converted to the usual body fixed coordinate system
 460     // rotated to the earth fiexed coordinates axis
 461     SGVec3d off = hlTrans.backTransform(_off);
 462
 463     // Add the position offset of the AIModel to gain the earth centered position
 464     SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos);
 465
 466     return cartPos + off;
 467 }
 468
 469 void FGAIGroundVehicle::AdvanceFP(){
 470
 471     double count = 0;
 472     string parent_next_name =_selected_ac->getStringValue("waypoint/name-next");
 473
 474     while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->name != "END" && count < 5){
 475         SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 476             <<" advancing waypoint to: " << parent_next_name);
 477
 478         if (fp->getNextWaypoint()->name == parent_next_name){
 479             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 480                 << " not setting waypoint already at: " << fp->getNextWaypoint()->name);
 481             return;
 482         }
 483
 484         prev = curr;
 485         fp->IncrementWaypoint(false);
 486         curr = fp->getCurrentWaypoint();
 487         next = fp->getNextWaypoint();
 488
 489         if (fp->getNextWaypoint()->name == parent_next_name){
 490             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 491             << " waypoint set to: " << fp->getNextWaypoint()->name);
 492             return;
 493         }
 494
 495         count++;
 496
 497     }// end while loop
 498
 499     while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->name != "END"
 500         && count > -10){
 501             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 502             << " retreating waypoint to: " << parent_next_name
 503             << " at: " << fp->getNextWaypoint()->name);
 504
 505         if (fp->getNextWaypoint()->name == parent_next_name){
 506             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 507                 << " not setting waypoint already at:" << fp->getNextWaypoint()->name );
 508             return;
 509         }
 510
 511         prev = curr;
 512         fp->DecrementWaypoint(false);
 513         curr = fp->getCurrentWaypoint();
 514         next = fp->getNextWaypoint();
 515
 516         if (fp->getNextWaypoint()->name == parent_next_name){
 517             SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 518             << " waypoint set to: " << fp->getNextWaypoint()->name);
 519             return;
 520         }
 521
 522         count--;
 523
 524     }// end while loop
 525 }
 526
 527 void FGAIGroundVehicle::setTowSpeed(){
 528
 529     double diff = _range_ft - _x_offset;
 530     double  x = 0;
 531
 532     if (_range_ft > _x_offset * 3) x = 50;
 533
 534     if (_relbrg < -90 || _relbrg > 90){
 535         setSpeed(_parent_speed - 5 - x);
 536         //cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
 537     }else if (_range_ft > _x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
 538         setSpeed(_parent_speed + 1 + x);
 539         //cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
 540         //    << diff << " range " << _range_ft << endl;
 541     } else if (_range_ft < _x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
 542         setSpeed(_parent_speed - 1 - x);
 543         //cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
 544         //    << diff << " " << _range_ft << endl;
 545     } else {
 546         setSpeed(_parent_speed);
 547         //cout << _name << " else r _relbrg " << _relbrg << " " << diff << endl;
 548     }
 549
 550 }
 551
 552 void FGAIGroundVehicle::RunGroundVehicle(double dt){
 553
 554     _dt_count += dt;
 555
 556     ///////////////////////////////////////////////////////////////////////////
 557     // Check execution time (currently once every 0.05 sec or 20 fps)
 558     // Add a bit of randomization to prevent the execution of all flight plans
 559     // in synchrony, which can add significant periodic framerate flutter.
 560     // Randomization removed to get better appearance
 561     ///////////////////////////////////////////////////////////////////////////
 562
 563     //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
 564     if (_dt_count < _next_run)
 565         return;
 566
 567     _next_run = 0.05 /*+ (0.015 * sg_random())*/;
 568
 569     if (getPitch()){
 570         setElevation(_elevation, _dt_count, _elevation_coeff);
 571         ClimbTo(_elevation_ft);
 572         setPitch(_pitch, _dt_count, _pitch_coeff);
 573         PitchTo(_pitch_deg);
 574     }
 575
 576     if(_parent == ""){
 577         AccelTo(prev->speed);
 578         _dt_count = 0;
 579         return;
 580     }
 581
 582     setParent();
 583
 584     string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
 585     bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
 586     bool parent_restart = _selected_ac->getBoolValue("controls/restart");
 587
 588     if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
 589         SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 590             << " setting END: getting new waypoints ");
 591         AdvanceFP();
 592         setWPNames();
 593         setTunnel(_initial_tunnel);
 594         if(_restart) _missed_count = 200;
 595         /*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
 596     } else if (parent_waiting && !_waiting){
 597         SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 598             << " setting WAIT/WAITUNTIL: getting new waypoints ");
 599         AdvanceFP();
 600         setWPNames();
 601         _waiting = true;
 602     } else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
 603         SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
 604             << " wait done: getting new waypoints ");
 605         _waiting = false;
 606         _wait_count = 0;
 607         fp->IncrementWaypoint(false);
 608         next = fp->getNextWaypoint();
 609
 610         if (next->name == "WAITUNTIL" || next->name == "WAIT"
 611             || next->name == "END"){
 612         } else {
 613             prev = curr;
 614             fp->IncrementWaypoint(false);
 615             curr = fp->getCurrentWaypoint();
 616             next = fp->getNextWaypoint();
 617         }
 618
 619         setWPNames();
 620     } else if (_range_ft > (_x_offset +_parent_x_offset)* 4
 621         ){
 622         SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name
 623             << " rescue: reforming train " << _range_ft
 624             );
 625
 626         setTowAngle(0, dt, 1);
 627         setSpeed(_parent_speed + (10 * sign(_parent_speed)));
 628
 629     } else if (_parent_speed > 1){
 630
 631         setTowSpeed();
 632         setTowAngle(_relbrg, dt, 1);
 633
 634     } else if (_parent_speed < -1){
 635
 636         setTowSpeed();
 637
 638         if (_relbrg < 0)
 639             setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
 640         else
 641             setTowAngle(-(180 - _relbrg), dt, 1);
 642
 643     } else
 644         setSpeed(_parent_speed);
 645
 646 //    FGAIShip::update(_dt_count);
 647     _dt_count = 0;
 648
 649 }
 650
 651 // end AIGroundvehicle