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