Adds initial intermediate qt6 porting state

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/CMakeLists.txt

@@ -0,0 +1,66 @@
+# Lib / module for TMS-IF communication - has to be its own lib because of conflicting DDS implementations
+#
+# $Id$
+# $HeadURL$
+# $Author$
+#
+
+FIND_PACKAGE(TMSIF REQUIRED)
+
+if(WIN32)
+  message(FATAL_ERROR "This won't work on Windows !")
+endif()
+
+
+# plugin
+set(src
+  ../tms_if_adapter.h
+  tms_if_adapter.cpp
+  ../uav_commander.h
+  uav_commander_impl.h
+  uav_commander_impl.cpp
+  abstract_dispatcher.h
+  reader.h
+  communication_master.h
+  logger.h
+  logger.cpp
+
+  dispatchers/report_positions.h
+  dispatchers/report_positions.cpp
+  dispatchers/report_mission_status.h
+  dispatchers/report_mission_status.cpp
+  dispatchers/report_radio_link_connection_state.h
+  dispatchers/report_radio_link_connection_state.cpp
+  dispatchers/report_cbit_status.h
+  dispatchers/report_cbit_status.cpp
+  dispatchers/report_device_status.h
+  dispatchers/report_device_status.cpp
+  dispatchers/report_point.h
+  dispatchers/report_point.cpp
+)
+
+NGL_ADD_LIBRARY2(SV TmsIfAdapter MODULE ${src})
+NGL_TARGET_INCLUDE_DIRECTORIES(TmsIfAdapter
+  PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../
+)
+
+NGL_TARGET_LINK_LIBRARIES(TmsIfAdapter
+PRIVATE
+  TMSIF::tmsif-cyclone
+  TMSIF::ddscxx
+  TMSIF::ddkit-cyclone
+
+  LtsUtilsCoreHeaderOnly
+  LtsCoordTrans
+  NGLLogger
+
+  Qt5::Core
+  Qt5::Gui
+  Qt5::Network
+  Qt5::WebSockets
+)
+
+target_compile_definitions(TmsIfAdapter PUBLIC DDS_VENDOR_CYCLONE)
+
+NGL_INSTALL_TARGET(TmsIfAdapter)

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/abstract_dispatcher.h

@@ -0,0 +1,78 @@
+
+
+
+#pragma once
+
+#include "reader.h"
+
+#include <LtsUtilsCore/await.h>
+
+#include <ddkit/actors/Session.hpp>
+
+#include <QObject>
+#include <QMutex>
+#include <QQueue>
+#include <QVector>
+#include <QMetaObject>
+#include <QEventLoop>
+#include <QMutexLocker>
+
+
+namespace UAS
+{
+template<typename T>
+class AbstractDispatcher
+{
+public:
+  void dispatch(const T& data) 
+  {
+    {
+      QMutexLocker lock(&m_mutex);
+      m_queue.enqueue(data);
+    }
+
+    
+    QVector<T> batch;
+    {
+      QMutexLocker lock(&m_mutex);
+      if (m_queue.isEmpty())
+      {
+        return;
+      }
+      batch.reserve(m_queue.size());
+      while (!m_queue.isEmpty())
+      {
+        batch.append(m_queue.dequeue());
+      }
+    }
+
+    QMutexLocker lock(&m_mutex);
+    for (auto *r : m_readers)
+    {
+      for (auto &d : batch)
+      {
+        r->enqueueDataDirect(d);
+      }
+    }
+  }
+
+
+  void registerReader(Reader<T>* r)
+  {
+    QMutexLocker lock(&m_mutex);
+    m_readers.append(r);
+  }
+
+
+  void unregisterReader(Reader<T>* r)
+  {
+    QMutexLocker lock(&m_mutex);
+    m_readers.removeAll(r);
+  }
+
+private:
+  QMutex m_mutex;
+  QQueue<T> m_queue;
+  QVector<Reader<T> *> m_readers;
+};
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/communication_master.h

@@ -0,0 +1,24 @@
+
+
+
+#pragma once
+
+#include <string>
+
+
+template<typename ClientType, typename DispatcherType>
+class CommunicationMaster 
+{
+public:
+  CommunicationMaster(std::string serviceName, int domainId, bool readOnly = false)
+    : m_dispatcher(new DispatcherType)
+    , m_client(readOnly ? ClientType::createMonitor(domainId, serviceName, *m_dispatcher) : ClientType::create(domainId, serviceName, *m_dispatcher))
+  {}
+
+  DispatcherType &dispatcher() { return *m_dispatcher; }
+  ClientType &client() { return m_client; }
+
+private:
+  DispatcherType *m_dispatcher;
+  ClientType m_client;
+};

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_cbit_status.cpp

@@ -0,0 +1,52 @@
+
+
+
+#include "report_cbit_status.h"
+
+
+namespace UAS
+{
+namespace BuiltInTest
+{
+TestResult::Type testResultFromTms(M_BuiltInTest::TestResult::Type testResultType)
+{
+  switch (testResultType)
+  {
+    case M_BuiltInTest::TestResult::OK: return TestResult::OK;
+    case M_BuiltInTest::TestResult::Fatal_Error: return TestResult::Fatal_Error;
+    case M_BuiltInTest::TestResult::Warning: return TestResult::Warning;
+    case M_BuiltInTest::TestResult::Reduced_Performance_Error: return TestResult::Reduced_Performance_Error;
+    case M_BuiltInTest::TestResult::Undefined: return TestResult::Undefined;
+  }
+}
+
+
+namespace ReportCBitStatus
+{
+Dispatcher::Dispatcher()
+  : m_logger(getLogger("ReportCBitStatus.Dispatcher"))
+{}
+
+void Dispatcher::ReportCBitStatus(const std::string &KeyId, const M_BuiltInTest::BitStatusType &status, bool IsRemoved)
+{
+  if (IsRemoved)
+  {
+    DEBUG_LOG(m_logger) << "isRemoved";
+    return;
+  }
+
+  Message resultMessage;
+  resultMessage.Key = KeyId;
+  resultMessage.Result = testResultFromTms(status.Result);
+  if (!status.ErrorCode.empty())
+  {
+    resultMessage.ErrorCode = status.ErrorCode.at(0);
+  }
+  resultMessage.ErrorCodeDescription = status.ErrorCodeDescription;
+  resultMessage.RoutineName = status.RoutineName;
+
+  dispatch(resultMessage);
+}
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_cbit_status.h

@@ -0,0 +1,32 @@
+
+
+
+#pragma once
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <TmsIfAdapter/src/abstract_dispatcher.h>
+#include <TmsIfAdapter/src/logger.h>
+
+#include <tmsif/api/BuiltInTest.hpp>
+
+
+namespace UAS
+{
+namespace BuiltInTest
+{
+TestResult::Type testResultFromTms(M_BuiltInTest::TestResult::Type testResultType);
+
+
+namespace ReportCBitStatus
+{
+class Dispatcher : public AbstractDispatcher<Message>, public M_BuiltInTest::IClient
+{
+public:
+  Dispatcher();
+  void ReportCBitStatus(const std::string &KeyId, const M_BuiltInTest::BitStatusType &status, bool IsRemoved) override;
+private:
+  NGL::INGLLogger &m_logger;
+};
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_device_status.cpp

@@ -0,0 +1,71 @@
+
+
+
+#include "report_device_status.h"
+
+
+
+namespace UAS
+{
+namespace Device
+{
+OperationModeStateKind::Type operationModeStateKindFromTms(M_Common::OperationModeStateKind::Type testResultType)
+{
+  switch (testResultType)
+  {
+    case M_Common::OperationModeStateKind::Undefined: return OperationModeStateKind::Undefined;
+    case M_Common::OperationModeStateKind::Off: return OperationModeStateKind::Off;
+    case M_Common::OperationModeStateKind::On: return OperationModeStateKind::On;
+    case M_Common::OperationModeStateKind::Standby: return OperationModeStateKind::Standby;
+    case M_Common::OperationModeStateKind::Init: return OperationModeStateKind::Init;
+    case M_Common::OperationModeStateKind::Shutdown:return OperationModeStateKind::Shutdown;
+    case M_Common::OperationModeStateKind::Normal: return OperationModeStateKind::Normal;
+    case M_Common::OperationModeStateKind::Manual: return OperationModeStateKind::Manual;
+    case M_Common::OperationModeStateKind::Auto: return OperationModeStateKind::Auto;
+    case M_Common::OperationModeStateKind::Calibrating: return OperationModeStateKind::Calibrating;
+    case M_Common::OperationModeStateKind::Maintenance: return OperationModeStateKind::Maintenance;
+  }
+  return OperationModeStateKind::Undefined; 
+}
+
+
+namespace ReportDeviceStatus
+{
+Dispatcher::Dispatcher()
+  : m_logger(getLogger("ReportDeviceStatus.Dispatcher"))
+{}
+
+void Dispatcher::ReportDeviceStatus(M_Device::DeviceStatusType const& status, bool IsRemoved)
+{
+  DEBUG_LOG(m_logger) << "start";
+
+  if (IsRemoved)
+  {
+    DEBUG_LOG(m_logger) << "isRemoved";
+    return;
+  }
+
+  if (status.OperationStatus.Mode.empty())
+  {
+    DEBUG_LOG(m_logger) << "no mode!";
+    return;
+  }
+
+  if (status.OperationStatus.Mode.front().EnumeratedValue.empty())
+  {
+    DEBUG_LOG(m_logger) << "no enum value in mode!";
+    return;
+  }
+
+  auto val = status.OperationStatus.Mode.front().EnumeratedValue.at(0);
+
+  Message msg;
+  msg.Status = operationModeStateKindFromTms(val);
+
+  DEBUG_LOG(m_logger) << "dispatch!";
+
+  dispatch(msg);
+} 
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_device_status.h

@@ -0,0 +1,33 @@
+
+
+
+#pragma once
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <TmsIfAdapter/src/abstract_dispatcher.h>
+#include <TmsIfAdapter/src/logger.h>
+
+#include <tmsif/api/Device.hpp>
+#include <tmsif/api/Common.hpp>
+
+
+namespace UAS
+{
+namespace Device
+{
+OperationModeStateKind::Type operationModeStateKindFromTms(M_Common::OperationModeStateKind::Type testResultType);
+
+
+namespace ReportDeviceStatus
+{
+class Dispatcher : public AbstractDispatcher<Message>, public M_Device::IClient
+{
+public:
+  Dispatcher();
+  void ReportDeviceStatus(M_Device::DeviceStatusType const& status, bool IsRemoved) override;
+private:
+  NGL::INGLLogger &m_logger;
+};
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_mission_status.cpp

@@ -0,0 +1,101 @@
+
+
+
+#include "report_mission_status.h"
+
+
+
+namespace UAS
+{
+namespace MissionCtrl
+{
+Task taskFromTms(M_MissionCtrl::TaskCategoryKind::Type taskType)
+{
+  switch (taskType) 
+  {
+    case M_MissionCtrl::TaskCategoryKind::Type::Undefined: return Task::Undefined;
+    case M_MissionCtrl::TaskCategoryKind::Type::Connect: return Task::Connect; 
+    case M_MissionCtrl::TaskCategoryKind::Type::Disconnect: return Task::Disconnect;
+    case M_MissionCtrl::TaskCategoryKind::Type::Arm: return Task::Arm;
+    case M_MissionCtrl::TaskCategoryKind::Type::Disarm: return Task::Disarm;
+    case M_MissionCtrl::TaskCategoryKind::Type::Fire: return Task::Fire;
+    case M_MissionCtrl::TaskCategoryKind::Type::Observe: return Task::Observe;
+    case M_MissionCtrl::TaskCategoryKind::Type::Climb: return Task::Climb;
+    case M_MissionCtrl::TaskCategoryKind::Type::Descend: return Task::Descend;
+    case M_MissionCtrl::TaskCategoryKind::Type::Takeoff: return Task::Takeoff;
+    case M_MissionCtrl::TaskCategoryKind::Type::Land: return Task::Land;
+    case M_MissionCtrl::TaskCategoryKind::Type::Reboot: return Task::Reboot;
+    case M_MissionCtrl::TaskCategoryKind::Type::Shutdown: return Task::Shutdown;
+    case M_MissionCtrl::TaskCategoryKind::Type::Loiter: return Task::Loiter;
+    case M_MissionCtrl::TaskCategoryKind::Type::Return: return Task::Return;
+    case M_MissionCtrl::TaskCategoryKind::Type::Follow: return Task::Follow;
+    case M_MissionCtrl::TaskCategoryKind::Type::Stop: return Task::Stop;
+    case M_MissionCtrl::TaskCategoryKind::Type::Resume: return Task::Resume;
+  }
+  return Task::Undefined; 
+}
+
+
+namespace ReportMissionStatus
+{
+Dispatcher::Dispatcher()
+  : m_logger(getLogger("ReportMissionStatus.Dispatcher"))
+{}
+
+void Dispatcher::ReportMissionStatus(M_MissionCtrl::MissionStatusType const& MissionStatus, bool IsRemoved)
+{
+  if (IsRemoved)
+  {
+    DEBUG_LOG(m_logger) << "isRemoved";
+    return;
+  }
+
+  if (MissionStatus.CurrentAction.CurrentTask.EnumeratedValue.empty())
+  {
+    DEBUG_LOG(m_logger) << "MissionStatus.CurrentAction.CurrentTask.EnumeratedValue empty";
+    return;
+  }
+
+  if (!MissionStatus.CurrentAction.CurrentTask.TextualValue.empty())
+  {
+    DEBUG_LOG(m_logger) << "text: " << QString::fromStdString(MissionStatus.CurrentAction.CurrentTask.TextualValue.front());
+  }
+  const Task currentTask = taskFromTms(MissionStatus.CurrentAction.CurrentTask.EnumeratedValue.front());
+
+  std::vector<Task> currentlyAllowedTasks;
+
+  for (const auto &tmsIfTask : qAsConst(MissionStatus.CurrentlyAllowedTasks))
+  {
+    if (tmsIfTask.EnumeratedValue.empty()) 
+    {
+      return;
+    }
+    const auto _tmsIfTask = tmsIfTask.EnumeratedValue.front();
+
+    const auto task = taskFromTms(_tmsIfTask);
+    if (task != Task::Undefined) 
+    {
+      currentlyAllowedTasks.push_back(task);
+    }
+  }
+
+  std::string missionName = MissionStatus.CurrentMissionName;
+  DEBUG_LOG(m_logger) << "missionName before: " << missionName;
+  if (missionName.empty())
+  {
+    for (const auto &additionalInfo : qAsConst(MissionStatus.CurrentAction.CurrentTask.AdditionalInfo))
+    {
+      if (additionalInfo.Name == "MissionName" && additionalInfo.StrValue != "NoMission") 
+      {
+        missionName = additionalInfo.StrValue;
+        break;
+      }
+    }
+  }
+  DEBUG_LOG(m_logger) << "missionName after: " << missionName;
+
+  dispatch({ missionName, currentTask, currentlyAllowedTasks, {} });
+}
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_mission_status.h

@@ -0,0 +1,31 @@
+
+
+
+#pragma once
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <TmsIfAdapter/src/abstract_dispatcher.h>
+#include <TmsIfAdapter/src/logger.h>
+
+#include <tmsif/api/MissionCtrl.hpp>
+
+
+namespace UAS
+{
+namespace MissionCtrl
+{
+Task taskFromTms(M_MissionCtrl::TaskCategoryKind::Type taskType);
+
+namespace ReportMissionStatus
+{
+class Dispatcher : public AbstractDispatcher<Message>, public M_MissionCtrl::IClient
+{
+public:
+  Dispatcher();
+  void ReportMissionStatus(M_MissionCtrl::MissionStatusType const& MissionStatus, bool IsRemoved) override;
+private:
+  NGL::INGLLogger &m_logger;
+};
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_point.cpp

@@ -0,0 +1,40 @@
+
+
+
+#include "report_point.h"
+
+
+
+namespace UAS
+{
+namespace ObjectManager
+{
+namespace ReportPoint
+{
+Dispatcher::Dispatcher()
+  : m_logger(getLogger("ReportPoint.Dispatcher"))
+{}
+
+void Dispatcher::ReportPoint(const M_Common::ObjectIdType &KeyId, const M_ObjectManager::PointObjectType &, bool IsRemoved)
+{
+  DEBUG_LOG(m_logger) << "start";
+
+  Message msg;
+
+  try {
+  msg.uasId = std::stoi(KeyId);
+  }catch(...)
+  {
+    ERROR_LOG(m_logger) << "KeyId not an int !";
+    return;
+  }
+
+  msg.isRemoved = IsRemoved;
+
+  DEBUG_LOG(m_logger) << "dispatch!";
+
+  dispatch(msg);
+}
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_point.h

@@ -0,0 +1,30 @@
+
+
+
+#pragma once
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <TmsIfAdapter/src/abstract_dispatcher.h>
+#include <TmsIfAdapter/src/logger.h>
+
+#include <tmsif/api/ObjectManager.hpp>
+#include <tmsif/api/Common.hpp>
+
+
+namespace UAS
+{
+namespace ObjectManager
+{
+namespace ReportPoint
+{
+class Dispatcher : public AbstractDispatcher<Message>, public M_ObjectManager::IClient
+{
+public:
+  Dispatcher();
+  void ReportPoint(const M_Common::ObjectIdType &KeyId, const M_ObjectManager::PointObjectType &Obj, bool IsRemoved) override;
+private:
+  NGL::INGLLogger &m_logger;
+};
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_positions.cpp

@@ -0,0 +1,71 @@
+
+
+
+#include "report_positions.h"
+
+
+
+namespace UAS
+{
+namespace CoordinateFrame
+{
+namespace ReportPositions
+{
+Dispatcher::Dispatcher()
+  : m_logger(getLogger("ReportPositions.Dispatcher"))
+{}
+
+void Dispatcher::ReportPositions(const std::string &KeyId, const M_Common::PositionListType &PositionList, bool IsRemoved)
+{
+  if (IsRemoved)
+  {
+    DEBUG_LOG(m_logger) << "isRemoved";
+    return;
+  }
+
+  
+  if (KeyId != "UTM_GLOBAL_POSITION" && KeyId != "CURRENT_AIRFRAME_RAW" && KeyId != "CURRENT_AIRFRAME")
+  {
+    DEBUG_LOG(m_logger) << "KeyId unwanted (is '" << QString::fromStdString(KeyId) << "')";
+    return;
+  }
+
+
+  
+  if (PositionList.empty())
+  {
+    DEBUG_LOG(m_logger) << "PositionList is empty";
+    return;
+  }
+
+  const auto &pos = PositionList.front();
+
+  if (pos.Ellipsoidal.empty())
+  {
+    DEBUG_LOG(m_logger) << "PositionList.Ellipsoidal is empty";
+    return;
+  }
+
+  const auto &elip = pos.Ellipsoidal.front();
+
+  if (elip.Altitude.empty())
+  {
+    DEBUG_LOG(m_logger) << "PositionList.Ellipsoidal.Altitude is empty";
+    return;
+  }
+
+  const double lat = elip.Latitude, lon = elip.Longitude, alt = elip.Altitude.front();
+
+  if (pos.Timestamp.empty())
+  {
+    DEBUG_LOG(m_logger) << "PositionList[0].Timestamp is empty";
+    return;
+  }
+
+  const auto timestamp = pos.Timestamp.front().Seconds;
+
+  dispatch({ lat, lon, alt, timestamp });
+}
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_positions.h

@@ -0,0 +1,30 @@
+
+
+
+#pragma once
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <TmsIfAdapter/src/abstract_dispatcher.h>
+#include <TmsIfAdapter/src/logger.h>
+
+#include <tmsif/api/CoordinateFrame.hpp>
+#include <tmsif/api/Common.hpp>
+
+
+namespace UAS
+{
+namespace CoordinateFrame
+{
+namespace ReportPositions
+{
+class Dispatcher : public AbstractDispatcher<Message>, public M_CoordinateFrame::IClient
+{
+public:
+  Dispatcher();
+  void ReportPositions(const std::string &KeyId, const M_Common::PositionListType &PositionList, bool IsRemoved) override;
+private:
+  NGL::INGLLogger &m_logger;
+};
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_radio_link_connection_state.cpp

@@ -0,0 +1,53 @@
+
+
+
+#include "report_radio_link_connection_state.h"
+
+
+
+namespace UAS
+{
+namespace TeleOperation
+{
+Type typeFromTms(M_Teleoperation::RadioLinkConnectionState::Type radioLinkConnectionState)
+{
+  switch (radioLinkConnectionState)
+  {
+    case M_Teleoperation::RadioLinkConnectionState::Undefined: return Type::Undefined;
+    case M_Teleoperation::RadioLinkConnectionState::Disconnected: return Type::Disconnected;
+    case M_Teleoperation::RadioLinkConnectionState::Connecting: return Type::Connecting;
+    case M_Teleoperation::RadioLinkConnectionState::Diconnecting: return Type::Diconnecting; 
+    case M_Teleoperation::RadioLinkConnectionState::Connected: return Type::Connected;
+    case M_Teleoperation::RadioLinkConnectionState::Error: return Type::Error;
+  }
+  return Type::Undefined; 
+}
+
+
+namespace RadioLinkConnectionState
+{
+Dispatcher::Dispatcher()
+  : m_logger(getLogger("RadioLinkConnectionState.Dispatcher"))
+{}
+
+void Dispatcher::ReportRadioConnectionStatus(const std::string &, const M_Teleoperation::RadioConnectionStatusType &RadioConnectionStatus, bool IsRemoved)
+{
+  if (IsRemoved)
+  {
+    DEBUG_LOG(m_logger) << "isRemoved";
+    return;
+  }
+
+  if (RadioConnectionStatus.LinkConnectionState == M_Teleoperation::RadioLinkConnectionState::Undefined)
+  {
+    DEBUG_LOG(m_logger) << "MissionStatus.CurrentAction.CurrentTask.EnumeratedValue empty";
+    return;
+  }
+
+  const Type currentConnectionType = typeFromTms(RadioConnectionStatus.LinkConnectionState);
+
+  dispatch({ currentConnectionType });
+}
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/dispatchers/report_radio_link_connection_state.h

@@ -0,0 +1,32 @@
+
+
+
+#pragma once
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <TmsIfAdapter/src/abstract_dispatcher.h>
+#include <TmsIfAdapter/src/logger.h>
+
+#include <tmsif/api/Teleoperation.hpp>
+
+
+namespace UAS
+{
+namespace TeleOperation
+{
+Type typeFromTms(M_Teleoperation::RadioLinkConnectionState::Type radioLinkConnectionState);
+
+
+namespace RadioLinkConnectionState
+{
+class Dispatcher : public AbstractDispatcher<Message>, public M_Teleoperation::IClient
+{
+public:
+  Dispatcher();
+  void ReportRadioConnectionStatus(std::string const& KeyId, M_Teleoperation::RadioConnectionStatusType const& RadioConnectionStatus, bool IsRemoved) override;
+private:
+  NGL::INGLLogger &m_logger;
+};
+} 
+} 
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/logger.cpp

@@ -0,0 +1,31 @@
+
+
+
+#include "logger.h"
+
+#include <NGLLogger/NGLLoggerBootstrap.h>
+
+
+namespace UAS
+{
+LOGGER_T getLogger(const char *name)
+{
+  static const char ns[] = "TAB.UAS.Adapter";
+  std::string loggerName;
+  const size_t len = strlen(name);
+  loggerName.reserve(sizeof(ns) + len);
+  loggerName.append(ns, sizeof(ns) - 1);
+  loggerName.append(name, len);
+
+  try
+  {
+    return NGL::NGLLoggerBootstrap::getNGLRootLogger().
+           getNGLLogger(loggerName);
+  }
+  catch (const std::exception &ex)
+  {
+    fprintf(stderr, "Error retrieving logger: %s", ex.what());
+    abort();
+  }
+}
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/logger.h

@@ -0,0 +1,18 @@
+
+
+
+#pragma once
+
+#include <NGLLogger/INGLLogger.h>
+
+
+namespace UAS
+{
+using LOGGER_T = NGL::INGLLogger&;
+
+LOGGER_T getLogger(const char *name);
+inline LOGGER_T getLogger(const QString &loggerName)
+{
+  return getLogger(loggerName.toUtf8().constData());
+}
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/reader.h

@@ -0,0 +1,84 @@
+
+
+
+#pragma once
+
+#include <QObject>
+#include <QQueue>
+#include <QMutex>
+
+
+namespace UAS
+{
+class ReaderSignal : public QObject 
+{
+  Q_OBJECT
+
+public:
+  void emitDataReady()
+  {
+    emit dataReady();
+  }
+
+signals:
+  void dataReady();
+};
+
+
+template<typename T> class AbstractDispatcher;
+
+
+template<typename T>
+class Reader
+{
+public:
+  explicit Reader(AbstractDispatcher<T> &dispatcher)
+    : m_readerSignal(new ReaderSignal)
+    , m_dispatcher(dispatcher)
+  {
+    m_dispatcher.registerReader(this);
+  }
+
+
+  ~Reader()
+  {
+    delete m_readerSignal;
+    m_dispatcher.unregisterReader(this);
+  }
+
+
+  ReaderSignal *readerSignal()
+  {
+    return m_readerSignal;
+  }
+
+
+  T getNext() noexcept(false)
+  {
+    QMutexLocker lock(&m_mutex);
+    if (!m_fifo.isEmpty())
+    {
+      return dequeue();
+    }
+    throw std::runtime_error("no next");
+  }
+
+
+  void enqueueDataDirect(const T& data) 
+  {
+    m_fifo.enqueue(data);
+    m_readerSignal->emitDataReady();
+  }
+
+private:
+  T dequeue()
+  {
+    return std::move(m_fifo.dequeue());
+  }
+
+  ReaderSignal *m_readerSignal;
+  AbstractDispatcher<T> &m_dispatcher;
+  QQueue<T> m_fifo;
+  QMutex m_mutex;
+};
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/tms_if_adapter.cpp

@@ -0,0 +1,196 @@
+ 
+
+
+#include "../tms_if_adapter.h"
+#include "uav_commander_impl.h"
+#include "dispatchers/report_point.h"
+#include "logger.h"
+
+#include <QObject>
+
+#include <set>
+
+
+namespace UAS
+{
+class InterfaceAdapterInitializer;
+
+
+
+class TmsIfAdapterImpl final : public TmsIfAdapter
+{
+public:
+  TmsIfAdapterImpl();
+  ~TmsIfAdapterImpl();
+
+  bool isReady() const override
+  {
+    return m_isReady;
+  }
+
+  void set_isReady_Callback(std::function<void()> callbackFn) override
+  {
+    m_isReady_Callback = callbackFn;
+    
+    m_iai = nullptr;
+  }
+
+  UAVCommander *createUAV(int domainId, int eqId) override
+  {
+    assert(m_isReady);
+    return new UAVCommanderImpl(domainId, eqId, m_guard);
+  }
+
+  void setReady() 
+  {
+    m_isReady = true;
+    m_isReady_Callback();
+  }
+
+private:
+  QObject *m_guard { new QObject };
+  bool m_isReady { false };
+  std::function<void()> m_isReady_Callback;
+  InterfaceAdapterInitializer *m_iai;
+  NGL::INGLLogger &m_logger;
+};
+
+
+
+
+class InterfaceAdapterInitializer : public QObject
+{
+  Q_OBJECT
+public:
+  InterfaceAdapterInitializer(TmsIfAdapterImpl *parent)
+    : m_parent(parent)
+    , m_master("UAV_ObjectManager", 4)
+    , m_reader(m_master.dispatcher())
+    , m_writer(m_master.client())
+    , m_logger(getLogger("InterfaceAdapterInitializer"))
+  {
+    ERROR_LOG(m_logger) << "ctor";
+
+    connect(m_reader.readerSignal(), &ReaderSignal::dataReady, this, &InterfaceAdapterInitializer::updatePoint);
+
+    QTimer *t = new QTimer(this);
+    t->setInterval(3000);
+    connect(t, &QTimer::timeout, this, [this, t]()
+    {
+      if (m_activeUas.empty())
+      {
+        ERROR_LOG(m_logger) << "trying to stop active uas - DONE!";
+
+        t->stop();
+        m_parent->setReady();
+        deleteLater();
+        return;
+      }
+
+      ERROR_LOG(m_logger) << "trying to stop active uas:" << m_activeUas.size();
+
+      for (const int activeUas : m_activeUas)
+      {
+        try{
+          m_writer->CmdRemove(std::to_string(activeUas));
+        }catch(...){}
+      }
+    });
+    t->start();
+  }
+
+private slots:
+  void updatePoint()
+  {
+    while (true)
+      {
+        try
+        {
+          const auto msg = m_reader.getNext();
+
+          if (msg.isRemoved)
+          {
+            m_activeUas.erase(msg.uasId);
+          }
+          else
+          {
+            m_activeUas.insert(msg.uasId);
+          }
+        }
+        catch (...)
+        {
+          break;
+        }
+      }
+  }
+
+private:
+  TmsIfAdapterImpl *m_parent;
+
+  
+  mutable CommunicationMaster<M_ObjectManager::Client, ObjectManager::ReportPoint::Dispatcher> m_master;
+  Reader<ObjectManager::ReportPoint::Message> m_reader;
+  M_ObjectManager::Client m_writer;
+
+  std::set<int> m_activeUas;
+  NGL::INGLLogger &m_logger;
+};
+
+
+TmsIfAdapterImpl::TmsIfAdapterImpl()
+  : m_iai(new InterfaceAdapterInitializer(this))
+  , m_logger(getLogger("TmsIfAdapterImpl"))
+{
+  ERROR_LOG(m_logger) << "TmsIfAdapter running!";
+}
+
+
+TmsIfAdapterImpl::~TmsIfAdapterImpl()
+{
+  delete m_iai;
+  delete m_guard;
+  ERROR_LOG(m_logger) << "!!! TmsIfAdapterImpl DTOR!";
+}
+
+
+
+
+std::unique_ptr<TmsIfAdapterImpl> s_instance;
+
+
+TmsIfAdapter &TmsIfAdapter::get()
+{
+  if (!s_instance)
+  {
+    s_instance = std::make_unique<TmsIfAdapterImpl>();
+  }
+
+  return *s_instance.get();
+}
+
+
+void TmsIfAdapter::stop()
+{
+  if (s_instance)
+  {
+    s_instance = nullptr;
+  }
+}
+} 
+
+
+
+
+extern "C" __attribute__((visibility("default"))) void* get()
+{
+  return &UAS::TmsIfAdapter::get();
+}
+
+
+extern "C" __attribute__((visibility("default"))) void stop()
+{
+  UAS::TmsIfAdapter::stop();
+}
+
+
+#include <tms_if_adapter.moc>

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/uav_commander_impl.cpp

@@ -0,0 +1,788 @@
+
+
+
+#include "uav_commander_impl.h"
+#include "logger.h"
+
+#include <LtsCoordTrans/lts_coord_trans.h>
+
+#include <QTimer>
+#include <QDateTime>
+
+
+using namespace LtsUtils;
+
+
+namespace UAS
+{
+Resolver::Resolver(QObject *parent)
+  : QObject(parent)
+{
+  connect(this, &Resolver::resolve, this, &QObject::deleteLater);
+  connect(this, &Resolver::reject, this, [this](){deleteLater();});
+}
+
+
+UAVCommanderImpl::UAVCommanderImpl(int domainId, int uasId, QObject *parent)
+  : QObject(parent)
+  , m_domainId(domainId)
+  , m_uasId(uasId)
+  , m_MissionCtrl(std::string("UAV_MissionCtrl_") + std::to_string(uasId), domainId)
+  , m_TeleOperation(std::string("UAV_Teleoperation_") + std::to_string(uasId), domainId)
+  , m_BuildInTest(std::string("UAV_BuiltInTest_") + std::to_string(uasId), domainId)
+  , m_Device(std::string("UAV_Device_") + std::to_string(uasId), domainId)
+  , m_CoordinateFrame(std::string("UAV_CoordinateFrame_") + std::to_string(uasId), domainId)
+  , m_logger(getLogger("UAVCommanderImpl"))
+{
+  ERROR_LOG(m_logger) << "ctor: " << (quintptr)this;
+
+  connect(m_MissionCtrl.reader.readerSignal(), &ReaderSignal::dataReady, this, &UAVCommanderImpl::updateMissionStatus);
+  connect(m_TeleOperation.reader.readerSignal(), &ReaderSignal::dataReady, this, &UAVCommanderImpl::updateOperationalState);
+  connect(m_BuildInTest.reader.readerSignal(), &ReaderSignal::dataReady, this, &UAVCommanderImpl::updateCBitStaus);
+  connect(m_Device.reader.readerSignal(), &ReaderSignal::dataReady, this, &UAVCommanderImpl::updateDeviceModeState);
+  connect(m_CoordinateFrame.reader.readerSignal(), &ReaderSignal::dataReady, this, &UAVCommanderImpl::updatePosition);
+
+  
+  updateDeviceModeState();
+  updateMissionStatus();
+}
+
+
+UAVCommanderImpl::TeleOperationContainer::TeleOperationContainer(const std::string &serviceName, int domainId)
+  : master(serviceName, domainId, true)
+  , reader(master.dispatcher())
+{}
+
+
+UAVCommanderImpl::BuildInTestContainer::BuildInTestContainer(const std::string &serviceName, int domainId)
+  : master(serviceName, domainId, true)
+  , reader(master.dispatcher())
+{}
+
+
+UAVCommanderImpl::DeviceContainer::DeviceContainer(const std::string &serviceName, int domainId)
+  : master(serviceName, domainId)
+  , reader(master.dispatcher())
+  , writer(master.client())
+{}
+
+
+UAVCommanderImpl::CoordinateFrameContainer::CoordinateFrameContainer(const std::string &serviceName, int domainId)
+  : master(serviceName, domainId, true)
+  , reader(master.dispatcher())
+{}
+
+
+UAVCommanderImpl::MissionCtrlContainer::MissionCtrlContainer(const std::string &serviceName, int domainId)
+  : master(serviceName, domainId)
+  , reader(master.dispatcher())
+  , writer(master.client())
+{}
+
+
+UAVCommanderImpl::~UAVCommanderImpl()
+{
+  ERROR_LOG(m_logger) << "dtor: " << (quintptr)this;
+
+  if (m_createInterfaceAdapterInstance)
+  {
+    delete m_createInterfaceAdapterInstance.data();
+  }
+
+  new KillInterfaceAdapterInstanceAsync(m_uasId);
+}
+
+
+void UAVCommanderImpl::startIA(const std::string &ip, const uint16_t port, const bool useTcp) noexcept(false)
+{
+  m_createInterfaceAdapterInstance = new CreateInterfaceAdapterInstance(m_uasId, ip, port, useTcp, this);
+}
+
+
+void UAVCommanderImpl::updateMissionStatus()
+{
+  while (true)
+  {
+    try
+    {
+      auto msg = m_MissionCtrl.reader.getNext();
+
+      const auto oldTask = m_MissionCtrl.currentTask;
+      m_MissionCtrl.currentTask = msg.currentTask;
+      ERROR_LOG(m_logger) << "currentTask = " << (int)m_MissionCtrl.currentTask;
+
+      const auto oldMission = m_MissionCtrl.currentMission;
+      m_MissionCtrl.currentMission = QString::fromStdString(msg.currentMissionName);
+      ERROR_LOG(m_logger) << "oldTask = " << (int)oldTask;
+
+      ERROR_LOG(m_logger) << "currentMission = " << QString::fromStdString(msg.currentMissionName);
+
+      m_MissionCtrl.currentlyAllowedTasks.swap(msg.currentlyAllowedTasks); 
+
+      if (m_MissionCtrl.ReportMissionStatus_Callback)
+      {
+        
+
+        ERROR_LOG(m_logger) << "Old Task" << oldTask << "new task " <<  m_MissionCtrl.currentTask ;
+        if (oldTask != m_MissionCtrl.currentTask)
+        {
+          if (!oldMission.isEmpty() && m_MissionCtrl.currentMission.isEmpty())
+          {
+            msg.missionEnded = oldMission.toStdString(); 
+          }
+
+          m_MissionCtrl.ReportMissionStatus_Callback(msg);
+          ERROR_LOG(m_logger) << "################################# sending " << (int)m_MissionCtrl.currentTask << " " << (quintptr)this;
+        }
+        else
+        {
+          ERROR_LOG(m_logger) << "################################# VERWERFING ! " << (int)m_MissionCtrl.currentTask << " " << (quintptr)this;
+        }
+      }
+
+
+      emit newCurrentTask(msg.currentTask);
+
+
+      switch (m_MissionCtrl.currentTask) 
+      {
+        case MissionCtrl::Undefined:
+        case MissionCtrl::Disconnect:
+        case MissionCtrl::Reboot:
+        case MissionCtrl::Shutdown:
+        {
+          m_isReady = false;
+          ERROR_LOG(m_logger) << "UAV not ready yet! currentTask: " << (int)m_MissionCtrl.currentTask;
+          break;
+        }
+
+        default:
+        {
+          if (m_isReady)
+          {
+            break;
+          }
+          m_isReady = true;
+          if (m_isReady_Callback)
+          {
+            m_isReady_Callback();
+          }
+          ERROR_LOG(m_logger) << "UAV now ready!";
+          break; 
+        }
+      }
+    }
+    catch (...)
+    {
+      break;
+    }
+  }
+}
+
+
+void UAVCommanderImpl::updatePosition()
+{
+  while (true)
+  {
+    try
+    {
+      const auto msg = m_CoordinateFrame.reader.getNext();
+
+      m_CoordinateFrame.currentPosition.clear();
+      m_CoordinateFrame.currentPosition.emplace_back(msg);
+
+      if (m_CoordinateFrame.ReportPosition_Callback)
+      {
+        m_CoordinateFrame.ReportPosition_Callback(msg);
+      }
+    }
+    catch (...)
+    {
+      break;
+    }
+  }
+}
+
+
+void UAVCommanderImpl::updateOperationalState()
+{
+  while (true)
+  {
+    try
+    {
+      const auto msg = m_TeleOperation.reader.getNext();
+
+      m_TeleOperation.currentOperationState.clear();
+      m_TeleOperation.currentOperationState.emplace_back(msg.status);
+
+      if (m_TeleOperation.ReportRadioLinkConnectionState_Callback)
+      {
+        m_TeleOperation.ReportRadioLinkConnectionState_Callback(msg);
+      }
+    }
+    catch (...)
+    {
+      break;
+    }
+  }
+}
+
+
+void UAVCommanderImpl::updateCBitStaus()
+{
+  while (true)
+  {
+    try
+    {
+      const auto msg = m_BuildInTest.reader.getNext();
+
+      m_BuildInTest.currentTestResult.clear();
+      m_BuildInTest.currentTestResult.emplace_back(msg);
+
+      if (m_BuildInTest.ReportCBitStatus_Callback)
+      {
+        m_BuildInTest.ReportCBitStatus_Callback(msg);
+      }
+    }
+    catch (...)
+    {
+      break;
+    }
+  }
+}
+
+
+void UAVCommanderImpl::updateDeviceModeState()
+{
+  ERROR_LOG(m_logger) << "start";
+
+  while (true)
+  {
+    try
+    {
+      const auto msg = m_Device.reader.getNext();
+
+      m_Device.currentMessage.clear();
+      m_Device.currentMessage.emplace_back(msg);
+
+      ERROR_LOG(m_logger) << "before callback";
+      if (m_Device.ReportDeviceStatus_Callback)
+      {
+        ERROR_LOG(m_logger) << "callback!";
+        m_Device.ReportDeviceStatus_Callback(msg);
+      }
+    }
+    catch (...)
+    {
+      break;
+    }
+  }
+}
+
+
+void UAVCommanderImpl::prepareMission(Resolver *resolver)
+{
+  ERROR_LOG(m_logger) << "resolver = " << (quintptr)resolver;
+
+  if (isTaskAllowed(MissionCtrl::Task::Observe) || m_MissionCtrl.currentTask == MissionCtrl::Task::Connect )
+  {
+    ERROR_LOG(m_logger) << "DONE! resolving " << (quintptr)resolver;
+    emit resolver->resolve();
+    return;
+  }
+
+  Resolver *r = new Resolver(this);
+  connect(r, &Resolver::reject, resolver, &Resolver::reject);
+  connect(r, &Resolver::resolve, resolver, [this, resolver]()
+  {
+    ERROR_LOG(m_logger) << "r ("<<(quintptr)resolver<<")! resolving!";
+    Resolver *r2 = new Resolver(this);
+    connect(r2, &Resolver::reject, resolver, &Resolver::reject);
+    connect(r2, &Resolver::resolve, resolver, &Resolver::resolve);
+    prepareMission(r2);
+  });
+
+  if (!m_MissionCtrl.currentMission.isEmpty()) 
+  {
+    ERROR_LOG(m_logger) << " > mission '"<< m_MissionCtrl.currentMission << "' currently active - aborting mission and waiting for loitering";
+    sendCommand(r, {QStringLiteral("mission:abort (%1)").arg(m_MissionCtrl.currentMission), MissionCtrl::Task::Connect, [this]() { _abortMission(m_MissionCtrl.currentMission); }}); 
+    return;
+  }
+
+  switch (m_MissionCtrl.currentTask) 
+  {
+    case MissionCtrl::Task::Disarm:
+    {
+      ERROR_LOG(m_logger) << " > arm";
+      arm(r);
+      break;
+    }
+
+    case MissionCtrl::Task::Arm:
+    {
+      ERROR_LOG(m_logger) << " > takeoff";
+      takeoff(r);
+      break;
+    }
+
+    case MissionCtrl::Task::Takeoff:
+    {
+      ERROR_LOG(m_logger) << " > waiting for Loiter ...";
+      sendCommand(r, {QStringLiteral("loiter"), MissionCtrl::Task::Loiter, nullptr});
+      break;
+    }
+
+    case MissionCtrl::Task::Loiter:
+    {
+      ERROR_LOG(m_logger) << " >WE loiter start to resolve";
+      emit resolver->resolve();
+      break;
+    }
+
+    
+
+    default:
+    {
+      ERROR_LOG(m_logger) << "help, I don't know what to do with "<<(int)m_MissionCtrl.currentTask<<" :(";
+      break;
+    }
+  }
+}
+
+
+void UAVCommanderImpl::sendMission(double lat, double lon, double alt, const QString &missionName) noexcept(false)
+{
+  if (!m_isReady)
+  {
+    ERROR_LOG(m_logger) << "not ready";
+    throw std::runtime_error("UAV not ready");
+  }
+
+  if (m_MissionCtrl.currentTask == MissionCtrl::Task::Takeoff)
+  {
+    throw std::runtime_error("UAV currently taking off");
+  }
+
+  if (m_MissionCtrl.currentTask == MissionCtrl::Task::Return)
+  {
+    throw std::runtime_error("UAV currently returning home");
+  }
+
+  if (m_MissionCtrl.currentTask == MissionCtrl::Task::Land)
+  {
+    throw std::runtime_error("UAV currently landing");
+  }
+
+  Resolver *r = new Resolver(this);
+  connect(r, &Resolver::reject, this, [this](const QString &msg) { ERROR_LOG(m_logger) << "rejected: " << msg; });
+  connect(r, &Resolver::resolve, [this, lat, lon, alt, missionName]()
+  {
+    ERROR_LOG(m_logger) << "!!!!!!!!!!!!!!!!!!!!!! going to observe now!!";
+
+    
+    Resolver *r2 = new Resolver(this);
+    connect(r2, &Resolver::reject, [this](const QString &msg) { ERROR_LOG(m_logger) << "################ failed to start mission :( " << msg; });
+    connect(r2, &Resolver::resolve, [this, missionName]()
+    {
+      Resolver *r3 = new Resolver(this);
+      connect(r3, &Resolver::reject, [this](const QString &msg) { ERROR_LOG(m_logger) << "################ failed to wait for loiter :( " << msg; });
+      connect(r3, &Resolver::resolve, [this, missionName]()
+      {
+        ERROR_LOG(m_logger) << "################ mission done" << missionName << " "  << this << " ";
+        
+        
+
+        
+        
+      });
+      sendCommand(r3, {QStringLiteral("wait for mission to end (Loiter)"), MissionCtrl::Task::Loiter, nullptr}, -1);
+      ERROR_LOG(m_logger) << "################ successfully started mission :)";
+    });
+    sendCommand(r2, { QStringLiteral("mission:observe"), MissionCtrl::Task::Observe, [this, lat, lon, alt, missionName]() { _sendMission(lat, lon, alt, missionName); } });
+  });
+  
+  
+  prepareMission(r); 
+}
+
+
+bool UAVCommanderImpl::isTaskAllowed(MissionCtrl::Task task) const
+{
+  return std::find(m_MissionCtrl.currentlyAllowedTasks.cbegin(), m_MissionCtrl.currentlyAllowedTasks.cend(), task) != m_MissionCtrl.currentlyAllowedTasks.cend();
+}
+
+
+void UAVCommanderImpl::_abortMission(const QString &missionName)
+{
+  m_MissionCtrl.writer->CmdAbortMission(missionName.toStdString());
+}
+
+
+void UAVCommanderImpl::_sendMission(double lat, double lon, double alt, const QString &missionName)
+{
+  ERROR_LOG(m_logger) << "lat = " << lat << ", lon = " << lon << ", alt = " << alt << ", missionName = " << missionName;
+
+  M_Common::EllipsoidalPositionType pos;
+
+  if (alt <= 0.1)
+  {
+    ERROR_LOG(m_logger) << "alt very low (<= 0.1) !";
+  }
+
+  pos.Latitude = lat / LtsCoordTrans::deg2rad();
+  pos.Longitude = lon / LtsCoordTrans::deg2rad();
+  ERROR_LOG(m_logger) << "DEG lat = " <<  pos.Latitude << ", lon = " << pos.Longitude << ", alt = " << alt << ", missionName = " << missionName;
+
+  pos.Altitude.push_back(alt);
+
+  M_Common::PositionType position;
+  position.Ellipsoidal.push_back(pos);
+
+  M_MissionCtrl::WaypointType waypoint;
+  waypoint.Position = position;
+
+  m_MissionCtrl.writer->CmdPerformMission(missionName.toStdString(), {{{{{{{M_MissionCtrl::TaskCategoryKind::Type::Observe},{},{},{}}, {waypoint}}}, 0, {}}}});
+}
+
+
+void UAVCommanderImpl::returnToHome()
+{
+  Resolver *r = new Resolver(this);
+  connect(r, &Resolver::reject, this, [this](const QString &error) { ERROR_LOG(m_logger) << "returnToHome failed: " << error; });
+  connect(r, &Resolver::resolve, this, [this]() { ERROR_LOG(m_logger) << "returnToHome done :)"; });
+  sendCommand(r, { QStringLiteral("return"), MissionCtrl::Task::Return, [this]() { _returnToHome(); } }, -1 );
+}
+
+
+void UAVCommanderImpl::_returnToHome()
+{
+  m_MissionCtrl.writer->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Return}, {}, {}, {}), {}));
+}
+
+
+void UAVCommanderImpl::sendCommand(Resolver *resolver, const Command &cmd, int timeoutMs) 
+{
+  const auto &commandName = cmd.name;
+  const auto &commandFn = cmd.fn;
+  const auto type = cmd.type;
+
+  if (m_tried != 0)
+  {
+    ERROR_LOG(m_logger) << commandName << ": another request already running - replacing ("<<m_tried__ACTIVE<<")";
+  }
+
+  if (m_MissionCtrl.currentTask == type)
+  {
+    ERROR_LOG(m_logger) << commandName << ": current task already matches new task '"<< commandName <<"' - replacing anyways";
+  }
+
+  if (m_currentResolver) 
+  {
+    m_currentResolver->reject(QStringLiteral("Command aborted"));
+  }
+  m_currentResolver = resolver;
+
+  ERROR_LOG(m_logger) << commandName << ": sending '" << commandName <<"' command ...";
+  m_tried = 1;
+  m_tried__ACTIVE = cmd.name;
+
+  QObject *guard = new QObject(resolver);
+
+  Reader<MissionCtrl::ReportMissionStatus::Message> *reader = new Reader<MissionCtrl::ReportMissionStatus::Message>(m_MissionCtrl.master.dispatcher()); 
+  connect(guard, &QObject::destroyed, this, [reader]() { delete reader; });
+
+  auto callCommandFn = [this, commandName, commandFn]()
+  {
+    if (commandFn)
+    {
+      ERROR_LOG(m_logger) << commandName << ": sending '"<< commandName <<"' command failed - timeout, trying again !";
+      try
+      {
+        commandFn();
+      }
+      catch(const std::exception &ex)
+      {
+        ERROR_LOG(m_logger) << commandName << ": sending '"<< commandName <<"' command failed:" << ex.what();
+      }
+    }
+  };
+
+  callCommandFn();
+
+  QTimer *timeout = nullptr;
+  if (timeoutMs > 0)
+  {
+    timeout = new QTimer(guard);
+    timeout->setInterval(timeoutMs);
+    timeout->setSingleShot(true);
+    connect(timeout, &QTimer::timeout, this, [this, timeout, guard, commandName, resolver, callCommandFn]()
+    {
+      static const int s_maxTries = 5;
+      if (m_tried++ > s_maxTries) 
+      {
+        timeout->stop();
+        guard->deleteLater();
+        ERROR_LOG(m_logger) << commandName << ": sending '"<< commandName <<"' command failed - max tries reached, giving up !";
+        m_tried = 0; 
+        m_currentResolver.clear();
+        emit resolver->reject(QStringLiteral("Sending %1 command timed out!").arg(commandName));
+        return;
+      }
+
+      
+      callCommandFn();
+
+      timeout->start();
+    });
+  }
+
+
+  
+  connect(reader->readerSignal(), &ReaderSignal::dataReady,
+          this, [this, reader, guard, timeout, commandName, type, resolver]()
+  {
+    ERROR_LOG(m_logger) << commandName << ": reader signal for '"<< commandName <<"' !";
+
+    while(true)
+    {
+      try
+      {
+        const auto msg = reader->getNext();
+
+        ERROR_LOG(m_logger) << "########## READER SIGNAL (should: " << (int)type << ", is: " << (int)msg.currentTask << ")";
+
+        if (msg.currentTask == type
+            
+            || (type == MissionCtrl::Task::Takeoff && msg.currentTask == MissionCtrl::Task::Connect)
+            || (type == MissionCtrl::Task::Return && (msg.currentTask == MissionCtrl::Task::Disarm || msg.currentTask == MissionCtrl::Task::Land)))
+        {
+          ERROR_LOG(m_logger) << commandName << ": successfully set new task '"<< commandName <<"' !";
+
+          
+          QTimer *x = new QTimer(guard);
+          x->setInterval(1000);
+          x->setSingleShot(true);
+          connect(resolver, &QObject::destroyed, x, &QTimer::stop);
+          connect(guard, &QObject::destroyed, x, &QTimer::stop);
+          connect(x, &QTimer::timeout, this, [this, timeout, guard, resolver]()
+          {
+            ERROR_LOG(m_logger) << "############# RESOLVE AFTER 1s";
+
+            m_tried = 0;
+            if (timeout != nullptr)
+            {
+              timeout->stop();
+            }
+            guard->deleteLater();
+            m_currentResolver.clear();
+
+            emit resolver->resolve();
+          });
+          x->start();
+
+          return; 
+        }
+      }
+      catch(...)
+      {
+        break;
+      }
+    }
+
+    ERROR_LOG(m_logger) << commandName << ": command '"<< commandName <<"' not accepted (yet)";
+
+    
+  });
+
+
+  if (timeout != nullptr)
+  {
+    timeout->start();
+  }
+}
+
+
+void UAVCommanderImpl::arm(Resolver *resolver)
+{
+  sendCommand(resolver, {QStringLiteral("arm"), MissionCtrl::Task::Arm, [this]() { _arm(); }});
+}
+
+
+void UAVCommanderImpl::_arm()
+{
+  m_MissionCtrl.writer->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Arm}, {}, {}, {}), {})); 
+}
+
+
+void UAVCommanderImpl::disarm(Resolver *resolver)
+{
+  
+  Q_UNUSED(resolver)
+}
+
+
+void UAVCommanderImpl::_disarm()
+{
+  m_MissionCtrl.writer->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Disarm}, {}, {}, {}), {})); 
+}
+
+
+void UAVCommanderImpl::takeoff(Resolver *resolver)
+{
+  sendCommand(resolver, {QStringLiteral("takeoff"), MissionCtrl::Task::Takeoff, [this]() { _takeoff(); }});
+}
+
+
+void UAVCommanderImpl::sendTryReclaimAutoMode()
+{
+  M_Common::OperationModeType mode;
+  mode.EnumeratedValue.push_back(M_Common::OperationModeStateKind::Type::Auto);
+  try
+  {
+    m_Device.writer->CmdSetDeviceMode(mode);
+  }
+  catch(const std::exception &ex)
+  {
+    ERROR_LOG(m_logger) << "Failed to send DeviceMode:" << ex.what();
+  }
+}
+
+
+void UAVCommanderImpl::_takeoff()
+{
+  m_MissionCtrl.writer->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Takeoff}, {}, {}, {}), {})); 
+}
+
+
+
+
+CreateInterfaceAdapterInstance::CreateInterfaceAdapterInstance(int uasId, const std::string &ip, uint16_t port, const bool useTcp, QObject *parent)
+  : QObject(parent)
+  , m_master("UAV_ObjectManager", 4)
+  , m_reader(m_master.dispatcher())
+  , m_writer(m_master.client())
+  , m_uasId(uasId)
+  , m_ip(ip)
+  , m_port(port)
+  , m_useTcp(useTcp)
+  , m_timeout(new QTimer(this))
+  , m_logger(getLogger("CreateInterfaceAdapterInstance"))
+{
+  connect(m_reader.readerSignal(), &ReaderSignal::dataReady,
+    this, [this, uasId]()
+    {
+      ERROR_LOG(m_logger) << "readerSignal for uasId" << m_uasId;
+
+      const auto msgs = readAll<ObjectManager::ReportPoint::Message>(m_reader);
+      ERROR_LOG(m_logger) << "msgs.size" << msgs.size() << m_uasId;
+
+      for (const auto &msg : msgs)
+      {
+        ERROR_LOG(m_logger) << "msg uas" << msg.uasId << "removed" << msg.isRemoved;
+        if (msg.uasId == uasId && !msg.isRemoved)
+        {
+          
+          ERROR_LOG(m_logger) << "DONE!!";
+          m_timeout->stop();
+          return;
+        }
+      }
+
+      
+      ERROR_LOG(m_logger) << "no ia instance for"<<m_uasId<<"found !! starting timeout";
+      if (!m_timeout->isActive()) 
+      {
+        m_timeout->start();
+      }
+    });
+
+  onStart();
+
+  emit m_reader.readerSignal(); 
+
+  m_timeout->setInterval(2000);
+  connect(m_timeout, &QTimer::timeout, this, &CreateInterfaceAdapterInstance::onStart);
+  m_timeout->start();
+}
+
+
+void CreateInterfaceAdapterInstance::onStart()
+{
+  ERROR_LOG(m_logger) << " -- CreateInterfaceAdapterInstance -- ::onStart() CmdAddUpdatePoint" << m_uasId<<" ip: "<<m_ip<<" use TCP ? "<<m_useTcp;
+
+  M_ObjectManager::PointObjectType pot;
+  pot.AdditionalInfo.push_back({ "IP", m_ip, {}, {}, M_Common::PropertyAccessKind::Type::Undefined });
+  pot.AdditionalInfo.push_back({ "Port", std::to_string(m_port), {}, {}, M_Common::PropertyAccessKind::Type::Undefined });
+  pot.AdditionalInfo.push_back({ "UseTcp", {}, {}, {static_cast<int>(m_useTcp)}, M_Common::PropertyAccessKind::Type::Undefined });
+
+  
+  try
+  {
+    m_writer->CmdAddUpdatePoint(std::to_string(m_uasId), pot);
+  }
+  catch (const std::exception &ex)
+  {
+    ERROR_LOG(m_logger) << "CmdAddUpdatePoint FAILED!" << m_uasId << ":" << ex.what();
+  }
+}
+
+
+
+
+KillInterfaceAdapterInstanceAsync::KillInterfaceAdapterInstanceAsync(int uasId)
+  : m_master("UAV_ObjectManager", 4)
+  , m_reader(m_master.dispatcher())
+  , m_writer(m_master.client())
+  , m_uasId(uasId)
+  , m_timeout(new QTimer(this))
+  , m_logger(getLogger("KillInterfaceAdapterInstanceAsync"))
+{
+  connect(m_reader.readerSignal(), &ReaderSignal::dataReady, 
+    this, [this, uasId]()
+    {
+      if (m_done)
+      {
+        return;
+      }
+      
+      ERROR_LOG(m_logger) << "readerSignal for uasId" << m_uasId;
+
+      const auto msgs = readAll<ObjectManager::ReportPoint::Message>(m_reader);
+      ERROR_LOG(m_logger) << "msgs.size" << msgs.size() << m_uasId;
+
+      for (const auto &msg : msgs)
+      {
+        ERROR_LOG(m_logger) << "msg uas" << msg.uasId << "removed" << msg.isRemoved;
+        if (msg.uasId == uasId && msg.isRemoved)
+        {
+          
+          ERROR_LOG(m_logger) << "DONE!!";
+          m_done = true;
+          deleteLater();
+          return;
+        }
+      }
+    });
+
+  emit m_reader.readerSignal(); 
+
+  m_timeout->setInterval(2000); 
+  connect(m_timeout, &QTimer::timeout, this, &KillInterfaceAdapterInstanceAsync::onStop);
+  m_timeout->start();
+}
+
+
+void KillInterfaceAdapterInstanceAsync::onStop()
+{
+  
+  try
+  {
+    m_writer->CmdRemove(std::to_string(m_uasId));
+  }
+  catch (const std::exception &ex)
+  {
+    ERROR_LOG(m_logger) << "CmdRemove" << m_uasId << ":" << ex.what();
+  }
+}
+
+} 

qt6-after/qt6-migrated-legatus/Lts/Proxies/TrainingAreaBase/Plugins/UAS/TmsIfAdapter/src/uav_commander_impl.h

@@ -0,0 +1,261 @@
+
+
+
+#include "communication_master.h"
+#include "dispatchers/report_positions.h"
+#include "dispatchers/report_mission_status.h"
+#include "dispatchers/report_radio_link_connection_state.h"
+#include "dispatchers/report_cbit_status.h"
+#include "dispatchers/report_device_status.h"
+#include "dispatchers/report_point.h"
+#include "logger.h"
+
+#include <TmsIfAdapter/uav_commander.h>
+#include <ddkit/actors/Session.hpp>
+#include <tmsif/api/ObjectManager.hpp>
+#include <tmsif/api/Common.hpp>
+
+#include <vector>
+
+#include <QQueue>
+#include <QPointer>
+
+
+template<class T> using Optional = std::vector<T>;
+
+
+namespace UAS
+{
+class Resolver : public QObject
+{
+  Q_OBJECT
+public:
+  Resolver(QObject *parent);
+
+signals:
+  void resolve();
+  void reject(const QString &error);
+};
+
+
+class CreateInterfaceAdapterInstance : public QObject
+{
+  Q_OBJECT
+public:
+  CreateInterfaceAdapterInstance(int uasId, const std::string &ip, uint16_t port, const bool useTcp, QObject *parent);
+
+private slots:
+  void onStart();
+
+private:
+  mutable CommunicationMaster<M_ObjectManager::Client, ObjectManager::ReportPoint::Dispatcher> m_master;
+  Reader<ObjectManager::ReportPoint::Message> m_reader;
+  M_ObjectManager::Client m_writer;
+  int m_uasId;
+  std::string m_ip;
+  uint16_t m_port;
+  bool m_useTcp;
+  QTimer *m_timeout;
+  NGL::INGLLogger &m_logger;
+};
+
+
+class KillInterfaceAdapterInstanceAsync : public QObject
+{
+  Q_OBJECT
+public:
+  KillInterfaceAdapterInstanceAsync(int uasId);
+
+private slots:
+  void onStop();
+
+private:
+  mutable CommunicationMaster<M_ObjectManager::Client, ObjectManager::ReportPoint::Dispatcher> m_master;
+  Reader<ObjectManager::ReportPoint::Message> m_reader;
+  M_ObjectManager::Client m_writer;
+  bool m_done { false };
+  int m_uasId;
+  std::string m_ip;
+  uint16_t m_port;
+  QTimer *m_timeout;
+  NGL::INGLLogger &m_logger;
+};
+
+
+class UAVCommanderImpl final : public QObject, public UAVCommander
+{
+  Q_OBJECT
+
+public:
+  UAVCommanderImpl(int domainId, int uasId, QObject *parent);
+  ~UAVCommanderImpl();
+
+  void startIA(const std::string &ip, const uint16_t port, const bool useTcp) noexcept(false) override;
+
+  void set_CoordinateFrame_ReportPosition_Callback(CoordinateFrame::ReportPositions::CallbackFn callbackFn) override
+  {
+    m_CoordinateFrame.ReportPosition_Callback = callbackFn;
+  }
+  void set_ReportCBitStatus_Callback(BuiltInTest::ReportCBitStatus::CallbackFn callbackFn) override
+  {
+    m_BuildInTest.ReportCBitStatus_Callback = callbackFn;
+  }
+  void set_ReportDeviceStatusType_Callback(Device::ReportDeviceStatus::CallbackFn callbackFn) override
+  {
+    m_Device.ReportDeviceStatus_Callback = callbackFn;
+  }
+  void set_ReportRadioLinkConnectionState_Callback(TeleOperation::RadioLinkConnectionState::CallbackFn callbackFn) override
+  {
+    m_TeleOperation.ReportRadioLinkConnectionState_Callback = callbackFn;
+  }
+  void set_ReportMissionStatus_Callback(MissionCtrl::ReportMissionStatus::CallbackFn callbackFn) override
+  {
+    m_MissionCtrl.ReportMissionStatus_Callback = callbackFn;
+  }
+  void set_isReady_Callback(std::function<void()> callbackFn) override
+  {
+    m_isReady_Callback = callbackFn;
+  }
+
+  void arm(Resolver *resolver);
+  void disarm(Resolver *resolver);
+  void takeoff(Resolver *resolver);
+  void sendMission(double lat, double lon, double alt, const QString &missionName) noexcept(false) override;
+  void sendTryReclaimAutoMode() override;
+  void returnToHome() override;
+
+protected:
+  void updateMissionStatus();
+  void updatePosition();
+  void updateOperationalState();
+  void updateCBitStaus();
+  void updateDeviceModeState();
+
+signals:
+  void newCurrentTask(MissionCtrl::Task task);
+
+private:
+  QString m_currentMission;
+  struct Command
+  {
+    QString name;
+    MissionCtrl::Task type;
+    std::function<void()> fn;
+  };
+
+  void prepareMission(Resolver *resolver);
+  bool isTaskAllowed(MissionCtrl::Task task) const;
+  void sendCommand(Resolver *resolver, const Command &cmd, int timeoutMs = 5000);
+  void _arm();
+  void _disarm();
+  void _takeoff();
+  void _returnToHome();
+  void _sendMission(double lat, double lon, double alt, const QString &missionName);
+  void _abortMission(const QString &missionName);
+  int m_tried { 0 }; 
+  QString m_tried__ACTIVE;
+
+  QPointer<Resolver> m_currentResolver;
+
+  int m_domainId;
+  int m_uasId;
+
+  QPointer<CreateInterfaceAdapterInstance> m_createInterfaceAdapterInstance;
+
+
+  struct MissionCtrlContainer
+  {
+    MissionCtrlContainer(const std::string &serviceName, int domainId);
+
+    mutable CommunicationMaster<M_MissionCtrl::Client, MissionCtrl::ReportMissionStatus::Dispatcher> master;
+    Reader<MissionCtrl::ReportMissionStatus::Message> reader;
+
+    M_MissionCtrl::Client writer;
+
+    MissionCtrl::Task currentTask { MissionCtrl::Undefined };
+    std::vector<MissionCtrl::Task> currentlyAllowedTasks;
+    QString currentMission;
+
+    MissionCtrl::ReportMissionStatus::CallbackFn ReportMissionStatus_Callback;
+  } m_MissionCtrl;
+
+  struct TeleOperationContainer
+  {
+    TeleOperationContainer(const std::string &serviceName, int domainId);
+
+    mutable CommunicationMaster<M_Teleoperation::Client, TeleOperation::RadioLinkConnectionState::Dispatcher> master;
+    Reader<TeleOperation::RadioLinkConnectionState::Message> reader;
+
+    Optional<TeleOperation::Type> currentOperationState;
+
+    TeleOperation::RadioLinkConnectionState::CallbackFn ReportRadioLinkConnectionState_Callback;
+  } m_TeleOperation;
+
+  struct BuildInTestContainer
+  {
+    BuildInTestContainer(const std::string &serviceName, int domainId);
+
+    mutable CommunicationMaster<M_BuiltInTest::Client, BuiltInTest::ReportCBitStatus::Dispatcher> master;
+    Reader<BuiltInTest::ReportCBitStatus::Message> reader;
+
+    Optional<BuiltInTest::ReportCBitStatus::Message> currentTestResult;
+
+    BuiltInTest::ReportCBitStatus::CallbackFn ReportCBitStatus_Callback;
+  } m_BuildInTest;
+
+  struct DeviceContainer
+  {
+    DeviceContainer(const std::string &serviceName, int domainId);
+
+    mutable CommunicationMaster<M_Device::Client, Device::ReportDeviceStatus::Dispatcher> master;
+    Reader<Device::ReportDeviceStatus::Message> reader;
+
+    M_Device::Client writer; 
+
+    Optional<Device::ReportDeviceStatus::Message> currentMessage;
+
+    Device::ReportDeviceStatus::CallbackFn ReportDeviceStatus_Callback;
+  } m_Device;
+
+  std::function<void()> m_isReady_Callback;
+
+  struct CoordinateFrameContainer
+  {
+    CoordinateFrameContainer(const std::string &serviceName, int domainId);
+
+    mutable CommunicationMaster<M_CoordinateFrame::Client, CoordinateFrame::ReportPositions::Dispatcher> master;
+    Reader<CoordinateFrame::ReportPositions::Message> reader;
+
+    Optional<CoordinateFrame::ReportPositions::Message> currentPosition;
+
+    CoordinateFrame::ReportPositions::CallbackFn ReportPosition_Callback;
+  } m_CoordinateFrame;
+
+
+  NGL::INGLLogger &m_logger;
+};
+
+
+template<typename MessageType>
+std::vector<MessageType> readAll(Reader<MessageType> &reader)
+{
+  std::vector<MessageType> res;
+
+  while (true)
+  {
+    try
+    {
+      const auto msg = reader.getNext();
+      res.emplace_back(msg);
+    }
+    catch (...)
+    {
+      break;
+    }
+  }
+
+  return res;
+}
+
+
+}