X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIAircraft.cxx;h=05860b60339eb1961a3738cd4d810668bd10d06a;hb=c51c0b1664b70c10a186f6b3d528a8cffec0ba56;hp=9e5a5675b96c5c7d296aa9d660b9bd3a6eca22ac;hpb=5fc834f7f5f103081baba8274afa4fcd1bf72dea;p=flightgear.git diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 9e5a5675b..05860b603 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -43,52 +43,54 @@ SG_USING_STD(string); #include "AIAircraft.hxx" - 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[] = { - // light aircraft - {2.0, 2.0, 450.0, 1000.0, 70.0, 80.0, 100.0, 80.0, 60.0}, - // ww2_fighter - {4.0, 2.0, 3000.0, 1500.0, 110.0, 180.0, 250.0, 200.0, 100.0}, - // jet_transport - {5.0, 2.0, 3000.0, 1500.0, 140.0, 300.0, 430.0, 300.0, 130.0}, - // jet_fighter - {7.0, 3.0, 4000.0, 2000.0, 150.0, 350.0, 500.0, 350.0, 150.0}, - // tanker - {5.0, 2.0, 3000.0, 1500.0, 140.0, 300.0, 430.0, 300.0, 130.0}, - // ufo (extreme accel/decel) - {30.0, 30.0, 6000.0, 6000.0, 150.0, 300.0, 430.0, 300.0, 130.0} -}; - - -FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : - FGAIBase(otAircraft) -{ +#include "performancedata.hxx" +#include "performancedb.hxx" + +//#include + +static string tempReg; + +class AI_OutOfSight{}; +class FP_Inactive{}; + +FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : FGAIBase(otAircraft) { trafficRef = ref; - if (trafficRef) + if (trafficRef) { groundOffset = trafficRef->getGroundOffset(); + setCallSign(trafficRef->getCallSign()); + } else groundOffset = 0; fp = 0; + controller = 0; + prevController = 0; dt_count = 0; dt_elev_count = 0; use_perf_vs = true; - isTanker = false; + + no_roll = false; + tgt_speed = 0; + speed = 0; + groundTargetSpeed = 0; // set heading and altitude locks hdg_lock = false; alt_lock = false; roll = 0; headingChangeRate = 0.0; + + holdPos = false; + + _performance = 0; //TODO initialize to JET_TRANSPORT from PerformanceDB + } FGAIAircraft::~FGAIAircraft() { //delete fp; + if (controller) + controller->signOff(getID()); } @@ -102,13 +104,6 @@ void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) { setFlightPlan(scFileNode->getStringValue("flightplan"), scFileNode->getBoolValue("repeat", false)); setCallSign(scFileNode->getStringValue("callsign")); - setTACANChannelID(scFileNode->getStringValue("TACAN-channel-ID")); -} - - -bool FGAIAircraft::init() { - //refuel_node = fgGetNode("systems/refuel/contact", true); - return FGAIBase::init(); } @@ -117,11 +112,7 @@ void FGAIAircraft::bind() { props->tie("controls/gear/gear-down", SGRawValueMethods(*this, - &FGAIAircraft::_getGearDown)); - props->tie("refuel/contact", SGRawValuePointer(&contact)); - props->setStringValue("callsign", callsign.c_str()); - props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); - props->setBoolValue("tanker",isTanker); + &FGAIAircraft::_getGearDown)); } @@ -129,7 +120,6 @@ void FGAIAircraft::unbind() { FGAIBase::unbind(); props->untie("controls/gear/gear-down"); - props->untie("refuel/contact"); } @@ -139,348 +129,36 @@ void FGAIAircraft::update(double dt) { Transform(); } - void FGAIAircraft::setPerformance(const std::string& acclass) { - if (acclass == "light") { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::LIGHT]); - } else if (acclass == "ww2_fighter") { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::WW2_FIGHTER]); - } else if (acclass == "jet_transport") { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_TRANSPORT]); - } else if (acclass == "jet_fighter") { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_FIGHTER]); - } else if (acclass == "tanker") { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_TRANSPORT]); - SetTanker(true); - } else if (acclass == "ufo") { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::UFO]); - } else { - SetPerformance(&FGAIAircraft::settings[FGAIAircraft::JET_TRANSPORT]); - } -} - - -void FGAIAircraft::SetPerformance(const PERF_STRUCT *ps) { - - performance = ps; -} - - -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. - if (no_roll) { - Transform(); // make sure aip is initialized. - 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 ); - } - - AccelTo( props->getDoubleValue("controls/flight/target-spd" ) ); - } - - 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 ( speed > tgt_speed ) speed = tgt_speed; // don't overshoot - } else if (speed_diff < 0.0) { - if (no_roll) { - // on ground (aircraft can't roll) - speed -= performance->decel * dt * 3; - } else { - speed -= performance->decel * dt; - } - if ( speed < tgt_speed ) speed = tgt_speed; // don't overshoot - } - - // 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); + static PerformanceDB perfdb; //TODO make it a global service + setPerformance(perfdb.getDataFor(acclass)); + } - 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; + void FGAIAircraft::setPerformance(PerformanceData *ps) { + _performance = ps; + } - 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); + void FGAIAircraft::Run(double dt) { + FGAIAircraft::dt = dt; - // 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; - } - } + try { + updatePrimaryTargetValues(); // target hdg, alt, speed + } + catch (AI_OutOfSight) { + return; + } + catch (FP_Inactive) { + return; + } - 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; + handleATCRequests(); // ATC also has a word to say + updateSecondaryTargetValues(); // target roll, vertical speed, pitch + updateActualState(); + UpdateRadar(manager); + } - if (fabs(tgt_vs) < fabs(min_vs)) - tgt_vs = min_vs; - } - } - // adjust vertical speed - double vs_diff = tgt_vs - vs; - if (fabs(vs_diff) > 10.0) { - if (vs_diff > 0.0) { - vs += 900.0 * dt; - - if (vs > tgt_vs) - vs = tgt_vs; - } else { - vs -= 400.0 * dt; - - if (vs < tgt_vs) - vs = tgt_vs; - } - } - - // match pitch angle to vertical speed - if (vs > 0) { - pitch = vs * 0.005; - } else { - pitch = vs * 0.002; - } - - //###########################// - // 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; - } -} void FGAIAircraft::AccelTo(double speed) { @@ -518,10 +196,10 @@ void FGAIAircraft::TurnTo(double heading) { double FGAIAircraft::sign(double x) { - if ( x < 0.0 ) - return -1.0; + if (x == 0.0) + return x; else - return 1.0; + return x/fabs(x); } @@ -541,159 +219,52 @@ void FGAIAircraft::SetFlightPlan(FGAIFlightPlan *f) { void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { - bool eraseWaypoints; - if (trafficRef) { -// FGAirport *arr; -// FGAirport *dep; - eraseWaypoints = true; -// cerr << trafficRef->getRegistration(); -// cerr << "Departure airport " << endl; -// dep = trafficRef->getDepartureAirport(); -// if (dep) -// cerr << dep->getId() << endl; -// cerr << "Arrival airport " << endl; -// arr = trafficRef->getArrivalAirport(); -// if (arr) -// cerr << arr->getId() <getPreviousWaypoint(); curr = fp->getCurrentWaypoint(); next = fp->getNextWaypoint(); - dt_count += dt; - if (!prev) { //beginning of flightplan, do this initialization once - //setBank(0.0); - spinCounter = 0; - tempReg = ""; - //prev_dist_to_go = HUGE; - //cerr << "Before increment " << curr-> name << endl; - fp->IncrementWaypoint(eraseWaypoints); - //prev = fp->getPreviousWaypoint(); //first waypoint - //curr = fp->getCurrentWaypoint(); //second waypoint - //next = fp->getNextWaypoint(); //third waypoint (might not exist!) - //cerr << "After increment " << prev-> name << endl; - if (!(fp->getNextWaypoint()) && trafficRef) - loadNextLeg(); + dt_count += dt; - //cerr << "After load " << prev-> name << endl; - prev = fp->getPreviousWaypoint(); //first waypoint - curr = fp->getCurrentWaypoint(); //second waypoint - next = fp->getNextWaypoint(); //third waypoint (might not exist!) - //cerr << "After load " << prev-> name << endl; - 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 exectuted first time around, so force a large dt value - //getGroundElev(60.1); // Need to do this twice. - //cerr << trafficRef->getRegistration() << " Setting altitude to " << tgt_altitude << endl; - doGroundAltitude(); //(tgt_altitude); - } - prevSpeed = 0; - //cout << "First waypoint: " << prev->name << endl; - //cout << " Target speed: " << tgt_speed << endl; - //cout << " Target altitude: " << tgt_altitude << endl; - //cout << " Target heading: " << tgt_heading << endl << endl; - //cerr << "Done Flightplan init" << endl; + /////////////////////////////////////////////////////////////////////////// + // Initialize the flightplan + ////////////////////////////////////////////////////////////////////////// + if (!prev) { + handleFirstWaypoint(); return; - } // end of initialization + } // end of initialization + if (! fpExecutable(now)) + return; + dt_count = 0; - // let's only process the flight plan every 100 ms. - if ((dt_count < 0.1) || (now < fp->getStartTime())) { - //cerr << "done fp dt" << endl; - return; + if (! leadPointReached(curr)) { + controlHeading(curr); + controlSpeed(curr, next); } 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); - - //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)) { - lead_dist = fabs(2*speed); //don't skip over the waypoint - //cerr << "Extending lead distance to " << lead_dist << endl; - } -// FGAirport * apt = trafficRef->getDepartureAirport(); -// if ((dist_to_go > prev_dist_to_go) && trafficRef && apt) { -// if (apt->getId() == string("EHAM")) -// cerr << "Alert: " << trafficRef->getRegistration() << " is moving away from waypoint " << curr->name << endl -// << "Target heading : " << tgt_heading << "act heading " << hdg << " Tgt speed : " << tgt_speed << endl -// << "Lead distance : " << lead_dist << endl -// << "Distance to go: " << dist_to_go << endl; -// } - - prev_dist_to_go = dist_to_go; - //cerr << "2" << endl; - //if (no_roll) - // lead_dist = 10.0; - //cout << "Leg : " << (fp->getLeg()-1) << ". dist_to_go: " << dist_to_go << ", lead_dist: " - // << lead_dist << ", tgt_speed " << tgt_speed << ", tgt_heading " << tgt_heading - // << " speed " << speed << " hdg " << hdg << ". Altitude " << altitude << " TAget alt :" - // << tgt_altitude << endl; - - if ( dist_to_go < lead_dist ) { - //prev_dist_to_go = HUGE; - // For traffic manager generated aircraft: - // check if the aircraft flies of of user range. And adjust the - // Current waypoint's elevation according to Terrain Elevation - if (curr->finished) { //end of the flight plan + if (curr->finished) //end of the flight plan + { if (fp->getRepeat()) fp->restart(); else setDie(true); - - //cerr << "Done die end of fp" << endl; return; } - // we've reached the lead-point for the waypoint ahead - //cerr << "4" << endl; - //cerr << "Situation after lead point" << endl; - //cerr << "Prviious: " << prev->name << endl; - //cerr << "Current : " << curr->name << endl; - //cerr << "Next : " << next->name << endl; 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(); @@ -702,186 +273,77 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { next = fp->getNextWaypoint(); // Now that we have incremented the waypoints, excute some traffic manager specific code - // based on the name of the waypoint we just passed. 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) { + //TODO isn't this best executed right at the beginning? + if (! aiTrafficVisible()) { setDie(true); - //cerr << "done fp die out of range" << endl; return; } - FGAirport * dep = trafficRef->getDepartureAirport(); - FGAirport * arr = trafficRef->getArrivalAirport(); - // At parking the beginning of the airport - if (!( dep && arr)) { + if (! handleAirportEndPoints(prev, now)) { setDie(true); return; } - //if ((dep->getId() == string("EHAM") || (arr->getId() == string("EHAM")))) { - // cerr << trafficRef->getRegistration() - // << " Enroute from " << dep->getId() - // << " to " << arr->getId() - // << " just crossed " << prev->name - // << " Assigned rwy " << fp->getRunwayId() - // << " " << fp->getRunway() << endl; - // } - //if ((dep->getId() == string("EHAM")) && (prev->name == "park2")) { - // cerr << "Schiphol ground " - // << trafficRef->getCallSign(); - // if (trafficRef->getHeavy()) - // cerr << "Heavy"; - // cerr << ", is type " - // << trafficRef->getAircraft() - // << " ready to go. IFR to " - // << arr->getId() <name == "park2") - dep->getDynamics()->releaseParking(fp->getGate()); - - // Some debug messages, specific to testing the Logical networks. - // if ((arr->getId() == string("EHAM")) && (prev->name == "Center")) { - // cerr << "Schiphol ground " - // << trafficRef->getRegistration() << " " - // << trafficRef->getCallSign(); - // if (trafficRef->getHeavy()) - // cerr << "Heavy"; - // cerr << ", arriving from " << dep->getName() ; - // cerr << " landed runway " - // << fp->getRunway() - // << " request taxi to gate " - // << arr->getDynamics()->getParkingName(fp->getGate()) - // << endl; - // } - if (prev->name == "END") - fp->setTime(trafficRef->getDepartureTime()); - //cerr << "5" << endl; + announcePositionToController(); + } if (next) { - //cerr << "Current waypoint" << curr->name << endl; - //cerr << "Next waypoint" << next->name << endl; 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; 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; 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() { } bool FGAIAircraft::_getGearDown() const { - return ((props->getFloatValue("position/altitude-agl-ft") < 900.0) - && (props->getFloatValue("velocities/airspeed-kt") - < performance->land_speed*1.25)); + return _performance->gearExtensible(this); } 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); + int leg; if ((leg = fp->getLeg()) == 10) { trafficRef->next(); + setCallSign(trafficRef->getCallSign()); + //props->setStringValue("callsign", callsign.c_str()); 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)) { 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 { 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 + cruiseAlt, //(trafficRef->getCruiseAlt() * 100), // convert from FL to feet trafficRef->getSpeed(), _getLatitude(), _getLongitude(), @@ -890,48 +352,7 @@ void FGAIAircraft::loadNextLeg() { 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; } - //else - //{ - //delete entity.fp; - //entity.fp = new FGAIFlightPlan(&entity, - // 999, // A hack - // trafficRef->getDepartureTime(), - // trafficRef->getDepartureAirport(), - // trafficRef->getArrivalAirport(), - // false, - // acType, - // company); - //SetFlightPlan(entity.fp); } @@ -947,8 +368,8 @@ void FGAIAircraft::getGroundElev(double dt) { // to prevent all IA objects doing this in synchrony 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) { @@ -960,7 +381,7 @@ 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; } @@ -976,30 +397,590 @@ 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; + } +} + + +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; + } - //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; + 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::setCallSign(const string& s) { - callsign = s; +void FGAIAircraft::processATC(FGATCInstruction instruction) { + if (instruction.getCheckForCircularWait()) { + // This is not exactly an elegant solution, + // but at least it gives me a chance to check + // if circular waits are resolved. + // For now, just take the offending aircraft + // out of the scene + setDie(true); + // a more proper way should be - of course - to + // let an offending aircraft take an evasive action + // for instance taxi back a little bit. + } + //cerr << "Processing ATC instruction (not Implimented yet)" << endl; + if (instruction.getHoldPattern ()) {} + + // 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::setTACANChannelID(const string& id) { - TACAN_channel_id = id; +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; } -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); +/** + * 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); // should be "next departure" + } + + 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) > 0.01) { + 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; + if (next) { + fp->setLeadDistance(speed, tgt_heading, curr, next); + } + } +} + + +/** + * Update target values (heading, alt, speed) depending on flight plan or control properties + */ +void FGAIAircraft::updatePrimaryTargetValues() { + 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); + + // 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 (onGround()) { + Transform(); // make sure aip is initialized. + getGroundElev(dt); + doGroundAltitude(); + // Transform(); + pos.setElevationFt(altitude_ft); + } + if (trafficRef) { + //cerr << trafficRef->getRegistration() << " Setting altitude to " << altitude_ft; + if (! aiTrafficVisible()) { + setDie(true); + //cerr << trafficRef->getRegistration() << " is set to die " << endl; + throw AI_OutOfSight(); + } + } + if (! fp->isActive(now)) { + throw FP_Inactive(); + } + } 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" ) ); + } } +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 (onGround()) { + 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::updateBankAngleTarget() { + // 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 < _performance->maximumBankAngle()) { + tgt_roll = diff * bank_sense; + } else { + tgt_roll = _performance->maximumBankAngle() * bank_sense; + } + if ((fabs((double) spinCounter) > 1) && (diff > _performance->maximumBankAngle())) { + 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. + } + } +} + + +void FGAIAircraft::updateVerticalSpeedTarget() { + // adjust target Altitude, based on ground elevation when on ground + if (onGround()) { + getGroundElev(dt); + doGroundAltitude(); + } else if (alt_lock) { + // find target vertical speed + if (use_perf_vs) { + if (altitude_ft < tgt_altitude_ft) { + tgt_vs = tgt_altitude_ft - altitude_ft; + if (tgt_vs > _performance->climbRate()) + tgt_vs = _performance->climbRate(); + } else { + tgt_vs = tgt_altitude_ft - altitude_ft; + if (tgt_vs < (-_performance->descentRate())) + tgt_vs = -_performance->descentRate(); + } + } else { + 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; + } + } //else + // tgt_vs = 0.0; +} + +void FGAIAircraft::updatePitchAngleTarget() { + // if on ground and above vRotate -> initial rotation + if (onGround() && (speed > _performance->vRotate())) + tgt_pitch = 8.0; // some rough B737 value + + //TODO pitch angle on approach and landing + + // match pitch angle to vertical speed + else if (tgt_vs > 0) { + tgt_pitch = tgt_vs * 0.005; + } else { + tgt_pitch = tgt_vs * 0.002; + } +} + +void FGAIAircraft::handleATCRequests() { + //TODO implement NullController for having no ATC to save the conditionals + if (controller) { + controller->update(getID(), + pos.getLatitudeDeg(), + pos.getLongitudeDeg(), + hdg, + speed, + altitude_ft, dt); + processATC(controller->getInstruction(getID())); + } +} + +void FGAIAircraft::updateActualState() { + //update current state + //TODO have a single tgt_speed and check speed limit on ground on setting tgt_speed + updatePosition(); + + if (onGround()) + speed = _performance->actualSpeed(this, groundTargetSpeed, dt); + else + speed = _performance->actualSpeed(this, tgt_speed, dt); + + updateHeading(); + roll = _performance->actualBankAngle(this, tgt_roll, dt); + + // adjust altitude (meters) based on current vertical speed (fpm) + altitude_ft += vs / 60.0 * dt; + pos.setElevationFt(altitude_ft); + + vs = _performance->actualVerticalSpeed(this, tgt_vs, dt); + pitch = _performance->actualPitch(this, tgt_pitch, dt); +} + +void FGAIAircraft::updateSecondaryTargetValues() { + // derived target state values + updateBankAngleTarget(); + updateVerticalSpeedTarget(); + updatePitchAngleTarget(); + + //TODO calculate wind correction angle (tgt_yaw) +}