Initial commit

Lts/Proxies/WirelessLocationSystem/wls_participant.h

@@ -0,0 +1,206 @@
+/**
+  * A single Participant
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Christian Ehrlicher, SLSE, 2017-01-10
+  */
+
+#pragma once
+
+#include <QByteArray>
+#include <QObject>
+#include <QQueue>
+#include <QStringBuilder>
+#include <QTimer>
+//#include <QDebug>
+
+#include <math.h>
+
+#include <TrainingAreaBase/training_area_telegrams.h>
+
+#include "logger.h"
+#include "settings.h"
+
+inline Q_CONSTEXPR double pi() { return M_PI; }
+inline Q_CONSTEXPR double deg2rad() { return pi() / 180.; }
+
+namespace WirelessLocationSystem
+{
+class Acknowledge_rx;
+class SfmMsgTetra_rx;
+class SfmMsgTetra_tx;
+
+class WlsParticipant : public QObject
+{
+  Q_OBJECT
+public:
+  static int calcWlsZone(int wlsAreaId, int wlsBuildingId, int wlsRoomNumber)
+  {
+    if (wlsAreaId == 0 && wlsBuildingId == 0 && wlsRoomNumber == 0)
+      return 3;
+    if (wlsAreaId != 0 && wlsBuildingId == 1002 && wlsRoomNumber == 1001)
+      return 4;
+    if (wlsAreaId != 0 && wlsBuildingId == 0 && wlsRoomNumber == 0)
+      return 5;
+
+    return 6;
+  }
+  static bool mustSendGpsOn(int wlsZoneOld, int wlsZoneNew)
+  {
+    // restart of service
+    if (wlsZoneOld == -1)
+      return wlsZoneNew <= 4;
+    // normal operation
+    return (wlsZoneOld >= 4 && wlsZoneNew <= 3);
+  }
+  static bool mustSendGpsOff(int wlsZoneOld, int wlsZoneNew)
+  {
+    // restart of service
+    if (wlsZoneOld == -1)
+      return wlsZoneNew >= 5;
+    // normal operation
+    return (wlsZoneOld <= 4 && wlsZoneNew >= 5);
+  }
+
+  enum class GpsMode : char
+  {
+    Off,
+    On
+  };
+  enum class DataCommunication : char
+  {
+    Unknown,
+    Off,
+    On
+  };
+
+  struct RelativePosition
+  {
+    double x = 0;
+    double y = 0;
+    double z = 0;
+    int wlsAreaId = 0;
+    int wlsBuildingId = 0;  // = externalWlsId
+    int wlsRoomNumber = 0;
+    int playerOrientation = 0;
+    int weaponOrientation = 0;
+    bool isValid = false;
+
+  } ;
+
+  static bool comparePos (const RelativePosition &rhs, const RelativePosition &lhs )
+  {
+    double dx = ((lhs.x/deg2rad())-(rhs.x/deg2rad()));
+    double dy = ((lhs.y/deg2rad())-rhs.y/deg2rad());
+    double dz = (lhs.z-rhs.z);
+    double dist = dx*dx + dy*dy + dz*dz;
+    //qWarning() << "Compare two pos: square distance= " << QString::number(dist,'g',10);
+
+    bool ret = dist < Settings::wlsSquareDistanceMax()
+        && lhs.wlsAreaId == rhs.wlsAreaId
+        && lhs.wlsBuildingId == rhs.wlsBuildingId
+        && lhs.wlsRoomNumber == rhs.wlsRoomNumber;
+    //  && lhs.isValid == rhs.isValid;
+    //qWarning() << "Return Value= " << ret;
+
+    return ret;
+  }
+protected:
+  explicit WlsParticipant(QObject *parent = nullptr);
+public:
+  void setPosition(const RelativePosition &newPosition, qint64 tsCurrentPos);
+  bool positionIsValid() const { return m_relativePosition.isValid; }
+  qint64 positionTimestamp() const { return m_lastValidPosition; }
+  RelativePosition position() const { return m_relativePosition; }
+  bool isGpsMode() const { return m_gpsMode == WlsParticipant::GpsMode::On; }
+  // returns true when timeout is reached
+  void checkGpsOnOffTimeoutTimer(qint64 tNow);
+  void setLastSeenWlsTelegramTime(qint64 ts);
+  qint64 lastSeenWlsTelegramTime() const { return m_lastSeenDataCommunication; }
+
+  void setWlsZone(int newWlsZone);
+  int wlsZone() const { return m_wlsZone; }
+
+  QByteArray getPlayerIdStr() const;
+
+  const QByteArray &tagId() const { return m_tagId; }
+  void setEquipmentNumber(int newEquipmentNumber);
+  int equipmentNumber() const { return m_equipmentNumber; }
+  int exerciseId() const { return m_exerciseId; }
+  void setExerciseId(int exerciseId) { m_exerciseId = exerciseId; }
+  bool isDataCommunicationPossible() const { return m_dataCommunication == DataCommunication::On; }
+  DataCommunication dataCommunication() const { return m_dataCommunication; }
+  GpsMode gpsMode() const { return m_gpsMode; }
+
+  bool handleSfmRxData(const SfmMsgTetra_rx *sfmTele);
+  bool isSfmRxDataAvailable() const { return m_bSfmPayloadReady; }
+  uint8_t nextSfmRxDataFragmentCounter() const { return m_sfmRxPayloadFragmentNumber; }
+  const QByteArray &sfmRxData() const { return m_sfmRxPayloadData; }
+
+  static bool calcChunkCountAndLen(const QByteArray &data, int &chunkCount, int &chunkLen);
+  void sendBinarySfmData(const std::vector<unsigned char> &data);
+  void handleWlsAck(const Acknowledge_rx *ack);
+
+  TrainingAreaBase::TetraIcd::WlsParticipantState wlsParticipantState() const;
+
+  // TODO
+  double voltage = 0;
+  qint64 lastEquipmentNumberRequest = 0;
+
+Q_SIGNALS:
+  void positionChanged(WlsParticipant *p);
+  void gpsOnOffChanged(WlsParticipant *p);
+  void wlsBinarySfmDataComplete(WlsParticipant *p);
+  void dataCommunicationStateChanged(WlsParticipant *p);
+  void dataForWls(WlsParticipant *p, const SfmMsgTetra_tx &data);
+private Q_SLOTS:
+  void checkSendTele();
+  void sendNextWlsTelegram(bool bIsResend);
+  void onWlsTeleTimeout();
+private:
+  uint8_t getNextSendCounter(bool bIsResend);
+  bool isRecvCounterCorrect(uint8_t recvCounter);
+private:
+  struct WlsPayloadData
+  {
+    QByteArray payload;
+    uint8_t retryCount = 0;
+    uint8_t fragmentCount = 0;
+    uint8_t fragmentNumber = 0;
+  };
+
+  friend class WlsParticipantHandler;
+  QByteArray m_tagId;
+  QByteArray m_sfmRxPayloadData;
+  QQueue<WlsPayloadData> m_currentFragmentedTeleQueue;
+  QQueue<WlsPayloadData> m_wlsQueue;
+
+  uint8_t m_sfmRxPayloadFragmentNumber = 0;
+  bool m_bSfmPayloadReady = false;
+
+  int m_equipmentNumber = 0;
+  int m_exerciseId = 0;
+  // zone 1 and 2 will never be met here - far outside wls area
+  int m_wlsZone = -1;  // 1 .. 6
+
+  RelativePosition m_relativePosition;
+  GpsMode m_gpsMode = GpsMode::On;
+  DataCommunication m_dataCommunication = DataCommunication::Unknown;
+  qint64 m_lastSeenDataCommunication = 0;
+  qint64 m_lastValidPosition = 0;
+  qint64 m_lastSeenPosition = 0;
+
+  QTimer *m_wlsTeleTimeoutTimer = nullptr;
+  int m_wlsTeleTimeoutCounter = 0;
+  uint8_t m_nextSendCounter = 0;
+  uint8_t m_lastRecvCounter = 0;
+
+  NGL::INGLLogger &m_logger;
+};
+
+std::ostream &operator<<(std::ostream &stream, WlsParticipant::GpsMode gm);
+std::ostream &operator<<(std::ostream &stream, WlsParticipant::DataCommunication dc);
+
+} // namespace WirelessLocationSystem
+