Hi @SivaRamaKrishnaNV,
Thank for your reply.
Here is my code.
dwStatus _dwSensorRadarPlugin_getDecoderConstants(_dwSensorRadarDecoder_constants *constants, dwSensorPluginSensorHandle_t sensor)
{
auto sensorContext = reinterpret_castdw::plugins::radar::ContinentalARS408*(sensor);
if (!checkValid(sensorContext))
{
return DW_INVALID_HANDLE;
}
// constants->dynamicsSizeInBytes
constants->maxPacketsPerScan = 401;
constants->maxPayloadSize = sizeof(QuantaAD::ADCANMessage);
// constants->maxPointsPerPacket[DW_RADAR_RETURN_TYPE_TRACK][DW_RADAR_RANGE_LONG] = 100;
// constants->packetsPerScan[DW_RADAR_RETURN_TYPE_TRACK][DW_RADAR8_RANGE_LONG] = 401;
// constants->properties.inputPacketsPerSecond =
constants->properties.isDecodingOn = 1;
constants->properties.maxReturnsPerScan[DW_RADAR_RETURN_TYPE_TRACK][DW_RADAR_RANGE_LONG] = 100;
constants->properties.numScanTypes = 1;
// constants->packetsPerScan = 401; // will be deprecated soon
constants->properties.scansPerSecond = 17;
constants->properties.supportedScanTypes[DW_RADAR_RETURN_TYPE_TRACK][DW_RADAR_RANGE_LONG] = 1;
return DW_SUCCESS;
}
dwStatus ContinentalARS408::parseData(dwRadarScan *output, const dwRadarScanType scanType)
{
processRadarScan(output);
// output->dopplerAmbiguity = 0.0;
output->numReturns = m_far.size();
output->scanIndex = 1;
output->scanType = scanType;
return DW_SUCCESS;
}
void ContinentalARS408::processRadarScan(dwRadarScan *output)
{
dwRadarTrack object;
if (m_msg->id == 1546){
m_far.clear();
m_nof_objects = m_msg->data[0];
dwContext_getCurrentTime(&output->hostTimestamp, m_ctx);
}
if(m_msg->id == 1547){
// std::cout<<std::hex <<"msg.id:"<<m_msg->id<<std::dec<<" msg.size:"<<m_msg->size<<std::endl;
object.id = m_msg->data[0];
Byte b1;
Byte b2;
Byte b3;
Byte b4;
Byte b5;
Byte b6;
b1.byte=m_msg->data[1];
b2.byte=m_msg->data[2];
b3.byte=m_msg->data[3];
b4.byte=m_msg->data[4];
b5.byte=m_msg->data[5];
b6.byte=m_msg->data[6];
object.x = OBJECT_DIST_LONG_MIN+OBJECT_DIST_RES*(pow(2,0)*b2.bit4+pow(2,1)*b2.bit5+pow(2,2)*b2.bit6+pow(2,3)*b2.bit7+pow(2,4)*b2.bit8+pow(2,5)*b1.bit1+pow(2,6)*b1.bit2+pow(2,7)*b1.bit3+pow(2,8)*b1.bit4+pow(2,9)*b1.bit5+pow(2,10)*b1.bit6+pow(2,11)*b1.bit7+pow(2,12)*b1.bit8);
object.y = OBJECT_DIST_LAT_MIN+OBJECT_DIST_RES*(pow(2,0)*b3.bit1+pow(2,1)*b3.bit2+pow(2,2)*b3.bit3+pow(2,3)*b3.bit4+pow(2,4)*b3.bit5+pow(2,5)*b3.bit6+pow(2,6)*b3.bit7+pow(2,7)*b3.bit8+pow(2,8)*b2.bit1+pow(2,9)*b2.bit2+pow(2,10)*b2.bit3);
object.Vx = OBJECT_VREL_LONG_MIN+OBJECT_VREL_RES*(pow(2,0)*b5.bit7+pow(2,1)*b5.bit8+pow(2,2)*b4.bit1+pow(2,3)*b4.bit2+pow(2,4)*b4.bit3+pow(2,5)*b4.bit4+pow(2,6)*b4.bit5+pow(2,7)*b4.bit6+pow(2,8)*b4.bit7+pow(2,9)*b4.bit8);
object.Vy = OBJECT_VREL_LAT_MIN+OBJECT_VREL_RES*(pow(2,0)*b6.bit6+pow(2,1)*b6.bit7+pow(2,2)*b6.bit8+pow(2,3)*b5.bit1+pow(2,4)*b5.bit2+pow(2,5)*b5.bit3+pow(2,6)*b5.bit4+pow(2,7)*b5.bit5+pow(2,8)*b5.bit6);
object.rcs = OBJECT_RCS_MIN+OBJECT_RCS_RES*m_msg->data[7];
obj[object.id].longitude_dist = object.x;
obj[object.id].lateral_dist = object.y;
obj[object.id].longitude_vel = object.Vx;
obj[object.id].lateral_vel = object.Vy;
obj[object.id].rcs = object.rcs;
}
if(m_msg->id == 1549){
object = dwRadarTrack();
object.id = m_msg->data[0];
Byte b4;
Byte b5;
b4.byte = m_msg->data[4];
b5.byte = m_msg->data[5];
object.azimuthRate = OBJECT_ORIENTATION_ANGEL_MIN+OBJECT_ORIENTATION_ANGEL_RES*(pow(2,0)*b5.bit7+pow(2,1)*b5.bit8+pow(2,2)*b4.bit1+pow(2,3)*b4.bit2+pow(2,4)*b4.bit3+pow(2,5)*b4.bit4+pow(2,6)*b4.bit5+pow(2,7)*b4.bit6+pow(2,8)*b4.bit7+pow(2,9)*b4.bit8);
object.x = obj[object.id].longitude_dist;
object.y = obj[object.id].lateral_dist;
object.Vx = obj[object.id].longitude_vel;
object.Vy = obj[object.id].lateral_vel;
object.rcs = obj[object.id].rcs;
m_far.push_back(object); // m_far is a vector to store the detected objects
}
output->data = reinterpret_cast<void *>(m_far.data());
}
output log:
_dwSensorPlugin_start
ContinentalARS408::startSensor()
_dwSensorPlugin_readRawData
ContinentalARS408::readRawData()
msg.id:60a msg.size:4
data: 26b1d10
_dwSensorRadarPlugin_validatePacket
ContinentalARS408::validatePacket()
_dwSensorPlugin_pushData
ContinentalARS408::pushData()
_dwSensorRadarPlugin_parseDataBuffer
ContinentalARS408::parseData()
msg.id:60a msg.size:4
Number of objects : 2
numReturns : 0
_dwSensorPlugin_returnRawData
ContinentalARS408::returnRawData()
_dwSensorPlugin_readRawData
ContinentalARS408::readRawData()
msg.id:60b msg.size:8
data: 154e8bf3802019e
_dwSensorRadarPlugin_validatePacket
ContinentalARS408::validatePacket()
_dwSensorPlugin_pushData
ContinentalARS408::pushData()
_dwSensorRadarPlugin_parseDataBuffer
ContinentalARS408::parseData()
msg.id:60b msg.size:8
numReturns : 0
_dwSensorPlugin_returnRawData
ContinentalARS408::returnRawData()
