Initial commit
This commit is contained in:
commit
b7d90b9afe
16245 files changed
+3396666
No files matched your search
+46
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;
|
||||
}
|
||||
@@ -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
@@ -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
@@ -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)
|
||||
+84
@@ -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
|
||||
+30
@@ -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;
|
||||
};
|
||||
Executable
+57
@@ -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
|
||||
Executable
+37
@@ -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
|
||||
Executable
+76
@@ -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
|
||||
Executable
+38
@@ -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
|
||||
Executable
+106
@@ -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
|
||||
Executable
+36
@@ -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
|
||||
+45
@@ -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
|
||||
+35
@@ -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
|
||||
Executable
+76
@@ -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
|
||||
+35
@@ -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
|
||||
+58
@@ -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
|
||||
+37
@@ -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
|
||||
+204
@@ -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>
|
||||
+794
@@ -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
|
||||
+266
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
|
||||
@@ -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
@@ -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
@@ -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
@@ -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
|
||||
@@ -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
|
||||
Reference in new issue
Block a user