qt-migration-legatus/qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBaseOC/connection_handler_tcp_ttci.cpp

#include "connection_handler_tcp_ttci.h"

#include <QCoreApplication>
#include <QDataStream>
#include <QTimer>
#include <QTcpServer>

#include <QTimer>

using namespace NGL;

namespace TrainingAreaBaseOC
{
ConnectionHandlerTcpTtci::ConnectionHandlerTcpTtci()
  : m_messageDataStart(true)
  , m_wirkfeldDataStart(true)
  , m_wirkfeldMultiplex(0)
  , m_sendingData(false)
  , m_segmentToggle(0)
  , m_canDevice(new Device(this))
  , m_canChannel(new Channel(QLatin1Char('@'), true, this))
{
  connect(m_canChannel, &Channel::dataAvailable, this, &ConnectionHandlerTcpTtci::slotReadCanFrame);
  connect(m_canChannel, &Channel::disconnectedFromEq, this, &ConnectionHandlerTcpTtci::disconnectedFromEq);
}

ConnectionHandlerTcpTtci::~ConnectionHandlerTcpTtci()
{
  delete m_canChannel;
  delete m_canDevice;
  qDeleteAll(m_sfmMessages);
}

bool ConnectionHandlerTcpTtci::init()
{
  initCanSocket();
  return true;
}


void ConnectionHandlerTcpTtci::initCanSocket()
{
  ERROR_LOG() << "InitCantSOcket";

  m_canChannel->setDevice(m_canDevice);
  m_canDevice->open();
}


bool ConnectionHandlerTcpTtci::findNextSyncByte(int &iCurReadPos) const
{
  if (iCurReadPos == m_buffer.size())
  {
    DEBUG_LOG() << "No more data available";
    return false;
  }
  DEBUG_LOG() << "Searching for next sync byte, start at: " << iCurReadPos;
  NGL::NGLLoggerHelper::hexDumpHelper(m_logger, NGL::LogLevel::Debug, m_buffer);

  while (iCurReadPos < m_buffer.size())
  {
    static const unsigned char syncByte = 0xAA;
    const unsigned char curByte = m_buffer.at(iCurReadPos);
    if (curByte == syncByte)
      break;
    ++iCurReadPos;
  }
  
  if (m_buffer.size() < 4 + iCurReadPos)
  {
    DEBUG_LOG() << "Could not find sync byte in stream";
    return false;
  }
  return true;
}

void ConnectionHandlerTcpTtci::slotReadCanFrame(const QByteArray &data)
{
  m_buffer += data;

  int iCurReadPos = 0;
  while (findNextSyncByte(iCurReadPos))
  {
    DEBUG_LOG() << "Found Sync byte at " << iCurReadPos;

    
    unsigned int len = (m_buffer.at(iCurReadPos + 1) & 0xF0) >> 4;
    DEBUG_LOG() << "Payload size: " << len;

    if (len > 8)
    {
      
      switch (len)
      {
      case 9:
        
        DEBUG_LOG() << "Read status data - skipping ";
        iCurReadPos += 4;
        break;
      case 11:
        
        DEBUG_LOG() << "Read communication SFM - PAN - skipping ";
        iCurReadPos += 5;
        break;
      case 12:
        
        DEBUG_LOG() << "Read parameter request - skipping ";
        iCurReadPos += 5;
        break;
      default:
        
        WARNING_LOG() << "Invalid payload size (" << len << " - skipping";
        ++iCurReadPos;
        break;
      }
      continue;
    }
    
    
    
    
    int neededBytes = 4 + len + iCurReadPos;
    if (m_buffer.size() < neededBytes)
    {
      INFO_LOG() << "To few bytes in buffer - waiting for more (want: "
                         << neededBytes << " have: " << m_buffer.size() << ")";
      
      break;
    }

    unsigned int id = (m_buffer.at(iCurReadPos + 1) & 0x07);
    unsigned char id2 = m_buffer.at(iCurReadPos + 2);
    id = ((id << 8) + id2);
    bool rtr = ((m_buffer.at(iCurReadPos + 1) & 0x08) == 0x08);
    DEBUG_LOG() << "CAN ID: " << id << " rtr: " << rtr;

    
    unsigned char crc = 0;
    for (unsigned int i = 0; i < len + 3; ++i)
      crc += m_buffer.at(iCurReadPos + i);
    crc = ~crc;

    unsigned char crcSent = m_buffer.at(iCurReadPos + len + 3);
    if (crc != crcSent)
    {
      
      WARNING_LOG() << "CRC checksum missmatch (is: " << crcSent
                            << " - calculated: " <<  crc << ") - skipping";
      iCurReadPos++;
      continue;
    }

    QByteArray data2(m_buffer.data() + iCurReadPos + 3, len);
    
    LtsUtils::SfmMessage msg;
    msg.setId(id);
    msg.setPayload(data2);
    msg.setRtr(rtr);

    emit received(msg);

    
    iCurReadPos += 4 + len;

  }
  m_buffer = m_buffer.mid(iCurReadPos);

}

bool ConnectionHandlerTcpTtci::isOpen()
{
  return m_canDevice->isOpen();
}

bool ConnectionHandlerTcpTtci::send(const LtsUtils::SfmMessage &message)
{
  if (message.payload().size() > 8)
    return false;

  INFO_LOG() << "start write";
  NGL::NGLLoggerHelper::hexDumpHelper(m_logger, NGL::LogLevel::Debug, message.payload());

  QByteArray data(message.payload().size() + 4, Qt::Uninitialized);
  data[0] = 0xAA;

  data[1] = (message.payload().size() << 4) | ((message.rtr() & 0x1) << 3) | ((message.id() & 0x700) >> 8);
  data[2] = (message.id() & 0xFF);

  for (int i = 0; i < message.payload().size(); ++i)
    data[i + 3] = message.payload().at(i);

  unsigned char crc = 0;
  for (int i = 0; i < data.size() - 1; ++i)
    crc += data.at(i);
  crc = ~crc;
  data[message.payload().size() + 3] = crc;
  INFO_LOG() << "end  write ";

  if (!m_canChannel->writeData(data))
    ERROR_LOG() << "Error writing data to can channel";

  return true;
}

}