Initial commit

Lts/Services/NmeaManagement/nmea_handler.cpp

@@ -0,0 +1,407 @@
+/**
+  * 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
+
+
+
+