Initial commit
This commit is contained in:
torsten committed
commit
b7d90b9afe
16245 files changed
+3396666
No files matched your search
@@ -0,0 +1,436 @@
|
||||
/**
|
||||
* 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
|
||||
Reference in new issue
Block a user