int uwb(int destNodeId)
{
// Determine range to a radio. May also get data and scan packets.
if (rcmRangeTo(destNodeId, RCM_ANTENNAMODE_TXA_RXA, 0, NULL,
&rangeInfo, &dataInfo, &scanInfo, &fullScanInfo) == 0)
{
// we always get a range info packet
// printf("RANGE_INFO: responder %d, msg ID %u, range status %d, "
// "stopwatch %d ms, channelRiseTime %d, vPeak %d, measurement type %d\n",
// rangeInfo.responderId, rangeInfo.msgId, rangeInfo.rangeStatus,
// rangeInfo.stopwatchTime, rangeInfo.channelRiseTime, rangeInfo.vPeak,
// rangeInfo.rangeMeasurementType);
// The RANGE_INFO can provide multiple types of ranges
if (rangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_PRECISION)
{
// printf("Precision range: %d mm, error estimate %d mm\n",
// rangeInfo.precisionRangeMm, rangeInfo.precisionRangeErrEst);
}
if (rangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_COARSE)
{
// printf("Coarse range: %d mm, error estimate %d mm\n",
// rangeInfo.coarseRangeMm, rangeInfo.coarseRangeErrEst);
}
if (rangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_FILTERED)
{
// printf("Filtered range: %d mm, error estimate %d mm\n",
// rangeInfo.filteredRangeMm, rangeInfo.filteredRangeErrEst);
// printf("Filtered velocity: %d mm/s, error estimate %d mm/s\n",
// rangeInfo.filteredRangeVel, rangeInfo.filteredRangeVelErrEst);
}
// only get a data info packet if the responder sent data
// dataSize will be non-zero if we there is data
if (dataInfo.dataSize)
printf("DATA_INFO from node %d: msg ID %u, channelRiseTime %d, vPeak %d, %d bytes\ndata: %*s\n",
dataInfo.sourceId, dataInfo.msgId, dataInfo.channelRiseTime, dataInfo.vPeak,
dataInfo.dataSize, dataInfo.dataSize, dataInfo.data);
// only get a scan info packet if the SEND_SCAN bit is set in the config
// numSamples will be non-zero if there is scan data
// we don't do anything with the scan data itself here
if (scanInfo.numSamples)
printf("SCAN_INFO from node %d: msg ID %u, %d samples, channelRiseTime %d, vPeak %d\n",
scanInfo.sourceId, scanInfo.msgId, scanInfo.numSamples,
scanInfo.channelRiseTime, scanInfo.vPeak);
// only get a full scan info packet if the FULL_SCAN bit is set in the config
// numSamplesInMessage will be non-zero if there is scan data
// we don't do anything with the scan data itself here
if (fullScanInfo.numSamplesInMessage)
printf("FULL_SCAN_INFO from node %d: msg ID %u, %d samples, channelRiseTime %d, vPeak %d\n",
fullScanInfo.sourceId, fullScanInfo.msgId, fullScanInfo.numSamplesInMessage,
fullScanInfo.channelRiseTime, fullScanInfo.vPeak);
// a rangeStatus of 0 means the range was calculated successfully
// if (rangeInfo.rangeStatus == 0)
// {
// // now broadcast the range in a data packet
// sprintf(str, "The range from %d to %d is %d mm.",
// config.nodeId, destNodeId,
// rangeInfo.precisionRangeMm);
// rcmDataSend(RCM_ANTENNAMODE_TXA_RXA, strlen(str), str);
// }
}
// perform cleanup
//rcmIfClose();
//printf("exit uwb\n");
return rangeInfo.precisionRangeMm;
}
void RefInterface::range()
{
if(radioNum == 0)
{
radioFd = radioCom;
if(antennaNum == 0)
{
buffer.reqNode = "101A";
//range to the controller using radio A
if (rcmRangeTo(destNode, ANTENNAMODE_A, 0, NULL,
&RangeInfo, &dataInfo, &scanInfo, &fullScanInfo) == 0)
{
//This gets the precision range measurement
if (RangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_PRECISION)
{
////qDebug()<<"Precision range: "<< RangeInfo.precisionRangeMm;
r0 = RangeInfo.precisionRangeMm;
}
//if the status comes back as 0, the radio ranged correctly
if (RangeInfo.rangeStatus == 0)
{
// add range to buffer structure
r0 = r0/1000;
buffer.R0 = r0;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
////qDebug()<<"Range Successful";
}
else if(RangeInfo.precisionRangeErrEst > 60)
{
if(!msg.isEmpty())
{
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
thresholdErrorCountR0++;
emit sendThresholdCount(thresholdErrorCountR0, thresholdErrorCountR1, thresholdErrorCountR2, thresholdErrorCountR3);
emit display(buffer.x, buffer.y, buffer.z, RangeInfo.precisionRangeMm, buffer.R1, buffer.R2,
buffer.R3,buffer.roll, buffer.pitch,buffer.yaw, buffer.mError,buffer.status);
r0 = msg.at(0).R0;
buffer.R0 = r0;
}
}
else
{
r0 = msg.at(0).R0;
buffer.R0 = r0;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
errorCountR0++;
emit sendErrorCount(errorCountR0, errorCountR1, errorCountR2, errorCountR3);
}
} //end if
antennaNum = 1;
return;
}
if(antennaNum == 1)
{
buffer.reqNode = "101B";
//range to controller using radio B
if (rcmRangeTo(destNode, ANTENNAMODE_B, 0, NULL,
&RangeInfo, &dataInfo, &scanInfo, &fullScanInfo) == 0)
{
// Get precision range measurement from msg received
if (RangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_PRECISION)
{
////qDebug()<<"Precision range: "<< RangeInfo.precisionRangeMm;
r1 = RangeInfo.precisionRangeMm;
}
//if the status is 0, range was successful
if (RangeInfo.rangeStatus == 0)
{
////qDebug()<<"Range Successful";
//add range to buffer struct
r1 = r1/1000;
buffer.R1 = r1;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
}
else if(RangeInfo.precisionRangeErrEst >60)
{
if(!msg.isEmpty())
{
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
thresholdErrorCountR1++;
emit sendThresholdCount(thresholdErrorCountR0, thresholdErrorCountR1, thresholdErrorCountR2, thresholdErrorCountR3);
emit display(buffer.x, buffer.y, buffer.z, buffer.R0, RangeInfo.precisionRangeMm, buffer.R2,
buffer.R3,buffer.roll, buffer.pitch,buffer.yaw, buffer.mError,buffer.status);
r1 = msg.at(0).R1;
buffer.R1 = r1;
}
}
else
{
r1 = msg.at(0).R1;
buffer.R1 = r1;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
errorCountR1++;
emit sendErrorCount(errorCountR0, errorCountR1, errorCountR2, errorCountR3);
}
} //end if
antennaNum = 0;
radioNum = 1;
return;
}
}
if(radioNum == 1)
{
radioFd = radioCom1;
if(antennaNum == 0)
{
buffer.reqNode = "102A";
if (rcmRangeTo(destNode, ANTENNAMODE_A, 0, NULL,
&RangeInfo, &dataInfo, &scanInfo, &fullScanInfo) == 0)
{
//This gets the precision range measurement
if (RangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_PRECISION)
{
////qDebug()<<"Precision range: "<< RangeInfo.precisionRangeMm;
r2 = RangeInfo.precisionRangeMm;
}
//if the status comes back as 0, the radio ranged correctly
if (RangeInfo.rangeStatus == 0)
{
////qDebug()<<"Range Successful";
// add range to buffer structure
r2 = r2/1000;
buffer.R2 = r2;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
}
else if(RangeInfo.precisionRangeErrEst > 60)
{
if(!msg.isEmpty())
{
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
thresholdErrorCountR3++;
emit sendThresholdCount(thresholdErrorCountR0, thresholdErrorCountR1, thresholdErrorCountR2, thresholdErrorCountR3);
emit display(buffer.x, buffer.y, buffer.z, buffer.R0,buffer.R1 , RangeInfo.precisionRangeMm,
buffer.R3,buffer.roll, buffer.pitch,buffer.yaw, buffer.mError,buffer.status);
r2 = msg.at(0).R2;
buffer.R2 = r2;
}
}
else
{
r2 = msg.at(0).R2;
buffer.R2 = r2;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
errorCountR2++;
emit sendErrorCount(errorCountR0, errorCountR1, errorCountR2, errorCountR3);
}
} //end if
antennaNum = 1;
return;
}
if(antennaNum == 1)
{
buffer.reqNode = "102B";
if (rcmRangeTo(destNode, ANTENNAMODE_B, 0, NULL,
&RangeInfo, &dataInfo, &scanInfo, &fullScanInfo) == 0)
{
// Get precision range measurement from msg received
if (RangeInfo.rangeMeasurementType & RCM_RANGE_TYPE_PRECISION)
{
////qDebug()<<"Precision range: "<< RangeInfo.precisionRangeMm;
r3 = RangeInfo.precisionRangeMm;
}
//if the status is 0, range was successful
if (RangeInfo.rangeStatus == 0)
{
////qDebug()<<"Range Successful"<<" r3"<<r3;
//add range to buffer struct
r3 = r3/1000;
buffer.R3 = r3;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
}
else if(RangeInfo.precisionRangeErrEst > 60)
{
if(!msg.isEmpty())
{
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
thresholdErrorCountR3++;
emit sendThresholdCount(thresholdErrorCountR0, thresholdErrorCountR1, thresholdErrorCountR2, thresholdErrorCountR3);
emit display(buffer.x, buffer.y, buffer.z, buffer.R0,buffer.R1 , buffer.R2,
RangeInfo.precisionRangeMm,buffer.roll, buffer.pitch,buffer.yaw, buffer.mError,buffer.status);
r3 = msg.at(0).R3;
buffer.R3 = r3;
}
}
else
{
r3 = msg.at(0).R3;
buffer.R3 = r3;
buffer.mError = RangeInfo.precisionRangeErrEst;
buffer.status = RangeInfo.rangeStatus;
errorCountR3++;
emit sendErrorCount(errorCountR0, errorCountR1, errorCountR2, errorCountR3);
}
} //end if
antennaNum = 0;
radioNum = 0;
return;
}
}
}
