TFDi_MD-11_QA/NavDataExplorer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
NavDataExplorer/FlightPlan.Resolution.cpp
Kilian Hofmann bdc8bb1567 Initial
2025-07-11 21:48:37 +02:00

351 lines
13 KiB
C++
Raw Blame History

#include "FMC.h"
#include <Helper.h>
#include <GeoFormulas/GeoFormulas.h>
#include <AerospaceFormulas.h>
using namespace std;
double slantRangeToGroundDist(double slantRangeNM, double verticalDistFt)
{
return FeetToNm(sqrt(pow(NmToFeet(slantRangeNM), 2) - pow(verticalDistFt, 2)));
}
void FMC::FlightPlan::CalculateLegs()
{
double dTol = 0.0;
double weight = fmc->gw != DataEntryType::None ? (double)fmc->gw : 280000.0;
for (size_t i = 0; i < legs.size(); i++)
{
shared_ptr<NavDataObj> leg = legs[i];
if (leg->usage == ObjectUsage::Origin)
{
double dist = 0;
if (arrApt.GetID() != -1)
dist = depApt.latlon.Dist(arrApt.latlon);
leg->vertSegments.emplace_back(VerticalSegment(0.0, fmc->GetPredictedSpeedTarget(depApt.elevation, weight, FLIGHT_PHASE::TAKEOFF, dist, &fmc->actFpln, true), depApt.elevation, SPD_INVALID, ObjectUsagePhase::Climb));
}
else if (leg->usage == ObjectUsage::Destination)
{
leg->vertSegments.emplace_back(VerticalSegment(0.0, fmc->GetPredictedSpeedTarget(arrApt.elevation, weight, FLIGHT_PHASE::APPROACH, 0, &fmc->actFpln, true), arrApt.elevation, SPD_INVALID, ObjectUsagePhase::Descent));
}
//NavLat/Lon -> destLatLon
//NavDist->distToDest
//WptLat/Lon -> refLatLon
//distance->distance
if (leg->GetType() == NavDataObjType::TerminalLeg)
{
shared_ptr<TerminalLeg> tl = dynamic_pointer_cast<TerminalLeg>(leg);
switch (tl->track_code)
{
case TrackCode::TrackFromFixToDistance:
tl->latlon = tl->refLatLon.Dest(tl->distance, modAngle(tl->course - tl->refLatLon.GetMagVar())); //this DEFINITELY works for CourseFromFixToDMEDistance
break;
case TrackCode::CourseFromFixToDMEDistance:
{
//check this, terminal ID 11546 on RW35L transition contains one
double alt = tl->GetPredictedAltitude();
if (alt == ALT_INVALID)
alt = tl->altRestrLower;
if (alt == ALT_INVALID)
alt = tl->altRestr;
double wptHeight = tl->nvd.GetID() != -1 ? tl->nvd.elevation : 0;
double vertDist = fabs(alt - wptHeight);
double groundDist = slantRangeToGroundDist(tl->distance, vertDist);
groundDist = CLAMP(groundDist, (tl->distance * 0.5), (tl->distance * 1.5));
tl->latlon = tl->refLatLon.Dest(groundDist, modAngle(tl->course - tl->refLatLon.GetMagVar())); //this DEFINITELY works for CourseFromFixToDMEDistance
tl->isFlyover = true;
break;
}
case TrackCode::ArcToFix:
//check this - segment generator must create the arc based on NavDist?
tl->latlon = tl->refLatLon;
break;
case TrackCode::RadiusToFix:
tl->latlon = tl->refLatLon;
break;
case TrackCode::CourseFromFixToAltitude:
case TrackCode::CourseToAltitude:
case TrackCode::HeadingToAltitude:
{
double lastAlt = 0;
LatitudeLongitude startLatLon;
double lastSpeed = 150;
for (int lastLeg = this->GetPreviousFlyableLeg((int)i, legs); lastLeg != -1; lastLeg = this->GetPreviousFlyableLeg((int)lastLeg, legs))
{
startLatLon = FMC::GetBestLatLonForLeg(this, legs[lastLeg], FMC::perf.get(), true); //this is because the segment will indefinitely extend otherwise
lastAlt = GetBestAltitudeForLeg(legs[lastLeg]);
lastSpeed = fmc->GetPredictedSpeedTarget(lastAlt, weight, legs[lastLeg]->GetUsagePhase() == ObjectUsagePhase::Climb || legs[lastLeg]->GetUsagePhase() == ObjectUsagePhase::MissedApproach ? FLIGHT_PHASE::CLIMB : FLIGHT_PHASE::CRUISE, 0, this, true);
break;
}
double angle = modAngle(tl->course + startLatLon.GetMagVar());
double minDist = 0;
if (tl->track_code == TrackCode::CourseFromFixToAltitude) //this breaks at airports
{
if (fabs(diffAngle(angle, tl->refLatLon.BearingTo(startLatLon))) < 10)
minDist = tl->refLatLon.Dist(startLatLon);
startLatLon = tl->refLatLon;
}
//the heading one should _technically_ account for wind drift here
double diffAlt = tl->altRestr - lastAlt;
if (tl->altRestrictionType == AltitudeRestrictionType::Above)
diffAlt = max(diffAlt, 1);
else if (tl->altRestrictionType == AltitudeRestrictionType::Below)
diffAlt = min(diffAlt, -1);
double fpm = FMC::perf->GetClimbRate(lastAlt, lastSpeed, weight);
{
double fpmMax = 0;
double groundHeight = 0;
if(tl->usage == ObjectUsage::Origin || tl->usage == ObjectUsage::DepartureLeg)
groundHeight = depApt.GetID() != -1 ? depApt.elevation : 0;
else if (tl->usage == ObjectUsage::Destination || tl->usage == ObjectUsage::ArrivalLeg || tl->usage == ObjectUsage::ApproachLeg)
groundHeight = arrApt.GetID() != -1 ? arrApt.elevation : 0;
double altAgl = tl->altRestr - groundHeight;
if (altAgl < 150)
fpmMax = 400;
else if (altAgl < 300)
fpmMax = 800;
else if (altAgl < 1000)
fpmMax = 1000;
else if (altAgl < 1500)
fpmMax = 1500;
if (fpmMax > 0)
fpm = min(fpm, fpmMax);
}
double minutes = fabs(diffAlt / fpm);
double dist = ((minutes / 60.0) * lastSpeed) + minDist;
tl->latlon = startLatLon.Dest(dist, angle);
tl->isFlyover = true; //is this right? aren't all of these automatically overfly?
break;
}
case TrackCode::HeadingToDMEDistance:
case TrackCode::CourseToDMEDistance:
{
if (i > 0) //needs to account for DISCOs, etc.
{
//DME distance is usually slant range - account for altitude in this?
LLPoint intcpt1;
LLPoint intcpt2;
std::shared_ptr<NavDataObj> ll = legs[i - 1];
LatitudeLongitude startPoint = ll->GetLatitudeLongitude();
double desCourse = tl->course;
double alt = tl->GetPredictedAltitude();
if (alt == ALT_INVALID)
alt = tl->altRestrLower;
if (alt == ALT_INVALID)
alt = tl->altRestr;
double wptHeight = tl->nvd.GetID() != -1 ? tl->nvd.elevation : 0;
double vertDist = alt != ALT_INVALID ? fabs(alt - wptHeight) : 0;
double groundDist = slantRangeToGroundDist(tl->distance, vertDist);
groundDist = CLAMP(groundDist, (tl->distance * 0.5), (tl->distance * 1.5));
//the heading one should _technically_ account for wind drift here
if (ll->GetLatitudeLongitude().Dist(tl->destLatlon) <= groundDist)
{
startPoint = ll->GetLatitudeLongitude().Dest(groundDist * 3.0, modAngle(tl->course + ll->GetLatitudeLongitude().GetMagVar()));
desCourse = modAngle(desCourse - 180.0);
}
if (GeodesicArcIntercept(startPoint, DEGTORAD(modAngle(desCourse + startPoint.GetMagVar())), tl->destLatlon, NmToMeters(groundDist), intcpt1, intcpt2, dTol) > 0)
{
bool intcpt1valid = fabs(diffAngle(startPoint.BearingTo(intcpt1), desCourse)) < 90.0;
bool intcpt2valid = fabs(diffAngle(startPoint.BearingTo(intcpt2), desCourse)) < 90.0;
if (intcpt1valid == true && intcpt2valid == false)
tl->latlon = intcpt1;
else if (intcpt1valid == false && intcpt2valid == true)
tl->latlon = intcpt2;
else
{
if (startPoint.Dist(intcpt1) < startPoint.Dist(intcpt2))
tl->latlon = intcpt1;
else
tl->latlon = intcpt2;
}
}
else
tl->latlon = tl->destLatlon;
}
else
tl->latlon = tl->destLatlon;
tl->isFlyover = true;
break;
}
case TrackCode::HoldAtFix:
case TrackCode::HoldUntilManualTermination:
case TrackCode::HoldToAltitude:
{
if (tl->hold.type == HoldType::Database)
{
tl->hold.holdDir = tl->turn_dir == ObjectTurnDirection::Left ? TurnDirection::Left : TurnDirection::Right;
tl->hold.holdDist = tl->distance != -1.0 ? tl->distance : tl->distToDest; //is this right?
tl->hold.holdDistManual = tl->hold.holdDist != -1;
tl->hold.holdInboundCourse = tl->course;
tl->hold.defaultCourse = tl->course;
tl->hold.wptIdent = i > 0 ? legs[i-1]->GetIdentifier() : L"";
}
break;
}
case TrackCode::HeadingUntilManualTermination:
{
LatitudeLongitude lastlatLon;
for (int lastLeg = this->GetPreviousFlyableLeg((int)i, legs); lastLeg != -1; lastLeg = this->GetPreviousFlyableLeg((int)lastLeg, legs))
{
lastlatLon = legs[lastLeg]->GetLatitudeLongitude();
break;
}
tl->latlon = lastlatLon.Dest(1000, modAngle(tl->course + lastlatLon.GetMagVar()));
break;
}
case TrackCode::CourseToNextIntercept: //is this right?
case TrackCode::HeadingToNextLegIntercept:
{
//TESTED WITH HEADING
bool set = false;
if (i + 1 < legs.size() && i > 0)
{
if (legs[i + 1]->GetType() == NavDataObjType::TerminalLeg)
{
std::shared_ptr<TerminalLeg> ntl = std::dynamic_pointer_cast<TerminalLeg>(legs[i + 1]);
double trueTlCourse = modAngle(tl->course + ntl->GetLatitudeLongitude().GetMagVar()); //using ntl MagVar as tl->latlon isn't set yet
LatitudeLongitude last = legs[i - 1]->GetLatitudeLongitude(); //maybe check this for special cases?
if (legs[i - 1]->isFlyover == true && legs[i - 1]->bearingFromTo != -1)
{
double diffCourse = diffAngleByDir(trueTlCourse, legs[i - 1]->bearingFromTo, (int)tl->turn_dir);
diffCourse = CLAMP(diffCourse, -180, 180);
double offset = fmc->GetTurnRadiusNM(legs[i]) * (fabs(diffCourse) / 90.0);
last = last.Dest(offset, modAngle(legs[i - 1]->bearingFromTo + CLAMP(diffCourse, -90, 90)));
}
if (ntl->track_code == TrackCode::ArcToFix)
{
LLPoint tan1, tan2;
if (PointToArcTangents(last, ntl->nvd.GetLatitudeLongitude(), NmToMeters(ntl->refLatLon.Dist(ntl->nvd.GetLatitudeLongitude())), tan1, tan2, dTol) != 0)
{
if(ntl->turn_dir == ObjectTurnDirection::Left) //this may not be the correct check
tl->latlon = tan2;
else
tl->latlon = tan1;
set = true;
}
}
else if(ntl->GetLocationType() != LocationType::Abstract && ntl->course != -1 && ntl->GetLatitudeLongitude() != LatitudeLongitude())
{
LLPoint intcpt;
double trueNtlCourse = modAngle(ntl->course + ntl->GetLatitudeLongitude().GetMagVar());
if (CrsIntersect(last, DEGTORAD(trueTlCourse), ntl->GetLatitudeLongitude(), DEGTORAD(modAngle(trueNtlCourse + 180.0)), dTol, intcpt))
tl->latlon = intcpt;
else
tl->latlon = last; //this will cause a bypass because it's the exact same location
set = true;
}
}
}
if (set == false) //sanity check to ensure it's never in the middle of the ocean
{
int lastFlyable = this->GetPreviousFlyableLeg((int)i, legs);
if (lastFlyable != -1)
tl->latlon = legs[lastFlyable]->GetLatitudeLongitude();
else
{
int nextFlyable = this->GetNextFlyableLeg((int)i, legs);
if (nextFlyable != -1)
tl->latlon = legs[nextFlyable]->GetLatitudeLongitude();
}
}
break;
}
case TrackCode::HeadingToRadialTermination:
case TrackCode::CourseToRadialTermination: //Tested and working on CourseToRadialTermination
{
//this leg does NOT do the outbound course from the previous leg, it's the outbound course from the point the plane is able to achieve tl->course
if (i + 1 < legs.size() && i > 1)
{
//needs to account for DISCOs, T/C, T/D, etc.
std::shared_ptr<NavDataObj> ll = legs[i - 1];
LatitudeLongitude startPoint = ll->GetLatitudeLongitude();
double tlCrs = modAngle(tl->course - tl->destLatlon.GetMagVar());
if (i > 2 && ll->isFlyover == true)
{
double lastBrg = legs[i - 2]->bearingFromTo;
TurnDirection dir = diffAngle(tlCrs, lastBrg) < 0 ? TurnDirection::Left : TurnDirection::Right;
double turnRad = fmc->GetTurnRadiusNM(ll);
double angleIn = modAngle(lastBrg + (dir == TurnDirection::Left ? -90 : 90));
double angleOut = modAngle(180.0 + (angleIn + diffAngle(tlCrs, lastBrg)));
LatitudeLongitude turnCenter = ll->GetLatitudeLongitude().Dest(turnRad, angleIn);
startPoint = turnCenter.Dest(turnRad, angleOut);
}
LLPoint intcpt;
if (CrsIntersect(startPoint, DEGTORAD(modAngle(tlCrs)), tl->destLatlon, DEGTORAD(tl->bearToDest), dTol, intcpt)
//&& fabs(diffAngle(last.BearingTo(intcpt), trueTlCourse)) < 90
//&& fabs(diffAngle(LatitudeLongitude(intcpt).BearingTo(ntl->GetLatitudeLongitude()), trueNtlCourse)) < 90
)
{
tl->latlon = intcpt;
}
}
break;
}
case TrackCode::ProcedureTurnToIntercept: //Test with KLBX ILS 07
if (i > 0 && i + 1 < legs.size() && legs[i -1]->GetLocationType() == LocationType::Absolute && legs[i + 1]->GetLocationType() == LocationType::Absolute)
{
double crsBetween = legs[i - 1]->GetLatitudeLongitude().BearingTo(legs[i + 1]->GetLatitudeLongitude());
tl->latlon = legs[i - 1]->GetLatitudeLongitude().Dest(tl->distance, crsBetween);
}
else
tl->latlon = tl->destLatlon; //just set to there is something
break;
case TrackCode::CourseFromFixToManualTermination:
tl->latlon = tl->wpt.GetLatitudeLongitude().Dest(1000, modAngle(tl->course + tl->wpt.GetLatitudeLongitude().GetMagVar()));
break;
case TrackCode::TrackToFix:
case TrackCode::CourseToFix:
case TrackCode::DirectToFix:
/*These are pretty standard, segment calculator processes them correctly already based on desired course*/
break;
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink