src/AIModel/AIWingman.cxx

   1 // FGAIWingman - FGAIBllistic-derived class creates an AI Wingman
   2 //
   3 // Written by Vivian Meazza, started February 2008.
   4 // - vivian.meazza at lineone.net
   5 //
   6 // This program is free software; you can redistribute it and/or
   7 // modify it under the terms of the GNU General Public License as
   8 // published by the Free Software Foundation; either version 2 of the
   9 // License, or (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful, but
  12 // WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 // General Public License for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License
  17 // along with this program; if not, write to the Free Software
  18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19
  20 #ifdef HAVE_CONFIG_H
  21 #  include <config.h>
  22 #endif
  23
  24 #include <simgear/sg_inlines.h>
  25 #include <simgear/math/SGMath.hxx>
  26
  27
  28 #include "AIWingman.hxx"
  29
  30 FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman),
  31 _formate_to_ac(true),
  32 _break(false),
  33 _join(false),
  34 _break_angle(-90),
  35 _coeff_hdg(5.0),
  36 _coeff_pch(5.0),
  37 _coeff_bnk(5.0),
  38 _coeff_spd(2.0)
  39
  40 {
  41     invisible = false;
  42     _parent="";
  43     tgt_heading = 250;
  44
  45 }
  46
  47 FGAIWingman::~FGAIWingman() {}
  48
  49 void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) {
  50     if (!scFileNode)
  51         return;
  52
  53     FGAIBase::readFromScenario(scFileNode);
  54
  55     setAzimuth(scFileNode->getDoubleValue("azimuth", 0.0));
  56     setElevation(scFileNode->getDoubleValue("elevation", 0.0));
  57     setLife(scFileNode->getDoubleValue("life", -1));
  58     setNoRoll(scFileNode->getBoolValue("no-roll", false));
  59     setName(scFileNode->getStringValue("name", "Wingman"));
  60     setParentName(scFileNode->getStringValue("parent", ""));
  61     setSubID(scFileNode->getIntValue("SubID", 0));
  62     setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
  63     setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
  64     setZoffset(scFileNode->getDoubleValue("z-offset", 0.0));
  65     setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
  66     setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
  67     setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
  68     setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
  69     setFormate(scFileNode->getBoolValue("formate", true));
  70     setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 300.0));
  71     setCoeffHdg(scFileNode->getDoubleValue("coefficients/heading", 5.0));
  72     setCoeffPch(scFileNode->getDoubleValue("coefficients/pitch", 5.0));
  73     setCoeffBnk(scFileNode->getDoubleValue("coefficients/bank", 4.0));
  74     setCoeffSpd(scFileNode->getDoubleValue("coefficients/speed", 2.0));
  75
  76
  77 }
  78
  79 void FGAIWingman::bind() {
  80     FGAIBallistic::bind();
  81
  82     props->untie("controls/slave-to-ac");
  83
  84     props->tie("id", SGRawValueMethods<FGAIBase,int>(*this,
  85         &FGAIBase::getID));
  86     props->tie("subID", SGRawValueMethods<FGAIBase,int>(*this,
  87         &FGAIBase::_getSubID));
  88     props->tie("position/altitude-ft",
  89         SGRawValueMethods<FGAIBase,double>(*this,
  90         &FGAIBase::_getElevationFt,
  91         &FGAIBase::_setAltitude));
  92     props->tie("position/latitude-deg",
  93         SGRawValueMethods<FGAIBase,double>(*this,
  94         &FGAIBase::_getLatitude,
  95         &FGAIBase::_setLatitude));
  96     props->tie("position/longitude-deg",
  97         SGRawValueMethods<FGAIBase,double>(*this,
  98         &FGAIBase::_getLongitude,
  99         &FGAIBase::_setLongitude));
 100
 101     props->tie("controls/break", SGRawValuePointer<bool>(&_break));
 102     props->tie("controls/join", SGRawValuePointer<bool>(&_join));
 103
 104     props->tie("controls/formate-to-ac",
 105         SGRawValueMethods<FGAIWingman,bool>
 106         (*this, &FGAIWingman::getFormate, &FGAIWingman::setFormate));
 107     props->tie("controls/tgt-heading-deg",
 108         SGRawValueMethods<FGAIWingman,double>
 109         (*this, &FGAIWingman::getTgtHdg, &FGAIWingman::setTgtHdg));
 110     props->tie("controls/tgt-speed-kt",
 111         SGRawValueMethods<FGAIWingman,double>
 112         (*this, &FGAIWingman::getTgtSpd, &FGAIWingman::setTgtSpd));
 113     props->tie("controls/break-deg-rel",
 114         SGRawValueMethods<FGAIWingman,double>
 115         (*this, &FGAIWingman::getBrkAng, &FGAIWingman::setBrkAng));
 116     props->tie("controls/coefficients/heading",
 117         SGRawValuePointer<double>(&_coeff_hdg));
 118     props->tie("controls/coefficients/pitch",
 119         SGRawValuePointer<double>(&_coeff_pch));
 120     props->tie("controls/coefficients/bank",
 121         SGRawValuePointer<double>(&_coeff_bnk));
 122     props->tie("controls/coefficients/speed",
 123         SGRawValuePointer<double>(&_coeff_spd));
 124
 125     props->tie("orientation/pitch-deg",   SGRawValuePointer<double>(&pitch));
 126     props->tie("orientation/roll-deg",    SGRawValuePointer<double>(&roll));
 127     props->tie("orientation/true-heading-deg", SGRawValuePointer<double>(&hdg));
 128
 129     props->tie("submodels/serviceable", SGRawValuePointer<bool>(&serviceable));
 130
 131     props->tie("load/rel-brg-to-user-deg",
 132         SGRawValueMethods<FGAIBallistic,double>
 133         (*this, &FGAIBallistic::getRelBrgHitchToUser));
 134     props->tie("load/elev-to-user-deg",
 135         SGRawValueMethods<FGAIBallistic,double>
 136         (*this, &FGAIBallistic::getElevHitchToUser));
 137
 138     props->tie("velocities/vertical-speed-fps",
 139         SGRawValuePointer<double>(&vs));
 140     props->tie("velocities/true-airspeed-kt",
 141         SGRawValuePointer<double>(&speed));
 142     props->tie("velocities/speed-east-fps",
 143         SGRawValuePointer<double>(&_speed_east_fps));
 144     props->tie("velocities/speed-north-fps",
 145         SGRawValuePointer<double>(&_speed_north_fps));
 146
 147     props->tie("position/x-offset",
 148         SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
 149     props->tie("position/y-offset",
 150         SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset));
 151     props->tie("position/z-offset",
 152         SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset));
 153     props->tie("position/tgt-x-offset",
 154         SGRawValueMethods<FGAIBallistic,double>(*this, &FGAIBallistic::getTgtXOffset, &FGAIBallistic::setTgtXOffset));
 155     props->tie("position/tgt-y-offset",
 156         SGRawValueMethods<FGAIBallistic,double>(*this, &FGAIBallistic::getTgtYOffset, &FGAIBallistic::setTgtYOffset));
 157     props->tie("position/tgt-z-offset",
 158         SGRawValueMethods<FGAIBallistic,double>(*this, &FGAIBallistic::getTgtZOffset, &FGAIBallistic::setTgtZOffset));
 159 }
 160
 161 void FGAIWingman::unbind() {
 162     FGAIBallistic::unbind();
 163
 164     props->untie("id");
 165     props->untie("SubID");
 166
 167     props->untie("orientation/pitch-deg");
 168     props->untie("orientation/roll-deg");
 169     props->untie("orientation/true-heading-deg");
 170
 171     props->untie("controls/formate-to-ac");
 172     props->untie("controls/break");
 173     props->untie("controls/join");
 174     props->untie("controls/tgt-heading-deg");
 175     props->untie("controls/tgt-speed-kt");
 176     props->untie("controls/break-deg-rel");
 177     props->untie("controls/coefficients/heading");
 178     props->untie("controls/coefficients/pitch");
 179     props->untie("controls/coefficients/bank");
 180     props->untie("controls/coefficients/speed");
 181
 182     props->untie("submodels/serviceable");
 183
 184     props->untie("velocities/true-airspeed-kt");
 185     props->untie("velocities/vertical-speed-fps");
 186     props->untie("velocities/speed_east_fps");
 187     props->untie("velocities/speed_north_fps");
 188
 189     props->untie("load/rel-brg-to-user-deg");
 190     props->untie("load/elev-to-user-deg");
 191
 192     props->untie("position/altitude-ft");
 193     props->untie("position/latitude-deg");
 194     props->untie("position/longitude-deg");
 195     props->untie("position/x-offset");
 196     props->untie("position/y-offset");
 197     props->untie("position/z-offset");
 198     props->untie("position/tgt-x-offset");
 199     props->untie("position/tgt-y-offset");
 200     props->untie("position/tgt-z-offset");
 201 }
 202
 203 bool FGAIWingman::init(bool search_in_AI_path) {
 204     if (!FGAIBallistic::init(search_in_AI_path))
 205         return false;
 206     reinit();
 207     return true;
 208 }
 209
 210 void FGAIWingman::reinit() {
 211     invisible = false;
 212
 213     _tgt_x_offset = _x_offset;
 214     _tgt_y_offset = _y_offset;
 215     _tgt_z_offset = _z_offset;
 216
 217     hdg = _azimuth;
 218     pitch = _elevation;
 219     roll = _rotation;
 220     _ht_agl_ft = 1e10;
 221
 222     if(_parent != ""){
 223         setParentNode();
 224     }
 225
 226     setParentNodes(_selected_ac);
 227
 228     props->setStringValue("submodels/path", _path.c_str());
 229     user_WoW_node      = fgGetNode("gear/gear[1]/wow", true);
 230
 231     FGAIBallistic::reinit();
 232 }
 233
 234 void FGAIWingman::update(double dt) {
 235
 236 //    FGAIBallistic::update(dt);
 237
 238     if (_formate_to_ac){
 239         formateToAC(dt);
 240         Transform();
 241         setBrkHdg(_break_angle);
 242     }else if (_break) {
 243         FGAIBase::update(dt);
 244         tgt_altitude_ft = altitude_ft;
 245         tgt_speed = speed;
 246         tgt_roll = roll;
 247         tgt_pitch = pitch;
 248         Break(dt);
 249         Transform();
 250     } else {
 251         Join(dt);
 252         Transform();
 253     }
 254
 255 }
 256
 257 double FGAIWingman::calcDistanceM(SGGeod pos1, SGGeod pos2) const {
 258     //calculate the distance load to hitch
 259     SGVec3d cartPos1 = SGVec3d::fromGeod(pos1);
 260     SGVec3d cartPos2 = SGVec3d::fromGeod(pos2);
 261
 262     SGVec3d diff = cartPos1 - cartPos2;
 263     double distance = norm(diff);
 264     return distance;
 265 }
 266
 267 double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){
 268
 269     double angle = 0;
 270     double distance = calcDistanceM(pos1, pos2);
 271     double daltM = pos1.getElevationM() - pos2.getElevationM();
 272
 273     if (fabs(distance) < SGLimits<float>::min()) {
 274         angle = 0;
 275     } else {
 276         double sAngle = daltM/range;
 277         sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
 278         angle = SGMiscd::rad2deg(asin(sAngle));
 279     }
 280
 281     return angle;
 282 }
 283
 284 void FGAIWingman::formateToAC(double dt){
 285
 286     double p_hdg, p_pch, p_rll, p_agl, p_ht, p_wow = 0;
 287
 288     setTgtOffsets(dt, 25);
 289
 290     if (_pnode != 0) {
 291         setParentPos();
 292         p_hdg = _p_hdg_node->getDoubleValue();
 293         p_pch = _p_pch_node->getDoubleValue();
 294         p_rll = _p_rll_node->getDoubleValue();
 295         p_ht  = _p_alt_node->getDoubleValue();
 296         setOffsetPos(_parentpos, p_hdg, p_pch, p_rll);
 297         setSpeed(_p_spd_node->getDoubleValue());
 298     }else {
 299         _setUserPos();
 300         p_hdg = manager->get_user_heading();
 301         p_pch = manager->get_user_pitch();
 302         p_rll = manager->get_user_roll();
 303         p_ht  = manager->get_user_altitude();
 304         setOffsetPos(userpos, p_hdg,p_pch, p_rll);
 305         setSpeed(manager->get_user_speed());
 306     }
 307
 308     // elapsed time has a random initialisation so that each
 309     // wingman moves differently
 310     _elapsed_time += dt;
 311
 312     // we derive a sine based factor to give us smoothly
 313     // varying error between -1 and 1
 314     double factor  = sin(SGMiscd::deg2rad(_elapsed_time * 10));
 315     double r_angle = 5 * factor;
 316     double p_angle = 2.5 * factor;
 317     double h_angle = 5 * factor;
 318     double h_feet  = 3 * factor;
 319
 320     p_agl = manager->get_user_agl();
 321     p_wow = user_WoW_node->getDoubleValue();
 322
 323     if(p_agl <= 10 || p_wow == 1) {
 324         _height = p_ht;
 325         //cout << "ht case1 " ;
 326     } else if (p_agl > 10 && p_agl <= 150 ) {
 327         setHt(p_ht, dt, 1.0);
 328         //cout << "ht case2 " ;
 329     } else if (p_agl > 150 && p_agl <= 250) {
 330         setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
 331         //cout << "ht case3 " ;
 332     } else{
 333         setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
 334         //cout << "ht case4 " ;
 335     }
 336
 337     pos.setElevationFt(_height);
 338     pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
 339     pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
 340
 341     // these calculations are unreliable at slow speeds
 342     // and we don't want random movement on the ground
 343     if(speed >= 10 && p_wow != 1) {
 344         setHdg(p_hdg + h_angle, dt, 0.9);
 345         setPch(p_pch + p_angle + _pitch_offset, dt, 0.9);
 346
 347         if (roll <= 115 && roll >= -115)
 348             setBnk(p_rll + r_angle + _roll_offset, dt, 0.5);
 349         else
 350             roll = p_rll + r_angle + _roll_offset;
 351
 352     } else {
 353         setHdg(p_hdg, dt, 0.9);
 354         setPch(p_pch + _pitch_offset, dt, 0.9);
 355         setBnk(p_rll + _roll_offset, dt, 0.9);
 356     }
 357
 358         setOffsetVelocity(dt, pos);
 359 }// end formateToAC
 360
 361 void FGAIWingman::Break(double dt) {
 362
 363     Run(dt);
 364
 365     //calculate the turn direction: 1 = right, -1 = left
 366     double rel_brg = calcRelBearingDeg(tgt_heading, hdg);
 367     int turn = SGMiscd::sign(rel_brg);
 368
 369     // set heading and pitch
 370     setHdg(tgt_heading, dt, _coeff_hdg);
 371     setPch(0, dt, _coeff_pch);
 372
 373     if (fabs(tgt_heading - hdg) >= 10)
 374         setBnk(45 * turn , dt, _coeff_bnk);
 375     else
 376         setBnk(0, dt, _coeff_bnk);
 377
 378 }  // end Break
 379
 380 void FGAIWingman::Join(double dt) {
 381
 382     double range, bearing, az2;
 383     double parent_hdg, parent_spd = 0;
 384     double p_hdg, p_pch, p_rll = 0;
 385
 386     setTgtOffsets(dt, 25);
 387
 388     if (_pnode != 0) {
 389         setParentPos();
 390         p_hdg = _p_hdg_node->getDoubleValue();
 391         p_pch = _p_pch_node->getDoubleValue();
 392         p_rll = _p_rll_node->getDoubleValue();
 393         setOffsetPos(_parentpos, p_hdg, p_pch, p_rll);
 394         parent_hdg = _p_hdg_node->getDoubleValue();
 395         parent_spd = _p_spd_node->getDoubleValue();
 396     }else {
 397         _setUserPos();
 398         p_hdg = manager->get_user_heading();
 399         p_pch = manager->get_user_pitch();
 400         p_rll = manager->get_user_roll();
 401         setOffsetPos(userpos, p_hdg, p_pch, p_rll);
 402         parent_hdg = manager->get_user_heading();
 403         parent_spd = manager->get_user_speed();
 404     }
 405
 406     setSpeed(parent_spd);
 407
 408     double distance = calcDistanceM(pos, _offsetpos);
 409     double daltM = _offsetpos.getElevationM() - pos.getElevationM();
 410     double limit = 10;
 411     double hdg_l_lim = parent_hdg - limit;
 412     SG_NORMALIZE_RANGE(hdg_l_lim, 0.0, 360.0);
 413     double hdg_r_lim = parent_hdg + limit;
 414     SG_NORMALIZE_RANGE(hdg_r_lim, 0.0, 360.0);
 415
 416     if (distance <= 2 && fabs(daltM) <= 2 &&
 417         (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
 418             _height = _offsetpos.getElevationFt();
 419             _formate_to_ac = true;
 420             _join = false;
 421
 422             SG_LOG(SG_AI, SG_ALERT, _name << " joined " << " RANGE " << distance
 423             << " SPEED " << speed );
 424
 425             return;
 426     }
 427
 428     geo_inverse_wgs_84(pos, _offsetpos, &bearing, &az2, &range);
 429
 430     double rel_brg   = calcRelBearingDeg(bearing, hdg);
 431     double recip_brg = calcRecipBearingDeg(bearing);
 432     double angle = calcAngle(distance,_offsetpos, pos);
 433     //double approx_angle = atan2(daltM, range);
 434     double frm_spd = 50; // formation speed
 435     double join_rnge = 1000.0;
 436 //    double recip_parent_hdg = calcRecipBearingDeg(parent_hdg);
 437     int turn = SGMiscd::sign(rel_brg);// turn direction: 1 = right, -1 = left
 438
 439     if (range <= join_rnge && (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
 440
 441         //these are the rules governing joining
 442
 443         if ((rel_brg <= -175 || rel_brg >= 175) && range <=10 ){
 444             // station is behind us - back up a bit
 445             setSpeed(parent_spd - ((frm_spd/join_rnge) * range));
 446             setHdg(recip_brg, dt, _coeff_hdg);
 447             setPch(angle, dt, _coeff_pch);
 448             //cout << _name << " backing up HEADING " << hdg
 449             //    << " RANGE " << range;
 450         } else if (rel_brg >= -5 || rel_brg <= 5) {
 451             // station is in front of us - slow down
 452             setSpeed(parent_spd + ((frm_spd/100) * range));
 453             //SGMiscd::clip
 454             setHdg(bearing, dt, 1.5);
 455             setPch(angle, dt, _coeff_pch);
 456             //cout << _name << " slowing HEADING " << hdg
 457             //    << " RANGE " << range <<endl;
 458         } else if ( range <=10 ){
 459             // station is to one side - equal speed and turn towards
 460             setSpd(parent_spd , dt, 2.0);
 461             setSpeed(_speed);
 462             setHdg(parent_hdg + (5 * turn), dt, _coeff_hdg);
 463             //cout << _name << " equal speed HEADING " << hdg
 464             //    << " RANGE " << range<< endl;
 465         } else {
 466             // we missed it - equal speed and turn to recip
 467             setSpd(parent_spd , dt, 2.0);
 468             setSpeed(_speed);
 469             setHdg(recip_brg, dt, _coeff_hdg);
 470             //cout << _name << " WHOOPS!! missed join HEADING " << hdg
 471             //    << " RANGE " << range<< endl;
 472         }
 473
 474     } else if (range <= join_rnge) {
 475         // we missed it - equal speed and turn to recip
 476         setSpd(parent_spd , dt, 2.0);
 477         setSpeed(_speed);
 478         setHdg(recip_brg , dt, _coeff_hdg);
 479         //cout << _name << " WHOOPS!! missed approach HEADING " << hdg
 480         //    << " " << recip_brg
 481         //    /*<< " " << recip_parent_hdg*/
 482         //    << " RANGE " << range<< endl;
 483     } else if (range > join_rnge && range <= 2000 ){
 484         //approach phase
 485         //cout << _name << " approach HEADING " << hdg
 486         //        << " RANGE " << range<< endl;
 487         setSpd(parent_spd + frm_spd, dt, 2.0);
 488         setSpeed(_speed);
 489         setHdg(bearing, dt, _coeff_hdg);
 490         setPch(angle, dt, _coeff_pch);
 491     } else {
 492         //hurry up
 493         //cout << _name << " hurry up HEADING " << hdg
 494         //        << " RANGE " << range<< endl;
 495         setSpd(_max_speed -10, dt, 2.0);
 496         setSpeed(_speed);
 497         setHdg(bearing, dt, _coeff_hdg);
 498         setPch(angle, dt, _coeff_pch);
 499     }
 500
 501     Run(dt);
 502
 503     // set roll
 504
 505     if (fabs(bearing - hdg) >= 10)
 506         setBnk(45 * turn , dt, _coeff_bnk);
 507     else
 508         setBnk(0, dt, _coeff_bnk);
 509
 510 }  // end Join
 511
 512 void FGAIWingman::Run(double dt) {
 513
 514     // don't let speed become negative
 515     SG_CLAMP_RANGE(speed, 100.0, _max_speed);
 516
 517     double speed_fps = speed * SG_KT_TO_FPS;
 518
 519     // calculate vertical and horizontal speed components
 520     if (speed == 0.0) {
 521         hs = vs = 0.0;
 522     } else {
 523         vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
 524         hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
 525     }
 526
 527     //cout << "vs hs " << vs << " " << hs << endl;
 528
 529     //resolve horizontal speed into north and east components:
 530     double speed_north_fps = cos(hdg / SG_RADIANS_TO_DEGREES) * hs;
 531     double speed_east_fps = sin(hdg / SG_RADIANS_TO_DEGREES) * hs;
 532
 533     // convert horizontal speed (fps) to degrees per second
 534     double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat;
 535     double speed_east_deg_sec  = speed_east_fps / ft_per_deg_lon;
 536
 537     //get wind components
 538     _wind_from_north = manager->get_wind_from_north();
 539     _wind_from_east = manager->get_wind_from_east();
 540
 541     // convert wind speed (fps) to degrees lat/lon per second
 542     double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
 543     double wind_speed_from_east_deg_sec  = _wind_from_east / ft_per_deg_lon;
 544
 545     //recombine the horizontal velocity components
 546     hs = sqrt(((speed_north_fps) * (speed_north_fps))
 547         + ((speed_east_fps)* (speed_east_fps )));
 548
 549     if (hs <= 0.00001)
 550         hs = 0;
 551
 552     if (vs <= 0.00001 && vs >= -0.00001)
 553         vs = 0;
 554
 555     //cout << "lat " << pos.getLatitudeDeg()<< endl;
 556     // set new position
 557     pos.setLatitudeDeg( pos.getLatitudeDeg()
 558             + (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt );
 559     pos.setLongitudeDeg( pos.getLongitudeDeg()
 560             + (speed_east_deg_sec - wind_speed_from_east_deg_sec ) * dt );
 561     pos.setElevationFt(pos.getElevationFt() + vs * dt);
 562
 563     //cout << _name << " run hs " << hs << " vs " << vs << endl;
 564
 565     // recalculate total speed
 566     if ( vs == 0 && hs == 0)
 567         speed = 0;
 568     else
 569         speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
 570
 571     // recalculate elevation and azimuth (velocity vectors)
 572     pitch = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
 573     hdg   = atan2((speed_east_fps),(speed_north_fps))* SG_RADIANS_TO_DEGREES;
 574
 575     // rationalise heading
 576     SG_NORMALIZE_RANGE(hdg, 0.0, 360.0);
 577
 578 }// end Run
 579
 580 // end AIWingman