static void OccupancyDetection3D_handleObjectDetResult
//copy to the format for output, and to future tracker
gMmwMssMCB.pointCloudToUart.header.length = sizeof(mmwLab_output_message_tl)
+ sizeof(mmwLab_output_message_point_unit)
+ sizeof(mmwLab_output_message_UARTpoint) * outputFromDSP->pointCloudOut.object_count;
if ( outputFromDSP->pointCloudOut.object_count == 0)
gMmwMssMCB.pointCloudToUart.header.length = 0;
gMmwMssMCB.pointCloudToUart.header.type = MMWDEMO_OUTPUT_MSG_POINT_CLOUD;
gMmwMssMCB.pointCloudToUart.pointUint.azimuthUnit = 0.01f;
gMmwMssMCB.pointCloudToUart.pointUint.elevationUnit = 0.01f;
gMmwMssMCB.pointCloudToUart.pointUint.rangeUnit = 0.00025f;
gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit = 0.00028f;
gMmwMssMCB.pointCloudToUart.pointUint.snrUint = 0.04f;
gMmwMssMCB.numDetectedPoints = outputFromDSP->pointCloudOut.object_count;
for (pntIdx = 0; pntIdx < (int32_t)outputFromDSP->pointCloudOut.object_count; pntIdx++ )
{
gMmwMssMCB.pointCloudToUart.point[pntIdx].azimuth = (int8_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].azimuthAngle / gMmwMssMCB.pointCloudToUart.pointUint.azimuthUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].elevation = (int8_t)round((outputFromDSP->pointCloudOut.pointCloud[pntIdx].elevAngle)/ gMmwMssMCB.pointCloudToUart.pointUint.elevationUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].range = (uint16_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].range / gMmwMssMCB.pointCloudToUart.pointUint.rangeUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].doppler = (int16_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].velocity / gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].snr = (uint16_t)round((float)outputFromDSP->pointCloudOut.snr[pntIdx].snr * 0.125f / gMmwMssMCB.pointCloudToUart.pointUint.snrUint);
gMmwMssMCB.pointCloudFromDSP[pntIdx].elevAngle = outputFromDSP->pointCloudOut.pointCloud[pntIdx].elevAngle;
gMmwMssMCB.pointCloudFromDSP[pntIdx].range = outputFromDSP->pointCloudOut.pointCloud[pntIdx].range;
gMmwMssMCB.pointCloudFromDSP[pntIdx].velocity = outputFromDSP->pointCloudOut.pointCloud[pntIdx].velocity;
gMmwMssMCB.pointCloudSideInfoFromDSP[pntIdx].snr = (float)outputFromDSP->pointCloudOut.snr[pntIdx].snr * 0.125f;
}
message 定義：mmwLab_output.h:
typedef enum mmwLab_output_message_type_e
{
/*! @brief List of detected points */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS = 1,
/*! @brief Range profile */
MMWDEMO_OUTPUT_MSG_RANGE_PROFILE,
/*! @brief Noise floor profile */
MMWDEMO_OUTPUT_MSG_NOISE_PROFILE,
/*! @brief Samples to calculate static azimuth heatmap */
MMWDEMO_OUTPUT_MSG_AZIMUT_STATIC_HEAT_MAP,
/*! @brief Range/Doppler detection matrix */
MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP,
/*! @brief Point Cloud - Array of detected points (range/angle/doppler) */
MMWDEMO_OUTPUT_MSG_POINT_CLOUD,
/*! @brief Target List - Array of detected targets (position, velocity, error covariance) */
MMWDEMO_OUTPUT_MSG_TARGET_LIST,
/*! @brief Target List - Array of target indices */
MMWDEMO_OUTPUT_MSG_TARGET_INDEX,
/*! @brief Classifier Output -- Array of target indices and tags */
MMWDEMO_OUTPUT_MSG_CLASSIFIER_OUTPUT,
/*! @brief Stats information */
MMWDEMO_OUTPUT_MSG_STATS,
/*! @brief Presence information */
MMWDEMO_OUTPUT_PRESENCE_IND,
MMWDEMO_OUTPUT_MSG_MAX
} mmwLab_output_message_type;
另外在 sdk 的 packages\ti\demo\xwr64xx\mmw\mmw_output.h 有：
typedef enum MmwDemo_output_message_type_e
{
/*! @brief List of detected points */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS = 1,
/*! @brief Range profile */
MMWDEMO_OUTPUT_MSG_RANGE_PROFILE,
/*! @brief Noise floor profile */
MMWDEMO_OUTPUT_MSG_NOISE_PROFILE,
/*! @brief Samples to calculate static azimuth heatmap */
MMWDEMO_OUTPUT_MSG_AZIMUT_STATIC_HEAT_MAP,
/*! @brief Range/Doppler detection matrix */
MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP,
/*! @brief Stats information */
MMWDEMO_OUTPUT_MSG_STATS,
/*! @brief List of detected points */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO,
/*! @brief Samples to calculate static azimuth/elevation heatmap, (all virtual antennas exported) */
MMWDEMO_OUTPUT_MSG_AZIMUT_ELEVATION_STATIC_HEAT_MAP,
/*! @brief temperature stats from Radar front end */
MMWDEMO_OUTPUT_MSG_TEMPERATURE_STATS,
MMWDEMO_OUTPUT_MSG_MAX
} MmwDemo_output_message_type;
前面一樣，6 開始不同，所以這個 example 的 tlv type 是自己用的。mss_main.c 從 UART 送出 TLV 的地方 MmwDemo_uartTxTask：
/* Send packet header */
UART_write (uartHandle,
(uint8_t*)&header,
sizeof(mmwLab_output_message_header));
/* Send detected Objects */
if (objOut->header.length > 0)
{
UART_write (uartHandle,
(uint8_t*)objOut,
objOut->header.length);
}
所以就是送 header，再送 objOut.header 歸 header，只是在說接下來的 object detection result tlv 有幾個 tlv。
length, checksum 都是 header 自己，沒有包含接下來的 detection result tlv
先看 header..
typedef struct mmwLab_output_message_header_t
{
/*! @brief Output buffer magic word (sync word). It is initialized to {0x0102,0x0304,0x0506,0x0708} */
uint16_t magicWord[4];
/*! brief Version: : MajorNum * 2^24 + MinorNum * 2^16 + BugfixNum * 2^8 + BuildNum */
uint32_t version;
/*! @brief Total packet length including header in Bytes */
uint32_t totalPacketLen;
/*! @brief platform type */
uint32_t platform;
/*! @brief Frame number */
uint32_t frameNumber;
/*! @brief For Advanced Frame config, this is the sub-frame number in the range
* 0 to (number of subframes - 1). For frame config (not advanced), this is always
* set to 0. */
uint32_t subFrameNumber;
/*! @brief Detection Layer timing */
uint32_t chirpProcessingMargin;
uint32_t frameProcessingTimeInUsec;
/*! @brief Localization Layer Timing */
uint32_t trackingProcessingTimeInUsec;
uint32_t uartSendingTimeInUsec;
/*! @brief Number of TLVs */
uint16_t numTLVs;
/*! @brief check sum of the header */
uint16_t checkSum;
} mmwLab_output_message_header;
設定值是...
header.platform = 0xA6843;
header.magicWord[0] = 0x0102;
header.magicWord[1] = 0x0304;
header.magicWord[2] = 0x0506;
header.magicWord[3] = 0x0708;
header.version = MMWAVE_SDK_VERSION_BUILD |
(MMWAVE_SDK_VERSION_BUGFIX << 8) |
(MMWAVE_SDK_VERSION_MINOR << 16) |
(MMWAVE_SDK_VERSION_MAJOR << 24);
...
...
packetLen = sizeof(mmwLab_output_message_header);
header.chirpProcessingMargin = timingInfo->interChirpProcessingMargin;
header.frameProcessingTimeInUsec = timingInfo->frameProcessingTimeInUsec;
header.uartSendingTimeInUsec = gMmwMssMCB.uartProcessingTimeInUsec;
if (objOut->header.length > 0)
{
packetLen += objOut->header.length;
tlvIdx++;
}
header.numTLVs = tlvIdx;
header.totalPacketLen = packetLen;
header.frameNumber = frameIdx;
header.subFrameNumber = subFrameIdx;
header.checkSum = 0;
headerPtr = (uint16_t *)&header;
for(n=0, sum = 0; n < sizeof(mmwLab_output_message_header)/sizeof(uint16_t); n++)
sum += *headerPtr++;
header.checkSum = ~((sum >> 16) + (sum & 0xFFFF));
payload 的 objOut 是...
objOut = &(gMmwMssMCB.pointCloudToUart);
其中，pointCloudToUart 是:
typedef struct MmwDemo_output_message_UARTpointCloud_t
{
mmwLab_output_message_tl header;
mmwLab_output_message_point_unit pointUint;
mmwLab_output_message_UARTpoint point[MAX_RESOLVED_OBJECTS_PER_FRAME];
} mmwLab_output_message_UARTpointCloud;
又分別是：
typedef struct mmwLab_output_message_tl_t
{
/*! @brief TLV type */
uint32_t type;
/*! @brief Length in bytes */
uint32_t length;
} mmwLab_output_message_tl;
typedef struct mmwLab_output_message_point_uint_t
{
/*! @brief elevation reporting unit, in radians */
float elevationUnit;
/*! @brief azimuth reporting unit, in radians */
float azimuthUnit;
/*! @brief Doppler reporting unit, in m/s */
float dopplerUnit;
/*! @brief range reporting unit, in m */
float rangeUnit;
/*! @brief SNR reporting unit, linear */
float snrUint;
} mmwLab_output_message_point_unit;
typedef struct mmwLab_output_message_UARTpoint_t
{
/*! @brief Detected point elevation, in number of azimuthUnit */
int8_t elevation;
/*! @brief Detected point azimuth, in number of azimuthUnit */
int8_t azimuth;
/*! @brief Detected point doppler, in number of dopplerUnit */
int16_t doppler;
/*! @brief Detected point range, in number of rangeUnit */
uint16_t range;
/*! @brief Range detection SNR, in number of snrUnit */
uint16_t snr;
} mmwLab_output_message_UARTpoint;
TI 有提供一個 python code, 做 pc 端UI : industrial visualizer
其中的 gui_parser.py 中 readAndParseUart( ) 就是 read uart 的進入點。
依序 sync magicword 之後，交給 parseFrame.py ，找出 tlvtype 之後，對應的 tlv parsing funtion 在 parseTLVs.py
occupancy detection 的 tlv frame 雖然是自己的，不是用 sdk 中 demo 的 標準 frame structure，
但是最後 parse point cloud 的格式，和 parseTLVs.py 的 parseCompressedSphericalPointCloudTLV( ) 一樣。
一個沒有 0 points 的 frame length 是 48，有 point 資料的frame 會是 48 + tlv length.
tlvlength 的資料中，在 CompressedSphericalCloudTLV 中，包含以下：
tlvHeaderStruct = struct(...
'type', {'uint32', 4}, ... % TLV object Type
'length', {'uint32', 4}); % TLV object Length, in bytes, including TLV header
% Point Cloud TLV reporting unit for all reported points
pointUintStruct = struct(...
'elevUnit', {'float', 4}, ... % elevation, in rad
'azimUnit', {'float', 4}, ... % azimuth, in rad
'dopplerUnit', {'float', 4}, ... % Doplper, in m/s
'rangeUnit', {'float', 4}, ... % Range, in m
'snrUnit', {'float', 4}); % SNR, ratio
之後是一堆points 的資料，每個 point 的內容是：
% Point Cloud TLV object consists of an array of points.
% Each point has a structure defined below
pointStruct = struct(...
'elevation', {'int8', 1}, ... % elevation, in rad
'azimuth', {'int8', 1}, ... % azimuth, in rad
'doppler', {'int16', 2}, ... % Doplper, in m/s
'range', {'uint16', 2}, ... % Range, in m
'snr', {'uint16', 2}); % SNR, ratio
以 frameLength 5098 的 frame 來看。扣掉 frame Header 48 bytes，剩下 460
460 扣掉 4+4+5x*4 = 28 之後，剩下 432，
這 432 bytes 都是 point data，每個 point data 是 8 bytes, 所以有.. 432/8 = 54 points
結果這個 unit 的內容是固定的...
ref: mss_main.c - OccupancyDetection3D_handleObjectDetResult()
gMmwMssMCB.pointCloudToUart.header.type = MMWDEMO_OUTPUT_MSG_POINT_CLOUD;
gMmwMssMCB.pointCloudToUart.pointUint.azimuthUnit = 0.01f;
gMmwMssMCB.pointCloudToUart.pointUint.elevationUnit = 0.01f;
gMmwMssMCB.pointCloudToUart.pointUint.rangeUnit = 0.00025f;
gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit = 0.00028f;
gMmwMssMCB.pointCloudToUart.pointUint.snrUint = 0.04f;
gMmwMssMCB.numDetectedPoints = outputFromDSP->pointCloudOut.object_count;
for (pntIdx = 0; pntIdx < (int32_t)outputFromDSP->pointCloudOut.object_count; pntIdx++ )
{
gMmwMssMCB.pointCloudToUart.point[pntIdx].azimuth = (int8_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].azimuthAngle / gMmwMssMCB.pointCloudToUart.pointUint.azimuthUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].elevation = (int8_t)round((outputFromDSP->pointCloudOut.pointCloud[pntIdx].elevAngle)/ gMmwMssMCB.pointCloudToUart.pointUint.elevationUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].range = (uint16_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].range / gMmwMssMCB.pointCloudToUart.pointUint.rangeUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].doppler = (int16_t)round(outputFromDSP->pointCloudOut.pointCloud[pntIdx].velocity / gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit);
gMmwMssMCB.pointCloudToUart.point[pntIdx].snr = (uint16_t)round((float)outputFromDSP->pointCloudOut.snr[pntIdx].snr * 0.125f / gMmwMssMCB.pointCloudToUart.pointUint.snrUint);
gMmwMssMCB.pointCloudFromDSP[pntIdx].elevAngle = outputFromDSP->pointCloudOut.pointCloud[pntIdx].elevAngle;
gMmwMssMCB.pointCloudFromDSP[pntIdx].range = outputFromDSP->pointCloudOut.pointCloud[pntIdx].range;
gMmwMssMCB.pointCloudFromDSP[pntIdx].velocity = outputFromDSP->pointCloudOut.pointCloud[pntIdx].velocity;
gMmwMssMCB.pointCloudSideInfoFromDSP[pntIdx].snr = (float)outputFromDSP->pointCloudOut.snr[pntIdx].snr * 0.125f;
}
沒有留言:
張貼留言