// 秒寻科技 // 计算第一径数据队列 // zt // 2023-03-25 #include "../../includes/includes.h" // 导出全局变量 extern flt32 gg_afOneFrameLocationData[PUB_SLOT_FRAME_LEN]; // 一帧临时定位数据 extern LCT_FIRSTPATH_PARAM_T gg_stFirstPathParam; // 第一径模块参数 extern flt32 g_afSincFilterDataList[24001]; // sinc滤波器 extern PUB_SYS_STATUS_T gg_stSysStatus; extern uint32 gg_dwFrameNo; extern LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T gLct_astPredictionDirectionDataList[PUB_SYS_CARRIER_NUM]; void lct_firstpath_init(LCT_FIRSTPATH_PARAM_T stFirstPathParam) { // 帧内最小值选择因子 gg_stFirstPathParam.byMinValueFactor = stFirstPathParam.byMinValueFactor; // 帧内锚点因子 gg_stFirstPathParam.byAnchorFactor = stFirstPathParam.byAnchorFactor; gg_stFirstPathParam.byAnchorThreshold = stFirstPathParam.byAnchorThreshold; // 峰间距 gg_stFirstPathParam.byPeakPeakDistance = stFirstPathParam.byPeakPeakDistance; // 计算帧内噪声门限时信号因子 gg_stFirstPathParam.byNoiseSignalFactor = stFirstPathParam.byNoiseSignalFactor; // 计算帧内噪声门限时噪声放大倍数 gg_stFirstPathParam.byNoiseHightScale = stFirstPathParam.byNoiseHightScale; gg_stFirstPathParam.byNoiseLowScale = stFirstPathParam.byNoiseLowScale; // cfar因子 gg_stFirstPathParam.byCfarLeftFactor = stFirstPathParam.byCfarLeftFactor; // sinc门限因子 gg_stFirstPathParam.bySincFactor = stFirstPathParam.bySincFactor; // 第一径联系锚点的因子 gg_stFirstPathParam.byFirstPathContactFactor = stFirstPathParam.byFirstPathContactFactor; // 第一径联系锚点的峰间距离 gg_stFirstPathParam.wFirstPathContactDistance = stFirstPathParam.wFirstPathContactDistance; //// 计算跟踪径的参数 // 跟踪窗口宽度（跟踪径索引左右两边的距离-单边距离） gg_stFirstPathParam.wFirstPathTrackWindowWidth = stFirstPathParam.wFirstPathTrackWindowWidth; // 跟踪径保护带宽 gg_stFirstPathParam.wFirstPathTrackPeakGuardWidth = stFirstPathParam.wFirstPathTrackPeakGuardWidth; // 跟踪径噪声查找宽度（从保护带宽左侧向左开始算） gg_stFirstPathParam.wFirstPathTrackNoiseFindWidth = stFirstPathParam.wFirstPathTrackNoiseFindWidth; // 跟踪径增益因子 gg_stFirstPathParam.byFirstPathTrackPeakGainFactor = stFirstPathParam.byFirstPathTrackPeakGainFactor; return; } // 模块主控函数:根据定位基站队列，生产第一径队列 // pstLctBtsList:一组定位基站 // pstLctDataList:定位数据队列 // pstFirstPathList:第一径数据队列(返回参数) void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T *pstFirstPathList) { uint32 dwFrameNo = 0; uint8 byRemainder = 0; int32 dwSyncOffset = 0; uint8 byLctBtsNum = pstLctBtsList->byBtsNum; // 有效定位基站组的基站个数 PUB_LCT_BTS_T *pstLctBts = NULL; // 定位基站 PUB_LOCATION_DATA_T *pstLctData = NULL; // 定位数据 uint8 byFreqIndex = 0; // 基站频率索引 uint16 wAheadFrameFirstIndex = 0; // 上一次帧内第一径索引 uint16 wMaxValueIndex = 0; flt32 fMaxValue; // 最大值 uint16 wAnchorIndex = 0; flt32 fNoise = 0; flt32 fPeakThreshold = 0; FIRST_PATH_T stFirstPath = {0}; uint8 byCounter = 0; uint16 wFrameTraceIndex = 0; // 帧内跟踪索引 LCT_FIRSTPATH_T *pstFirstPath = NULL; // 复位预测方向数据有效标识符 lct_firstpath_reset_direction_valid_flag(gLct_astPredictionDirectionDataList); // 遍历定位基站队列，计算基站第一径 for (uint8 i = 0; i < byLctBtsNum; i++) { pstLctBts = &pstLctBtsList->astLocationBts[i]; // 定位基站 byFreqIndex = pstLctBts->dwFreqIndex; // 基站的频率索引 pstLctData = &pstLctDataList[byFreqIndex]; // 基站的定位数据 // 获取该基站一帧定位数据 lct_firstpath_get_lct_data(pstLctData, gg_afOneFrameLocationData); // TODO测试 if (PUB_DEBUG_ENABLE) { if (pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 232) { char *pfileName = "E:\\work\\ips8000\\aplm8000sdk\\output\\lctdata\\firstpath.bin"; util_write_flt32_to_bin_file(pfileName, PUB_SLOT_FRAME_LEN, gg_afOneFrameLocationData, 1); uint8 byTmp = 0; } } // 当前一秒数据有效(是定位基站、定位基站有效、当前幅度值有效) if (pstLctData->byLctBtsFlag && pstLctData->byBtsValidFlag && pstLctData->byCurrentAmplValidFlag) { // 帧内跟踪索引 wFrameTraceIndex = pstLctData->wAheadFrameTrackPathIndex; // 基站第一径数据 pstFirstPath = &pstFirstPathList->astFirstPath[byCounter]; // 返回参数 memset(pstFirstPath, 0, sizeof(LCT_FIRSTPATH_T)); ////将定位基站数据中的数据，复制到第一径数据结构中 pstFirstPath->byMainFlag = (uint8)pstLctBts->dwMainBtsFlag; pstFirstPath->dwBuildId = pstLctBts->dwBuildId; pstFirstPath->dwLayerId = pstLctBts->dwLayerId; pstFirstPath->dwBtsId = pstLctBts->dwBtsId; pstFirstPath->byFreqIndex = (uint8)pstLctBts->dwFreqIndex; pstFirstPath->bySlot = (uint8)pstLctBts->dwSlot; pstFirstPath->dwLctType = pstLctBts->dwLctType; pstFirstPath->dwTwoBtsNum = pstLctBts->dwTwoBtsNum; memcpy(pstFirstPath->adwTwoBtsList, pstLctBts->adwTwoBtsList, sizeof(uint32) * pstLctBts->dwTwoBtsNum); pstFirstPath->dwCoordX = pstLctBts->dwCoordX; pstFirstPath->dwCoordY = pstLctBts->dwCoordY; pstFirstPath->dwCoordZ = pstLctBts->dwCoordZ; // 计算"计算径"，并存参数 lct_firstpath_cal_calpath(gg_afOneFrameLocationData, wFrameTraceIndex, &wMaxValueIndex, &wAnchorIndex, &fNoise, &fPeakThreshold, &stFirstPath); if (stFirstPath.wFirstPathIndex > 3000) // 计算径结果有效 { // pstFirstPath->dwCalPathIndex = pstLctData->dwDdcIndex + pstLctData->dwSignalSlotSyncOffset + pstLctData->dwSignalSyncIndex + stFirstPath.wFirstPathIndex; // 第一径绝对索引 // pstFirstPath->dwCalPathIndex = pstLctData->dwDdcIndex + pstLctData->dwSignalSyncIndex + stFirstPath.wFirstPathIndex; // 第一径绝对索引 pstFirstPath->dwCalPathIndex = PUB_SLOT_FRAME_LEN + pstLctData->dwSignalSlotSyncOffset + pstLctData->dwSignalSyncIndex + stFirstPath.wFirstPathIndex; // 第一径绝对索引 pstFirstPath->wFrameCalIndex = stFirstPath.wFirstPathIndex; // 第一径帧内索引 pstFirstPath->fFrameCalPathValue = gg_afOneFrameLocationData[stFirstPath.wFirstPathIndex]; // 计算径值 pstFirstPath->fCalPathIndexSnr = stFirstPath.fSnr; // 计算径信噪比 fMaxValue = gg_afOneFrameLocationData[wMaxValueIndex]; pstFirstPath->fMaxAmplValue = fMaxValue; // 帧内最大值 // TODO测试 if (PUB_DEBUG_ENABLE) { if (pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 220) { printf("frame no:%d, freq index:%d, sync index:%d, cal frame index:%d, cal index:%d\n", pstLctData->dwFrameNo - 1, pstFirstPath->byFreqIndex, pstLctData->dwSignalSyncIndex, pstFirstPath->wFrameCalIndex, pstFirstPath->dwCalPathIndex); uint8 byTmp = 0; } } } else { pstFirstPath->wFrameCalIndex = 0; } // 计算"跟踪径" if (wFrameTraceIndex && wFrameTraceIndex < 12000) { lct_firstpath_cal_trackpath(gg_afOneFrameLocationData, wFrameTraceIndex, fNoise, fPeakThreshold, wAnchorIndex, wMaxValueIndex, &stFirstPath); // 跟踪成功 if (stFirstPath.byTrackSuccessFlag && stFirstPath.wFirstPathIndex > 3000) { // pstFirstPath->dwTrackPathIndex = pstLctData->dwDdcIndex + pstLctData->dwSignalSlotSyncOffset + pstLctData->dwSignalSyncIndex + stFirstPath.wFirstPathIndex; // 第一径绝对索引 // pstFirstPath->dwTrackPathIndex = pstLctData->dwDdcIndex + pstLctData->dwSignalSyncIndex + stFirstPath.wFirstPathIndex; // 第一径绝对索引 pstFirstPath->dwTrackPathIndex = PUB_SLOT_FRAME_LEN + pstLctData->dwSignalSlotSyncOffset + pstLctData->dwSignalSyncIndex + stFirstPath.wFirstPathIndex; // 第一径绝对索引 pstFirstPath->wFrameTrackIndex = stFirstPath.wFirstPathIndex; // 第一径帧内索引 pstFirstPath->fFrameTrackPathValue = gg_afOneFrameLocationData[stFirstPath.wFirstPathIndex]; // 跟踪径值 pstFirstPath->fTrackPathIndexSnr = stFirstPath.fSnr; // 跟踪径信噪比 } // 跟踪失败，则使用计算径的数据 else { pstFirstPath->dwTrackPathIndex = pstFirstPath->dwCalPathIndex; // 第一径绝对索引 pstFirstPath->fFrameTrackPathValue = pstFirstPath->fFrameCalPathValue; pstFirstPath->wFrameTrackIndex = pstFirstPath->wFrameCalIndex; pstFirstPath->fTrackPathIndexSnr = pstFirstPath->fCalPathIndexSnr; } } else { pstFirstPath->dwTrackPathIndex = pstFirstPath->dwCalPathIndex; // 第一径绝对索引 pstFirstPath->fFrameTrackPathValue = pstFirstPath->fFrameCalPathValue; pstFirstPath->wFrameTrackIndex = pstFirstPath->wFrameCalIndex; pstFirstPath->fTrackPathIndexSnr = pstFirstPath->fCalPathIndexSnr; } // TODO测试 if (PUB_DEBUG_ENABLE) { if (pstFirstPath->wFrameTrackIndex > 0 && pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 220) { printf("frame no:%d, freq index:%d, sync index:%d, track frame index:%d, track index:%d\n", pstLctData->dwFrameNo - 1, pstFirstPath->byFreqIndex, pstLctData->dwSignalSyncIndex, pstFirstPath->wFrameTrackIndex, pstFirstPath->dwTrackPathIndex); uint8 byTmp = 0; } } // 第一径合法 if (pstFirstPath->wFrameCalIndex > 3000 || pstFirstPath->wFrameTrackIndex > 3000) { byCounter++; // 备份第一径数据，用于预测移动方向 lct_firstpath_save_first_path_data(pstLctData, pstFirstPath, gLct_astPredictionDirectionDataList); } } } // 保存第一径数据队列的基站数量 pstFirstPathList->byBtsNum = byCounter; // 预测移动方向 lct_firstpath_prediction_move_direction(gLct_astPredictionDirectionDataList); return; } // 获取一帧定位数据 // pstLctData:定位数据队列 // pfOutData:输出参数，一帧定位数据 void lct_firstpath_get_lct_data(PUB_LOCATION_DATA_T *pstLctData, flt32 *pfOutData) { int32 dwStartIndex = 0; uint32 dwTmpIndex = 0; // 计算定位数据所在队列中的开始索引 // dwStartIndex = pstLctData->dwLocationIndex + (bySolt * PUB_SLOT_FRAME_LEN); dwStartIndex = pstLctData->dwLocationIndex + pstLctData->dwSignalSyncIndex; if (dwStartIndex < 0) { dwStartIndex += PUB_LCT_DATA_LIST_LEN; } else if (dwStartIndex >= PUB_LCT_DATA_LIST_LEN) { dwStartIndex -= PUB_LCT_DATA_LIST_LEN; } // 获取一帧数据 util_looplist_get_flt32(pstLctData->afData, PUB_LCT_DATA_LIST_LEN, dwStartIndex, pfOutData, PUB_SLOT_FRAME_LEN, &dwTmpIndex); return; } // 计算 "计算径" // pfLocationData:定位数据 // pwMaxValueIndex:大值索引（返回参数） // pwAnchorIndex:锚点索引（返回参数） // pdwNoise:噪声（返回参数） // pdwPeakThreshold:选峰门限（返回参数） // pstFirstPath：计算径（返回参数） void lct_firstpath_cal_calpath(flt32 *pfLocationData, uint16 wAheadFrameFirstPath, uint16 *pwMaxValueIndex, uint16 *pwAnchorIndex, flt32 *pfNoise, flt32 *pfPeakThreshold, FIRST_PATH_T *pstFirstPath) { uint16 wMaxValueIndex = 0; // 帧内最大值索引 uint16 wMinValueIndex = 0; flt32 fNoise = 0; // 帧内噪声 flt32 fSnr = 0; // 信噪比 flt32 fSelPeakThreshold = 0; // 帧内峰值门限 flt32 fMiniValueFactor = (flt32)gg_stFirstPathParam.byMinValueFactor / 100; uint16 awPeakList[PUBLIC_LCT_PEAK_LIST_LEN]; // 峰值索引队列 uint16 wPeakListLen = 0; // 峰值索引队列长度 uint16 wAnchorIndex = 0; // 帧内锚点索引 flt32 fThresholdWithAnchor = 0; uint16 wFirstPathLen = 0; // 第一径队列长度 uint16 awOriginalList[PUBLIC_LCT_PEAK_LIST_LEN]; // 原始峰值索引队列 uint16 wOriginalListLen = 0; // 原始峰值索引队列长度 uint16 wFirstPathIndex = 0; // 第一径索引 memset(awOriginalList, 0, sizeof(uint16) * PUBLIC_LCT_PEAK_LIST_LEN); memset(awPeakList, 0, sizeof(uint16) * PUBLIC_LCT_PEAK_LIST_LEN); // 计算帧内最大值索引、噪声和信噪比 lct_firstpath_cal_frame_max_min_value_index_noise_snr(pfLocationData, wAheadFrameFirstPath, &wMaxValueIndex, &wMinValueIndex, &fNoise, &fSnr); *pwMaxValueIndex = wMaxValueIndex; *pfNoise = fNoise; // 计算选峰门限 lct_firstpath_cal_peak_threshold(fNoise, fSnr, &fSelPeakThreshold); if (fSelPeakThreshold < fMiniValueFactor * pfLocationData[wMaxValueIndex]) { fSelPeakThreshold = fMiniValueFactor * pfLocationData[wMaxValueIndex]; } *pfPeakThreshold = fSelPeakThreshold; // 选左侧峰 lct_firstpath_select_left_peak(pfLocationData, fSelPeakThreshold, wMinValueIndex, wMaxValueIndex, awPeakList, &wPeakListLen); // 计算锚点索引 lct_firstpath_cal_anchor_index(awPeakList, wPeakListLen, pfLocationData, fSnr, &wAnchorIndex); *pwAnchorIndex = wAnchorIndex; // 二次计算选峰门限 fSelPeakThreshold = lct_firstpath_recal_peak_value_threshold(pfLocationData, wMaxValueIndex, wAnchorIndex, fSelPeakThreshold); *pfPeakThreshold = fSelPeakThreshold; // 二次选峰 lct_firstpath_select_peak_by_anchor(awPeakList, wPeakListLen, wAnchorIndex, pfLocationData, fSelPeakThreshold, &wPeakListLen); //// 基于峰值门限计算第一径 wFirstPathLen = 0; lct_firstpath_cal_first_path_by_peak_value_threshold(pfLocationData, wMaxValueIndex, fSelPeakThreshold, awPeakList, wPeakListLen, 0, &wFirstPathLen); wPeakListLen = wFirstPathLen; // 调整峰值队列长度 //// 基于CFAR规则计算第一径 wFirstPathLen = 0; lct_firstpath_cal_first_path_by_cfar(pfLocationData, wMaxValueIndex, awPeakList, wPeakListLen, 0, &wFirstPathLen); wPeakListLen = wFirstPathLen; // 调整峰值队列长度 //// 基于SINC规则计算第一径 wFirstPathLen = 0; lct_firstpath_cal_first_path_by_sinc(pfLocationData, wMaxValueIndex, fNoise, awPeakList, wPeakListLen, 0, &wFirstPathLen); wPeakListLen = wFirstPathLen; // 调整峰值队列长度 // 检查锚点是否还是有效峰 lct_firstpath_check_anchor_index(awPeakList, wPeakListLen, wAnchorIndex, wMaxValueIndex, &wAnchorIndex, &wPeakListLen); // 检查第一径与锚点间的关联性 memcpy(awOriginalList, awPeakList, sizeof(uint16) * wPeakListLen); wOriginalListLen = wPeakListLen; lct_firstpath_check_first_path_contact(pfLocationData, awPeakList, wPeakListLen, awOriginalList, wOriginalListLen, wAnchorIndex, wMaxValueIndex, &wFirstPathIndex); // 保存输出参数 pstFirstPath->wFirstPathIndex = wFirstPathIndex; pstFirstPath->fSnr = pfLocationData[wFirstPathIndex] / fNoise; return; } // 计算帧内最大值索引、最小值索引、噪声和信噪比 // pdwData:定位数据 // wAheadFrameFirstPath:上一次第一径帧内索引 // pwMaxValueIndex:帧内最大值索引(返回参数) // pwMinValueIndex:帧内最小值索引(返回参数) // pdwNose:帧内噪声(返回参数) // pfSnr:帧内信噪比(返回参数) void lct_firstpath_cal_frame_max_min_value_index_noise_snr(flt32 *pfData, uint16 wAheadFrameFirstPath, uint16 *pwMaxValueIndex, uint16 *pwMinValueIndex, flt32 *pfNose, flt32 *pfSnr) { flt32 fMaxValue = 0; uint16 wMaxValueIndex = 0; // 最大值索引 flt32 *pfDataHead = NULL; flt32 fTmpValue = 0; flt32 fMinValue = FLT_MAX; // FLT_MAX在系统库中有定义，最大值 uint16 wMinValueIndex = 0; flt32 afDataNoiseList[PUB_SLOT_FRAME_LEN] = {0}; flt32 fMiddleValue = 0; flt32 fNoiseHightScale = (flt32)gg_stFirstPathParam.byNoiseHightScale / 10; flt32 fLeftAverageValue = 0; flt32 fNoise = 0; uint16 wStartIndex = 0; //// 1、计算最大值索引 if (wAheadFrameFirstPath > 1500) { wStartIndex = wAheadFrameFirstPath - 1500; for (uint16 k = wStartIndex; k < PUB_SLOT_FRAME_LEN; k++) { if (pfData[k] > fMaxValue) { fMaxValue = pfData[k]; wMaxValueIndex = k; } } } else { wMaxValueIndex = util_looplist_find_max_value_index_flt32(pfData, PUB_SLOT_FRAME_LEN); fMaxValue = pfData[wMaxValueIndex]; } ////2、在最大值左侧找一个最小值 wStartIndex = wMaxValueIndex > 1000 ? 1000 : 0; pfDataHead = pfData; for (uint16 i = wStartIndex; i < wMaxValueIndex; i++) { fTmpValue = *pfDataHead; if (fTmpValue < fMinValue) { fMinValue = fTmpValue; wMinValueIndex = i; } pfDataHead++; } *pwMinValueIndex = wMinValueIndex; fMinValue *= fNoiseHightScale; ////3、 计算最小值左右两侧均值(左侧有可能包含泄露噪声) wMinValueIndex += 501; for (uint16 k = 0; k < wMinValueIndex; k++) { fLeftAverageValue += pfData[k] / wMinValueIndex; afDataNoiseList[k] = pfData[k]; } ////4、 计算中位数 util_sort_flt32_asc(afDataNoiseList, wMinValueIndex); fMiddleValue = afDataNoiseList[wMinValueIndex * 65 / 100]; ////5、计算帧内噪声 fNoise = fLeftAverageValue > fMinValue ? fLeftAverageValue : fMinValue; fNoise = fMiddleValue > fNoise ? fMiddleValue : fNoise; ////6、保存参数 *pwMaxValueIndex = wMaxValueIndex; *pfNose = fNoise; *pfSnr = fMaxValue / fNoise; return; } // 计算噪声、信噪比和峰值门限 // fNoise:噪声 // fSnr:信噪比 // pfPeakThreshold(返回参数) void lct_firstpath_cal_peak_threshold(flt32 fNoise, flt32 fSnr, flt32 *pfPeakThreshold) { uint8 byNoiseSignalFactor = gg_stFirstPathParam.byNoiseSignalFactor; flt32 fNoiseLowScale = (flt32)gg_stFirstPathParam.byNoiseLowScale / 10; flt32 fNoiseShapeSinrRatio = 1; flt32 fThresholdSnr = 0; // 门限snr flt32 fPeakThreshold = 0; // 峰值门限 // 计算峰值门限 if (fSnr < 10) { fThresholdSnr = fSnr > fNoiseLowScale ? fSnr : fNoiseLowScale; fNoiseShapeSinrRatio += (10 - fThresholdSnr) / (flt32)byNoiseSignalFactor; } fPeakThreshold = ((flt32)fNoiseLowScale / fNoiseShapeSinrRatio) * fNoise; *pfPeakThreshold = (uint32)fPeakThreshold; // 选峰门限 return; } // 选最大值的左侧峰 // pfData:定位数据 // fPeakThreshold:选峰门限 // wMinValueIndex:最小值索引 // wMaxValueIndex：最大值索引 // pwPeakList：峰列表（返回参数） // pwPeakListLen：峰列表长度（返回参数） void lct_firstpath_select_left_peak(flt32 *pfData, flt32 fPeakThreshold, uint16 wMinValueIndex, uint16 wMaxValueIndex, uint16 *pwPeakList, uint16 *pwPeakListLen) { flt32 fLeftData1 = 0; flt32 fLeftData2 = 0; flt32 fLeftData3 = 0; flt32 fCenterData = 0; // 峰值 flt32 fRightData1 = 0; flt32 fRightData2 = 0; flt32 fRightData3 = 0; uint16 wAheadPeakIndex = 0; // 前一个峰值索引 flt32 fAheadPeakValue = 0; // 前一个峰值值 uint16 wPeakPeakDistance = 0; // 峰间距 uint8 byDistanceGap = gg_stFirstPathParam.byPeakPeakDistance; // 选峰间隔 uint16 wCounter = 0; uint16 wTmpIndex = 0; // 从最大峰向左选峰 fAheadPeakValue = pfData[wMaxValueIndex]; wAheadPeakIndex = wMaxValueIndex; for (int16 i = (wMaxValueIndex - byDistanceGap); i > wMinValueIndex; i--) { if ((i - 3) >= 0) { fLeftData3 = pfData[i - 3]; fLeftData2 = pfData[i - 2]; fLeftData1 = pfData[i - 1]; fCenterData = pfData[i]; fRightData1 = pfData[i + 1]; fRightData2 = pfData[i + 2]; fRightData3 = pfData[i + 3]; wPeakPeakDistance = wAheadPeakIndex - i; // 峰间距 if (fCenterData > fPeakThreshold && fCenterData > fLeftData1 && fCenterData > fLeftData2 && fCenterData > fLeftData3 && fCenterData > fRightData1 && fCenterData > fRightData2 && fCenterData > fRightData3 && fCenterData > 0.15 * fAheadPeakValue && wPeakPeakDistance < 1000) { fAheadPeakValue = fCenterData; wAheadPeakIndex = i; pwPeakList[wCounter] = i; i -= (byDistanceGap + 1); // 调整峰值索引到下一个峰开始位置 wCounter++; } } } // 保存最大峰值索引 pwPeakList[wCounter] = wMaxValueIndex; // 最大值是锚点索引或者前一次第一径索引 wCounter++; // 对峰值索引队列重新排序(从小到大) for (uint16 k = 0; k < wCounter; k++) { for (uint16 m = k + 1; m < wCounter; m++) { if (pwPeakList[m] < pwPeakList[k]) { wTmpIndex = pwPeakList[k]; pwPeakList[k] = pwPeakList[m]; pwPeakList[m] = wTmpIndex; } } } // 保存峰值列表长度 *pwPeakListLen = wCounter; return; } // 选跟踪峰 // pfData:定位数据 // wMaxValueIndex：最大值索引 // wTrackIndex:跟踪径索引 // pwPeakList：峰列表（返回参数） // pwPeakListLen：峰列表长度（返回参数） void lct_firstpath_sel_track_peak(flt32 *pfData, uint16 wMaxValueIndex, uint16 wTrackIndex, uint16 *pwPeakList, uint16 *pwPeakListLen) { uint16 wStartIndex = 0; uint16 wStopIndex = 0; uint16 wGuardWidth = gg_stFirstPathParam.wFirstPathTrackPeakGuardWidth; // 跟踪峰保护带宽 uint16 wTrackWindowWidth = gg_stFirstPathParam.wFirstPathTrackWindowWidth; // 跟踪窗口宽度 flt32 fLeftData1 = 0; flt32 fLeftData2 = 0; flt32 fLeftData3 = 0; flt32 fCenterData = 0; // 峰值 flt32 fRightData1 = 0; flt32 fRightData2 = 0; flt32 fRightData3 = 0; uint8 byDistance = gg_stFirstPathParam.byPeakPeakDistance; uint16 wCounter = 0; uint16 wTmpIndex = 0; // 计算左侧选峰结束索引 if ((wTrackIndex - wGuardWidth - wTrackWindowWidth) > 0) { wStartIndex = wTrackIndex - wGuardWidth - wTrackWindowWidth; } else { wStartIndex = 0; } // 从跟踪索引向左选峰 for (int16 i = wTrackIndex; i > wStartIndex; i--) { if ((i - 3) >= 0 && (i + 3) < PUB_SLOT_FRAME_LEN) { fLeftData3 = pfData[i - 3]; fLeftData2 = pfData[i - 2]; fLeftData1 = pfData[i - 1]; fCenterData = pfData[i]; fRightData1 = pfData[i + 1]; fRightData2 = pfData[i + 2]; fRightData3 = pfData[i + 3]; if (fCenterData > fLeftData1 && fCenterData > fLeftData2 && fCenterData > fLeftData3 && fCenterData > fRightData1 && fCenterData > fRightData2 && fCenterData > fRightData3) { pwPeakList[wCounter] = i; i -= (byDistance + 1); // 调整峰值索引到下一个峰开始位置 wCounter++; } } } // 计算向右侧选峰的结束索引 if ((wTrackIndex + wGuardWidth + wTrackWindowWidth + 1) < PUB_SLOT_FRAME_LEN) { wStopIndex = wTrackIndex + wGuardWidth + wTrackWindowWidth + 1; } else { wStopIndex = PUB_SLOT_FRAME_LEN; } // 向右侧选峰 for (int16 j = (wTrackIndex + byDistance); j < wStopIndex; j++) { if ((j + 3) < PUB_SLOT_FRAME_LEN) { fLeftData3 = pfData[j - 3]; fLeftData2 = pfData[j - 2]; fLeftData1 = pfData[j - 1]; fCenterData = pfData[j]; fRightData1 = pfData[j + 1]; fRightData2 = pfData[j + 2]; fRightData3 = pfData[j + 3]; if (fCenterData > fLeftData1 && fCenterData > fLeftData2 && fCenterData > fLeftData3 && fCenterData > fRightData1 && fCenterData > fRightData2 && fCenterData > fRightData3) { pwPeakList[wCounter] = j; j += (byDistance - 1); // 调整峰值索引到下一个峰左侧侧位置 wCounter++; } } } // 对峰值列表重新排序(从小到大) for (uint16 k = 0; k < wCounter; k++) { for (uint16 m = k + 1; m < wCounter; m++) { if (pwPeakList[m] < pwPeakList[k]) { wTmpIndex = pwPeakList[k]; pwPeakList[k] = pwPeakList[m]; pwPeakList[m] = wTmpIndex; } } } // 过滤在最大值索引右侧的峰，包括最大值 for (uint16 p = 0; p < wCounter; p++) { wTmpIndex = pwPeakList[p]; if (wTmpIndex >= wMaxValueIndex) { wCounter = p; break; } } // 保存峰值列表长度 *pwPeakListLen = wCounter; return; } // 计算锚点索引 // pwPeakList:峰队列 // wPeakListLen:峰队列长度 // pfData:定位数据 // fSnr：本帧的信噪比 // pwAnchorIndex：锚点索引（返回参数） void lct_firstpath_cal_anchor_index(uint16 *pwPeakList, uint16 wPeakListLen, flt32 *pfData, flt32 fSnr, uint16 *pwAnchorIndex) { flt32 fMaxValue = 0; uint16 wMaxValueIndex = 0; uint8 byAnchorFactor = gg_stFirstPathParam.byAnchorFactor; // 锚点因子 flt32 fAnchorThreshold = ((flt32)gg_stFirstPathParam.byAnchorThreshold) / 100; // 锚点默认门限值 uint16 wAnchorIndex = 0; // 锚点索引 uint16 wTempIndex = 0; uint32 fTempValue = 0; uint8 byExistFlag = 0; // 计算锚点门限值 wMaxValueIndex = pwPeakList[wPeakListLen - 1]; fMaxValue = pfData[wMaxValueIndex]; // 信噪比较小，则抬升锚点门限 if (fSnr < 10) { fSnr = fSnr > 1.5 ? fSnr : 1.5; fAnchorThreshold += (10 - fSnr) / byAnchorFactor; // 抬升锚点门限 } // 计算锚点门限 if (fMaxValue < 5000) { fAnchorThreshold = 0.45 * fMaxValue; } else { fAnchorThreshold = fMaxValue * fAnchorThreshold; } // 计算锚点索引 if (wPeakListLen <= 1) { // 只有一个峰 *pwAnchorIndex = pwPeakList[0]; } else { // 有多个峰，则从多个峰中选择锚点 for (int16 i = 0; i < wPeakListLen; i++) { wTempIndex = pwPeakList[i]; fTempValue = pfData[wTempIndex]; if (fTempValue > fAnchorThreshold) { wAnchorIndex = wTempIndex; byExistFlag = 1; // 找到了 break; } } if (byExistFlag) { // 找到了锚点 *pwAnchorIndex = wAnchorIndex; } else { // 未找到锚点，最大峰为锚点 *pwAnchorIndex = pwPeakList[wPeakListLen - 1]; } } return; } // 计算二次选峰门限 // pfLocationData:定位数据 // wMaxValueIndex:帧内最大值索引 // wAnchorIndex:帧内锚点索引 // fThreshold:一次选峰门限 // 返回值：二次选峰门限 flt32 lct_firstpath_recal_peak_value_threshold(flt32 *pfLocationData, uint16 wMaxValueIndex, uint16 wAnchorIndex, flt32 fThreshold) { int16 wIndexGap = 0; flt32 fAnchorValue = 0; flt32 fOutThreshold = fThreshold; fAnchorValue = pfLocationData[wAnchorIndex]; // 锚点值 // 根据最大峰和锚点间距调整二次选峰门限 if (wAnchorIndex != wMaxValueIndex) { wIndexGap = wMaxValueIndex - wAnchorIndex; // 严重多径 if (wIndexGap > 2000) { if (0.35 * fAnchorValue > fThreshold) { fOutThreshold = 0.35 * fAnchorValue; } } else if (wIndexGap > 1500) { if (0.30 * fAnchorValue > fThreshold) { fOutThreshold = 0.30 * fAnchorValue; } } else if (wIndexGap > 1000) { if (0.25 * fAnchorValue > fThreshold) { fOutThreshold = 0.25 * fAnchorValue; } } else { if (0.15 * fAnchorValue > fThreshold) { fOutThreshold = 0.15 * fAnchorValue; } } } // 根据锚点幅度再次调整门限(小信号) if (fAnchorValue < 3000) { fOutThreshold = 0.35 * fAnchorValue; } return fOutThreshold; } // 根据锚点二次选锋，只选锚点左侧的峰 // pwPeakList:峰值索引列表 // wPeakListLen:峰值索引列表长度 // wAnchorIndex：锚点 // pwPeakIndexListLen:峰值索引列表长度(返回参数) // 返回值，1：成功，0：失败 void lct_firstpath_select_peak_by_anchor(uint16 *pwPeakList, uint16 wPeakListLen, uint16 wAnchorIndex, flt32 *pfData, flt32 fThresholdWithAnchor, uint16 *pwPeakListLen) { uint16 wCounter = 0; uint16 wTempIndex = 0; flt32 fPeakValue = 0; // 选锋 for (uint16 i = 0; i < wPeakListLen; i++) { wTempIndex = pwPeakList[i]; fPeakValue = pfData[wTempIndex]; if (fPeakValue > fThresholdWithAnchor && wTempIndex <= wAnchorIndex) { pwPeakList[wCounter] = wTempIndex; wCounter++; } } // 保存最大值 if (wAnchorIndex < pwPeakList[wPeakListLen - 1]) { pwPeakList[wCounter] = pwPeakList[wPeakListLen - 1]; wCounter++; } // 队列长度 *pwPeakListLen = wCounter; return; } // 通过噪声门限计算第一径点集 // pfData:定位数据 // wMaxValueIndex:最大值索引 // fPeakValueThreshold:选峰门限 // pwPeakList:峰值索引列表 // wPeakListLen:峰值索引列表长度 // byTrackFlag:跟踪标志（0：不跟踪，1：跟踪） // pwFirstPathLen:第一径长度(返回参数) void lct_firstpath_cal_first_path_by_peak_value_threshold(flt32 *pfData, uint16 wMaxValueIndex, flt32 fPeakValueThreshold, uint16 *pwPeakList, uint16 wPeakListLen, uint8 byTrackFlag, uint16 *pwFirstPathLen) { uint16 awList[PUB_SLOT_FRAME_LEN]; uint16 wPeakIndex = 0; flt32 fTmpValue = 0; uint16 wCounter = 0; memset(awList, 0, sizeof(uint16) * PUB_SLOT_FRAME_LEN); // 遍历峰值索引队列，精选第一径 for (uint16 i = 0; i < wPeakListLen; i++) { wPeakIndex = pwPeakList[i]; fTmpValue = pfData[wPeakIndex]; if (fTmpValue > fPeakValueThreshold) { pwPeakList[wCounter] = wPeakIndex; wCounter++; } } // 存在第一径 if (wCounter > 0) { *pwFirstPathLen = wCounter; // 更新队列长度 } // 不存在第一径 else { // 不跟踪第一径 if (byTrackFlag <= 0) { pwPeakList[0] = wMaxValueIndex; // 将最大值作为第一径 *pwFirstPathLen = 1; } // 跟踪第一径 else { pwPeakList[0] = PUB_SLOT_FRAME_LEN; // 索引默认值，标识跟踪失败 *pwFirstPathLen = 0; // 标识跟踪失败 } } return; } // 根据CFAR规则计算第一径 // pfData:定位数据 // wMaxValueIndex:最大值索引 // pwPeakList:峰值索引列表（输入输出参数） // wPeakListLen:峰值索引列表长度 // byTrackFlag:跟踪标志（0：不跟踪，1：跟踪） // pwFirstPathLen:第一径长度(返回参数) void lct_firstpath_cal_first_path_by_cfar(flt32 *pfData, uint16 wMaxValueIndex, uint16 *pwPeakList, uint16 wPeakListLen, uint8 byTrackFlag, uint16 *pwFirstPathLen) { // 左侧因子 flt32 fCfarLeftFactor = ((flt32)gg_stFirstPathParam.byCfarLeftFactor) / 100; flt32 fLeftValueThres = 0; // 左侧门限 flt32 fLeftMax = 0; // 左侧最大值 flt32 fLeftTemp = 0; flt32 fSignalValue = 0; // 信号值 uint16 wSignalIndex = 0; // 队列索引 uint8 byErrorFlag = 0; // 是否满足cfar规格检查, 0:满足，1：不满足 uint16 wCounter = 0; // 符合检查规则的峰值计数器 // 遍历第一径队列，以cfar规则检查第一径 for (uint16 k = 0; k < wPeakListLen; k++) { byErrorFlag = 0; wSignalIndex = pwPeakList[k]; // 第一径在信号队列中的索引 // 检查该队列左边是否足够进行cfar判断 if ((wSignalIndex - 2 * PUBLIC_LCT_CFAR_LEN) < 0) { byErrorFlag = 1; } // 左侧满足检测要求 if (!byErrorFlag) { fSignalValue = pfData[wSignalIndex]; // 当前信号值 // 选择左边最大值 fLeftMax = 0; for (uint16 i = (wSignalIndex - 2 * PUBLIC_LCT_CFAR_LEN); i < (wSignalIndex - PUBLIC_LCT_CFAR_LEN); i++) { fLeftTemp = pfData[i]; if (fLeftTemp > fLeftMax) { fLeftMax = fLeftTemp; } } fLeftValueThres = fLeftMax * fCfarLeftFactor; // cfar门限 if (fSignalValue > fLeftValueThres) { pwPeakList[wCounter] = wSignalIndex; wCounter++; } } } // 存在第一径 if (wCounter > 0) { // 将最大值插入第一径队列 if (pwPeakList[wCounter - 1] != wMaxValueIndex) { pwPeakList[wCounter] = wMaxValueIndex; wCounter++; } *pwFirstPathLen = wCounter; // 更新队列长度 } // 不存在第一径 else { // 当前是计算径 if (byTrackFlag <= 0) { pwPeakList[0] = wMaxValueIndex; // 将最大值作为第一径 *pwFirstPathLen = 1; } // 当前是跟踪径 else { pwPeakList[0] = PUB_SLOT_FRAME_LEN; // 索引默认值，标识跟踪失败 *pwFirstPathLen = 0; // 标识跟踪失败 } } return; } // 根据sinc规则计算第一径 // pfData:定位数据 // wMaxValueIndex:最大值索引 // fNoise:噪声 // pwPeakList:峰值索引列表 // wPeakListLen:峰值索引列表长度 // byTrackFlag:跟踪标志（0：不跟踪，1：跟踪） // pwFirstPathLen:第一径长度(返回参数) void lct_firstpath_cal_first_path_by_sinc(flt32 *pfData, uint16 wMaxValueIndex, flt32 fNoise, uint16 *pwPeakList, uint16 wPeakListLen, uint8 byTrackFlag, uint16 *pwFirstPathLen) { flt32 *pfSincDataModel = g_afSincFilterDataList; // sinc模型 uint8 bySincFactor = gg_stFirstPathParam.bySincFactor; // sinc因子 flt32 fSnr = 0; flt32 fSincFactor = 0; // sinc门限因子 flt32 fSincThreshold = 0; // sinc门限 uint16 wSincValueIndex = 0; int16 wPeakPeakGap = 0; // 峰间距 flt32 fPeakValue = 0; // 第一径峰值 uint16 wPeakValuelIndex = 0; // 第一径峰值索引 uint16 wTempIndex = 0; flt32 fTempValue = 0; uint16 awInvalidPeakValueList[PUB_SLOT_FRAME_LEN]; // 无效峰队列 uint16 wInvalidPeakCounter = 0; // 无效峰计算器 uint16 wInvalidPeakValuelIndex = 0; // 无效峰值索引 uint8 byExistFlag = 0; // 存在标识 uint16 wCounter = 0; // 保存sinc门限的测试用变量 flt32 afSincDataTestList[2 * PUB_SLOT_FRAME_LEN + 1]; memset(afSincDataTestList, 0, sizeof(flt32) * (2 * PUB_SLOT_FRAME_LEN + 1)); memset(awInvalidPeakValueList, 0, sizeof(uint16) * PUB_SLOT_FRAME_LEN); // 根据信噪比调整信号门限 // fSnr = (flt32)pfData[wMaxValueIndex] / fNoise; // if (fSnr < 10) // { // bySincFactor = bySincFactor * 0.95; // } // 遍历队列，以sinc规格重新计算第一径 for (uint16 i = 0; i < wPeakListLen; i++) { // 生成sinc门限因子 wPeakValuelIndex = pwPeakList[i]; // 当前峰值索引 fPeakValue = pfData[wPeakValuelIndex]; // 当前峰值 fSincFactor = fPeakValue * bySincFactor / 100; // sinc因子 // 将sinc门限保存到文件 // if (i >= 0) { // // 计算sinc门限索引 // wStartIndex = 16000 - wPeakValuelIndex; // for (uint16 sincIndex = 0; sincIndex < 16000; sincIndex++) { // afSincDataTestList[sincIndex] = pfSincDataModel[wStartIndex] * fSincFactor; // wStartIndex++; // } // // 保存sinc门限到文件 // char filePath[256]; // char fileIndex[8]; // memset(filePath, 0, 256); // memset(fileIndex, 0, 8); // char* fileName = "sincthres"; // itoa(i, fileIndex, 10); // char* fileTail = ".txt"; // char* filePathHead = "E:\\05item\\ips\\location\\output\\sincthres\\"; // strcat(filePath, filePathHead); // strcat(filePath, fileName); // strcat(filePath, fileIndex); // strcat(filePath, fileTail); // util_write_real_number_to_txt_file(filePath, 16000, afSincDataTestList); // } // sinc检查 for (uint16 j = 0; j < wPeakListLen; j++) { wTempIndex = pwPeakList[j]; fTempValue = pfData[wTempIndex]; if (wTempIndex != wPeakValuelIndex) { ////计算sinc滤波器的索引 // 被检查峰在当前峰的左侧 wSincValueIndex = 0; if (wPeakValuelIndex > wTempIndex) { wPeakPeakGap = wPeakValuelIndex - wTempIndex; // 两峰间距 wSincValueIndex = PUB_SLOT_FRAME_LEN - wPeakPeakGap; // // 如果是遮挡峰，则降低主峰左侧700-1200点区间内的检查门限，以正确选出遮挡峰 // if ((wPeakValuelIndex - wTempIndex) > 700 && (wPeakValuelIndex - wTempIndex) <= 1200 && i == (wPeakListLen - 1)) { // fSincThreshold = pfSincDataModel[wSincValueIndex] * fSincFactor * 0.6; // 门限 // } else { // fSincThreshold = pfSincDataModel[wSincValueIndex] * fSincFactor; // 门限 // } } // 被检查峰在当前峰的右侧 else { wPeakPeakGap = wTempIndex - wPeakValuelIndex; // 两峰间距 wSincValueIndex = PUB_SLOT_FRAME_LEN + wPeakPeakGap; // 检查峰在右侧 } // 修正选峰门限。峰间隔超过1500，说明存在严重的多径现象 if (wPeakPeakGap > 1500) { fSincThreshold = pfSincDataModel[wSincValueIndex] * fSincFactor * 0.90; // sinc门限 } else { fSincThreshold = pfSincDataModel[wSincValueIndex] * fSincFactor; // sinc门限 } // 当前峰不合法 if (fTempValue <= fSincThreshold) { awInvalidPeakValueList[wInvalidPeakCounter] = wTempIndex; wInvalidPeakCounter++; } } } } // 存在无效峰 if (wInvalidPeakCounter > 0) { wCounter = 0; // 遍历有效峰队列 for (uint16 k = 0; k < wPeakListLen; k++) { byExistFlag = 0; wPeakValuelIndex = pwPeakList[k]; // 峰值索引; // 遍历无效峰队列 for (uint16 m = 0; m < wInvalidPeakCounter; m++) { wInvalidPeakValuelIndex = awInvalidPeakValueList[m]; if (wPeakValuelIndex == wInvalidPeakValuelIndex) { byExistFlag = 1; break; } } if (!byExistFlag) { // 在无效峰值队列总没有找到，则保存该峰值索引 pwPeakList[wCounter] = wPeakValuelIndex; wCounter++; } } // 更新队列长度 if (wCounter > 0) { *pwFirstPathLen = wCounter; } else { *pwFirstPathLen = 1; if (byTrackFlag == 0) { pwPeakList[0] = wMaxValueIndex; // 最大值索引 } else if (byTrackFlag == 1) { // 跟踪径计算 pwPeakList[0] = 0; // 索引默认值，标识跟踪失败 } } } // 没有无效峰 else { *pwFirstPathLen = wPeakListLen; } return; } // 检查锚点是否是合法峰 // pwPeakList:峰值索引队列 // wPeakListLen:峰值索引队列长度 // wAnchorIndex：锚点索引 // wMaxValueIndex：最大值索引 // pwAnchorIndex：锚点索引，返回值 // 返回值 void lct_firstpath_check_anchor_index(uint16 *pwPeakList, uint16 wPeakListLen, uint16 wAnchorIndex, uint16 wMaxValueIndex, uint16 *pwAnchorIndex, uint16 *pwPeakListLen) { uint16 wTempIndex = 0; uint8 byExistFlag = 0; // int32 dwGap = 0; uint16 wCounter = wPeakListLen; uint16 wTemp = 0; // 遍历队列检查峰值索引是否包含锚点 for (uint16 i = 0; i < wPeakListLen; i++) { wTempIndex = pwPeakList[i]; if (wTempIndex == wAnchorIndex) { byExistFlag = 1; break; } } // // 锚点在峰值索引队列中 // if (byExistFlag) // { // *pwAnchorIndex = wAnchorIndex; // } // // 锚点不在峰值索引队列中 // else // { // dwGap = abs(wMaxValueIndex - wAnchorIndex); // // 相差了3.5米 // if (dwGap > 500) // { // *pwAnchorIndex = wAnchorIndex; // 最大值与锚点间间隔大于440 // // 把锚点重新加入队列 // pwPeakList[wPeakListLen] = wAnchorIndex; // wCounter++; // // 对峰值列表重新排序(从小到大) // for (uint16 k = 0; k < wCounter; k++) // { // for (uint16 m = k + 1; m < wCounter; m++) // { // if (pwPeakList[m] < pwPeakList[k]) // { // wTemp = pwPeakList[k]; // pwPeakList[k] = pwPeakList[m]; // pwPeakList[m] = wTemp; // } // } // } // } // // 相差不到3米 // else // { // *pwAnchorIndex = wMaxValueIndex; // 最大值索引当成锚点 // } // } // 锚点不在峰值队列中 if (!byExistFlag) { // 把锚点重新加入队列 pwPeakList[wPeakListLen] = wAnchorIndex; wCounter++; // 对峰值队列重新排序(从小到大) for (uint16 k = 0; k < wCounter; k++) { for (uint16 m = k + 1; m < wCounter; m++) { if (pwPeakList[m] < pwPeakList[k]) { wTemp = pwPeakList[k]; pwPeakList[k] = pwPeakList[m]; pwPeakList[m] = wTemp; } } } } *pwAnchorIndex = wAnchorIndex; *pwPeakListLen = wCounter; return; } // 检测第一径各峰是否能关联到锚点 // pdwData:定位数据 // pwPeakList:峰值队列 // wPeakListLen:峰值队列长度 // pwOriginalList:原始峰值队列 // wOriginalListLen：原始峰值队列长度 // wAnchorIndex:锚点 // wMaxValueIndex:最大值索引 // pwFirstPathIndex:第一径索引 // 返回值，1：成功，0：失败 void lct_firstpath_check_first_path_contact(flt32 *pfData, uint16 *pwPeakList, uint16 wPeakListLen, uint16 *pwOriginalList, uint16 wOriginalListLen, uint16 wAnchorIndex, uint16 wMaxValueIndex, uint16 *pwFirstPathIndex) { uint8 byContactFactor = gg_stFirstPathParam.byFirstPathContactFactor; uint16 wContactDistance = gg_stFirstPathParam.wFirstPathContactDistance; uint16 wPeakValueIndex = 0; // 峰值索引 // 第一峰 flt32 fFirstpeakValue = 0; uint16 wFirstPeakIndex = 0; // 第二峰 flt32 fSecondPeakValue = 0; uint16 wSecondPeakIndex = 0; flt32 fSecondpeakThreshold = 0; // 选择第二峰的门限 uint8 bySelectFlag = 0; // 关联锚点标识，1：关联成功，0：关联失败 // 依次检查第一径是否能关联到锚点 if (wPeakListLen > 1) { // 有两个点或以上 for (uint16 i = 0; i < wPeakListLen - 1; i++) { wPeakValueIndex = pwPeakList[i]; wFirstPeakIndex = pwPeakList[i]; fFirstpeakValue = pfData[wFirstPeakIndex]; // 第一峰峰值 fSecondpeakThreshold = (fFirstpeakValue / 100) * byContactFactor; for (uint16 k = 0; k < wOriginalListLen; k++) { wSecondPeakIndex = pwOriginalList[k]; if (wSecondPeakIndex > wFirstPeakIndex) { fSecondPeakValue = pfData[wSecondPeakIndex]; // 第二峰峰值 // 第二峰有效 if (fSecondPeakValue > fSecondpeakThreshold) { // 第二峰与第一峰间的距离“满足”系统要求 if ((wSecondPeakIndex - wFirstPeakIndex) <= wContactDistance) { // 关联成功 if ((wSecondPeakIndex == wAnchorIndex) || (wSecondPeakIndex == wMaxValueIndex)) { *pwFirstPathIndex = wPeakValueIndex; // 保存真正的第一径索引 bySelectFlag = 1; break; } else { // 关联失败，继续向后查找 wFirstPeakIndex = wSecondPeakIndex; fFirstpeakValue = fSecondPeakValue; fSecondpeakThreshold = (fFirstpeakValue / 100) * byContactFactor; } } else { // 第二峰与第一峰间的距离“不满足”系统要求 bySelectFlag = 0; break; } } } } if (bySelectFlag) { break; } } if (!bySelectFlag) { // 没有关联成功 *pwFirstPathIndex = wAnchorIndex; // 锚点为第一径 } } else { // 只有一个峰 *pwFirstPathIndex = pwPeakList[wPeakListLen - 1]; } return; } /******************************************************上面是计算径*********************************************/ /******************************************************下面是跟踪径*********************************************/ // 计算"跟踪径" // pfData:原始信号 // wTrackIndex:跟踪索引 // fNoise:噪声 // fSelPeakThreshold:选峰门限 // wAnchorIndex:锚点索引 // pstTrackPath：跟踪径(返回参数) void lct_firstpath_cal_trackpath(flt32 *pfData, uint16 wTrackIndex, flt32 fNoise, flt32 fSelPeakThreshold, uint16 wAnchorIndex, uint16 wMaxValueIndex, FIRST_PATH_T *pstTrackPath) { uint16 awPeakList[2 * PUBLIC_LCT_TRACK_WINDOW_WIDTH + 1]; // 峰值索引队列 uint16 wPeakListLen = 0; // 峰值索引队列长度 memset(awPeakList, 0, sizeof(uint16) * (2 * PUBLIC_LCT_TRACK_WINDOW_WIDTH + 1)); // 选跟踪峰，不超过最大峰值索引 lct_firstpath_sel_track_peak(pfData, wMaxValueIndex, wTrackIndex, awPeakList, &wPeakListLen); // 补偿峰值增益 lct_firstpath_compensate_peak_gain(pfData, awPeakList, wPeakListLen); // 将最大值添加到峰值队列中 awPeakList[wPeakListLen] = wMaxValueIndex; wPeakListLen++; //// 基于门限规则计算第一径 lct_firstpath_cal_first_path_by_peak_value_threshold(pfData, wMaxValueIndex, fSelPeakThreshold, awPeakList, wPeakListLen, 1, &wPeakListLen); // 基于CFAR规则检查第一径 lct_firstpath_cal_first_path_by_cfar(pfData, wMaxValueIndex, awPeakList, wPeakListLen, 1, &wPeakListLen); // 基于SINC规则检查第一径 lct_firstpath_cal_first_path_by_sinc(pfData, wMaxValueIndex, fNoise, awPeakList, wPeakListLen, 1, &wPeakListLen); // 跟踪失败 if (wPeakListLen <= 1) { pstTrackPath->byTrackSuccessFlag = 0; } // 跟踪径成功 else { pstTrackPath->byTrackSuccessFlag = 1; pstTrackPath->wFirstPathIndex = awPeakList[0]; pstTrackPath->fSnr = pfData[awPeakList[0]] / fNoise; } return; } // // 选择跟踪窗口中的峰 // // pfData:定位数据 // // fPeakValueThreshold:选峰门限 // // wTrackIndex:跟踪索引 // // wMaxValueIndex:最大值索引 // // pwPeakList:峰值索引队列 // // pwPeakListLen：峰值索引队列长度 // void lct_firstpath_trackpath_select_peak(flt32 *pfData, flt32 fPeakValueThreshold, uint16 wTrackIndex, uint16 wMaxValueIndex, uint16 *pwPeakList, uint16 *pwPeakListLen) // { // uint16 wGuardWidth = gg_stFirstPathParam.wFirstPathTrackPeakGuardWidth; // 跟踪峰保护带宽 // uint16 wTrackWindowWidth = gg_stFirstPathParam.wFirstPathTrackWindowWidth; // 跟踪窗口宽度 // uint16 wStartIndex = 0; // uint16 wStopIndex = 0; // uint16 wListLen = 0; // // 计算开始引 // if ((wTrackIndex - wGuardWidth - wTrackWindowWidth) >= 0) // { // wStartIndex = wTrackIndex - wGuardWidth - wTrackWindowWidth; // } // else // { // wStartIndex = 0; // } // // 计算结束索引 // if ((wTrackIndex + wGuardWidth + wTrackWindowWidth + 1) < PUB_SLOT_FRAME_LEN) // { // wStopIndex = wTrackIndex + wGuardWidth + wTrackWindowWidth + 1; // } // else // { // wStopIndex = PUB_SLOT_FRAME_LEN; // } // // 选左侧峰 // // 选右侧峰 // // 从定位数据中，在跟踪径左右两侧选峰 // lct_firstpath_select_peak(pfData, fPeakValueThreshold, wMaxValueIndex, 1, wTrackIndex, wStartIndex, wStopIndex, pwPeakList, &wListLen); // // 跟踪径两侧未选到峰 // if (!wListLen) // { // pwPeakList[0] = wTrackIndex; // wListLen++; // } // *pwPeakListLen = wListLen; // return; // } // 补偿跟踪窗口内的峰值增益 // pfData:定位数据 // pwPeakList:峰值索引队列 // pwPeakListLen：峰值索引队列长度 void lct_firstpath_compensate_peak_gain(flt32 *pfData, uint16 *pwPeakList, uint16 wPeakListLen) { uint16 wGuardWidth = gg_stFirstPathParam.wFirstPathTrackPeakGuardWidth; // 跟踪峰保护带宽 uint16 wFindWidth = gg_stFirstPathParam.wFirstPathTrackNoiseFindWidth; // 补偿增益时噪声选择窗口宽度 uint8 byPeakGainFactor = gg_stFirstPathParam.byFirstPathTrackPeakGainFactor; // 增益补偿因子 flt32 fPeakRatio = 0; // 峰值比例 uint16 wPeakValueIndex = 0; flt32 fPeakValue = 0; uint16 wStartIndex = 0; uint16 wStopIndex = 0; flt32 fNoiseValue = 0; flt32 fTmpValue = 0; // 遍历峰值队列，计算峰值补偿增益 for (uint16 i = 0; i < wPeakListLen; i++) { wPeakValueIndex = pwPeakList[i]; fPeakValue = pfData[wPeakValueIndex]; fNoiseValue = 0; // 计算开始索引 if ((wPeakValueIndex - wGuardWidth - wFindWidth) >= 0) { wStartIndex = wPeakValueIndex - wGuardWidth - wFindWidth; } else { wStartIndex = 0; } // 计算结束索引 if ((wStartIndex + wFindWidth) <= (wPeakValueIndex - wGuardWidth)) { wStopIndex = wStartIndex + wFindWidth; } else { wStopIndex = wPeakValueIndex - wGuardWidth; } // 查找噪声(wStartIndex 到 wStopIndex范围内的最大值) for (uint16 i = wStartIndex; i < wStopIndex; i++) { fTmpValue = pfData[i]; if (fTmpValue > fNoiseValue) { fNoiseValue = fTmpValue; } } // 计算增益因子 fPeakRatio = (fPeakValue / fNoiseValue * byPeakGainFactor) / 100; if (fPeakRatio > 1.2) { fPeakRatio = 1.2; } else if (fPeakRatio < 0.5) { fPeakRatio = 0.5; } // 补偿增益 pfData[wPeakValueIndex] = fPeakValue * fPeakRatio; } return; } // 镜像当前第一径数据，以便一下次计算时使用 void lct_firstpath_mirror_first_path_data(LCT_FIRSTPATH_LIST_T *pstFirstPathList, LCT_FIRSTPATH_LIST_T *pstAheadFirstPathList) { uint8 byCounter = pstFirstPathList->byBtsNum; // 镜像第一径数据 for (uint8 i = 0; i < byCounter; i++) { memcpy(&pstAheadFirstPathList->astFirstPath[i], &pstFirstPathList->astFirstPath[i], sizeof(LCT_FIRSTPATH_T)); } pstAheadFirstPathList->byBtsNum = byCounter; return; } // 反向镜像第一径数据（用前一次的第一径数据填充当前一次第一径数据） void lct_firstpath_demirror_first_path_data(LCT_FIRSTPATH_LIST_T *pstFirstPathList, LCT_FIRSTPATH_LIST_T *pstAheadFirstPathList) { uint8 byCounter = 0; byCounter = pstAheadFirstPathList->byBtsNum; // 镜像第一径数据 for (uint8 i = 0; i < byCounter; i++) { memcpy(&pstFirstPathList->astFirstPath[i], &pstAheadFirstPathList->astFirstPath[i], sizeof(LCT_FIRSTPATH_T)); pstFirstPathList->astFirstPath[i].dwCalPathIndex += PUB_SIGNAL_SAMPLE_RATIO; pstFirstPathList->astFirstPath[i].dwTrackPathIndex += PUB_SIGNAL_SAMPLE_RATIO; } pstFirstPathList->byBtsNum = byCounter; return; } // 从上一次第一径数据中获得当前需要跟踪的索引 uint16 lct_firstpath_get_trackpath(LCT_FIRSTPATH_LIST_T *pstAheadFirstPathList, uint32 dwBuildId, uint32 dwLayerId, uint32 dwBtsId, uint8 byFreqIndex, uint8 bySlot) { uint8 byCounter = 0; uint32 dwTmpBuildId = 0; uint32 dwTmpLayerId = 0; uint32 dwTmpBtsId = 0; uint8 byTmpFreqIndex = 0; uint8 byTmpSlot = 0; uint16 wTrackIndex = 0; byCounter = pstAheadFirstPathList->byBtsNum; for (uint8 i = 0; i < byCounter; i++) { dwTmpBuildId = pstAheadFirstPathList->astFirstPath[i].dwBuildId; dwTmpLayerId = pstAheadFirstPathList->astFirstPath[i].dwLayerId; dwTmpBtsId = pstAheadFirstPathList->astFirstPath[i].dwBtsId; byTmpFreqIndex = pstAheadFirstPathList->astFirstPath[i].byFreqIndex; byTmpSlot = pstAheadFirstPathList->astFirstPath[i].bySlot; // 上一次第一径参数中有该基站 if ((dwTmpBuildId == dwBuildId) && (dwTmpLayerId == dwLayerId) && (dwTmpBtsId == dwBtsId) && (byTmpFreqIndex == byFreqIndex) && (byTmpSlot == bySlot)) { if (pstAheadFirstPathList->astFirstPath[i].wFrameTrackIndex) { wTrackIndex = pstAheadFirstPathList->astFirstPath[i].wFrameTrackIndex; // 上一次帧内跟踪索引 } else { wTrackIndex = pstAheadFirstPathList->astFirstPath[i].wFrameCalIndex; // 上一次帧内计算索引 } break; } } return wTrackIndex; } // 拷贝前一次第一径数据，本次bit表征值为0，则沿用前一次的第一径数据 // pstFirstPath:第一径数据 // pstAheadFirstPathList:前一次第一径数据 // dwSyncOffset:同步偏移 void lct_firstpath_copy_ahead_first_path(LCT_FIRSTPATH_T *pstFirstPath, uint32 dwLocationIndex, uint8 byCalSlot, LCT_FIRSTPATH_LIST_T *pstAheadFirstPathList) { uint8 byBtsNum = 0; LCT_FIRSTPATH_T stAheadFirstPath = {0}; byBtsNum = pstAheadFirstPathList->byBtsNum; for (uint8 i = 0; i < byBtsNum; i++) { stAheadFirstPath = pstAheadFirstPathList->astFirstPath[i]; if (pstFirstPath->dwBuildId == stAheadFirstPath.dwBuildId && pstFirstPath->dwLayerId == stAheadFirstPath.dwLayerId && pstFirstPath->dwBtsId == stAheadFirstPath.dwBtsId && pstFirstPath->byFreqIndex == stAheadFirstPath.byFreqIndex && pstFirstPath->bySlot == stAheadFirstPath.bySlot) { // 跟踪径有效，则用跟踪径 if (stAheadFirstPath.wFrameTrackIndex > 0) { pstFirstPath->wFrameCalIndex = stAheadFirstPath.wFrameTrackIndex; pstFirstPath->wFrameTrackIndex = stAheadFirstPath.wFrameTrackIndex; pstFirstPath->dwCalPathIndex = dwLocationIndex + byCalSlot * PUB_SLOT_FRAME_LEN + stAheadFirstPath.wFrameTrackIndex; pstFirstPath->dwTrackPathIndex = dwLocationIndex + byCalSlot * PUB_SLOT_FRAME_LEN + stAheadFirstPath.wFrameTrackIndex; if (stAheadFirstPath.fTrackPathIndexSnr <= 0) { pstFirstPath->fCalPathIndexSnr = 3.5; pstFirstPath->fTrackPathIndexSnr = 3.5; } else { pstFirstPath->fCalPathIndexSnr = stAheadFirstPath.fTrackPathIndexSnr; pstFirstPath->fTrackPathIndexSnr = stAheadFirstPath.fTrackPathIndexSnr; } } // 跟踪径无效，则用计算径 else { pstFirstPath->wFrameCalIndex = stAheadFirstPath.wFrameCalIndex; pstFirstPath->wFrameTrackIndex = stAheadFirstPath.wFrameCalIndex; pstFirstPath->dwCalPathIndex = dwLocationIndex + byCalSlot * PUB_SLOT_FRAME_LEN + stAheadFirstPath.wFrameCalIndex; pstFirstPath->dwTrackPathIndex = dwLocationIndex + byCalSlot * PUB_SLOT_FRAME_LEN + stAheadFirstPath.wFrameCalIndex; if (stAheadFirstPath.fCalPathIndexSnr <= 0) { pstFirstPath->fCalPathIndexSnr = 3.5; pstFirstPath->fTrackPathIndexSnr = 3.5; } else { pstFirstPath->fCalPathIndexSnr = stAheadFirstPath.fCalPathIndexSnr; pstFirstPath->fTrackPathIndexSnr = stAheadFirstPath.fCalPathIndexSnr; } } break; } } } // 所有基站拷贝前一次第一径数据，本次bit表征值为0，则沿用前一次的第一径数据 // pstLocationDataList:定位数据队列 // pstAheadFirstPathList:前一次第一径数据队列 // pstFirstPathList:第一径数据队列(返回参数) void lct_firstpath_copy_ahead_first_path_for_all_bts(PUB_LOCATION_DATA_T *pstLocationDataList, LCT_FIRSTPATH_LIST_T *pstAheadFirstPathList, LCT_FIRSTPATH_LIST_T *pstFirstPathList) { uint8 byBtsNum = 0; LCT_FIRSTPATH_T *pstAheadFirstPath = NULL; LCT_FIRSTPATH_T *pstFirstPath = NULL; PUB_LOCATION_DATA_T *pstLctData = NULL; uint8 byIndex = 0; byBtsNum = pstAheadFirstPathList->byBtsNum; for (uint8 i = 0; i < byBtsNum; i++) { pstAheadFirstPath = &pstAheadFirstPathList->astFirstPath[i]; byIndex = pstAheadFirstPath->byFreqIndex; pstLctData = &pstLocationDataList[byIndex]; pstFirstPath = &pstFirstPathList->astFirstPath[i]; memcpy(pstFirstPath, pstAheadFirstPath, sizeof(LCT_FIRSTPATH_T)); // 跟踪径有效，则用跟踪径 if (pstAheadFirstPath->wFrameTrackIndex > 0) { pstFirstPath->wFrameCalIndex = pstAheadFirstPath->wFrameTrackIndex; pstFirstPath->wFrameTrackIndex = pstAheadFirstPath->wFrameTrackIndex; pstFirstPath->dwCalPathIndex = pstLctData->dwLocationIndex + pstLctData->byCalSlot * PUB_SLOT_FRAME_LEN + pstAheadFirstPath->wFrameTrackIndex; pstFirstPath->dwTrackPathIndex = pstLctData->dwLocationIndex + pstLctData->byCalSlot * PUB_SLOT_FRAME_LEN + pstAheadFirstPath->wFrameTrackIndex; if (pstAheadFirstPath->fTrackPathIndexSnr <= 0) { pstFirstPath->fCalPathIndexSnr = 3.5; pstFirstPath->fTrackPathIndexSnr = 3.5; } else { pstFirstPath->fCalPathIndexSnr = pstAheadFirstPath->fTrackPathIndexSnr; pstFirstPath->fTrackPathIndexSnr = pstAheadFirstPath->fTrackPathIndexSnr; } } // 跟踪径无效，则用计算径 else { pstFirstPath->wFrameCalIndex = pstAheadFirstPath->wFrameCalIndex; pstFirstPath->wFrameTrackIndex = pstAheadFirstPath->wFrameCalIndex; pstFirstPath->dwCalPathIndex = pstLctData->dwLocationIndex + pstLctData->byCalSlot * PUB_SLOT_FRAME_LEN + pstAheadFirstPath->wFrameCalIndex; pstFirstPath->dwTrackPathIndex = pstLctData->dwLocationIndex + pstLctData->byCalSlot * PUB_SLOT_FRAME_LEN + pstAheadFirstPath->wFrameCalIndex; if (pstAheadFirstPath->fCalPathIndexSnr <= 0) { pstFirstPath->fCalPathIndexSnr = 3.5; pstFirstPath->fTrackPathIndexSnr = 3.5; } else { pstFirstPath->fCalPathIndexSnr = pstAheadFirstPath->fCalPathIndexSnr; pstFirstPath->fTrackPathIndexSnr = pstAheadFirstPath->fCalPathIndexSnr; } } } pstFirstPathList->byBtsNum = byBtsNum; return; } // 重新计算绝对索引 uint32 lct_firstpath_recal_absolute_index(uint32 dwOldIndex) { uint32 dwNewIndex = 0; if (dwOldIndex >= 0 && dwOldIndex < PUB_SLOT_FRAME_LEN) { dwNewIndex = dwOldIndex; } else if (dwOldIndex >= PUB_SLOT_FRAME_LEN && dwOldIndex < 2 * PUB_SLOT_FRAME_LEN) { dwNewIndex = dwOldIndex - PUB_SLOT_FRAME_LEN; } else if (dwOldIndex >= 2 * PUB_SLOT_FRAME_LEN && dwOldIndex < 3 * PUB_SLOT_FRAME_LEN) { dwNewIndex = dwOldIndex - 2 * PUB_SLOT_FRAME_LEN; } else if (dwOldIndex >= 3 * PUB_SLOT_FRAME_LEN && dwOldIndex < 4 * PUB_SLOT_FRAME_LEN) { dwNewIndex = dwOldIndex - 3 * PUB_SLOT_FRAME_LEN; } else { dwNewIndex = 0; } return dwNewIndex; } // 复位预测方向数据有效标识符 void lct_firstpath_reset_direction_valid_flag(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList) { LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstData = NULL; for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++) { pstData = &pstPredictionDirectionDataList[i]; pstData->byValidFlag = 0; } return; } // 保存第一径数据 // pstLctData:定位数据 // pstFirstPathData:第一径数据 // pstPredictionDirectionDataList:预测方向数据队列，返回参数 void lct_firstpath_save_first_path_data(PUB_LOCATION_DATA_T *pstLctData, LCT_FIRSTPATH_T *pstFirstPathData, LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList) { uint8 byFreqIndex = 0; LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstResultData = NULL; uint32 dwBackupIndex = 0; byFreqIndex = pstLctData->byFreqIndex; // 频率索引 pstResultData = &pstPredictionDirectionDataList[byFreqIndex]; // 预测方向的数据 dwBackupIndex = pstResultData->dwFirstPathBackupIndex; // 数据备份索引 flt32 fAmpl = 0; if (dwBackupIndex < LCT_FIRST_PATH_BACKUP_NUM) { pstResultData->dwFirstPathBackupIndex++; pstResultData->astFirstPathBackupList[dwBackupIndex].dwFrameNo = pstLctData->dwFrameNo - 1; // 帧号 pstResultData->astFirstPathBackupList[dwBackupIndex].fMaxAmplValue = pstFirstPathData->fMaxAmplValue; // 幅度 pstResultData->astFirstPathBackupList[dwBackupIndex].dwCalPathIndex = pstFirstPathData->dwCalPathIndex; // 计算径 pstResultData->astFirstPathBackupList[dwBackupIndex].dwTrackPathIndex = pstFirstPathData->dwTrackPathIndex; // 跟踪经 } else { // 移动队列 memmove(pstResultData->astFirstPathBackupList, &pstResultData->astFirstPathBackupList[1], sizeof(LCT_FIRSTPATH_BACKUP_T) * (LCT_FIRST_PATH_BACKUP_NUM - 1)); // 保存数据 dwBackupIndex = LCT_FIRST_PATH_BACKUP_NUM - 1; pstResultData->astFirstPathBackupList[dwBackupIndex].dwFrameNo = pstLctData->dwFrameNo - 1; // 帧号 pstResultData->astFirstPathBackupList[dwBackupIndex].fMaxAmplValue = pstFirstPathData->fMaxAmplValue; // 幅度 pstResultData->astFirstPathBackupList[dwBackupIndex].dwCalPathIndex = pstFirstPathData->dwCalPathIndex; // 计算径 pstResultData->astFirstPathBackupList[dwBackupIndex].dwTrackPathIndex = pstFirstPathData->dwTrackPathIndex; // 跟踪经 } pstResultData->dwCoordX = pstFirstPathData->dwCoordX; pstResultData->dwCoordY = pstFirstPathData->dwCoordY; pstResultData->dwCoordZ = pstFirstPathData->dwCoordZ; // 计算径 pstResultData->dwCalPathIndex = pstFirstPathData->dwCalPathIndex; // 有效标识符置1 pstResultData->byValidFlag = 1; } // 预测移动方向 // pstPredictionDirectionDataList:预测方向的数据队列 void lct_firstpath_prediction_move_direction(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList) { LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionData = NULL; uint8 byLen = 0; LCT_FIRSTPATH_BACKUP_T *pstCurrentData = NULL; // 当前数据 LCT_FIRSTPATH_BACKUP_T *pstAheadData = NULL; // 前一帧数据 uint32 dwFrameGap = 0; uint8 byAheadTendency = 0; uint8 byCurrentTendency = 0; for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++) { pstPredictionData = &pstPredictionDirectionDataList[i]; byAheadTendency = pstPredictionData->byDirectionTendency; // 保存上一次趋势 pstPredictionData->byDirectionTendency = 0; // 方向趋势复位 // 遍历方向数据队列 byLen = pstPredictionData->dwFirstPathBackupIndex; if (byLen >= LCT_FIRST_PATH_BACKUP_NUM) { pstCurrentData = &pstPredictionData->astFirstPathBackupList[byLen - 1]; pstAheadData = pstPredictionData->astFirstPathBackupList; dwFrameGap = pstCurrentData->dwFrameNo - pstAheadData->dwFrameNo; if (dwFrameGap == 8) // 数据有效 { // 第一径维度靠近该基站 if ((pstCurrentData->dwCalPathIndex <= pstAheadData->dwCalPathIndex) || (pstCurrentData->dwTrackPathIndex <= pstAheadData->dwTrackPathIndex)) { pstPredictionData->byDirectionTendency = 1; // 靠近 // 上一次是靠近该基站 if (byAheadTendency == 1) { pstPredictionData->byDirectionTendency = 1; // 靠近 } // 上一次是远离该基站 else if (byAheadTendency == 2) { // 幅度维度靠近该基站 if (pstCurrentData->fMaxAmplValue >= (0.85 * pstAheadData->fMaxAmplValue)) { pstPredictionData->byDirectionTendency = 1; // 靠近 } else { pstPredictionData->byDirectionTendency = 2; // 远离 } } } // 第一径维度远离该基站 else { pstPredictionData->byDirectionTendency = 2; // 远离 // 上一次是靠近该基站 if (byAheadTendency == 1) { // 幅度维度靠近该基站 if (pstCurrentData->fMaxAmplValue >= (0.9 * pstAheadData->fMaxAmplValue)) { pstPredictionData->byDirectionTendency = 1; // 靠近 } else { pstPredictionData->byDirectionTendency = 2; // 远离 } } // 上一次是远离该基站 else if (byAheadTendency == 2) { pstPredictionData->byDirectionTendency = 2; // 远离 } } } } } }