Initial commit

Lts/Proxies/WirelessLocationSystem/wls_participant.cpp

@@ -0,0 +1,464 @@
+/**
+  * A single Participant
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Christian Ehrlicher, SLSE, 2017-01-10
+  */
+
+#include "wls_participant.h"
+#include "wls_sfm_message.h"
+#include "wls_handler.h"
+#include "settings.h"
+#include <QDateTime>
+#include <LtsUtilsCore/dds_vector.h>
+#include <LtsUtilsCore/sfm_bytemessage_converter.h>
+
+static constexpr qint64 s_dataCommunicationAliveTimeout = 5 * 1000;
+static constexpr int s_maxWlsRetryCount = 3;
+
+namespace WirelessLocationSystem
+{
+WlsParticipant::WlsParticipant(QObject *parent)
+  : QObject(parent)
+  , m_wlsTeleTimeoutTimer(new QTimer(this))
+  , m_logger(getLogger("WlsParticipant"))
+{
+  m_wlsTeleTimeoutTimer->setSingleShot(true);
+  m_wlsTeleTimeoutTimer->setTimerType(Qt::VeryCoarseTimer);
+  m_wlsTeleTimeoutTimer->setInterval(s_dataCommunicationAliveTimeout);
+  connect(m_wlsTeleTimeoutTimer, &QTimer::timeout, this, &WlsParticipant::onWlsTeleTimeout);
+}
+
+QByteArray WlsParticipant::getPlayerIdStr() const
+{
+  QByteArray ret;
+  if (m_equipmentNumber > 0)
+    ret = QByteArray::number(m_equipmentNumber);
+  else
+    ret = "unknown";
+  if (m_tagId.isEmpty())
+    ret += "(unknown tag id)";
+  else
+    ret += "(" + m_tagId + ")";
+  return ret;
+}
+
+void WlsParticipant::setEquipmentNumber(int newEquipmentNumber)
+{
+  if (m_equipmentNumber == newEquipmentNumber)
+    return;
+  m_equipmentNumber = newEquipmentNumber;
+
+  emit dataCommunicationStateChanged(this);
+  emit positionChanged(this);
+}
+
+void WlsParticipant::setPosition(const RelativePosition &newPosition,
+                                 qint64 tsCurrentPos)
+{
+  if (tsCurrentPos == m_lastSeenPosition ||
+      (((tsCurrentPos - m_lastSeenPosition) <= Settings::deadPosTimeout())  && comparePos(m_relativePosition, newPosition)))// same timestamp or same pos for 5 sec
+  {
+    DEBUG_LOG(m_logger) << "Check for pos Update";
+    DEBUG_LOG(m_logger) << "Is Outdated= " <<((tsCurrentPos - m_lastSeenPosition) <= Settings::deadPosTimeout());
+
+    if (((tsCurrentPos - m_lastSeenPosition) <= Settings::deadPosTimeout())  && comparePos(m_relativePosition, newPosition))
+    {
+      DEBUG_LOG(m_logger) << "Same Position or not older than 5000msecs " << tsCurrentPos - m_lastSeenPosition;
+      return;
+    }
+    const qint64 tsNow = QDateTime::currentMSecsSinceEpoch();
+    if (tsCurrentPos == m_lastSeenPosition && m_lastValidPosition + 9 * 60 * 1000 > tsNow) // younger than 9 min -> ignore //should never be true as agilion always send with new timestamp
+      return; // position did not change
+
+    // did not move in the last 9min, send a new position report to excon
+    m_lastValidPosition = tsNow;
+    if (isDataCommunicationPossible())
+      emit positionChanged(this);
+
+    return;
+  }
+
+  const int newWlsZone = WlsParticipant::calcWlsZone(newPosition.wlsAreaId,
+                                                     newPosition.wlsBuildingId,
+                                                     newPosition.wlsRoomNumber);
+  setWlsZone(newWlsZone);
+
+  // to create a new position report on restart / after 9 min timeout
+  m_lastSeenPosition = tsCurrentPos;
+  m_lastValidPosition = tsCurrentPos;
+
+  m_relativePosition = newPosition;
+  m_relativePosition.isValid = true;
+
+  if (isDataCommunicationPossible())
+    emit positionChanged(this);
+}
+
+void WlsParticipant::checkGpsOnOffTimeoutTimer(qint64 tNow)
+{
+  // full blown tetra mode -> nothing to do
+  if (m_gpsMode == GpsMode::On ||
+      m_dataCommunication == DataCommunication::Off)
+    return;
+  if (m_lastSeenDataCommunication > tNow - s_maxWlsRetryCount * s_dataCommunicationAliveTimeout)
+    return;
+
+  DEBUG_LOG(m_logger) << getPlayerIdStr() << ": lastSeenDataCommunication: "
+                      << m_lastSeenDataCommunication << ", now: "
+                      << (tNow - s_maxWlsRetryCount * s_dataCommunicationAliveTimeout)
+                      << " (" << tNow << "), "
+                      << m_dataCommunication << ", "
+                      << m_gpsMode;
+
+  // no alive from participant -> switch back to
+  if (m_dataCommunication == DataCommunication::On)
+  {
+    m_dataCommunication = DataCommunication::Off;
+    emit dataCommunicationStateChanged(this);
+  }
+  if (m_gpsMode == GpsMode::Off)
+  {
+    m_gpsMode = GpsMode::On;
+    emit gpsOnOffChanged(this);
+  }
+  m_wlsZone = 0;
+}
+
+void WlsParticipant::setLastSeenWlsTelegramTime(qint64 ts)
+{
+  m_lastSeenDataCommunication = ts;
+  // current only allowed when wlsZone > 3
+  //  m_relativePosition.dataCommunication = WlsParticipant::DataCommunication::On;
+}
+
+void WlsParticipant::setWlsZone(int newWlsZone)
+{
+  const auto oldGpsMode = m_gpsMode;
+  const auto oldDataComm = m_dataCommunication;
+  if (newWlsZone >= 0) // < 0 == restart
+  {
+    if (WlsParticipant::mustSendGpsOff(m_wlsZone, newWlsZone))
+      m_gpsMode = GpsMode::Off;
+    else if (WlsParticipant::mustSendGpsOn(m_wlsZone, newWlsZone))
+      m_gpsMode = GpsMode::On;
+
+    m_dataCommunication = (newWlsZone > 3)
+                          ? DataCommunication::On
+                          : DataCommunication::Off;
+    INFO_LOG(m_logger) << getPlayerIdStr() << ": wlsZone old: " << m_wlsZone << ", new: " << newWlsZone
+                       << ", new gps mode: " << m_gpsMode
+                       << ", new data comm mode: " << m_dataCommunication
+                       << ", " << this;
+  }
+  else
+  {
+    INFO_LOG(m_logger) << getPlayerIdStr() << ": Reset zone/data comunication status - sfm was restarted";
+    m_gpsMode = GpsMode::On;
+    m_dataCommunication = DataCommunication::Unknown;
+    m_lastSeenPosition = -1;
+  }
+  m_wlsZone = newWlsZone;
+
+  if (oldDataComm != m_dataCommunication)
+  {
+    if (m_dataCommunication == DataCommunication::On)
+      m_lastSeenDataCommunication = QDateTime::currentMSecsSinceEpoch();
+    emit dataCommunicationStateChanged(this);
+  }
+  if (oldGpsMode != m_gpsMode)
+    emit gpsOnOffChanged(this);
+}
+
+bool WlsParticipant::handleSfmRxData(const SfmMsgTetra_rx *sfmTele)
+{
+  if (!isRecvCounterCorrect(sfmTele->sendCounter()))
+  {
+    WARNING_LOG(m_logger) << "Got data telegram with counter " << int(sfmTele->sendCounter())
+                          << " last one was " << int(m_lastRecvCounter) << " - ignoring";
+    return false;
+  }
+
+  m_bSfmPayloadReady = false;
+  if (sfmTele->isFragmented())
+  {
+    if (m_sfmRxPayloadFragmentNumber + 1 != sfmTele->fragmentNumber())
+    {
+      WARNING_LOG(m_logger) << getPlayerIdStr() << ": Missing a fragment (waiting for "
+                            << (m_sfmRxPayloadFragmentNumber + 1)
+                            << ", got " << sfmTele->fragmentNumber() << ")";
+      if (sfmTele->fragmentNumber() != 1)
+        return false;
+      // assume the last fragmented telegram did not came through, start over
+      m_sfmRxPayloadFragmentNumber = 0;
+    }
+    if (m_sfmRxPayloadFragmentNumber == 0)  // first fragment - clear buffer
+      m_sfmRxPayloadData.clear();
+    ++m_sfmRxPayloadFragmentNumber;
+    m_sfmRxPayloadData += sfmTele->payload();
+    m_bSfmPayloadReady = (sfmTele->fragmentCount() == sfmTele->fragmentNumber());
+
+    if (m_bSfmPayloadReady) // last fragment - reset fragment counter
+      m_sfmRxPayloadFragmentNumber = 0;
+  }
+  else
+  {
+    // not fragmented
+    if (m_sfmRxPayloadFragmentNumber != 0)
+      WARNING_LOG(m_logger) << getPlayerIdStr()
+                            << ": Got unfragmented telegram but currently waiting for next fragment of a fragmented telegram";
+    m_sfmRxPayloadFragmentNumber = 0; // assume the last fragmenation did not came through
+    m_sfmRxPayloadData = sfmTele->payload();
+    m_bSfmPayloadReady = true;
+  }
+
+  if (m_bSfmPayloadReady)
+    emit wlsBinarySfmDataComplete(this);
+
+  return true;
+}
+
+bool WlsParticipant::calcChunkCountAndLen(const QByteArray &data, int &chunkCount, int &chunkLen)
+{
+  // hex as ascii -> / 2
+  // STX
+  // msgid (2bytes, current msgIds are all fine - no substitution needed),
+  // msg length is in message, we assume it must be subsituted (using 0x02 for checks)
+  // fragment byte is correctly calculated
+  // checksum byte (2 bytes worst case)
+  // ETX
+  constexpr int maxBytesPerChunk = (WlsHandler::s_maxLenTransparentData / 2) - (1 + 2 + 2 + 1);
+
+  chunkLen = 0;
+  chunkCount = 1;
+  bool bChunkCountIsOk = true;
+  QByteArray chunk;
+  for (int i = chunkCount; i < 16; ++i) // max. 15 chunks
+  {
+    bChunkCountIsOk = true;
+    chunkLen = (data.size() + i - 1) / i;
+
+    for (int j = 0; j < i; ++j)
+    {
+      const char fragByte = SfmMsgTetra_tx::fragmentByte(i, j + 1);
+      chunk.clear();
+      chunk += 0x02;  // a send counter which needs to be substituted
+      chunk += fragByte;
+      chunk += data.mid(j * chunkLen, chunkLen);
+
+      const int payloadLen = LtsUtils::lenMaskedData(chunk);
+      if (payloadLen > maxBytesPerChunk)
+      {
+        bChunkCountIsOk = false;
+        break;
+      }
+    }
+    if (bChunkCountIsOk)
+    {
+      chunkCount = i;
+      break;
+    }
+  }
+  return bChunkCountIsOk;
+}
+
+void WlsParticipant::sendBinarySfmData(const std::vector<unsigned char> &data)
+{
+  int chunkLen = 0;
+  int chunkCount = 0;
+
+  const QByteArray ba = LtsUtils::toQByteArray(data);
+  if (!calcChunkCountAndLen(ba, chunkCount, chunkLen))
+  {
+    WARNING_LOG(m_logger) << getPlayerIdStr()
+                          << ": Internal error calculating chunk size for payload size " << data.size();
+    return;
+  }
+
+  WlsPayloadData payload;
+  payload.fragmentCount = chunkCount;
+  payload.fragmentNumber = 0;
+  int iPos = 0;
+  while (iPos < ba.size())
+  {
+    ++payload.fragmentNumber;
+    payload.payload = ba.mid(iPos, chunkLen);
+    m_wlsQueue.enqueue(payload);
+    iPos += chunkLen;
+  }
+
+  QTimer::singleShot(0, this, &WlsParticipant::checkSendTele);
+}
+
+void WlsParticipant::handleWlsAck(const Acknowledge_rx *ack)
+{
+  // the ack recv counter must match our send counter
+  if (ack->recvCounter() != m_nextSendCounter)
+  {
+    WARNING_LOG(m_logger) << "Got ack for telegram with counter " << int(ack->recvCounter())
+                          << " but expected " << int(m_nextSendCounter) << " - ignoring";
+    return;
+  }
+  if (ack->isOk())
+  {
+    if (m_wlsQueue.isEmpty())
+    {
+      ERROR_LOG(m_logger) << "got an ack but nothing was sent - something went terribly wrong - resetting send/recv counter";
+      m_nextSendCounter = 0;
+      m_lastRecvCounter = 0;
+      return;
+    }
+    const WlsPayloadData &h = m_wlsQueue.head();
+    if (h.fragmentCount > 1)
+    {
+      if (h.fragmentCount == h.fragmentNumber)
+        m_currentFragmentedTeleQueue.clear(); // sent complete fragmented telegram - can throw away the segments
+      else
+        m_currentFragmentedTeleQueue.enqueue(h);  // save for possible resend
+    }
+    m_wlsQueue.dequeue();
+    m_wlsTeleTimeoutTimer->stop();
+  }
+  else
+  {
+    // if we sent a fragmented telegram then we've to start over from the beginning
+    while (!m_currentFragmentedTeleQueue.isEmpty())
+      m_wlsQueue.push_front(m_currentFragmentedTeleQueue.dequeue());
+  }
+  checkSendTele();
+}
+
+void WlsParticipant::checkSendTele()
+{
+  if (m_wlsTeleTimeoutTimer->isActive())
+    return;
+  sendNextWlsTelegram(false);
+}
+
+void WlsParticipant::sendNextWlsTelegram(bool bIsResend)
+{
+  if (m_wlsQueue.isEmpty())
+    return;
+  if (m_dataCommunication == WlsParticipant::DataCommunication::Off)
+  {
+    if (bIsResend)
+    {
+      // when we try to resend due to timeout but now we know that there is no
+      // data communication anymore -> throw away
+      m_wlsQueue.clear();
+      return;
+    }
+    WARNING_LOG(m_logger) << getPlayerIdStr()
+                          << ": Trying to send a wls telegram but no data communication possible";
+    // should not happen so leave the assertion to find out the reasons for it
+    Q_ASSERT(false);
+    m_wlsQueue.clear();
+    return;
+  }
+  const auto &tele = m_wlsQueue.head();
+  SfmMsgTetra_tx tx(getNextSendCounter(bIsResend),
+                    tele.fragmentCount,
+                    tele.fragmentNumber,
+                    tele.payload);
+  emit dataForWls(this, tx);
+  if (!bIsResend)
+    m_wlsTeleTimeoutCounter = 0;
+  m_wlsTeleTimeoutTimer->start();
+}
+
+void WlsParticipant::onWlsTeleTimeout()
+{
+  ++m_wlsTeleTimeoutCounter;
+  DEBUG_LOG(m_logger) << getPlayerIdStr()
+                      << ": Resend WLS connection to participant " << m_equipmentNumber
+                      << "(" << m_tagId << ")"
+                      << " m_wlsTeleTimeoutCounter =" << m_wlsTeleTimeoutCounter
+                      << " s_maxWlsRetryCount=" << s_maxWlsRetryCount;
+  if (m_wlsTeleTimeoutCounter == s_maxWlsRetryCount)
+  {
+    WARNING_LOG(m_logger) << getPlayerIdStr()
+                          << ": Lost WLS connection to participant " << m_equipmentNumber
+                          << "(" << m_tagId << ")";
+    m_lastSeenDataCommunication = -1;
+    m_wlsTeleTimeoutCounter = 0;
+    m_nextSendCounter = 0;
+    m_lastRecvCounter = 0;
+    checkGpsOnOffTimeoutTimer(QDateTime::currentMSecsSinceEpoch());
+    return;
+  }
+  sendNextWlsTelegram(true);
+}
+
+uint8_t WlsParticipant::getNextSendCounter(bool bIsResend)
+{
+  if (bIsResend)
+    return m_nextSendCounter;
+  ++m_nextSendCounter;
+  if (m_nextSendCounter == 0)
+    ++m_nextSendCounter;
+  return m_nextSendCounter;
+}
+
+bool WlsParticipant::isRecvCounterCorrect(uint8_t recvCounter)
+{
+  // same as in tetra - the counter may not be monotonically ascending ...
+  bool bRet = ((recvCounter == 0 && m_lastRecvCounter == 0) ||
+               recvCounter != m_lastRecvCounter);
+  m_lastRecvCounter = recvCounter;
+  return bRet;
+}
+
+TrainingAreaBase::TetraIcd::WlsParticipantState WlsParticipant::wlsParticipantState() const
+{
+  TrainingAreaBase::TetraIcd::WlsParticipantState ps;
+  ps.equipmentNumber = m_equipmentNumber;
+  ps.inBlockedArea = false;
+  ps.gpsOn = isGpsMode();
+  ps.lastSeen = m_lastSeenDataCommunication;
+  ps.tacticalState = -1;
+  ps.tagId = QString::fromLatin1(tagId());
+  ps.voltage = voltage;
+
+  ps.relativePosition.x = m_relativePosition.x;
+  ps.relativePosition.y = m_relativePosition.y;
+  ps.relativePosition.z = m_relativePosition.z;
+
+  ps.relativePosition.wlsAreaId = m_relativePosition.wlsAreaId;
+  ps.relativePosition.wlsZone = m_wlsZone;
+  ps.relativePosition.wlsBuildingId = m_relativePosition.wlsBuildingId;
+  ps.relativePosition.wlsRoomNumber = m_relativePosition.wlsRoomNumber;
+  ps.relativePosition.playerOrientation = m_relativePosition.playerOrientation;
+  ps.relativePosition.weaponOrientation = m_relativePosition.weaponOrientation;
+  ps.relativePosition.isValid = m_relativePosition.isValid;
+
+  return ps;
+}
+
+std::ostream &operator<<(std::ostream &stream, WlsParticipant::GpsMode gm)
+{
+  switch (gm)
+  {
+  case WlsParticipant::GpsMode::On:
+    return stream << "GpsMode::On";
+  case WlsParticipant::GpsMode::Off:
+    return stream << "GpsMode::Off";
+  }
+  return stream << "Unknown GpsMode (" << std::to_string(int(gm)) << ")";
+}
+
+std::ostream &operator<<(std::ostream &stream, WlsParticipant::DataCommunication dc)
+{
+  switch (dc)
+  {
+  case WlsParticipant::DataCommunication::Unknown:
+    return stream << "DataCommunication::Unknown";
+  case WlsParticipant::DataCommunication::On:
+    return stream << "DataCommunication::On";
+  case WlsParticipant::DataCommunication::Off:
+    return stream << "DataCommunication::Off";
+  }
+  return stream << "Unknown DataCommunication (" << std::to_string(int(dc)) << ")";
+}
+
+} // namespace WirelessLocationSystem