489 lines
16 KiB
C++
Executable File
489 lines
16 KiB
C++
Executable File
/**
|
|
* 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
|