407 lines
14 KiB
C++
Executable File
407 lines
14 KiB
C++
Executable File
// **************************************************************************
|
|
//
|
|
// (C) Copyright 2009 Swiss SIMTEC AG
|
|
//
|
|
// **************************************************************************
|
|
//
|
|
// $HeadURL$
|
|
// $LastChangedBy$
|
|
// $LastChangedDate$
|
|
// $LastChangedRevision$
|
|
//
|
|
// **************************************************************************
|
|
|
|
// manual class headers
|
|
#include "GeoPairingImpl.hpp"
|
|
#include <GBL_ConfigSystem/geoconfig.h>
|
|
#include <FW_Ballistic/AmmunitionTypes.hpp>
|
|
#include "../settings.h"
|
|
|
|
// from cpp_includes
|
|
#include <GBL_Types/Radian64.hpp>
|
|
#include "GeoPairingImplAirTargetCacheData.hpp"
|
|
|
|
#include "GeoPairingDataFireReport.hpp"
|
|
#include <QDateTime>
|
|
|
|
#define PAN_AMMU_TIMEPROG(ammunition) (ammunition & 0x01)
|
|
#define PAN_AMMU_INDEX(ammunition) ((ammunition >> 1) & 0x03)
|
|
#define PAN_AMMU_CHARGE(ammunition) ((ammunition >> 3) & 0x07)
|
|
|
|
#define PAN_AMMU_INDEX_G5_G6(ammunition) (ammunition & 0x0F)
|
|
#define PAN_AMMU_CHARGE_G5_G6(ammunition) ((ammunition >> 4) & 0x0F)
|
|
|
|
namespace N_GeoPairingImpl
|
|
{
|
|
|
|
inline void endian_swap(UInt16 &a_value)
|
|
{
|
|
a_value = (a_value >> 8) |
|
|
(a_value << 8);
|
|
}
|
|
inline void endian_swap(UInt32 &a_value)
|
|
{
|
|
a_value = (a_value >> 24) |
|
|
((a_value << 8) & 0x00FF0000) |
|
|
((a_value >> 8) & 0x0000FF00) |
|
|
(a_value << 24);
|
|
}
|
|
|
|
GeoPairingImpl::GeoPairingImpl(const char *loggerName)
|
|
: m_Initialized(false)
|
|
, m_logger(CesGeoPairing::getLogger(loggerName))
|
|
{
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// creates a new air target tracker
|
|
// ---------------------------------------------------------------
|
|
GeoPairingImplAirTargetTracker *GeoPairingImpl::createAirTargetTracker()
|
|
{
|
|
return new GeoPairingImplAirTargetTracker(&m_ListAirTargets);
|
|
}
|
|
|
|
|
|
// ---------------------------------------------------------------
|
|
// creates a new air target updater
|
|
// ---------------------------------------------------------------
|
|
GeoPairingImplAirTargetUpdater *GeoPairingImpl::createAirTargetUpdater()
|
|
{
|
|
return new GeoPairingImplAirTargetUpdater(&m_ListAirTargets);
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// creates a new air target cache
|
|
// ---------------------------------------------------------------
|
|
GeoPairingImplAirTargetCache *GeoPairingImpl::createAirTargetCache(UInt32 a_playerID)
|
|
{
|
|
return new GeoPairingImplAirTargetCache(a_playerID, &m_CacheData);
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// Pupulates a SensorData instance according to the given
|
|
// transparent datastream
|
|
// ---------------------------------------------------------------
|
|
SInt32 GeoPairingImpl::createSensorData(GeoPairingImplSensorData *a_sensorData, const QByteArray& a_transparendData, int &isDefault)
|
|
{
|
|
INFO_LOG(m_logger) << "createSensorData";
|
|
if (!m_Initialized)
|
|
{
|
|
ERROR_LOG(m_logger) << "Implementation not initialized";
|
|
return -1;
|
|
}
|
|
|
|
if (!a_sensorData)
|
|
{
|
|
ERROR_LOG(m_logger) << "SensorData Object argument not valid";
|
|
return -1;
|
|
}
|
|
|
|
GeoPairingDataFireReport l_Report;
|
|
if (!l_Report.parse(a_transparendData))
|
|
{
|
|
ERROR_LOG(m_logger) << "Couldn't parse FireReport";
|
|
return -1;
|
|
}
|
|
|
|
N_Types::Radian64 l_Azimuth;
|
|
l_Azimuth.setDegree(l_Report.m_Heading / 10.0);
|
|
|
|
N_Types::Radian64 l_Elevation;
|
|
l_Elevation.setDegree(l_Report.m_Pitch / 10.0);
|
|
|
|
bool l_ConsiderGeoMagneticField = false;
|
|
Float64 l_GeoMagneticCorrection = 0.0;
|
|
|
|
if (l_Report.m_DeviceID == PAN_GP_REPORT_MORTAR ||
|
|
l_Report.m_DeviceID == PAN_GP_REQUEST_IGLA ||
|
|
l_Report.m_DeviceID == PAN_GP_REPORT_IGLA)
|
|
l_ConsiderGeoMagneticField = true;
|
|
|
|
if (l_ConsiderGeoMagneticField)
|
|
{
|
|
auto l_config = N_ConfigSystem::GeoConfig::getInstance();
|
|
|
|
QString geoMagneticFieldStr = QStringLiteral("GeoMagneticField");
|
|
QString dateStr = QStringLiteral("Date");
|
|
QString declinationStr = QStringLiteral("Declination");
|
|
QString changePerYearStr = QStringLiteral("ChangePerYear");
|
|
|
|
QString l_DateRecord = l_config->getString(geoMagneticFieldStr, dateStr, QString());
|
|
Float64 l_Declination = l_config->getFloat(geoMagneticFieldStr, declinationStr, 0.0);
|
|
Float64 l_ChangePerYear = l_config->getFloat(geoMagneticFieldStr, changePerYearStr, 0.0);
|
|
|
|
int l_Year = 0, l_Month = 0, l_Day = 0;
|
|
//int l_Result = sscanf(l_DateRecord.c_str(), "%d-%d-%d", &l_Year, &l_Month, &l_Day);
|
|
|
|
QDate date = QDate::fromString(l_DateRecord, QStringLiteral("yyyy-MM-dd"));
|
|
l_Year = date.year();
|
|
l_Month = date.month();
|
|
l_Day = date.day();
|
|
|
|
if (date.isValid())
|
|
{
|
|
INFO_LOG(m_logger) << "GeoMagneticField DateRecord = " << l_DateRecord;
|
|
INFO_LOG(m_logger) << "GeoMagneticField Declination = " << l_Declination;
|
|
INFO_LOG(m_logger) << "GeoMagneticField ChangePerYear = " << l_ChangePerYear;
|
|
|
|
time_t l_TimeNow = time(nullptr);
|
|
struct tm l_TimeInfo;
|
|
memset(&l_TimeInfo, 0, sizeof(tm));
|
|
l_TimeInfo.tm_year = l_Year - 1900;
|
|
l_TimeInfo.tm_mon = l_Month - 1;
|
|
l_TimeInfo.tm_mday = l_Day;
|
|
time_t l_TimeRec = mktime(&l_TimeInfo);
|
|
|
|
double l_TimeDiff = difftime(l_TimeNow, l_TimeRec);
|
|
|
|
int l_SecondsPerYear = 365 * 24 * 3600;
|
|
int l_Years = l_TimeDiff / l_SecondsPerYear; // number of years since DateRecords
|
|
DEBUG_LOG(m_logger) << "GeoMagneticField Years since DateRecord : " << l_Years;
|
|
|
|
l_GeoMagneticCorrection = l_Declination + (l_Years * l_ChangePerYear);
|
|
INFO_LOG(m_logger) << "GeoMagneticField Azimuth correction [degrees]: " << l_GeoMagneticCorrection;
|
|
l_GeoMagneticCorrection *= N_SST::PI_OVER_180;
|
|
l_Azimuth += l_GeoMagneticCorrection;
|
|
|
|
}
|
|
}
|
|
|
|
INFO_LOG(m_logger) << "createSensorData";
|
|
|
|
if (l_Report.m_DeviceID == PAN_GP_REPORT_MORTAR)
|
|
{
|
|
int l_AmmunitionField = PAN_AMMU_INDEX(l_Report.m_Ammunition);
|
|
INFO_LOG(m_logger) << "createSensorData PAN_GP_REPORT_MORTAR l_AmmunitionField=" << +l_AmmunitionField
|
|
<< " l_Report.m_Ammunition=" << +l_Report.m_Ammunition
|
|
<< " l_Report.m_Ammunition & 0x03=" << +(l_Report.m_Ammunition & 0x03);
|
|
switch (l_Report.m_Ammunition & 0x03)
|
|
{
|
|
case 1:
|
|
a_sensorData->AmmunitionType = MortarType1;
|
|
INFO_LOG(m_logger) << "Using Mortar Ammunition 1";
|
|
break;
|
|
case 2:
|
|
a_sensorData->AmmunitionType = MortarType2;
|
|
INFO_LOG(m_logger) << "Using Mortar Ammunition 2";
|
|
break;
|
|
default:
|
|
a_sensorData->AmmunitionType = MortarType1;
|
|
ERROR_LOG(m_logger) << "Ammunition value means 'reserved' : "
|
|
<< l_AmmunitionField;
|
|
return -1;
|
|
}
|
|
a_sensorData->IgnitionType = (l_Report.m_Ammunition >> 3) & 0x07;
|
|
INFO_LOG(m_logger) << "a_sensorData->IgnitionType= " << a_sensorData->IgnitionType;
|
|
}
|
|
else if ((l_Report.m_DeviceID == PAN_GP_REPORT_G5_G6) || (l_Report.m_DeviceID == PAN_GP_REPORT_M109))
|
|
{
|
|
int l_AmmunitionField = PAN_AMMU_INDEX_G5_G6(l_Report.m_Ammunition);
|
|
switch (l_AmmunitionField)
|
|
{
|
|
case 1:
|
|
a_sensorData->AmmunitionType = HE_M1A2;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypePercussion;
|
|
INFO_LOG(m_logger) << "Using HE_M1A2 Percussion";
|
|
break;
|
|
case 2:
|
|
a_sensorData->AmmunitionType = HE_M1A2;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypeProximity;
|
|
INFO_LOG(m_logger) << "Using HE_M1A2 Proximity";
|
|
break;
|
|
case 3:
|
|
a_sensorData->AmmunitionType = VLAP_M9703;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypePercussion;
|
|
INFO_LOG(m_logger) << "Using VLAP_M9703 Percussion";
|
|
break;
|
|
case 4:
|
|
a_sensorData->AmmunitionType = VLAP_M9703;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypeProximity;
|
|
INFO_LOG(m_logger) << "Using VLAP_M9703 Proximity";
|
|
break;
|
|
case 5:
|
|
a_sensorData->AmmunitionType = SMK_BE_M2;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypeTime;
|
|
INFO_LOG(m_logger) << "Using SMK_BE_M2 Time";
|
|
break;
|
|
case 6:
|
|
a_sensorData->AmmunitionType = CLUSTER_HEHC_M9609;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypeTime;
|
|
INFO_LOG(m_logger) << "Using CLUSTER_HEHC_M9609 Time";
|
|
break;
|
|
default:
|
|
a_sensorData->AmmunitionType = HE_M1A2;
|
|
a_sensorData->IgnitionTypeG5G6 = IgnitionTypePercussion;
|
|
isDefault = 1;
|
|
ERROR_LOG(m_logger) << "Ammunition value means 'reserved' : " << l_AmmunitionField;
|
|
return -1;
|
|
}
|
|
}
|
|
else if (l_Report.m_DeviceID == PAN_GP_REPORT_MK19)
|
|
{
|
|
|
|
N_Types::Radian64 l_AzimuthMin;
|
|
l_AzimuthMin.setDegree(l_Report.m_HeadingMin / 10.0);
|
|
N_Types::Radian64 l_AzimuthMax;
|
|
l_AzimuthMax.setDegree(l_Report.m_HeadingMax / 10.0);
|
|
|
|
l_AzimuthMin += l_GeoMagneticCorrection;
|
|
l_AzimuthMax += l_GeoMagneticCorrection;
|
|
|
|
N_Types::Radian64 l_ElevationMin;
|
|
l_ElevationMin.setDegree(l_Report.m_PitchMin / 10.0);
|
|
N_Types::Radian64 l_ElevationMax;
|
|
l_ElevationMax.setDegree(l_Report.m_PitchMax / 10.0);
|
|
|
|
// we have to negate the sensor heading for SST coordinate system which is ccw
|
|
a_sensorData->WeaponAzimuthMin = - l_AzimuthMin;
|
|
a_sensorData->WeaponAzimuthMax = - l_AzimuthMax;
|
|
|
|
if (a_sensorData->WeaponAzimuthMin > a_sensorData->WeaponAzimuthMax)
|
|
{
|
|
a_sensorData->WeaponAzimuthMin = - l_AzimuthMax;
|
|
a_sensorData->WeaponAzimuthMax = - l_AzimuthMin;
|
|
}
|
|
|
|
a_sensorData->WeaponElevationMin = l_ElevationMin;
|
|
a_sensorData->WeaponElevationMax = l_ElevationMax;
|
|
|
|
if (a_sensorData->WeaponElevationMin > a_sensorData->WeaponElevationMax)
|
|
{
|
|
a_sensorData->WeaponElevationMin = l_ElevationMax;
|
|
a_sensorData->WeaponElevationMax = l_ElevationMin;
|
|
}
|
|
|
|
int l_AmmunitionField = PAN_AMMU_INDEX(l_Report.m_Ammunition);
|
|
switch (l_AmmunitionField)
|
|
{
|
|
case 1:
|
|
a_sensorData->AmmunitionType = MK19Type_m385;
|
|
INFO_LOG(m_logger) << "Using MK19 Ammunition 1 (m385)";
|
|
break;
|
|
case 2:
|
|
a_sensorData->AmmunitionType = MK19Type_m430;
|
|
INFO_LOG(m_logger) << "Using MK19 Ammunition 2 (m430)";
|
|
break;
|
|
default:
|
|
a_sensorData->AmmunitionType = MK19Type_m385;
|
|
ERROR_LOG(m_logger) << "Ammunition value means 'reserved' : " << l_AmmunitionField;
|
|
return -1;
|
|
}
|
|
}
|
|
else if (l_Report.m_DeviceID == PAN_GP_REPORT_IGLA ||
|
|
l_Report.m_DeviceID == PAN_GP_REQUEST_IGLA)
|
|
{
|
|
a_sensorData->AmmunitionType = AmmunitionTypeUnknown; // irrelevant
|
|
INFO_LOG(m_logger) << "Using IGLA Ammunition 1";
|
|
}
|
|
else
|
|
{
|
|
ERROR_LOG(m_logger) << "Couldn't create sensorData for DeviceID : " << l_Report.m_DeviceID;
|
|
return -1;
|
|
}
|
|
|
|
a_sensorData->UseMinMax = l_Report.m_UseMinMax;
|
|
a_sensorData->WeaponAzimuth = - l_Azimuth; // we have to negate the sensor heading for SST coordinate system which is ccw
|
|
a_sensorData->WeaponElevation = l_Elevation;
|
|
a_sensorData->WeaponTemperature = l_Report.m_Temperature;
|
|
|
|
INFO_LOG(m_logger) << "a_sensorData->ChargeType=" << a_sensorData->ChargeType
|
|
<< " l_Report.m_Ammunition=" << l_Report.m_Ammunition
|
|
<< " l_Report.m_DeviceID=" << l_Report.m_DeviceID;
|
|
|
|
if (l_Report.m_DeviceID == PAN_GP_REPORT_MORTAR)
|
|
{
|
|
|
|
switch (l_Report.m_Ammunition)
|
|
{
|
|
case MortarType1:
|
|
default:
|
|
a_sensorData->ChargeType = N_Ballistic::MOERSER;
|
|
break;
|
|
case MortarType2:
|
|
a_sensorData->ChargeType = N_Ballistic::NEBEL;
|
|
break;
|
|
}
|
|
|
|
|
|
// if ( l_Report.m_Ammunition == 2)
|
|
// a_sensorData->ChargeType = 382;
|
|
// else
|
|
// a_sensorData->ChargeType = 385;
|
|
INFO_LOG(m_logger) << "PAN_GP_REPORT_MORTAR: l_Report.m_Ammunition" << l_Report.m_Ammunition
|
|
<< " a_sensorData->ChargeType=" << a_sensorData->ChargeType;
|
|
}
|
|
else if ((l_Report.m_DeviceID != PAN_GP_REPORT_G5_G6) && (l_Report.m_DeviceID != PAN_GP_REPORT_M109))
|
|
{
|
|
a_sensorData->ChargeType = PAN_AMMU_CHARGE(l_Report.m_Ammunition);
|
|
if (PAN_AMMU_TIMEPROG(l_Report.m_Ammunition))
|
|
a_sensorData->IgnitionType = l_Report.m_TimeToDetonate;
|
|
else
|
|
a_sensorData->IgnitionType = 0;
|
|
INFO_LOG(m_logger) << "if a_sensorData->ChargeType=" << a_sensorData->ChargeType;
|
|
}
|
|
else
|
|
{
|
|
INFO_LOG(m_logger) << "else a_sensorData->ChargeType=" << a_sensorData->ChargeType;
|
|
|
|
SInt32 ch = PAN_AMMU_CHARGE_G5_G6(l_Report.m_Ammunition);
|
|
|
|
if (ch == 1) // M64 Base
|
|
{
|
|
a_sensorData->ChargeType = 0;
|
|
INFO_LOG(m_logger) << "Using Charge M64 Base";
|
|
}
|
|
else if (ch == 3) // 3 x M64 Incr
|
|
{
|
|
a_sensorData->ChargeType = 1;
|
|
INFO_LOG(m_logger) << "Using Charge M64 INCR x 3";
|
|
}
|
|
else if (ch == 4) // 4 x M64 Incr
|
|
{
|
|
a_sensorData->ChargeType = 2;
|
|
INFO_LOG(m_logger) << "Using Charge M64 INCR x 4";
|
|
}
|
|
else if (ch == 5) // 5 x M64 Incr
|
|
{
|
|
a_sensorData->ChargeType = 3;
|
|
INFO_LOG(m_logger) << "Using Charge M64 INCR x 5";
|
|
}
|
|
else if (ch == 6) // 5 x M64 Incr + Base Bleed
|
|
{
|
|
a_sensorData->ChargeType = 4;
|
|
INFO_LOG(m_logger) << "Using Charge M64 INCR x 5 + Base Bleed";
|
|
}
|
|
else
|
|
{
|
|
a_sensorData->ChargeType = 0;
|
|
isDefault = 1;
|
|
WARNING_LOG(m_logger) << "Unknown Charge Type. Using Charge M64 Base";
|
|
}
|
|
}
|
|
INFO_LOG(m_logger) << "a_sensorData->ChargeType=" << a_sensorData->ChargeType;
|
|
return l_Report.m_DeviceID;
|
|
}
|
|
|
|
|
|
// ---------------------------------------------------------------
|
|
// Sets global meteo conditions
|
|
// ---------------------------------------------------------------
|
|
void GeoPairingImpl::setMeteoData(const GeoPairingImplMeteoData &a_meteoData)
|
|
{
|
|
if (!m_Initialized)
|
|
{
|
|
ERROR_LOG(m_logger) << "Implementation not initialized";
|
|
return;
|
|
}
|
|
m_MeteoData = a_meteoData;
|
|
|
|
INFO_LOG(m_logger) << "---------- Setting Meteo Data ------------";
|
|
INFO_LOG(m_logger) << "AirPressure : " << m_MeteoData.AirPressure;
|
|
INFO_LOG(m_logger) << "Temperature : " << m_MeteoData.Temperature;
|
|
INFO_LOG(m_logger) << "WindDirection : " << m_MeteoData.WindDirection;
|
|
INFO_LOG(m_logger) << "WindSpeed : " << m_MeteoData.WindSpeed;
|
|
}
|
|
|
|
} // namespace N_GeoPairingImpl
|