3D People 鍑芥暟娴佽瑙?
IWR6843 3D people tracking 鏍稿績鍑芥暟鐨勪綔鐢ㄣ€佽緭鍏ヨ緭鍑哄拰璋冪敤鍏崇郴锛岄檮 OpenRadar 瀵圭収
3D People 鍑芥暟娴佽瑙?
杩欓〉鏄嚱鏁扮骇鐨勭簿纭储寮曪紝姣忎釜鍑芥暟閮借鏄庯細**鍋氫粈涔?鈫?杈撳叆鏄粈涔?鈫?杈撳嚭鏄粈涔?鈫?涓轰粈涔堣繖鏍疯璁?*銆?
鏁版嵁娴佸叏鏅浘瑙侊細3D People 浠g爜鍦板浘
涓诲共 1锛氱郴缁熷惎鍔紙MSS 渚э級
绯荤粺涓婄數鍚?MSS 鍏堝惎鍔紝鍒濆鍖栧畬鎴愬悗鍐嶆媺璧?DSS銆?
main()
鈹斺攢鈻?Pcount3DDemo_initTask() # BIOS Task锛屾墍鏈夊垵濮嬪寲鍦ㄨ繖閲?
鈹溾攢鈻?Pcount3DDemo_platformInit() # 鏃堕挓銆丒DMA銆丠WA銆乵mWave 椹卞姩
鈹溾攢鈻?Pcount3DDemo_dataPathOpen() # 鍒涘缓 DPM 瀹炰緥
鈹溾攢鈻?Pcount3DDemo_openSensor() # mmWave_open()锛屽缓绔?BSS 杩炴帴
鈹?
鈹? # 绛夊緟 CLI 鍛戒护杩涙潵锛岀敤鎴烽€氳繃涓插彛鍙?sensorStart
鈹?
鈹溾攢鈻?Pcount3DDemo_dataPathConfig() # DPM ioctl 鎶婇厤缃笅鍙戠粰 DSS DPC
鈹溾攢鈻?MmwDemo_trackerConfig() # 閰嶇疆 Tracker DPU
鈹溾攢鈻?Pcount3DDemo_configSensor() # 鎶?chirp 閰嶇疆鍐欒繘 mmWave 灏勯鍓嶇
鈹斺攢鈻?Pcount3DDemo_startSensor() # 寮€濮嬪彂灏勶紝鏁版嵁寮€濮嬫祦鍔?鍏抽敭鍑芥暟璇存槑
main
MSS 杩涚▼鍏ュ彛銆傚彧鍋氫竴浠朵簨锛氬垱寤?initTask 鐒跺悗鍚姩 BIOS 璋冨害鍣ㄣ€備箣鍚庢墍鏈夐€昏緫閮藉湪 BIOS 浠诲姟閲屻€?
Pcount3DDemo_initTask
MSS 鐨勬€诲垵濮嬪寲浠诲姟銆傛寜椤哄簭鍒濆鍖栨墍鏈夊瓙绯荤粺锛岀劧鍚庤繘鍏ヤ簨浠跺惊鐜瓑寰?CLI 鍛戒护銆傝繖鏄暣涓簲鐢ㄧ殑"涓荤嚎绋?銆?
Pcount3DDemo_dataPathOpen
鍒涘缓 DPM 瀹炰緥锛屽悓鏃舵敞鍐屼袱涓?DPC锛堟暟鎹鐞嗛摼锛夛細
DPC_ObjectDetectionRangeHWA锛歁SS 渚э紝椹卞姩 HWA 鍋?Range FFTDPC_ObjectDetection锛欴SS 渚э紝鍋?Capon + CFAR锛堣窇鍦?C674x 涓婏級
MmwDemo_trackerConfig
鎶?GTRACK 鍙傛暟锛堟渶澶х洰鏍囨暟銆佸満鏅竟鐣屻€丼NR 闂ㄩ檺绛夛級鍐欏叆 Tracker DPU銆傝繖閲岀殑鍙傛暟鏉ヨ嚜 CLI 鍛戒护 trackerCfg銆?
涓诲共 2锛氳繍琛屾椂浠诲姟璋冨害锛圡SS 渚э紝姣忓抚瑙﹀彂锛?
MSS 鍚屾椂杩愯澶氫釜 BIOS 浠诲姟锛岀敤 Semaphore 鍗忚皟锛?
甯у紑濮嬩腑鏂?(纭欢)
鈹?
鈻?
Pcount3DDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn()
鈹? 閫氱煡 DPM 寮€濮嬪鐞?
鈻?
[DSS 澶勭悊涓€甯э紝瑙佷富骞?3]
鈹?
鈻? DPM_Report_NOTIFY_DPC_RESULT
Pcount3DDemo_DPC_ObjectDetection_reportFxn()
鈹? case NOTIFY_DPC_RESULT
鈻?
Pcount3DDemo_handleObjectDetResult() # mss_main.c:1688
鈹? 鎷疯礉鐐逛簯锛屽彂涓や釜淇″彿閲?
鈹溾攢鈹€鈻?Semaphore_post(trackerDPUSemHandle)
鈹? 鈹斺攢鈻?MmwDemo_trackerDPUTask() # 鍞ら啋 tracker 浠诲姟
鈹斺攢鈹€鈻?Semaphore_post(uartTxSemHandle)
鈹斺攢鈻?MmwDemo_uartTxTask() # 鍞ら啋 UART 鍙戦€佷换鍔?鍏抽敭鍑芥暟璇存槑
Pcount3DDemo_handleObjectDetResult
杩欐槸 MSS 渚ф渶鏍稿績鐨?缁撴灉澶勭悊"鍑芥暟銆?
- 杈撳叆锛歚DPM_Buffer *ptrResult
锛岄噷闈㈠瓨鐨勬槸 DSS 鍐呭瓨閲?DPC_ObjectDetection_ExecuteResult` 鐨勫湴鍧€ - 鐢?
SOC_translateAddress()鎶?DSS 鍦板潃缈昏瘧鎴?MSS 鍙闂殑鍦板潃 - 鎶?
radarProcessOutput.pointCloudOut閲屾瘡涓偣鐨?(azimuth, elevation, range, velocity, snr)鎷疯礉鍒?gMmwMssMCB.pointCloudFromDSP[] - 鍚屾鍘嬬缉缂栫爜鎴?
pointCloudToUart锛堟暣鏁伴噺鍖栵紝鑺傜渷 UART 甯﹀锛?
MmwDemo_trackerDPUTask
鐙珛 BIOS 浠诲姟锛岀瓑寰?trackerDPUSemHandle 淇″彿閲忥紝琚敜閱掑悗璋冪敤 Tracker DPU 澶勭悊涓€甯х偣浜戙€?
- 杈撳叆锛歚gMmwMssMCB.pointCloudFromDSP[]`锛圢 涓悆鍧愭爣鐐癸級
- 杈撳嚭锛歚gMmwMssMCB.trackerOutput`锛堢洰鏍囧垪琛?+ 鐩爣绱㈠紩 + 鍦ㄥ満鏍囧織锛?
MmwDemo_uartTxTask
鐙珛 BIOS 浠诲姟锛岀瓑寰?uartTxSemHandle锛岃鍞ら啋鍚庢墦鍖?TLV 鏁版嵁鍙?UART銆?
姣忓抚鏁版嵁鍖呯粨鏋勶細
[Header: 榄旀湳瀛?+ 甯у彿 + 鍖呴暱搴
[TLV 1: 鍘嬬缉鐐逛簯] type=6
[TLV 2: 鐩爣鍒楄〃] type=7
[TLV 3: 鐩爣绱㈠紩] type=8
[TLV 4: 鐩爣楂樺害] (鍙€? type=9涓诲共 3锛欴SS 淇″彿澶勭悊閾撅紙姣忓抚鎵ц锛?
DSS 渚ф槸鐪熸鐨?璁$畻閲嶅績"銆?
main() 鈫?dss_main.c:793
鈹斺攢鈻?Pcount3DDemo_dssInitTask()
鈹溾攢鈻?MmwDemo_edmaInit() # 鍒濆鍖?EDMA锛堝唴瀛樻惉杩愮‖浠讹級
鈹溾攢鈻?MmwDemo_edmaOpen()
鈹斺攢鈻?Pcount3DDemo_DPC_RadarProc_dpmTask() # DPM 涓讳换鍔★紝寰幆绛夊緟
鈹?
鈹?姣忓抚琚?DPM 璋冨害
鈻?
DPC_ObjectDetection_execute() 鈫?objectdetection.c:518
鈹?
鈻?
DPU_radarProcess_process() 鈫?radarProcess.c (Capon 鏍稿績)
鈹?
鈹溾攢鈹€ 闈欐€佹潅娉㈡秷闄?
鈹溾攢鈹€ Capon 娉㈡潫褰㈡垚 鈫?range-azimuth heatmap
鈹溾攢鈹€ 2D CFAR (azimuth + range 涓や釜鏂瑰悜)
鈹斺攢鈹€ 澶氭櫘鍕掍及璁?
鈹?
鈻? 杈撳嚭: radarProcessOutput
鎵撳寘缁撴灉鍒?DPM_Buffer
DPM 鑷姩閫氱煡 MSS鍏抽敭鍑芥暟璇存槑
DPC_ObjectDetection_init
鍦?DSS 渚у垱寤?ObjectDetection DPC 瀵硅薄锛屽垎閰嶆墍鏈夐渶瑕佺殑鍐呭瓨锛坮adar cube銆乭eatmap buffer銆乸oint cloud buffer锛夈€傚彧鍦ㄥ惎鍔ㄦ椂鎵ц涓€娆°€?
DPC_ObjectDetection_execute
DPC 鐨勬牳蹇冩墽琛屽嚱鏁帮紝姣忓抚琚?DPM 璋冨害涓€娆°€?
- 瑙﹀彂甯ч棿鍥炶皟锛堝憡璇変笂灞?甯ч棿澶勭悊寮€濮?锛?
- 璋冪敤
DPU_radarProcess_process()锛屾妸 radar cube 鍙樻垚鐐逛簯 - 鎶婄粨鏋滄墦鍖呰繘
DPM_Buffer锛堜袱涓?slot锛氱粨鏋?+ 缁熻锛? - 杩斿洖鍚?DPM 鑷姩鎶婄粨鏋滄帹閫佺粰 MSS
DPC_ObjectDetection_ioctl
閰嶇疆涓嬪彂鍏ュ彛銆侻SS 閫氳繃 DPM_ioctl() 鎶婂弬鏁板彂杩囨潵锛坈hirp 鏁般€佸抚鍛ㄦ湡銆丆FAR 闂ㄩ檺绛夛級锛岃繖涓嚱鏁版帴鏀跺苟鏇存柊 DPC 鍐呴儴鐘舵€併€?
DPC 鐢熷懡鍛ㄦ湡椤哄簭锛?
init 鈫?preStartConfig 鈫?start 鈫?[execute 脳 N甯 鈫?stop 鈫?deinit涓诲共 4锛歍racker DPU锛圡SS 渚э紝trackerproc_3d.c锛?
DPU_TrackerProc_init() # 鍒嗛厤 tracker 瀵硅薄鍐呭瓨
鈹?
DPU_TrackerProc_config() # 鍐欏叆 GTRACK 閰嶇疆鍙傛暟
鈹?
姣忓抚:
DPU_TrackerProc_process() 鈫?trackerproc_3d.c:521
鈹?
鈹溾攢鈹€ 鏍煎紡杞崲:
鈹? DPIF_PointCloudSpherical 鈫?GTRACK_measurementPoint
鈹? (range, azimuthAngle, elevAngle, velocity, snr)
鈹?
鈹斺攢鈹€ gtrack_step(handle, points, NULL, mNum, ...)
鈹? TI GTRACK 搴撴牳蹇冿紙闂簮锛?
鈻?
targetDescr[] 鈫?姣忎釜鐩爣: tid, 浣嶇疆, 閫熷害, 鍗忔柟宸?
targetIndex[] 鈫?姣忎釜杈撳叆鐐瑰綊灞炲摢涓洰鏍?
presenceInd 鈫?鍦烘櫙鍐呮槸鍚︽湁浜猴紙bool锛?鍏抽敭鍑芥暟璇存槑
DPU_TrackerProc_process
Tracker DPU 鐨勬瘡甯у鐞嗗嚱鏁般€?
- 杈撳叆锛歚numObjsIn
涓?DPIF_PointCloudSpherical` 鐐?+ 瀵瑰簲 SNR - 鍐呴儴鎶婄悆鍧愭爣鐐硅浆鎹㈡垚
GTRACK_measurementPoint锛圙TRACK 搴撹姹傜殑鏍煎紡锛? - 璋冪敤
gtrack_step()锛氭暟鎹叧鑱?+ EKF 棰勬祴/鏇存柊 + 鐩爣绠$悊 - 杈撳嚭锛氱洰鏍囧垪琛紙浣嶇疆銆侀€熷害锛夈€佺偣浜戝綊灞炵储寮曘€佸湪鍦烘娴嬬粨鏋?
DPU_TrackerProc_CartesianToSpherical
鍧愭爣杞崲宸ュ叿鍑芥暟锛氱洿瑙掑潗鏍?(x,y,z) 鈫?鐞冨潗鏍?(range, azimuth, elevation)銆?
GTRACK 鍐呴儴鐢ㄧ瑳鍗″皵鍧愭爣锛岃緭鍑烘椂鍐嶈浆鍥炴潵銆?
DPU_TrackerProc_config
鍐欏叆 GTRACK 鐨勬墍鏈夐潤鎬佸弬鏁帮細
- 鏈€澶х洰鏍囨暟
maxNumPoints - 鍦烘櫙杈圭晫锛堟娴嬭寖鍥寸殑 x/y/z 鑼冨洿锛?
- SNR 闂ㄩ檺銆侀€熷害闂ㄩ檺
- EKF 杩囩▼鍣0銆佹祴閲忓櫔澹扮煩闃?
涓诲共 5锛歄penRadar Python 瀵圭収
Python 绔埌绔?demo 鍜屽浐浠剁畻娉曚竴涓€瀵瑰簲锛?
| 鍥轰欢鍑芥暟 | Python 瀵瑰簲 | 璇存槑 |
|---|---|---|
| HWA Range FFT | dsp.range_processing(frame) | 杈撳嚭 radar_cube |
| DSS 闈欐€佹潅娉㈡秷闄? | radar_cube - radar_cube.mean(0) | 鏃堕棿鍧囧€肩浉鍑? |
RADARDEMO_aoaEst2DCaponBF | dsp.aoa_capon(radar_cube[:,:,i].T, steering_vec) | 姣忎釜 range bin 鍋?Capon |
RADARDEMO_detectionCFAR | dsp.ca_(heatmap_log, l_bound, guard_len, noise_len) | CA-CFAR |
| 澶氭櫘鍕掍及璁? | np.fft.fft(dopplerFFTInput, axis=0) | beamWeights 鍔犳潈鍚庡仛 FFT |
gtrack_step() | tracker.step() | EKF 鍏宠仈 + 鐘舵€佹洿鏂? |
| UART TLV 杈撳嚭 | gtrack_visualize.update_frame() | 鍙鍖栫瓑浠? |
Python EKF 涓変釜鏍稿績鍑芥暟锛?
EKF.__init__
鍒涘缓璺熻釜鍣紝璁剧疆榛樿 EKF 鍙傛暟锛圦 鐭╅樀銆丷 鐭╅樀銆佹渶澶х洰鏍囨暟锛夈€?
绛変环浜庡浐浠剁殑 GTRACK_moduleCreate() + DPU_TrackerProc_config()銆?
EKF.update_point_cloud
鎺ユ敹鏂颁竴甯х殑鐐逛簯鏁版嵁锛坮anges, azimuths, dopplers, snrs锛夊瓨鍏ュ唴閮ㄣ€?
绛変环浜庡浐浠朵腑鎶?pointCloudFromDSP[] 浼犵粰 tracker 鐨勮繃绋嬨€?
EKF.step
鎵ц涓€姝ヨ窡韪細鏁版嵁鍏宠仈 + EKF 棰勬祴/鏇存柊 + 鐩爣鐢熷懡鍛ㄦ湡绠$悊銆?
绛変环浜庡浐浠剁殑 gtrack_step()銆?
瀹屾暣鍑芥暟绱㈠紩
mss_main.c 鍏ㄥ嚱鏁板垪琛紙灞曞紑锛�
| 鍑芥暟 | 琛屽彿 | 涓€鍙ヨ瘽璇存槑 |
|---|---|---|
Pcount3DDemo_edmaInit | 268 | 鍒濆鍖?MSS 渚?EDMA 鎺у埗鍣? |
Pcount3DDemo_edmaOpen | 323 | 鎵撳紑 EDMA 閫氶亾 |
Pcount3DDemo_hwaInit | 377 | 鍒濆鍖?HWA锛圧ange FFT 纭欢鍔犻€熷櫒锛? |
Pcount3DDemo_setSubFramePendingState | 460 | 鏍囪瀛愬抚閰嶇疆寰呬笅鍙? |
Pcount3DDemo_CfgUpdate | 548 | 鎶?CLI 瑙f瀽鍑虹殑閰嶇疆鍐欏叆鍐呴儴缁撴瀯浣? |
MmwDemo_uartTxTask | 575 | UART 鍙戦€佷换鍔★紝鎵撳寘 TLV 鍙戠粰涓婁綅鏈? |
Pcount3DDemo_mmWaveCtrlTask | 800 | mmWave 鎺у埗浠诲姟锛屽鐞嗗皠棰戝墠绔簨浠? |
Pcount3DDemo_DPM_ioctl_blocking | 828 | 鍙?DPM ioctl 骞剁瓑寰?semaphore 纭瀹屾垚 |
Pcount3DDemo_dataPathOpen | 859 | 鍒涘缓 DPM 瀹炰緥锛屾敞鍐?MSS + DSS 涓や釜 DPC |
Pcount3DDemo_dataPathConfig | 881 | 閫氳繃 DPM ioctl 鎶?chirp/甯ч厤缃笅鍙戠粰涓や釜 DPC |
MmwDemo_trackerConfig | 1226 | 閰嶇疆 Tracker DPU 鍙傛暟锛堜粠 CLI trackerCfg 鍛戒护璇诲彇锛? |
Pcount3DDemo_dataPathStart | 1258 | 鍚姩 DPM锛屽紑濮嬫帴鏀跺抚鏁版嵁 |
Pcount3DDemo_eventCallbackFxn | 1327 | 灏勯鍓嶇锛圔SS锛変簨浠跺洖璋冿紝澶勭悊甯у紑濮?缁撴潫涓柇 |
Pcount3DDemo_DPC_ObjectDetection_reportFxn | 1447 | DPM 鎶ュ憡鍥炶皟锛屾帴鏀?DSS 澶勭悊缁撴灉閫氱煡 |
Pcount3DDemo_handleObjectDetResult | 1688 | 鏍稿績缁撴灉澶勭悊锛氬湴鍧€缈昏瘧鈫掔偣浜戞嫹璐濃啋淇″彿閲忛€氱煡 |
MmwDemo_trackerDPUTask | 1874 | Tracker 浠诲姟锛氱瓑淇″彿閲忊啋璋?tracker鈫掑啓杈撳嚭 |
mmwDemo_mssDPMTask | 1913 | MSS 渚?DPM 璋冨害浠诲姟锛屽惊鐜皟 DPM_execute() |
Pcount3DDemo_openSensor | 1947 | mmWave_open()锛屽缓绔嬩笌 BSS 灏勯鍓嶇鐨勮繛鎺? |
Pcount3DDemo_configSensor | 2146 | mmWave_config()锛屾妸閰嶇疆鍐欒繘灏勯纭欢 |
Pcount3DDemo_startSensor | 2196 | mmWave_start()锛屽紑濮嬪彂灏?chirp锛屾暟鎹紑濮嬫祦鍔? |
Pcount3DDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn | 2333 | 甯у紑濮嬪洖璋冿紝瑙﹀彂鏂颁竴甯х殑澶勭悊 |
Pcount3DDemo_platformInit | 2371 | 骞冲彴搴曞眰鍒濆鍖栵細SOC銆佹椂閽熴€佸璁? |
Pcount3DDemo_initTask | 2417 | MSS 鎬诲垵濮嬪寲浠诲姟锛屾寜椤哄簭璋冪敤鎵€鏈?init 鍑芥暟 |
main | 2702 | MSS 杩涚▼鍏ュ彛锛屽垱寤?initTask 鍚庡惎鍔?BIOS |
objectdetection.c 鍏ㄥ嚱鏁板垪琛紙灞曞紑锛�
| 鍑芥暟 | 琛屽彿 | 涓€鍙ヨ瘽璇存槑 |
|---|---|---|
DPC_ObjectDetection_dataInjection | 177 | 娉ㄥ叆娴嬭瘯鏁版嵁锛堣皟璇曠敤锛岀粫杩囩湡瀹?ADC锛? |
DPC_ObjDetDSP_reconfigSubFrame | 206 | 瀛愬抚鍒囨崲鏃堕噸鏂伴厤缃?DPU 鍙傛暟 |
DPC_ObjDetDSP_initDPU | 238 | 璋冪敤 radarProcess DPU 鐨?init锛屽垎閰嶅鐞嗗唴瀛? |
DPC_ObjDetDSP_preStartConfig | 313 | start 鍓嶆妸鎵€鏈夊弬鏁板啓鍏ュ悇 DPU |
DPC_ObjectDetection_frameStart | 404 | 甯у紑濮嬫椂琚?DPM 璋冪敤锛屽噯澶?input buffer |
DPC_ObjectDetection_start | 441 | 鍚姩 DPC锛岃皟鐢?preStartConfig锛屼娇鑳藉鐞嗛摼 |
DPC_ObjectDetection_stop | 485 | 鍋滄 DPC锛岀鐢ㄥ鐞嗛摼 |
DPC_ObjectDetection_execute | 518 | 鏍稿績锛氭瘡甯ц皟鐢?radarProcess锛屾墦鍖呯粨鏋? |
DPC_ObjectDetection_ioctl | 608 | 鎺ユ敹鏉ヨ嚜 MSS 鐨勯厤缃懡浠? |
DPC_ObjectDetection_init | 748 | 鍒涘缓 DPC 瀵硅薄锛屽垎閰嶆墍鏈?buffer |
DPC_ObjectDetection_deinit | 861 | 閿€姣?DPC锛岄噴鏀炬墍鏈夊唴瀛? |
trackerproc_3d.c 鍏ㄥ嚱鏁板垪琛紙灞曞紑锛�
| 鍑芥暟 | 琛屽彿 | 涓€鍙ヨ瘽璇存槑 |
|---|---|---|
DPU_trackerProc_updateParamTables | 166 | 鎶婇厤缃弬鏁拌浆鎹㈡垚 GTRACK 鍐呴儴鏌ユ壘琛? |
DPU_TrackerProc_init | 214 | 鍒嗛厤 tracker DPU 瀵硅薄鍐呭瓨 |
DPU_TrackerProc_config | 252 | 鍐欏叆 GTRACK 鍙傛暟锛岃皟鐢?GTRACK_moduleCreate() |
DPU_TrackerProc_process | 521 | 鏍稿績锛氱偣浜戔啋鏍煎紡杞崲鈫抔track_step()鈫掕緭鍑虹洰鏍? |
DPU_TrackerProc_control | 671 | 杩愯鏃跺姩鎬佹洿鏂?tracker 鍙傛暟 |
DPU_TrackerProc_deinit | 732 | 閲婃斁 tracker 璧勬簮锛岃皟鐢?GTRACK_moduleDelete() |
DPU_TrackerProc_CartesianToSpherical | 775 | 鍧愭爣杞崲锛氱洿瑙?x,y,z) 鈫?鐞冨潗鏍?r,az,el) |
濡備綍鐢ㄨ繖椤?
- 鎯崇湅鍚姩娴佺▼ 鈫?鐪嬩富骞?1锛屼粠
main寰€涓嬭拷 - *鎯崇湅姣忓抚鏁版嵁鎬庝箞璧? 鈫?鐪嬩富骞?2 鍜屼富骞?3 鐨勭粍鍚堬紝閲嶇偣鏄?
handleObjectDetResult - 鎯崇湅淇″彿澶勭悊缁嗚妭 鈫?鐪嬩富骞?3锛岀劧鍚庡幓璇?
radarProcess.c鍜?RADARDEMO_aoaEst2DCaponBF.c - 鎯崇湅 tracker 缁嗚妭 鈫?鐪嬩富骞?4锛宍DPU_TrackerProc_process()` 鏄叆鍙?
- 鎯崇敤 Python 鐞嗚В绠楁硶 鈫?鐪嬩富骞?5锛岄厤鍚?
people_tracking.py鍗曟璋冭瘯