270 lines
10 KiB
C++
Executable File
270 lines
10 KiB
C++
Executable File
|
|
// **************************************************************************
|
|
//
|
|
// (C) Copyright 2009 Swiss SIMTEC AG
|
|
//
|
|
// **************************************************************************
|
|
//
|
|
// $HeadURL$
|
|
// $LastChangedBy$
|
|
// $LastChangedDate$
|
|
// $LastChangedRevision$
|
|
//
|
|
// **************************************************************************
|
|
|
|
// manual class headers
|
|
#include "GeoPairingImplFiredBulletBalistic.hpp"
|
|
#include <GBL_ConfigSystem/geoconfig.h>
|
|
|
|
// from cpp_includes
|
|
#include <FW_Ballistic/ITrajectory.hpp>
|
|
#include <FW_Ballistic/IFireControl.hpp>
|
|
#include <FW_Ballistic/BallisticService.hpp>
|
|
#include <GBL_Types/Angle3.hpp>
|
|
#include "geo_settings.h"
|
|
|
|
namespace N_GeoPairingImpl
|
|
{
|
|
|
|
// ---------------------------------------------------------------
|
|
// Constructur taking the SensorData as input values
|
|
// ---------------------------------------------------------------
|
|
GeoPairingImplFiredBulletBalistic::GeoPairingImplFiredBulletBalistic(const GeoPairingImplSensorData &a_sensorData, const char *loggerName)
|
|
: GeoPairingImplFiredBullet(a_sensorData, loggerName ? loggerName : "GeoPairingImplFiredBulletBalistic")
|
|
{
|
|
m_UtmSensor.setLlPositionDegree(m_SensorData.WeaponPositionLat,
|
|
m_SensorData.WeaponPositionLong,
|
|
m_SensorData.WeaponPositionHeight);
|
|
m_ballisticService = std::make_shared<N_Ballistic::BallisticService>();
|
|
|
|
m_ballisticService->init(N_GeoPairingImpl::Settings::cesGeopairingIniPath(),
|
|
N_GeoPairingImpl::Settings::cesGeopairingIniFile(),
|
|
N_GeoPairingImpl::Settings::cesAmmunitionIniFile());
|
|
m_SensorData.IgnitionType = a_sensorData.IgnitionType;
|
|
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// Performs the calculation of the trajectory and also returns
|
|
// the result of calculation
|
|
// ---------------------------------------------------------------
|
|
GeoPairingImplFiredBullet::E_CalcResult GeoPairingImplFiredBulletBalistic::calculate(const std::shared_ptr<N_Ballistic::BallisticService> &ballisticService)
|
|
{
|
|
m_ballisticService = ballisticService;
|
|
E_CalcResult l_cr = e_NONE;
|
|
N_ConfigSystem::GeoConfig *l_config = N_ConfigSystem::GeoConfig::getInstance();
|
|
|
|
QMap<int, QString> ammuMap = l_config->getAmmuMap();
|
|
QMap<int, QVector<double> > chargeMap = l_config->getChargeMap();
|
|
|
|
INFO_LOG(m_logger) << "----------- Calculating Impact -----------";
|
|
|
|
N_Types::Coord3 l_sensorPos(m_UtmSensor.utmEasting(),
|
|
m_UtmSensor.utmNorthing(),
|
|
m_UtmSensor.height());
|
|
INFO_LOG(m_logger) << "l_sensorPos : " << l_sensorPos;
|
|
|
|
N_Math::GeoCoord l_checkUtm;
|
|
l_checkUtm.setTotalUtmPosition(l_sensorPos.m_X, l_sensorPos.m_Y, l_sensorPos.m_Z,
|
|
m_UtmSensor.utmZone(), m_UtmSensor.northP());
|
|
|
|
N_Types::Coord3 l_checkPos(l_checkUtm.latDeg(), l_checkUtm.lonDeg(), l_checkUtm.height());
|
|
INFO_LOG(m_logger) << "o_checkPos : " << l_checkPos;
|
|
INFO_LOG(m_logger) << "m_SensorData.AmmunitionType : " << m_SensorData.AmmunitionType;
|
|
QString l_ammunition = ammuMap[m_SensorData.AmmunitionType];
|
|
INFO_LOG(m_logger) << "l_ammunition=" << l_ammunition;
|
|
|
|
|
|
// ---------------- Preparing Calculation
|
|
N_Ballistic::IFireControl *lp_fireControl = nullptr;
|
|
if (!m_ballisticService)
|
|
{
|
|
ERROR_LOG(m_logger) << "!m_ballisticService";
|
|
return l_cr;
|
|
}
|
|
bool ok = m_ballisticService->createInterfaceOf((N_Ballistic::IUnknown **)&lp_fireControl, N_Ballistic::BallisticService::e_IFireControl, l_ammunition);
|
|
if (!ok)
|
|
{
|
|
ERROR_LOG(m_logger) << "couldn't create interface ";
|
|
return l_cr;
|
|
}
|
|
if (!lp_fireControl)
|
|
{
|
|
ERROR_LOG(m_logger) << "couldn't initialize fireControl for " << l_ammunition;
|
|
return l_cr;
|
|
}
|
|
|
|
double l_density = lp_fireControl->getAirDensity(m_MeteoData.AirPressure, m_MeteoData.Temperature);
|
|
INFO_LOG(m_logger) << "l_density = " << l_density;
|
|
|
|
INFO_LOG ( m_logger) << "getInitialAmmuVelocity=" << m_SensorData.WeaponTemperature;
|
|
double l_initialspeed = lp_fireControl->getInitialAmmuVelocity(m_SensorData.WeaponTemperature);
|
|
INFO_LOG ( m_logger) << "l_initialspeed=" << l_initialspeed;
|
|
|
|
|
|
QMap<int, QVector<double> >::iterator chargeIt = chargeMap.find(m_SensorData.AmmunitionType);
|
|
if (chargeIt != chargeMap.end())
|
|
{
|
|
QVector<double> l_charges = chargeMap[m_SensorData.AmmunitionType];
|
|
if (m_SensorData.ChargeType >= 0 && m_SensorData.ChargeType < l_charges.size())
|
|
{
|
|
l_initialspeed = l_charges[m_SensorData.ChargeType];
|
|
INFO_LOG(m_logger) << "Charge chosen for " << l_ammunition << " : " << l_initialspeed;
|
|
}
|
|
}
|
|
else
|
|
INFO_LOG(m_logger) << "l_initialspeed = " << l_initialspeed;
|
|
|
|
GeoPairingImplTrajectoryData::TrajectoryPoint tpSensor;
|
|
tpSensor.m_H = l_sensorPos.m_Z;
|
|
tpSensor.m_Vel = l_initialspeed;
|
|
tpSensor.m_Rho = l_density;
|
|
m_TrajectoryData.m_Points.push_back(tpSensor);
|
|
|
|
N_Types::Radian64 l_windDir;
|
|
l_windDir.setDegree(m_MeteoData.WindDirection);
|
|
N_Types::Coord3 l_windAngle(0, m_MeteoData.WindSpeed, 0);
|
|
l_windAngle = l_windAngle.rotateZ(l_windDir);
|
|
INFO_LOG(m_logger) << "o_windAngle = " << l_windAngle;
|
|
double l_crossWind = l_windAngle.rotateZ(-m_SensorData.WeaponAzimuth).getX();
|
|
INFO_LOG(m_logger) << "l_crossWind = " << l_crossWind;
|
|
|
|
// ------------------ Prepare Trajectory
|
|
N_Ballistic::ITrajectory *lp_trajectory = nullptr;
|
|
//if (!BallisticService::createInterfaceOf((IUnknown **)&lp_trajectory, BallisticService::e_ITrajectory, l_ammunition))
|
|
//m_ballisticService->createInterfaceOf((IUnknown **)&lp_fireControl, BallisticService::e_IFireControl, l_ammunition);
|
|
m_ballisticService->createInterfaceOf((N_Ballistic::IUnknown **)&lp_trajectory, N_Ballistic::BallisticService::e_ITrajectory, l_ammunition);
|
|
|
|
if (!lp_trajectory)
|
|
{
|
|
ERROR_LOG(m_logger) << "Couldn't create Trajectory object for " << l_ammunition;
|
|
return l_cr;
|
|
}
|
|
|
|
N_Types::Angle3 l_sensorAngle(m_SensorData.WeaponAzimuth,
|
|
m_SensorData.WeaponElevation,
|
|
0);
|
|
INFO_LOG(m_logger) << "o_sensorAngle : " << l_sensorAngle;
|
|
|
|
if (!lp_trajectory->init(l_sensorPos, l_sensorAngle, l_crossWind, l_density, l_initialspeed))
|
|
{
|
|
lp_trajectory->release();
|
|
ERROR_LOG(m_logger) << "Couldn't initialize Trajectory";
|
|
return l_cr;
|
|
}
|
|
|
|
|
|
// ------------------- Loop Trajectory
|
|
double l_time = 0;
|
|
QString s_Precision = QStringLiteral("Precision");
|
|
QString s_DeltaTime = QStringLiteral("DeltaTime");
|
|
|
|
double l_timeDelta = l_config->getFloat(s_Precision, s_DeltaTime, 0.05);
|
|
INFO_LOG(m_logger) << "Used Precision DeltaTime = " << l_timeDelta;
|
|
|
|
N_Types::Coord3 l_pos;
|
|
|
|
while (lp_trajectory->isActive())
|
|
{
|
|
l_time += l_timeDelta;
|
|
lp_trajectory->move(l_timeDelta);
|
|
|
|
if (m_SensorData.IgnitionType == 0 && !lp_trajectory->isActive())
|
|
{
|
|
INFO_LOG(m_logger) << "Timeout of Bullet! IgnitionType=" << m_SensorData.IgnitionType;
|
|
l_cr = e_TIMEOUT;
|
|
break;
|
|
}
|
|
|
|
l_pos = lp_trajectory->getPosition();
|
|
|
|
N_Math::GeoCoord l_checkUtmShadow;
|
|
l_checkUtmShadow.setTotalUtmPosition(l_pos.m_X, l_pos.m_Y, l_pos.m_Z,
|
|
m_UtmSensor.utmZone(), m_UtmSensor.northP());
|
|
|
|
N_Types::Coord3 l_checkPosShadow(l_checkUtmShadow.lonDeg(), l_checkUtmShadow.latDeg(), l_checkUtmShadow.height());
|
|
|
|
INFO_LOG(m_logger) << "l_pos "
|
|
<< " X=" << l_pos.getX()
|
|
<< " Y=" << l_pos.getY()
|
|
<< " Z=" << l_pos.getZ()
|
|
<< " m_X=" << l_pos.m_X
|
|
<< " m_Y=" << l_pos.m_Y
|
|
<< " m_Z=" << l_pos.m_Z
|
|
<< " l_checkUtm.height=" << l_checkUtmShadow.height()
|
|
<< " l_checkPos.z=" << l_checkPosShadow.getZ();
|
|
|
|
double l_height = 0;
|
|
if (checkTerrainHit(l_checkPosShadow, l_height))
|
|
{
|
|
INFO_LOG(m_logger) << "deactivate";
|
|
lp_trajectory->deactivate();
|
|
l_cr = e_OK;
|
|
}
|
|
else
|
|
{
|
|
if (m_SensorData.IgnitionType > 0 && l_time > m_SensorData.IgnitionType)
|
|
{
|
|
INFO_LOG(m_logger) << "deactivate";
|
|
lp_trajectory->deactivate();
|
|
l_cr = e_OK;
|
|
INFO_LOG(m_logger) << "Detonation by Timer : " << m_SensorData.IgnitionType;
|
|
}
|
|
}
|
|
|
|
GeoPairingImplTrajectoryData::TrajectoryPoint tpImpact;
|
|
tpImpact.m_X = l_pos.m_X - l_sensorPos.m_X;
|
|
tpImpact.m_Y = l_pos.m_Y - l_sensorPos.m_Y;
|
|
tpImpact.m_Z = l_pos.m_Z - l_sensorPos.m_Z;
|
|
tpImpact.m_H = l_height;
|
|
tpImpact.m_Vel = lp_trajectory->getVelocity().magnitude();
|
|
tpImpact.m_Time = l_time;
|
|
m_TrajectoryData.m_Points.push_back(tpImpact);
|
|
|
|
}
|
|
|
|
l_pos = lp_trajectory->getPosition();
|
|
|
|
// INFO_LOG(m_logger) << "Total Distance = " << l_pos.length(l_sensorPos);
|
|
INFO_LOG(m_logger) << "Total Time = " << l_time;
|
|
|
|
lp_trajectory->release();
|
|
lp_fireControl->release();
|
|
|
|
INFO_LOG(m_logger) << "l_pos : " << l_pos;
|
|
|
|
double l_Distance = l_pos.length(l_sensorPos);
|
|
INFO_LOG(m_logger) << "l_Distance = " << l_Distance;
|
|
|
|
N_Math::GeoCoord l_impactUtm;
|
|
l_impactUtm.setTotalUtmPosition(l_pos.m_X, l_pos.m_Y, l_pos.m_Z,
|
|
m_UtmSensor.utmZone(), m_UtmSensor.northP());
|
|
|
|
INFO_LOG(m_logger) << "l_impactUtm (lat/lon/h) : " << l_impactUtm.latDeg()
|
|
<< "/" << l_impactUtm.lonDeg() << "/" << l_impactUtm.height()
|
|
<< " zone=" << m_UtmSensor.utmZone()
|
|
<< " northP=" << m_UtmSensor.northP();
|
|
|
|
|
|
m_ImpactData.AmmunitionType = m_SensorData.AmmunitionType;
|
|
m_ImpactData.ImpactTime = m_SensorData.FireTime + l_time;
|
|
m_ImpactData.ImpactPositionLat = l_impactUtm.latDeg();
|
|
m_ImpactData.ImpactPositionLong = l_impactUtm.lonDeg();
|
|
m_ImpactData.ImpactPositionHeight = l_impactUtm.height();
|
|
|
|
return l_cr;
|
|
}
|
|
|
|
|
|
// ---------------------------------------------------------------
|
|
bool GeoPairingImplFiredBulletBalistic::checkTerrainHit(const N_Types::Coord3 &a_pos, double &a_height)
|
|
{
|
|
a_height = 0;
|
|
INFO_LOG(m_logger) << "checkTerrainHit"
|
|
<< " a_pos.m_Z=" << a_pos.m_Z
|
|
<< " m_UtmSensor.height()=" << m_UtmSensor.height();
|
|
return a_pos.m_Z < m_UtmSensor.height();
|
|
}
|
|
|
|
} // namespace N_GeoPairingImpl
|