Initial commit

Lts/Proxies/DifferentialGPS/tetra_if.cpp

@@ -0,0 +1,248 @@
+/**
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  */
+#include "tetra_if.h"
+#include "dgps_config.h"
+
+#include <NGLLogger/NGLLoggerBootstrap.h>
+#include <QTcpSocket>
+#include <QHostAddress>
+#include <QNetworkInterface>
+#include <QTimer>
+
+#include <CoPrtCnv/CoPrtCnvRadioTelegram.hpp>
+#include <CoPrtCnv/CoPrtCnvTelegramConverter.hpp>
+#include <CoPrtCnv/CoPrtCnvHexDumpHelper.hpp>
+
+namespace DifferentialGPS
+{
+void TetraIF::startThreadInternal(int threadIndex)
+{
+  const DgpsConfig::TetraConfig_t &cfg = DgpsConfig::instance().getTetraConfig();
+
+  m_srcIssi             = cfg.issi[threadIndex];
+  m_broadcastIssi       = cfg.broadcastIssi[threadIndex];
+  m_feedbackTimeout_ms  = cfg.feedbackTimeout_ms;
+  TcpSocket::startThreadInternal(cfg.ip[threadIndex], cfg.port[threadIndex]);
+}
+
+
+void TetraIF::run()
+{
+  m_feedbackTimeout = new QTimer(this);
+  m_feedbackTimeout->setSingleShot(true);
+  connect(m_feedbackTimeout, &QTimer::timeout,
+          this, &TetraIF::onFeedbackTimeout);
+
+  TcpSocket::run();
+
+  m_feedbackTimeout->disconnect(this);
+  delete m_feedbackTimeout;
+  m_feedbackTimeout = nullptr;
+}
+
+void TetraIF::onNewDataAvailable()
+{
+  if (m_socket->state() != QAbstractSocket::ConnectedState)
+  {
+    DEBUG_LOG(m_logger) << "Can not send new data to Tetra - not yet connected ("
+                        << m_socket->state() << ")";
+    return;
+  }
+  if (!m_bTetraOk)
+  {
+    DEBUG_LOG(m_logger) << "Can not send new data to Tetra - not yet logged in";
+    return;
+  }
+  if (onNewDataAvailableInternal())
+  {
+    // feedback expected; start feedback timeout timer
+    m_feedbackTimeout->start(m_feedbackTimeout_ms);
+
+  }
+}
+
+void TetraIF::onFeedbackTimeout()
+{
+  ERROR_LOG(m_logger) << "Tetra feedback timeout"
+                      << m_socket->peerAddress().toString()
+                      << ":" << m_socket->peerPort();
+
+  m_socket->disconnectFromHost();
+  if (m_socket->state() != QAbstractSocket::UnconnectedState)
+    m_socket->waitForDisconnected();
+
+  exit(3);
+}
+
+bool TetraIF::checkTetraConnecting()
+{
+  return m_bTetraOk;
+}
+
+void TetraIF::doInitialLogin()
+{
+  N_CoPrtCnv::CoPrtCnvRadioTelegram tele(m_srcIssi);
+  m_socket->write(tele.data());
+  m_socket->flush();
+}
+
+void TetraIF::onConnected()
+{
+  TcpSocket::onConnected();
+
+  m_counter = 1;
+  doInitialLogin();
+}
+
+void TetraIF::onDisconnected()
+{
+  INFO_LOG(m_logger) << "Disconnected from " << m_socket->peerAddress().toString()
+                     << ":" << m_socket->peerPort();
+
+  exit(3); // stop the thread
+}
+
+void TetraIF::onReadyRead()
+{
+  do
+  {
+    DEBUG_LOG(m_logger) << "Reading " << m_socket->bytesAvailable()
+                        << " from " << m_socket->peerAddress().toString()
+                        << ":" << m_socket->peerPort();
+
+    // read the 4-byte frame-header for find out the length of whole telegram
+    N_CoPrtCnv::CoPrtCnvRadioTelegram::PrtCnvRadioGwHeader header;
+    if (m_socket->bytesAvailable() < (int)sizeof(header))
+      return;
+
+    m_socket->peek((char *)&header, sizeof(header));
+    if (N_CoPrtCnv::CoPrtCnvRadioTelegram::MAGIC_NUMBER != header.m_MagicNumber)
+    {
+      ERROR_LOG(m_logger) << "Incorrect magic number (0x"
+                          << (int)header.m_MagicNumber
+                          << " - aborting";
+      exit(2);
+    }
+    if (!N_CoPrtCnv::CoPrtCnvRadioTelegram::isTelegramLengthOk(header.m_Length))
+    {
+      ERROR_LOG(m_logger) <<  "Radio-Telegram length to big: "
+                          << header.m_Length
+                          << ", but I proceed with this telegram.";
+    }
+
+    quint16 dataSize = header.m_Length + sizeof(header);
+    if (m_socket->bytesAvailable() < dataSize)
+    {
+      INFO_LOG(m_logger) << "not enough bytes available";
+      return;
+    }
+    uint8_t *data = new uint8_t[dataSize];
+    m_socket->read((char *)data, dataSize);
+
+    handleReceivedTelegram(data, dataSize);
+
+  }
+  while (m_socket->bytesAvailable());
+}
+
+void TetraIF::onError(QAbstractSocket::SocketError err)
+{
+  ERROR_LOG(m_logger) << "Socket error: " << err
+                      << " " << m_socket->errorString()
+                      << ":" << m_socket->peerPort();
+  m_socket->disconnectFromHost();
+  if (m_socket->state() != QAbstractSocket::UnconnectedState)
+    m_socket->waitForDisconnected();
+
+  exit(3);
+}
+
+void TetraIF::handleReceivedTelegram(uint8_t *a_data, quint16 a_dataSize)
+{
+  if (a_dataSize < 4)
+  {
+    ERROR_LOG(m_logger) << "To few bytes for handle receieved telegramm ("
+                        << a_dataSize << ")";
+    return;
+  }
+  switch (a_data[4])
+  {
+  // response to register indication
+  case N_CoPrtCnv::CoPrtCnvRadioTelegram::e_DataIndReq:
+    INFO_LOG(m_logger) << "Got data from Tetra ...";
+    if (m_feedbackTimeout->isActive())
+      m_feedbackTimeout->stop();
+    break;
+  case N_CoPrtCnv::CoPrtCnvRadioTelegram::e_RegisterRsp:
+    if (a_dataSize < 5)
+    {
+      ERROR_LOG(m_logger) << "To few bytes for handle register response telegramm ("
+                          << a_dataSize << ")";
+      return;
+    }
+    handleRegisterRsp(a_data[5]); // registration status
+    break;
+  default:
+    INFO_LOG(m_logger) << "Radio-Telegram with PDU-Type="
+                       << (int)a_data[4]
+                       << " ignored";
+    break;
+  }
+}
+
+void TetraIF::handleRegisterRsp(const uint8_t a_registrationStatus)
+{
+  std::stringstream logText;
+  switch (a_registrationStatus)
+  {
+  case N_CoPrtCnv::CoPrtCnvRadioTelegram::e_IssiSuccessfulRegistered:
+    INFO_LOG(m_logger) << "Valid register response from Tetra";
+    m_bTetraOk = true;
+    onAddGroupMonitoring();
+    onNewDataAvailable();
+    return;
+  case N_CoPrtCnv::CoPrtCnvRadioTelegram::e_DuplicateISSI:
+    logText << "TETRA: Register-Response: Status=Duplicate ISSI";
+    break;
+  case N_CoPrtCnv::CoPrtCnvRadioTelegram::e_InvalidISSI:
+    logText << "TETRA: Register-Response: Status=Invalid ISSI";
+    break;
+  case N_CoPrtCnv::CoPrtCnvRadioTelegram::e_InvalidIpAddress:
+    logText << "TETRA: Register-Response: Status=Invalid IP address => Configure TETRA for accepting our IP-address (";
+    {
+      const QList<QHostAddress> ipList = QNetworkInterface::allAddresses();
+      QList<QHostAddress>::const_iterator iter = ipList.begin();
+      while (iter != ipList.end())
+      {
+        logText << (*iter).toString().toStdString() << ", ";
+        ++iter;
+      }
+      logText << ")...";
+    }
+    break;
+  default:
+    logText << "Unknown registration status: " << (int)a_registrationStatus;
+    break;
+  }
+
+  ERROR_LOG(m_logger) << logText.str();
+  exit(2);
+}
+
+void TetraIF::onAddGroupMonitoring()
+{
+  std::set<uint32_t> groupsToMonitor;
+  groupsToMonitor.insert(m_broadcastIssi);
+  N_CoPrtCnv::CoPrtCnvRadioTelegram tele(m_srcIssi, groupsToMonitor);
+  tele.setZaehler(m_counter++);
+
+  m_socket->write(tele.data());
+  m_socket->flush();
+
+  NGL::NGLLoggerHelper::hexDumpHelper(m_logger, NGL::LogLevel::Debug, tele.data());
+
+}
+} // namespace DifferentialGPS