Initial commit

Lts/Proxies/Buildings/BuildingControlUnit/BSM/bsm_socket_handler.cpp

@@ -0,0 +1,469 @@
+/**
+  * @RDE_HEADER@
+  * Handle incoming/outgoing data from/to all BSMs of one building
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Christian Ehrlicher, SLSE, 2017-01-18
+  */
+
+#include "bsm_socket_handler.h"
+#include "bsm_telegrams_base.h"
+#include "bsm_telegrams_bit_timeout_request.h"
+#include "bsm_telegrams_chopped_signal_flash.h"
+#include "bsm_telegrams_configuration_request.h"
+#include "bsm_telegrams_direct_fire.h"
+#include "bsm_telegrams_observer_controller.h"
+#include "bsm_telegrams_sw_version_report.h"
+#include "bsm_telegrams_sw_version_request.h"
+#include "bsm_telegrams_technical_status_request.h"
+#include <QDateTime>
+#include <QHostInfo>
+
+namespace BuildingControlUnit
+{
+namespace BSM
+{
+static const qint64 s_opticalTimeSlot = 100;
+
+SocketHandler::SocketHandler(QObject *parent)
+  : QObject(parent)
+  , m_logger(BuildingControlUnit::getLogger("BSM.SocketHandler"))
+{
+  connect(&m_cleanupQueueTimer, &QTimer::timeout,
+          this, &SocketHandler::onCleanupQueue);
+
+  m_cleanupQueueTimer.setSingleShot(false);
+  m_cleanupQueueTimer.setInterval(10000);
+  m_cleanupQueueTimer.setTimerType(Qt::VeryCoarseTimer);
+  m_cleanupQueueTimer.start();
+
+
+  connect(&m_bsmStateTimer, &QTimer::timeout, this, &SocketHandler::onBsmStateTimerTimeOut);
+  m_bsmStateTimer.setSingleShot(true);
+  m_bsmStateTimer.setInterval(500);
+  m_bsmStateTimer.setTimerType(Qt::VeryCoarseTimer);
+}
+
+bool SocketHandler::init(const BuildingBsmConfig &bsmConfig)
+{
+  // remove old sockets (should never happen)
+  qDeleteAll(m_sockets);
+  m_sockets.clear();
+  m_socketData.clear();
+  for (const Bsm &c : bsmConfig.bsmMap)
+  {
+    Socket *s = new Socket(c, this);//THIS
+
+    connect(s, &Socket::dataAvailable,
+            this, &SocketHandler::onDataAvailable);
+    connect(s, &Socket::disconnected,
+            this, &SocketHandler::onSocketDisconnected);
+    connect(s, &Socket::connected,
+            this, &SocketHandler::onSocketConnected);
+    m_sockets[c.id] = s;
+  }
+  return true;
+}
+
+bool SocketHandler::triggerFlashlight(int bsmId, const QString &hostname, int numberOfFlashes, int pauseBetweenFlashesSec)
+{
+  Socket *s = hostname.isEmpty() ? getSocketForBsm(bsmId) : getSocketForBsm(hostname);
+  if (!s)
+    return false;
+  DEBUG_LOG(m_logger) << "triggerFlashlight BSM Socket with socketId " << bsmId;  
+  Telegrams::PanChoppedSignalFlashlight flash;
+  flash.m_noOfFlashes = qBound(0, numberOfFlashes, 255);
+  flash.m_pauseDuration = qBound(1, pauseBetweenFlashesSec, 10);
+  flash.m_flashDuration = 1;
+  return s->sendData(flash.toBytestream());
+}
+
+bool SocketHandler::triggerFlashlightPulse(int bsmId, const QString &hostname,
+                                           int numberOfFlashes,
+                                           int pauseDuration,
+                                           int choppedConfiguration,
+                                           int flashDuration)
+{
+  Socket *s = hostname.isEmpty() ? getSocketForBsm(bsmId) : getSocketForBsm(hostname);
+  if (!s)
+    return false;
+  DEBUG_LOG(m_logger) << "triggerFlashlightPulse BSM Socket with socketId " << bsmId
+                      << " hostname=" << hostname
+                      << " numberOfFlashes=" << numberOfFlashes
+                      << " pauseDuration=" << pauseDuration
+                      << " choppedConfiguration=" << choppedConfiguration
+                      << " flashDuration=" << flashDuration;
+  Telegrams::PanChoppedSignalFlashlight flash;
+  flash.m_noOfFlashes = qBound(0, numberOfFlashes, 255);
+  flash.m_pauseDuration = qBound(1, pauseDuration, 10);
+  flash.m_choppedFlashConfig = qBound(2, choppedConfiguration, 10);
+  flash.m_flashDuration = flashDuration;
+  return s->sendData(flash.toBytestream());
+}
+
+
+bool SocketHandler::triggerFlashlights(int numberOfFlashes,
+                                       int pauseDuration,
+                                       int choppedConfiguration,
+                                       int flashDuration)
+{
+  int cntSocketsActive = 0;
+  Telegrams::PanChoppedSignalFlashlight flash;
+    flash.m_noOfFlashes = qBound(0, numberOfFlashes, 255);
+    flash.m_pauseDuration = qBound(1, pauseDuration, 10);
+    flash.m_choppedFlashConfig = qBound(2, choppedConfiguration, 10);
+    flash.m_flashDuration = flashDuration;
+  const QByteArray data = flash.toBytestream();
+  for (Socket *s : qAsConst(m_sockets))
+  {
+    if (s->isConnected())
+    {
+      if (s->sendData(data))
+        ++cntSocketsActive;
+    }
+    else
+      WARNING_LOG(m_logger) << "BSM " << s->bsmConfig().id
+                            << " is not connected - can not activate flash light";
+  }
+  if (cntSocketsActive == 0)
+  {
+    WARNING_LOG(m_logger) << "No active bsm - can not trigger flash light for all bsm";
+    return false;
+  }
+  return true;
+}
+
+bool SocketHandler::getBsmConfiguration(int bsmId)
+{
+  Socket *s = getSocketForBsm(bsmId);
+  if (!s)
+    return false;
+  Telegrams::PanConfigurationRequest req;
+  return s->sendData(req.toBytestream());
+}
+
+bool SocketHandler::getBsmBitTimeout(int bsmId)
+{
+  Socket *s = getSocketForBsm(bsmId);
+  if (!s)
+    return false;
+  Telegrams::PanBitTimeoutRequest req;
+  return s->sendData(req.toBytestream());
+}
+
+bool SocketHandler::getBsmTechnicalStatus(int bsmId)
+{
+  Socket *s = getSocketForBsm(bsmId);
+  if (!s)
+    return false;
+  Telegrams::PanTechnicalStatusRequest req;
+  return s->sendData(req.toBytestream());
+}
+
+QVector<BsmState> SocketHandler::bsmStates() const
+{
+  QVector<BsmState> ret;
+  ret.reserve(m_sockets.size());
+  for (const Socket *s : m_sockets)
+  {
+    BsmState bs(s->bsmConfig());
+    bs.bConnected = s->isConnected();
+    bs.versionInfo = s->versionInfo();
+    ret += bs;
+  }
+
+  return ret;
+}
+
+void SocketHandler::onBsmStateTimerTimeOut()
+{
+  emit bsmStateChanged(bsmStates());
+}
+
+void SocketHandler::onDataAvailable(int id, const QByteArray &data)
+{
+  m_socketData[id] = parseBuffer(id, m_socketData[id] + data);
+}
+
+void SocketHandler::onSocketConnected(int id)
+{
+  requestVersion(id); // hopefully 500ms are enough for a version request
+  DEBUG_LOG(m_logger) << "BSM Socket with id " << id << " connected";
+  m_bsmStateTimer.start();
+}
+
+void SocketHandler::onSocketDisconnected(int id)
+{
+  DEBUG_LOG(m_logger) << "BSM Socket with id " << id << " disconnected";
+  m_socketData[id].clear();
+  m_bsmStateTimer.start();
+}
+
+
+void SocketHandler::onCleanupQueue()
+{
+  if (m_lastShots.isEmpty())
+    return;
+  QVector<OpticalShot> activeShots;
+  qint64 tNow = QDateTime::currentMSecsSinceEpoch();
+  for (const OpticalShot &os : qAsConst(m_lastShots))
+  {
+    if (os.timeStampMSec < tNow - s_opticalTimeSlot)
+      continue;
+    activeShots += os;
+  }
+  m_lastShots = activeShots;
+}
+
+Socket *SocketHandler::getSocketForBsm(int bsmId)
+{
+  Socket *s = m_sockets.value(bsmId);
+  if (!s)
+  {
+    WARNING_LOG(m_logger) << "Got invalid bsm id (" << bsmId << ") to trigger flashlight";
+    return nullptr;
+  }
+  if (!s->isConnected())
+  {
+    WARNING_LOG(m_logger) << "Bsm with id " << bsmId
+                          << " is not connected - can not activate flash light";
+    return nullptr;
+  }
+  return s;
+}
+
+Socket *SocketHandler::getSocketForBsm(const QString &hostname)
+{
+  const QHostInfo hi = QHostInfo::fromName(hostname);
+  const auto addresses = hi.addresses();
+  if (addresses.isEmpty())
+    return nullptr;
+  for (const QHostAddress &ha : addresses)
+  {
+    if (ha.isLoopback())
+      continue;
+    for (Socket *s : qAsConst(m_sockets))
+    {
+      if (s->peerAddress() == ha)
+        return s;
+    }
+  }
+  return nullptr;
+}
+
+
+QByteArray SocketHandler::parseBuffer(int id, const QByteArray &buffer)
+{
+  int iEndPos = 0;
+  int iCurReadPos = 0;
+  while (findNextSyncStartByte(buffer, iCurReadPos))
+  {
+    const int iStartPos = iCurReadPos;
+    if (!findNextSyncEndByte(buffer, iCurReadPos))
+      break;
+    iEndPos = iCurReadPos;
+    parseBsmTelegram(id, buffer.mid(iStartPos + 1, iEndPos - iStartPos - 1));
+  }
+  return buffer.mid(iEndPos);
+}
+
+void SocketHandler::parseBsmTelegram(int id, const QByteArray &baTele)
+{
+  Telegrams::Telegram *tele = Telegrams::Telegram::telegramFromBytestream(baTele);
+  if (!tele)
+    return;
+  if (tele->telId() != Telegrams::PanTelegramId::ALIVE)
+    DEBUG_LOG(m_logger) << "Got tele from bsm with id " << id << ": " << tele->print();
+  switch (tele->telId())
+  {
+  case Telegrams::PanTelegramId::ALIVE:
+  {
+    Socket *s = m_sockets.value(id);
+    if (s)
+      s->gotAlive();
+    break;
+  }
+  case Telegrams::PanTelegramId::REQ_BIT_TIMEOUT:
+  case Telegrams::PanTelegramId::BIT_TIMEOUT_REPORT:
+  case Telegrams::PanTelegramId::REQ_SW_VERSION:
+  case Telegrams::PanTelegramId::CONFIGURATION_SETUP:
+  case Telegrams::PanTelegramId::REQ_CONFIGURATION:
+  case Telegrams::PanTelegramId::REQ_TECHNICAL_STATUS:
+  case Telegrams::PanTelegramId::TECHNICAL_STATUS:
+  case Telegrams::PanTelegramId::SIGNAL_FLASHLIGHT:
+  case Telegrams::PanTelegramId::CHOPPED_SIGNAL_FLASH:
+    break;
+  case Telegrams::PanTelegramId::CONFIGURATION_REPORT:
+    // request version
+    //requestVersion(id);
+    break;
+  case Telegrams::PanTelegramId::SW_VERSION_REPORT:
+  {
+    Telegrams::PanSwVersionReport *rep = static_cast<Telegrams::PanSwVersionReport *>(tele);
+    Socket *s = m_sockets.value(id);
+    if (s)
+      s->setVersionInformation(rep->m_mainVersion,
+                               rep->m_subVersion,
+                               rep->m_revision,
+                               QString::fromLatin1(rep->m_manufacturer));
+    break;
+  }
+  case Telegrams::PanTelegramId::ER_DIRECT_FIRE:
+    handleDirectFire(id, static_cast<Telegrams::PanDirectFire *>(tele));
+    break;
+  case Telegrams::PanTelegramId::ER_OBSERVER_CONTR:
+    handleObserverController(id, static_cast<Telegrams::PanObserverController *>(tele));
+    break;
+  }
+  delete tele;
+}
+
+
+bool SocketHandler::requestVersion(int id)
+{
+  DEBUG_LOG(m_logger) << "Requesting version for BSM " << id;
+  Socket *s = m_sockets.value(id);
+  if (!s || !s->isConnected())
+    return false;
+
+  Telegrams::PanSwVersionRequest tele;
+  return s->sendData(tele.toBytestream());
+}
+
+void SocketHandler::handleDirectFire(int id, const Telegrams::PanDirectFire *tele)
+{
+  if (!tele)
+    return;
+
+  if (tele->m_ammunitionId == 0)
+  {
+    WARNING_LOG(m_logger) << "Got direct fire without valid ammunition - ignoring: " << tele->print();
+    return;
+  }
+  OpticalShot osNew;
+  osNew.id           = id;
+  osNew.ammunitionId = tele->m_ammunitionId;
+  osNew.originatorId = tele->m_originatorId;
+  osNew.deposition1 = tele->m_deposition1;
+  osNew.deposition2 = tele->m_deposition2;
+  osNew.timeStampMSec = QDateTime::currentMSecsSinceEpoch();
+
+  m_lastShots += osNew;
+  QTimer::singleShot(500, this, &SocketHandler::handleDirectFireSummray);
+
+}
+
+void SocketHandler::handleDirectFireSummray()
+{
+  DEBUG_LOG(m_logger) << "handleDirectFireSummray m_lastShots.size=" << m_lastShots.size();
+  OpticalShot osNew;
+  if ( m_lastShots.size() == 0)
+    return;
+  int distance = 3000; // Unrealistisch hohe Distanz
+  DEBUG_LOG(m_logger) << "distance=" << distance;
+  for (const OpticalShot &os : qAsConst(m_lastShots))
+  {
+    int act_distance = os.deposition1 * os.deposition1 + os.deposition2 * os.deposition2;
+    DEBUG_LOG(m_logger) << "act_distance=" << act_distance << " BSM-ID=" << os.id
+                         << " ammunitionId=" << os.ammunitionId;
+    if ( act_distance < distance)
+    {
+      distance = act_distance;
+      DEBUG_LOG(m_logger) << "distance=" << distance << " BSM-ID=" << os.id;
+      osNew = os;
+    }
+  }
+
+  emit opticalShot(QDateTime::currentMSecsSinceEpoch(),
+                   osNew.id,
+                   osNew.originatorId,
+                   osNew.ammunitionId,
+                   osNew.deposition1,
+                   osNew.deposition2);
+  m_lastShots.clear();
+}
+
+void SocketHandler::handleObserverController(int id, const Telegrams::PanObserverController *tele)
+{
+  DamageState damageState = DamageState::Unknown;
+  if (tele->m_codeNo == 2)  // Zerstoerung
+    damageState = DamageState::Destroyed;
+  else if (tele->m_codeNo == 4 || tele->m_codeNo == 18 || tele->m_codeNo == 28)  // Aktivierung, Reaktivierung
+    damageState = DamageState::NoDamage;
+  if (damageState == DamageState::Unknown)
+    return;
+  if (m_timerOC.isValid())
+  {
+    if (m_timerOC.elapsed() < 500)  // < 500ms -> ignore event
+      return;
+  }
+  m_timerOC.start();
+
+  emit opticalShotOC(QDateTime::currentMSecsSinceEpoch(),
+                     id,
+                     tele->m_originatorId,
+                     tele->m_ammunitionId,
+                     damageState);
+}
+
+bool SocketHandler::findNextSyncStartByte(const QByteArray &buffer, int &iCurReadPos)
+{
+  // Suchen nach s_stx
+  while (iCurReadPos < buffer.size())
+  {
+    const char data = buffer.at(iCurReadPos);
+    if (data == Telegrams::Telegram::s_stx)
+      return true;
+    ++iCurReadPos;
+  }
+  return false;
+}
+
+bool SocketHandler::findNextSyncEndByte(const QByteArray &buffer, int &iCurReadPos)
+{
+  // Suchen nach s_endMark
+  while (iCurReadPos < buffer.size())
+  {
+    const char data = buffer.at(iCurReadPos);
+    if (data == Telegrams::Telegram::s_etx)
+      return true;
+    ++iCurReadPos;
+  }
+  return false;
+}
+
+static inline bool isSameAmmuniton(int a1, int a2)
+{
+  if (a1 == a2 - 1)
+    return true;
+  if (a1 == a2)
+    return true;
+  if (a1 == a2 + 1)
+    return true;
+  return false;
+}
+
+static inline bool isSameOriginator(int o1, int o2)
+{
+  if (o1 == o2)
+    return true;
+  // o1 == 0 || o2 == 0 is filtered out before
+  if (o1 == 0)
+    return true;
+  if (o2 == 0)
+    return true;
+  return false;
+}
+
+bool SocketHandler::isSameOpticalShot(const OpticalShot &o1, const OpticalShot &o2)
+{
+  // mehr als 100ms auseinander -> nicht der gleiche Treffer
+  if (o1.timeStampMSec < o2.timeStampMSec - s_opticalTimeSlot)
+    return false;
+  return (isSameAmmuniton(o1.ammunitionId, o2.ammunitionId) &&
+          isSameOriginator(o1.originatorId, o2.originatorId));
+}
+
+} // namespace BSM
+} // namespace BuildingControlUnit