Initial commit

This commit is contained in:
torsten committed 2026-08-13 18:33:10 +02:00
commit b7d90b9afe
16245 files changed
+3396666

No files matched your search

+46
View File
@@ -0,0 +1,46 @@
add_subdirectory(TmsIfAdapter)
if (BUILD_TESTING)
add_subdirectory(Test)
endif()
# plugin
set(src
logger.h
logger.cpp
plugin.h
plugin.cpp
plugin_instance.h
plugin_instance.cpp
from_tab.h
from_tab.cpp
uav_ia.h
uav_ia.cpp
uav_ia_manager.h
uav_ia_manager.cpp
)
NGL_ADD_LIBRARY(SV tabPluginUAS MODULE ${src})
NGL_TARGET_LINK_LIBRARIES(tabPluginUAS
TmsIfAdapterInterface
NGLLogger
nglsettings
Qt5::Core
Qt5::Gui
Qt5::Network
tabTelegrams
tabPluginInterface
svcCesClient
svcPdmClient
svcTabCommon
svcPpmClient
svcFlmClient
LtsUtilsCoreHeaderOnly
LtsCoordTrans # geocentric2LonLatAlt
)
NGL_INSTALL_TARGET(tabPluginUAS)
@@ -0,0 +1,74 @@
MACRO(_BUILD_TEST test_id)
add_executable(TabUASTest${test_id} test${test_id}.cpp)
FIND_PACKAGE(TMSIF REQUIRED)
target_compile_definitions(TabUASTest${test_id} PUBLIC DDS_VENDOR_CYCLONE)
target_link_libraries(TabUASTest${test_id} PRIVATE
TMSIF::tmsif-cyclone
TMSIF::ddscxx
TMSIF::ddkit-cyclone
)
ENDMACRO()
#_BUILD_TEST(1)
#_BUILD_TEST(2)
_BUILD_TEST(3)
# # test4
# add_executable(TabUASTest4 test4.cpp)
# target_link_libraries(TabUASTest4
# PRIVATE
# TmsIfAdapter
# tabPluginInterface
# )
# # test4.1
# add_executable(TabUASTest4_1 test4.cpp)
# target_link_libraries(TabUASTest4_1
# PRIVATE
# TmsIfAdapter
# # /opt/RTI/lib/x64Linux4gcc7.3.0/libnddsc.so
# # /opt/RTI/lib/x64Linux4gcc7.3.0/libnddscore.so
# # /opt/RTI/lib/x64Linux4gcc7.3.0/libnddscpp2.so
# )
# #target_include_directories(TabUASTest${test_id} PRIVATE
# #
# #)
# test4_2
add_executable(TabUASTest4_2 test4_2.cpp)
target_link_libraries(TabUASTest4_2
PRIVATE
#TmsIfAdapter via dlopen !
TmsIfAdapterInterface
tabPluginInterface
)
# # test5
# add_executable(TabUASTest5 test5.cpp)
# target_link_libraries(TabUASTest5
# PRIVATE
# tabPluginInterface
# )
# test6
add_executable(TabUASTest6 test6.cpp)
target_link_libraries(TabUASTest6
PRIVATE
Qt5::Core
Qt5::Widgets
#TmsIfAdapter via dlopen !
TmsIfAdapterInterface
)
+163
View File
@@ -0,0 +1,163 @@
// std
#include <iostream>
#include <chrono>
#include <thread>
// tmsif
#include <tmsif/api/MissionCtrl.hpp>
// ddkit
#include <ddkit/actors/Session.hpp>
int main(int argc, char *argv[])
{
std::cout << "App started ..." << std::endl;
const int domain = 4; // TMS-IF MAVLink Adapter runs on domain 4
ddkit::actors::Session session(domain);
// starting Service
const std::string serviceName = "UAV_MissionCtrl1";
auto client = M_MissionCtrl::Client::createWriteOnly(session, serviceName);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
client->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Arm}, {}, {}, {}), {}));
// std::this_thread::sleep_for(std::chrono::milliseconds(500));
client->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Arm}, {}, {}, {}), {}));
while(true) // keep app running
{
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
}
return 0;
}
// // std
// #include <iostream>
// #include <chrono>
// #include <thread>
// // tmsif
// #include <tmsif/api/MissionCtrl.hpp>
// // ddkit
// #include <ddkit/actors/Session.hpp>
// // class DeviceClient : public M_MissionCtrl::IClient
// // {
// // public:
// // void ReportDeviceStatus(M_MissionCtrl::DeviceStatusType const& Status, bool IsRemoved) override
// // {
// // fprintf(stdout, "ReportDeviceStatus: Status.OperationStatus.Mode set? %d\n", !(int)Status.OperationStatus.Mode.empty());
// // }
// // };
// int main(int argc, char *argv[])
// {
// std::cout << "App started ..." << std::endl;
// const int domain = 4; // TMS-IF MAVLink Adapter runs on domain 4
// ddkit::actors::Session session(domain);
// // starting Service
// const std::string serviceName = "UAV_Device1";
// M_MissionCtrl::IService callbacksFromClients;
// auto service = M_MissionCtrl::Service::create(session, serviceName, callbacksFromClients);
// // messages
// // auto msg = M_MissionCtrl::AlarmLogType(M_Alert::AlarmStatusKind::Active, false, false,
// // false, {}, M_Alert::AlarmCategoryKind::Type::Warning, M_Common::TimeType(4000, 0), {});
// uint64_t i = 1;
// while(true)
// {
// std::this_thread::sleep_for(std::chrono::milliseconds(500));
// // send data cyclic for example
// service. // ReportAlarmLog("test123", msg, false);
// }
// return 0;
// }
+74
View File
@@ -0,0 +1,74 @@
// std
#include <cstdlib>
#include <iostream>
#include <chrono>
#include <thread>
// tmsif
#include <tmsif/api/Alert.hpp>
// ddkit
#include <ddkit/actors/Session.hpp>
// callback-implementations - messages from clients
class AlertService : public M_Alert::IService
{
public:
void CmdRaiseAlarm(M_Alert::AlarmConditionInfoType const& Condition) override
{
fprintf(stderr, "CmdRaiseAlarm !!\n");
}
};
class AlertClient : public M_Alert::IClient
{
public:
void setAlertClient(M_Alert::Client *uglyPointer)
{m_client = uglyPointer;}
M_Alert::Client *m_client;
void ReportAlarmLog(M_Alert::AlertIdType const& KeyAlarmId, M_Alert::AlarmLogType const& Alarm, bool IsRemoved)
{
fprintf(stderr, "ReportAlarmLog !!\n");
(*m_client)->CmdRaiseAlarm(M_Alert::AlarmConditionInfoType("a", "b", "c", "d", "e"));
}
};
int main(int argc, char *argv[])
{
std::cout << "App started ..." << std::endl;
int domain = 5;
ddkit::actors::Session session(domain);
// constructing callbacks
AlertService callbacksFromClients;
// starting Service
const std::string serviceName = "AlertService123";
auto service = M_Alert::Service::create(session, serviceName, callbacksFromClients);
// client
AlertClient callbacksFromServices;
auto client = M_Alert::Client::create(session, serviceName, callbacksFromServices);
callbacksFromServices.setAlertClient(&client);
// messages
auto msg = M_Alert::AlarmLogType(M_Alert::AlarmStatusKind::Active, false, false,
false, {}, M_Alert::AlarmCategoryKind::Type::Warning, M_Common::TimeType(4000, 0), {});
uint64_t i = 1;
while(true) {
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// send data cyclic for example
service->ReportAlarmLog("test123", msg, false);
}
return 0;
}
+114
View File
@@ -0,0 +1,114 @@
// std
#include <iostream>
#include <chrono>
#include <thread>
// tmsif
#include <tmsif/api/CoordinateFrame.hpp>
// ddkit
#include <ddkit/actors/Session.hpp>
class CoordinateFrameClient : public M_CoordinateFrame::IClient
{
public:
void ReportPositions(std::string const& KeyId, M_Common::PositionListType const& PositionList, bool IsRemoved) override
{
fprintf(stdout, "ReportPositions: !!!\n");
}
};
int main(int argc, char *argv[])
{
std::cout << "App started ..." << std::endl;
const int domain = 4; // TMS-IF MAVLink Adapter runs on domain 4
// ddkit::actors::Session session(domain);
// starting Service
const std::string serviceName = "UAV_Device1";
// client
CoordinateFrameClient callbacksFromServices;
auto client = M_CoordinateFrame::Client::create(domain, serviceName, callbacksFromServices);
fprintf(stderr, "now waiting");
while(true) // keep app running
{
fprintf(stderr, ".");
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
return 0;
}
// // std
// #include <iostream>
// #include <chrono>
// #include <thread>
// // tmsif
// #include <tmsif/api/Device.hpp>
// // ddkit
// #include <ddkit/actors/Session.hpp>
// class DeviceClient : public M_Device::IClient
// {
// public:
// void ReportDeviceStatus(M_Device::DeviceStatusType const& Status, bool IsRemoved) override
// {
// fprintf(stdout, "ReportDeviceStatus: Status.OperationStatus.Mode set? %d\n", !(int)Status.OperationStatus.Mode.empty());
// }
// };
// int main(int argc, char *argv[])
// {
// std::cout << "App started ..." << std::endl;
// const int domain = 4; // TMS-IF MAVLink Adapter runs on domain 4
// ddkit::actors::Session session(domain);
// // starting Service
// const std::string serviceName = "UAV_Device1";
// // client
// DeviceClient callbacksFromServices;
// auto client = M_Device::Client::create(session, serviceName, callbacksFromServices);
// while(true) // keep app running
// {
// std::this_thread::sleep_for(std::chrono::milliseconds(500));
// }
// return 0;
// }
+36
View File
@@ -0,0 +1,36 @@
#include <iostream>
#include <thread>
#include <TmsIfAdapter/tms_if.h>
int main(int , char *[])
{
std::cout << "App started ..." << std::endl;
UAS::TmsIfAdapter::get().test();
std::cout << "Now waiting ..." << std::endl;
// keep app running:
while(true){std::this_thread::sleep_for(std::chrono::milliseconds(500));}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// std::string blub;
// std::cin >> blub;
std::cout << "Stopping ..." << std::endl;
UAS::TmsIfAdapter::stop();
std::cout << "Stopped ..." << std::endl;
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::cout << "Exit ..." << std::endl;
return 0;
}
+60
View File
@@ -0,0 +1,60 @@
#include <iostream>
#include <thread>
#include <dlfcn.h> // TODO: no UAS for windoof
#include <TmsIfAdapter/tms_if_adapter.h>
using GetFn = void *(*)();
int main(int , char *[])
{
std::cout << "App started ..." << std::endl;
void *handle = dlopen("libTmsIfAdapter.so", RTLD_NOW | RTLD_LOCAL | RTLD_DEEPBIND);
if (handle == nullptr)
{
std::cerr << "no libTmsIfAdapter.so :( => " << dlerror() << std::endl;
return 1;
}
auto get = reinterpret_cast<GetFn>(dlsym(handle, "get"));
if (get == nullptr)
{
std::cerr << "no 'get' in libTmsIfAdapter.so :( => " << dlerror() << std::endl;
return 1;
}
UAS::TmsIfAdapter *adapter = reinterpret_cast<UAS::TmsIfAdapter *>(get());
// adapter->test();
std::cout << "Now waiting ..." << std::endl;
// keep app running:
while(true){std::this_thread::sleep_for(std::chrono::milliseconds(500));}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// std::string blub;
// std::cin >> blub;
std::cout << "Stopping ..." << std::endl;
// UAS::TmsIfAdapter::stop();
std::cout << "Stopped ..." << std::endl;
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::cout << "Exit ..." << std::endl;
return 0;
}
+16
View File
@@ -0,0 +1,16 @@
#include <iostream>
#include <thread>
int main(int , char *[])
{
std::cout << "App started ..." << std::endl;
std::cout << "Now waiting ..." << std::endl;
while(true){std::this_thread::sleep_for(std::chrono::milliseconds(500));}
std::cout << "Exit ..." << std::endl;
return 0;
}
+37
View File
@@ -0,0 +1,37 @@
#include <QApplication>
#include <QTimer>
#include <iostream>
#include <thread>
#include <dlfcn.h> // TODO: no UAS for windoof
#include <TmsIfAdapter/module_loader.h>
#include <TmsIfAdapter/tms_if_adapter.h>
#include <TmsIfAdapter/uav_commander.h>
int main(int argc, char **argv)
{
QCoreApplication app(argc, argv);
std::cout << "App started ..." << std::endl;
UAS::ModuleLoader::dlopen_handle handle = UAS::ModuleLoader::load("../lib/");
/*auto &adapter =*/ UAS::ModuleLoader::get(handle);
app.exec();
std::cout << "Stopping ..." << std::endl;
UAS::ModuleLoader::unload(handle);
std::cout << "Stopped ..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::cout << "Exit ..." << std::endl;
return 0;
}
@@ -0,0 +1,15 @@
# Interface Lib (API) for TMS-IF communication - has to be its own lib because of conflicting DDS implementations
#
# $Id$
# $HeadURL$
# $Author$
#
add_subdirectory(src)
# TmsIfAdapterInterface
add_library(TmsIfAdapterInterface INTERFACE)
target_link_libraries(TmsIfAdapterInterface INTERFACE dl)
target_include_directories(TmsIfAdapterInterface INTERFACE
${CMAKE_CURRENT_SOURCE_DIR}/../
)
@@ -0,0 +1,78 @@
/**
* Loads and manages the libTmsIfAdapter module
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-04-17
*/
#pragma once
#include "tms_if_adapter.h"
#include <stdexcept>
#include <string>
/* there's a massive problem with symbol collision between RTI DDS & Cyclone DDS (namely dds::core::Time::invalid())
* so we need the linux specific RTLD_DEEPBIND to force the use of the Cyclone DDS symbols inside the library */
#include <dlfcn.h>
namespace UAS
{
namespace ModuleLoader
{
using dlopen_handle = void *;
dlopen_handle load(std::string libPath) noexcept(false)
{
dlopen_handle handle = dlopen((libPath + "/libTmsIfAdapter.so").c_str(), RTLD_NOW | RTLD_LOCAL | RTLD_DEEPBIND);
if (handle == nullptr)
{
throw std::runtime_error((std::string("tms_if.h => ModuleLoader::load: unable to load libTmsIfAdapter.so: ") + dlerror()).c_str());
}
return handle;
}
TmsIfAdapter &get(dlopen_handle handle) noexcept(false)
{
auto get = reinterpret_cast<void *(*)()>(dlsym(handle, "get"));
if (get == nullptr)
{
throw std::runtime_error((std::string("tms_if.h => ModuleLoader::get: function symbol 'get' does not exist in libTmsIfAdapter.so: ") + dlerror()).c_str());
}
UAS::TmsIfAdapter &adapter = *reinterpret_cast<UAS::TmsIfAdapter *>(get());
return adapter;
}
void unload(dlopen_handle handle) noexcept(false)
{
if (handle == nullptr)
{
return;
}
auto stop = reinterpret_cast<void *(*)()>(dlsym(handle, "stop"));
if (stop == nullptr)
{
throw std::runtime_error((std::string("tms_if.h => ModuleLoader::unload: function symbol 'stop' does not exist in libTmsIfAdapter.so: ") + dlerror()).c_str());
}
stop();
const int ret = dlclose(handle);
if (ret != 0)
{
throw std::runtime_error((std::string("tms_if.h => ModuleLoader::unload: dlclose failed - ret != 0 (was "+std::to_string(ret)+"): ") + dlerror()).c_str());
}
}
} // ModuleLoader
} // namespace UAS
@@ -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)
@@ -0,0 +1,84 @@
/**
* Receives data from TMS-IF thread and dispatches data into this application in a thread-safe manner
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-04-17
*/
#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) // called from thread !
{
{
QMutexLocker lock(&m_mutex);
m_queue.enqueue(data);
}
// copy and empty queue then send all queued items to all readers
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;
};
} // namespace UAS
@@ -0,0 +1,30 @@
/**
* Creates a tms-if client and feeds it a new dispatcher for convenience
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-10-15
*/
#pragma once
#include <string>
template<typename ClientType, typename DispatcherType>
class CommunicationMaster // FIXME: one client per message type is not so cool - better to have one client for ALL message types but the dispatcher doesn't work this way
{
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;
};
@@ -0,0 +1,57 @@
/**
* Dispatcher for ReportCBitStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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);
}
} // namespace ReportCBitStatus
} // namespace BuiltInTest
} // namespace UAS
@@ -0,0 +1,37 @@
/**
* Dispatcher for ReportCBitStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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;
};
} // namespace ReportCBitStatus
} // namespace BuiltInTest
} // namespace UAS
@@ -0,0 +1,76 @@
/**
* Dispatcher for ReportDeviceStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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; // compiler meckert
}
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);
}
} // namespace ReportDeviceStatus
} // namespace Device
} // namespace UAS
@@ -0,0 +1,38 @@
/**
* Dispatcher for ReportDeviceStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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;
};
} // namespace ReportDeviceStatus
} // namespace Device
} // namespace UAS
@@ -0,0 +1,106 @@
/**
* Dispatcher for ReportMissionStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#include "report_mission_status.h"
namespace UAS
{
namespace MissionCtrl
{
Task taskFromTms(M_MissionCtrl::TaskCategoryKind::Type taskType)
{
switch (taskType) // oof ....
{
case M_MissionCtrl::TaskCategoryKind::Type::Undefined: return Task::Undefined;
case M_MissionCtrl::TaskCategoryKind::Type::Connect: return Task::Connect; //Is in Manual
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; // compiler meckert
}
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()) // aka. not set - then don't add
{
return;
}
const auto _tmsIfTask = tmsIfTask.EnumeratedValue.front();
const auto task = taskFromTms(_tmsIfTask);
if (task != Task::Undefined) // only add non-undefined tasks
{
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") // yea thanks for that
{
missionName = additionalInfo.StrValue;
break;
}
}
}
DEBUG_LOG(m_logger) << "missionName after: " << missionName;
dispatch({ missionName, currentTask, currentlyAllowedTasks, {} });
}
} // namespace ReportMissionStatus
} // namespace MissionCtrl
} // namespace UAS
@@ -0,0 +1,36 @@
/**
* Dispatcher for ReportMissionStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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;
};
} // namespace ReportMissionStatus
} // namespace MissionCtrl
} // namespace UAS
@@ -0,0 +1,45 @@
/**
* Dispatcher for ReportPoint message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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 &/*Obj*/, 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);
}
} // namespace ReportPoint
} // namespace ObjectManager
} // namespace UAS
@@ -0,0 +1,35 @@
/**
* Dispatcher for ReportPoint message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-21
*/
#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;
};
} // namespace ReportPoint
} // namespace ObjectManager
} // namespace UAS
@@ -0,0 +1,76 @@
/**
* Dispatcher for ReportPositions message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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;
}
// key should be different per drone ... so filter out keys we don't need
if (KeyId != "UTM_GLOBAL_POSITION" && KeyId != "CURRENT_AIRFRAME_RAW" && KeyId != "CURRENT_AIRFRAME")
{
DEBUG_LOG(m_logger) << "KeyId unwanted (is '" << QString::fromStdString(KeyId) << "')";
return;
}
// position
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 });
}
} // namespace ReportPositions
} // namespace CoordinateFrame
} // namespace UAS
@@ -0,0 +1,35 @@
/**
* Dispatcher for ReportPositions message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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;
};
} // namespace ReportPositions
} // namespace CoordinateFrame
} // namespace UAS
@@ -0,0 +1,58 @@
/**
* Dispatcher for ReportRadioConnectionStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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; // Yes, "Diconnecting"
case M_Teleoperation::RadioLinkConnectionState::Connected: return Type::Connected;
case M_Teleoperation::RadioLinkConnectionState::Error: return Type::Error;
}
return Type::Undefined; // compiler meckert
}
namespace RadioLinkConnectionState
{
Dispatcher::Dispatcher()
: m_logger(getLogger("RadioLinkConnectionState.Dispatcher"))
{}
void Dispatcher::ReportRadioConnectionStatus(const std::string &/*KeyId*/, 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 });
}
} // namespace RadioLinkConnectionState
} // namespace TeleOperation
} // namespace UAS
@@ -0,0 +1,37 @@
/**
* Dispatcher for ReportRadioConnectionStatus message
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-15
*/
#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;
};
} // namespace RadioLinkConnectionState
} // namespace TeleOperation
} // namespace UAS
@@ -0,0 +1,37 @@
/**
* @RDE_HEADER@
* Logger
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-27
*/
#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();
}
}
} // namespace UAS
@@ -0,0 +1,24 @@
/**
* @RDE_HEADER@
* Logger
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-10-27
*/
#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());
}
} // namespace UAS
@@ -0,0 +1,90 @@
/**
* Filled with data by a tms-if thread and can be used for thread-safe access to this data to outside
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-10-15
*/
#pragma once
#include <QObject>
#include <QQueue>
#include <QMutex>
namespace UAS
{
class ReaderSignal : public QObject // helper class as QObject can't be used with templates
{
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) // called from thread, locked
{
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;
};
} // namespace UAS
@@ -0,0 +1,204 @@
/**
* Starts and manages TMS-IF
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-04-17
*/
#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;
/// TmsIfAdapterImpl
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->deleteLater();
m_iai = nullptr;
}
UAVCommander *createUAV(int domainId, int eqId) override
{
assert(m_isReady);
return new UAVCommanderImpl(domainId, eqId, m_guard);
}
void setReady() // called by InterfaceAdapterInitializer
{
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;
};
/**
* InterfaceAdapterInitializer stops all ia instances and waits until all have
* stopped, then reports ready to the parent
*/
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"))
{
/*DEBUG_LOG(m_logger)*/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())
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "trying to stop active uas - DONE!";
t->stop();
m_parent->setReady();
deleteLater();
return;
}
/*DEBUG_LOG(m_logger)*/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;
// ObjectManager
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"))
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "TmsIfAdapter running!";
}
TmsIfAdapterImpl::~TmsIfAdapterImpl()
{
delete m_iai;
delete m_guard;
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "!!! TmsIfAdapterImpl DTOR!";
}
/// TmsIfAdapter
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;
}
}
} // namespace UAS
/// [exports]
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>
@@ -0,0 +1,794 @@
/**
* Implementation of UAV - actual communication to TMS-IF
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#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"))
{
/*DEBUG_LOG(m_logger)*/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);
// force first state
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()
{
/*DEBUG_LOG(m_logger)*/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;
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "currentTask = " << (int)m_MissionCtrl.currentTask;
const auto oldMission = m_MissionCtrl.currentMission;
m_MissionCtrl.currentMission = QString::fromStdString(msg.currentMissionName);
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "oldTask = " << (int)oldTask;
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "currentMission = " << QString::fromStdString(msg.currentMissionName);
m_MissionCtrl.currentlyAllowedTasks.swap(msg.currentlyAllowedTasks); // we will throw away 'msg' anyways
if (m_MissionCtrl.ReportMissionStatus_Callback)
{
/* only send new status if either task and/or mission have changed (because e.g. sending
* loiter 2 times (for some reason idk) would skip one mission plan action in CES,
* because it is reacting to the loiter task) */
/*DEBUG_LOG(m_logger)*/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(); // mission now ended (mission was set and is now unset)
}
m_MissionCtrl.ReportMissionStatus_Callback(msg);
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "################################# sending " << (int)m_MissionCtrl.currentTask << " " << (quintptr)this;
}
else
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "################################# VERWERFING ! " << (int)m_MissionCtrl.currentTask << " " << (quintptr)this;
}
}
emit newCurrentTask(msg.currentTask);
switch (m_MissionCtrl.currentTask) // initially get uav into a good state
{
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; // good state, nothing to do
}
}
}
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()
{
/*DEBUG_LOG(m_logger)*/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);
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "before callback";
if (m_Device.ReportDeviceStatus_Callback)
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "callback!";
m_Device.ReportDeviceStatus_Callback(msg);
}
}
catch (...)
{
break;
}
}
}
void UAVCommanderImpl::prepareMission(Resolver *resolver)
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "resolver = " << (quintptr)resolver;
if (isTaskAllowed(MissionCtrl::Task::Observe) || m_MissionCtrl.currentTask == MissionCtrl::Task::Connect /* the simulator doesn't tell us that observe (with missions) is allowed while loitering, but it is - so this is a workaround */)
{
/*DEBUG_LOG(m_logger)*/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]()
{
/*DEBUG_LOG(m_logger)*/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()) // mission active? abort mission first!
{
/*DEBUG_LOG(m_logger)*/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); }}); // aborting mission (should) results into loitering
return;
}
switch (m_MissionCtrl.currentTask) // step by step
{
case MissionCtrl::Task::Disarm:
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << " > arm";
arm(r);
break;
}
case MissionCtrl::Task::Arm:
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << " > takeoff";
takeoff(r);
break;
}
case MissionCtrl::Task::Takeoff:
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << " > waiting for Loiter ...";
sendCommand(r, {QStringLiteral("loiter"), MissionCtrl::Task::Loiter, nullptr});
break;
}
case MissionCtrl::Task::Loiter:
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << " >WE loiter start to resolve";
emit resolver->resolve();
break;
}
// was tun bei takeoff oder return? da muss man einfach warten. aber kann es überhaupt dazu kommen dass takeoff oder return kommen? bei neuer mission? idk?
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)
{
/*DEBUG_LOG(m_logger)*/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]()
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "!!!!!!!!!!!!!!!!!!!!!! going to observe now!!";
// step 2: start mission
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]()
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "################ mission done" << missionName << " " << this << " ";
// We could do cleanup here.
// But don't delete mission, this is done pre upload or after delete/all missions fullfilled
//_abortMission(missionName);
//m_MissionCtrl.Writer->CmdDeleteMission(currentMission.toStdString());
});
sendCommand(r3, {QStringLiteral("wait for mission to end (Loiter)"), MissionCtrl::Task::Loiter, nullptr}, -1);
/*DEBUG_LOG(m_logger)*/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); } });
});
// step 1: prepare UAV for mission
prepareMission(r); // UAV needs to loiter (in the air ofc) before a mission can be performed
}
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)
{
/*DEBUG_LOG(m_logger)*/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();
/*DEBUG_LOG(m_logger)*/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]() { /*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "returnToHome done :)"; });
sendCommand(r, { QStringLiteral("return"), MissionCtrl::Task::Return, [this]() { _returnToHome(); } }, -1 /* takes as long as it takes */);
}
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) // can also be used to just wait for a state change with empty cmd.fn
{
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) // in case that a return was sent while e.g. a mission is active, reject the running command and continue with the new command
{
m_currentResolver->reject(QStringLiteral("Command aborted"));
}
m_currentResolver = resolver;
/*DEBUG_LOG(m_logger)*/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()); // create a reader that receives ReportMissions from the existing data dispatcher
connect(guard, &QObject::destroyed, this, [reader]() { delete reader; });
auto callCommandFn = [this, commandName, commandFn]()
{
if (commandFn)
{
/*DEBUG_LOG(m_logger)*/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 + max tries reached - giving up
{
timeout->stop();
guard->deleteLater();
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << commandName << ": sending '"<< commandName <<"' command failed - max tries reached, giving up !";
m_tried = 0; // reset, so other requests can start now
m_currentResolver.clear();
emit resolver->reject(QStringLiteral("Sending %1 command timed out!").arg(commandName));
return;
}
// try again
callCommandFn();
timeout->start();
});
}
// read until timeout and stop on success (if task matches requested task)
connect(reader->readerSignal(), &ReaderSignal::dataReady,
this, [this, reader, guard, timeout, commandName, type, resolver]()
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << commandName << ": reader signal for '"<< commandName <<"' !";
while(true)
{
try
{
const auto msg = reader->getNext();
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "########## READER SIGNAL (should: " << (int)type << ", is: " << (int)msg.currentTask << ")";
if (msg.currentTask == type
// special case: takeoff first executes a takeoff (task = "takeoff") and when it's done, task becomes "loiter" - similar for return
|| (type == MissionCtrl::Task::Takeoff && msg.currentTask == MissionCtrl::Task::Connect)
|| (type == MissionCtrl::Task::Return && (msg.currentTask == MissionCtrl::Task::Disarm || msg.currentTask == MissionCtrl::Task::Land)))
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << commandName << ": successfully set new task '"<< commandName <<"' !";
// delay to let the drone catch up (parrot might get stuck if sent to quick ??)
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]()
{
/*DEBUG_LOG(m_logger)*/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; // done !
}
}
catch(...)
{
break;
}
}
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << commandName << ": command '"<< commandName <<"' not accepted (yet)";
// timeout timer will take care of the rest
});
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}, {}, {}, {}), {})); // send arm to drone
}
void UAVCommanderImpl::disarm(Resolver *resolver)
{
// TODO: ????????
Q_UNUSED(resolver)
}
void UAVCommanderImpl::_disarm()
{
m_MissionCtrl.writer->CmdPerformAction(M_MissionCtrl::CommandedActionType(M_MissionCtrl::TaskType({M_MissionCtrl::TaskCategoryKind::Type::Disarm}, {}, {}, {}), {})); // send disarm to drone
}
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}, {}, {}, {}), {})); // send takeoff to drone
}
/// CreateInterfaceAdapterInstance
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]()
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "readerSignal for uasId" << m_uasId;
const auto msgs = readAll<ObjectManager::ReportPoint::Message>(m_reader);
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "msgs.size" << msgs.size() << m_uasId;
for (const auto &msg : msgs)
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "msg uas" << msg.uasId << "removed" << msg.isRemoved;
if (msg.uasId == uasId && !msg.isRemoved)
{
// (already) started, done
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "DONE!!";
m_timeout->stop();
return;
}
}
// at this point, there's no 'point' for us - start instance
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "no ia instance for"<<m_uasId<<"found !! starting timeout";
if (!m_timeout->isActive()) // don't restart timer if it's already running to keep the 2 seconds interval correctly
{
m_timeout->start();
}
});
onStart();
emit m_reader.readerSignal(); // trigger first read for messages that were received before connecting the signal
m_timeout->setInterval(2000);
connect(m_timeout, &QTimer::timeout, this, &CreateInterfaceAdapterInstance::onStart);
m_timeout->start();
}
void CreateInterfaceAdapterInstance::onStart()
{
/*DEBUG_LOG(m_logger)*/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 });
// start new ia instance
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 (almost exact copy of CreateInterfaceAdapterInstance)
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;
}
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "readerSignal for uasId" << m_uasId;
const auto msgs = readAll<ObjectManager::ReportPoint::Message>(m_reader);
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "msgs.size" << msgs.size() << m_uasId;
for (const auto &msg : msgs)
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "msg uas" << msg.uasId << "removed" << msg.isRemoved;
if (msg.uasId == uasId && msg.isRemoved)
{
// now stopped, done
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "DONE!!";
m_done = true;
deleteLater();
return;
}
}
});
emit m_reader.readerSignal(); // trigger first read for messages that were received before connecting the signal
m_timeout->setInterval(2000); // delay remove, don't directly remove
connect(m_timeout, &QTimer::timeout, this, &KillInterfaceAdapterInstanceAsync::onStop);
m_timeout->start();
}
void KillInterfaceAdapterInstanceAsync::onStop()
{
// stop ia instance
try
{
m_writer->CmdRemove(std::to_string(m_uasId));
}
catch (const std::exception &ex)
{
/*DEBUG_LOG(m_logger)*/ERROR_LOG(m_logger) << "CmdRemove" << m_uasId << ":" << ex.what();
}
}
} // namespace UAS
@@ -0,0 +1,266 @@
/**
* Implementation of UAV - actual communication to TMS-IF
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#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 }; // as multiple commands cannot be issued to the uav, this variable is shared between all commands and resets when a command is called
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; //SendInfo
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;
}
} // namespace UAS
@@ -0,0 +1,31 @@
/**
* Creates and manages the TmsIfAdapter class to interface with the libTmsIfAdapter interface
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-04-17
*/
#pragma once
#include <functional>
namespace UAS
{
class UAVCommander;
class TmsIfAdapter
{
public:
virtual ~TmsIfAdapter() = default;
static TmsIfAdapter &get(); // NOTE: call stop() before application exit
static void stop();
virtual bool isReady() const = 0;
virtual void set_isReady_Callback(std::function<void()> callbackFn) = 0;
virtual UAVCommander *createUAV(int domainId, int uasId) = 0;
};
} // namespace UAS
@@ -0,0 +1,223 @@
/**
* Sends / receives data to / from one UAVCommander via TMS-IF
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#pragma once
#include <string>
#include <functional>
#include <vector>
#include <QVector3D>
#include <QString>
namespace UAS
{
namespace CoordinateFrame
{
namespace ReportPositions
{
struct Message
{
double lat;
double lon;
double alt;
int64_t timestamp;
};
using CallbackFn = std::function<void(const CoordinateFrame::ReportPositions::Message &data)>;
}
}
namespace TeleOperation {
/// Enumeration of states of a the radio link.
enum Type
{
/// The connection is in a undefined state.
Undefined,
/// The link is currently disconnected.
Disconnected,
/// The link is currently connecting.
Connecting,
/// The link is currently disconnecting
Diconnecting,
/// The link is currently connected.
Connected,
/// The link is currently in error state.
Error
};
namespace RadioLinkConnectionState {
struct Message
{
Type status;
};
using CallbackFn = std::function<void(const TeleOperation::RadioLinkConnectionState::Message &data)>;
}
}
namespace BuiltInTest {
/// Enumeration of states of a the radio link.
namespace TestResult {
/// Test result type enumeration.
enum Type
{
/// Unknown -- test has not yet been completed or could not be initiated.
Undefined,
/// OK -- no failure was detected.
OK,
/// Warning -- a situation which warrants a warning was detected.
Warning,
/// Reduced performance -- an error was detected which results in reduced performance.
Reduced_Performance_Error,
/// Fatal error -- a failure was detected which results in complete loss of functionality.
Fatal_Error
};
}
namespace ReportCBitStatus {
struct Message
{
std::string Key;
std::string RoutineName;
BuiltInTest::TestResult::Type Result;
unsigned long ErrorCode;
std::string ErrorCodeDescription;
};
using CallbackFn = std::function<void(const ReportCBitStatus::Message &data)>;
}
}
namespace Device {
/// Enumeration of states of a the radio link.
namespace OperationModeStateKind {
/// Operation mode state enumeration.
enum Type
{
/// Unknown mode state.
Undefined,
/// Off.
Off,
/// On.
On,
/// Standby.
Standby,
/// Initialising.
Init,
/// Shutting down.
Shutdown,
/// Normal operation.
Normal,
/// Manual operation.
Manual,
/// Automatic operation.
Auto,
/// Calibrating.
Calibrating,
/// Maintenance.
Maintenance
};
}
namespace ReportDeviceStatus
{
struct Message
{
OperationModeStateKind::Type Status;
};
using CallbackFn = std::function<void(const ReportDeviceStatus::Message &data)>;
}
}
namespace ObjectManager
{
namespace ReportPoint
{
struct Message
{
int uasId;
bool isRemoved;
};
}
}
namespace MissionCtrl
{
enum Task // see tmsif/type/MissionCtrl.hpp:43
{
Undefined, // 0
Connect, // 1
Disconnect, // 2
Arm, // 3
Disarm, // 4
Fire,
Observe, // 6
/// Let vehicle ascend.
Climb,
/// Let vehicle descend.
Descend,
/// Perform vehicle takeoff.
Takeoff, // 9
/// Perform vehicle landing.
Land, // 10
Reboot, // 11
Shutdown, // 12
/// Loiter around a specified position.
Loiter, // 13
/// Return to a specified position.
Return, // 14
/// Follow a specified entity.
Follow, // 15
/// Stop current motion/action.
Stop,
/// Resume the stopped motion/action.
Resume
};
namespace ReportMissionStatus
{
struct Message
{
std::string currentMissionName;
Task currentTask;
std::vector<Task> currentlyAllowedTasks;
std::string missionEnded;
};
using CallbackFn = std::function<void(const MissionCtrl::ReportMissionStatus::Message &data)>;
}
}
class UAVCommander
{
public:
virtual ~UAVCommander() = default;
bool isReady() const { return m_isReady; }
virtual void startIA(const std::string &ip, const uint16_t port, const bool useTcp) noexcept(false) = 0;
virtual void sendMission(double lat, double lon, double alt = 0, const QString &missionName = {}) noexcept(false) = 0;
virtual void returnToHome() = 0;
virtual void sendTryReclaimAutoMode() = 0;
virtual void set_CoordinateFrame_ReportPosition_Callback(CoordinateFrame::ReportPositions::CallbackFn callbackFn) = 0;
virtual void set_isReady_Callback(std::function<void()> callbackFn) = 0;
virtual void set_ReportCBitStatus_Callback(BuiltInTest::ReportCBitStatus::CallbackFn callbackFn) = 0;
virtual void set_ReportDeviceStatusType_Callback(Device::ReportDeviceStatus::CallbackFn callbackFn) = 0;
virtual void set_ReportRadioLinkConnectionState_Callback(TeleOperation::RadioLinkConnectionState::CallbackFn callbackFn) = 0;
virtual void set_ReportMissionStatus_Callback(MissionCtrl::ReportMissionStatus::CallbackFn callbackFn) = 0;
protected:
bool m_isReady { false };
};
} // namespace UAS
+247
View File
@@ -0,0 +1,247 @@
/**
* "SendToTab" implementation to receive requests from TAB (which originate from the OpApp)
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#include "from_tab.h"
#include "uav_ia_manager.h"
#include <NGLLogger/NGLLoggerBootstrap.h>
#include <ProjectDataManagement/pdm_dtc.h>
#include <TrainingAreaBase/recv_from_tab.h>
#include <QJsonDocument>
#include <QJsonObject>
namespace UAS
{
FromTab::FromTab(const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &sendToDds,
UAVInterfaceAdapterManager *uavIAManager, int exerciseId)
: m_isConnected(false)
, m_dtcPdm(ProjectDataManagement::Interface::Dtc::create(ddsHandler->ddsConnector()))
, m_dtcPpm(ParticipantManagement::Interface::Dtc::create(ddsHandler->ddsConnector()))
, m_sendToDds(sendToDds)
, m_uavIAManager(uavIAManager)
, m_exerciseId(exerciseId)
, m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger().getNGLLogger("FromTab"))
{
m_dtcPpm->enableUAS();
m_dtcPpm->enableParticipant();
}
TrainingAreaBase::SendToTab::HandleState FromTab::canHandleTelegram(int equipmentNumber, int telegramId) const
{
bool found = false;
const auto &uasList = m_dtcPpm->uASs();
for (const auto &uas : uasList)
{
if (uas.exerciseId() != m_exerciseId)
{
continue;
}
const auto &part = m_dtcPpm->participant(uas.participantId());
if (!part.equipmentNumber().is_set())
{
continue;
}
if (part.equipmentNumber().get() == equipmentNumber)
{
found = true;
break;
}
}
using namespace TrainingAreaBase::TetraIcd;
if (!found)
{
return HandleState::No;
}
if (telegramId == Constraints<UAVPerformMission>::id())
{
return HandleState::YesConsume; // we're the only plugin able to fly drones ;)
}
if (telegramId == Constraints<UAVChangeState>::id())
{
return HandleState::YesConsume; // we're the only plugin able to fly drones ;)
}
if (telegramId == Constraints<CesHitRequest>::id())
{
return HandleState::YesShare; // participants hit (jam) the drone, we need this info to tell the drone to e.g. return to home after a hit
}
if (telegramId == Constraints<DemandVulnerabilityStateChange>::id())
{
return HandleState::YesConsume;
}
if (telegramId == Constraints<Init>::id())
{
return HandleState::YesConsume;
}
return HandleState::No; // we handle drone telegrams ONLY
}
bool FromTab::txData(const TrainingAreaBase::TetraIcd::Init &data)
{
if (!data.resetVulnerabilityState)
{
return false;
}
//send statechange init
TrainingAreaBase::TetraIcd::StateChange msg;
msg.header.equipmentNumber = data.header.equipmentNumber;
msg.header.timeStamp = QDateTime::currentMSecsSinceEpoch(); //data.timestamp * 1000;
msg.originatorId = data.workstationId;
msg.originatorType = TrainingAreaBase::TetraIcd::OriginatorType::ExerciseControl;
msg.tasVitality = 100;
msg.vulnerabilityBits = 0;
msg.vulnerabilityState = TrainingAreaBase::TetraIcd::VulnerabilityState::CombatReady;
m_sendToDds->rxData(msg);
return true;
}
bool FromTab::txData(const TrainingAreaBase::TetraIcd::UAVPerformMission &data)
{
const int eqId = data.header.equipmentNumber;
DEBUG_LOG(m_logger) << "eqId: " << eqId;
const ParticipantManagement::Participant p = m_dtcPpm->participant(m_dtcPpm->participantIdByEquipmentNumber(eqId));
if (p.id() <= 0)
{
WARNING_LOG(m_logger) << "participant not found - stop";
return false;
}
// NOTE: only first action is performed ...
const auto &action = data.actions.front();
if (!m_uavIAManager->waypoint(p.id(), action.absPosition, QStringLiteral("%1_%2").arg(data.missionId).arg(data.missionName))) // send waypoint to uav
{
WARNING_LOG(m_logger) << "sending waypoint failed!";
return false;
}
return true;
}
bool FromTab::txData(const TrainingAreaBase::TetraIcd::UAVChangeState &data)
{
const int eqId = data.header.equipmentNumber;
const ParticipantManagement::Participant p = m_dtcPpm->participant(m_dtcPpm->participantIdByEquipmentNumber(eqId));
if (p.id() <= 0)
{
WARNING_LOG(m_logger) << "participant not found - stop";
return false;
}
// NOTE: only first action is performed ...
switch (data.requestedState)
{
case TrainingAreaBase::TetraIcd::UAVState::Auto:
return m_uavIAManager->tryChangeModeToAuto(p.id());
default:
return true;
}
return true;
}
bool FromTab::txData(const TrainingAreaBase::TetraIcd::CesHitRequest &data)
{
// drone was hit by jamming
DEBUG_LOG(m_logger) << "data.chargeTasCodeDirect = " << data.chargeTasCodeDirect;
// NOTE: igniterType/IgniterFunciton = 0 as long as CesHitRequest does not contain values
if (!m_dtcPdm->isJamming(data.chargeTasCodeDirect,0,0))
{
WARNING_LOG(m_logger) << "not jamming";
return false;
}
DEBUG_LOG(m_logger) << "participantEventChanged: JAMMING! chargetasCodeDirect = " << data.chargeTasCodeDirect;
const int eqId = data.header.equipmentNumber;
const ParticipantManagement::Participant p = m_dtcPpm->participant(m_dtcPpm->participantIdByEquipmentNumber(eqId));
if (p.id() <= 0)
{
WARNING_LOG(m_logger) << "participant not found - stop";
return false;
}
m_uavIAManager->kill(p.id()); // kill UAV in EXCON and return UAV home
TrainingAreaBase::TetraIcd::StateChangeDirect msg;
msg.chargeTypeId = data.chargeTasCodeDirect; // = jamming
msg.header.equipmentNumber = eqId;
msg.header.timeStamp = data.header.timeStamp;
msg.vulnerabilityState = TrainingAreaBase::TetraIcd::VulnerabilityState::CatastrophicKill;
msg.originatorId = data.originatorEquipmentNumber;
DEBUG_LOG(m_logger) << "sending state change direct (kill) with jamming for eqId " << msg.header.equipmentNumber;
m_sendToDds->rxData(msg);
return true;
}
bool FromTab::txData(const TrainingAreaBase::TetraIcd::DemandVulnerabilityStateChange &data)
{
const auto p = m_dtcPpm->participantIdByEquipmentNumber(data.header.equipmentNumber);
// we could trigger RTH if needed with:
// if (p > 0 && m_uavIAManager)
// m_uavIAManager->kill(p); // kill UAV in EXCON and return UAV home
const auto &pData = m_dtcPpm->dynamicParticipantData(p);
for (const auto &dyn : pData.stateHistory())
{
if (dyn.vulnerabilityState() == ProjectDataManagement::VulnerabilityState::CatastrophicKill)
{
return false;
}
}
//send statechange init
TrainingAreaBase::TetraIcd::StateChange msg;
msg.header.equipmentNumber = data.header.equipmentNumber;
msg.header.timeStamp = QDateTime::currentMSecsSinceEpoch(); //data.timestamp * 1000;
msg.originatorId = data.workstationId;
msg.originatorType = TrainingAreaBase::TetraIcd::OriginatorType::ExerciseControl;
msg.tasVitality = 0;
msg.vulnerabilityBits = 0;
msg.vulnerabilityState = data.reqestedState;
m_sendToDds->rxData(msg);
return true;
}
bool FromTab::txData(const TrainingAreaBase::TetraIcd::OcCommand &data)
{
Q_UNUSED(data)
return false;
}
} // namespace UAS
+103
View File
@@ -0,0 +1,103 @@
/**
* "SendToTab" implementation to receive requests from TAB (which originate from the OpApp)
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#pragma once
#include <NGLLogger/INGLLogger.h>
#include <TrainingAreaBase/send_to_tab.h>
#include <TrainingAreaBase/dds_handler.h>
#include <ParticipantManagement/ppm_dtc.h>
namespace ProjectDataManagement { namespace Interface { class Dtc; } }
namespace ParticipantManagement { namespace Interface { class Dtc; } }
namespace TrainingAreaBase { class RecvFromTab; }
namespace UAS
{
class UAVInterfaceAdapterManager;
class FromTab final : public QObject, public TrainingAreaBase::SendToTab
{
Q_OBJECT
public:
explicit FromTab(const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &sendToDds,
UAVInterfaceAdapterManager *uavIAManager,
int exerciseId);
const char *name() const override { return "UAS"; }
HandleState canHandleTelegram(int equipmentNumber, int telegramId) const override;
void resetSendCounter() override {};
void setConnected(bool connected) { m_isConnected = connected; }
bool txData(const TrainingAreaBase::TetraIcd::Acknowledge &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DemandBite &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::Init &data) override;
bool txData(const TrainingAreaBase::TetraIcd::SetVulnerabilityModel &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DemandEquipmentConfigurationReport &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DemandVulnerabilityStateChange &data) override;
bool txData(const TrainingAreaBase::TetraIcd::LanguageSelection &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::EquipmentMessage &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::EquipmentMessageOC &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateSingleExplosion &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateAreaExplosion &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateMineArea &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::MineAreaSingleMineState &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateNContamination &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateCContamination &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DeactivateEffectSimulation &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ModifyAreaEffectSimulationName &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::PauseEffectSimulation &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ResumeEffectSimulation &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::MineTriggered &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DusMineReactivation &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateFunctionalArea &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CreateDusSingleMine &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ExerciseEnvironmentData &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ExerciseStateData &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ParticipantSwVersionsRequest &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::OpaquePayloadDataOut &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::TechnicalArea &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::CesHitInformation &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ParticipantMode &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DemandParticipantMode &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ParticipantConfiguration &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::DemandParticipantConfiguration &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ExerciseEquipmentAssignmentComplete &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::Sanitation &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::SelectiveReammunition &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::SetDevicePowerRequest &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::OcCommand &data) override;
bool txData(const TrainingAreaBase::TetraIcd::UAVPerformMission &) override;
bool txData(const TrainingAreaBase::TetraIcd::UAVChangeState &) override;
bool txData(const TrainingAreaBase::TetraIcd::CesHitRequest &) override;
bool txData(const TrainingAreaBase::TetraIcd::ExFireReadiness &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ExFiredReport &) override { return false; };
bool txData(const TrainingAreaBase::TetraIcd::ExConfirmationReport &) override { return false; };
bool sendToTx(const TrainingAreaBase::TetraIcd::TrainingAreaTelegram *data);
Q_SIGNALS:
void outgoingMessage(const QByteArray &msg);
protected:
bool m_isConnected;
std::shared_ptr<ProjectDataManagement::Interface::Dtc> m_dtcPdm;
std::shared_ptr<ParticipantManagement::Interface::Dtc> m_dtcPpm;
std::shared_ptr<TrainingAreaBase::RecvFromTab> m_sendToDds;
UAVInterfaceAdapterManager *m_uavIAManager;
int m_exerciseId;
NGL::INGLLogger &m_logger;
// SendToTab interface
public:
};
} // namespace UAS
+37
View File
@@ -0,0 +1,37 @@
/**
* @RDE_HEADER@
* Logger
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#include "logger.h"
#include <NGLLogger/NGLLoggerBootstrap.h>
namespace UAS
{
LOGGER_T getLogger(const char *name)
{
static const char ns[] = "TAB.UAS.";
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();
}
}
} // namespace UAS
+24
View File
@@ -0,0 +1,24 @@
/**
* @RDE_HEADER@
* Logger
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#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());
}
} // namespace UAS
+56
View File
@@ -0,0 +1,56 @@
/**
* UAS Plugin Interface Factory
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#include "plugin.h"
#include "plugin_instance.h"
#include <nglsettings/nglsettings_settings.h>
#include <QSettings>
#include <QDebug>
namespace UAS
{
std::shared_ptr<TrainingAreaBase::Plugin::Interface> TabPluginUAS::createInterface(TrainingAreaBase::Plugin::DataProvider *dataProvider,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &recvFromTabCb,
const std::shared_ptr<TrainingAreaBase::TxMixin> &,
const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
NGL::Settings *settings)
{
qDebug() << "UAS Plugin starting for ex "<<dataProvider->exerciseId()<<"!\n";
/* set this on the machine running the TMS-IF TrainingAreaBase plugin, to the
* ip address of the interface of the server which is in the same network as the
* gateway machines (e.g. Salchau's tds01 is 10.5.144.32 and the gateway machines
* are in the same subnet, so enter "10.5.144.32" on tds01) */
const QString ip(settings->settings()->value(QStringLiteral("UAS/TmsIfIpAddress"), QStringLiteral("127.0.0.1")).toString()); // 127.0.0.1 if IA runs on the same machine
qDebug() << "TabPluginUAS::createInterface: TMS-IF will be started on the network interface with ip address: '" << ip << "' \n";
qputenv("CYCLONEDDS_URI", QStringLiteral("<CycloneDDS><Domain><General><Interfaces><NetworkInterface address=\"%1\"></NetworkInterface></Interfaces></General></Domain></CycloneDDS>").arg(ip).toUtf8());
if ( !hostAvailable(ip))
return nullptr;
return std::make_shared<PluginInstance>(dataProvider, recvFromTabCb, ddsHandler, settings);
}
bool TabPluginUAS::hostAvailable(QString hostname) {
// Blocking call: This pauses execution until the lookup is finished
QHostInfo info = QHostInfo::fromName(hostname);
// Check if the lookup was successful
if (info.error() == QHostInfo::NoError) {
qCritical() << "Success! Host found. IP addresses:" << hostname;
return true;
}
// Handle errors like 'Host not found' or 'Network unreachable'
return false;
}
} // namespace UAS
+37
View File
@@ -0,0 +1,37 @@
/**
* UAS Plugin Interface Factory
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#pragma once
#include <QObject>
#include <QHostInfo>
#include <TrainingAreaBase/tab_plugin_interface.h>
namespace UAS
{
class TabPluginUAS : public QObject, public TrainingAreaBase::Plugin::Plugin
{
Q_OBJECT
Q_PLUGIN_METADATA(IID "special:3:tab.uas")
Q_INTERFACES(TrainingAreaBase::Plugin::Plugin)
private:
bool hostAvailable(QString hostname);
public:
TabPluginUAS() = default;
virtual ~TabPluginUAS() = default;
std::shared_ptr<TrainingAreaBase::Plugin::Interface> createInterface(TrainingAreaBase::Plugin::DataProvider *dataProvider,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &recvFromTabCb,
const std::shared_ptr<TrainingAreaBase::TxMixin> &sendToTabCb,
const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
NGL::Settings *settings) override;
};
} // namespace UAS
@@ -0,0 +1,118 @@
/**
* Instance of the UAS plugin
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#include "plugin_instance.h"
#include "from_tab.h"
#include "uav_ia_manager.h"
#include <TrainingAreaBase/send_to_dds.h>
#include <TrainingAreaBase/training_area_telegrams.h>
#include <CentralEffectSimulation/ces_dtc.h>
#include <ProjectDataManagement/pdm_dtc.h>
#include <ParticipantManagement/ppm_dtc.h>
#include <FileManagement/flm_dtc.h>
#include <FileManagement/flm_ftc.h>
#include <TmsIfAdapter/module_loader.h>
#include <TmsIfAdapter/tms_if_adapter.h>
#include <TmsIfAdapter/uav_commander.h>
#include <QCoreApplication>
namespace UAS
{
TmsIfModuleHandle::TmsIfModuleHandle()
: m_handle(ModuleLoader::load(QString(QCoreApplication::applicationDirPath() + QStringLiteral("/../lib/")).toStdString())) // won't work on windows as libs are always inside the bin dir (QCoreApplication::applicationFilePath)
{}
TmsIfModuleHandle::~TmsIfModuleHandle()
{
try
{
ModuleLoader::unload(m_handle);
}
catch(std::exception &ex)
{
qDebug() << "failed to unload tms-if adapter module! " << ex.what();
}
}
PluginInstance::PluginInstance(TrainingAreaBase::Plugin::DataProvider *dataProvider,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &sendToDds,
const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
NGL::Settings *settings)
: m_exerciseId(dataProvider->exerciseId())
, m_sendToDds(sendToDds)
, m_settings(settings)
, m_tmsIfAdapter(ModuleLoader::get(m_tmsIfModuleHandle()))
, m_ddsHandler(ddsHandler)
, m_dtcPdm(ProjectDataManagement::Interface::Dtc::create(m_ddsHandler->ddsConnector()))
, m_dtcPpm(ParticipantManagement::Interface::Dtc::create(m_ddsHandler->ddsConnector()))
, m_uavIAManager(new UAVInterfaceAdapterManager(m_ddsHandler, m_sendToDds, m_tmsIfAdapter, m_exerciseId, this))
, m_fromTab(new FromTab(ddsHandler, m_sendToDds, m_uavIAManager, m_exerciseId))
, m_logger(getLogger("PluginInstance"))
{
DEBUG_LOG(m_logger) << "UAS PluginInstance running!";
if (!m_dtcPdm)
{
WARNING_LOG(m_logger) << "m_dtcPdm failed";
throw std::runtime_error("m_dtcPdm failed");
}
m_dtcPdm->enableChargeType(); // for m_dtcPdm->isJamming(..)
m_dtcPdm->waitForHistoricalDataChargeType(3000);
if (!m_dtcPpm)
{
WARNING_LOG(m_logger) << "m_dtcPpm failed";
throw std::runtime_error("m_dtcPpm failed");
}
m_dtcPpm->enableParticipant(); // for looking up and receiving UAV participants to start UAV IA process(es)
m_dtcPpm->waitForHistoricalDataParticipant(3000);
// only start interface adapter *instances* when tmsIfAdapter is done setting up the interface adapter itself
if (m_tmsIfAdapter.isReady())
{
m_uavIAManager->init();
}
else
{
m_tmsIfAdapter.set_isReady_Callback([this]()
{
m_uavIAManager->init();
});
}
}
bool PluginInstance::init()
{
return true;
}
std::shared_ptr<TrainingAreaBase::SendToTab> PluginInstance::sendToTab()
{
return m_fromTab;
}
bool PluginInstance::canSendData() const
{
return true;
}
} // namespace UAS
//#include <plugin_instance.moc> // temp! for Positions test class
+81
View File
@@ -0,0 +1,81 @@
/**
* Instance of the UAS plugin
* $Id$
* $HeadURL$
* $Author$
* Created by Mike Wille, ESZS, 2025-04-17
*/
#pragma once
#include "logger.h"
#include <TrainingAreaBase/tab_plugin_interface.h>
#include <TrainingAreaBase/dds_handler.h>
#include <DDS/Core/function_reply_object.h> // tr_lts
#include <QObject>
namespace ProjectDataManagement { namespace Interface { class Dtc; } }
namespace ParticipantManagement { namespace Interface { class Dtc; } }
namespace ProjectDataManagement { namespace Interface { class Ftc; } }
namespace ParticipantManagement { namespace Interface { class Ftc; } }
namespace UAS
{
class TmsIfAdapter;
class FromTab;
class UAVInterfaceAdapterManager;
class TmsIfModuleHandle
{
public:
TmsIfModuleHandle();
~TmsIfModuleHandle();
void *operator()()
{
return m_handle;
}
private:
void *m_handle { nullptr };
};
class PluginInstance final : public TrainingAreaBase::Plugin::Interface
{
Q_OBJECT
public:
PluginInstance(TrainingAreaBase::Plugin::DataProvider *dataProvider,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &recvFromTabCb,
const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
NGL::Settings *settings);
std::shared_ptr<TrainingAreaBase::SendToTab> sendToTab() override; // the implementation / our version if SendToTab, so that TAB can send us telegrams
bool isDebugInterface() const override { return false; }
bool canSendData() const override;
void exerciseChanged() override {}
const char *name() const override { return tr_lts("dds", "UAS"); }
public slots:
bool init() override;
private:
int m_exerciseId;
std::shared_ptr<TrainingAreaBase::RecvFromTab> m_sendToDds;
NGL::Settings *m_settings;
/*void * */ TmsIfModuleHandle m_tmsIfModuleHandle; //{ nullptr };
TmsIfAdapter &m_tmsIfAdapter;
std::shared_ptr<TrainingAreaBase::DdsHandler> m_ddsHandler;
std::shared_ptr<ProjectDataManagement::Interface::Dtc> m_dtcPdm;
std::shared_ptr<ParticipantManagement::Interface::Dtc> m_dtcPpm;
std::shared_ptr<ProjectDataManagement::Interface::Ftc> m_ftcPpm;
UAVInterfaceAdapterManager *m_uavIAManager;
std::/*unique_ptr*/shared_ptr<FromTab> m_fromTab;
NGL::INGLLogger &m_logger;
};
} // namespace UAS
+30
View File
@@ -0,0 +1,30 @@
/**
* Contains uav ia instances (for tms-if) per uav participant
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-05-22
*/
#include "uav_ia.h"
#include "logger.h"
#include <TmsIfAdapter/uav_commander.h>
namespace UAS
{
UAVInterfaceAdapter::UAVInterfaceAdapter(UAVInterfaceAdapterConfig uavConfig, UAVCommander *commander)
: m_config(std::move(uavConfig))
, m_commander(commander)
, m_logger(getLogger(QStringLiteral("UAVInterfaceAdapter.%1").arg(m_config.m_uasId)))
{}
UAVInterfaceAdapter::~UAVInterfaceAdapter()
{
DEBUG_LOG(m_logger) << "terminating IA for UAV - " << (quintptr)this;
delete m_commander;
}
} // namespace UAS
+70
View File
@@ -0,0 +1,70 @@
/**
* Starts / stops uav ia instances (for tms-if) per uav participant
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-05-22
*/
#pragma once
#include <QObject>
namespace NGL
{
class INGLLogger;
}
namespace UAS
{
class UAVCommander;
using UasId = int;
struct UAVInterfaceAdapterConfig
{
public:
UAVInterfaceAdapterConfig(const QString &ip, int port, UasId uasId, bool useTcp)
: m_ip(ip)
, m_port(port)
, m_uasId(uasId)
, m_useTcp(useTcp)
{}
bool operator==(const UAVInterfaceAdapterConfig &o) const
{
return (o.m_ip == m_ip
&& o.m_port == m_port
&& o.m_uasId == m_uasId
&& o.m_useTcp == m_useTcp);
}
QString m_ip;
int m_port;
UasId m_uasId;
bool m_useTcp;
};
class UAVInterfaceAdapter : public QObject
{
Q_OBJECT
public:
UAVInterfaceAdapter(UAVInterfaceAdapterConfig uavConfig, UAVCommander *commander); // copy elision
~UAVInterfaceAdapter();
UAVInterfaceAdapterConfig config() const { return m_config; }
UAVCommander &commander() { return *m_commander; }
private:
UAVInterfaceAdapterConfig m_config;
UAVCommander *m_commander;
NGL::INGLLogger &m_logger;
};
} // namespace UAS
+488
View File
@@ -0,0 +1,488 @@
/**
* Manages the uav interface instances (for TMS-IF) per uav participant
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-05-22
*/
#include "uav_ia_manager.h"
#include "ParticipantManagement/ppm_ftc.h"
#include "logger.h"
#include <LtsUtilsCore/json.h>
#include <LtsCoordTrans/lts_coord_trans.h>
#include <ParticipantManagement/ppm_dtc.h>
#include <TrainingAreaBase/recv_from_tab.h>
#include <TmsIfAdapter/tms_if_adapter.h>
#include <TmsIfAdapter/uav_commander.h>
#include <QVector3D>
#include <QTimer>
#include <vector>
namespace UAS
{
const int UAVInterfaceAdapterManager::s_domainId = 4;
UAVInterfaceAdapterManager::UAVInterfaceAdapterManager(const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &sendToDds,
TmsIfAdapter &tmsIfAdapter, int exerciseId, QObject *parent)
: QObject(parent)
, m_exerciseId(exerciseId)
, m_dtcPpm(ParticipantManagement::Interface::Dtc::create(ddsHandler->ddsConnector()))
, m_ftcPpm(ParticipantManagement::Interface::Ftc::create(ddsHandler->ddsConnector()))
, m_tmsIfAdapter(tmsIfAdapter)
, m_sendToDds(sendToDds)
, m_logger(getLogger("UAVManager"))
{}
void UAVInterfaceAdapterManager::init()
{
DEBUG_LOG(m_logger) << "UAVInterfaceAdapterManager starting for ex " << m_exerciseId;
m_dtcPpm->waitForHistoricalDataDynamicParticipantData(3000);
m_dtcPpm->waitForHistoricalDataParticipant(3000);
m_dtcPpm->waitForHistoricalDataUAS(3000);
m_dtcPpm->waitForHistoricalDataMissionPlan(3000);
connect(m_dtcPpm.get(), &ParticipantManagement::Interface::Dtc::uASChanged,
this, [this](const ParticipantManagement::UAS &p, DataModification mod)
{
switch (mod)
{
case DataModification::eRemoved:
{
removed(p);
return;
}
case DataModification::eModified:
{
modified(p);
return;
}
case DataModification::eAdded:
{
added(p);
return;
}
}
});
for (const auto &p : m_dtcPpm->uASs())
{
added(p);
}
if (!m_dtcPpm->isUASEnabled())
{
m_dtcPpm->enableUAS();
}
DEBUG_LOG(m_logger) << "done";
}
bool UAVInterfaceAdapterManager::tryChangeModeToAuto(int participantId)
{
return kill(participantId);
}
void UAVInterfaceAdapterManager::removed(const ParticipantManagement::UAS &uas)
{
//There should be no active UAV but just in case
if (m_uavs.find(uas.id()) != m_uavs.cend())
{
WARNING_LOG(m_logger) << "tried removing participant " << uas.id() << " that doesn't exist";
return;
}
DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " found! removing.....";
stop(uas);
}
void UAVInterfaceAdapterManager::modified(const ParticipantManagement::UAS &uas)
{
DEBUG_LOG(m_logger) << "modify uas " << uas.id();
// if typeId has changed, it's not a UAV anymore, so stop and done
if (uas.participantId() <= 0)
{
WARNING_LOG(m_logger) << "UAV with uas id " << uas.id() << " not a UAV anymore - removing";
stop(uas); // remove
return;
}
const UasId uasId = uas.id();
// relying on dds working as intended and not doing anything if UAV doesn't exist
const auto i = m_uavs.find(uasId);
if (i == m_uavs.cend())
{
WARNING_LOG(m_logger) << "UAV with uas id " << uas.id() << " was modified but now found in our list - feeding it as 'added' instead as a bail-out";
added(uas); // add, maybe UAV was linked to an participant again - note that if the UAV was unlinked, it would have been stopped some lines above (see uas.participantId check above)
return;
}
const UAVInterfaceAdapter &uav2 = *i->second;
DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " found";
try
{
UAVInterfaceAdapterConfig config(QString::fromStdString(uas.hostname()), uas.hostport(), uas.id(), uas.useTcp());
if (uav2.config() == config)
{
DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " already running and config has not changed - doing nothing (irrelevant data has changed)";
return;
}
// restart:
stop(uas);
QTimer::singleShot(5000, this, [this, uas, config] // wait until everything is stopped
{
start(uas, config);
});
}
catch(const std::exception &ex)
{
WARNING_LOG(m_logger) << "Failed to modify UAV: " << ex.what();
}
}
void UAVInterfaceAdapterManager::added(const ParticipantManagement::UAS &uas)
{
DEBUG_LOG(m_logger) << "exId: " << m_exerciseId << ", uas: " << uas.id() << ", participantId: " << uas.participantId()<<" ## use tcp ? "<<uas.useTcp();
const auto participant = m_dtcPpm->participant(uas.participantId());
if (!participant.equipmentNumber())
{
return;
}
if (uas.exerciseId() != m_exerciseId)
{
return;
}
DEBUG_LOG(m_logger) << "added uav with id " << uas.id() << " for pId " << uas.participantId();
if (m_uavs.find(uas.id()) != m_uavs.cend())
{
// assert(false);
DEBUG_LOG(m_logger) << "participant " << uas.id() << " already started - doing nothing";
return;
}
DEBUG_LOG(m_logger) << "############# ALLES OKAY uas: " << uas.id() << " - pId: " << uas.participantId();
DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " found";
try
{
UAVInterfaceAdapterConfig config(QString::fromStdString(uas.hostname()), uas.hostport(), uas.id(), uas.useTcp());
start(uas, config);
}
catch (const std::exception &ex)
{
WARNING_LOG(m_logger) << "Failed to start uav ia: " << ex.what();
}
}
void UAVInterfaceAdapterManager::start(const ParticipantManagement::UAS &uas, UAVInterfaceAdapterConfig config)
{
try
{
const ParticipantId pId = uas.participantId();
const auto participant = m_dtcPpm->participant(uas.participantId());
if (participant.id() < 1 || !participant.equipmentNumber())
{
throw std::runtime_error(QStringLiteral("pid < 1 or eqid not set for pid %1").arg(pId).toStdString());
}
const auto uasId = uas.id();
const auto droneEqId = participant.equipmentNumber().value();
const auto droneParticipantId = uas.participantId();
const int droneDomainId = s_domainId;
const bool useTcp = uas.useTcp();
DEBUG_LOG(m_logger) << "starting IA for eqId = " << droneEqId << ", pid = " << droneParticipantId << ", domain = " << droneDomainId << ", uasId = " << uasId<<" // ** useTcp: "<<uas.useTcp();
auto *uav = m_tmsIfAdapter.createUAV(droneDomainId, uasId); // Do weneed one drone per domain or n drones per domain but with different service names (instead of always 1) ?
m_uavs[uasId] = new UAVInterfaceAdapter(/*std::move*/(config), uav); // starts tms if uav ia
const auto fn = [this, droneDomainId, droneParticipantId, droneEqId, useTcp]()
{
DEBUG_LOG(m_logger) << "UAV " << droneDomainId << " / " << droneParticipantId <<" / use TCP ? "<<useTcp<< " ready !!"; // << " ("<<(quintptr)this<<")";
const auto pdata = m_dtcPpm->dynamicParticipantData(droneParticipantId);
if (pdata.participantId() < 1 || pdata.stateHistory().empty()) // if a participant doesn't have dynamic pdata or a state history, that means it's not even combat ready or got whatever state - so set it on first ready
{
DEBUG_LOG(m_logger) << "pdata or state history for pid " << droneParticipantId << " is empty - sending init";
TrainingAreaBase::TetraIcd::StateChange msg;
msg.header.equipmentNumber = droneEqId;
msg.header.timeStamp = QDateTime::currentMSecsSinceEpoch(); //data.timestamp * 1000;
msg.originatorId = 0;
msg.originatorType = TrainingAreaBase::TetraIcd::OriginatorType::Initialisation;
msg.tasVitality = 100;
msg.vulnerabilityBits = 0;
msg.vulnerabilityState = TrainingAreaBase::TetraIcd::VulnerabilityState::CombatReady;
m_sendToDds->rxData(msg);
DEBUG_LOG(m_logger) << "sent combat ready for " << droneParticipantId << " - done";
}
else
{
WARNING_LOG(m_logger) << "state history for pid " << droneParticipantId << " not empty - nothing to do";
}
};
uav->set_isReady_Callback(fn);
uav->set_CoordinateFrame_ReportPosition_Callback([this, droneEqId](const CoordinateFrame::ReportPositions::Message &data)
{
// send drone position to DDS, to display the participant on the map
TrainingAreaBase::TetraIcd::PositionReport msg;
msg.header.equipmentNumber = droneEqId;
msg.header.timeStamp = QDateTime::currentMSecsSinceEpoch(); //data.timestamp * 1000;
msg.geoPosition = { data.lon * LtsCoordTrans::deg2rad(), data.lat * LtsCoordTrans::deg2rad(), data.alt }; // convert lat/lon in deg (uav format) to lat/lon in rad (excon format)
DEBUG_LOG(m_logger) << "POSITION! sending PositionReport into TAB"; // << " ("<<(quintptr)this<<")";
m_sendToDds->rxData(msg);
});
uav->set_ReportCBitStatus_Callback([this, droneEqId](const BuiltInTest::ReportCBitStatus::Message &data)
{
Q_UNUSED(droneEqId)
Q_UNUSED(data)
Q_UNUSED(this)
});
uav->set_ReportDeviceStatusType_Callback([this, droneEqId](const Device::ReportDeviceStatus::Message &data)
{
TrainingAreaBase::TetraIcd::UAVStatus msg;
msg.header.equipmentNumber = droneEqId;
//now we need to handle the stuff
switch (data.Status)
{
case Device::OperationModeStateKind::Auto:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Auto;
break;
case Device::OperationModeStateKind::Calibrating:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Calibrating;
break;
case Device::OperationModeStateKind::Init:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Init;
break;
case Device::OperationModeStateKind::Maintenance:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Maintenance;
break;
case Device::OperationModeStateKind::Manual:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Manual;
setMissionActionFulfilled(droneEqId);
break;
case Device::OperationModeStateKind::Normal:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Normal;
break;
case Device::OperationModeStateKind::Off:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Off;
break;
case Device::OperationModeStateKind::On:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::On;
break;
case Device::OperationModeStateKind::Undefined:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Undefined;
break;
case Device::OperationModeStateKind::Standby:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Standby;
break;
case Device::OperationModeStateKind::Shutdown:
msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Shutdown;
break;
}
m_sendToDds->rxData(msg);
DEBUG_LOG(m_logger) << "RadioLinkConnectionState! sending RadioLinkConnectionState into TAB"; // << " ("<<(quintptr)this<<")";
});
uav->set_ReportRadioLinkConnectionState_Callback([this, droneEqId](const TeleOperation::RadioLinkConnectionState::Message &data)
{
DEBUG_LOG(m_logger) << "COULD! send RadioLinkConnectionState! sending RadioLinkConnectionState into TAB << " << droneEqId << " " << data.status; // << " ("<<(quintptr)this<<")";
});
uav->set_ReportMissionStatus_Callback([this, droneEqId](const MissionCtrl::ReportMissionStatus::Message &data)
{
DEBUG_LOG(m_logger) << "ReportMissionStatus! sending ReportMissionStatus into TAB << " << droneEqId << " - currentTask = " << (int)data.currentTask << ", currentMission = " << data.currentMissionName;
TrainingAreaBase::TetraIcd::UAVMissionStatus msg;
msg.header.equipmentNumber = droneEqId;
msg.missionEnded = QString::fromStdString(data.missionEnded);
msg.currentMissionName = QString::fromStdString(data.currentMissionName);
// NOTE: currentlyAllowedTasks ignored here
switch (data.currentTask)
{
case MissionCtrl::Task::Disarm: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Disarm; break;
case MissionCtrl::Task::Arm: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Arm; break;
case MissionCtrl::Task::Takeoff: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Takeoff; break;
case MissionCtrl::Task::Connect: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Connect; break;
case MissionCtrl::Task::Loiter: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Loiter; break;
case MissionCtrl::Task::Return: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Return;
DEBUG_LOG(m_logger) << "Return";
setMissionActionFulfilled(droneEqId);
break;
case MissionCtrl::Task::Observe: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Observe; break;
// NOTE: all other cases are unused and will be returned as undefined
default: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Undefined; break;
}
m_sendToDds->rxData(msg);
});
uav->startIA(config.m_ip.toStdString(), config.m_port, config.m_useTcp);
DEBUG_LOG(m_logger) << "DONE STARTING UAV!";
}
catch (const std::exception &ex)
{
//try again later??
WARNING_LOG(m_logger) << "Failed to start UAV: " << ex.what();
}
}
void UAVInterfaceAdapterManager::stop(const ParticipantManagement::UAS &uas)
{
DEBUG_LOG(m_logger) << "############################### STOP !! " << uas.id();
auto it = m_uavs.find(uas.id());
if (it == m_uavs.cend())
{
DEBUG_LOG(m_logger) << "uav not found - ignoring :/";
return;
}
it->second->deleteLater();
m_uavs.erase(it);
}
bool UAVInterfaceAdapterManager::kill(int participantId)
{
const auto uas = m_dtcPpm->getUasByParticipantId(participantId, m_exerciseId);
if (uas.id() < 1)
{
DEBUG_LOG(m_logger) << "uas by participant id " << participantId << " not found - stop";
return false;
}
auto i = m_uavs.find(uas.id());
if (i == m_uavs.cend())
{
DEBUG_LOG(m_logger) << "uav by participant id " << participantId << " not found - stop";
return false;
}
auto &uav = *i;
uav.second->commander().returnToHome();
return true;
}
bool UAVInterfaceAdapterManager::waypoint(int participantId, TrainingAreaBase::TetraIcd::AbsolutePosition absPos, const QString &missionName)
{
const auto uas = m_dtcPpm->getUasByParticipantId(participantId, m_exerciseId);
if (uas.id() < 1)
{
DEBUG_LOG(m_logger) << "uas by participant id " << participantId << " not found - stop";
return false;
}
auto i = m_uavs.find(uas.id());
if (i == m_uavs.cend())
{
DEBUG_LOG(m_logger) << "uav by participant id " << participantId << " not found - stop";
for (const auto &uav : m_uavs)
{
DEBUG_LOG(m_logger) << "pid: " << uav.first;
}
return false;
}
if (i->second == nullptr)
{
WARNING_LOG(m_logger) << "UAVInterfaceAdapterManager::waypoint: NULLPTR for "<<participantId;
return false;
}
auto &uav = *i;
try
{
// I dont think z will fit for alt
uav.second->commander().sendMission(absPos.lat, absPos.lon, absPos.z, missionName);
return true;
}
catch (const std::exception& e)
{
WARNING_LOG(m_logger) << "UAVInterfaceAdapterManager::waypoint: " << e.what();
return false;
}
}
void UAVInterfaceAdapterManager::setMissionActionFulfilled(int equipmentNumber)
{
///- send setFulfillment request to svcParticipantManagement
DEBUG_LOG(m_logger) << "setMissionActionFulfilled equipmentId=" << equipmentNumber;
const int missionId = m_dtcPpm->uasMissionIdByEquipmentNumber(equipmentNumber); // Todo get from equipmentId
ParticipantManagement::SetMissionActionFulfilledRequest abortReq;
abortReq.missionId(missionId);
abortReq.itemNumber(-1);
abortReq.actionNumber(-1);
try
{
m_ftcPpm->setMissionActionFulfilledRequest(abortReq);
}
catch (const std::exception &e)
{
WARNING_LOG(m_logger) << " -- unable to abort mission with planId:"<<abortReq.missionId() << ": " << e.what();
}
return;
}
} // namespace UAS
+77
View File
@@ -0,0 +1,77 @@
/**
* Manages the uav interface instances (for TMS-IF) per uav participant
* $Id$
* $HeadURL$
* $Author$
*
* Created by Mike Wille, ESZS, 2025-05-22
*/
#pragma once
#include "uav_ia.h"
#include <TrainingAreaBase/training_area_telegrams.h>
#include <TrainingAreaBase/dds_handler.h>
#include <ParticipantManagement/ppm_ftc.h>
#include <QObject>
#include <QMap>
#include <memory>
namespace ParticipantManagement
{
class UAS;
class MissionPlan;
namespace Interface { class Dtc; }
}
namespace TrainingAreaBase { class RecvFromTab; }
namespace NGL
{
class INGLLogger;
}
namespace UAS
{
class TmsIfAdapter;
using ParticipantId = int;
class UAVInterfaceAdapterManager : public QObject
{
Q_OBJECT
public:
UAVInterfaceAdapterManager(const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
const std::shared_ptr<TrainingAreaBase::RecvFromTab> &sendToDds,
TmsIfAdapter &tmsIfAdapter, int exerciseId, QObject *parent);
void init();
bool kill(int participantId);
bool waypoint(int participantId, TrainingAreaBase::TetraIcd::AbsolutePosition absPos, const QString &missionName = {});
bool tryChangeModeToAuto(int participantId);
private:
void removed(const ParticipantManagement::UAS &uas);
void modified(const ParticipantManagement::UAS &uas);
void added(const ParticipantManagement::UAS &uas);
void start(const ParticipantManagement::UAS &uas, UAVInterfaceAdapterConfig config);
void stop(const ParticipantManagement::UAS &uas);
void setMissionActionFulfilled(int equipmentNumber);
static const int s_domainId;
int m_exerciseId;
std::map<UasId, UAVInterfaceAdapter *> m_uavs;
std::shared_ptr<ParticipantManagement::Interface::Dtc> m_dtcPpm;
std::shared_ptr<ParticipantManagement::Interface::Ftc> m_ftcPpm;
TmsIfAdapter &m_tmsIfAdapter;
std::shared_ptr<TrainingAreaBase::RecvFromTab> m_sendToDds;
NGL::INGLLogger &m_logger;
};
} // namespace UAS