1 // FGAIAircraft - FGAIBase-derived class creates an AI airplane
3 // Written by David Culp, started October 2003.
5 // Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
25 #include <simgear/math/point3d.hxx>
26 #include <Main/fg_props.hxx>
27 #include <Main/globals.hxx>
28 #include <Scenery/scenery.hxx>
35 #include "AIAircraft.hxx"
38 // accel, decel, climb_rate, descent_rate, takeoff_speed, climb_speed,
39 // cruise_speed, descent_speed, land_speed
41 const FGAIAircraft::PERF_STRUCT FGAIAircraft::settings[] = {
43 {2.0, 2.0, 450.0, 1000.0, 70.0, 80.0, 100.0, 80.0, 60.0},
45 {4.0, 2.0, 3000.0, 1500.0, 110.0, 180.0, 250.0, 200.0, 100.0},
47 {5.0, 2.0, 3000.0, 1500.0, 140.0, 300.0, 430.0, 300.0, 130.0},
49 {7.0, 3.0, 4000.0, 2000.0, 150.0, 350.0, 500.0, 350.0, 150.0},
51 {5.0, 2.0, 3000.0, 1500.0, 140.0, 300.0, 430.0, 300.0, 130.0}
55 FGAIAircraft::FGAIAircraft(FGAIManager* mgr, FGAISchedule *ref) {
58 _type_str = "aircraft";
65 // set heading and altitude locks
71 FGAIAircraft::~FGAIAircraft() {
75 bool FGAIAircraft::init() {
76 refuel_node = fgGetNode("systems/refuel/contact", true);
77 return FGAIBase::init();
80 void FGAIAircraft::bind() {
83 props->tie("controls/gear/gear-down",
84 SGRawValueMethods<FGAIAircraft,bool>(*this,
85 &FGAIAircraft::_getGearDown));
87 props->getNode("controls/lighting/landing-lights", true)
88 ->alias("controls/gear/gear-down");
92 void FGAIAircraft::unbind() {
95 props->untie("controls/gear/gear-down");
97 props->getNode("controls/lighting/landing-lights")->unalias();
102 void FGAIAircraft::update(double dt) {
104 FGAIBase::update(dt);
109 void FGAIAircraft::SetPerformance(const PERF_STRUCT *ps) {
115 void FGAIAircraft::Run(double dt) {
117 FGAIAircraft::dt = dt;
121 ProcessFlightPlan(dt);
122 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
123 if (now < fp->getStartTime())
125 //ProcessFlightPlan(dt);
128 double turn_radius_ft;
129 double turn_circum_ft;
130 double speed_north_deg_sec;
131 double speed_east_deg_sec;
132 double dist_covered_ft;
136 double speed_diff = tgt_speed - speed;
137 if (fabs(speed_diff) > 0.2) {
138 if (speed_diff > 0.0) speed += performance->accel * dt;
139 if (speed_diff < 0.0) {
141 speed -= performance->decel * dt * 3;
143 speed -= performance->decel * dt;
148 // convert speed to degrees per second
149 speed_north_deg_sec = cos( hdg / SG_RADIANS_TO_DEGREES )
150 * speed * 1.686 / ft_per_deg_lat;
151 speed_east_deg_sec = sin( hdg / SG_RADIANS_TO_DEGREES )
152 * speed * 1.686 / ft_per_deg_lon;
155 pos.setlat( pos.lat() + speed_north_deg_sec * dt);
156 pos.setlon( pos.lon() + speed_east_deg_sec * dt);
158 // adjust heading based on current bank angle
160 turn_radius_ft = 0.088362 * speed * speed
161 / tan( fabs(roll) / SG_RADIANS_TO_DEGREES );
162 turn_circum_ft = SGD_2PI * turn_radius_ft;
163 dist_covered_ft = speed * 1.686 * dt;
164 alpha = dist_covered_ft / turn_circum_ft * 360.0;
165 hdg += alpha * sign( roll );
166 if ( hdg > 360.0 ) hdg -= 360.0;
167 if ( hdg < 0.0) hdg += 360.0;
170 // adjust target bank angle if heading lock engaged
172 double bank_sense = 0.0;
173 double diff = fabs(hdg - tgt_heading);
174 if (diff > 180) diff = fabs(diff - 360);
175 double sum = hdg + diff;
176 if (sum > 360.0) sum -= 360.0;
177 if (fabs(sum - tgt_heading) < 1.0) {
178 bank_sense = 1.0; // right turn
180 bank_sense = -1.0; // left turn
183 tgt_roll = diff * bank_sense;
185 tgt_roll = 30.0 * bank_sense;
189 // adjust bank angle, use 9 degrees per second
190 double bank_diff = tgt_roll - roll;
191 if (fabs(bank_diff) > 0.2) {
192 if (bank_diff > 0.0) roll += 9.0 * dt;
193 if (bank_diff < 0.0) roll -= 9.0 * dt;
196 // adjust altitude (meters) based on current vertical speed (fpm)
197 altitude += vs / 60.0 * dt;
198 pos.setelev(altitude * SG_FEET_TO_METER);
199 double altitude_ft = altitude;
201 // find target vertical speed if altitude lock engaged
202 if (alt_lock && use_perf_vs) {
203 if (altitude_ft < tgt_altitude) {
204 tgt_vs = tgt_altitude - altitude_ft;
205 if (tgt_vs > performance->climb_rate)
206 tgt_vs = performance->climb_rate;
208 tgt_vs = tgt_altitude - altitude_ft;
209 if (tgt_vs < (-performance->descent_rate))
210 tgt_vs = -performance->descent_rate;
214 if (alt_lock && !use_perf_vs) {
215 double max_vs = 4*(tgt_altitude - altitude);
217 if (tgt_altitude < altitude) min_vs = -100.0;
218 if ((fabs(tgt_altitude - altitude) < 1500.0) &&
219 (fabs(max_vs) < fabs(tgt_vs))) tgt_vs = max_vs;
220 if (fabs(tgt_vs) < fabs(min_vs)) tgt_vs = min_vs;
223 // adjust vertical speed
224 double vs_diff = tgt_vs - vs;
225 if (fabs(vs_diff) > 10.0) {
228 if (vs > tgt_vs) vs = tgt_vs;
231 if (vs < tgt_vs) vs = tgt_vs;
235 // match pitch angle to vertical speed
242 //###########################//
243 // do calculations for radar //
244 //###########################//
245 double range_ft2 = UpdateRadar(manager);
247 //************************************//
249 //************************************//
252 if ( (range_ft2 < 250.0 * 250.0) &&
254 (elevation > 0.0) ) {
255 refuel_node->setBoolValue(true);
257 refuel_node->setBoolValue(false);
263 void FGAIAircraft::AccelTo(double speed) {
268 void FGAIAircraft::PitchTo(double angle) {
274 void FGAIAircraft::RollTo(double angle) {
280 void FGAIAircraft::YawTo(double angle) {
285 void FGAIAircraft::ClimbTo(double altitude) {
286 tgt_altitude = altitude;
291 void FGAIAircraft::TurnTo(double heading) {
292 tgt_heading = heading;
297 double FGAIAircraft::sign(double x) {
298 if ( x < 0.0 ) { return -1.0; }
302 void FGAIAircraft::SetFlightPlan(FGAIFlightPlan *f) {
306 void FGAIAircraft::ProcessFlightPlan( double dt ) {
309 FGAIFlightPlan::waypoint* prev = 0; // the one behind you
310 FGAIFlightPlan::waypoint* curr = 0; // the one ahead
311 FGAIFlightPlan::waypoint* next = 0; // the next plus 1
312 prev = fp->getPreviousWaypoint();
313 curr = fp->getCurrentWaypoint();
314 next = fp->getNextWaypoint();
317 if (!prev) { //beginning of flightplan, do this initialization once
318 fp->IncrementWaypoint();
319 prev = fp->getPreviousWaypoint(); //first waypoint
320 curr = fp->getCurrentWaypoint(); //second waypoint
321 next = fp->getNextWaypoint(); //third waypoint (might not exist!)
322 setLatitude(prev->latitude);
323 setLongitude(prev->longitude);
324 setSpeed(prev->speed);
325 setAltitude(prev->altitude);
326 setHeading(fp->getBearing(prev->latitude, prev->longitude, curr));
327 if (next) fp->setLeadDistance(speed, hdg, curr, next);
329 if (curr->crossat > -1000.0) { //use a calculated descent/climb rate
331 tgt_vs = (curr->crossat - prev->altitude)/
332 (fp->getDistanceToGo(pos.lat(), pos.lon(), curr)/
333 6076.0/prev->speed*60.0);
334 tgt_altitude = curr->crossat;
337 tgt_altitude = prev->altitude;
339 alt_lock = hdg_lock = true;
340 no_roll = prev->on_ground;
341 //cout << "First waypoint: " << prev->name << endl;
342 //cout << " Target speed: " << tgt_speed << endl;
343 //cout << " Target altitude: " << tgt_altitude << endl;
344 //cout << " Target heading: " << tgt_heading << endl << endl;
346 } // end of initialization
348 // let's only process the flight plan every 100 ms.
349 if (dt_count < 0.1) {
354 // check to see if we've reached the lead point for our next turn
355 double dist_to_go = fp->getDistanceToGo(pos.lat(), pos.lon(), curr);
356 double lead_dist = fp->getLeadDistance();
357 if (lead_dist < (2*speed)) lead_dist = 2*speed; //don't skip over the waypoint
358 //cout << "dist_to_go: " << dist_to_go << ", lead_dist: " << lead_dist << endl;
360 if ( dist_to_go < lead_dist ) {
361 if (curr->finished) { //end of the flight plan, so terminate
365 //time_t now = time(NULL) + fgGetLong("/sim/time/warp");
368 FGAIModelEntity entity;
369 entity.m_class = "jet_transport";
370 //entity.path = modelPath.c_str();
371 entity.flightplan = "none";
372 entity.latitude = _getLatitude();
373 entity.longitude = _getLongitude();
374 entity.altitude = trafficRef->getCruiseAlt() * 100; // convert from FL to feet
376 //entity.fp = new FGAIFlightPlan(&entity, courseToDest, i->getDepartureTime(), dep, arr);
377 entity.fp = new FGAIFlightPlan(&entity,
379 trafficRef->getDepartureTime(),
380 trafficRef->getDepartureAirport(),
381 trafficRef->getArrivalAirport());
382 SetFlightPlan(entity.fp);
390 // we've reached the lead-point for the waypoint ahead
391 if (next) tgt_heading = fp->getBearing(curr, next);
392 fp->IncrementWaypoint();
393 prev = fp->getPreviousWaypoint();
394 curr = fp->getCurrentWaypoint();
395 next = fp->getNextWaypoint();
396 if (next) fp->setLeadDistance(speed, tgt_heading, curr, next);
397 if (curr->crossat > -1000.0) {
399 tgt_vs = (curr->crossat - altitude)/
400 (fp->getDistanceToGo(pos.lat(), pos.lon(), curr)/6076.0/speed*60.0);
401 tgt_altitude = curr->crossat;
404 tgt_altitude = prev->altitude;
406 tgt_speed = prev->speed;
407 hdg_lock = alt_lock = true;
408 no_roll = prev->on_ground;
409 //cout << "Crossing waypoint: " << prev->name << endl;
410 //cout << " Target speed: " << tgt_speed << endl;
411 //cout << " Target altitude: " << tgt_altitude << endl;
412 //cout << " Target heading: " << tgt_heading << endl << endl;
414 double calc_bearing = fp->getBearing(pos.lat(), pos.lon(), curr);
415 double hdg_error = calc_bearing - tgt_heading;
416 if (fabs(hdg_error) > 1.0) {
417 TurnTo( calc_bearing );
426 bool FGAIAircraft::_getGearDown() const {
427 return ((props->getFloatValue("position/altitude-agl-ft") < 900.0)
428 && (props->getFloatValue("velocities/airspeed-kt")
429 < performance->land_speed*1.25));