fly/flightgear
1
0
Fork 0
Code Activity

* Improved lead distance & handling of sharp turns

Browse source 
* Takeoff leg respects displaced threshold
This commit is contained in:
PortreeKid 2021-09-17 19:33:37 +02:00 committed by James Turner
parent d783008c08
commit cf4801e11c
7 changed files with 109 additions and 68 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

57
src/AIModel/AIAircraft.cxx
View file

@ -308,7 +308,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
///////////////////////////////////////////////////////////////////////////
// Initialize the flightplan
//////////////////////////////////////////////////////////////////////////
if (!prev) {
if (!prev||repositioned) {
handleFirstWaypoint();
return;
} // end of initialization
@ -534,10 +534,18 @@ bool FGAIAircraft::loadNextLeg(double distance) {
int leg;
if ((leg = fp->getLeg()) == 9) {
FGAirport *oldArr = trafficRef->getArrivalAirport();
if (!trafficRef->next()) {
//FIXME I'm on leg 9 and don't even reach parking.
return false;
}
FGAirport *dep = trafficRef->getDepartureAirport();
if (oldArr!=dep) {
// as though we are first leg
repositioned = true;
} else {
repositioned = false;
}
setCallSign(trafficRef->getCallSign());
leg = 0;
fp->setLeg(leg);
@ -865,7 +873,7 @@ bool FGAIAircraft::fpExecutable(time_t now) {
bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int nextTurnAngle) {
double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
// Leaddistance should be ft
double lead_dist = fp->getLeadDistance();
double lead_distance_m = fp->getLeadDistance() * SG_FEET_TO_METER;
const double arrivalDist = fabs(10.0*fp->getCurrentWaypoint()->getSpeed());
// arrive at pushback end
if ((dist_to_go_m < arrivalDist) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) {
@ -896,9 +904,11 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
}
}
if (lead_dist < fabs(2*speed)) {
if (lead_distance_m < fabs(2*speed) * SG_FEET_TO_METER) {
//don't skip over the waypoint
lead_dist = fabs(2*speed);
SG_LOG(SG_AI, SG_BULK, "Set lead_distance_m due to speed " << lead_distance_m << " to " << fabs(2*speed) * SG_FEET_TO_METER);
lead_distance_m = fabs(2*speed) * SG_FEET_TO_METER;
fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
}
double bearing = 0;
@ -908,14 +918,12 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
if (next) {
nextBearing = getBearing(fp->getBearing(pos, next));
}
double bearingDiff = fabs(bearing-nextBearing);
if (onGround() && nextTurnAngle > 30 && bearingDiff > 50) {
// Next turn is pretty sharp so we do a preturn
SG_LOG(SG_AI, SG_BULK, "Leadpoint reached due to excessive heading diff " << bearingDiff);
minBearing = 360;
speedFraction = 1.0;
prev_dist_to_go = HUGE_VAL;
return true;
if (onGround() && fabs(nextTurnAngle) > 50 ) {
//don't skip over the waypoint
const int multiplicator = 4;
SG_LOG(SG_AI, SG_BULK, "Set lead_distance_m due to next turn angle " << lead_distance_m << " to " << fabs(multiplicator*speed) * SG_FEET_TO_METER << " dist_to_go_m " << dist_to_go_m << " Next turn angle : " << fabs(nextTurnAngle) );
lead_distance_m = fabs(multiplicator*speed) * SG_FEET_TO_METER;
fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
}
if (bearing < minBearing) {
minBearing = bearing;
@ -929,9 +937,9 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
}
}
if ((dist_to_go_m < lead_dist) ||
if ((dist_to_go_m < lead_distance_m) ||
((dist_to_go_m > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) {
SG_LOG(SG_AI, SG_BULK, "Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing);
SG_LOG(SG_AI, SG_BULK, "Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing << " Next Turn Angle : " << fabs(nextTurnAngle));
minBearing = 360;
speedFraction = 1.0;
prev_dist_to_go = HUGE_VAL;
@ -1565,13 +1573,15 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) {
o << "WP Lat\t";
o << "WP Lon\t";
o << "Dist\t";
o << "Next Lat\t";
o << "Next Lon\t";
o << "Departuretime\t";
o << "Time\t";
o << "Startup diff\t";
o << "dist_to_go_m\t";
o << "Leaddistance\t";
o << "Leg\t";
o << "Num WP\t";
o << "Leaddistance\t";
o << "no_roll\t";
o << "roll\t";
o << "stuckCounter";
@ -1588,7 +1598,7 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) {
o << this->getCallSign() << "\t";
o << headingDiff << "\t";
o << headingChangeRate << "\t";
o << headingError << "\t";
o << headingError << "\t";
o << hdg << "\t";
o << tgt_heading << "\t";
o << tgt_speed << "\t";
@ -1602,11 +1612,20 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) {
o << this->getTrueHeadingDeg() << "\t";
FGAIFlightPlan* fp = this->GetFlightPlan();
FGAIWaypoint* currentWP = this->GetFlightPlan()->getCurrentWaypoint();
FGAIWaypoint* nextWP = this->GetFlightPlan()->getNextWaypoint();
if (currentWP) {
o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t";
o << currentWP->getName() << "\t";
o << this->GetFlightPlan()->getCurrentWaypoint()->getPos().getLatitudeDeg() << "\t";
o << this->GetFlightPlan()->getCurrentWaypoint()->getPos().getLongitudeDeg() << "\t";
o << currentWP->getPos().getLatitudeDeg() << "\t";
o << currentWP->getPos().getLongitudeDeg() << "\t";
}
if (nextWP) {
o << nextWP->getPos().getLatitudeDeg() << "\t";
o << nextWP->getPos().getLongitudeDeg() << "\t";
} else {
o << "\t\t";
}
if (currentWP) {
o << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t";
o << this->GetFlightPlan()->getStartTime() << "\t";
o << globals->get_time_params()->get_cur_time() << "\t";
@ -1617,9 +1636,9 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) {
o << "No WP\t\t\t\t\t\t\t\t";
}
if (fp->isValidPlan()) {
o << fp->getLeadDistance() * SG_FEET_TO_METER << "\t";
o << fp->getLeg() << "\t";
o << fp->getNrOfWayPoints() << "\t";
o << fp->getLeadDistance() << "\t";
} else {
o << "FP NotValid\t\t";
}

5
src/AIModel/AIAircraft.hxx
View file

@ -176,6 +176,11 @@ private:
/**Kills a flight when it's stuck */
const int AI_STUCK_LIMIT = 100;
int stuckCounter;
/**
* Signals a reset to leg 1 at a different airport.
* The leg loading happens at a different place than the parking loading.
* */
bool repositioned;
double prevSpeed;
double prev_dist_to_go;

63
src/AIModel/AIFlightPlan.cxx
View file

@ -105,7 +105,7 @@ FGAIFlightPlan::FGAIFlightPlan() :
sid(NULL),
repeat(false),
distance_to_go(0),
lead_distance(0),
lead_distance_ft(0),
leadInAngle(0),
start_time(0),
arrivalTime(0),
@ -120,7 +120,7 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) :
sid(NULL),
repeat(false),
distance_to_go(0),
lead_distance(0),
lead_distance_ft(0),
leadInAngle(0),
start_time(0),
arrivalTime(0),
@ -155,7 +155,7 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
sid(NULL),
repeat(false),
distance_to_go(0),
lead_distance(0),
lead_distance_ft(0),
leadInAngle(0),
start_time(start),
arrivalTime(0),
@ -360,24 +360,7 @@ FGAIWaypoint* FGAIFlightPlan::getNextWaypoint( void ) const
int FGAIFlightPlan::getNextTurnAngle( void ) const
{
if (wpt_iterator == waypoints.end())
return 0;
if (wpt_iterator+1 == waypoints.end())
return 0;
if (wpt_iterator+2 == waypoints.end())
return 0;
FGAIWaypoint* currentWP = *(wpt_iterator);
FGAIWaypoint* nextWP = *(wpt_iterator + 1);
FGAIWaypoint* afterNextWP = *(wpt_iterator + 2);
int currentBearing = this->getBearing(currentWP, nextWP);
int nextBearing = this->getBearing(nextWP, afterNextWP);
int turnAngle = nextBearing - currentBearing;
if (turnAngle>180) {
turnAngle -= 180;
}
return turnAngle;
return nextTurnAngle;
}
@ -402,6 +385,26 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
else {
wpt_iterator++;
}
// Calculate the angle of the next turn.
if (wpt_iterator == waypoints.end())
return;
if (wpt_iterator == waypoints.begin())
return;
if (wpt_iterator+1 == waypoints.end())
return;
FGAIWaypoint* previousWP = *(wpt_iterator -1);
FGAIWaypoint* currentWP = *(wpt_iterator);
FGAIWaypoint* nextWP = *(wpt_iterator + 1);
int currentBearing = this->getBearing(previousWP, currentWP);
int nextBearing = this->getBearing(currentWP, nextWP);
nextTurnAngle = SGMiscd::normalizePeriodic(-180, 180, nextBearing - currentBearing);
if (previousWP->getSpeed()>0&&nextWP->getSpeed()<0 ||
previousWP->getSpeed()<0&&nextWP->getSpeed()>0) {
nextTurnAngle += 180;
SG_LOG(SG_AI, SG_BULK, "Add 180 to turn angle pushback end");
}
SG_LOG(SG_AI, SG_BULK, "Calculated next turn angle " << nextTurnAngle << " " << previousWP->getName() << " " << currentWP->getName() << " Previous Speed " << previousWP->getSpeed() << " Next Speed " << nextWP->getSpeed());
}
void FGAIFlightPlan::DecrementWaypoint()
@ -438,7 +441,7 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
// we travel on. Get the turn radius by dividing by PI (*2).
// FIXME Why when going backwards? No fabs
if (speed < 0.5) {
lead_distance = 0.5;
lead_distance_ft = 0.5;
return;
}
@ -455,26 +458,26 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
//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 * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
if (lead_distance > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
//lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
lead_distance_ft = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
if (lead_distance_ft > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
}
/*
if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) {
if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) {
SG_LOG(SG_AI, SG_ALERT, "Warning: Lead-in distance is large. Inbound = " << inbound
<< ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle << ". Turn radius = " << turn_radius);
lead_distance = 3 * turn_radius;
lead_distance_ft = 3 * turn_radius;
return;
}
if ((leadInAngle > 90) && (current->on_ground == true)) {
lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
lead_distance_ft = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
return;
}*/
}
void FGAIFlightPlan::setLeadDistance(double distance_ft){
lead_distance = distance_ft;
lead_distance_ft = distance_ft;
}

18
src/AIModel/AIFlightPlan.hxx
View file

@ -175,7 +175,7 @@ public:
void setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next);
void setLeadDistance(double distance_ft);
double getLeadDistance( void ) const {return lead_distance;}
double getLeadDistance( void ) const {return lead_distance_ft;}
double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const;
double getBearing(const SGGeod& aPos, FGAIWaypoint* next) const;
@ -231,10 +231,10 @@ public:
void setGate(const ParkingAssignment& pka);
FGParking* getParkingGate();
FGAirportRef departureAirport() const;
FGAirportRef arrivalAirport() const;
FGAirportRef departureAirport() const;
FGAirportRef arrivalAirport() const;
bool empty() const;
bool empty() const;
private:
FGAIFlightPlan *sid;
@ -246,12 +246,14 @@ private:
wpt_vector_iterator wpt_iterator;
bool repeat;
double distance_to_go;
double lead_distance;
double leadInAngle;
double distance_to_go = 0;
//FIXME ft
double lead_distance_ft = 0;
double leadInAngle = 0;
double nextTurnAngle = 0;
time_t start_time;
time_t arrivalTime; // For AI/ATC purposes.
int leg;
int leg = 0;
ParkingAssignment gate;
FGTaxiNodeRef lastNodeVisited;
std::string activeRunway;

22
src/AIModel/AIFlightPlanCreate.cxx
View file

@ -69,11 +69,11 @@ bool FGAIFlightPlan::create(FGAIAircraft * ac, FGAirport * dep,
const string & airline, double distance)
{
if( legNr <= 3 )
SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " Airport : " << dep->getId());
SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " At Airport : " << dep->getId());
else if( legNr<= 6 )
SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " Departure Airport : " << dep->getId() << " Arrival Airport : " << arr->getId());
else
SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " Airport : " << arr->getId());
SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " At Airport : " << arr->getId());
bool retVal = true;
int currWpt = wpt_iterator - waypoints.begin();
@ -281,7 +281,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
SG_LOG(SG_AI, SG_BULK, "Taxi to " << apt->getId() << "/" << activeRunway);
assert( rwy != NULL );
SGGeod runwayTakeoff = rwy->pointOnCenterline(5.0);
SGGeod runwayTakeoff = rwy->pointOnCenterlineDisplaced(5.0);
FGGroundNetwork *gn = apt->groundNetwork();
if (!gn->exists()) {
@ -389,7 +389,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
pushBackWaypoint(wpt);
}
// Acceleration point, 105 meters into the runway,
SGGeod accelPoint = rwy->pointOnCenterline(105.0);
SGGeod accelPoint = rwy->pointOnCenterlineDisplaced(105.0);
FGAIWaypoint *wpt = createOnRunway(ac, "Accel", accelPoint, apt->getElevation(), ac->getPerformance()->vRotate());
wpt->setFlaps(0.5f);
pushBackWaypoint(wpt);
@ -468,8 +468,8 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
// int route;
for (int i = 0; i < size - 2; i++) {
taxiRoute.next(node, &route);
char buffer[16];
snprintf(buffer, 16, "landingtaxi-%d", node->getIndex());
char buffer[20];
snprintf(buffer, 20, "landingtaxi-%d-%d", node->getIndex(), i);
FGAIWaypoint *wpt =
createOnGround(ac, buffer, node->geod(), apt->getElevation(),
ac->getPerformance()->vTaxi());
@ -560,14 +560,14 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
// distance from the runway threshold to accelerate to rotation speed.
double d = accelDistance(vTaxiMetric, vRotateMetric, accelMetric) + ACCEL_POINT;
SGGeod rotatePoint = rwy->pointOnCenterline(d);
SGGeod rotatePoint = rwy->pointOnCenterlineDisplaced(d);
wpt = createOnRunway(ac, "rotate", rotatePoint, airportElev, vRotate);
wpt->setFlaps(0.5f);
pushBackWaypoint(wpt);
// After rotation, we still need to accelerate to the take-off speed.
double t = d + accelDistance(vRotateMetric, vTakeoffMetric, accelMetric);
SGGeod takeoffPoint = rwy->pointOnCenterline(t);
SGGeod takeoffPoint = rwy->pointOnCenterlineDisplaced(t);
wpt = createOnRunway(ac, "takeoff", takeoffPoint, airportElev, vTakeoff);
wpt->setGear_down(true);
wpt->setFlaps(0.5f);
@ -580,7 +580,7 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
// With closely spaced waypoints on climb-out this can occur almost immediately,
// so we put the waypoint further away.
double gearUpDist = t + 2*vRef*SG_FEET_TO_METER + pitchDistance(INITIAL_PITCH_ANGLE, 400 * SG_FEET_TO_METER);
SGGeod gearUpPoint = rwy->pointOnCenterline(gearUpDist);
SGGeod gearUpPoint = rwy->pointOnCenterlineDisplaced(gearUpDist);
wpt = createInAir(ac, "gear-up", gearUpPoint, airportElev + 400, vRef);
wpt->setFlaps(0.5f);
pushBackWaypoint(wpt);
@ -592,12 +592,12 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight,
// be a (sometimes large) turn towards the destination, and we don't want to
// commence that turn below 2000'
double climbOut = t + 2*vClimbBelow10000*SG_FEET_TO_METER + pitchDistance(INITIAL_PITCH_ANGLE, 2000 * SG_FEET_TO_METER);
SGGeod climbOutPoint = rwy->pointOnCenterline(climbOut);
SGGeod climbOutPoint = rwy->pointOnCenterlineDisplaced(climbOut);
wpt = createInAir(ac, "2000'", climbOutPoint, airportElev + 2000, vClimbBelow10000);
pushBackWaypoint(wpt);
climbOut = t + 2*vClimbBelow10000*SG_FEET_TO_METER + pitchDistance(INITIAL_PITCH_ANGLE, 2500 * SG_FEET_TO_METER);
SGGeod climbOutPoint2 = rwy->pointOnCenterline(climbOut);
SGGeod climbOutPoint2 = rwy->pointOnCenterlineDisplaced(climbOut);
wpt = createInAir(ac, "2500'", climbOutPoint2, airportElev + 2500, vClimbBelow10000);
pushBackWaypoint(wpt);

5
src/Airports/runways.cxx
View file

@ -124,6 +124,11 @@ SGGeod FGRunway::threshold() const
return pointOnCenterline(_displ_thresh);
}
SGGeod FGRunway::pointOnCenterlineDisplaced(double aOffset) const
{
return pointOnCenterline(_displ_thresh+aOffset);
}
void FGRunway::setReciprocalRunway(PositionedID other)
{
assert(_reciprocal == 0);

7
src/Airports/runways.hxx
View file

@ -73,6 +73,13 @@ public:
*/
SGGeod threshold() const;
/**
* Retrieve a position on the extended centerline. Positive values
* are in the direction of the runway heading, negative values are in the
* opposited direction. 0.0 corresponds to the possibly threshold
*/
SGGeod pointOnCenterlineDisplaced(double aOffset) const;
/**
* Get the 'far' end - this is equivalent to calling
* pointOnCenterline(lengthFt());