1040 lines
27 KiB
C++
Executable File
1040 lines
27 KiB
C++
Executable File
|
|
|
|
|
|
#include "canag_handler.h"
|
|
|
|
#include <LtsUtils/LtsUtilsCore/dds_vector.h>
|
|
#include <LtsUtilsCore/qbytearray_byteaccess.h>
|
|
#include <NGLLogger/NGLLoggerBootstrap.h>
|
|
|
|
#include "connection_handler.h"
|
|
#include "main_handler.h"
|
|
#include "settings.h"
|
|
|
|
namespace TrainingAreaBaseOC
|
|
{
|
|
|
|
TxDdlHandler::TxDdlHandler() : m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger())
|
|
, m_state(DdlState::NotStarted)
|
|
, m_segmentToggle(0)
|
|
, m_sentCnt(0)
|
|
, m_responseTimer(new QTimer(this)) {
|
|
m_responseTimer->setInterval(1000);
|
|
m_responseTimer->setSingleShot(true);
|
|
connect(m_responseTimer, &QTimer::timeout, this, &TxDdlHandler::sendAbortOrRestartTimeout);
|
|
}
|
|
|
|
void TxDdlHandler::addData(const QByteArray &data)
|
|
{
|
|
if (data.size() == 0)
|
|
return;
|
|
|
|
DdlData ddlData;
|
|
ddlData.data = data;
|
|
|
|
m_dataToSend.append(ddlData);
|
|
|
|
startNextDomainDownload();
|
|
}
|
|
|
|
void TxDdlHandler::process(const QByteArray &data)
|
|
{
|
|
m_responseTimer->stop();
|
|
|
|
qint32 scs = ((data[0] & 0xE0) >> 5);
|
|
switch (scs)
|
|
{
|
|
case 1:
|
|
{
|
|
DEBUG_LOG(m_logger) << "Segment download confirmation received";
|
|
|
|
qint32 toggle = ((data[0] & 0x10) >> 4);
|
|
|
|
if (m_segmentToggle != toggle)
|
|
{
|
|
ERROR_LOG(m_logger) << "Toggle bit of confirmation not equal to request";
|
|
sendAbort(130);
|
|
}
|
|
|
|
(0 == m_segmentToggle) ? m_segmentToggle = 1 : m_segmentToggle = 0;
|
|
domainDownload();
|
|
break;
|
|
}
|
|
case 3:
|
|
{
|
|
DEBUG_LOG(m_logger) << "Initiate download confirmation received";
|
|
|
|
if (m_dataToSend.isEmpty())
|
|
break;
|
|
|
|
if (DdlState::Initiated != m_state)
|
|
{
|
|
ERROR_LOG(m_logger) << "Initiate download confirmation received but ddl wasn't startet yet.";
|
|
sendAbort();
|
|
break;
|
|
}
|
|
domainDownload();
|
|
break;
|
|
}
|
|
case 4:
|
|
{
|
|
WARNING_LOG(m_logger) << "Error during domain download";
|
|
|
|
m_state = DdlState::NotStarted;
|
|
if (m_dataToSend.isEmpty())
|
|
break;
|
|
|
|
m_dataToSend.first().offset = 0;
|
|
startNextDomainDownload();
|
|
break;
|
|
}
|
|
default:
|
|
|
|
ERROR_LOG(m_logger) << "unknown scs value received: " << scs;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void TxDdlHandler::startNextDomainDownload()
|
|
{
|
|
if (m_dataToSend.isEmpty() || (DdlState::NotStarted != m_state))
|
|
return;
|
|
|
|
initiateDomainDownload();
|
|
}
|
|
|
|
void TxDdlHandler::sendAbortOrRestartTimeout()
|
|
{
|
|
if (m_state != DdlState::NotStarted)
|
|
sendAbort();
|
|
else
|
|
startNextDomainDownload();
|
|
}
|
|
|
|
void TxDdlHandler::initiateDomainDownload()
|
|
{
|
|
if (m_dataToSend.isEmpty())
|
|
{
|
|
ERROR_LOG(m_logger) << "::initiateDomainDownload called, but no data to send available";
|
|
return;
|
|
}
|
|
|
|
if (m_sentCnt >= s_maxResendCnt)
|
|
{
|
|
ERROR_LOG(m_logger) << "Maximum trys of resending DDL exceeded. ";
|
|
m_dataToSend.removeFirst();
|
|
m_sentCnt = 0;
|
|
startNextDomainDownload();
|
|
return;
|
|
}
|
|
|
|
quint32 dataSize = m_dataToSend.first().data.size();
|
|
|
|
QByteArray data(8, '\0');
|
|
data[0] = 0x21;
|
|
data[4] = (dataSize & 0xFF);
|
|
data[5] = ((dataSize >> 8) & 0xFF);
|
|
data[6] = ((dataSize >> 16) & 0xFF);
|
|
data[7] = ((dataSize >> 24) & 0xFF);
|
|
|
|
DEBUG_LOG(m_logger) << "Initiate DDL Req";
|
|
|
|
m_state = DdlState::Initiated;
|
|
m_sentCnt++;
|
|
emit send(data);
|
|
m_responseTimer->start();
|
|
}
|
|
|
|
void TxDdlHandler::domainDownload()
|
|
{
|
|
if (m_dataToSend.isEmpty())
|
|
{
|
|
ERROR_LOG(m_logger) << "::domainDownload called, but no data to send available";
|
|
return;
|
|
}
|
|
|
|
|
|
|
|
if (m_dataToSend.first().data.size() <= m_dataToSend.first().offset)
|
|
{
|
|
m_dataToSend.removeFirst();
|
|
m_sentCnt = 0;
|
|
m_state = DdlState::NotStarted;
|
|
startNextDomainDownload();
|
|
return;
|
|
}
|
|
|
|
QByteArray data(8, '\0');
|
|
|
|
qint32 numFreeBytes = 0;
|
|
qint32 endBit = 0;
|
|
|
|
qint32 dataSize = m_dataToSend.first().data.size();
|
|
qint32 offset = m_dataToSend.first().offset;
|
|
qint32 newOffset = m_dataToSend.first().offset + 7;
|
|
|
|
if (0 == offset)
|
|
m_segmentToggle = 0;
|
|
|
|
if (dataSize > newOffset)
|
|
{
|
|
numFreeBytes = 0;
|
|
endBit = 0;
|
|
}
|
|
else if (dataSize == newOffset)
|
|
{
|
|
numFreeBytes = 0;
|
|
endBit = 1;
|
|
}
|
|
else
|
|
{
|
|
numFreeBytes = 7 - (dataSize - offset);
|
|
endBit = 1;
|
|
}
|
|
|
|
data[0] = (((m_segmentToggle << 4) & 0x10) | ((numFreeBytes << 1) & 0x0E) | (endBit & 0x01));
|
|
|
|
for (qint32 i = 0; i < 7; i++)
|
|
{
|
|
if (m_dataToSend.first().data.size() >= offset + i)
|
|
data[i + 1] = m_dataToSend.first().data[offset + i];
|
|
}
|
|
|
|
m_dataToSend.first().offset = newOffset;
|
|
|
|
DEBUG_LOG(m_logger) << "Segment DDL Req";
|
|
m_state = DdlState::Downloading;
|
|
|
|
emit send(data);
|
|
m_responseTimer->start();
|
|
}
|
|
|
|
void TxDdlHandler::sendAbort(quint8 error)
|
|
{
|
|
QByteArray data(8, '\0');
|
|
data[0] = 0x80;
|
|
data[1] = error;
|
|
DEBUG_LOG(m_logger) << "Abort DDL Req";
|
|
|
|
m_state = DdlState::NotStarted;
|
|
m_dataToSend.first().offset = 0;
|
|
emit send(data);
|
|
m_responseTimer->start();
|
|
}
|
|
|
|
RxDdlHandler::RxDdlHandler(bool isMultiplexed)
|
|
: m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger())
|
|
, m_isMultiplexed(isMultiplexed)
|
|
{}
|
|
|
|
void RxDdlHandler::process(const QByteArray &data)
|
|
{
|
|
unsigned int ccs = ((data[0] & 0xE0) >> 5);
|
|
|
|
switch (ccs)
|
|
{
|
|
|
|
case 1:
|
|
{
|
|
DEBUG_LOG(m_logger) << "initiate download received";
|
|
if (m_isMultiplexed)
|
|
m_multiplexer = (((data[1] & 0xFF) << 16) | ((data[2] & 0xFF) << 8) | (data[3] & 0xFF));
|
|
|
|
m_messageData.clear();
|
|
|
|
|
|
if (m_state == DdlState::NotStarted)
|
|
sendDPConfirmation(ccs, 0, m_multiplexer);
|
|
else
|
|
sendDPConfirmation(4, 0);
|
|
break;
|
|
}
|
|
|
|
case 0:
|
|
{
|
|
DEBUG_LOG(m_logger) << "download segment received";
|
|
if (m_state == DdlState::NotStarted)
|
|
{
|
|
WARNING_LOG(m_logger) << "download segment and data m_state == DdlState::NotStarted, something went wrong!";
|
|
sendDPConfirmation(4, 0);
|
|
return;
|
|
}
|
|
|
|
unsigned int toggle = ((data[0] & 0x10) >> 4);
|
|
unsigned int freebytes = ((data[0] & 0x0E) >> 1);
|
|
|
|
for (unsigned int i = 1; i < 8 - freebytes; i++)
|
|
m_messageData.append(data.at(i));
|
|
|
|
unsigned int dataend = (data[0] & 0x01);
|
|
|
|
|
|
sendDPConfirmation(ccs, toggle);
|
|
|
|
|
|
if (1 == dataend)
|
|
{
|
|
if (m_isMultiplexed)
|
|
emit receivedMultiplexed(m_messageData, m_multiplexer);
|
|
else
|
|
emit received(m_messageData);
|
|
m_state = DdlState::NotStarted;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case 4:
|
|
{
|
|
DEBUG_LOG(m_logger) << "abort download received";
|
|
m_messageData.clear();
|
|
m_state = DdlState::NotStarted;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
WARNING_LOG(m_logger) << "Unknown ccs ID: " << ccs;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void RxDdlHandler::sendDPConfirmation(const quint32 ccs, const quint32 toggle, const quint32 wirkfeldMultiplex)
|
|
{
|
|
QByteArray data(8, '\0');
|
|
|
|
|
|
if (1 == ccs)
|
|
{
|
|
|
|
data[0] = 0x60;
|
|
m_state = DdlState::Initiated;
|
|
}
|
|
else if (0 == ccs)
|
|
{
|
|
if (0 == toggle)
|
|
data[0] = 0x20;
|
|
else if (1 == toggle)
|
|
data[0] = 0x30;
|
|
else
|
|
WARNING_LOG(m_logger) << "Unknown toggle state: " << toggle;
|
|
m_state = DdlState::Downloading;
|
|
}
|
|
else if (4 == ccs)
|
|
{
|
|
data[0] = 0x80;
|
|
m_state = DdlState::NotStarted;
|
|
}
|
|
|
|
if (0 != wirkfeldMultiplex)
|
|
{
|
|
data[1] = ((wirkfeldMultiplex >> 16) & 0xFF);
|
|
data[2] = ((wirkfeldMultiplex >> 8) & 0xFF);
|
|
data[3] = (wirkfeldMultiplex & 0xFF);
|
|
}
|
|
emit send(data);
|
|
}
|
|
|
|
CanagHandler::CanagHandler(MainHandler *mainHandler)
|
|
: m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger())
|
|
, m_mainHandler(mainHandler)
|
|
, m_connectionHandler(mainHandler->connectionHandler())
|
|
, m_rxHandler(mainHandler->rxHandler())
|
|
, m_freeText(QByteArray())
|
|
, m_deviceMessageTimer(new QTimer(this))
|
|
, m_freeTextCollectionTimer(new QTimer(this))
|
|
, m_sfmAliveTimer(new QTimer(this))
|
|
, m_sfmConnectionCheckTimer(new QTimer(this))
|
|
, m_syncSocket(new QTcpSocket(this))
|
|
, m_technicalStateSfm(OcTelegrams::TechnicalStateSfm())
|
|
, m_sfmConnectionEstablished(false)
|
|
{}
|
|
|
|
CanagHandler::~CanagHandler()
|
|
{
|
|
delete m_deviceMessageTimer;
|
|
delete m_freeTextCollectionTimer;
|
|
delete m_sfmAliveTimer;
|
|
delete m_sfmConnectionCheckTimer;
|
|
qDeleteAll(m_txDdlHandler);
|
|
qDeleteAll(m_rxDdlHandler);
|
|
qDeleteAll(m_messages);
|
|
}
|
|
|
|
bool CanagHandler::init()
|
|
{
|
|
m_handlerAesData = std::make_shared<HandlerAesData>(m_mainHandler->dds(), m_mainHandler->aesDtc(), m_mainHandler->aesFtc(), m_mainHandler->pdmDtc());
|
|
m_handlerAesData->setUse2BytesForSingleFireDiameter(Settings::use2BytesForSingleFireDiameter());
|
|
|
|
if (!m_handlerAesData->init())
|
|
{
|
|
ERROR_LOG(m_logger) << "Could not initialise HandlerAesData.";
|
|
return false;
|
|
}
|
|
|
|
m_deviceMessageTimer->setSingleShot(true);
|
|
connect(m_deviceMessageTimer, &QTimer::timeout, this, &CanagHandler::sendDeviceMessage);
|
|
m_freeTextCollectionTimer->setInterval(1000);
|
|
connect(m_freeTextCollectionTimer, &QTimer::timeout, this, &CanagHandler::sendFreeText);
|
|
m_sfmAliveTimer->setSingleShot(true);
|
|
m_sfmAliveTimer->setInterval(6000);
|
|
connect(m_connectionHandler.get(), &ConnectionHandler::disconnectedFromEq, this, &CanagHandler::sfmAliveTimeout);
|
|
connect(m_sfmAliveTimer, &QTimer::timeout, this, &CanagHandler::sfmAliveTimeout);
|
|
m_sfmConnectionCheckTimer->setInterval(5000);
|
|
connect(m_sfmConnectionCheckTimer, &QTimer::timeout, this, &CanagHandler::sendConnectionTestSfm);
|
|
connect(m_connectionHandler.get(), &ConnectionHandler::received, this, &CanagHandler::handleSfmTelegram);
|
|
|
|
|
|
|
|
TxDdlHandler *txDdlHandler = new TxDdlHandler;
|
|
connect(txDdlHandler, &TxDdlHandler::send, this, &CanagHandler::sendOcExcon);
|
|
m_txDdlHandler.insert(CanId::OcExconDataTx, txDdlHandler);
|
|
|
|
txDdlHandler = new TxDdlHandler;
|
|
connect(txDdlHandler, &TxDdlHandler::send, this, &CanagHandler::sendFireSmokeMarkerExcon);
|
|
m_txDdlHandler.insert(CanId::FireSmokeMarkerExconDataTx, txDdlHandler);
|
|
|
|
|
|
|
|
RxDdlHandler *rxDdlHandler = new RxDdlHandler;
|
|
connect(rxDdlHandler, &RxDdlHandler::send, this, &CanagHandler::sendExconOc);
|
|
connect(rxDdlHandler, &RxDdlHandler::received, this, &CanagHandler::receivedExconOc);
|
|
m_rxDdlHandler.insert(CanId::ExconOcDataTx, rxDdlHandler);
|
|
|
|
rxDdlHandler = new RxDdlHandler;
|
|
connect(rxDdlHandler, &RxDdlHandler::send, this, &CanagHandler::sendExconFireSmokeMarker);
|
|
connect(rxDdlHandler, &RxDdlHandler::received, this, &CanagHandler::receivedExconFireSmokeMarker);
|
|
m_rxDdlHandler.insert(CanId::ExconFireSmokeMarkerDataTx, rxDdlHandler);
|
|
|
|
rxDdlHandler = new RxDdlHandler(true);
|
|
connect(rxDdlHandler, &RxDdlHandler::send, this, &CanagHandler::sendSfmAwes);
|
|
connect(rxDdlHandler, &RxDdlHandler::receivedMultiplexed, this, &CanagHandler::receivedSfmAwes);
|
|
m_rxDdlHandler.insert(CanId::AwesDataTx, rxDdlHandler);
|
|
|
|
fillMessageMap();
|
|
|
|
m_sfmConnectionCheckTimer->start();
|
|
openSocket();
|
|
return true;
|
|
}
|
|
|
|
bool CanagHandler::txData(const OcTelegrams::DemandCyclicPositions &data)
|
|
{
|
|
LtsUtils::SfmMessage msg;
|
|
msg.setId(CanId::DemandCyclicPosition);
|
|
|
|
QByteArray outData(1, '\0');
|
|
outData[0] = data.cycleTime;
|
|
msg.setPayload(outData);
|
|
|
|
send(msg);
|
|
return true;
|
|
}
|
|
|
|
bool CanagHandler::txData(const OcTelegrams::TechnicalStateOc &data)
|
|
{
|
|
LtsUtils::SfmMessage *msg = m_messages[CanId::TechnicalStateOc];
|
|
|
|
QByteArray outData(8, '\0');
|
|
unsigned int unixTime = m_mainHandler->dds()->currentPartitionTime() / 1000;
|
|
outData[0] = (unixTime >> 24 & 0xFF);
|
|
outData[1] = (unixTime >> 16 & 0xFF);
|
|
outData[2] = (unixTime >> 8 & 0xFF);
|
|
outData[3] = (unixTime >> 0 & 0xFF);
|
|
outData[7] = data.errorCode;
|
|
msg->setPayload(outData);
|
|
|
|
send(*msg);
|
|
return true;
|
|
}
|
|
|
|
bool CanagHandler::txData(const OcTelegrams::DemandSelftestSfm &data)
|
|
{
|
|
Q_UNUSED(data);
|
|
send(*m_messages[CanId::ParameterSfmTx]);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool CanagHandler::txData(const TrainingAreaBase::TetraIcd::OpaquePayloadDataBase &data)
|
|
{
|
|
switch (data.payloadType)
|
|
{
|
|
case TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::OC:
|
|
{
|
|
INFO_LOG(m_logger) << "adding DDL CanId::CAN_ID_DATENVERKEHR_SRT_KOPPELRECHNER_TX";
|
|
m_txDdlHandler[CanId::OcExconDataTx]->addData(LtsUtils::toQByteArray(data.payload));
|
|
break;
|
|
}
|
|
case TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::FireSmokeMarker:
|
|
{
|
|
INFO_LOG(m_logger) << "adding DDL CanId::CAN_ID_DATENVERKEHR_DMF_KOPPELRECHNER_TX";
|
|
m_txDdlHandler[CanId::FireSmokeMarkerExconDataTx]->addData(LtsUtils::toQByteArray(data.payload));
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
ERROR_LOG(m_logger) << "Tryed to send unhandled TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type " << static_cast<int>(data.payloadType);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void CanagHandler::handleSfmTelegram(const LtsUtils::SfmMessage &message)
|
|
{
|
|
if (message.id() == 0)
|
|
{
|
|
ERROR_LOG(m_logger) << "Error while receiving data from sfm.";
|
|
return;
|
|
}
|
|
|
|
DEBUG_LOG(m_logger) << "Received new CAN-Message with ID: " << message.id() << " RTR: " << message.rtr() << " data: " << message.payload().toHex(' ');
|
|
switch (message.id())
|
|
{
|
|
case CanId::DemandDeviceId:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::DemandDeviceId";
|
|
m_deviceMessageTimer->start();
|
|
break;
|
|
}
|
|
case CanId::Time:
|
|
{
|
|
DEBUG_LOG(m_logger) << "Received CanId::Time";
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::TimeSync outData;
|
|
|
|
uint32_t gpsTime;
|
|
LtsUtils::get4Byte(data, gpsTime);
|
|
outData.gpsTimeMsec = gpsTime * 1000ULL;
|
|
outData.timezone = data[4];
|
|
DEBUG_LOG(m_logger) << "Received " << gpsTime;
|
|
sendSyncTime(gpsTime);
|
|
|
|
m_rxHandler->rxData(outData);
|
|
break;
|
|
}
|
|
case CanId::EnterLeaveAes:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::EnterLeaveAes";
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::EnterLeaveAes outData;
|
|
|
|
|
|
|
|
bool is_NOT_OperationalArea;
|
|
|
|
outData.aesId = data.mid(0, 2).toUInt();
|
|
outData.aesEntered = (data[2] & 0x80) >> 7;
|
|
outData.nbcArea = (data[2] & 0x40) >> 6;
|
|
is_NOT_OperationalArea = (data[2] & 0x38) == 0;
|
|
outData.chargeTasCode = data[3];
|
|
outData.cAreaType = data[4];
|
|
|
|
if (is_NOT_OperationalArea)
|
|
m_rxHandler->rxData(outData);
|
|
break;
|
|
}
|
|
case CanId::ParameterOcRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::ParameterOcRx";
|
|
send(*m_messages[CanId::ParameterOcTx]);
|
|
break;
|
|
}
|
|
case CanId::TechnicalStateSfm:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::TechnicalStateSfm";
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::TechnicalStateSfm outData;
|
|
|
|
outData.sfmConnected = true;
|
|
|
|
|
|
|
|
uint32_t gpsTime;
|
|
LtsUtils::get4Byte(data, gpsTime);
|
|
outData.gpsTimeMsec = gpsTime * 10000ULL;
|
|
outData.batteryTest = (data[4] & 0x80) >> 7;
|
|
outData.radioAvailable = !((data[4] & 0x40) >> 6);
|
|
outData.manipulationEquipment = (data[4] & 0x20) >> 5;
|
|
outData.licensed = !((data[4] & 0x10) >> 4);
|
|
outData.manipulationSfm = (data[4] & 0x01);
|
|
outData.errorCode = data[5];
|
|
|
|
m_technicalStateSfm = outData;
|
|
|
|
m_rxHandler->rxData(outData);
|
|
break;
|
|
}
|
|
case CanId::TechnicalStateOc:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::TechnicalStateOc";
|
|
|
|
if (message.rtr())
|
|
send(*m_messages[CanId::TechnicalStateOc]);
|
|
break;
|
|
}
|
|
case CanId::ExconOcDataRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::ExconOcDataRx";
|
|
QByteArray data = message.payload();
|
|
m_rxDdlHandler[CanId::ExconOcDataTx]->process(data);
|
|
break;
|
|
}
|
|
case CanId::OcExconDataRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::OcExconDataRx";
|
|
QByteArray data = message.payload();
|
|
m_txDdlHandler[CanId::OcExconDataTx]->process(data);
|
|
break;
|
|
}
|
|
case CanId::ExconFireSmokeMarkerDataRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::ExconFireSmokeMarkerDataRx";
|
|
QByteArray data = message.payload();
|
|
m_rxDdlHandler[CanId::ExconFireSmokeMarkerDataTx]->process(data);
|
|
break;
|
|
}
|
|
case CanId::FireSmokeMarkerExconDataRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::FireSmokeMarkerExconDataRx";
|
|
QByteArray data = message.payload();
|
|
m_txDdlHandler[CanId::FireSmokeMarkerExconDataTx]->process(data);
|
|
break;
|
|
}
|
|
case CanId::AwesDataRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::AwesDataRx";
|
|
QByteArray data = message.payload();
|
|
m_rxDdlHandler[CanId::AwesDataTx]->process(data);
|
|
break;
|
|
}
|
|
case CanId::Position:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::Position";
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::OwnPosition outData;
|
|
|
|
|
|
outData.valid = ((data[0] & 0x80) == 0);
|
|
size_t pos = 2;
|
|
LtsUtils::get3Byte(data, pos, outData.northing);
|
|
LtsUtils::get3Byte(data, pos, outData.easting);
|
|
if (!outData.valid)
|
|
break;
|
|
outData.gpsTimeMsec = m_mainHandler->dds()->currentPartitionTime();
|
|
m_rxHandler->rxData(outData);
|
|
break;
|
|
}
|
|
case CanId::DisplayMessage:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::DisplayMessage";
|
|
|
|
handleDisplayMessage(message);
|
|
|
|
break;
|
|
}
|
|
case CanId::SpecificsExercise:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::SpecificsExercise";
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::SpecificsExercise outData;
|
|
|
|
size_t pos = 0;
|
|
LtsUtils::get2Byte(data, pos, outData.exerciseId);
|
|
outData.zone = data[2];
|
|
outData.band = data[3];
|
|
outData.square = data.mid(4, 2);
|
|
pos = 6;
|
|
LtsUtils::get2Byte(data, pos, outData.easting);
|
|
outData.easting *= 1000;
|
|
LtsUtils::get2Byte(data, pos, outData.northing);
|
|
outData.northing *= 1000;
|
|
|
|
Settings::baseCoord(QString::number(outData.zone) +
|
|
QLatin1Char(outData.band) +
|
|
QString::fromLatin1(outData.square) +
|
|
QString::number(outData.northing).rightJustified(7, QLatin1Char('0')) +
|
|
QString::number(outData.easting).rightJustified(7, QLatin1Char('0')));
|
|
|
|
m_handlerAesData->specificsExercise(outData);
|
|
|
|
m_rxHandler->rxData(outData);
|
|
break;
|
|
}
|
|
case CanId::SpecificsOc:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::SpecificsOc";
|
|
|
|
if (message.rtr())
|
|
send(*m_messages[CanId::SpecificsOc]);
|
|
break;
|
|
}
|
|
case CanId::SpecificsEquipment:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::SpecificsEquipment";
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::SpecificsEquipment outData;
|
|
|
|
|
|
LtsUtils::get2Byte(data, outData.equipmentNumber);
|
|
outData.playerType = data[2];
|
|
outData.language = static_cast<TrainingAreaBase::TetraIcd::Language>(data.at(3));
|
|
outData.pyrotechnics = (data[4] & 0x80) >> 7;
|
|
outData.voiceOutput = (data[4] & 0x40) >> 6;
|
|
outData.sfm = (data[4] & 0x30) >> 4;
|
|
outData.panCoordinator = (data[4] & 0x0C) >> 2;
|
|
outData.viss = (data[4] & 0x03);
|
|
outData.geopairing = (data[5] & 0xC0) >> 6;
|
|
outData.ecu = (data[5] & 0x30) >> 4;
|
|
outData.hsu = (data[5] & 0x0C) >> 2;
|
|
outData.halt = (data[5] & 0x03);
|
|
|
|
outData.exerciseBreak = (data[5] & 0x02);
|
|
|
|
m_rxHandler->rxData(outData);
|
|
break;
|
|
}
|
|
case CanId::ParameterSfmRx:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::ParameterSfmRx";
|
|
|
|
break;
|
|
}
|
|
case CanId::ConnectionTestSfmReply:
|
|
{
|
|
INFO_LOG(m_logger) << "Received CanId::TestConnectionSfmReply";
|
|
if (!m_sfmConnectionEstablished)
|
|
sendDemandDeviceId();
|
|
else
|
|
m_sfmAliveTimer->start();
|
|
|
|
break;
|
|
}
|
|
default:
|
|
DEBUG_LOG(m_logger) << "message id " << message.id() << " is not handled!";
|
|
break;
|
|
}
|
|
}
|
|
|
|
void CanagHandler::sendDemandDeviceId()
|
|
{
|
|
INFO_LOG(m_logger) << "Requesting Device ID.";
|
|
send(*m_messages[CanId::DemandDeviceId]);
|
|
m_deviceMessageTimer->start();
|
|
}
|
|
|
|
void CanagHandler::sendDeviceMessage()
|
|
{
|
|
INFO_LOG(m_logger) << "Sending Device ID.";
|
|
send(*m_messages[CanId::DeviceId]);
|
|
if (!m_technicalStateSfm.sfmConnected)
|
|
demandInitialSfmData();
|
|
}
|
|
|
|
void CanagHandler::sendOcExcon(const QByteArray &data)
|
|
{
|
|
send(CanId::OcExconDataTx, data);
|
|
}
|
|
|
|
void CanagHandler::sendFireSmokeMarkerExcon(const QByteArray &data)
|
|
{
|
|
send(CanId::FireSmokeMarkerExconDataTx, data);
|
|
}
|
|
|
|
void CanagHandler::sendExconOc(const QByteArray &data)
|
|
{
|
|
send(CanId::ExconOcDataTx, data);
|
|
}
|
|
|
|
void CanagHandler::sendExconFireSmokeMarker(const QByteArray &data)
|
|
{
|
|
send(CanId::ExconFireSmokeMarkerDataTx, data);
|
|
}
|
|
|
|
void CanagHandler::sendSfmAwes(const QByteArray &data)
|
|
{
|
|
send(CanId::AwesDataTx, data);
|
|
}
|
|
|
|
void CanagHandler::sendConnectionTestSfm()
|
|
{
|
|
if (m_connectionHandler->isOpen())
|
|
{
|
|
m_sfmAliveTimer->start();
|
|
send(*m_messages[CanId::ConnectionTestSfm]);
|
|
}
|
|
|
|
}
|
|
|
|
void CanagHandler::receivedExconOc(const QByteArray &data)
|
|
{
|
|
TrainingAreaBase::TetraIcd::OpaquePayloadDataOut outData;
|
|
outData.payloadType = TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::OC;
|
|
outData.payload = LtsUtils::convert<std::vector<unsigned char>>(data);
|
|
|
|
m_rxHandler->rxData(outData);
|
|
}
|
|
|
|
void CanagHandler::receivedExconFireSmokeMarker(const QByteArray &data)
|
|
{
|
|
TrainingAreaBase::TetraIcd::OpaquePayloadDataOut outData;
|
|
outData.payloadType = TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::FireSmokeMarker;
|
|
outData.payload = LtsUtils::convert<std::vector<unsigned char>>(data);
|
|
|
|
m_rxHandler->rxData(outData);
|
|
}
|
|
|
|
void CanagHandler::receivedSfmAwes(const QByteArray &data, quint32 multiplexer)
|
|
{
|
|
m_handlerAesData->process(data, multiplexer);
|
|
}
|
|
|
|
void CanagHandler::sfmAliveTimeout()
|
|
{
|
|
m_technicalStateSfm.sfmConnected = false;
|
|
m_technicalStateSfm.radioAvailable = false;
|
|
m_technicalStateSfm.licensed = false;
|
|
m_sfmConnectionEstablished = false;
|
|
|
|
|
|
m_rxHandler->rxData(m_technicalStateSfm);
|
|
}
|
|
|
|
void CanagHandler::sendFreeText()
|
|
{
|
|
m_freeTextCollectionTimer->stop();
|
|
|
|
OcTelegrams::Display outData;
|
|
int length = m_freeText.length() - 1;
|
|
if (*(m_freeText.constEnd()) == 0x03)
|
|
length -= 1;
|
|
outData.multiplexer = 1;
|
|
outData.text = m_freeText.mid(1, length);
|
|
m_freeText.clear();
|
|
|
|
m_rxHandler->rxData(outData);
|
|
}
|
|
|
|
void CanagHandler::sendSyncTime(uint32_t syncTimeSec)
|
|
{
|
|
INFO_LOG(m_logger) << "sendSyncTime " << syncTimeSec;
|
|
|
|
if (syncTimeSec > QDateTime::currentSecsSinceEpoch() - 3 &&
|
|
syncTimeSec < QDateTime::currentSecsSinceEpoch() + 3)
|
|
{
|
|
DEBUG_LOG(m_logger) << "PartitionTime is Still valid";
|
|
return;
|
|
}
|
|
if (!m_syncSocket->isOpen())
|
|
openSocket();
|
|
|
|
if (m_syncSocket->isOpen())
|
|
{
|
|
m_syncSocket->write(QByteArray::number(syncTimeSec));
|
|
m_syncSocket->flush();
|
|
DEBUG_LOG(m_logger) << "Send: " << syncTimeSec << " " << QByteArray::number(syncTimeSec);
|
|
}
|
|
}
|
|
|
|
void CanagHandler::send(const LtsUtils::SfmMessage &message)
|
|
{
|
|
|
|
|
|
DEBUG_LOG(m_logger) << "send message - " << message.id() << " RTR: " << message.rtr();
|
|
if (!message.payload().isEmpty())
|
|
DEBUG_LOG(m_logger) << " size: " << message.payload().size() << " data: " << message.payload().toHex(' ');
|
|
|
|
|
|
if (!m_connectionHandler->send(message))
|
|
ERROR_LOG(m_logger) << "Could not send message!";
|
|
else
|
|
emit sendMessage(message);
|
|
}
|
|
|
|
void CanagHandler::send(quint32 canId, const QByteArray &data)
|
|
{
|
|
LtsUtils::SfmMessage msg;
|
|
msg.setId(canId);
|
|
msg.setPayload(data);
|
|
send(msg);
|
|
}
|
|
|
|
void CanagHandler::openSocket()
|
|
{
|
|
if (m_syncSocket->isOpen())
|
|
return;
|
|
|
|
m_syncSocket->connectToHost(Settings::hostName(), 13777);
|
|
if (!m_syncSocket->waitForConnected())
|
|
WARNING_LOG(m_logger) << "Could not (re)open tcp destination " << m_syncSocket->peerName() << ":" << m_syncSocket->peerPort() << ", error: " << m_syncSocket->errorString();
|
|
else
|
|
{
|
|
connect(m_syncSocket, &QTcpSocket::disconnected, this, [this]
|
|
{
|
|
DEBUG_LOG(m_logger) << "Disconnected from Host";
|
|
m_syncSocket->close();
|
|
});
|
|
DEBUG_LOG(m_logger) << "(re)open tcp destination " << m_syncSocket->peerName() << ":" << m_syncSocket->peerPort();
|
|
}
|
|
}
|
|
|
|
void CanagHandler::fillMessageMap()
|
|
{
|
|
m_messages.clear();
|
|
|
|
|
|
LtsUtils::SfmMessage *message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::DemandDeviceId);
|
|
m_messages.insert(CanId::DemandDeviceId, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::DeviceId);
|
|
QByteArray dataDeviceId = QByteArray(4, '\0');
|
|
dataDeviceId[0] = Settings::manufacturerId();
|
|
dataDeviceId[1] = Settings::deviceId();
|
|
dataDeviceId[2] = (Settings::serialNumber() << 8) & 0xFF;
|
|
dataDeviceId[3] = Settings::manufacturerId() & 0xFF;
|
|
message->setPayload(dataDeviceId);
|
|
m_messages.insert(CanId::DeviceId, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::TechnicalStateOc);
|
|
QByteArray dataTechSrt(8, '\0');
|
|
unsigned int unixTime = m_mainHandler->dds()->currentPartitionTime() / 1000;
|
|
dataTechSrt[0] = (unixTime >> 24 & 0xFF);
|
|
dataTechSrt[1] = (unixTime >> 16 & 0xFF);
|
|
dataTechSrt[2] = (unixTime >> 8 & 0xFF);
|
|
dataTechSrt[3] = (unixTime >> 0 & 0xFF);
|
|
|
|
message->setPayload(dataTechSrt);
|
|
m_messages.insert(CanId::TechnicalStateOc, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::SpecificsOc);
|
|
QByteArray dataSpecificsOc = QByteArray(5, '\0');
|
|
dataSpecificsOc[0] = (((Settings::verX() & 0x3F) << 2) | ((Settings::verY() & 0x18) >> 3));
|
|
dataSpecificsOc[1] = (((Settings::verY() & 0x07) << 5) | (Settings::verZ() & 0x1F));
|
|
dataSpecificsOc[2] = (Settings::biteTimeOut() >> 8) & 0xFF;
|
|
dataSpecificsOc[3] = Settings::biteTimeOut() & 0xFF;
|
|
dataSpecificsOc[4] = 1;
|
|
message->setPayload(dataSpecificsOc);
|
|
m_messages.insert(CanId::SpecificsOc, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::ConnectionTestSfm);
|
|
m_messages.insert(CanId::ConnectionTestSfm, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::ParameterSfmTx);
|
|
QByteArray dataParaTne(3, '\0');
|
|
dataParaTne[0] = 0;
|
|
dataParaTne[1] = 1;
|
|
dataParaTne[2] = 0;
|
|
message->setPayload(dataParaTne);
|
|
m_messages.insert(CanId::ParameterSfmTx, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::ParameterOcTx);
|
|
QByteArray dataParaSrt(2, '\0');
|
|
dataParaSrt[0] = 0;
|
|
dataParaSrt[1] = 0;
|
|
message->setPayload(dataParaSrt);
|
|
m_messages.insert(CanId::ParameterOcTx, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::TechnicalStateSfm);
|
|
message->setRtr(true);
|
|
m_messages.insert(CanId::TechnicalStateSfm, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::SpecificsEquipment);
|
|
message->setRtr(true);
|
|
m_messages.insert(CanId::SpecificsEquipment, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::SpecificsExercise);
|
|
message->setRtr(true);
|
|
m_messages.insert(CanId::SpecificsExercise, message);
|
|
|
|
|
|
message = new LtsUtils::SfmMessage();
|
|
message->setId(CanId::WakeUp);
|
|
m_messages.insert(CanId::WakeUp, message);
|
|
|
|
}
|
|
|
|
void CanagHandler::demandInitialSfmData()
|
|
{
|
|
sendDemandSpecificsEquipment();
|
|
sendDemandSpecificsExercise();
|
|
sendDemandTechnicalStateSfm();
|
|
OcTelegrams::DemandCyclicPositions data = {};
|
|
data.cycleTime = 1;
|
|
txData(data);
|
|
m_sfmAliveTimer->start();
|
|
m_sfmConnectionCheckTimer->start();
|
|
m_sfmConnectionEstablished = true;
|
|
}
|
|
|
|
void CanagHandler::sendDemandTechnicalStateSfm()
|
|
{
|
|
send(*m_messages[CanId::TechnicalStateSfm]);
|
|
}
|
|
|
|
void CanagHandler::sendDemandSpecificsEquipment()
|
|
{
|
|
send(*m_messages[CanId::SpecificsEquipment]);
|
|
}
|
|
|
|
void CanagHandler::sendDemandSpecificsExercise()
|
|
{
|
|
send(*m_messages[CanId::SpecificsExercise]);
|
|
}
|
|
|
|
void CanagHandler::handleDisplayMessage(const LtsUtils::SfmMessage &message)
|
|
{
|
|
QByteArray data = message.payload();
|
|
OcTelegrams::Display outData;
|
|
|
|
|
|
outData.multiplexer = (data[0] & 0x7F);
|
|
if (0 == outData.multiplexer)
|
|
{
|
|
outData.textType = static_cast<OcTelegrams::TextType>(data.at(1));
|
|
size_t pos = 2;
|
|
LtsUtils::get2Byte(data, pos, outData.indexVbg);
|
|
LtsUtils::get2Byte(data, pos, outData.indexVba);
|
|
|
|
if (outData.indexVbg != 0 || outData.indexVba != 0)
|
|
m_rxHandler->rxData(outData);
|
|
}
|
|
else if (1 == outData.multiplexer)
|
|
{
|
|
if (data.at(1) == 0x02)
|
|
{
|
|
if (!m_freeText.isEmpty())
|
|
sendFreeText();
|
|
}
|
|
|
|
|
|
m_freeText.append(data.mid(1, data.length() - 1));
|
|
|
|
if (data.at(data.length() - 1) == 0x03)
|
|
sendFreeText();
|
|
else
|
|
m_freeTextCollectionTimer->start();
|
|
}
|
|
}
|
|
|
|
}
|