285 lines
8.4 KiB
C++
Executable File
285 lines
8.4 KiB
C++
Executable File
|
|
|
|
|
|
#include "position_handler.h"
|
|
#include "exm_handler.h"
|
|
#include "logger.h"
|
|
#include <NGLLogger/NGLLoggerBootstrap.h>
|
|
#include <AssetManagement/asm_dtc.h>
|
|
|
|
using namespace TrainingAreaBase;
|
|
|
|
PositionHandler::PositionHandler(MainHandler *mainHandler) : QObject()
|
|
, m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger())
|
|
, m_mainHandler(mainHandler) {}
|
|
|
|
bool PositionHandler::init()
|
|
{
|
|
m_dtcAsm = AssetManagement::Interface::Dtc::create(m_mainHandler->ddsHandler()->ddsConnector());
|
|
return m_dtcAsm->enableTrainingArea();
|
|
}
|
|
|
|
bool PositionHandler::initCoordinateConversion() const
|
|
{
|
|
if (m_dtcAsm == nullptr)
|
|
return true;
|
|
|
|
if (m_mainHandler->exerciseId() == 0)
|
|
return true;
|
|
|
|
constexpr int ddsTimeout = 10 * 1000;
|
|
const auto ex = m_mainHandler->exmHandler()->exercise(m_mainHandler->exerciseId());
|
|
if (ex.id <= 0)
|
|
{
|
|
ERROR_LOG(m_logger) << "Can not retrieve exercise information for id " << m_mainHandler->exerciseId();
|
|
return false;
|
|
}
|
|
if (!m_dtcAsm->waitForHistoricalDataTrainingArea(ddsTimeout))
|
|
{
|
|
ERROR_LOG(m_logger) << "Error initializing AssetManagement DTC";
|
|
return false;
|
|
}
|
|
const auto ta = m_dtcAsm->trainingArea(ex.trainingAreaId);
|
|
if (ta.id <= 0)
|
|
{
|
|
ERROR_LOG(m_logger) << "Can not retrieve training are information for id " << ex.trainingAreaId;
|
|
return false;
|
|
}
|
|
|
|
m_dtcAsm.reset();
|
|
|
|
|
|
const LtsCoordTrans::UtmCoordinate bottomLeft(LtsCoordTrans::UtmCoordinate::fromGeographic(ta.bottomLeft.lon, ta.bottomLeft.lat));
|
|
const QPointF mid(bottomLeft.geographic(ta.width / 2, ta.height / 2));
|
|
|
|
|
|
|
|
QString mgrs = LtsCoordTrans::mgrsFromGeographic(mid.x(), mid.y(), -1);
|
|
mgrs += QStringLiteral("00000000000000");
|
|
|
|
|
|
m_utm2Geographic = LtsCoordTrans::UtmCoordinate::fromMgrs(mgrs);
|
|
|
|
LtsCoordTrans::UtmCoordinate topRight(bottomLeft);
|
|
topRight.x += ta.width;
|
|
topRight.y += ta.height;
|
|
|
|
m_relPosBottomLeft = bottomLeft.relativeTo(m_utm2Geographic);
|
|
m_relPosTopRight = topRight.relativeTo(m_utm2Geographic);
|
|
|
|
NOTICE_LOG(m_logger) << "Training area bottomLeft: " << m_relPosBottomLeft.x() << "," << m_relPosBottomLeft.y()
|
|
<< ", topRight: " << m_relPosTopRight.x() << "," << m_relPosTopRight.y()
|
|
<< ", base point: " << mgrs
|
|
<< ", width:" << ta.width
|
|
<< ", height: " << ta.height;
|
|
|
|
return true;
|
|
}
|
|
|
|
Core::GeoPosition2D PositionHandler::relativeToGeographic2D(const TetraIcd::RelativePosition &p) const
|
|
{
|
|
initCoordinateConversion();
|
|
|
|
Core::GeoPosition2D ret;
|
|
if (!isPositionMarkedInvalid(p))
|
|
{
|
|
|
|
const QPointF point(m_utm2Geographic.geographic(p.offsetX, p.offsetY));
|
|
ret.lon = point.x();
|
|
ret.lat = point.y();
|
|
}
|
|
else
|
|
{
|
|
ret.lon = nan("");
|
|
ret.lat = nan("");
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
Core::GeoPosition PositionHandler::relativeToGeographic(const TetraIcd::RelativePosition &p) const
|
|
{
|
|
const Core::GeoPosition2D pos2d(relativeToGeographic2D(p));
|
|
Core::GeoPosition ret;
|
|
ret.lon = pos2d.lon;
|
|
ret.lat = pos2d.lat;
|
|
ret.z = p.offsetZ;
|
|
DEBUG_LOG(m_logger) << "x/y -> lon/lat: " << p.offsetX << "," << p.offsetY
|
|
<< " -> " << ret.lon * LtsCoordTrans::rad2deg() << "," << ret.lat * LtsCoordTrans::rad2deg();
|
|
return ret;
|
|
}
|
|
|
|
QPointF PositionHandler::geographicToRelativePointF(const Core::GeoPosition2D &p) const
|
|
{
|
|
return geographicToRelativePointF(Core::GeoPosition(p.lon, p.lat, 0));
|
|
}
|
|
|
|
QPointF PositionHandler::geographicToRelativePointF(const Core::GeoPosition &p) const
|
|
{
|
|
initCoordinateConversion();
|
|
|
|
|
|
const LtsCoordTrans::UtmCoordinate point(LtsCoordTrans::UtmCoordinate::fromGeographic(p.lon, p.lat, m_utm2Geographic.zone));
|
|
|
|
return QPointF(point.x - m_utm2Geographic.x, point.y - m_utm2Geographic.y);
|
|
}
|
|
|
|
TetraIcd::RelativePosition PositionHandler::geographicToRelative(const Core::GeoPosition2D &p, double z) const
|
|
{
|
|
return geographicToRelative(Core::GeoPosition(p.lon, p.lat, z));
|
|
}
|
|
|
|
TetraIcd::RelativePosition PositionHandler::geographicToRelative(const Core::GeoPosition &p) const
|
|
{
|
|
const QPointF rel(geographicToRelativePointF(p));
|
|
const TetraIcd::RelativePosition ret(qRound(rel.x()), qRound(rel.y()), p.z);
|
|
DEBUG_LOG(m_logger) << "lon/lat -> x/y: " << p.lon * LtsCoordTrans::rad2deg() << "," << p.lat * LtsCoordTrans::rad2deg()
|
|
<< " -> " << ret.offsetX << "," << ret.offsetY;
|
|
return ret;
|
|
}
|
|
|
|
const LtsCoordTrans::UtmCoordinate &PositionHandler::baseCoordinate() const
|
|
{
|
|
initCoordinateConversion();
|
|
|
|
return m_utm2Geographic;
|
|
}
|
|
|
|
double PositionHandler::speed (const TetraIcd::RelativePosition &relPos, const int equipNo, const qint64 timeStamp) const
|
|
{
|
|
if (!m_lastPosition.contains(equipNo) )
|
|
return 0.0;
|
|
|
|
ParticipantPosition lastParticipantPosition = m_lastPosition.value(equipNo);
|
|
qint64 deltaTime = timeStamp - lastParticipantPosition.time;
|
|
if (deltaTime == 0)
|
|
return 0.0;
|
|
|
|
TetraIcd::RelativePosition lastRelPosition = geographicToRelative(lastParticipantPosition.pos.position);
|
|
|
|
QLineF line(QPointF(lastRelPosition.offsetX, lastRelPosition.offsetY),
|
|
QPointF(relPos.offsetX, relPos.offsetY));
|
|
|
|
DEBUG_LOG(m_logger) << "POS alt=" << lastRelPosition.offsetX << "/" << lastRelPosition.offsetY
|
|
<< " POS neu=" << relPos.offsetX << "/" << relPos.offsetY
|
|
<< " distance= " << line.length()
|
|
<< " timeStamp=" << timeStamp
|
|
<< " lastParticipantPosition=" << lastParticipantPosition.time
|
|
<< " deltaTime=" << deltaTime;
|
|
|
|
|
|
double speed = line.length() / (deltaTime / 1000);
|
|
return speed;
|
|
}
|
|
|
|
|
|
bool PositionHandler::isTimeToPerformSpeedCheckExceeded(const int equipNo, const qint64 timeStamp, const int maxTime) const
|
|
{
|
|
if (!m_lastPosition.contains(equipNo))
|
|
return true;
|
|
|
|
qint64 deltaTime = timeStamp - m_lastPosition[equipNo].time;
|
|
|
|
return (deltaTime > (maxTime * 1000));
|
|
}
|
|
|
|
|
|
TrainingAreaBase::TabPosition PositionHandler::lastPosition(int equipNo) const
|
|
{
|
|
ParticipantPosition lastPos;
|
|
TrainingAreaBase::TabPosition ret;
|
|
if ( m_lastPosition.contains(equipNo))
|
|
{
|
|
lastPos = m_lastPosition.value(equipNo);
|
|
return lastPos.pos;
|
|
}
|
|
ret.position = Core::GeoPosition(nan(""), nan(""), nan(""));
|
|
return ret;
|
|
}
|
|
|
|
void PositionHandler::setLastPosition(int equipNo, const TrainingAreaBase::TabPosition &pos)
|
|
{
|
|
ParticipantPosition lastPos;
|
|
if ( m_lastPosition.contains(equipNo))
|
|
lastPos = m_lastPosition.value(equipNo);
|
|
lastPos.pos = pos;
|
|
m_lastPosition.insert (equipNo, lastPos);
|
|
}
|
|
|
|
|
|
void PositionHandler::setLastMountPointTypeId(int equipNo, const int lastMountPointTypeId)
|
|
{
|
|
ParticipantPosition lastPos;
|
|
if ( m_lastPosition.contains(equipNo))
|
|
lastPos = m_lastPosition.value(equipNo);
|
|
lastPos.mountPointTypeId = lastMountPointTypeId;
|
|
m_lastPosition.insert (equipNo, lastPos);
|
|
|
|
}
|
|
|
|
void PositionHandler::setLastBuildingId(int equipNo, const int lastBuildingId)
|
|
{
|
|
ParticipantPosition lastPos;
|
|
if ( m_lastPosition.contains(equipNo))
|
|
lastPos = m_lastPosition.value(equipNo);
|
|
lastPos.buildingId = lastBuildingId;
|
|
m_lastPosition.insert (equipNo, lastPos);
|
|
|
|
}
|
|
|
|
void PositionHandler::setLastRoomId(int equipNo, const int lastRoomId)
|
|
{
|
|
ParticipantPosition lastPos;
|
|
if ( m_lastPosition.contains(equipNo))
|
|
lastPos = m_lastPosition.value(equipNo);
|
|
lastPos.roomId = lastRoomId;
|
|
m_lastPosition.insert (equipNo, lastPos);
|
|
|
|
}
|
|
|
|
void PositionHandler::setLastTime(int equipNo, const qint64 time)
|
|
{
|
|
ParticipantPosition lastPos;
|
|
if ( m_lastPosition.contains(equipNo))
|
|
lastPos = m_lastPosition.value(equipNo);
|
|
lastPos.time = time;
|
|
m_lastPosition.insert (equipNo, lastPos);
|
|
|
|
}
|
|
|
|
|
|
int PositionHandler::lastMountPointTypeId(const int equipNo) const
|
|
{
|
|
if ( m_lastPosition.contains(equipNo))
|
|
return (m_lastPosition.value(equipNo).mountPointTypeId);
|
|
else
|
|
return -1;
|
|
}
|
|
|
|
int PositionHandler::lastBuildingId(const int equipNo) const
|
|
{
|
|
if ( m_lastPosition.contains(equipNo))
|
|
return (m_lastPosition.value(equipNo).buildingId);
|
|
else
|
|
return -1;
|
|
}
|
|
|
|
int PositionHandler::lastRoomId(const int equipNo) const
|
|
{
|
|
if ( m_lastPosition.contains(equipNo))
|
|
return (m_lastPosition.value(equipNo).roomId);
|
|
else
|
|
return -1;
|
|
}
|
|
|
|
bool PositionHandler::isInTrainingArea(const TetraIcd::RelativePosition &relPos) const
|
|
{
|
|
if (m_relPosBottomLeft.isNull() || m_relPosTopRight.isNull())
|
|
return true;
|
|
|
|
if (relPos.offsetX < m_relPosBottomLeft.x() || relPos.offsetX > m_relPosTopRight.x())
|
|
return false;
|
|
if (relPos.offsetY < m_relPosBottomLeft.y() || relPos.offsetY > m_relPosTopRight.y())
|
|
return false;
|
|
return true;
|
|
}
|