qt5-legatus-legacy/Lts/Proxies/DifferentialGPS/tetra_if.cpp

/**
  * $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