437 lines
13 KiB
C++
Executable File
437 lines
13 KiB
C++
Executable File
/**
|
|
* Read XDR telegrams from a directory and convert it to EtsTetraIcd structures
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by Christian Ehrlicher, SLSE, 2016-04-14
|
|
*/
|
|
|
|
#include "nmea_handler_rx.h"
|
|
#include <TrainingAreaBase/training_area_telegrams.h>
|
|
#include <QDateTime>
|
|
#include <QJsonDocument>
|
|
#include <QJsonObject>
|
|
#include <QTimer>
|
|
|
|
namespace Nmea
|
|
{
|
|
|
|
NmeaHandlerRx::NmeaHandlerRx(const std::shared_ptr<TrainingAreaBase::RecvFromTab> &rxHandler)
|
|
: m_rxHandler(rxHandler)
|
|
, m_bWarnedOnce(false)
|
|
, m_logger(getLogger("TetraHandlerRx"))
|
|
{
|
|
|
|
// NOTE: Just for checking if the communication is working
|
|
// QTimer *testTimer = new QTimer(this);
|
|
// testTimer->setInterval(4000);
|
|
// testTimer->start();
|
|
// connect(testTimer, &QTimer::timeout, this, &NmeaHandlerRx::sendTestMessages);
|
|
}
|
|
|
|
bool NmeaHandlerRx::handleTelegram(const QString &data)
|
|
{
|
|
decode(data);
|
|
return true;
|
|
}
|
|
|
|
void NmeaHandlerRx::decode(QString nmeaPackage)
|
|
{
|
|
QString nmeaType;
|
|
int startingPoint;
|
|
|
|
|
|
startingPoint = 0;
|
|
if (nmeaPackage == QStringLiteral(""))
|
|
return;
|
|
while (nmeaPackage.at(startingPoint) != QStringLiteral("$"))
|
|
{
|
|
startingPoint++;
|
|
if (startingPoint >= nmeaPackage.size())
|
|
return;
|
|
}
|
|
|
|
nmeaPackage.remove(0, startingPoint);
|
|
|
|
nmeaType = nmeaPackage.mid(1, 5);
|
|
INFO_LOG(m_logger) << "nmeaType=" << nmeaType;
|
|
|
|
if (nmeaType == QStringLiteral("IIXDR"))
|
|
{
|
|
NOTICE_LOG(m_logger) << "IIXDR Battery-State";
|
|
}
|
|
|
|
if (nmeaType == QStringLiteral("GPGGA"))
|
|
{
|
|
INFO_LOG(m_logger) << "Eigenposition";
|
|
if (checksum(nmeaPackage))
|
|
{
|
|
INFO_LOG(m_logger) << "Eigenposition checksum ok";
|
|
GPGGA(nmeaPackage);
|
|
}
|
|
}
|
|
else if (nmeaType == QStringLiteral("GPRMC"))
|
|
{
|
|
if (checksum(nmeaPackage))
|
|
GPRMC(nmeaPackage);
|
|
|
|
}
|
|
else if (nmeaType == QStringLiteral("IIXDR"))
|
|
{
|
|
NOTICE_LOG(m_logger) << "IIXDR";
|
|
if (checksum(nmeaPackage))
|
|
{
|
|
NOTICE_LOG(m_logger) << "IIXDR checksum OK";
|
|
IIXDR(nmeaPackage);
|
|
}
|
|
else
|
|
NOTICE_LOG(m_logger) << "IIXDR checksum NOT OK";
|
|
|
|
}
|
|
else if (nmeaType == QStringLiteral("IIALR"))
|
|
{
|
|
if (checksum(nmeaPackage))
|
|
IIALR(nmeaPackage);
|
|
}
|
|
else
|
|
packagesInvalid++;
|
|
}
|
|
|
|
bool NmeaHandlerRx::checksum(QString package)
|
|
{
|
|
short start = 0;
|
|
|
|
start = package.indexOf(QStringLiteral("*"));
|
|
if (start < 0)
|
|
packagesInvalid++;
|
|
QString to_chk = package.mid(1, start - 1);
|
|
package.remove(0, start + 1);
|
|
int cksm = 0;
|
|
|
|
for (int i = 0; i < to_chk.size(); i++)
|
|
{
|
|
QChar letra = to_chk.at(i);
|
|
cksm = cksm ^ letra.toLatin1();
|
|
}
|
|
|
|
if (cksm == package.toInt(nullptr, 16))
|
|
return true;
|
|
packagesInvalid++;
|
|
return false;
|
|
}
|
|
|
|
void NmeaHandlerRx::IIXDR(const QString &package)
|
|
{
|
|
int battery;
|
|
int temperature;
|
|
int frequency;
|
|
double generic;
|
|
int current;
|
|
bool switchOrValue;
|
|
int voltage;
|
|
QString serialNumber = QStringLiteral("");
|
|
QString timeStr;
|
|
using namespace TrainingAreaBase::TetraIcd;
|
|
|
|
BatteryInfo batteryInfo;
|
|
BatteryState batteryState;
|
|
ServiceControl serviceControl;
|
|
QRegExp sep(QStringLiteral("(\\,|\\*)"));
|
|
QStringList pkt_data = package.split(sep);
|
|
INFO_LOG(m_logger) << "----------------------------------------------------";
|
|
INFO_LOG(m_logger) << "batteryState pkt_data.size=" << pkt_data.size();
|
|
for (int i = 0; i < pkt_data.size(); i++)
|
|
INFO_LOG(m_logger) << "i=" << i << "Value=" << pkt_data[i];
|
|
|
|
|
|
int index = pkt_data.indexOf(QLatin1String("S")) + 1;
|
|
if ( index >= 1)
|
|
{
|
|
INFO_LOG(m_logger) << "Ausschalten!!!";
|
|
if (pkt_data[4] == QStringLiteral("SERVICE_CONTROL_PRESHUTDOWN"))
|
|
{
|
|
INFO_LOG(m_logger) << "SERVICE_CONTROL_PRESHUTDOWN";
|
|
serviceControl.serviceControlState = ServiceControlEnum::PRESHUTDOWN;
|
|
}
|
|
if (pkt_data[4] == QStringLiteral("SERVICE_CONTROL_SHUTDOWN"))
|
|
{
|
|
INFO_LOG(m_logger) << "SERVICE_CONTROL_SHUTDOWN";
|
|
serviceControl.serviceControlState = ServiceControlEnum::SHUTDOWN;
|
|
}
|
|
if (pkt_data[4] == QStringLiteral("SERVICE_CONTROL_STOP"))
|
|
{
|
|
INFO_LOG(m_logger) << "SERVICE_CONTROL_STOP";
|
|
serviceControl.serviceControlState = ServiceControlEnum::STOP;
|
|
}
|
|
serviceControl.header.equipmentNumber = 1;
|
|
serviceControl.header.timeStamp = QDateTime::currentMSecsSinceEpoch();
|
|
rxData(serviceControl);
|
|
return;
|
|
}
|
|
|
|
if ( pkt_data.size()>=4)
|
|
serialNumber = (pkt_data[4]);
|
|
|
|
{
|
|
if ( !m_batteries.contains(serialNumber))
|
|
{
|
|
if (m_batteries.size() == 2)
|
|
{
|
|
m_batteries.clear();
|
|
}
|
|
m_batteries.append(serialNumber);
|
|
}
|
|
|
|
batteryState.batteryPort = m_batteries.indexOf(serialNumber);
|
|
}
|
|
|
|
index = pkt_data.indexOf(QLatin1String("B")) + 1;
|
|
if ( index >= 0)
|
|
battery = pkt_data[index].toInt() ;
|
|
else
|
|
battery = -1;
|
|
batteryState.batteryPercent = battery * 20 ;
|
|
|
|
index = pkt_data.indexOf(QLatin1String("C")) + 1;
|
|
if ( index >= 0)
|
|
temperature = pkt_data[index].toInt() ;
|
|
else
|
|
temperature = -1;
|
|
batteryState.temperature = temperature;
|
|
|
|
index = pkt_data.indexOf(QLatin1String("F")) + 1;
|
|
if ( index >= 0)
|
|
frequency = pkt_data[index].toInt() ;
|
|
else
|
|
frequency = -1;
|
|
batteryState.frequency = frequency;
|
|
|
|
index = pkt_data.indexOf(QLatin1String("F")) + 1;
|
|
if ( index >= 0)
|
|
generic = pkt_data[index].toDouble() ;
|
|
else
|
|
generic = -1.0;
|
|
batteryState.generic = generic;
|
|
|
|
index = pkt_data.lastIndexOf(QLatin1String("I")) + 1;
|
|
|
|
if ( index >= 0)
|
|
current = pkt_data[index].toInt();
|
|
else
|
|
current = -1;
|
|
batteryState.current = current;
|
|
index = pkt_data.indexOf(QLatin1String("S")) + 1;
|
|
switchOrValue = false;
|
|
if ( index >= 0)
|
|
if ( pkt_data[index].toInt() == 1 )
|
|
switchOrValue = true;
|
|
batteryState.switchOrValue = switchOrValue;
|
|
|
|
index = pkt_data.indexOf(QLatin1String("U")) + 1;
|
|
if ( index >= 0)
|
|
voltage = pkt_data[index].toInt() ;
|
|
else
|
|
voltage = -1;
|
|
batteryState.voltage = voltage;
|
|
|
|
batteryState.serialNumber = serialNumber;
|
|
INFO_LOG(m_logger) << "publish batteryState serialNumber=" << serialNumber;
|
|
batteryInfo.batteryStates = {batteryState};
|
|
batteryInfo.header.equipmentNumber = 1;
|
|
batteryInfo.header.timeStamp = QDateTime::currentMSecsSinceEpoch();
|
|
rxData(batteryInfo);
|
|
}
|
|
|
|
void NmeaHandlerRx::IIALR(const QString &package)
|
|
{
|
|
using namespace TrainingAreaBase::TetraIcd;
|
|
BatteryAlarm batteryAlarm;
|
|
QRegExp sep(QStringLiteral("(\\,|\\*)"));
|
|
QStringList pkt_data = package.split(sep);
|
|
alarm_state packageData;
|
|
INFO_LOG(m_logger) << "pkt_data.size=" << pkt_data.size();
|
|
// for ( int i = 0; i < pkt_data.size(); i++)
|
|
// {
|
|
// INFO_LOG(m_logger)<< "i=" << i << "Value=" << pkt_data[i];
|
|
// }
|
|
packageData.time = pkt_data[1];
|
|
packageData.number = pkt_data[2].toInt();
|
|
if (pkt_data[3] == QStringLiteral("A"))
|
|
packageData.condition = true;
|
|
else
|
|
packageData.condition = false;
|
|
if (pkt_data[4] == QStringLiteral("A"))
|
|
packageData.acknowledgeState = true;
|
|
else
|
|
packageData.acknowledgeState = false;
|
|
packageData.description = pkt_data[5];
|
|
|
|
qint64 timeAlarm = getDateTime(packageData.time);
|
|
|
|
INFO_LOG(m_logger) << "time=" << packageData.time
|
|
<< " timeAlarm=" << timeAlarm
|
|
<< " number=" << packageData.number
|
|
<< " condition=" << packageData.condition
|
|
<< " acknowledgeState=" << packageData.acknowledgeState
|
|
<< " description=" << packageData.description;
|
|
batteryAlarm.id = 0;
|
|
batteryAlarm.alarmTime = timeAlarm;
|
|
batteryAlarm.description = packageData.description;
|
|
batteryAlarm.acknowledge = packageData.acknowledgeState;
|
|
batteryAlarm.condition = packageData.condition;
|
|
batteryAlarm.uniqueAlarmNumber = packageData.number;
|
|
batteryAlarm.header.equipmentNumber = 1;
|
|
batteryAlarm.header.timeStamp = QDateTime::currentMSecsSinceEpoch();
|
|
rxData(batteryAlarm);
|
|
}
|
|
|
|
|
|
void NmeaHandlerRx::GPGGA(const QString &package)
|
|
{
|
|
using namespace TrainingAreaBase::TetraIcd;
|
|
|
|
PositionReport PositionExcon;
|
|
QRegExp sep(QStringLiteral("(\\,|\\*)"));
|
|
QStringList pkt_data = package.split(sep);
|
|
gpgga_data packageData;
|
|
|
|
packageData.A_hour = pkt_data[1];
|
|
packageData.B_latitude = pkt_data[2];
|
|
packageData.C_latitude_dir = pkt_data[3].toStdString()[0];
|
|
packageData.D_longitude = pkt_data[4];
|
|
packageData.E_longitude_dir = pkt_data[5].toStdString()[0];
|
|
packageData.F_qualidade_fixa = pkt_data[6].toInt();
|
|
packageData.G_quantidade_satelites = pkt_data[7].toInt();
|
|
packageData.H_diluicao_da_precisao_horizontal = pkt_data[8].toFloat();
|
|
packageData.I_altitude = pkt_data[9].toFloat();
|
|
packageData.J_unidade_medida_altura = pkt_data[10].toStdString()[0];
|
|
packageData.K_altura_geoide_acima_elipsoide = pkt_data[11].toFloat();
|
|
packageData.L_unidade_medida_geoide = pkt_data[12].toStdString()[0];
|
|
packageData.M_idade_ultima_atualizacao_dgps = pkt_data[13].toFloat();
|
|
packageData.N_dgps_id = pkt_data[14].toInt();
|
|
|
|
QString hora;
|
|
hora = packageData.A_hour.mid(0, 6);
|
|
int inserts = 0;
|
|
for (int var = 1; var < hora.size() - inserts; ++var)
|
|
{
|
|
if (var % 2 == 0)
|
|
{
|
|
hora.insert(var + inserts, QStringLiteral(":"));
|
|
inserts++;
|
|
}
|
|
}
|
|
|
|
while (packageData.B_latitude[0] == QStringLiteral("0"))
|
|
packageData.B_latitude.remove(0, 1);
|
|
|
|
double lat = packageData.B_latitude.toDouble();
|
|
|
|
while (packageData.D_longitude[0] == QStringLiteral("0"))
|
|
packageData.D_longitude.remove(0, 1);
|
|
|
|
double lon = packageData.D_longitude.toDouble();
|
|
|
|
double latDeg = floor(lat / 100) + (lat - floor(lat / 100) * 100) / 60;
|
|
double lonDeg = floor(lon / 100) + (lon - floor(lon / 100) * 100) / 60;
|
|
|
|
if (packageData.C_latitude_dir == 'S' || packageData.C_latitude_dir == 's')
|
|
{
|
|
latDeg *= -1;
|
|
}
|
|
|
|
if (packageData.E_longitude_dir == 'W' || packageData.E_longitude_dir == 'w')
|
|
{
|
|
lonDeg *= -1;
|
|
}
|
|
|
|
DEBUG_LOG(m_logger) << "RAW Nmea Position: " << packageData.D_longitude << " | " << packageData.B_latitude;
|
|
DEBUG_LOG(m_logger) << "Lat: " << latDeg;
|
|
DEBUG_LOG(m_logger) << "Lon: " << lonDeg;
|
|
|
|
PositionExcon.geoPosition.lat = latDeg * M_PI / 180.0;
|
|
PositionExcon.geoPosition.lon = lonDeg * M_PI / 180.0;
|
|
//PositionExcon.geoPosition.isValid = true;
|
|
PositionExcon.header.equipmentNumber = 1;
|
|
PositionExcon.header.timeStamp = QDateTime::currentMSecsSinceEpoch();
|
|
rxData(PositionExcon);
|
|
|
|
}
|
|
|
|
void NmeaHandlerRx::GPRMC(const QString &package)
|
|
{
|
|
QRegExp sep(QStringLiteral("(\\,|\\*)"));
|
|
QStringList pkt_data = package.split(sep);
|
|
gprmc_data packageData;
|
|
|
|
packageData.A_hour = (int)pkt_data[1].toFloat();
|
|
packageData.B_warning = pkt_data[2].toStdString()[0];
|
|
packageData.C_latitude = pkt_data[3].toFloat();
|
|
packageData.D_latitude_coord = pkt_data[4].toStdString()[0];
|
|
packageData.E_longitude = pkt_data[5].toFloat();
|
|
packageData.F_longitude_coord = pkt_data[6].toStdString()[0];
|
|
packageData.G_velocity = pkt_data[7].toFloat();
|
|
packageData.H_CMG = pkt_data[8].toFloat();
|
|
packageData.I_data = pkt_data[9].toInt();
|
|
packageData.J_variacao_magnetica = pkt_data[10].toFloat();
|
|
packageData.K_variacao_magnetica_coord = pkt_data[11].toStdString()[0];
|
|
|
|
|
|
INFO_LOG(m_logger) << "velocidade=" << QString::number(packageData.G_velocity * 1.852);
|
|
|
|
}
|
|
|
|
qint64 NmeaHandlerRx::getDateTime(QString receivedTimeStr)
|
|
{
|
|
int pos = receivedTimeStr.indexOf(QStringLiteral("."));
|
|
QString receivedTimeStr1 = receivedTimeStr.left(pos);
|
|
QTime currentTime = QTime::currentTime();
|
|
qint64 currentDateTime = QDateTime::currentMSecsSinceEpoch();
|
|
QTime receivedTime;
|
|
int msecsSinceStartOfDay = currentTime.msecsSinceStartOfDay();
|
|
qint64 currentDate = currentDateTime - msecsSinceStartOfDay;
|
|
|
|
QString dateFormat = QStringLiteral("hhmmss");
|
|
receivedTime = QTime::fromString(receivedTimeStr1, dateFormat);
|
|
qint64 receivedTimeMs = receivedTime.msecsSinceStartOfDay();
|
|
|
|
return currentDate + receivedTimeMs;
|
|
}
|
|
|
|
void NmeaHandlerRx::sendTestMessages()
|
|
{
|
|
using namespace TrainingAreaBase::TetraIcd;
|
|
BatteryInfo batteryInfo;
|
|
BatteryState batteryState;
|
|
batteryState.batteryPort = rand() % 2;
|
|
batteryState.batteryPercent = (0 + rand() % 5) * 20;
|
|
batteryState.current = 5;
|
|
batteryState.frequency = 3;
|
|
batteryState.generic = 1;
|
|
batteryState.serialNumber = QStringLiteral("Test");
|
|
batteryState.temperature = 38.8;
|
|
DEBUG_LOG(m_logger) << "Send BatteryInfo Message";
|
|
batteryInfo.batteryStates = {batteryState};
|
|
batteryInfo.header.equipmentNumber = 1;
|
|
batteryInfo.header.timeStamp = QDateTime::currentMSecsSinceEpoch();
|
|
rxData(batteryInfo);
|
|
|
|
}
|
|
|
|
template<typename T>
|
|
void NmeaHandlerRx::rxData(const T &data)
|
|
{
|
|
if (!m_rxHandler)
|
|
{
|
|
if (!m_bWarnedOnce)
|
|
{
|
|
ERROR_LOG(m_logger) << "No RecvFromTab available";
|
|
m_bWarnedOnce = true;
|
|
}
|
|
return;
|
|
}
|
|
return m_rxHandler->rxData(data);
|
|
}
|
|
|
|
} // namespace Nmea
|