From ba8ed137cfd05f3112592ee2c87be08d4a2123c7 Mon Sep 17 00:00:00 2001 From: durk Date: Sat, 9 Jun 2007 11:49:16 +0000 Subject: [PATCH] Code cleanup of AIModels (mainly AIAircraft), by Thomas Foerster --- src/AIModel/AIAircraft.cxx | 1624 ++++++++++++++++++---------------- src/AIModel/AIAircraft.hxx | 31 +- src/AIModel/AIFlightPlan.cxx | 1 + src/AIModel/AIFlightPlan.hxx | 2 +- src/AIModel/AIManager.cxx | 7 +- src/AIModel/AITanker.cxx | 72 ++ src/AIModel/AITanker.hxx | 58 ++ src/AIModel/Makefile.am | 15 +- src/Include/config.h-msvc8 | 5 +- 9 files changed, 1033 insertions(+), 782 deletions(-) create mode 100644 src/AIModel/AITanker.cxx create mode 100644 src/AIModel/AITanker.hxx diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index b7ad31f0f..7002bbeaf 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -44,12 +44,15 @@ SG_USING_STD(string); #include "AIAircraft.hxx" //#include - static string tempReg; + +static string tempReg; // // accel, decel, climb_rate, descent_rate, takeoff_speed, climb_speed, // cruise_speed, descent_speed, land_speed // -const FGAIAircraft::PERF_STRUCT FGAIAircraft::settings[] = { + +const FGAIAircraft::PERF_STRUCT FGAIAircraft::settings[] = +{ // light aircraft {2.0, 2.0, 450.0, 1000.0, 70.0, 80.0, 100.0, 80.0, 60.0}, // ww2_fighter @@ -64,9 +67,7 @@ const FGAIAircraft::PERF_STRUCT FGAIAircraft::settings[] = { {30.0, 30.0, 6000.0, 6000.0, 150.0, 300.0, 430.0, 300.0, 130.0} }; - -FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : - FGAIBase(otAircraft) +FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : FGAIBase(otAircraft) { trafficRef = ref; if (trafficRef) @@ -80,7 +81,6 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : dt_count = 0; dt_elev_count = 0; use_perf_vs = true; - isTanker = false; no_roll = false; tgt_speed = 0; @@ -97,14 +97,16 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : } -FGAIAircraft::~FGAIAircraft() { +FGAIAircraft::~FGAIAircraft() +{ //delete fp; - if (controller) - controller->signOff(getID()); + if (controller) + controller->signOff(getID()); } -void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) { +void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) +{ if (!scFileNode) return; @@ -112,467 +114,167 @@ void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) { setPerformance(scFileNode->getStringValue("class", "jet_transport")); setFlightPlan(scFileNode->getStringValue("flightplan"), - scFileNode->getBoolValue("repeat", false)); + scFileNode->getBoolValue("repeat", false)); setCallSign(scFileNode->getStringValue("callsign")); - setTACANChannelID(scFileNode->getStringValue("TACAN-channel-ID")); -} - - -bool FGAIAircraft::init(bool search_in_AI_path) { - //refuel_node = fgGetNode("systems/refuel/contact", true); - return FGAIBase::init(search_in_AI_path); } -void FGAIAircraft::bind() { +void FGAIAircraft::bind() +{ FGAIBase::bind(); props->tie("controls/gear/gear-down", - SGRawValueMethods(*this, - &FGAIAircraft::_getGearDown)); - props->tie("refuel/contact", SGRawValuePointer(&contact)); + SGRawValueMethods(*this, + &FGAIAircraft::_getGearDown)); props->setStringValue("callsign", callsign.c_str()); - props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); - props->setBoolValue("tanker",isTanker); } -void FGAIAircraft::unbind() { +void FGAIAircraft::unbind() +{ FGAIBase::unbind(); props->untie("controls/gear/gear-down"); - props->untie("refuel/contact"); } -void FGAIAircraft::update(double dt) { +void FGAIAircraft::update(double dt) +{ FGAIBase::update(dt); Run(dt); Transform(); } -void FGAIAircraft::setPerformance(const std::string& acclass) { - if (acclass == "light") { +void FGAIAircraft::setPerformance(const std::string& acclass) +{ + if (acclass == "light") + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::LIGHT]); - } else if (acclass == "ww2_fighter") { + } + else if (acclass == "ww2_fighter") + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::WW2_FIGHTER]); - } else if (acclass == "jet_transport") { + } + else if (acclass == "jet_transport") + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_TRANSPORT]); - } else if (acclass == "jet_fighter") { + } + else if (acclass == "jet_fighter") + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_FIGHTER]); - } else if (acclass == "tanker") { + } + else if (acclass == "tanker") + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_TRANSPORT]); - SetTanker(true); - } else if (acclass == "ufo") { + } + else if (acclass == "ufo") + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::UFO]); - } else { + } + else + { SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_TRANSPORT]); } } -void FGAIAircraft::SetPerformance(const PERF_STRUCT *ps) { +void FGAIAircraft::SetPerformance(const PERF_STRUCT *ps) +{ - performance = ps; + performance = ps; } -void FGAIAircraft::Run(double dt) { +void FGAIAircraft::Run(double dt) +{ FGAIAircraft::dt = dt; - - if (fp) { - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); - ProcessFlightPlan(dt, now); - if (now < fp->getStartTime()) { - // Do execute Ground elev for inactive aircraft, so they - // Are repositioned to the correct ground altitude when the user flies within visibility range. - // In addition, check whether we are out of user range, so this aircraft - // can be deleted. - if (no_roll) { - Transform(); // make sure aip is initialized. - if (trafficRef) { - double userLatitude = fgGetDouble("/position/latitude-deg"); - double userLongitude = fgGetDouble("/position/longitude-deg"); - double course, distance; - SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); - SGWayPoint user (userLongitude, userLatitude, 0); - user.CourseAndDistance(current, &course, &distance); - if ((distance * SG_METER_TO_NM) > TRAFFICTOAIDIST) { - setDie(true); - return; - } - getGroundElev(dt); // make sure it's exectuted first time around, so force a large dt value - //getGroundElev(dt); // Need to do this twice. - //cerr << trafficRef->getRegistration() << " Setting altitude to " << tgt_altitude; - doGroundAltitude(); - //cerr << " Actual altitude " << altitude << endl; - // Transform(); - pos.setElevationFt(altitude_ft); - } - } - return; - } - } else { - // no flight plan, update target heading, speed, and altitude - // from control properties. These default to the initial - // settings in the config file, but can be changed "on the - // fly". - string lat_mode = props->getStringValue("controls/flight/lateral-mode"); - if ( lat_mode == "roll" ) { - double angle - = props->getDoubleValue("controls/flight/target-roll" ); - RollTo( angle ); - } else { - double angle - = props->getDoubleValue("controls/flight/target-hdg" ); - TurnTo( angle ); - } - - string lon_mode - = props->getStringValue("controls/flight/longitude-mode"); - if ( lon_mode == "alt" ) { - double alt = props->getDoubleValue("controls/flight/target-alt" ); - ClimbTo( alt ); - } else { - double angle - = props->getDoubleValue("controls/flight/target-pitch" ); - PitchTo( angle ); - } + if(!updateTargetValues()) + return; - AccelTo( props->getDoubleValue("controls/flight/target-spd" ) ); - } if (controller) - { - controller->update(getID(), - pos.getLatitudeDeg(), - pos.getLongitudeDeg(), - hdg, - speed, - altitude_ft, dt); - processATC(controller->getInstruction(getID())); - } - - double turn_radius_ft; - double turn_circum_ft; - double speed_north_deg_sec; - double speed_east_deg_sec; - double dist_covered_ft; - double alpha; - - // adjust speed - double speed_diff; //= tgt_speed - speed; - if (!no_roll) { - speed_diff = tgt_speed - speed; - } else { - speed_diff = groundTargetSpeed - speed; - } - - if (speed_diff > 0.0) { - speed += performance->accel * dt; - if (no_roll) { // apply overshoot correction - if ( speed > groundTargetSpeed ) speed = groundTargetSpeed; - }else { - if ( speed > tgt_speed ) speed = tgt_speed; - } - } else if (speed_diff < 0.0) { - if (no_roll) { - // on ground (aircraft can't roll) - // deceleration performance is better due to wheel brakes. - speed -= performance->decel * dt * 3; - } else { - speed -= performance->decel * dt; - } - if (no_roll) { // apply overshoot correction - if (speed < groundTargetSpeed ) speed = groundTargetSpeed; - } else { - if ( speed < tgt_speed ) speed = tgt_speed; - } - } - - - // convert speed to degrees per second - speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS ) - * speed * 1.686 / ft_per_deg_lat; - speed_east_deg_sec = sin( hdg * SGD_DEGREES_TO_RADIANS ) - * speed * 1.686 / ft_per_deg_lon; - - // set new position - pos.setLatitudeDeg( pos.getLatitudeDeg() + speed_north_deg_sec * dt); - pos.setLongitudeDeg( pos.getLongitudeDeg() + speed_east_deg_sec * dt); - - /* printf("%.7f %0.4f %.7f %.5f %.7f %.7f %.8f %.8f %.9f %.9f\n", - dt, hdg, speed, ft_per_deg_lat, - cos( hdg * SGD_DEGREES_TO_RADIANS ), - speed * 1.686 / ft_per_deg_lat, - speed_north_deg_sec, speed_east_deg_sec, - pos.getLatitudeDeg(), pos.getLongitudeDeg()); */ - - //if (!(finite(pos.lat()) && finite(pos.lon()))) - // { - // cerr << "Position is not finite" << endl; - // cerr << "speed = " << speed << endl; - // cerr << "dt" << dt << endl; - // cerr << "heading " << hdg << endl; - // cerr << "speed east " << speed_east_deg_sec << endl; - // cerr << "speed nrth " << speed_north_deg_sec << endl; - // cerr << "deg_lat " << ft_per_deg_lat << endl; - // cerr << "deg_lon " << ft_per_deg_lon << endl; - // } - - // adjust heading based on current bank angle - if (roll == 0.0) - roll = 0.01; - - if (roll != 0.0) { - // double turnConstant; - //if (no_roll) - // turnConstant = 0.0088362; - //else - // turnConstant = 0.088362; - // If on ground, calculate heading change directly - if (no_roll) { - double headingDiff = fabs(hdg-tgt_heading); - - if (headingDiff > 180) - headingDiff = fabs(headingDiff - 360); - - groundTargetSpeed = tgt_speed - (tgt_speed * (headingDiff/45)); - if (sign(groundTargetSpeed) != sign(tgt_speed)) - groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode - - if (headingDiff > 30.0) { - headingChangeRate += dt * sign(roll); // invert if pushed backward - // Print some debug statements to find out why aircraft may get stuck - // forever turning - //if (trafficRef->getDepartureAirport()->getId() == string("EHAM")) { - // cerr << "Turning : " << trafficRef->getRegistration() - // cerr << " Speed = " << speed << " Heading " << hdg - // << " Target Heading " << tgt_heading - // << " Lead Distance " << fp->getLeadDistance() - // << " Distance to go " - // << fp->getDistanceToGo(pos.lat(), pos.lon(), fp->getCurrentWaypoint()) - // << "waypoint name " << fp->getCurrentWaypoint()->name - // << endl; - //} - if (headingChangeRate > 30) - headingChangeRate = 30; - else if (headingChangeRate < -30) - headingChangeRate = -30; - - } else { - if (fabs(headingChangeRate) > headingDiff) - headingChangeRate = headingDiff*sign(roll); - else - headingChangeRate += dt * sign(roll); - } - hdg += headingChangeRate * dt; - //cerr << "On ground. Heading: " << hdg << ". Target Heading: " << tgt_heading - // << ". Target speed: " << groundTargetSpeed << ". heading change rate" - // << headingChangeRate << endl; - - } else { - if (fabs(speed) > 1.0) { - turn_radius_ft = 0.088362 * speed * speed - / tan( fabs(roll) / SG_RADIANS_TO_DEGREES ); - } else { - turn_radius_ft = 1.0; // Check if turn_radius_ft == 0; this might lead to a division by 0. - } - turn_circum_ft = SGD_2PI * turn_radius_ft; - dist_covered_ft = speed * 1.686 * dt; - alpha = dist_covered_ft / turn_circum_ft * 360.0; - hdg += alpha * sign(roll); - } - while ( hdg > 360.0 ) { - hdg -= 360.0; - spinCounter++; - } - while ( hdg < 0.0) { - hdg += 360.0; - spinCounter--; - } - } - - - // adjust target bank angle if heading lock engaged - if (hdg_lock) { - double bank_sense = 0.0; - double diff = fabs(hdg - tgt_heading); - if (diff > 180) - diff = fabs(diff - 360); - - double sum = hdg + diff; - if (sum > 360.0) - sum -= 360.0; - if (fabs(sum - tgt_heading) < 1.0) { - bank_sense = 1.0; // right turn - } else { - bank_sense = -1.0; // left turn - } - if (diff < 30) { - tgt_roll = diff * bank_sense; - } else { - tgt_roll = 30.0 * bank_sense; - } - if ((fabs((double) spinCounter) > 1) && (diff > 30)) { - tgt_speed *= 0.999; // Ugly hack: If aircraft get stuck, they will continually spin around. - // The only way to resolve this is to make them slow down. - //if (tempReg.empty()) - // tempReg = trafficRef->getRegistration(); - //if (trafficRef->getRegistration() == tempReg) { - // cerr << trafficRef->getRegistration() - // << " appears to be spinning: " << spinCounter << endl - // << " speed " << speed << endl - // << " heading " << hdg << endl - // << " lead distance " << fp->getLeadDistance() << endl - // << " waypoint " << fp->getCurrentWaypoint()->name - // << " target heading " << tgt_heading << endl - // << " lead in angle " << fp->getLeadInAngle()<< endl - // << " roll " << roll << endl - // << " target_roll " << tgt_roll << endl; - //} - } - } - - // adjust bank angle, use 9 degrees per second - double bank_diff = tgt_roll - roll; - if (fabs(bank_diff) > 0.2) { - if (bank_diff > 0.0) - roll += 9.0 * dt; - - if (bank_diff < 0.0) - roll -= 9.0 * dt; - //while (roll > 180) roll -= 360; - //while (roll < 180) roll += 360; - } - - // adjust altitude (meters) based on current vertical speed (fpm) - altitude_ft += vs / 60.0 * dt; - pos.setElevationFt(altitude_ft); - - // adjust target Altitude, based on ground elevation when on ground - if (no_roll) { - getGroundElev(dt); - doGroundAltitude(); - } else { - // find target vertical speed if altitude lock engaged - if (alt_lock && use_perf_vs) { - if (altitude_ft < tgt_altitude_ft) { - tgt_vs = tgt_altitude_ft - altitude_ft; - if (tgt_vs > performance->climb_rate) - tgt_vs = performance->climb_rate; - } else { - tgt_vs = tgt_altitude_ft - altitude_ft; - if (tgt_vs < (-performance->descent_rate)) - tgt_vs = -performance->descent_rate; - } - } - - if (alt_lock && !use_perf_vs) { - double max_vs = 4*(tgt_altitude_ft - altitude_ft); - double min_vs = 100; - if (tgt_altitude_ft < altitude_ft) - min_vs = -100.0; - if ((fabs(tgt_altitude_ft - altitude_ft) < 1500.0) - && (fabs(max_vs) < fabs(tgt_vs))) - tgt_vs = max_vs; - - if (fabs(tgt_vs) < fabs(min_vs)) - tgt_vs = min_vs; - } + { + controller->update(getID(), + pos.getLatitudeDeg(), + pos.getLongitudeDeg(), + hdg, + speed, + altitude_ft, dt); + processATC(controller->getInstruction(getID())); } - // adjust vertical speed - double vs_diff = tgt_vs - vs; - if (fabs(vs_diff) > 10.0) { - if (vs_diff > 0.0) { - vs += (performance->climb_rate / 3.0) * dt; - - if (vs > tgt_vs) - vs = tgt_vs; - } else { - vs -= (performance->descent_rate / 3.0) * dt; - if (vs < tgt_vs) - vs = tgt_vs; - } + if (no_roll) + { + adjustSpeed(groundTargetSpeed); } - - // match pitch angle to vertical speed - if (vs > 0) { - pitch = vs * 0.005; - } else { - pitch = vs * 0.002; + else + { + adjustSpeed(tgt_speed); } - //###########################// - // do calculations for radar // - //###########################// - double range_ft2 = UpdateRadar(manager); - - //************************************// - // Tanker code // - //************************************// - - if ( isTanker) { - if ( (range_ft2 < 250.0 * 250.0) && (y_shift > 0.0) - && (elevation > 0.0) ) { - //refuel_node->setBoolValue(true); - contact = true; - } else { - //refuel_node->setBoolValue(false); - contact = false; - } - } else { - contact = false; - } + updatePosition(); + updateHeading(); + updateBankAngles(); + updateAltitudes(); + updateVerticalSpeed(); + matchPitchAngle(); } -void FGAIAircraft::AccelTo(double speed) { +void FGAIAircraft::AccelTo(double speed) +{ tgt_speed = speed; } -void FGAIAircraft::PitchTo(double angle) { +void FGAIAircraft::PitchTo(double angle) +{ tgt_pitch = angle; alt_lock = false; } -void FGAIAircraft::RollTo(double angle) { +void FGAIAircraft::RollTo(double angle) +{ tgt_roll = angle; hdg_lock = false; } -void FGAIAircraft::YawTo(double angle) { +void FGAIAircraft::YawTo(double angle) +{ tgt_yaw = angle; } -void FGAIAircraft::ClimbTo(double alt_ft ) { +void FGAIAircraft::ClimbTo(double alt_ft ) +{ tgt_altitude_ft = alt_ft; alt_lock = true; } -void FGAIAircraft::TurnTo(double heading) { +void FGAIAircraft::TurnTo(double heading) +{ tgt_heading = heading; hdg_lock = true; } -double FGAIAircraft::sign(double x) { - if ( x < 0.0 ) - return -1.0; +double FGAIAircraft::sign(double x) +{ + if (x == 0.0) + return x; else - return 1.0; + return x/fabs(x); } -void FGAIAircraft::setFlightPlan(const std::string& flightplan, bool repeat) { - if (!flightplan.empty()) { +void FGAIAircraft::setFlightPlan(const std::string& flightplan, bool repeat) +{ + if (!flightplan.empty()) + { FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan); fp->setRepeat(repeat); SetFlightPlan(fp); @@ -580,111 +282,52 @@ void FGAIAircraft::setFlightPlan(const std::string& flightplan, bool repeat) { } -void FGAIAircraft::SetFlightPlan(FGAIFlightPlan *f) { +void FGAIAircraft::SetFlightPlan(FGAIFlightPlan *f) +{ delete fp; fp = f; } -void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { - bool eraseWaypoints; - if (trafficRef) { - eraseWaypoints = true; - } else - eraseWaypoints = false; +void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) +{ + + //if (! fpExecutable(now)) + // return; + + // the one behind you + FGAIFlightPlan::waypoint* prev = 0; + // the one ahead + FGAIFlightPlan::waypoint* curr = 0; + // the next plus 1 + FGAIFlightPlan::waypoint* next = 0; - - FGAIFlightPlan::waypoint* prev = 0; // the one behind you - FGAIFlightPlan::waypoint* curr = 0; // the one ahead - FGAIFlightPlan::waypoint* next = 0; // the next plus 1 prev = fp->getPreviousWaypoint(); curr = fp->getCurrentWaypoint(); next = fp->getNextWaypoint(); + dt_count += dt; /////////////////////////////////////////////////////////////////////////// // Initialize the flightplan ////////////////////////////////////////////////////////////////////////// - if (!prev) { - - spinCounter = 0; - tempReg = ""; - fp->IncrementWaypoint(eraseWaypoints); - if (!(fp->getNextWaypoint()) && trafficRef) - loadNextLeg(); - - prev = fp->getPreviousWaypoint(); //first waypoint - curr = fp->getCurrentWaypoint(); //second waypoint - next = fp->getNextWaypoint(); //third waypoint (might not exist!) - - setLatitude(prev->latitude); - setLongitude(prev->longitude); - setSpeed(prev->speed); - setAltitude(prev->altitude); - - if (prev->speed > 0.0) - setHeading(fp->getBearing(prev->latitude, prev->longitude, curr)); - else - setHeading(fp->getBearing(curr->latitude, curr->longitude, prev)); - - // If next doesn't exist, as in incrementally created flightplans for - // AI/Trafficmanager created plans, - // Make sure lead distance is initialized otherwise - if (next) - fp->setLeadDistance(speed, hdg, curr, next); - - if (curr->crossat > -1000.0) { //use a calculated descent/climb rate - use_perf_vs = false; - tgt_vs = (curr->crossat - prev->altitude) - / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr) - / 6076.0 / prev->speed*60.0); - tgt_altitude_ft = curr->crossat; - } else { - use_perf_vs = true; - tgt_altitude_ft = prev->altitude; - } - alt_lock = hdg_lock = true; - no_roll = prev->on_ground; - if (no_roll) { - Transform(); // make sure aip is initialized. - getGroundElev(60.1); // make sure it's executed first time around, so force a large dt value - doGroundAltitude(); - } - // Make sure to announce the aircraft's position - announcePositionToController(); - prevSpeed = 0; - return; - } // end of initialization - /////////////////////////////////////////////////////////////////////////// - // Check Execution time (currently once every 100 ms) - // Add a bit of randomization to prevent the execution of all flight plans - // in synchrony, which can add significant periodic framerate flutter. - /////////////////////////////////////////////////////////////////////////// - double rand_exec_time = (rand() % 100) / 100; - if ((dt_count < (0.1+rand_exec_time)) || (now < fp->getStartTime())) { - //cerr << "done fp dt" << endl; - return; - } else { - dt_count = 0; - } - - // check to see if we've reached the lead point for our next turn - double dist_to_go = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); + if (!prev) + { + handleFirstWaypoint(); + return; + } // end of initialization - //cerr << "2" << endl; - double lead_dist = fp->getLeadDistance(); - //cerr << " Distance : " << dist_to_go << ": Lead distance " << lead_dist << endl; - // experimental: Use fabs, because speed can be negative (I hope) during push_back. + dt_count = 0; - if (lead_dist < fabs(2*speed)) { - lead_dist = fabs(2*speed); //don't skip over the waypoint - //cerr << "Extending lead distance to " << lead_dist << endl; + if (! leadPointReached(curr)) + { + controlHeading(curr); + controlSpeed(curr, next); } - - //prev_dist_to_go = dist_to_go; - - if ( dist_to_go < lead_dist ) { - if (curr->finished) { //end of the flight plan + else + { + if (curr->finished) //end of the flight plan + { if (fp->getRepeat()) fp->restart(); else @@ -692,12 +335,15 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { return; } - if (next) { + if (next) + { + //TODO more intelligent method in AIFlightPlan, no need to send data it already has :-) tgt_heading = fp->getBearing(curr, next); spinCounter = 0; } - fp->IncrementWaypoint(eraseWaypoints); + //TODO let the fp handle this (loading of next leg) + fp->IncrementWaypoint((bool) trafficRef); if (!(fp->getNextWaypoint()) && trafficRef) loadNextLeg(); @@ -706,215 +352,101 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { next = fp->getNextWaypoint(); // Now that we have incremented the waypoints, excute some traffic manager specific code - if (trafficRef) { - double userLatitude = fgGetDouble("/position/latitude-deg"); - double userLongitude = fgGetDouble("/position/longitude-deg"); - double course, distance; - SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); - SGWayPoint user (userLongitude, userLatitude, 0); - user.CourseAndDistance(current, &course, &distance); - if ((distance * SG_METER_TO_NM) > TRAFFICTOAIDIST) { + if (trafficRef) + { + //TODO isn't this best executed right at the beginning? + if (! aiTrafficVisible()) + { setDie(true); return; } - FGAirport * dep = trafficRef->getDepartureAirport(); - FGAirport * arr = trafficRef->getArrivalAirport(); - if (!( dep && arr)) { + if (! handleAirportEndPoints(prev, now)) + { setDie(true); return; } - // This waypoint marks the fact that the aircraft has passed the initial taxi - // departure waypoint, so it can release the parking. - if (prev->name == "park2") - dep->getDynamics()->releaseParking(fp->getGate()); - // This is the last taxi waypoint, and marks the the end of the flight plan - // so, the schedule should update and wait for the next departure time. - if (prev->name == "END") { - // make sure to wait at least 20 minutes at parking to prevent "nervous" taxi behavior - // delayed aircraft. - time_t nextDeparture = trafficRef->getDepartureTime(); - if (nextDeparture < (now+1200)) { - nextDeparture = now + 1200; - } - fp->setTime(trafficRef->getDepartureTime()); - } - announcePositionToController(); - + + announcePositionToController(); + } - if (next) { - //cerr << "Current waypoint" << curr->name << endl; - //cerr << "Next waypoint" << next->name << endl; + if (next) + { fp->setLeadDistance(speed, tgt_heading, curr, next); } - //cerr << "5.1" << endl; - if (!(prev->on_ground)) { // only update the tgt altitude from flightplan if not on the ground + if (!(prev->on_ground)) // only update the tgt altitude from flightplan if not on the ground + { tgt_altitude_ft = prev->altitude; - if (curr->crossat > -1000.0) { - //cerr << "5.1a" << endl; + if (curr->crossat > -1000.0) + { use_perf_vs = false; tgt_vs = (curr->crossat - altitude_ft) / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr) - / 6076.0 / speed*60.0); - //cerr << "5.1b" << endl; + / 6076.0 / speed*60.0); tgt_altitude_ft = curr->crossat; - } else { - //cerr << "5.1c" << endl; + } + else + { use_perf_vs = true; - //cerr << "5.1d" << endl; - //cerr << "Setting target altitude : " <speed; hdg_lock = alt_lock = true; no_roll = prev->on_ground; - //cout << "Crossing waypoint: " << prev->name << endl; - //cout << " Target speed: " << tgt_speed << endl; - //cout << " Target altitude: " << tgt_altitude_ft << endl; - //cout << " Target heading: " << tgt_heading << endl << endl; - - } else { - double calc_bearing = fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); - //cerr << "Bearing = " << calc_bearing << endl; - if (speed < 0) { - calc_bearing +=180; - if (calc_bearing > 360) - calc_bearing -= 360; - } - - if (finite(calc_bearing)) { - double hdg_error = calc_bearing - tgt_heading; - if (fabs(hdg_error) > 1.0) { - TurnTo( calc_bearing ); - } - - } else { - cerr << "calc_bearing is not a finite number : " - << "Speed " << speed - << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() - << "waypoint " << curr->latitude << ", " << curr->longitude << endl; - cerr << "waypoint name " << curr->name; - exit(1); // FIXME - } - - double speed_diff = speed - prevSpeed; - // Update the lead distance calculation if speed has changed sufficiently - // to prevent spinning (hopefully); - if (fabs(speed_diff) > 10) { - prevSpeed = speed; - fp->setLeadDistance(speed, tgt_heading, curr, next); - } - - //cerr << "Done Processing FlightPlan"<< endl; } } -void FGAIAircraft::initializeFlightPlan() + +void FGAIAircraft::initializeFlightPlan() { } -bool FGAIAircraft::_getGearDown() const { +bool FGAIAircraft::_getGearDown() const +{ return ((props->getFloatValue("position/altitude-agl-ft") < 900.0) - && (props->getFloatValue("velocities/airspeed-kt") - < performance->land_speed*1.25)); + && (props->getFloatValue("velocities/airspeed-kt") + < performance->land_speed*1.25)); } -void FGAIAircraft::loadNextLeg() { - //delete fp; - //time_t now = time(NULL) + fgGetLong("/sim/time/warp"); - - //FGAIModelEntity entity; - //entity.m_class = "jet_transport"; - //entity.path = modelPath.c_str(); - //entity.flightplan = "none"; - //entity.latitude = _getLatitude(); - //entity.longitude = _getLongitude(); - //entity.altitude = trafficRef->getCruiseAlt() * 100; // convert from FL to feet - //entity.speed = 450; // HACK ALERT - //entity.fp = new FGAIFlightPlan(&entity, courseToDest, i->getDepartureTime(), dep, arr); +void FGAIAircraft::loadNextLeg() +{ + int leg; - if ((leg = fp->getLeg()) == 10) { + if ((leg = fp->getLeg()) == 10) + { trafficRef->next(); leg = 1; fp->setLeg(leg); - //cerr << "Resetting leg : " << leg << endl; } - //leg++; - //fp->setLeg(leg); - //cerr << "Creating leg number : " << leg << endl; + FGAirport *dep = trafficRef->getDepartureAirport(); FGAirport *arr = trafficRef->getArrivalAirport(); - if (!(dep && arr)) { + if (!(dep && arr)) + { setDie(true); - //cerr << "Failed to get airport in AIAircraft::ProcessFlightplan()" << endl; - //if (dep) - // cerr << "Departure " << dep->getId() << endl; - //if (arr) - // cerr << "Arrival " << arr->getId() << endl; - } else { + } + else + { double cruiseAlt = trafficRef->getCruiseAlt() * 100; - //cerr << "Creating new leg using " << cruiseAlt << " as cruise altitude."<< endl; fp->create (dep, - arr, - leg, - cruiseAlt, //(trafficRef->getCruiseAlt() * 100), // convert from FL to feet - trafficRef->getSpeed(), - _getLatitude(), - _getLongitude(), - false, - trafficRef->getRadius(), - trafficRef->getFlightType(), - acType, - company); - //prev = fp->getPreviousWaypoint(); - //curr = fp->getCurrentWaypoint(); - //next = fp->getNextWaypoint(); - //cerr << "25" << endl; - //if (next) - // { - // //cerr << "Next waypoint" << next->name << endl; - // fp->setLeadDistance(speed, tgt_heading, curr, next); - // } - //cerr << "25.1" << endl; - //if (curr->crossat > -1000.0) { - // //cerr << "25.1a" << endl; - // use_perf_vs = false; - // - // tgt_vs = (curr->crossat - altitude_ft)/ - // (fp->getDistanceToGo(pos.lat(), pos.lon(), curr)/6076.0/speed*60.0); - // //cerr << "25.1b" << endl; - // tgt_altitude_ft = curr->crossat; - //} else { - // //cerr << "25.1c" << endl; - // use_perf_vs = true; - // //cerr << "25.1d" << endl; - // tgt_altitude_ft = prev->altitude; - // //cerr << "Setting target altitude : " <speed; - //hdg_lock = alt_lock = true; - //no_roll = prev->on_ground; + arr, + leg, + cruiseAlt, //(trafficRef->getCruiseAlt() * 100), // convert from FL to feet + trafficRef->getSpeed(), + _getLatitude(), + _getLongitude(), + false, + trafficRef->getRadius(), + trafficRef->getFlightType(), + acType, + company); } - //else - //{ - //delete entity.fp; - //entity.fp = new FGAIFlightPlan(&entity, - // 999, // A hack - // trafficRef->getDepartureTime(), - // trafficRef->getDepartureAirport(), - // trafficRef->getArrivalAirport(), - // false, - // acType, - // company); - //SetFlightPlan(entity.fp); } @@ -923,7 +455,8 @@ void FGAIAircraft::loadNextLeg() { // Either this function or the logic of how often it is called // will almost certainly change. -void FGAIAircraft::getGroundElev(double dt) { +void FGAIAircraft::getGroundElev(double dt) +{ dt_elev_count += dt; // Update minimally every three secs, but add some randomness @@ -931,10 +464,11 @@ void FGAIAircraft::getGroundElev(double dt) { if (dt_elev_count < (3.0) + (rand() % 10)) return; else - dt_elev_count = 0; + dt_elev_count = 0; // Only do the proper hitlist stuff if we are within visible range of the viewer. - if (!invisible) { + if (!invisible) + { double visibility_meters = fgGetDouble("/environment/visibility-m"); FGViewer* vw = globals->get_current_view(); @@ -943,14 +477,16 @@ void FGAIAircraft::getGroundElev(double dt) { SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); SGWayPoint view (vw->getLongitude_deg(), vw->getLatitude_deg(), 0); view.CourseAndDistance(current, &course, &distance); - if(distance > visibility_meters) { + if(distance > visibility_meters) + { //aip.getSGLocation()->set_cur_elev_m(aptElev); return; } // FIXME: make sure the pos.lat/pos.lon values are in degrees ... double range = 500.0; - if (!globals->get_tile_mgr()->scenery_available(pos.getLatitudeDeg(), pos.getLongitudeDeg(), range)) { + if (!globals->get_tile_mgr()->scenery_available(pos.getLatitudeDeg(), pos.getLongitudeDeg(), range)) + { // Try to shedule tiles for that position. globals->get_tile_mgr()->update( aip.getSGLocation(), range ); } @@ -959,140 +495,706 @@ void FGAIAircraft::getGroundElev(double dt) { double alt; if (globals->get_scenery()->get_elevation_m(pos.getLatitudeDeg(), pos.getLongitudeDeg(), 20000.0, alt, 0)) tgt_altitude_ft = alt * SG_METER_TO_FEET; - - //cerr << "Target altitude : " << tgt_altitude_ft << endl; - // if (globals->get_scenery()->get_elevation_m(pos.lat(), pos.lon(), - // 20000.0, alt)) - // tgt_altitude_ft = alt * SG_METER_TO_FEET; - //cerr << "Target altitude : " << tgt_altitude_ft << endl; } } -void FGAIAircraft::setCallSign(const string& s) { +void FGAIAircraft::setCallSign(const string& s) +{ callsign = s; } -void FGAIAircraft::setTACANChannelID(const string& id) { - TACAN_channel_id = id; +void FGAIAircraft::doGroundAltitude() +{ + if (fabs(altitude_ft - (tgt_altitude_ft+groundOffset)) > 1000.0) + altitude_ft = (tgt_altitude_ft + groundOffset); + else + altitude_ft += 0.1 * ((tgt_altitude_ft+groundOffset) - altitude_ft); } -void FGAIAircraft::doGroundAltitude() { - if (fabs(altitude_ft - (tgt_altitude_ft+groundOffset)) > 1000.0) - altitude_ft = (tgt_altitude_ft + groundOffset); - else - altitude_ft += 0.1 * ((tgt_altitude_ft+groundOffset) - altitude_ft); -} +void FGAIAircraft::announcePositionToController() +{ + if (trafficRef) + { + int leg = fp->getLeg(); + // Note that leg was been incremented after creating the current leg, so we should use + // leg numbers here that are one higher than the number that is used to create the leg + // + switch (leg) + { + case 3: // Taxiing to runway + if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists()) + controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork(); + break; + case 4: //Take off tower controller + if (trafficRef->getDepartureAirport()->getDynamics()) + { + controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); + //if (trafficRef->getDepartureAirport()->getId() == "EHAM") { + //cerr << trafficRef->getCallSign() << " at runway " << fp->getRunway() << "Ready for departure " + // << trafficRef->getFlightType() << " to " << trafficRef->getArrivalAirport()->getId() << endl; + // if (controller == 0) { + //cerr << "Error in assigning controller at " << trafficRef->getDepartureAirport()->getId() << endl; + //} + + } + else + { + cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; + } + break; + case 9: // Taxiing for parking + if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists()) + controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork(); + break; + default: + controller = 0; + break; + } -void FGAIAircraft::announcePositionToController() -{ - if (trafficRef) { - //FGTaxiRoute *taxiRoute = fp->getTaxiRoute(); - - int leg = fp->getLeg(); - - //if (fp->getCurrentWaypoint()->routeIndex != 0) { - //char buffer[10]; - //snprintf (buffer, 10, "%d", node); - // Note that leg was been incremented after creating the current leg, so we should use - // leg numbers here that are one higher than the number that is used to create the leg - // - switch (leg) { - case 3: // Taxiing to runway - if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists()) - controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork(); - break; - case 4: //Take off tower controller - if (trafficRef->getDepartureAirport()->getDynamics()) - { - controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); - //if (trafficRef->getDepartureAirport()->getId() == "EHAM") { - //cerr << trafficRef->getCallSign() << " at runway " << fp->getRunway() << "Ready for departure " - // << trafficRef->getFlightType() << " to " << trafficRef->getArrivalAirport()->getId() << endl; - // if (controller == 0) { - //cerr << "Error in assigning controller at " << trafficRef->getDepartureAirport()->getId() << endl; - //} - - } - else { - cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; - } - break; - case 9: // Taxiing for parking - if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists()) - controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork(); - break; - default: - controller = 0; - break; - } - - if ((controller != prevController) && (prevController != 0)) { - prevController->signOff(getID()); - string callsign = trafficRef->getCallSign(); - if ( trafficRef->getHeavy()) - callsign += "Heavy"; - switch (leg) { - case 3: - //cerr << callsign << " ready to taxi to runway " << fp->getRunway() << endl; - break; - case 4: - //cerr << callsign << " at runway " << fp->getRunway() << "Ready for take-off. " - // << trafficRef->getFlightRules() << " to " << trafficRef->getArrivalAirport()->getId() - // << "(" << trafficRef->getArrivalAirport()->getName() << ")."<< endl; - break; - } - } - prevController = controller; - if (controller) { - controller->announcePosition(getID(), fp, fp->getCurrentWaypoint()->routeIndex, - _getLatitude(), _getLongitude(), hdg, speed, altitude_ft, - trafficRef->getRadius(), leg, trafficRef->getCallSign()); + if ((controller != prevController) && (prevController != 0)) + { + prevController->signOff(getID()); + string callsign = trafficRef->getCallSign(); + if ( trafficRef->getHeavy()) + callsign += "Heavy"; + switch (leg) + { + case 3: + //cerr << callsign << " ready to taxi to runway " << fp->getRunway() << endl; + break; + case 4: + //cerr << callsign << " at runway " << fp->getRunway() << "Ready for take-off. " + // << trafficRef->getFlightRules() << " to " << trafficRef->getArrivalAirport()->getId() + // << "(" << trafficRef->getArrivalAirport()->getName() << ")."<< endl; + break; + } + } + prevController = controller; + if (controller) + { + controller->announcePosition(getID(), fp, fp->getCurrentWaypoint()->routeIndex, + _getLatitude(), _getLongitude(), hdg, speed, altitude_ft, + trafficRef->getRadius(), leg, trafficRef->getCallSign()); + } } - } } void FGAIAircraft::processATC(FGATCInstruction instruction) { - //cerr << "Processing ATC instruction (not Implimented yet)" << endl; - if (instruction.getHoldPattern ()) { - } - - // Hold Position - if (instruction.getHoldPosition ()) { - if (!holdPos) { - //if (trafficRef) - //cerr << trafficRef->getCallSign() << "Holding Position " << endl; - holdPos = true; - } - AccelTo(0.0); - } else { - if (holdPos) { - //if (trafficRef) - // cerr << trafficRef->getCallSign() << " Resuming Taxi " << endl; - holdPos = false; - } - // Change speed Instruction. This can only be excecuted when there is no - // Hold position instruction. - if (instruction.getChangeSpeed ()) { - // if (trafficRef) - //cerr << trafficRef->getCallSign() << " Changing Speed " << endl; - AccelTo(instruction.getSpeed()); - }else { - if (fp) AccelTo(fp->getPreviousWaypoint()->speed); + //cerr << "Processing ATC instruction (not Implimented yet)" << endl; + if (instruction.getHoldPattern ()) + { } - } - if (instruction.getChangeHeading ()) { - hdg_lock = false; - TurnTo(instruction.getHeading()); - } else { - if (fp) {hdg_lock = true;} - } - if (instruction.getChangeAltitude()) { - } + // Hold Position + if (instruction.getHoldPosition ()) + { + if (!holdPos) + { + holdPos = true; + } + AccelTo(0.0); + } + else + { + if (holdPos) + { + //if (trafficRef) + // cerr << trafficRef->getCallSign() << " Resuming Taxi " << endl; + holdPos = false; + } + // Change speed Instruction. This can only be excecuted when there is no + // Hold position instruction. + if (instruction.getChangeSpeed ()) + { + // if (trafficRef) + //cerr << trafficRef->getCallSign() << " Changing Speed " << endl; + AccelTo(instruction.getSpeed()); + } + else + { + if (fp) AccelTo(fp->getPreviousWaypoint()->speed); + } + } + if (instruction.getChangeHeading ()) + { + hdg_lock = false; + TurnTo(instruction.getHeading()); + } + else + { + if (fp) {hdg_lock = true;} + } + if (instruction.getChangeAltitude()) + { + } + +} + + +void FGAIAircraft::handleFirstWaypoint() +{ + bool eraseWaypoints; //TODO YAGNI + if (trafficRef) + { + eraseWaypoints = true; + } + else + { + eraseWaypoints = false; + } + + FGAIFlightPlan::waypoint* prev = 0; // the one behind you + FGAIFlightPlan::waypoint* curr = 0; // the one ahead + FGAIFlightPlan::waypoint* next = 0;// the next plus 1 + + spinCounter = 0; + tempReg = ""; + + //TODO fp should handle this + fp->IncrementWaypoint(eraseWaypoints); + if (!(fp->getNextWaypoint()) && trafficRef) + loadNextLeg(); + + prev = fp->getPreviousWaypoint(); //first waypoint + curr = fp->getCurrentWaypoint(); //second waypoint + next = fp->getNextWaypoint(); //third waypoint (might not exist!) + + setLatitude(prev->latitude); + setLongitude(prev->longitude); + setSpeed(prev->speed); + setAltitude(prev->altitude); + + if (prev->speed > 0.0) + setHeading(fp->getBearing(prev->latitude, prev->longitude, curr)); + else + setHeading(fp->getBearing(curr->latitude, curr->longitude, prev)); + + // If next doesn't exist, as in incrementally created flightplans for + // AI/Trafficmanager created plans, + // Make sure lead distance is initialized otherwise + if (next) + fp->setLeadDistance(speed, hdg, curr, next); + + if (curr->crossat > -1000.0) //use a calculated descent/climb rate + { + use_perf_vs = false; + tgt_vs = (curr->crossat - prev->altitude) + / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr) + / 6076.0 / prev->speed*60.0); + tgt_altitude_ft = curr->crossat; + } + else + { + use_perf_vs = true; + tgt_altitude_ft = prev->altitude; + } + alt_lock = hdg_lock = true; + no_roll = prev->on_ground; + if (no_roll) + { + Transform(); // make sure aip is initialized. + getGroundElev(60.1); // make sure it's executed first time around, so force a large dt value + doGroundAltitude(); + } + // Make sure to announce the aircraft's position + announcePositionToController(); + prevSpeed = 0; +} + + +/** + * Check Execution time (currently once every 100 ms) + * Add a bit of randomization to prevent the execution of all flight plans + * in synchrony, which can add significant periodic framerate flutter. + * + * @param now + * @return + */ +bool FGAIAircraft::fpExecutable(time_t now) +{ + double rand_exec_time = (rand() % 100) / 100; + return (dt_count > (0.1+rand_exec_time)) && (fp->isActive(now)); +} + + +/** + * Check to see if we've reached the lead point for our next turn + * + * @param curr + * @return + */ +bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) +{ + double dist_to_go = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); + + //cerr << "2" << endl; + double lead_dist = fp->getLeadDistance(); + //cerr << " Distance : " << dist_to_go << ": Lead distance " << lead_dist << endl; + // experimental: Use fabs, because speed can be negative (I hope) during push_back. + + if (lead_dist < fabs(2*speed)) + { + //don't skip over the waypoint + lead_dist = fabs(2*speed); + //cerr << "Extending lead distance to " << lead_dist << endl; + } + + //prev_dist_to_go = dist_to_go; + return dist_to_go < lead_dist; +} + + +bool FGAIAircraft::aiTrafficVisible() +{ + double userLatitude = fgGetDouble("/position/latitude-deg"); + double userLongitude = fgGetDouble("/position/longitude-deg"); + double course, distance; + + SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); + SGWayPoint user (userLongitude, userLatitude, 0); + + user.CourseAndDistance(current, &course, &distance); + + return ((distance * SG_METER_TO_NM) <= TRAFFICTOAIDIST); +} + + +/** + * Handle release of parking gate, once were taxiing. Also ensure service time at the gate + * in the case of an arrival. + * + * @param prev + * @return + */ + +//TODO the trafficRef is the right place for the method +bool FGAIAircraft::handleAirportEndPoints(FGAIFlightPlan::waypoint* prev, time_t now) +{ + // prepare routing from one airport to another + FGAirport * dep = trafficRef->getDepartureAirport(); + FGAirport * arr = trafficRef->getArrivalAirport(); + + if (!( dep && arr)) + return false; + + // This waypoint marks the fact that the aircraft has passed the initial taxi + // departure waypoint, so it can release the parking. + if (prev->name == "park2") + dep->getDynamics()->releaseParking(fp->getGate()); + + // This is the last taxi waypoint, and marks the the end of the flight plan + // so, the schedule should update and wait for the next departure time. + if (prev->name == "END") + { + time_t nextDeparture = trafficRef->getDepartureTime(); + // make sure to wait at least 20 minutes at parking to prevent "nervous" taxi behavior + if (nextDeparture < (now+1200)) + { + nextDeparture = now + 1200; + } + fp->setTime(nextDeparture); + } + + return true; +} + + +/** + * Check difference between target bearing and current heading and correct if necessary. + * + * @param curr + */ +void FGAIAircraft::controlHeading(FGAIFlightPlan::waypoint* curr) +{ + double calc_bearing = fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); + //cerr << "Bearing = " << calc_bearing << endl; + if (speed < 0) + { + calc_bearing +=180; + if (calc_bearing > 360) + calc_bearing -= 360; + } + + if (finite(calc_bearing)) + { + double hdg_error = calc_bearing - tgt_heading; + if (fabs(hdg_error) > 1.0) + { + TurnTo( calc_bearing ); + } + + } + else + { + cerr << "calc_bearing is not a finite number : " + << "Speed " << speed + << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() + << "waypoint " << curr->latitude << ", " << curr->longitude << endl; + cerr << "waypoint name " << curr->name; + exit(1); // FIXME + } +} + + +/** + * Update the lead distance calculation if speed has changed sufficiently + * to prevent spinning (hopefully); + * + * @param curr + * @param next + */ +void FGAIAircraft::controlSpeed(FGAIFlightPlan::waypoint* curr, FGAIFlightPlan::waypoint* next) +{ + double speed_diff = speed - prevSpeed; + + if (fabs(speed_diff) > 10) + { + prevSpeed = speed; + fp->setLeadDistance(speed, tgt_heading, curr, next); + } +} + + +/** + * Update target values (heading, alt, speed) depending on flight plan or control properties + */ +bool FGAIAircraft::updateTargetValues() +{ + if (fp) // AI object has a flightplan + { + //TODO make this a function of AIBase + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + //cerr << "UpateTArgetValues() " << endl; + ProcessFlightPlan(dt, now); + if (! fp->isActive(now)) + { + // Do execute Ground elev for inactive aircraft, so they + // Are repositioned to the correct ground altitude when the user flies within visibility range. + // In addition, check whether we are out of user range, so this aircraft + // can be deleted. + if (no_roll) + { + Transform(); // make sure aip is initialized. + if (trafficRef) + { + //cerr << trafficRef->getRegistration() << " Setting altitude to " << altitude_ft; + if (! aiTrafficVisible()) + { + setDie(true); + return false; + } + getGroundElev(dt); + doGroundAltitude(); + // Transform(); + pos.setElevationFt(altitude_ft); + } + } + return false; + } + } + else + { + // no flight plan, update target heading, speed, and altitude + // from control properties. These default to the initial + // settings in the config file, but can be changed "on the + // fly". + string lat_mode = props->getStringValue("controls/flight/lateral-mode"); + if ( lat_mode == "roll" ) + { + double angle + = props->getDoubleValue("controls/flight/target-roll" ); + RollTo( angle ); + } + else + { + double angle + = props->getDoubleValue("controls/flight/target-hdg" ); + TurnTo( angle ); + } + + string lon_mode + = props->getStringValue("controls/flight/longitude-mode"); + if ( lon_mode == "alt" ) + { + double alt = props->getDoubleValue("controls/flight/target-alt" ); + ClimbTo( alt ); + } + else + { + double angle + = props->getDoubleValue("controls/flight/target-pitch" ); + PitchTo( angle ); + } + + AccelTo( props->getDoubleValue("controls/flight/target-spd" ) ); + } + return true; +} + + +/** + * Adjust the speed (accelerate/decelerate) to tgt_speed. + */ +void FGAIAircraft::adjustSpeed(double tgt_speed) +{ + double speed_diff = tgt_speed - speed; + speed_diff = groundTargetSpeed - speed; + + if (speed_diff > 0.0) // need to accelerate + { + speed += performance->accel * dt; + if ( speed > tgt_speed ) + speed = tgt_speed; + + } + else if (speed_diff < 0.0) + { + if (no_roll) + { + // on ground (aircraft can't roll) + // deceleration performance is better due to wheel brakes. + speed -= performance->decel * dt * 3; + } + else + { + speed -= performance->decel * dt; + } + + if ( speed < tgt_speed ) + speed = tgt_speed; + + } +} + + +void FGAIAircraft::updatePosition() +{ + // convert speed to degrees per second + double speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS ) + * speed * 1.686 / ft_per_deg_lat; + double speed_east_deg_sec = sin( hdg * SGD_DEGREES_TO_RADIANS ) + * speed * 1.686 / ft_per_deg_lon; + + // set new position + pos.setLatitudeDeg( pos.getLatitudeDeg() + speed_north_deg_sec * dt); + pos.setLongitudeDeg( pos.getLongitudeDeg() + speed_east_deg_sec * dt); +} + + +void FGAIAircraft::updateHeading() +{ + // adjust heading based on current bank angle + if (roll == 0.0) + roll = 0.01; + + if (roll != 0.0) + { + // double turnConstant; + //if (no_roll) + // turnConstant = 0.0088362; + //else + // turnConstant = 0.088362; + // If on ground, calculate heading change directly + if (no_roll) + { + double headingDiff = fabs(hdg-tgt_heading); + + if (headingDiff > 180) + headingDiff = fabs(headingDiff - 360); + + groundTargetSpeed = tgt_speed - (tgt_speed * (headingDiff/45)); + if (sign(groundTargetSpeed) != sign(tgt_speed)) + groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode + + if (headingDiff > 30.0) + { + // invert if pushed backward + headingChangeRate += dt * sign(roll); + + if (headingChangeRate > 30) + headingChangeRate = 30; + else if (headingChangeRate < -30) + headingChangeRate = -30; + + } + else + { + if (fabs(headingChangeRate) > headingDiff) + headingChangeRate = headingDiff*sign(roll); + else + headingChangeRate += dt * sign(roll); + } + hdg += headingChangeRate * dt; + } + else + { + if (fabs(speed) > 1.0) + { + turn_radius_ft = 0.088362 * speed * speed + / tan( fabs(roll) / SG_RADIANS_TO_DEGREES ); + } + else + { + // Check if turn_radius_ft == 0; this might lead to a division by 0. + turn_radius_ft = 1.0; + } + double turn_circum_ft = SGD_2PI * turn_radius_ft; + double dist_covered_ft = speed * 1.686 * dt; + double alpha = dist_covered_ft / turn_circum_ft * 360.0; + hdg += alpha * sign(roll); + } + while ( hdg > 360.0 ) + { + hdg -= 360.0; + spinCounter++; + } + while ( hdg < 0.0) + { + hdg += 360.0; + spinCounter--; + } + } +} + + +void FGAIAircraft::updateBankAngles() +{ + // adjust target bank angle if heading lock engaged + if (hdg_lock) + { + double bank_sense = 0.0; + double diff = fabs(hdg - tgt_heading); + if (diff > 180) + diff = fabs(diff - 360); + + double sum = hdg + diff; + if (sum > 360.0) + sum -= 360.0; + if (fabs(sum - tgt_heading) < 1.0) + { + bank_sense = 1.0; // right turn + } + else + { + bank_sense = -1.0; // left turn + } + if (diff < 30) + { + tgt_roll = diff * bank_sense; + } + else + { + tgt_roll = 30.0 * bank_sense; + } + if ((fabs((double) spinCounter) > 1) && (diff > 30)) + { + tgt_speed *= 0.999; // Ugly hack: If aircraft get stuck, they will continually spin around. + // The only way to resolve this is to make them slow down. + } + } + + // adjust bank angle, use 9 degrees per second + double bank_diff = tgt_roll - roll; + if (fabs(bank_diff) > 0.2) + { + if (bank_diff > 0.0) + roll += 9.0 * dt; + + if (bank_diff < 0.0) + roll -= 9.0 * dt; + //while (roll > 180) roll -= 360; + //while (roll < 180) roll += 360; + } +} + + +void FGAIAircraft::updateAltitudes() +{ + // adjust altitude (meters) based on current vertical speed (fpm) + altitude_ft += vs / 60.0 * dt; + pos.setElevationFt(altitude_ft); + + // adjust target Altitude, based on ground elevation when on ground + if (no_roll) + { + getGroundElev(dt); + doGroundAltitude(); + } + else + { + // find target vertical speed if altitude lock engaged + if (alt_lock && use_perf_vs) + { + if (altitude_ft < tgt_altitude_ft) + { + tgt_vs = tgt_altitude_ft - altitude_ft; + if (tgt_vs > performance->climb_rate) + tgt_vs = performance->climb_rate; + } + else + { + tgt_vs = tgt_altitude_ft - altitude_ft; + if (tgt_vs < (-performance->descent_rate)) + tgt_vs = -performance->descent_rate; + } + } + + if (alt_lock && !use_perf_vs) + { + double max_vs = 4*(tgt_altitude_ft - altitude_ft); + double min_vs = 100; + if (tgt_altitude_ft < altitude_ft) + min_vs = -100.0; + if ((fabs(tgt_altitude_ft - altitude_ft) < 1500.0) + && (fabs(max_vs) < fabs(tgt_vs))) + tgt_vs = max_vs; + + if (fabs(tgt_vs) < fabs(min_vs)) + tgt_vs = min_vs; + } + } +} + + +void FGAIAircraft::updateVerticalSpeed() +{ + // adjust vertical speed + double vs_diff = tgt_vs - vs; + if (fabs(vs_diff) > 10.0) + { + if (vs_diff > 0.0) + { + vs += (performance->climb_rate / 3.0) * dt; + + if (vs > tgt_vs) + vs = tgt_vs; + } + else + { + vs -= (performance->descent_rate / 3.0) * dt; + + if (vs < tgt_vs) + vs = tgt_vs; + } + } +} + + +void FGAIAircraft::matchPitchAngle() +{ + // match pitch angle to vertical speed + if (vs > 0) + { + pitch = vs * 0.005; + } + else + { + pitch = vs * 0.002; + } } diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index d1b331d71..035b44bc7 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -62,7 +62,7 @@ public: virtual void readFromScenario(SGPropertyNode* scFileNode); - virtual bool init(bool search_in_AI_path=false); + // virtual bool init(bool search_in_AI_path=false); virtual void bind(); virtual void unbind(); virtual void update(double dt); @@ -81,7 +81,6 @@ public: void TurnTo(double heading); void ProcessFlightPlan( double dt, time_t now ); void setCallSign(const string& ); - void setTACANChannelID(const string& ); void getGroundElev(double dt); void doGroundAltitude(); @@ -93,9 +92,11 @@ public: void announcePositionToController(); void processATC(FGATCInstruction instruction); - inline void SetTanker(bool setting) { isTanker = setting; }; virtual const char* getTypeString(void) const { return "aircraft"; } +protected: + void Run(double dt); + private: FGAISchedule *trafficRef; FGATCController *controller, *prevController; @@ -112,9 +113,25 @@ private: const PERF_STRUCT *performance; bool use_perf_vs; SGPropertyNode_ptr refuel_node; - bool isTanker; - void Run(double dt); + // helpers for Run + bool fpExecutable(time_t now); + void handleFirstWaypoint(void); + bool leadPointReached(FGAIFlightPlan::waypoint* curr); + bool handleAirportEndPoints(FGAIFlightPlan::waypoint* prev, time_t now); + bool aiTrafficVisible(void); + void controlHeading(FGAIFlightPlan::waypoint* curr); + void controlSpeed(FGAIFlightPlan::waypoint* curr, + FGAIFlightPlan::waypoint* next); + bool updateTargetValues(); + void adjustSpeed(double tgt_speed); + void updatePosition(); + void updateHeading(); + void updateBankAngles(); + void updateAltitudes(); + void updateVerticalSpeed(); + void matchPitchAngle(); + double sign(double x); string acType; @@ -124,13 +141,11 @@ private: double prevSpeed; double prev_dist_to_go; - bool holdPos; + bool holdPos; bool _getGearDown() const; bool reachedWaypoint; string callsign; // The callsign of this tanker. - string TACAN_channel_id; // The TACAN channel of this tanker - bool contact; // set if this tanker is within fuelling range }; diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index febaf3d15..2d1887582 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -349,6 +349,7 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints ) } else wpt_iterator++; + } // gives distance in feet from a position to a waypoint diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 90a7d0acd..cff51aaa6 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -102,7 +102,7 @@ public: FGTaxiRoute *getTaxiRoute() { return taxiRoute; }; void deleteTaxiRoute(); string getRunway() { return activeRunway; }; - + bool isActive(time_t time) {return time >= this->getStartTime();}; private: FGRunway rwy; diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index effbf63b6..6d9a3cf5a 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -32,6 +32,7 @@ #include "AICarrier.hxx" #include "AIStatic.hxx" #include "AIMultiplayer.hxx" +#include "AITanker.hxx" #include @@ -262,7 +263,11 @@ FGAIManager::processScenario( const string &filename ) { continue; std::string type = scEntry->getStringValue("type", "aircraft"); - if (type == "aircraft") { + if (type == "tanker") { // refueling scenarios + FGAITanker* aircraft = new FGAITanker; + aircraft->readFromScenario(scEntry); + attach(aircraft); + } else if (type == "aircraft") { FGAIAircraft* aircraft = new FGAIAircraft; aircraft->readFromScenario(scEntry); attach(aircraft); diff --git a/src/AIModel/AITanker.cxx b/src/AIModel/AITanker.cxx new file mode 100644 index 000000000..d184b2b97 --- /dev/null +++ b/src/AIModel/AITanker.cxx @@ -0,0 +1,72 @@ +// AITanker.cxx Based on David Culp's AIModel code +// - Tanker specific code isolated from AI Aircraft code +// by Thomas Foerster, started June 2007 +// +// +// Original code written by David Culp, started October 2003. +// - davidculp2@comcast.net/ +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + +#include "AITanker.hxx" + +FGAITanker::FGAITanker(FGAISchedule* ref): FGAIAircraft(ref){ +} + +FGAITanker::~FGAITanker() {} + +void FGAITanker::readFromScenario(SGPropertyNode* scFileNode) { + if (!scFileNode) + return; + + FGAIAircraft::readFromScenario(scFileNode); + setTACANChannelID(scFileNode->getStringValue("TACAN-channel-ID")); +} + +void FGAITanker::bind() { + FGAIAircraft::bind(); + + props->tie("refuel/contact", SGRawValuePointer(&contact)); + props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); + props->setBoolValue("tanker", true); +} + +void FGAITanker::unbind() { + FGAIAircraft::unbind(); + props->untie("refuel/contact"); +} + +void FGAITanker::setTACANChannelID(const string& id) { + TACAN_channel_id = id; +} + +void FGAITanker::Run(double dt) { + FGAIAircraft::Run(dt); + + //###########################// + // do calculations for radar // + //###########################// + double range_ft2 = UpdateRadar(manager); + + // check if radar contact + if ( (range_ft2 < 250.0 * 250.0) && (y_shift > 0.0) + && (elevation > 0.0) ) { + //refuel_node->setBoolValue(true); + contact = true; + } else { + //refuel_node->setBoolValue(false); + contact = false; + } +} diff --git a/src/AIModel/AITanker.hxx b/src/AIModel/AITanker.hxx new file mode 100644 index 000000000..9114dc200 --- /dev/null +++ b/src/AIModel/AITanker.hxx @@ -0,0 +1,58 @@ +// AITanker.hxx Based on David Culp's AIModel code +// - Tanker specific code isolated from AI Aircraft code +// by Thomas Foerster, started June 2007 +// +// +// Original code written by David Culp, started October 2003. +// - davidculp2@comcast.net/ +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef FGAITANKER_HXX +#define FGAITANKER_HXX + +#include + +/** + * An AI tanker for air-air refueling. + * + * This class is just a refactoring of the AA refueling related code in FGAIAircraft. The idea + * is to have a clean generic AIAircraft class without any special functionality. In your + * scenario specification use 'tanker' as the scenario type to use this class. + * + * @author Thomas Förster +*/ + +class FGAITanker : public FGAIAircraft { +public: + FGAITanker(FGAISchedule* ref = 0); + ~FGAITanker(); + + virtual void readFromScenario(SGPropertyNode* scFileNode); + virtual void bind(); + virtual void unbind(); + + virtual const char* getTypeString(void) const { return "tanker"; } + + void setTACANChannelID(const string& id); + +private: + string TACAN_channel_id; // The TACAN channel of this tanker + bool contact; // set if this tanker is within fuelling range + + virtual void Run(double dt); + +}; + +#endif diff --git a/src/AIModel/Makefile.am b/src/AIModel/Makefile.am index e5b376584..4c768c681 100644 --- a/src/AIModel/Makefile.am +++ b/src/AIModel/Makefile.am @@ -1,18 +1,19 @@ noinst_LIBRARIES = libAIModel.a -libAIModel_a_SOURCES = \ - submodel.cxx submodel.hxx \ +libAIModel_a_SOURCES = submodel.cxx submodel.hxx \ AIManager.hxx AIManager.cxx \ AIBase.hxx AIBase.cxx \ - AIAircraft.hxx AIAircraft.cxx \ - AIMultiplayer.hxx AIMultiplayer.cxx \ + AIAircraft.hxx AIAircraft.cxx AIMultiplayer.hxx \ + AIMultiplayer.cxx \ AIShip.hxx AIShip.cxx \ AIBallistic.hxx AIBallistic.cxx \ AIStorm.hxx AIStorm.cxx \ AIThermal.hxx AIThermal.cxx \ AIFlightPlan.hxx AIFlightPlan.cxx \ - AIFlightPlanCreate.cxx AIFlightPlanCreateCruise.cxx \ - AICarrier.hxx AICarrier.cxx \ - AIStatic.hxx AIStatic.cxx + AIFlightPlanCreate.cxx \ + AIFlightPlanCreateCruise.cxx \ + AICarrier.hxx AICarrier.cxx \ + AIStatic.hxx AIStatic.cxx \ + AITanker.cxx AITanker.hxx INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src diff --git a/src/Include/config.h-msvc8 b/src/Include/config.h-msvc8 index 4a719d7e1..d30ede6a9 100755 --- a/src/Include/config.h-msvc8 +++ b/src/Include/config.h-msvc8 @@ -1,8 +1,5 @@ -/* Special single config.h for MSVC8 build */ - -#define ENABLE_OSGVIEWER -#define PU_USE_NATIVE +/* Special single config.h for MSVC6 build - Geoff McLane - 23 July, 2003 */ /* Define to enable plib joystick support */ #ifndef ENABLE_PLIB_JOYSTICK -- 2.39.5