From 7783e6495336cb7ed83778381701c521c43a102f Mon Sep 17 00:00:00 2001 From: durk Date: Wed, 2 Apr 2008 19:01:48 +0000 Subject: [PATCH] Some tweaks and finetuning made possible by the multihreaded modelloader and the pushback code: - Traffic manager starts modelload requests immediately upon program loading - Only create legs 1 (push back) or five (cruise) of AI traffic. - AIAircraft's rather obsessive behavior to circle around a waypoint is largely resolved - More realistic ground steering for AI aircraft. --- src/AIModel/AIAircraft.cxx | 68 +++++++++++++++++++++++++++--------- src/AIModel/AIAircraft.hxx | 1 + src/AIModel/AIFlightPlan.cxx | 41 +++++++++++++--------- src/Traffic/TrafficMgr.cxx | 12 +------ 4 files changed, 78 insertions(+), 44 deletions(-) diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 2228f83d9..c73919d0a 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -1,4 +1,4 @@ -// // FGAIAircraft - FGAIBase-derived class creates an AI airplane +// // // FGAIAircraft - FGAIBase-derived class creates an AI airplane // // Written by David Culp, started October 2003. // @@ -290,7 +290,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { } if (next) { - fp->setLeadDistance(speed, tgt_heading, curr, next); + fp->setLeadDistance(tgt_speed, tgt_heading, curr, next); } if (!(prev->on_ground)) // only update the tgt altitude from flightplan if not on the ground @@ -522,6 +522,7 @@ void FGAIAircraft::processATC(FGATCInstruction instruction) { void FGAIAircraft::handleFirstWaypoint() { bool eraseWaypoints; //TODO YAGNI + headingError = 0; if (trafficRef) { eraseWaypoints = true; } else { @@ -609,7 +610,6 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* 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)) { @@ -619,6 +619,8 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) { } //prev_dist_to_go = dist_to_go; + //if (dist_to_go < lead_dist) + // cerr << trafficRef->getCallSign() << " Distance : " << dist_to_go << ": Lead distance " << lead_dist << " " << curr->name << endl; return dist_to_go < lead_dist; } @@ -819,30 +821,62 @@ void FGAIAircraft::updateHeading() { // If on ground, calculate heading change directly if (onGround()) { double headingDiff = fabs(hdg-tgt_heading); + 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 (headingDiff > 180) headingDiff = fabs(headingDiff - 360); - - groundTargetSpeed = tgt_speed - (tgt_speed * (headingDiff/45)); + double sum = hdg + headingDiff; + if (sum > 360.0) + sum -= 360.0; + if (fabs(sum - tgt_heading) > 0.0001) { + bank_sense = -1.0; + } else { + bank_sense = 1.0; + } + if (trafficRef) + //cerr << trafficRef->getCallSign() << " Heading " + // << hdg << ". Target " << tgt_heading << ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << endl; + //if (headingDiff > 60) { + groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS); + //groundTargetSpeed = tgt_speed - tgt_speed * (headingDiff/180); + //} else { + // groundTargetSpeed = tgt_speed; + //} 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; - + headingChangeRate += 10.0 * dt * sign(roll); + + // Clamp the maximum steering rate to 30 degrees per second, + // But only do this when the heading error is decreasing. + if ((headingDiff < headingError)) { + 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); + if (fabs(headingChangeRate) > headingDiff) + headingChangeRate = headingDiff*sign(roll); + else + headingChangeRate += dt * sign(roll); } - hdg += headingChangeRate * dt; + hdg += headingChangeRate * dt; + headingError = headingDiff; } else { if (fabs(speed) > 1.0) { turn_radius_ft = 0.088362 * speed * speed diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 2105a4a73..1c23c2576 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -95,6 +95,7 @@ private: double dt_count; double dt_elev_count; double headingChangeRate; + double headingError; double groundTargetSpeed; double groundOffset; double dt; diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 48af6afb8..f18f0a346 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -172,7 +172,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, cerr << "Errno = " << errno << endl; if (errno == ENOENT) { - cerr << "Reason: No such file or directory" << endl; + SG_LOG(SG_GENERAL, SG_WARN, "Reason: No such file or directory"); } } } @@ -184,6 +184,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, time_t now = time(NULL) + fgGetLong("/sim/time/warp"); time_t timeDiff = now-start; leg = 1; + /* if ((timeDiff > 300) && (timeDiff < 1200)) leg = 2; else if ((timeDiff >= 1200) && (timeDiff < 1500)) @@ -192,6 +193,9 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, leg = 4; else if (timeDiff >= 2000) leg = 5; + */ + if (timeDiff >= 2000) + leg = 5; SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg); wpt_iterator = waypoints.begin(); @@ -353,39 +357,44 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints ) // gives distance in feet from a position to a waypoint double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{ + double course, distance; // get size of a degree2 at the present latitude // this won't work over large distances - double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES); - double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES); - double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat; - double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon; - return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft)); + //double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES); + //double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES); + //double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat; + //double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon; + //return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft)); + SGWayPoint sgWp(wp->longitude,wp->latitude, wp->altitude, SGWayPoint::WGS84, string("temp")); + sgWp.CourseAndDistance(lon, lat, wp->altitude, &course, &distance); + return distance; } // sets distance in feet from a lead point to the current waypoint void FGAIFlightPlan::setLeadDistance(double speed, double bearing, waypoint* current, waypoint* next){ double turn_radius; - if (fabs(speed) > 1) + // Handle Ground steering + // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn + // So, to get an estimate of the turn radius, calculate the cicumference of the circle + // we travel on. Get the turn radius by dividing by PI (*2). + if (speed < 25) { + turn_radius = ((360/30)*15) / (2*M_PI); + } else turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank - else - turn_radius = 1.0; double inbound = bearing; double outbound = getBearing(current, next); leadInAngle = fabs(inbound - outbound); if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle; - if (leadInAngle < 1.0) // To prevent lead_dist from getting so small it is skipped - leadInAngle = 1.0; + //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped + // leadInAngle = 30.0; - lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); + //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); + lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2); // if ((errno == EDOM) || (errno == ERANGE) || lead_distance < 1.0) // { - // cerr << "Lead Distance = " << lead_distance - // << "Diff = " << diff - // << "Turn Radius = " << turn_radius - // << "Speed = " << speed << endl; // } } diff --git a/src/Traffic/TrafficMgr.cxx b/src/Traffic/TrafficMgr.cxx index 76608d2e1..82a97f2c3 100644 --- a/src/Traffic/TrafficMgr.cxx +++ b/src/Traffic/TrafficMgr.cxx @@ -167,16 +167,7 @@ void FGTrafficManager::init() void FGTrafficManager::update(double /*dt*/) { - //SG_LOG( SG_GENERAL, SG_INFO, "Running TrafficManager::Update() "); - // Hack alert: Skip running for the first frames 1000 after - // initialization to allow proper initialization of wheather stuff - // and runway assignments - if (runCount < 1000) - { - runCount++; - return; - } - //runCount = 0; + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); if (scheduledAircraft.size() == 0) { //SG_LOG( SG_GENERAL, SG_INFO, "Returned Running TrafficManager::Update() "); @@ -184,7 +175,6 @@ void FGTrafficManager::update(double /*dt*/) } if(currAircraft == scheduledAircraft.end()) { - //cerr << "resetting schedule " << endl; currAircraft = scheduledAircraft.begin(); } if (!((*currAircraft)->update(now))) -- 2.39.5