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>
34 #include "AIAircraft.hxx"
37 // accel, decel, climb_rate, descent_rate, takeoff_speed, climb_speed,
38 // cruise_speed, descent_speed, land_speed
40 const FGAIAircraft::PERF_STRUCT FGAIAircraft::settings[] = {
42 {2.0, 2.0, 450.0, 1000.0, 70.0, 80.0, 100.0, 80.0, 60.0},
44 {4.0, 2.0, 3000.0, 1500.0, 110.0, 180.0, 250.0, 200.0, 100.0},
46 {5.0, 2.0, 3000.0, 1500.0, 140.0, 300.0, 430.0, 300.0, 130.0},
48 {7.0, 3.0, 4000.0, 2000.0, 150.0, 350.0, 500.0, 350.0, 150.0}
52 FGAIAircraft::FGAIAircraft(FGAIManager* mgr) {
54 _type_str = "aircraft";
60 // set heading and altitude locks
66 FGAIAircraft::~FGAIAircraft() {
71 bool FGAIAircraft::init() {
72 return FGAIBase::init();
75 void FGAIAircraft::bind() {
79 props->tie("controls/gear/gear-down",
80 SGRawValueFunctions<bool>(FGAIAircraft::_getGearDown));
82 props->getNode("controls/lighting/landing-lights", true)
83 ->alias("controls/gear/gear-down");
87 void FGAIAircraft::unbind() {
90 // props->untie("controls/gear/gear-down");
91 // props->getNode("controls/lighting/landing-lights")->unalias();
95 void FGAIAircraft::update(double dt) {
102 void FGAIAircraft::SetPerformance(const PERF_STRUCT *ps) {
108 void FGAIAircraft::Run(double dt) {
110 FGAIAircraft::dt = dt;
112 if (fp) ProcessFlightPlan(dt);
114 double turn_radius_ft;
115 double turn_circum_ft;
116 double speed_north_deg_sec;
117 double speed_east_deg_sec;
118 double ft_per_deg_lon;
119 double ft_per_deg_lat;
120 double dist_covered_ft;
123 // get size of a degree at this latitude
124 ft_per_deg_lat = 366468.96 - 3717.12 * cos(pos.lat()/SG_RADIANS_TO_DEGREES);
125 ft_per_deg_lon = 365228.16 * cos(pos.lat() / SG_RADIANS_TO_DEGREES);
128 double speed_diff = tgt_speed - speed;
129 if (fabs(speed_diff) > 0.2) {
130 if (speed_diff > 0.0) speed += performance->accel * dt;
131 if (speed_diff < 0.0) {
133 speed -= performance->decel * dt * 3;
135 speed -= performance->decel * dt;
140 // convert speed to degrees per second
141 speed_north_deg_sec = cos( hdg / SG_RADIANS_TO_DEGREES )
142 * speed * 1.686 / ft_per_deg_lat;
143 speed_east_deg_sec = sin( hdg / SG_RADIANS_TO_DEGREES )
144 * speed * 1.686 / ft_per_deg_lon;
147 pos.setlat( pos.lat() + speed_north_deg_sec * dt);
148 pos.setlon( pos.lon() + speed_east_deg_sec * dt);
150 // adjust heading based on current bank angle
152 turn_radius_ft = 0.088362 * speed * speed
153 / tan( fabs(roll) / SG_RADIANS_TO_DEGREES );
154 turn_circum_ft = SGD_2PI * turn_radius_ft;
155 dist_covered_ft = speed * 1.686 * dt;
156 alpha = dist_covered_ft / turn_circum_ft * 360.0;
157 hdg += alpha * sign( roll );
158 if ( hdg > 360.0 ) hdg -= 360.0;
159 if ( hdg < 0.0) hdg += 360.0;
162 // adjust target bank angle if heading lock engaged
164 double bank_sense = 0.0;
165 double diff = fabs(hdg - tgt_heading);
166 if (diff > 180) diff = fabs(diff - 360);
167 double sum = hdg + diff;
168 if (sum > 360.0) sum -= 360.0;
169 if (fabs(sum - tgt_heading) < 1.0) {
170 bank_sense = 1.0; // right turn
172 bank_sense = -1.0; // left turn
175 tgt_roll = diff * bank_sense;
177 tgt_roll = 30.0 * bank_sense;
181 // adjust bank angle, use 9 degrees per second
182 double bank_diff = tgt_roll - roll;
183 if (fabs(bank_diff) > 0.2) {
184 if (bank_diff > 0.0) roll += 9.0 * dt;
185 if (bank_diff < 0.0) roll -= 9.0 * dt;
188 // adjust altitude (meters) based on current vertical speed (fpm)
189 altitude += vs / 60.0 * dt;
190 pos.setelev(altitude * SG_FEET_TO_METER);
191 double altitude_ft = altitude;
193 // find target vertical speed if altitude lock engaged
194 if (alt_lock && use_perf_vs) {
195 if (altitude_ft < tgt_altitude) {
196 tgt_vs = tgt_altitude - altitude_ft;
197 if (tgt_vs > performance->climb_rate)
198 tgt_vs = performance->climb_rate;
200 tgt_vs = tgt_altitude - altitude_ft;
201 if (tgt_vs < (-performance->descent_rate))
202 tgt_vs = -performance->descent_rate;
206 if (alt_lock && !use_perf_vs) {
207 double max_vs = 4*(tgt_altitude - altitude);
209 if (tgt_altitude < altitude) min_vs = -100.0;
210 if ((fabs(tgt_altitude - altitude) < 1500.0) &&
211 (fabs(max_vs) < fabs(tgt_vs))) tgt_vs = max_vs;
212 if (fabs(tgt_vs) < fabs(min_vs)) tgt_vs = min_vs;
215 // adjust vertical speed
216 double vs_diff = tgt_vs - vs;
217 if (fabs(vs_diff) > 10.0) {
220 if (vs > tgt_vs) vs = tgt_vs;
223 if (vs < tgt_vs) vs = tgt_vs;
227 // match pitch angle to vertical speed
234 //###########################//
235 // do calculations for radar //
236 //###########################//
238 // copy values from the AIManager
239 double user_latitude = manager->get_user_latitude();
240 double user_longitude = manager->get_user_longitude();
241 double user_altitude = manager->get_user_altitude();
242 double user_heading = manager->get_user_heading();
243 double user_pitch = manager->get_user_pitch();
244 double user_yaw = manager->get_user_yaw();
245 double user_speed = manager->get_user_speed();
247 // calculate range to target in feet and nautical miles
248 double lat_range = fabs(pos.lat() - user_latitude) * ft_per_deg_lat;
249 double lon_range = fabs(pos.lon() - user_longitude) * ft_per_deg_lon;
250 double range_ft = sqrt( lat_range*lat_range + lon_range*lon_range );
251 range = range_ft / 6076.11549;
253 // calculate bearing to target
254 if (pos.lat() >= user_latitude) {
255 bearing = atan2(lat_range, lon_range) * SG_RADIANS_TO_DEGREES;
256 if (pos.lon() >= user_longitude) {
257 bearing = 90.0 - bearing;
259 bearing = 270.0 + bearing;
262 bearing = atan2(lon_range, lat_range) * SG_RADIANS_TO_DEGREES;
263 if (pos.lon() >= user_longitude) {
264 bearing = 180.0 - bearing;
266 bearing = 180.0 + bearing;
270 // calculate look left/right to target, without yaw correction
271 horiz_offset = bearing - user_heading;
272 if (horiz_offset > 180.0) horiz_offset -= 360.0;
273 if (horiz_offset < -180.0) horiz_offset += 360.0;
275 // calculate elevation to target
276 elevation = atan2( altitude_ft - user_altitude, range_ft )
277 * SG_RADIANS_TO_DEGREES;
279 // calculate look up/down to target
280 vert_offset = elevation + user_pitch;
282 /* this calculation needs to be fixed, but it isn't important anyway
283 // calculate range rate
284 double recip_bearing = bearing + 180.0;
285 if (recip_bearing > 360.0) recip_bearing -= 360.0;
286 double my_horiz_offset = recip_bearing - hdg;
287 if (my_horiz_offset > 180.0) my_horiz_offset -= 360.0;
288 if (my_horiz_offset < -180.0) my_horiz_offset += 360.0;
289 rdot = (-user_speed * cos( horiz_offset * SG_DEGREES_TO_RADIANS ))
290 + (-speed * 1.686 * cos( my_horiz_offset * SG_DEGREES_TO_RADIANS ));
293 // now correct look left/right for yaw
294 horiz_offset += user_yaw;
296 // calculate values for radar display
297 y_shift = range * cos( horiz_offset * SG_DEGREES_TO_RADIANS);
298 x_shift = range * sin( horiz_offset * SG_DEGREES_TO_RADIANS);
299 rotation = hdg - user_heading;
300 if (rotation < 0.0) rotation += 360.0;
305 void FGAIAircraft::AccelTo(double speed) {
310 void FGAIAircraft::PitchTo(double angle) {
316 void FGAIAircraft::RollTo(double angle) {
322 void FGAIAircraft::YawTo(double angle) {
327 void FGAIAircraft::ClimbTo(double altitude) {
328 tgt_altitude = altitude;
333 void FGAIAircraft::TurnTo(double heading) {
334 tgt_heading = heading;
339 double FGAIAircraft::sign(double x) {
340 if ( x < 0.0 ) { return -1.0; }
344 void FGAIAircraft::SetFlightPlan(FGAIFlightPlan *f) {
348 void FGAIAircraft::ProcessFlightPlan( double dt ) {
349 FGAIFlightPlan::waypoint* prev = 0; // the one behind you
350 FGAIFlightPlan::waypoint* curr = 0; // the one ahead
351 FGAIFlightPlan::waypoint* next = 0; // the next plus 1
352 prev = fp->getPreviousWaypoint();
353 curr = fp->getCurrentWaypoint();
354 next = fp->getNextWaypoint();
357 if (!prev) { //beginning of flightplan, do this initialization once
358 fp->IncrementWaypoint();
359 prev = fp->getPreviousWaypoint(); //first waypoint
360 curr = fp->getCurrentWaypoint(); //second waypoint
361 next = fp->getNextWaypoint(); //third waypoint (might not exist!)
362 setLatitude(prev->latitude);
363 setLongitude(prev->longitude);
364 setSpeed(prev->speed);
365 setAltitude(prev->altitude);
366 setHeading(fp->getBearing(prev->latitude, prev->longitude, curr));
367 if (next) fp->setLeadDistance(speed, hdg, curr, next);
369 if (curr->crossat > -1000.0) { //use a calculated descent/climb rate
371 tgt_vs = (curr->crossat - prev->altitude)/
372 (fp->getDistanceToGo(pos.lat(), pos.lon(), curr)/
373 6076.0/prev->speed*60.0);
374 tgt_altitude = curr->crossat;
377 tgt_altitude = prev->altitude;
379 alt_lock = hdg_lock = true;
380 no_roll = prev->on_ground;
381 //cout << "First waypoint: " << prev->name << endl;
382 //cout << " Target speed: " << tgt_speed << endl;
383 //cout << " Target altitude: " << tgt_altitude << endl;
384 //cout << " Target heading: " << tgt_heading << endl << endl;
386 } // end of initialization
388 // let's only process the flight plan every 100 ms.
389 if (dt_count < 0.1) {
394 // check to see if we've reached the lead point for our next turn
395 double dist_to_go = fp->getDistanceToGo(pos.lat(), pos.lon(), curr);
396 double lead_dist = fp->getLeadDistance();
397 if (lead_dist < (2*speed)) lead_dist = 2*speed; //don't skip over the waypoint
398 //cout << "dist_to_go: " << dist_to_go << ", lead_dist: " << lead_dist << endl;
400 if ( dist_to_go < lead_dist ) {
401 if (curr->finished) { //end of the flight plan, so terminate
405 // we've reached the lead-point for the waypoint ahead
406 if (next) tgt_heading = fp->getBearing(curr, next);
407 fp->IncrementWaypoint();
408 prev = fp->getPreviousWaypoint();
409 curr = fp->getCurrentWaypoint();
410 next = fp->getNextWaypoint();
411 if (next) fp->setLeadDistance(speed, tgt_heading, curr, next);
412 if (curr->crossat > -1000.0) {
414 tgt_vs = (curr->crossat - altitude)/
415 (fp->getDistanceToGo(pos.lat(), pos.lon(), curr)/6076.0/speed*60.0);
416 tgt_altitude = curr->crossat;
419 tgt_altitude = prev->altitude;
421 tgt_speed = prev->speed;
422 hdg_lock = alt_lock = true;
423 no_roll = prev->on_ground;
424 //cout << "Crossing waypoint: " << prev->name << endl;
425 //cout << " Target speed: " << tgt_speed << endl;
426 //cout << " Target altitude: " << tgt_altitude << endl;
427 //cout << " Target heading: " << tgt_heading << endl << endl;
429 double calc_bearing = fp->getBearing(pos.lat(), pos.lon(), curr);
430 double hdg_error = calc_bearing - tgt_heading;
431 if (fabs(hdg_error) > 1.0) {
432 TurnTo( calc_bearing );