1 // FGAIWingman - FGAIBllistic-derived class creates an AI Wingman
3 // Written by Vivian Meazza, started February 2008.
4 // - vivian.meazza at lineone.net
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.
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.
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.
24 #include <simgear/sg_inlines.h>
25 #include <simgear/math/SGMath.hxx>
28 #include "AIWingman.hxx"
30 FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman),
47 FGAIWingman::~FGAIWingman() {}
49 void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) {
53 FGAIBase::readFromScenario(scFileNode);
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));
79 void FGAIWingman::bind() {
80 FGAIBallistic::bind();
82 props->untie("controls/slave-to-ac");
84 props->tie("id", SGRawValueMethods<FGAIBase,int>(*this,
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));
101 props->tie("controls/break", SGRawValuePointer<bool>(&_break));
102 props->tie("controls/join", SGRawValuePointer<bool>(&_join));
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));
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));
129 props->tie("submodels/serviceable", SGRawValuePointer<bool>(&serviceable));
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));
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));
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));
161 void FGAIWingman::unbind() {
162 FGAIBallistic::unbind();
165 props->untie("SubID");
167 props->untie("orientation/pitch-deg");
168 props->untie("orientation/roll-deg");
169 props->untie("orientation/true-heading-deg");
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");
182 props->untie("submodels/serviceable");
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");
189 props->untie("load/rel-brg-to-user-deg");
190 props->untie("load/elev-to-user-deg");
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");
203 bool FGAIWingman::init(bool search_in_AI_path) {
204 if (!FGAIBallistic::init(search_in_AI_path))
210 void FGAIWingman::reinit() {
213 _tgt_x_offset = _x_offset;
214 _tgt_y_offset = _y_offset;
215 _tgt_z_offset = _z_offset;
226 setParentNodes(_selected_ac);
228 props->setStringValue("submodels/path", _path.c_str());
229 user_WoW_node = fgGetNode("gear/gear[1]/wow", true);
231 FGAIBallistic::reinit();
234 void FGAIWingman::update(double dt) {
236 // FGAIBallistic::update(dt);
241 setBrkHdg(_break_angle);
243 FGAIBase::update(dt);
244 tgt_altitude_ft = altitude_ft;
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);
262 SGVec3d diff = cartPos1 - cartPos2;
263 double distance = norm(diff);
267 double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){
270 double distance = calcDistanceM(pos1, pos2);
271 double daltM = pos1.getElevationM() - pos2.getElevationM();
273 if (fabs(distance) < SGLimits<float>::min()) {
276 double sAngle = daltM/range;
277 sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
278 angle = SGMiscd::rad2deg(asin(sAngle));
284 void FGAIWingman::formateToAC(double dt){
286 double p_hdg, p_pch, p_rll, p_agl, p_ht, p_wow = 0;
288 setTgtOffsets(dt, 25);
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());
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());
308 // elapsed time has a random initialisation so that each
309 // wingman moves differently
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;
320 p_agl = manager->get_user_agl();
321 p_wow = user_WoW_node->getDoubleValue();
323 if(p_agl <= 10 || p_wow == 1) {
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 " ;
333 setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
334 //cout << "ht case4 " ;
337 pos.setElevationFt(_height);
338 pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
339 pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
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);
347 if (roll <= 115 && roll >= -115)
348 setBnk(p_rll + r_angle + _roll_offset, dt, 0.5);
350 roll = p_rll + r_angle + _roll_offset;
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);
358 setOffsetVelocity(dt, pos);
361 void FGAIWingman::Break(double dt) {
365 //calculate the turn direction: 1 = right, -1 = left
366 double rel_brg = calcRelBearingDeg(tgt_heading, hdg);
367 int turn = SGMiscd::sign(rel_brg);
369 // set heading and pitch
370 setHdg(tgt_heading, dt, _coeff_hdg);
371 setPch(0, dt, _coeff_pch);
373 if (fabs(tgt_heading - hdg) >= 10)
374 setBnk(45 * turn , dt, _coeff_bnk);
376 setBnk(0, dt, _coeff_bnk);
380 void FGAIWingman::Join(double dt) {
382 double range, bearing, az2;
383 double parent_hdg, parent_spd = 0;
384 double p_hdg, p_pch, p_rll = 0;
386 setTgtOffsets(dt, 25);
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();
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();
406 setSpeed(parent_spd);
408 double distance = calcDistanceM(pos, _offsetpos);
409 double daltM = _offsetpos.getElevationM() - pos.getElevationM();
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);
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;
422 SG_LOG(SG_AI, SG_ALERT, _name << " joined " << " RANGE " << distance
423 << " SPEED " << speed );
428 geo_inverse_wgs_84(pos, _offsetpos, &bearing, &az2, &range);
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
439 if (range <= join_rnge && (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
441 //these are the rules governing joining
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));
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);
462 setHdg(parent_hdg + (5 * turn), dt, _coeff_hdg);
463 //cout << _name << " equal speed HEADING " << hdg
464 // << " RANGE " << range<< endl;
466 // we missed it - equal speed and turn to recip
467 setSpd(parent_spd , dt, 2.0);
469 setHdg(recip_brg, dt, _coeff_hdg);
470 //cout << _name << " WHOOPS!! missed join HEADING " << hdg
471 // << " RANGE " << range<< endl;
474 } else if (range <= join_rnge) {
475 // we missed it - equal speed and turn to recip
476 setSpd(parent_spd , dt, 2.0);
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 ){
485 //cout << _name << " approach HEADING " << hdg
486 // << " RANGE " << range<< endl;
487 setSpd(parent_spd + frm_spd, dt, 2.0);
489 setHdg(bearing, dt, _coeff_hdg);
490 setPch(angle, dt, _coeff_pch);
493 //cout << _name << " hurry up HEADING " << hdg
494 // << " RANGE " << range<< endl;
495 setSpd(_max_speed -10, dt, 2.0);
497 setHdg(bearing, dt, _coeff_hdg);
498 setPch(angle, dt, _coeff_pch);
505 if (fabs(bearing - hdg) >= 10)
506 setBnk(45 * turn , dt, _coeff_bnk);
508 setBnk(0, dt, _coeff_bnk);
512 void FGAIWingman::Run(double dt) {
514 // don't let speed become negative
515 SG_CLAMP_RANGE(speed, 100.0, _max_speed);
517 double speed_fps = speed * SG_KT_TO_FPS;
519 // calculate vertical and horizontal speed components
523 vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
524 hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
527 //cout << "vs hs " << vs << " " << hs << endl;
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;
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;
537 //get wind components
538 _wind_from_north = manager->get_wind_from_north();
539 _wind_from_east = manager->get_wind_from_east();
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;
545 //recombine the horizontal velocity components
546 hs = sqrt(((speed_north_fps) * (speed_north_fps))
547 + ((speed_east_fps)* (speed_east_fps )));
552 if (vs <= 0.00001 && vs >= -0.00001)
555 //cout << "lat " << pos.getLatitudeDeg()<< endl;
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);
563 //cout << _name << " run hs " << hs << " vs " << vs << endl;
565 // recalculate total speed
566 if ( vs == 0 && hs == 0)
569 speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
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;
575 // rationalise heading
576 SG_NORMALIZE_RANGE(hdg, 0.0, 360.0);