264 lines
9.9 KiB
C++
Executable File
264 lines
9.9 KiB
C++
Executable File
#include "saa_interface.h"
|
|
#include "qpoint.h"
|
|
#include <LtsCoordTrans/lts_coord_trans.h>
|
|
#include <TrainingAreaBase/Plugins/AdlerJson/telegrams.h>
|
|
#include <TrainingAreaBase/training_area_telegrams.h>
|
|
|
|
#include <QTransform>
|
|
|
|
namespace AdlerJson
|
|
{
|
|
QString statusToStr(ScenarioStatus status)
|
|
{
|
|
switch (status)
|
|
{
|
|
case ScenarioStatus::INACTIVE:
|
|
return QStringLiteral("Inaktiv");
|
|
case ScenarioStatus::PLANNED:
|
|
return QStringLiteral("Geplant");
|
|
case ScenarioStatus::ACTIVE:
|
|
return QStringLiteral("Aktiv");
|
|
case ScenarioStatus::STOPPED:
|
|
return QStringLiteral("Gestoppt");
|
|
default:
|
|
return QStringLiteral("");
|
|
}
|
|
}
|
|
ScenarioStatus strToStatus(QString status)
|
|
{
|
|
if (status == QStringLiteral("Inaktiv"))
|
|
{
|
|
return ScenarioStatus::INACTIVE;
|
|
}
|
|
else if (status == QStringLiteral("Geplant"))
|
|
{
|
|
return ScenarioStatus::PLANNED;
|
|
}
|
|
else if (status == QStringLiteral("Aktiv"))
|
|
{
|
|
return ScenarioStatus::ACTIVE;
|
|
}
|
|
else if (status == QStringLiteral("Gestoppt"))
|
|
{
|
|
return ScenarioStatus::STOPPED;
|
|
}
|
|
else
|
|
{
|
|
return ScenarioStatus::NONE;
|
|
}
|
|
}
|
|
|
|
SAAInterface::SAAInterface(int wsId, int partyId, const QString &saaName, const QString &szenarName, bool isVerbund, QObject *parent)
|
|
: QObject(parent)
|
|
, m_wsId(wsId)
|
|
, m_partyId(partyId)
|
|
, m_saaName(saaName)
|
|
, m_szenarName(szenarName)
|
|
, m_status(Szenarstatus::Inaktiv)
|
|
, m_isVerbund(isVerbund)
|
|
, m_logger(getLogger(QStringLiteral("SAAInterface.") + QString::number(partyId)))
|
|
{
|
|
}
|
|
|
|
bool SAAInterface::init(std::shared_ptr<PpmHandler> ppmHandler,
|
|
std::shared_ptr<PdmHandler> pdmHandler,
|
|
std::shared_ptr<AesHandler> aesHandler,
|
|
std::shared_ptr<TrainingAreaBase::Interface::Dts> dtsTab)
|
|
{
|
|
m_ppmHandler = ppmHandler;
|
|
m_pdmHandler = pdmHandler;
|
|
m_aesHandler = aesHandler;
|
|
m_dtsTab = dtsTab;
|
|
|
|
m_ftcAes = m_aesHandler->getFtc();
|
|
|
|
DEBUG_LOG(m_logger) << "INIT SAA Interface";
|
|
|
|
return true;
|
|
}
|
|
|
|
void SAAInterface::setStatus(Szenarstatus status)
|
|
{
|
|
m_status = status;
|
|
if (m_status == Szenarstatus::Gestoppt)
|
|
{
|
|
// Remove all Particpants
|
|
m_ppmHandler->removeAllSAAParticipants(m_partyId);
|
|
// Remove all pending effect cluster
|
|
m_aesHandler->removeAllPendingEffectCluster(m_partyId);
|
|
}
|
|
}
|
|
|
|
|
|
static Core::RelativePositionRelativeTime convertFlugbahnpunkt(const Flugbahnpunkt &fbPos,
|
|
const QTransform &transformMatrix,
|
|
const LtsCoordTrans::UtmCoordinate &utmLaunch,
|
|
LOGGER_T logger)
|
|
{
|
|
// vergl. GUEZSZ50_ICD-0036_0.1.0_Zentrale-SAA.pdf
|
|
// fbPos.x -> y ??
|
|
// fbPos.z -> x ??
|
|
// fbPos.y -> z ??
|
|
|
|
const QPointF fbPosOrdered = QPointF(0, fbPos.xRelativ);
|
|
|
|
const auto rotatePoint = transformMatrix.map(fbPosOrdered);
|
|
|
|
if (logger.isDebugEnabled())
|
|
{
|
|
// see also: coordtrans_test::testRelativeMetricalOffet()
|
|
const LtsCoordTrans::UtmCoordinate utmCoord(utmLaunch.zone,
|
|
utmLaunch.x + rotatePoint.x(),
|
|
utmLaunch.y + rotatePoint.y());
|
|
|
|
DEBUG_LOG(logger) << "-----Flugbahnpunkt fbPosOrdered:" << fbPosOrdered << " -> rotatePoint=" << rotatePoint;
|
|
DEBUG_LOG(logger) << " -> utmCoord:" << utmCoord << "; mgrs: " << utmCoord.mgrs();
|
|
}
|
|
|
|
return Core::RelativePositionRelativeTime(rotatePoint.x(), rotatePoint.y(), fbPos.yRelativ,
|
|
std::round(fbPos.zeit * 1000.0));
|
|
}
|
|
|
|
// Kann nur mit aktiver SAA-Verbindung angefragt werden, deshalb verbleiben die Flugbahndaten hier
|
|
void SAAInterface::parseFlugbahndaten(const Flugbahndaten &flugbahndaten, int aesItemId, int exposionId, const Core::GeoPosition &positionLaunch)
|
|
{
|
|
DEBUG_LOG(m_logger) << "SAAInterface::parseFlugbahndaten";
|
|
|
|
std::vector<Core::RelativePositionRelativeTime> geometryRef;
|
|
geometryRef.clear();
|
|
geometryRef.reserve(flugbahndaten.flugbahnpunkt.size());
|
|
|
|
AreaEffectSimulation::TrajectoryAdlerModifyRequest *newRequest = new AreaEffectSimulation::TrajectoryAdlerModifyRequest();
|
|
newRequest->aesItemId(aesItemId);
|
|
newRequest->explosionIndex(exposionId);
|
|
|
|
int zone = -1;
|
|
|
|
const auto &utmLaunch = LtsCoordTrans::UtmCoordinate::fromGeographic(positionLaunch.lon(), positionLaunch.lat(), zone);
|
|
|
|
// Rotiert im Uhrzeigersinn um den Nullpunkt (0,0)
|
|
DEBUG_LOG(m_logger) << " mmilirad: " << flugbahndaten.schussrichtung;
|
|
double grad = (flugbahndaten.schussrichtung / 6400.) * 360.0;
|
|
DEBUG_LOG(m_logger) << " milli rad to degree" << (flugbahndaten.schussrichtung / 6400.) * 360;
|
|
DEBUG_LOG(m_logger) << "Degree to target" << grad;
|
|
|
|
DEBUG_LOG(m_logger) << "Start point: " << positionLaunch;
|
|
|
|
const auto &coordEnd = LtsCoordTrans::UtmCoordinate::fromGeographic(flugbahndaten.zielkoordinate.koordinate.longitude * (M_PI / 180.0),
|
|
flugbahndaten.zielkoordinate.koordinate.latitude * (M_PI / 180.0));
|
|
DEBUG_LOG(m_logger) << "End point: " << coordEnd;
|
|
LtsCoordTrans::UtmCoordinate coordDiff = coordEnd;
|
|
|
|
coordDiff.x = coordDiff.x - utmLaunch.x;
|
|
coordDiff.y = coordDiff.y - utmLaunch.y;
|
|
const double useNewGrad = -90 + std::atan2(coordDiff.y, coordDiff.x) * (180.0 / M_PI);
|
|
DEBUG_LOG(m_logger) << "use new Degree " << useNewGrad;
|
|
|
|
QTransform matrix;
|
|
matrix.rotate(useNewGrad);
|
|
|
|
|
|
for (Flugbahnpunkt pos : qAsConst(flugbahndaten.flugbahnpunkt))
|
|
{
|
|
//
|
|
//
|
|
// an easier solution should be this:
|
|
//
|
|
//
|
|
if (m_logger.isDebugEnabled())
|
|
{
|
|
const auto &fbPosRelative = convertFlugbahnpunkt(pos, matrix, utmLaunch, m_logger);
|
|
DEBUG_LOG(m_logger) << " fbPosRelative " << fbPosRelative;
|
|
}
|
|
|
|
|
|
TRACE_LOG(m_logger) << "Flugbahnpunkt:"
|
|
<< ", xRelativ :" << pos.xRelativ
|
|
<< ", yRelativ :" << pos.yRelativ
|
|
<< ", zRelativ :" << pos.zRelativ
|
|
<< ", zeit :" << pos.zeit
|
|
<< ", positionLaunch :" << positionLaunch.lon()
|
|
<< " / " << positionLaunch.lat()
|
|
<< " / " << positionLaunch.z()
|
|
<< "<< utmLaunch :" << utmLaunch.x
|
|
<< " / " << utmLaunch.y
|
|
<< " / " << utmLaunch.zone;
|
|
|
|
QPointF utm = QPointF(pos.xRelativ, 0);//pos.zRelativ);
|
|
QPointF utmLaunchPt(utmLaunch.x, utmLaunch.y);
|
|
|
|
utm += utmLaunchPt;
|
|
double z = positionLaunch.z() + pos.yRelativ;
|
|
|
|
// see also: coordtrans_test::testRelativeMetricalOffet()
|
|
LtsCoordTrans::UtmCoordinate utmCoord;
|
|
utmCoord.x = utm.x();
|
|
utmCoord.y = utm.y();
|
|
utmCoord.zone = utmLaunch.zone;
|
|
QPointF pntGeo = utmCoord.geographic();
|
|
|
|
TRACE_LOG(m_logger) << "utmCoord :" << utmCoord.x << " / " << utmCoord.y << ", utmCoord.zone :" << utmCoord.zone << ", utmCoord.isValid() :" << utmCoord.isValid()
|
|
<< ", pntGeo :" << pntGeo.x() << " / " << pntGeo.y() << " / " << z;
|
|
QTransform matrix;
|
|
matrix.rotate(useNewGrad);
|
|
|
|
Core::RelativePositionRelativeTime flightPoint;
|
|
flightPoint.offsetX() = 0;// pos.zRelativ;
|
|
flightPoint.offsetY() = pos.xRelativ;
|
|
flightPoint.offsetZ() = pos.yRelativ;
|
|
flightPoint.relativeTime() = pos.zeit;
|
|
QPointF point(flightPoint.offsetX(), flightPoint.offsetY());
|
|
|
|
TRACE_LOG(m_logger) << " POS bevore rot: " << point;
|
|
|
|
auto rotatePoint = matrix.map(point);
|
|
TRACE_LOG(m_logger) << " POS after rot: " << rotatePoint;
|
|
|
|
pntGeo.setX(rotatePoint.x());
|
|
pntGeo.setY(rotatePoint.y());
|
|
|
|
TRACE_LOG(m_logger) << "utmCoord :" << utmCoord.x << " / " << utmCoord.y << ", utmCoord.zone :" << utmCoord.zone << ", utmCoord.isValid() :" << utmCoord.isValid()
|
|
<< ", pntGeo :" << pntGeo.x() << " / " << pntGeo.y() << " / " << z;
|
|
// geometryRef.emplace_back(Core::GeoPositionRelativeTime(pntGeo.x(), pntGeo.y(), z, pos.zeit * 1000));
|
|
// We Send the Trajectory as IS as the Effector will use them now.
|
|
// Then we would apply the
|
|
// Core::GeoPositionRelativeTime flightPoint;
|
|
flightPoint.offsetX() = rotatePoint.x();
|
|
flightPoint.offsetY() = rotatePoint.y();
|
|
flightPoint.offsetZ() = pos.yRelativ;
|
|
flightPoint.relativeTime() = std::round(pos.zeit * 1000.0);
|
|
|
|
geometryRef.emplace_back(flightPoint);
|
|
}
|
|
|
|
Core::GeoPosition impactPosition;
|
|
impactPosition.lat(flugbahndaten.zielkoordinate.koordinate.latitude * M_PI / 180.0);
|
|
impactPosition.lon(flugbahndaten.zielkoordinate.koordinate.longitude * M_PI / 180.0);
|
|
impactPosition.z(flugbahndaten.zielkoordinate.hoehe);
|
|
newRequest->impactPosition(impactPosition);
|
|
newRequest->geometry(geometryRef);
|
|
newRequest->peakAltitude(flugbahndaten.gipfelhoehe);
|
|
newRequest->mhpScatterLenght(flugbahndaten.streuung.laengsstreuung);
|
|
newRequest->mhpScatterLengthDetonationHeight(flugbahndaten.streuung.laengsstreuungH);
|
|
newRequest->mhpScatterWidth(flugbahndaten.streuung.seitenstreuung);
|
|
newRequest->mhpScatterWidthDetonationHeight(flugbahndaten.streuung.seitenstreuungH);
|
|
newRequest->shootDirection((flugbahndaten.schussrichtung / 6400.) * 360);
|
|
newRequest->errorcode(static_cast<int>(flugbahndaten.fehlercode));
|
|
DEBUG_LOG(m_logger) << "!!!!!!!!!!!geometryRef.size=" << geometryRef.size() << ", newRequest->geometry().size() :" << newRequest->geometry().size()
|
|
<< ", newRequest->errorcode :" << newRequest->errorcode() << ", newRequest->Peak:" << newRequest->peakAltitude()
|
|
<< ", newRequest->direction:" << newRequest->shootDirection() << ", flugbahndaten.fehlercode :" << static_cast<int>(flugbahndaten.fehlercode);
|
|
|
|
m_ftcAes->trajectoryAdlerModifyRequest(*newRequest, this);
|
|
}
|
|
|
|
void SAAInterface::onTrajectoryAdlerModifyReply(const Core::FunctionReplyObject &reply)
|
|
{
|
|
DEBUG_LOG(m_logger) << "onTrajectoryAdlerModifyReply";
|
|
if (reply.error())
|
|
{
|
|
ERROR_LOG(m_logger) << reply.translated();
|
|
}
|
|
}
|
|
|
|
} // namespace AdlerJson
|