//------------------------------------------------------------------------------
// process() -- Process phase
//------------------------------------------------------------------------------
void IrSeeker::process(const LCreal dt)
{
BaseClass::process(dt);
// ---
// Update IR query queues: from 'in-use' to 'free'
// ---
lcLock(inUseQueryLock);
int n = inUseQueryQueue.entries();
for (int i = 0; i < n; i++) {
IrQueryMsg* query = inUseQueryQueue.get();
if (query != 0) {
if (query->getRefCount() <= 1) {
// No one else is referencing the query, push on free stack
query->clear();
lcLock(freeQueryLock);
if (freeQueryStack.isNotFull()) {
freeQueryStack.push(query);
}
else {
query->unref();
}
lcUnlock(freeQueryLock);
}
else {
// Others are still referencing the query, put back on in-use queue
inUseQueryQueue.put(query);
}
}
}
lcUnlock(inUseQueryLock);
}
void IrSensor::process(const double dt)
{
BaseClass::process(dt);
int numRecords = storedMessagesQueue.entries();
if (numRecords > 0) {
AngleOnlyTrackManager* tm = static_cast<AngleOnlyTrackManager*>(getTrackManager());
if (tm != nullptr) {
base::lock(storedMessagesLock);
numRecords = storedMessagesQueue.entries();
// Send on all messages EXCEPT those with signal below threshold and those merged
// into another signal. Those will simply be ignored and unreferenced.
for (int i=0; i < numRecords; i++) {
IrQueryMsg* msg = storedMessagesQueue.get();
if (msg->getQueryMergeStatus() != IrQueryMsg::MERGED_OUT) {
if (msg->getSignalToNoiseRatio() > getThreshold())
tm->newReport(msg, msg->getSignalToNoiseRatio());
}
msg->unref();
}
base::unlock(storedMessagesLock);
}
}
}
//------------------------------------------------------------------------------
// clearTracksAndQueues() -- clear out tracks and queues
//------------------------------------------------------------------------------
void IrSensor::clearTracksAndQueues()
{
// ---
// Clear out the queues
// ---
base::lock(storedMessagesLock);
for (IrQueryMsg* msg = storedMessagesQueue.get(); msg != nullptr; msg = storedMessagesQueue.get()) {
msg->unref();
}
base::unlock(storedMessagesLock);
}
//------------------------------------------------------------------------------
// clearQueues() -- clear out all queues
//------------------------------------------------------------------------------
void IrSeeker::clearQueues()
{
lcLock(freeQueryLock);
IrQueryMsg* query = freeQueryStack.pop();
while (query != 0) {
query->unref();
query = freeQueryStack.pop();
}
lcUnlock(freeQueryLock);
lcLock(inUseQueryLock);
query = inUseQueryQueue.get();
while (query != 0) {
query->unref();
query = inUseQueryQueue.get();
}
lcUnlock(inUseQueryLock);
}
void MergingIrSensor::mergeIrReturns()
{
int numRecords = storedMessagesQueue.entries();
if (numRecords > 0) {
//int* deleteArray = new int [numRecords];
//if (deleteArray == 0) {
// if (isMessageEnabled(MSG_ERROR)) {
// std::cerr << "Error: Allocation memory failure in IrSensor::mergeIrReturns" << std::endl;
// }
//}
//else {
//for (int i=0; i < numRecords; i++) {
// deleteArray[i] = 0;
//}
base::lock(storedMessagesLock);
// Traverse the stored message queue using peek (). Examine every
// message. Compare every stored message against every OTHER stored
// message. If the two are too close together, merge the two signals
// and mark the second message in the delete array.
// Proceed through the loop, ignoring all messages marked "deleted"
// in the delete array.
numRecords = storedMessagesQueue.entries();
if (isMessageEnabled(MSG_DEBUG)) {
std::cout << "IrSensor: numRecords returned " << numRecords << std::endl;
}
for (int i=0; i < numRecords; i++) {
//if (deleteArray[i] == 0) { // Do not bother processing those marked
// for deletion -- these have already been
// merged and must be ignored.
IrQueryMsg* currentMsg = storedMessagesQueue.peek0(i);
// Do not bother processing those marked
// for deletion -- these have already been
// merged and must be ignored.
if (currentMsg->getQueryMergeStatus()!= IrQueryMsg::MERGED_OUT) {
for (int j = i+1; j < numRecords; j++) {
IrQueryMsg* nextMsg = storedMessagesQueue.peek0(j);
double azimuthDelta = currentMsg->getRelativeAzimuth() - nextMsg->getRelativeAzimuth();
double elevationDelta = currentMsg->getRelativeElevation()
- nextMsg->getRelativeElevation();
if (azimuthDelta < 0)
azimuthDelta = -azimuthDelta;
if (elevationDelta < 0)
elevationDelta = -elevationDelta;
if ((azimuthDelta < azimuthBin) &&
(elevationDelta < elevationBin)) { // two signals are too close together
// for the sensor to distinguish between them;
// we will merge the two signals based
// on their weighted signal-to-noise.
double currentRatio = 0.0;
double nextRatio = 0.0;
// find current ratio.
if (isMessageEnabled(MSG_DEBUG)) {
std::cout << "IrSensor: merging target " << nextMsg->getTarget()->getName()->getString()
<< " into target " <<currentMsg->getTarget()->getName()->getString() << std::endl;
}
if (currentMsg->getSignalToNoiseRatio() >
currentMsg->getBackgroundNoiseRatio()) {
currentRatio = currentMsg->getSignalToNoiseRatio() +
currentMsg->getBackgroundNoiseRatio();
} else {
if (currentMsg->getSignalToNoiseRatio() < 0) {
currentRatio = -currentMsg->getSignalToNoiseRatio() -
currentMsg->getBackgroundNoiseRatio();
} else {
currentRatio = -currentMsg->getSignalToNoiseRatio() -
currentMsg->getBackgroundNoiseRatio();
} // signaltonoise < 0
} // if current signal > background
//now do the same thing for the next message.
if (nextMsg->getSignalToNoiseRatio() >
nextMsg->getBackgroundNoiseRatio()) {
nextRatio = nextMsg->getSignalToNoiseRatio() +
nextMsg->getBackgroundNoiseRatio();
} else {
if (nextMsg->getSignalToNoiseRatio() < 0) {
nextRatio = -nextMsg->getSignalToNoiseRatio() -
nextMsg->getBackgroundNoiseRatio();
} else {
nextRatio = -nextMsg->getSignalToNoiseRatio() -
nextMsg->getBackgroundNoiseRatio();
} // signaltonoise < 0
} // if next signal > background
// use ratios to find weights.
double sumRatio = currentRatio + nextRatio;
const double currentWeight = currentRatio / sumRatio;
const double nextWeight = 1.0 - currentWeight;
//combine line-of-sight vector using weights
currentMsg->setLosVec((currentMsg->getLosVec() * currentWeight) +
(nextMsg->getLosVec() * nextWeight));
// combine position
currentMsg->setPosVec((currentMsg->getPosVec() * currentWeight) +
(nextMsg->getPosVec() * nextWeight));
// combine velocity
currentMsg->setVelocityVec((currentMsg->getVelocityVec() * currentWeight) +
(nextMsg->getVelocityVec() * nextWeight));
// combine acceleration
currentMsg->setAccelVec((currentMsg->getAccelVec() * currentWeight) +
(nextMsg->getAccelVec() * nextWeight));
// combine signal to noise ratios.
sumRatio = sumRatio - currentMsg->getBackgroundNoiseRatio();
if (sumRatio < 0)
sumRatio = -sumRatio;
currentMsg->setSignalToNoiseRatio(sumRatio);
//combine Azimuth and Elevation.
currentMsg->setAzimuthAoi((currentMsg->getAzimuthAoi() * currentWeight) +
nextMsg->getAzimuthAoi() * nextWeight);
currentMsg->setElevationAoi((currentMsg->getElevationAoi()* currentWeight) +
(nextMsg->getElevationAoi() * nextWeight));
currentMsg->setAngleAspect((currentMsg->getAngleAspect() * currentWeight) +
(nextMsg->getAngleAspect() * nextWeight));
currentMsg->setRelativeAzimuth((currentMsg->getRelativeAzimuth() * currentWeight) +
(nextMsg->getRelativeAzimuth() * nextWeight));
currentMsg->setRelativeElevation((currentMsg->getRelativeElevation() * currentWeight) +
(nextMsg->getRelativeElevation() * nextWeight));
// signal that this report has merged targets
currentMsg->setQueryMergeStatus(IrQueryMsg::MERGED);
nextMsg->setQueryMergeStatus(IrQueryMsg::MERGED_OUT);
//deleteArray[j] = 1; // now that we have merged this signal with the
// Ith signal, it must be deleted. It will not
// be passed on to the track manager.
//if (isMessageEnabled(MSG_INFO)) {
//std::cout << "IrSensor: End Merge" << std::endl;
//}
} // if we merge
} // end for j = i + 1;
} // End if delete Array
else { // debug - this target ws merged into another
int x=0;
x=x+1;
}
} // end for i = 0;
base::unlock(storedMessagesLock);
//delete[] deleteArray;
//} // newArray is not null.
} // numRecords > 0
}
//------------------------------------------------------------------------------
// irRequestSignature() -- Send an IR query packet at all active players to request an IR signature
//------------------------------------------------------------------------------
void IrSeeker::irRequestSignature(IrQueryMsg* const irQuery)
{
// Need something to store the required data for the IR signatures and someone to send to
Tdb* tdb0 = getCurrentTDB();
Player* ownship = getOwnship();
if (irQuery == 0 || tdb0 == 0 || ownship == 0) {
// Clean up and leave
if (tdb0 != 0) tdb0->unref();
return;
}
// ---
// Compute gimbal boresight data for our targets
// ---
// FAB - cannot use ownHdgOnly
unsigned int ntgts = tdb0->computeBoresightData();
if (ntgts > MAX_PLAYERS) ntgts = MAX_PLAYERS;
// ---
// If we have targets
// ---
const osg::Vec3d* losG = tdb0->getGimbalLosVectors();
if (ntgts > 0 && losG != 0) {
// Fetch the required data arrays from the TargetDataBlock
const double* ranges = tdb0->getTargetRanges();
const double* rngRates = tdb0->getTargetRangeRates();
const double* anglesOffBoresight = tdb0->getBoresightErrorAngles();
const osg::Vec3d* losO2T = tdb0->getLosVectors();
const osg::Vec3d* losT2O = tdb0->getTargetLosVectors();
Player** targets = tdb0->getTargets();
LCreal maximumRange = irQuery->getMaxRangeNM()*Basic::Distance::NM2M;
// ---
// Send query packets to the targets
// ---
for (unsigned int i = 0; i < ntgts; i++) {
// filter on sensor max range
// can't filter on sensor range in processPlayers - different sensors can have different max range
if (maximumRange > 0.0 && ranges[i] > maximumRange)
continue;
// Get a free query packet
lcLock(freeQueryLock);
IrQueryMsg* query = freeQueryStack.pop();
lcUnlock(freeQueryLock);
if (query == 0) {
query = new IrQueryMsg();
//if (ownship->getID() != 1) {
// static tcnt = 0;
// tcnt++;
//if (isMessageEnabled(MSG_INFO)) {
// std::cout << "new IrQueryMsg(" << this << "): " << tcnt << ", inused: " << inUseEmQueue.entries() << ", em = " << em << std::endl;
//}
//}
}
// Send the IR query message to the other player
if (query != 0) {
// a) Copy the template query msg
*query = *irQuery;
// b) Set target unique data
query->setGimbal(this);
query->setOwnship(ownship);
query->setRange( LCreal(ranges[i]) );
query->setLosVec( losO2T[i] );
query->setTgtLosVec( losT2O[i] );
query->setRangeRate( LCreal(rngRates[i]) );
query->setTarget(targets[i]);
query->setAngleOffBoresight( LCreal(anglesOffBoresight[i]) );
query->setGimbalAzimuth( LCreal(getAzimuth()) );
query->setGimbalElevation( LCreal(getElevation()) );
// c) Send the query to the target
targets[i]->event(IR_QUERY, query);
// d) Dispose of the query
if (query->getRefCount() <= 1) {
// Recycle the query packet
query->clear();
lcLock(freeQueryLock);
if (freeQueryStack.isNotFull()) {
freeQueryStack.push(query);
}
else {
query->unref();
}
lcUnlock(freeQueryLock);
}
else {
// Store for future reference
lcLock(inUseQueryLock);
if (inUseQueryQueue.isNotFull()) {
inUseQueryQueue.put(query);
}
else {
// Just forget it
query->unref();
}
lcUnlock(inUseQueryLock);
}
}
else {
// When we couldn't get a free query packet
if (isMessageEnabled(MSG_WARNING)) {
std::cerr << "Iw Seeker: OUT OF Query messages!" << std::endl;
}
}
}
}
// Unref() the TDB
tdb0->unref();
}
