364 lines
14 KiB
C++
364 lines
14 KiB
C++
/*
|
||
* Author:
|
||
* Sven Czarnian <devel@svcz.de>
|
||
* Brief:
|
||
* Implements the inbound
|
||
* Copyright:
|
||
* 2021 Sven Czarnian
|
||
* License:
|
||
* GNU General Public License v3 (GPLv3)
|
||
*/
|
||
|
||
#include <Windows.h>
|
||
|
||
#include <gsl/gsl>
|
||
|
||
#include <aman/helper/String.h>
|
||
#include <aman/types/Inbound.h>
|
||
|
||
using namespace aman;
|
||
|
||
static __inline GeoCoordinate __convert(const EuroScopePlugIn::CPosition& position) {
|
||
return GeoCoordinate(static_cast<float>(position.m_Longitude) * degree, static_cast<float>(position.m_Latitude) * degree);
|
||
}
|
||
|
||
Inbound::Inbound(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound,
|
||
const google::protobuf::RepeatedPtrField<aman::WindData>& wind) :
|
||
m_windLevels(),
|
||
m_windDirections(),
|
||
m_windSpeeds(),
|
||
m_performanceData(),
|
||
m_fixedPlan(inbound.fixed()),
|
||
m_star(inbound.arrivalroute()),
|
||
m_runway(inbound.arrivalrunway()),
|
||
m_nextStarWaypoint(),
|
||
m_arrivalRoute(),
|
||
m_timeToLose() {
|
||
this->createWindTables(wind);
|
||
this->updatePrediction(target, inbound, true);
|
||
auto flightplan = target.GetCorrelatedFlightPlan();
|
||
this->update(flightplan);
|
||
}
|
||
|
||
void Inbound::createWindTables(const google::protobuf::RepeatedPtrField<aman::WindData>& wind) {
|
||
this->m_windLevels.clear();
|
||
this->m_windDirections.clear();
|
||
this->m_windSpeeds.clear();
|
||
|
||
this->m_windLevels.reserve(static_cast<std::size_t>(wind.size()));
|
||
this->m_windDirections.reserve(static_cast<std::size_t>(wind.size()));
|
||
this->m_windSpeeds.reserve(static_cast<std::size_t>(wind.size()));
|
||
|
||
for (int i = 0; i < wind.size(); ++i) {
|
||
const auto& level = wind.Get(i);
|
||
|
||
this->m_windLevels.push_back(static_cast<float>(level.altitude()));
|
||
this->m_windDirections.push_back(static_cast<float>(level.direction()));
|
||
this->m_windSpeeds.push_back(static_cast<float>(level.speed()));
|
||
}
|
||
}
|
||
|
||
void Inbound::updatePrediction(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound, bool forceUpdate) {
|
||
bool updatedFlightplan = false;
|
||
|
||
this->m_performanceData.speedAboveFL240 = static_cast<float>(inbound.performance().iasabovefl240()) * knot;
|
||
this->m_performanceData.speedAboveFL100 = static_cast<float>(inbound.performance().iasabovefl100()) * knot;
|
||
this->m_performanceData.speedBelowFL100 = static_cast<float>(inbound.performance().iasbelowfl100()) * knot;
|
||
this->m_performanceData.speedApproach = static_cast<float>(inbound.performance().iasapproach()) * knot;
|
||
|
||
if (true == forceUpdate || this->m_star != target.GetCorrelatedFlightPlan().GetFlightPlanData().GetStarName() || this->m_runway != target.GetCorrelatedFlightPlan().GetFlightPlanData().GetArrivalRwy()) {
|
||
std::string route(target.GetCorrelatedFlightPlan().GetFlightPlanData().GetRoute());
|
||
std::string arrival = this->m_star + "/" + this->m_runway;
|
||
|
||
auto split = String::splitString(route, " ");
|
||
std::string newRoute;
|
||
|
||
/* create the new route */
|
||
if (1 < split.size()) {
|
||
for (std::size_t i = 0; i < split.size() - 1; ++i)
|
||
newRoute += gsl::at(split, i) + " ";
|
||
}
|
||
/* check if the last entry is the arrival route */
|
||
const auto& lastEntry = gsl::at(split, split.size() - 1);
|
||
if (lastEntry.cend() == std::find_if(lastEntry.cbegin(), lastEntry.cend(), ::isdigit))
|
||
newRoute += gsl::at(split, split.size() - 1) + " ";
|
||
/* add the arrival route */
|
||
newRoute += arrival;
|
||
|
||
/* write into the flight plan */
|
||
target.GetCorrelatedFlightPlan().GetFlightPlanData().SetRoute(newRoute.c_str());
|
||
target.GetCorrelatedFlightPlan().GetFlightPlanData().AmendFlightPlan();
|
||
|
||
updatedFlightplan = true;
|
||
}
|
||
|
||
if (true == updatedFlightplan) {
|
||
this->m_arrivalRoute.clear();
|
||
this->m_arrivalRoute.reserve(inbound.waypoints_size());
|
||
auto route = target.GetCorrelatedFlightPlan().GetExtractedRoute();
|
||
int lastExtractedIndex = 0;
|
||
|
||
for (int i = 0; i < inbound.waypoints_size(); ++i) {
|
||
const auto& plannedPoint = inbound.waypoints(i);
|
||
|
||
const auto pta = UtcTime::stringToTime(plannedPoint.pta());
|
||
const auto altitude = static_cast<float>(plannedPoint.altitude()) * feet;
|
||
const auto ias = static_cast<float>(plannedPoint.indicatedairspeed()) * knot;
|
||
GeoCoordinate coordinate;
|
||
|
||
bool found = false;
|
||
for (int c = lastExtractedIndex; c < route.GetPointsNumber(); ++c) {
|
||
if (route.GetPointName(c) == plannedPoint.name()) {
|
||
coordinate = __convert(route.GetPointPosition(c));
|
||
lastExtractedIndex = c;
|
||
found = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (true == found)
|
||
this->m_arrivalRoute.push_back(ArrivalWaypoint(plannedPoint.name(), coordinate, altitude, ias, pta));
|
||
}
|
||
}
|
||
}
|
||
|
||
void Inbound::update(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound,
|
||
const google::protobuf::RepeatedPtrField<aman::WindData>& wind) {
|
||
this->m_fixedPlan = inbound.fixed();
|
||
this->m_star = inbound.arrivalroute();
|
||
this->m_runway = inbound.arrivalrunway();
|
||
this->createWindTables(wind);
|
||
|
||
this->updatePrediction(target, inbound, false);
|
||
auto flightplan = target.GetCorrelatedFlightPlan();
|
||
this->update(flightplan);
|
||
}
|
||
|
||
Velocity Inbound::indicatedAirspeed(const Length& altitude) const noexcept {
|
||
if (24000_ft <= altitude)
|
||
return this->m_performanceData.speedAboveFL240;
|
||
else if (10000_ft <= altitude)
|
||
return this->m_performanceData.speedAboveFL100;
|
||
else if (1000_ft < altitude)
|
||
return this->m_performanceData.speedBelowFL100;
|
||
else
|
||
return this->m_performanceData.speedApproach;
|
||
}
|
||
|
||
template <typename T, typename U>
|
||
static __inline U __interpolate(const std::vector<T>& xAxis, const std::vector<U>& yAxis, const T& xValue, bool limit) {
|
||
bool inverse = gsl::at(xAxis, 0) > gsl::at(xAxis, xAxis.size() - 1);
|
||
|
||
/* define the search value */
|
||
T value = xValue;
|
||
if (true == limit) {
|
||
if (true == inverse) {
|
||
if (value > gsl::at(xAxis, 0))
|
||
value = gsl::at(xAxis, 0);
|
||
else if (value < gsl::at(xAxis, xAxis.size() - 1))
|
||
value = gsl::at(xAxis, xAxis.size() - 1);
|
||
}
|
||
else {
|
||
if (value < gsl::at(xAxis, 0))
|
||
value = gsl::at(xAxis, 0);
|
||
else if (value > gsl::at(xAxis, xAxis.size() - 1))
|
||
value = gsl::at(xAxis, xAxis.size() - 1);
|
||
}
|
||
}
|
||
|
||
/* search the correct value */
|
||
for (std::size_t i = 0; i < xAxis.size() - 1; ++i) {
|
||
const auto& prevX = gsl::at(xAxis, i);
|
||
const auto& nextX = gsl::at(xAxis, i + 1);
|
||
bool inside;
|
||
|
||
if (true == inverse)
|
||
inside = prevX >= xValue && nextX <= xValue;
|
||
else
|
||
inside = prevX <= xValue && nextX >= xValue;
|
||
|
||
if (true == inside) {
|
||
auto ratio = (xValue - prevX) / (nextX - prevX);
|
||
const auto& prevY = gsl::at(yAxis, i);
|
||
const auto& nextY = gsl::at(yAxis, i + 1);
|
||
return prevY + ratio * (nextY - prevY);
|
||
}
|
||
}
|
||
|
||
return U();
|
||
}
|
||
|
||
Velocity Inbound::groundSpeed(const Length& altitude, const Velocity& ias, const Angle& heading) {
|
||
static std::vector <float> levels = {
|
||
50000.0f, 45000.0f, 40000.0f, 38000.0f, 36000.0f, 34000.0f, 32000.0f, 30000.0f, 28000.0f,
|
||
26000.0f, 24000.0f, 22000.0f, 20000.0f, 18000.0f, 16000.0f, 15000.0f, 14000.0f, 13000.0f,
|
||
12000.0f, 11000.0f, 10000.0f, 9000.0f, 8000.0f, 7000.0f, 6000.0f, 5000.0f, 4000.0f,
|
||
3000.0f, 2000.0f, 1000.0f, 0.0f
|
||
};
|
||
static std::vector<float> densities = {
|
||
0.18648f, 0.23714f, 0.24617f, 0.33199f, 0.36518f, 0.39444f, 0.42546f, 0.45831f, 0.402506f, 0.432497f, 0.464169f,
|
||
0.60954f, 0.65269f, 0.69815f, 0.74598f, 0.77082f, 0.79628f, 0.82238f, 0.84914f, 0.87655f, 0.90464f, 0.93341f,
|
||
0.96287f, 0.99304f, 1.02393f, 1.05555f, 1.08791f, 1.12102f, 1.1549f, 1.18955f, 1.225f
|
||
};
|
||
|
||
const auto density = __interpolate(levels, densities, altitude.convert(feet), true);
|
||
const auto tas = ias * std::sqrtf(1.225f / density);
|
||
Angle windDirection(0.0_deg);
|
||
Velocity windSpeed(0.0_mps);
|
||
|
||
if (0 != this->m_windLevels.size()) {
|
||
windDirection = __interpolate(this->m_windLevels, this->m_windDirections, altitude.convert(feet), true) * degree;
|
||
windSpeed = __interpolate(this->m_windLevels, this->m_windSpeeds, altitude.convert(feet), true) * knot;
|
||
}
|
||
|
||
return tas + windSpeed * std::cosf(windDirection.convert(radian) - heading.convert(radian));
|
||
}
|
||
|
||
static __inline Angle __normalize(const Angle& angle) {
|
||
auto retval(angle);
|
||
|
||
while (-1.0f * 180_deg > retval)
|
||
retval += 360_deg;
|
||
while (180_deg < retval)
|
||
retval -= 360_deg;
|
||
|
||
return retval;
|
||
}
|
||
|
||
int Inbound::findIndexInPredictedPath(const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions, const GeoCoordinate& position) {
|
||
if (0 == predictions.GetPointsNumber())
|
||
return 0;
|
||
|
||
GeoCoordinate lastPosition(__convert(predictions.GetPosition(0)));
|
||
|
||
for (int i = 1; i < predictions.GetPointsNumber(); ++i) {
|
||
GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));
|
||
|
||
const auto prev = lastPosition.bearingTo(position);
|
||
const auto next = coordinate.bearingTo(position);
|
||
const auto delta = __normalize(prev - next);
|
||
if (100_deg <= delta.abs())
|
||
return i;
|
||
|
||
lastPosition = coordinate;
|
||
}
|
||
|
||
return predictions.GetPointsNumber();
|
||
}
|
||
|
||
void Inbound::update(EuroScopePlugIn::CRadarTarget& target) {
|
||
if (this->m_arrivalRoute.size() <= this->m_nextStarWaypoint) {
|
||
this->m_timeToLose = 0_s;
|
||
return;
|
||
}
|
||
|
||
const auto& destination = gsl::at(this->m_arrivalRoute, this->m_nextStarWaypoint);
|
||
const auto& predictions = target.GetCorrelatedFlightPlan().GetPositionPredictions();
|
||
GeoCoordinate lastPosition(__convert(target.GetPosition().GetPosition()));
|
||
Length distanceToNextWaypoint = 0_m;
|
||
|
||
/* calculate the distance to the correct waypoint */
|
||
for (int i = 0; i < predictions.GetPointsNumber(); ++i) {
|
||
GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));
|
||
|
||
const auto prev = lastPosition.bearingTo(destination.position());
|
||
const auto next = coordinate.bearingTo(destination.position());
|
||
const auto delta = __normalize(prev - next);
|
||
if (100_deg <= delta.abs())
|
||
break;
|
||
|
||
distanceToNextWaypoint += coordinate.distanceTo(lastPosition);
|
||
lastPosition = coordinate;
|
||
}
|
||
|
||
/* predict the flight and the descend */
|
||
Velocity groundSpeed = static_cast<float>(target.GetPosition().GetReportedGS()) * knot;
|
||
Length altitude = static_cast<float>(target.GetPosition().GetFlightLevel()) * feet;
|
||
Angle heading = __convert(predictions.GetPosition(0)).bearingTo(destination.position());
|
||
|
||
Time flightTime = 0_s;
|
||
while (0.0_m < distanceToNextWaypoint) {
|
||
Length distance = groundSpeed * 1_s;
|
||
|
||
if (altitude > destination.altitude()) {
|
||
/* new descend required based on 3<> glide */
|
||
if (((altitude - destination.altitude()).convert(feet) / 1000.0f * 3.0f) > distanceToNextWaypoint.convert(nauticmile)) {
|
||
const auto oldGS = groundSpeed;
|
||
const auto descendRate = oldGS * 1_s * std::sinf(0.0523599f);
|
||
altitude -= descendRate;
|
||
|
||
const auto newGS = this->groundSpeed(altitude, this->indicatedAirspeed(altitude), heading);
|
||
groundSpeed = std::min(groundSpeed, newGS);
|
||
|
||
distance = (groundSpeed + oldGS) * 0.5f * 11_s;
|
||
}
|
||
}
|
||
|
||
distanceToNextWaypoint -= distance;
|
||
flightTime += 1_s;
|
||
}
|
||
|
||
auto currentUtc = UtcTime::currentUtc();
|
||
auto pta = UtcTime::timeToString(destination.plannedArrivalTime());
|
||
auto estimated = UtcTime::timeToString(currentUtc + std::chrono::seconds(static_cast<int>(flightTime.convert(second))));
|
||
auto delta = std::chrono::duration_cast<std::chrono::seconds>(destination.plannedArrivalTime() - currentUtc);
|
||
auto plannedFlightTime = static_cast<float>(delta.count()) * second;
|
||
this->m_timeToLose = plannedFlightTime - flightTime;
|
||
}
|
||
|
||
void Inbound::update(EuroScopePlugIn::CFlightPlan& plan) {
|
||
this->m_nextStarWaypoint = 0;
|
||
if (0 == this->m_arrivalRoute.size())
|
||
return;
|
||
|
||
/* find the point on the route */
|
||
std::string_view direct(plan.GetControllerAssignedData().GetDirectToPointName());
|
||
auto route = plan.GetExtractedRoute();
|
||
int starEntry = route.GetPointsNumber(), directEntry = route.GetPointsNumber();
|
||
|
||
/* TODO search point if direct is empty */
|
||
|
||
for (int c = 0; c < route.GetPointsNumber(); ++c) {
|
||
std::string_view waypointName(route.GetPointName(c));
|
||
|
||
if (waypointName == this->m_arrivalRoute.front().name())
|
||
starEntry = c;
|
||
else if (waypointName == direct)
|
||
directEntry = c;
|
||
}
|
||
|
||
/* search the direct to entry */
|
||
if (directEntry > starEntry && directEntry < route.GetPointsNumber()) {
|
||
/* try to find the closest next waypoint */
|
||
while (0 == this->m_nextStarWaypoint) {
|
||
for (std::size_t i = 0; i < this->m_arrivalRoute.size(); ++i) {
|
||
if (direct == gsl::at(this->m_arrivalRoute, i).name()) {
|
||
this->m_nextStarWaypoint = i;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (0 == this->m_nextStarWaypoint) {
|
||
directEntry += 1;
|
||
if (directEntry >= route.GetPointsNumber()) {
|
||
this->m_nextStarWaypoint = this->m_arrivalRoute.size() - 1;
|
||
break;
|
||
}
|
||
|
||
direct = std::string_view(route.GetPointName(directEntry));
|
||
}
|
||
}
|
||
}
|
||
|
||
EuroScopePlugIn::CRadarTarget target = plan.GetCorrelatedRadarTarget();
|
||
this->update(target);
|
||
}
|
||
|
||
bool Inbound::fixedPlan() const noexcept {
|
||
return this->m_fixedPlan;
|
||
}
|
||
|
||
const Time& Inbound::timeToLose() const noexcept {
|
||
return this->m_timeToLose;
|
||
}
|