291 lines
9.0 KiB
C++
Executable File
291 lines
9.0 KiB
C++
Executable File
/**
|
|
* MainHandler for TetraRadio
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by Christian Ehrlicher, SLSE, 2016-04-14
|
|
*/
|
|
|
|
#include "position_handler.h"
|
|
#include "exm_handler.h"
|
|
#include "logger.h"
|
|
#include <AssetManagement/asm_dtc.h>
|
|
|
|
using namespace TrainingAreaBase;
|
|
|
|
PositionHandler::PositionHandler(MainHandler *mainHandler)
|
|
: QObject()
|
|
, m_mainHandler(mainHandler)
|
|
, m_logger(getLogger("PositionHandler"))
|
|
{}
|
|
|
|
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; // 10s
|
|
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;
|
|
}
|
|
// no longer needed
|
|
m_dtcAsm.reset();
|
|
|
|
// calc tetra base point from training area center
|
|
const LtsCoordTrans::UtmCoordinate bottomLeft(LtsCoordTrans::UtmCoordinate::fromGeographic(ta.bottomLeft().lon(), ta.bottomLeft().lat()));
|
|
const QPointF mid(bottomLeft.geographic(ta.width() / 2, ta.height() / 2));
|
|
|
|
// our center point is NNxYY 00000 00000 by definition
|
|
// (we can also use current ta center but the it's hard to debug the coordinates in Tetra LogViewer)
|
|
QString mgrs = LtsCoordTrans::mgrsFromGeographic(mid.x(), mid.y(), -1);
|
|
mgrs += QStringLiteral("00000000000000");
|
|
|
|
// now we've the correct tetra base point
|
|
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))
|
|
{
|
|
// the relative position is relative to the base point
|
|
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();
|
|
|
|
// convert lon/lat to utm with the given zone from the center point
|
|
const LtsCoordTrans::UtmCoordinate point(LtsCoordTrans::UtmCoordinate::fromGeographic(p.lon(), p.lat(), m_utm2Geographic.zone));
|
|
// now calc distance from point to base point == relative position
|
|
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));
|
|
// v = s / t
|
|
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); // speed in [m/s]
|
|
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;
|
|
}
|