408 lines
11 KiB
C++
Executable File
408 lines
11 KiB
C++
Executable File
/**
|
|
* NmeaHandler
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by Gerlinde Rudolphus, SLTE, 2022-06-09
|
|
*/
|
|
#include <QTime>
|
|
#include <QDateTime>
|
|
#include "nmea_handler.h"
|
|
namespace NmeaManagement
|
|
{
|
|
NmeaHandler::NmeaHandler(const std::shared_ptr<DdsHandler> &ddsHandler,
|
|
QObject *parent)
|
|
: QObject(parent)
|
|
, m_ddsHandler(ddsHandler)
|
|
, m_logger(getLogger("NmeaHandler"))
|
|
{
|
|
|
|
}
|
|
|
|
|
|
void NmeaHandler::init()
|
|
{
|
|
|
|
}
|
|
|
|
|
|
bool NmeaHandler::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 NmeaHandler::IIXDR(QString package)
|
|
{
|
|
int battery;
|
|
int temperature;
|
|
int frequency;
|
|
double generic;
|
|
int current;
|
|
bool switchOrValue;
|
|
int voltage;
|
|
QString serialNumber = QStringLiteral("");
|
|
QString timeStr;
|
|
|
|
|
|
NmeaManagement::BatteryState batteryState;
|
|
NmeaManagement::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_def::PRESHUTDOWN);
|
|
}
|
|
if (pkt_data[4] == QStringLiteral("SERVICE_CONTROL_SHUTDOWN"))
|
|
{
|
|
INFO_LOG(m_logger) << "SERVICE_CONTROL_SHUTDOWN";
|
|
serviceControl.ServiceControlState(ServiceControlEnum_def::SHUTDOWN);
|
|
}
|
|
if (pkt_data[4] == QStringLiteral("SERVICE_CONTROL_STOP"))
|
|
{
|
|
INFO_LOG(m_logger) << "SERVICE_CONTROL_STOP";
|
|
serviceControl.ServiceControlState(ServiceControlEnum_def::STOP);
|
|
}
|
|
m_ddsHandler->dts()->publish(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.id(m_batteries.indexOf(serialNumber));
|
|
}
|
|
|
|
index = pkt_data.indexOf(QLatin1String("B")) + 1;
|
|
if ( index >= 0)
|
|
battery = pkt_data[index].toInt() ;
|
|
else
|
|
battery = -1;
|
|
batteryState.battery(battery );
|
|
|
|
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.toStdString()) ;
|
|
INFO_LOG(m_logger) << "publish batteryState serialNumber=" << serialNumber;
|
|
m_ddsHandler->dts()->publish(batteryState);
|
|
}
|
|
|
|
qint64 NmeaHandler::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 NmeaHandler::IIALR(QString package)
|
|
{
|
|
NmeaManagement::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.time(timeAlarm);
|
|
batteryAlarm.description(packageData.description.toStdString());
|
|
batteryAlarm.acknowledge(packageData.acknowledgeState);
|
|
batteryAlarm.condition(packageData.condition);
|
|
batteryAlarm.uniqueAlarmNumber(packageData.number);
|
|
|
|
m_ddsHandler->dts()->publish(batteryAlarm);
|
|
}
|
|
|
|
|
|
void NmeaHandler::GPGGA(QString package)
|
|
{
|
|
NmeaManagement::PositionExcon PositionExcon;
|
|
double latDecimal;
|
|
double lonDecimal;
|
|
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++;
|
|
}
|
|
}
|
|
|
|
|
|
int inteiro;
|
|
double real;
|
|
QString val;
|
|
while (packageData.B_latitude[0] == QStringLiteral("0"))
|
|
packageData.B_latitude.remove(0, 1);
|
|
val = packageData.B_latitude.mid(0, 2);
|
|
packageData.B_latitude = packageData.B_latitude.remove(0, 2);
|
|
inteiro = val.toInt();
|
|
val = packageData.B_latitude;
|
|
real = val.toDouble();
|
|
|
|
|
|
if (packageData.C_latitude_dir == 'S' || packageData.C_latitude_dir == 's')
|
|
latDecimal = ((real / 60) + inteiro) * -1;
|
|
//INFO_LOG(m_logger) << QString::number(((real / 60) + inteiro) * -1, 'g', 9);
|
|
else
|
|
latDecimal = ((real / 60) + inteiro);
|
|
//INFO_LOG(m_logger) << "latitude=" << QString::number(((real / 60) + inteiro), 'g', 9);
|
|
|
|
|
|
while (packageData.D_longitude[0] == QStringLiteral("0"))
|
|
packageData.D_longitude.remove(0, 1);
|
|
|
|
val = packageData.D_longitude.mid(0, 2);
|
|
packageData.D_longitude = packageData.D_longitude.remove(0, 2);
|
|
inteiro = val.toInt();
|
|
val = packageData.D_longitude;
|
|
real = val.toDouble();
|
|
|
|
if (packageData.E_longitude_dir == 'W' || packageData.E_longitude_dir == 'w')
|
|
{
|
|
lonDecimal = ((real / 60) + inteiro) * -1;
|
|
}
|
|
else
|
|
{
|
|
lonDecimal = ((real / 60) + inteiro);
|
|
}
|
|
|
|
INFO_LOG(m_logger) << "longitude=" << lonDecimal
|
|
<< " latitude=" << latDecimal;
|
|
|
|
|
|
PositionExcon.position().lat(latDecimal * M_PI / 180.0);
|
|
PositionExcon.position().lon(lonDecimal * M_PI / 180.0);
|
|
PositionExcon.valid(true);
|
|
m_ddsHandler->dts()->publish(PositionExcon);
|
|
}
|
|
|
|
|
|
|
|
|
|
void NmeaHandler::GPRMC(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);
|
|
|
|
}
|
|
|
|
|
|
|
|
void NmeaHandler::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++;
|
|
}
|
|
} // namespace NmeaManagement
|
|
|
|
|
|
|
|
|