]> git.mxchange.org Git - flightgear.git/blob - src/AIModel/AIShip.cxx
ATC/Traffic doesn’t crash reset.
[flightgear.git] / src / AIModel / AIShip.cxx
1 // FGAIShip - FGAIBase-derived class creates an AI ship
2 //
3 // Written by David Culp, started October 2003.
4 // with major amendments and additions by Vivian Meazza, 2004 - 2007
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 <cmath>
25
26 #ifdef _MSC_VER
27 #  include <float.h>
28     double fgIsFinite(double x) { return _finite(x); }
29 #else
30     double fgIsFinite(double x) { return std::isfinite(x); }
31 #endif
32
33 #include <simgear/sg_inlines.h>
34 #include <simgear/math/sg_geodesy.hxx>
35 #include <simgear/timing/sg_time.hxx>
36 #include <simgear/math/sg_random.h>
37
38 #include <simgear/scene/util/SGNodeMasks.hxx>
39 #include <Scenery/scenery.hxx>
40 #include <Main/globals.hxx>
41
42 #include "AIShip.hxx"
43
44 using std::string;
45
46 FGAIShip::FGAIShip(object_type ot) :
47 // allow HOT to be enabled
48 FGAIBase(ot, true),
49
50
51 _waiting(false),
52 _new_waypoint(true),
53 _tunnel(false),
54 _initial_tunnel(false),
55 _restart(false),
56 _hdg_constant(0.01),
57 _limit(100),
58 _elevation_m(0),
59 _elevation_ft(0),
60 _tow_angle(0),
61 _missed_count(0),
62 _wp_range(0),
63 _dt_count(0),
64 _next_run(0),
65 _roll_constant(0.001),
66 _roll_factor(-0.0083335),
67 _old_range(0),
68 _range_rate(0),
69 _missed_time_sec(30),
70 _day(86400),
71 _lead_angle(0),
72 _xtrack_error(0),
73 _curr_alt(0),
74 _prev_alt(0),
75 _until_time(""),
76 _fp_init(false),
77 _missed(false)
78 {
79         invisible = false;
80 }
81
82 FGAIShip::~FGAIShip() {
83 }
84
85 void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) {
86
87     if (!scFileNode)
88         return;
89
90     FGAIBase::readFromScenario(scFileNode);
91
92     setRudder(scFileNode->getFloatValue("rudder", 0.0));
93     setName(scFileNode->getStringValue("name", "Titanic"));
94     setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000));
95     const std::string& flightplan = scFileNode->getStringValue("flightplan");
96     setRepeat(scFileNode->getBoolValue("repeat", false));
97     setRestart(scFileNode->getBoolValue("restart", false));
98     setStartTime(scFileNode->getStringValue("time", ""));
99     setLeadAngleGain(scFileNode->getDoubleValue("lead-angle-gain", 1.5));
100     setLeadAngleLimit(scFileNode->getDoubleValue("lead-angle-limit-deg", 15));
101     setLeadAngleProp(scFileNode->getDoubleValue("lead-angle-proportion", 0.75));
102     setRudderConstant(scFileNode->getDoubleValue("rudder-constant", 0.5));
103     setFixedTurnRadius(scFileNode->getDoubleValue("fixed-turn-radius-ft", 500));
104     setSpeedConstant(scFileNode->getDoubleValue("speed-constant", 0.5));
105     setSMPath(scFileNode->getStringValue("submodel-path", ""));
106     setRollFactor(scFileNode->getDoubleValue("roll-factor", 1));
107
108     if (!flightplan.empty()) {
109         SG_LOG(SG_AI, SG_ALERT, "getting flightplan: " << _name );
110
111         FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan);
112         setFlightPlan(fp);
113     }
114
115 }
116
117 bool FGAIShip::init(bool search_in_AI_path) {
118     reinit();
119     return FGAIBase::init(search_in_AI_path);
120 }
121
122 void FGAIShip::reinit()
123 {
124     prev = 0; // the one behind you
125     curr = 0; // the one ahead
126     next = 0; // the next plus 1
127
128     props->setStringValue("name", _name.c_str());
129     props->setStringValue("waypoint/name-prev", _prev_name.c_str());
130     props->setStringValue("waypoint/name-curr", _curr_name.c_str());
131     props->setStringValue("waypoint/name-next", _next_name.c_str());
132     props->setStringValue("submodels/path", _path.c_str());
133     props->setStringValue("waypoint/start-time", _start_time.c_str());
134     props->setStringValue("waypoint/wait-until-time", _until_time.c_str());
135
136     _hdg_lock = false;
137     _rudder = 0.0;
138     no_roll = false;
139
140     _rd_turn_radius_ft = _sp_turn_radius_ft = turn_radius_ft;
141
142     if (fp)
143         _fp_init = initFlightPlan();
144
145     FGAIBase::reinit();
146 }
147
148 void FGAIShip::bind() {
149     FGAIBase::bind();
150
151     tie("surface-positions/rudder-pos-deg",
152         SGRawValuePointer<float>(&_rudder));
153     tie("controls/heading-lock",
154         SGRawValuePointer<bool>(&_hdg_lock));
155     tie("controls/tgt-speed-kts",
156         SGRawValuePointer<double>(&tgt_speed));
157     tie("controls/tgt-heading-degs",
158         SGRawValuePointer<double>(&tgt_heading));
159     tie("controls/constants/roll-factor",
160         SGRawValuePointer<double>(&_roll_factor));
161     tie("controls/constants/roll",
162         SGRawValuePointer<double>(&_roll_constant));
163     tie("controls/constants/rudder",
164         SGRawValuePointer<double>(&_rudder_constant));
165     tie("controls/constants/speed",
166         SGRawValuePointer<double>(&_speed_constant));
167     tie("waypoint/range-nm",
168         SGRawValuePointer<double>(&_wp_range));
169     tie("waypoint/brg-deg",
170         SGRawValuePointer<double>(&_course));
171     tie("waypoint/rangerate-nm-sec",
172         SGRawValuePointer<double>(&_range_rate));
173     tie("waypoint/new",
174         SGRawValuePointer<bool>(&_new_waypoint));
175     tie("waypoint/missed",
176         SGRawValuePointer<bool>(&_missed));
177     tie("waypoint/missed-count-sec",
178         SGRawValuePointer<double>(&_missed_count));
179     tie("waypoint/missed-range-nm",
180         SGRawValuePointer<double>(&_missed_range));
181     tie("waypoint/missed-time-sec",
182         SGRawValuePointer<double>(&_missed_time_sec));
183     tie("waypoint/wait-count-sec",
184         SGRawValuePointer<double>(&_wait_count));
185     tie("waypoint/xtrack-error-ft",
186         SGRawValuePointer<double>(&_xtrack_error));
187     tie("waypoint/waiting",
188         SGRawValuePointer<bool>(&_waiting));
189     tie("waypoint/lead-angle-deg",
190         SGRawValuePointer<double>(&_lead_angle));
191     tie("waypoint/tunnel",
192         SGRawValuePointer<bool>(&_tunnel));
193     tie("waypoint/alt-curr-m",
194         SGRawValuePointer<double>(&_curr_alt));
195     tie("waypoint/alt-prev-m",
196         SGRawValuePointer<double>(&_prev_alt));
197     tie("submodels/serviceable",
198         SGRawValuePointer<bool>(&_serviceable));
199     tie("controls/turn-radius-ft",
200         SGRawValuePointer<double>(&turn_radius_ft));
201     tie("controls/turn-radius-corrected-ft",
202         SGRawValuePointer<double>(&_rd_turn_radius_ft));
203     tie("controls/constants/lead-angle/gain",
204         SGRawValuePointer<double>(&_lead_angle_gain));
205     tie("controls/constants/lead-angle/limit-deg",
206         SGRawValuePointer<double>(&_lead_angle_limit));
207     tie("controls/constants/lead-angle/proportion",
208         SGRawValuePointer<double>(&_proportion));
209     tie("controls/fixed-turn-radius-ft",
210         SGRawValuePointer<double>(&_fixed_turn_radius));
211     tie("controls/restart",
212         SGRawValuePointer<bool>(&_restart));
213     tie("velocities/speed-kts",
214         SGRawValuePointer<double>(&speed));
215 }
216
217 void FGAIShip::update(double dt) {
218     //SG_LOG(SG_AI, SG_ALERT, "updating Ship: " << _name <<hdg<<pitch<<roll);
219     // For computation of rotation speeds we just use finite differences here.
220     // That is perfectly valid since this thing is not driven by accelerations
221     // but by just apply discrete changes at its velocity variables.
222     // Update the velocity information stored in those nodes.
223     // Transform that one to the horizontal local coordinate system.
224     SGQuatd ec2hl = SGQuatd::fromLonLat(pos);
225     // The orientation of the ship wrt the horizontal local frame
226     SGQuatd hl2body = SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
227     // and postrotate the orientation of the AIModel wrt the horizontal
228     // local frame
229     SGQuatd ec2body = ec2hl*hl2body;
230     // The cartesian position of the ship in the wgs84 world
231     //SGVec3d cartPos = SGVec3d::fromGeod(pos);
232
233     // The simulation time this transform is meant for
234     aip.setReferenceTime(globals->get_sim_time_sec());
235
236     // Compute the velocity in m/s in the body frame
237     aip.setBodyLinearVelocity(SGVec3d(0.51444444*speed, 0, 0));
238
239     FGAIBase::update(dt);
240     Run(dt);
241     Transform();
242     if (fp)
243         setXTrackError();
244
245     // Only change these values if we are able to compute them safely
246     if (SGLimits<double>::min() < dt) {
247         // Now here is the finite difference ...
248
249         // Transform that one to the horizontal local coordinate system.
250         SGQuatd ec2hlNew = SGQuatd::fromLonLat(pos);
251         // compute the new orientation
252         SGQuatd hl2bodyNew = SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
253         // The rotation difference
254         SGQuatd dOr = inverse(ec2body)*ec2hlNew*hl2bodyNew;
255         SGVec3d dOrAngleAxis;
256         dOr.getAngleAxis(dOrAngleAxis);
257         // divided by the time difference provides a rotation speed vector
258         dOrAngleAxis /= dt;
259
260         aip.setBodyAngularVelocity(dOrAngleAxis);
261     }
262 }
263
264 void FGAIShip::Run(double dt) {
265     if (_fp_init)
266         ProcessFlightPlan(dt);
267
268     const string& type = getTypeString();
269
270     double alpha;
271     double rudder_limit;
272     double raw_roll;
273
274     // adjust speed
275     double speed_diff = tgt_speed - speed;
276
277     if (fabs(speed_diff) > 0.1) {
278
279         if (speed_diff > 0.0)
280             speed += _speed_constant * dt;
281
282         if (speed_diff < 0.0)
283             speed -= _speed_constant * dt;
284
285     }
286
287     // do not allow unreasonable speeds
288     SG_CLAMP_RANGE(speed, -_limit * 0.75, _limit);
289
290     // convert speed to degrees per second
291     speed_north_deg_sec = cos(hdg / SGD_RADIANS_TO_DEGREES)
292         * speed * 1.686 / ft_per_deg_lat;
293     speed_east_deg_sec = sin(hdg / SGD_RADIANS_TO_DEGREES)
294         * speed * 1.686 / ft_per_deg_lon;
295
296     // set new position
297     //cout << _name << " " << type << " run: " << _elevation_m << " " <<_elevation_ft << endl;
298     pos.setLatitudeDeg(pos.getLatitudeDeg() + speed_north_deg_sec * dt);
299     pos.setLongitudeDeg(pos.getLongitudeDeg() + speed_east_deg_sec * dt);
300     pos.setElevationFt(tgt_altitude_ft);
301     pitch = tgt_pitch;
302
303     // adjust heading based on current _rudder angle
304     if (turn_radius_ft <= 0)
305         turn_radius_ft = 0; // don't allow nonsense values
306
307     if (_rudder > 45)
308         _rudder = 45;
309
310     if (_rudder < -45)
311         _rudder = -45;
312
313
314     //we assume that at slow speed ships will manoeuvre using engines/bow thruster
315         if(type == "ship" || type == "carrier" || type == "escort"){
316
317                 if (fabs(speed)<=5)
318                         _sp_turn_radius_ft = _fixed_turn_radius;
319                 else {
320                         // adjust turn radius for speed. The equation is very approximate.
321                         // we need to allow for negative speeds
322                         _sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
323                 }
324
325         } else {
326                 
327                 if (fabs(speed) <= 40)
328            _sp_turn_radius_ft = _fixed_turn_radius;
329                 else {
330                         // adjust turn radius for speed. 
331                         _sp_turn_radius_ft = turn_radius_ft;
332                 }
333         }
334
335
336     if (_rudder <= -0.25 || _rudder >= 0.25) {
337         // adjust turn radius for _rudder angle. The equation is even more approximate.
338         float a = 19;
339         float b = -0.2485;
340         float c = 0.543;
341
342         _rd_turn_radius_ft = (a * exp(b * fabs(_rudder)) + c) * _sp_turn_radius_ft;
343
344         // calculate the angle, alpha, subtended by the arc traversed in time dt
345         alpha = ((speed * 1.686 * dt) / _rd_turn_radius_ft) * SG_RADIANS_TO_DEGREES;
346         //cout << _name << " alpha " << alpha << endl;
347         // make sure that alpha is applied in the right direction
348         hdg += alpha * sign(_rudder);
349
350         SG_NORMALIZE_RANGE(hdg, 0.0, 360.0);
351
352         //adjust roll for rudder angle and speed. Another bit of voodoo
353         raw_roll = _roll_factor * speed * _rudder;
354     } else {
355         // _rudder angle is 0
356         raw_roll = 0;
357     }
358
359     //low pass filter
360     if (speed < 0)
361         roll = -roll;
362
363     roll = (raw_roll * _roll_constant) + (roll * (1 - _roll_constant));
364
365     // adjust target _rudder angle if heading lock engaged
366     double rudder_diff = 0.0;
367     if (_hdg_lock) {
368         double rudder_sense = 0.0;
369         double diff = fabs(hdg - tgt_heading);
370         //cout << "_rudder diff" << diff << endl;
371         if (diff > 180)
372             diff = fabs(diff - 360);
373
374         double sum = hdg + diff;
375
376         if (sum > 360.0)
377             sum -= 360.0;
378
379         if (fabs(sum - tgt_heading)< 1.0)
380             rudder_sense = 1.0;
381         else
382             rudder_sense = -1.0;
383
384         if (speed < 0)
385             rudder_sense = -rudder_sense;
386
387         if (diff < 15)
388             _tgt_rudder = diff * rudder_sense;
389         else
390             _tgt_rudder = 45 * rudder_sense;
391
392         rudder_diff = _tgt_rudder - _rudder;
393     }
394
395     // set the _rudder limit by speed
396     if (type == "ship" || type == "carrier" || type == "escort"){
397
398         if (speed <= 40)
399             rudder_limit = (-0.825 * speed) + 35;
400         else
401             rudder_limit = 2;
402
403     } else
404         rudder_limit = 20;
405
406     if (fabs(rudder_diff)> 0.1) { // apply dead zone
407
408         if (rudder_diff > 0.0) {
409             _rudder += _rudder_constant * dt;
410
411             if (_rudder > rudder_limit) // apply the _rudder limit
412                 _rudder = rudder_limit;
413
414         } else if (rudder_diff < 0.0) {
415             _rudder -= _rudder_constant * dt;
416
417             if (_rudder < -rudder_limit)
418                 _rudder = -rudder_limit;
419
420         }
421
422         // do calculations for radar
423         UpdateRadar(manager);
424     }
425 }//end function
426
427 void FGAIShip::AccelTo(double speed) {
428     tgt_speed = speed;
429 }
430
431 void FGAIShip::PitchTo(double angle) {
432     tgt_pitch = angle;
433 }
434
435 void FGAIShip::RollTo(double angle) {
436     tgt_roll = angle;
437 }
438
439 void FGAIShip::YawTo(double angle) {
440 }
441
442 void FGAIShip::ClimbTo(double altitude) {
443     tgt_altitude_ft = altitude;
444     _setAltitude(altitude);
445 }
446
447 void FGAIShip::TurnTo(double heading) {
448     tgt_heading = heading - _lead_angle + _tow_angle;
449     SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0);
450     _hdg_lock = true;
451 }
452
453 double FGAIShip::sign(double x) {
454     if (x < 0.0)
455         return -1.0;
456     else
457         return 1.0;
458 }
459
460 void FGAIShip::setFlightPlan(FGAIFlightPlan* f) {
461     fp = f;
462 }
463
464 void FGAIShip::setStartTime(const string& st) {
465     _start_time = st;
466 }
467
468 void FGAIShip::setUntilTime(const string& ut) {
469     _until_time = ut;
470     props->setStringValue("waypoint/wait-until-time", _until_time.c_str());
471 }
472
473 void FGAIShip::setCurrName(const string& c) {
474     _curr_name = c;
475     props->setStringValue("waypoint/name-curr", _curr_name.c_str());
476 }
477
478 void FGAIShip::setNextName(const string& n) {
479     _next_name = n;
480     props->setStringValue("waypoint/name-next", _next_name.c_str());
481 }
482
483 void FGAIShip::setPrevName(const string& p) {
484     _prev_name = p;
485     props->setStringValue("waypoint/name-prev", _prev_name.c_str());
486 }
487
488 void FGAIShip::setRepeat(bool r) {
489     _repeat = r;
490 }
491
492 void FGAIShip::setRestart(bool r) {
493     _restart = r;
494 }
495
496 void FGAIShip::setMissed(bool m) {
497     _missed = m;
498     props->setBoolValue("waypoint/missed", _missed);
499 }
500
501 void FGAIShip::setRudder(float r) {
502     _rudder = r;
503 }
504
505 void FGAIShip::setRoll(double rl) {
506     roll = rl;
507 }
508
509 void FGAIShip::setLeadAngleGain(double g) {
510     _lead_angle_gain = g;
511 }
512
513 void FGAIShip::setLeadAngleLimit(double l) {
514     _lead_angle_limit = l;
515 }
516
517 void FGAIShip::setLeadAngleProp(double p) {
518     _proportion = p;
519 }
520
521 void FGAIShip::setRudderConstant(double rc) {
522     _rudder_constant = rc;
523 }
524
525 void FGAIShip::setSpeedConstant(double sc) {
526     _speed_constant = sc;
527 }
528
529 void FGAIShip::setFixedTurnRadius(double ftr) {
530     _fixed_turn_radius = ftr;
531 }
532
533 void FGAIShip::setRollFactor(double rf) {
534     _roll_factor = rf * -0.0083335;
535 }
536
537 void FGAIShip::setInitialTunnel(bool t) {
538     _initial_tunnel = t;
539     setTunnel(_initial_tunnel);
540 }
541
542 void FGAIShip::setTunnel(bool t) {
543     _tunnel = t;
544 }
545
546 void FGAIShip::setWPNames() {
547
548     if (prev != 0)
549         setPrevName(prev->getName());
550     else
551         setPrevName("");
552
553     if (curr != 0)
554         setCurrName(curr->getName());
555     else{
556         setCurrName("");
557         SG_LOG(SG_AI, SG_ALERT, "AIShip: current wp name error" );
558     }
559
560     if (next != 0)
561         setNextName(next->getName());
562     else
563         setNextName("");
564
565     SG_LOG(SG_AI, SG_DEBUG, "AIShip: prev wp name " << prev->getName());
566     SG_LOG(SG_AI, SG_DEBUG, "AIShip: current wp name " << curr->getName());
567     SG_LOG(SG_AI, SG_DEBUG, "AIShip: next wp name " << next->getName());
568
569 }
570
571 double FGAIShip::getRange(double lat, double lon, double lat2, double lon2) const {
572
573     double course, distance, az2;
574
575     //calculate the bearing and range of the second pos from the first
576     geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &az2, &distance);
577     distance *= SG_METER_TO_NM;
578     return distance;
579 }
580
581 double FGAIShip::getCourse(double lat, double lon, double lat2, double lon2) const {
582
583     double course, distance, recip;
584
585     //calculate the bearing and range of the second pos from the first
586     geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &recip, &distance);
587     if (tgt_speed >= 0) {
588         return course;
589         SG_LOG(SG_AI, SG_DEBUG, "AIShip: course " << course);
590     } else {
591         return recip;
592         SG_LOG(SG_AI, SG_DEBUG, "AIShip: recip " << recip);
593     }
594 }
595
596 void FGAIShip::ProcessFlightPlan(double dt) {
597
598     if ( dt < 0.00001 ) {
599         return;
600     }
601
602     double time_sec = getDaySeconds();
603
604     _dt_count += dt;
605
606     ///////////////////////////////////////////////////////////////////////////
607     // Check Execution time (currently once every 1 sec)
608     // Add a bit of randomization to prevent the execution of all flight plans
609     // in synchrony, which can add significant periodic framerate flutter.
610     ///////////////////////////////////////////////////////////////////////////
611
612     //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
613     if (_dt_count < _next_run && _start_sec < time_sec)
614         return;
615
616     _next_run = 0.05 + (0.025 * sg_random());
617
618     double until_time_sec = 0;
619     _missed = false;
620
621     // check to see if we've reached the point for our next turn
622     // if the range to the waypoint is less than the calculated turn
623     // radius we can start the turn to the next leg
624     _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->getLatitude(), curr->getLongitude());
625     _range_rate = (_wp_range - _old_range) / _dt_count;
626     double sp_turn_radius_nm = _sp_turn_radius_ft / 6076.1155;
627     // we need to try to identify a _missed waypoint
628
629     // calculate the time needed to turn through an arc of 90 degrees,
630     // and allow a time error
631     if (speed != 0)
632         _missed_time_sec = 10 + ((SGD_PI * sp_turn_radius_nm * 60 * 60) / (2 * fabs(speed)));
633     else
634         _missed_time_sec = 10;
635
636     _missed_range = 4 * sp_turn_radius_nm;
637
638     //cout << _name << " range_rate " << _range_rate << " " << _new_waypoint<< endl ;
639     //if ((_range_rate > 0) && !_new_waypoint){
640     if (_range_rate > 0 && _wp_range < _missed_range && !_new_waypoint){
641         _missed_count += _dt_count;
642     }
643
644     if (_missed_count >= 120)
645         setMissed(true);
646     else if (_missed_count >= _missed_time_sec)
647         setMissed(true);
648     else
649         setMissed(false);
650
651     _old_range = _wp_range;
652     setWPNames();
653
654     if ((_wp_range < (sp_turn_radius_nm * 1.25)) || _missed || (_waiting && !_new_waypoint)) {
655
656         if (_next_name == "TUNNEL"){
657             _tunnel = !_tunnel;
658
659             SG_LOG(SG_AI, SG_DEBUG, "AIShip: " << _name << " " << sp_turn_radius_nm );
660
661             fp->IncrementWaypoint(false);
662             next = fp->getNextWaypoint();
663
664             if (next->getName() == "WAITUNTIL" || next->getName() == "WAIT"
665                 || next->getName() == "END" || next->getName() == "TUNNEL")
666                 return;
667
668             prev = curr;
669             fp->IncrementWaypoint(false);
670             curr = fp->getCurrentWaypoint();
671             next = fp->getNextWaypoint();
672
673         }else if(_next_name == "END" || fp->getNextWaypoint() == 0) {
674
675             if (_repeat) {
676                 SG_LOG(SG_AI, SG_INFO, "AIShip: "<< _name << " Flightplan repeating ");
677                 fp->restart();
678                 prev = curr;
679                 curr = fp->getCurrentWaypoint();
680                 next = fp->getNextWaypoint();
681                 setWPNames();
682                 _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->getLatitude(), curr->getLongitude());
683                 _old_range = _wp_range;
684                 _range_rate = 0;
685                 _new_waypoint = true;
686                 _missed_count = 0;
687                 _lead_angle = 0;
688                 AccelTo(prev->getSpeed());
689             } else if (_restart){
690                 SG_LOG(SG_AI, SG_INFO, "AIShip: " << _name << " Flightplan restarting ");
691                 _missed_count = 0;
692                 initFlightPlan();
693             } else {
694                 SG_LOG(SG_AI, SG_ALERT, "AIShip: " << _name << " Flightplan dying ");
695                 setDie(true);
696                 _dt_count = 0;
697                 return;
698             }
699
700         } else if (_next_name == "WAIT") {
701
702             if (_wait_count < next->getTime_sec()) {
703                 SG_LOG(SG_AI, SG_DEBUG, "AIShip: " << _name << " waiting ");
704                 setSpeed(0);
705                 _waiting = true;
706                 _wait_count += _dt_count;
707                 _dt_count = 0;
708                 _lead_angle = 0;
709                 return;
710             } else {
711                 SG_LOG(SG_AI, SG_DEBUG, "AIShip: " << _name
712                     << " wait done: getting new waypoints ");
713                 _waiting = false;
714                 _wait_count = 0;
715                 fp->IncrementWaypoint(false);
716                 next = fp->getNextWaypoint();
717
718                 if (next->getName() == "WAITUNTIL" || next->getName() == "WAIT"
719                     || next->getName() == "END" || next->getName() == "TUNNEL")
720                     return;
721
722                 prev = curr;
723                 fp->IncrementWaypoint(false);
724                 curr = fp->getCurrentWaypoint();
725                 next = fp->getNextWaypoint();
726             }
727
728         } else if (_next_name == "WAITUNTIL") {
729             time_sec = getDaySeconds();
730             until_time_sec = processTimeString(next->getTime());
731             _until_time = next->getTime();
732             setUntilTime(next->getTime());
733             if (until_time_sec > time_sec) {
734                 SG_LOG(SG_AI, SG_INFO, "AIShip: " << _name << " "
735                     << curr->getName() << " waiting until: "
736                     << _until_time << " " << until_time_sec << " now " << time_sec );
737                 setSpeed(0);
738                 _lead_angle = 0;
739                 _waiting = true;
740                 return;
741             } else {
742                 SG_LOG(SG_AI, SG_INFO, "AIShip: "
743                     << _name << " wait until done: getting new waypoints ");
744                 setUntilTime("");
745                 fp->IncrementWaypoint(false);
746
747                 while (next->getName() == "WAITUNTIL") {
748                     fp->IncrementWaypoint(false);
749                     next = fp->getNextWaypoint();
750                 }
751
752                 if (next->getName() == "WAIT")
753                     return;
754
755                 prev = curr;
756                 fp->IncrementWaypoint(false);
757                 curr = fp->getCurrentWaypoint();
758                 next = fp->getNextWaypoint();
759                 _waiting = false;
760             }
761
762         } else {
763             //now reorganise the waypoints, so that next becomes current and so on
764             SG_LOG(SG_AI, SG_DEBUG, "AIShip: " << _name << " getting new waypoints ");
765             fp->IncrementWaypoint(false);
766             prev = fp->getPreviousWaypoint(); //first waypoint
767             curr = fp->getCurrentWaypoint();  //second waypoint
768             next = fp->getNextWaypoint();     //third waypoint (might not exist!)
769         }
770
771         setWPNames();
772         _new_waypoint = true;
773         _missed_count = 0;
774         _range_rate = 0;
775         _lead_angle = 0;
776         _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->getLatitude(), curr->getLongitude());
777         _old_range = _wp_range;
778         setWPPos();
779         object_type type = getType();
780
781         if (type != 10)
782             AccelTo(prev->getSpeed());
783
784         _curr_alt = curr->getAltitude();
785         _prev_alt = prev->getAltitude();
786
787     } else {
788         _new_waypoint = false;
789     }
790
791     //   now revise the required course for the next way point
792     _course = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->getLatitude(), curr->getLongitude());
793
794     if (fgIsFinite(_course))
795         TurnTo(_course);
796     else
797         SG_LOG(SG_AI, SG_ALERT, "AIShip: Bearing or Range is not a finite number");
798
799     _dt_count = 0;
800 } // end Processing FlightPlan
801
802 bool FGAIShip::initFlightPlan() {
803
804     SG_LOG(SG_AI, SG_ALERT, "AIShip: " << _name << " initializing waypoints ");
805
806     bool init = false;
807     _start_sec = 0;
808     _tunnel = _initial_tunnel;
809
810     fp->restart();
811     fp->IncrementWaypoint(false);
812
813     prev = fp->getPreviousWaypoint(); //first waypoint
814     curr = fp->getCurrentWaypoint();  //second waypoint
815     next = fp->getNextWaypoint();     //third waypoint (might not exist!)
816
817     while (curr->getName() == "WAIT" || curr->getName() == "WAITUNTIL") {  // don't wait when initialising
818         SG_LOG(SG_AI, SG_DEBUG, "AIShip: " << _name << " re-initializing waypoints ");
819         fp->IncrementWaypoint(false);
820         curr = fp->getCurrentWaypoint();
821         next = fp->getNextWaypoint();
822     } // end while loop
823
824     if (!_start_time.empty()){
825         _start_sec = processTimeString(_start_time);
826         double day_sec = getDaySeconds();
827
828         if (_start_sec < day_sec){
829             //cout << "flight plan has already started " << _start_time << endl;
830             init = advanceFlightPlan(_start_sec, day_sec);
831
832         } else if (_start_sec > day_sec && _repeat) {
833             //cout << "flight plan has not started, " << _start_time;
834             //cout << "offsetting start time by -24 hrs" << endl;
835             _start_sec -= _day;
836             init = advanceFlightPlan(_start_sec, day_sec);
837         }
838
839         if (init)
840             _start_sec = 0; // set to zero for an immediate start of the Flight Plan
841         else {
842             fp->restart();
843             fp->IncrementWaypoint(false);
844             prev = fp->getPreviousWaypoint();
845             curr = fp->getCurrentWaypoint();
846             next = fp->getNextWaypoint();
847             return false;
848         }
849
850     } else {
851         setLatitude(prev->getLatitude());
852         setLongitude(prev->getLongitude());
853         setSpeed(prev->getSpeed());
854     }
855
856     setWPNames();
857     setHeading(getCourse(prev->getLatitude(), prev->getLongitude(), curr->getLatitude(), curr->getLongitude()));
858     _wp_range = getRange(prev->getLatitude(), prev->getLongitude(), curr->getLatitude(), curr->getLongitude());
859     _old_range = _wp_range;
860     _range_rate = 0;
861     _hdg_lock = true;
862     _missed = false;
863     _missed_count = 0;
864     _new_waypoint = true;
865
866     SG_LOG(SG_AI, SG_ALERT, "AIShip: " << _name << " done initialising waypoints " << _tunnel);
867     if (prev)
868         init = true;
869
870     if (init)
871         return true;
872     else
873         return false;
874
875 } // end of initialization
876
877
878 double FGAIShip::processTimeString(const string& theTime) {
879
880     int Hour;
881     int Minute;
882     int Second;
883
884     // first split theTime string into
885     //  hour, minute, second and convert to int;
886     Hour   = atoi(theTime.substr(0,2).c_str());
887     Minute = atoi(theTime.substr(3,5).c_str());
888     Second = atoi(theTime.substr(6,8).c_str());
889
890     // offset by a day-sec to allow for starting a day earlier
891     double time_seconds = Hour * 3600
892         + Minute * 60
893         + Second;
894
895     return time_seconds;
896 }
897
898 double FGAIShip::getDaySeconds () {
899     // Date and time
900     struct tm *t = globals->get_time_params()->getGmt();
901
902     double day_seconds = t->tm_hour * 3600
903         + t->tm_min * 60
904         + t->tm_sec;
905
906     return day_seconds;
907 }
908
909 bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) {
910
911     double elapsed_sec = start_sec;
912     double distance_nm = 0;
913
914     //cout << "advancing flight plan start_sec: " << start_sec << " " << day_sec << endl;
915
916     while ( elapsed_sec < day_sec ) {
917
918         if (next->getName() == "END" || fp->getNextWaypoint() == 0) {
919
920             if (_repeat ) {
921                 //cout << _name << ": " << "restarting flightplan" << endl;
922                 fp->restart();
923                 curr = fp->getCurrentWaypoint();
924                 next = fp->getNextWaypoint();
925             } else {
926                 //cout << _name << ": " << "ending flightplan" << endl;
927                 setDie(true);
928                 return false;
929             }
930
931         } else if (next->getName() == "WAIT") {
932             //cout << _name << ": begin WAIT: " << prev->name << " ";
933             //cout << curr->name << " " << next->name << endl;
934
935             elapsed_sec += next->getTime_sec();
936
937             if ( elapsed_sec >= day_sec)
938                 continue;
939
940             fp->IncrementWaypoint(false);
941             next = fp->getNextWaypoint();
942
943             if (next->getName() != "WAITUNTIL" && next->getName() != "WAIT"
944                 && next->getName() != "END") {
945                     prev = curr;
946                     fp->IncrementWaypoint(false);
947                     curr = fp->getCurrentWaypoint();
948                     next = fp->getNextWaypoint();
949             }
950
951         } else if (next->getName() == "WAITUNTIL") {
952             double until_sec = processTimeString(next->getTime());
953
954             if (until_sec > _start_sec && start_sec < 0)
955                 until_sec -= _day;
956
957             if (elapsed_sec < until_sec)
958                 elapsed_sec = until_sec;
959
960             if (elapsed_sec >= day_sec )
961                 break;
962
963             fp->IncrementWaypoint(false);
964             next = fp->getNextWaypoint();
965
966             if (next->getName() != "WAITUNTIL" && next->getName() != "WAIT") {
967                 prev = curr;
968                 fp->IncrementWaypoint(false);
969                 curr = fp->getCurrentWaypoint();
970                 next = fp->getNextWaypoint();
971             }
972
973             //cout << _name << ": end WAITUNTIL: ";
974             //cout << prev->name << " " << curr->name << " " << next->name <<  endl;
975
976         } else {
977             distance_nm = getRange(prev->getLatitude(), prev->getLongitude(), curr->getLatitude(), curr->getLongitude());
978             elapsed_sec += distance_nm * 60 * 60 / prev->getSpeed();
979
980             if (elapsed_sec >= day_sec)
981                 continue;
982
983             fp->IncrementWaypoint(false);
984             prev = fp->getPreviousWaypoint();
985             curr = fp->getCurrentWaypoint();
986             next = fp->getNextWaypoint();
987         }
988
989     }   // end while
990
991     // the required position lies between the previous and current waypoints
992     // so we will calculate the distance back up the track from the current waypoint
993     // then calculate the lat and lon.
994
995     //cout << "advancing flight plan done elapsed_sec: " << elapsed_sec
996     //    << " " << day_sec << endl;
997
998     double time_diff = elapsed_sec - day_sec;
999     double lat, lon, recip;
1000
1001     //cout << " time diff " << time_diff << endl;
1002
1003     if (next->getName() == "WAIT" ){
1004         setSpeed(0);
1005         lat = curr->getLatitude();
1006         lon = curr->getLongitude();
1007         _wait_count= time_diff;
1008         _waiting = true;
1009     } else if (next->getName() == "WAITUNTIL") {
1010         setSpeed(0);
1011         lat = curr->getLatitude();
1012         lon = curr->getLongitude();
1013         _waiting = true;
1014     } else {
1015         setSpeed(prev->getSpeed());
1016         distance_nm = speed * time_diff / (60 * 60);
1017         double brg = getCourse(curr->getLatitude(), curr->getLongitude(), prev->getLatitude(), prev->getLongitude());
1018
1019         //cout << " brg " << brg << " from " << curr->name << " to " << prev->name << " "
1020         //    << " lat "  << curr->latitude << " lon " << curr->longitude
1021         //    << " distance m " << distance_nm * SG_NM_TO_METER << endl;
1022
1023         lat = geo_direct_wgs_84 (curr->getLatitude(), curr->getLongitude(), brg,
1024             distance_nm * SG_NM_TO_METER, &lat, &lon, &recip );
1025         lon = geo_direct_wgs_84 (curr->getLatitude(), curr->getLongitude(), brg,
1026             distance_nm * SG_NM_TO_METER, &lat, &lon, &recip );
1027         recip = geo_direct_wgs_84 (curr->getLatitude(), curr->getLongitude(), brg,
1028             distance_nm * SG_NM_TO_METER, &lat, &lon, &recip );
1029     }
1030
1031     setLatitude(lat);
1032     setLongitude(lon);
1033
1034     return true;
1035 }
1036
1037 void FGAIShip::setWPPos() {
1038
1039     if (curr->getName() == "END" || curr->getName() == "WAIT"
1040         || curr->getName() == "WAITUNTIL" || curr->getName() == "TUNNEL"){
1041             //cout << curr->name << " returning" << endl;
1042             return;
1043     }
1044
1045     double elevation_m = 0;
1046     wppos.setLatitudeDeg(curr->getLatitude());
1047     wppos.setLongitudeDeg(curr->getLongitude());
1048     wppos.setElevationM(0);
1049
1050     if (curr->getOn_ground()){
1051
1052         if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(wppos, 3000),
1053             elevation_m, NULL, 0)){
1054                 wppos.setElevationM(elevation_m);
1055         }
1056
1057         //cout << curr->name << " setting measured  elev " << elevation_m << endl;
1058
1059     } else {
1060         wppos.setElevationM(curr->getAltitude());
1061         //cout << curr->name << " setting FP elev " << elevation_m << endl;
1062     }
1063
1064     curr->setAltitude(wppos.getElevationM());
1065
1066 }
1067
1068 void FGAIShip::setXTrackError() {
1069
1070     double course = getCourse(prev->getLatitude(), prev->getLongitude(),
1071         curr->getLatitude(), curr->getLongitude());
1072     double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(),
1073         curr->getLatitude(), curr->getLongitude());
1074     double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range;
1075     double factor = -0.0045 * speed + 1;
1076     double limit = _lead_angle_limit * factor;
1077
1078     if (_wp_range > 0){
1079         _lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES;
1080     } else
1081         _lead_angle = 0;
1082
1083     _lead_angle *= _lead_angle_gain * factor;
1084     _xtrack_error = xtrack_error_nm * 6076.1155;
1085
1086     SG_CLAMP_RANGE(_lead_angle, -limit, limit);
1087
1088 }