aplm8000sdk/src/modules/location/lct_firstpath.c

// 秒寻科技
// 计算第一径数据队列
// 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)
{
    uint8 byLctBtsNum = pstLctBtsList->byBtsNum; // 有效定位基站组的基站个数
    PUB_LCT_BTS_T *pstLctBts = NULL;             // 定位基站
    PUB_LOCATION_DATA_T *pstLctData = NULL;      // 定位数据
    uint8 byFreqIndex = 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->dwFrameNo > 56)
            {
                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;
            }
        }

        // 当前一秒数据有效(byLctBtsFlag是定位基站、byBtsValidFlag定位基站有效(前4秒信号的幅度有效，并且已实现同步)、byCurrentAmplValidFlag当前幅度值有效)
        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->dwFrameNo > 0 && pstFirstPath->wFrameCalIndex > 0)
                    {
                        printf("frame no:%d, freq index:%d, sync index:%d, cal frame index:%d, cal index:%d\n", pstLctData->dwFrameNo - 1, pstFirstPath->byFreqIndex + 1, 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 (pstLctData->dwFrameNo > 0 && pstFirstPath->wFrameTrackIndex > 0)
                {
                    printf("frame no:%d, freq index:%d, sync index:%d, track frame index:%d, track index:%d\n", pstLctData->dwFrameNo - 1, pstFirstPath->byFreqIndex + 1, 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;

    // TODO预测移动方向，放到了计算模块去预测了
    // 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; // 帧内锚点索引

    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;
}

// 复位预测方向数据有效标识符
// pstPredictionDirectionDataList:预测方向数据队列
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;        // 数据备份索引

    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;

    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; // 远离
                    }
                }
            }
        }
    }
}