aplm8000sdk/src/modules/location/lct_main.c

// 秒寻科技
// 定位主控模块
// zt
// 2023-03-25

#include "../../includes/includes.h"
#include "./lct_public.h"

// 定义仅本模块使用的全局变量
LCT_FIRSTPATH_LIST_T gLct_stFirstPathList = {0};                                                          // 第一径数据队列
LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T gLct_astPredictionDirectionDataList[PUB_SYS_CARRIER_NUM] = {0}; // 预测移动方向数据队列
LCT_COORDSEL_ORIGINAL_COORD_T gLct_stOriginalLctCoordList = {0};                                          // 原始定位坐标队列
LCT_COORDSEL_MEAN_COORD_T gLct_stMeanLctCoordList = {0};                                                  // 平滑定位坐标队列
flt32 gLct_afMeanWeight[LCT_COORDSEL_ORIGINAL_COORD_LIST_LEN] = {0.20, 0.30, 0.50};                       // 历史定位坐标权重
uint32 gLct_dwPredictionTimes = 0;                                                                        // 预测次数
uint32 gLct_dwFpBaseOffset = 0;                                                                           // 第一径基本偏移量

// 导出全局变量
extern PUB_SYS_STATUS_T gg_stSysStatus; // 系统运行状态
extern uint32 gg_dwFrameNo;             // 全局帧号

void lct_main_init(LCT_FIRSTPATH_PARAM_T stLctFirstpathParam, LCT_COORDCAL_PARAM_T stLctCoordcalParam, LCT_COORDSEL_PARAM_T stLctCoordselParam)
{
    // 第一径全局变量初始化
    memset(&gLct_stFirstPathList, 0, sizeof(LCT_FIRSTPATH_LIST_T));
    memset(&gLct_stOriginalLctCoordList, 0, sizeof(LCT_COORDSEL_ORIGINAL_COORD_T));
    memset(&gLct_stMeanLctCoordList, 0, sizeof(LCT_COORDSEL_MEAN_COORD_T));
    memset(gLct_astPredictionDirectionDataList, 0, sizeof(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T) * PUB_SYS_CARRIER_NUM);

    lct_firstpath_init(stLctFirstpathParam);
    lct_coordcal_init(stLctCoordcalParam);
    lct_coordsel_init(stLctCoordselParam);
}

// 模块全局数据结构复位
void lct_main_reset()
{
    // 系统处于待同步状态
    // if (!gg_stSysStatus.bySyncFlag || !gg_stSysStatus.byBtsCaptureFlag)
    if (!gg_stSysStatus.bySyncFlag)
    {
        memset(&gLct_stFirstPathList, 0, sizeof(LCT_FIRSTPATH_LIST_T));
        memset(&gLct_stOriginalLctCoordList, 0, sizeof(LCT_COORDSEL_ORIGINAL_COORD_T));
        memset(&gLct_stMeanLctCoordList, 0, sizeof(LCT_COORDSEL_MEAN_COORD_T));
        memset(gLct_astPredictionDirectionDataList, 0, sizeof(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T) * PUB_SYS_CARRIER_NUM);

        gLct_dwPredictionTimes = 0;
        gg_stSysStatus.byBtsCaptureFlag = 0; // 系统处于捕获基站状态
        gg_stSysStatus.byReLocationFlag = 0; // 系统进入重新定位状态
    }

    return;
}

// 定位主控函数
// pstLctBtsList:一组定位基站
// pstLctDataList:定位数据
// pstCurrentLctResult:定位结果
void lct_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *pstLctDataList, PUB_CURRENT_LCT_RESULT_T *pstCurrentLctResult)
{
    uint8 byRemainder = 0;

    LCT_FIRSTPATH_T *pstFirstPathList = NULL;
    uint8 byBtsNum = 0;

    LCT_MINIMUM_VALUE_GROUP_T stMinimumValueGroup = {0}; // 极小值数据组

    uint8 byPredictionFlag = 0;      // 预测坐标成功标识符
    COORD_T stPredictionCoord = {0}; // 预测坐标

    uint16 wMinimumNum = 0;
    uint8 byCalMinimumNum = 0;
    uint8 byTrackMinimumNum = 0;
    uint8 byPredicMinimumNum = 0;

    LCT_COORDSEL_OPTIMUM_COORD_T stOptimumCoord = {0}; // 最优坐标

    uint32 dwMainBtsId = 0;
    int32 dwMainBtsBuildId = 0;
    int32 dwMainBtsLayerId = 0;

    uint8 byFlag = 0;
    COORD_T stMeanCoord = {0};

    PUB_LOCATION_DATA_T *pstLctData = NULL;

    uint8 byImuMoveFlag = 0; // imu计算结果，0：计算错误，1：成功，但静止，50：成功，但移动

    // 模块全局数据结构复位
    lct_main_reset();

    // 系统已经同步并且定位基站数量至少有3个
    if (gg_stSysStatus.bySyncFlag && pstLctBtsList->byBtsNum > 1)
    {
        // 当前帧需要定位
        byRemainder = (gg_dwFrameNo - 1) % PUB_SYS_SLOT_NUM;
        if (byRemainder == 0)
        {

            // 1、 计算第一径，生成基站第一径数据组，同时镜像基站第一径数据组
            lct_firstpath_main(pstLctBtsList, pstLctDataList, &gLct_stFirstPathList);

            // TODO测试用的代码
            if (PUB_DEBUG_ENABLE)
            {
                lct_main_write_text_file(&gLct_stFirstPathList);
            }

            // 2、根据基站第一径数据组计算“极小值数据组”
            lct_coordcal_main(pstLctBtsList, pstLctDataList, &gLct_stFirstPathList, &byPredictionFlag, &stPredictionCoord, &stMinimumValueGroup, &wMinimumNum);

            // 3、根据“极小值数据组”选择最优定位坐标
            if (wMinimumNum > 0)
            {
                // 选择坐标
                lct_coordsel_main(&stMinimumValueGroup, byPredictionFlag, stPredictionCoord, &stOptimumCoord);

                // 存在有效的极小值点
                if (stOptimumCoord.byResultFlag)
                {
                    // 定位坐标来自预测
                    if (stOptimumCoord.bySourceFlag == 3)
                    {
                        gLct_dwPredictionTimes++;
                    }
                    else
                    {
                        gLct_dwPredictionTimes = 0;
                    }

                    // 连续4次使用预测坐标，则停止输出
                    if (gLct_dwPredictionTimes >= LCT_MAIN_MAX_USE_PREDICTION_COORD_TIMES)
                    {
                        pstCurrentLctResult->dwCpltFlag = 1;    // 定位完成
                        pstCurrentLctResult->dwSuccessFlag = 1; // 定位成功

                        gg_stSysStatus.byBtsCaptureFlag = 0; // 系统进入基站捕获阶段
                        gg_stSysStatus.byReLocationFlag = 1; // 重新定位标识符置1
                    }
                    else
                    {
                        pstCurrentLctResult->dwCpltFlag = 1;    // 定位完成
                        pstCurrentLctResult->dwSuccessFlag = 1; // 定位成功

                        gg_stSysStatus.byBtsCaptureFlag = 1; // 系统进入基站跟踪阶段
                        gg_stSysStatus.byReLocationFlag = 0; // 重新定位标识符置0
                    }

                    // 二次校验坐标(已改成在计算模块校验)
                    // lct_main_dbcheck_coord_by_bts_polygon(&gLct_stFirstPathList, &stOptimumCoord);

                    // 根据定位结果，更新基站第一径帧内跟踪索引
                    lct_main_update_bts_firstpath_trace_index(stOptimumCoord.bySourceFlag, &gLct_stFirstPathList, pstLctDataList);

                    // 查找主站，包括所在的大楼编号、楼层编号和基站编号
                    lct_main_find_main_bts(&gLct_stFirstPathList, &dwMainBtsId, &dwMainBtsBuildId, &dwMainBtsLayerId);

                    // 保存原始坐标
                    lct_main_save_original_coord(dwMainBtsBuildId, dwMainBtsLayerId, stOptimumCoord.stCoord, &gLct_stOriginalLctCoordList);
                    stMeanCoord = stOptimumCoord.stCoord;

                    // 平滑原始坐标
                    lct_main_mean_original_coord(&gLct_stOriginalLctCoordList, gLct_afMeanWeight, &gLct_stMeanLctCoordList, &stMeanCoord);

                    // 计算第一径基本偏移量
                    lct_toa_cal_bts_first_path_index_offset(stOptimumCoord.bySourceFlag, &gLct_stFirstPathList, &gLct_stMeanLctCoordList, gLct_dwFpBaseOffset, &gLct_dwFpBaseOffset);

                    // 根据定位坐标修改手机相对于基站的移动趋势
                    lct_main_update_phone_tendency(&gLct_stMeanLctCoordList, gLct_astPredictionDirectionDataList);

                    // 通过imu计算位移，判断手机是否移动
                    byImuMoveFlag = imu_cal_distance_lct();

                    // 组织定位结果
                    lct_main_organize_lct_result(dwMainBtsBuildId, dwMainBtsLayerId, &gLct_stFirstPathList, &stMeanCoord, byImuMoveFlag, pstCurrentLctResult);

                    // TODO测试
                    if (PUB_DEBUG_ENABLE)
                    {
                        printf("frame no:%d, source:%d, x :%d, y: %d\n", gg_dwFrameNo - 1, stOptimumCoord.bySourceFlag, stMeanCoord.dwX, stMeanCoord.dwY);

                        char *filePathx = "E:\\work\\ips8000\\aplm8000sdk\\output\\coord\\coordx.bin";
                        char *filePathy = "E:\\work\\ips8000\\aplm8000sdk\\output\\coord\\coordy.bin";
                        util_write_coord_to_bin_file(filePathx, 1, (uint32 *)&stMeanCoord.dwX);
                        util_write_coord_to_bin_file(filePathy, 1, (uint32 *)&stMeanCoord.dwY);
                    }
                }
                // 没有有效的极小值点
                else
                {
                    pstCurrentLctResult->dwCpltFlag = 1;    // 定位完成
                    pstCurrentLctResult->dwSuccessFlag = 0; // 定位失败
                    gg_stSysStatus.byBtsCaptureFlag = 0;

                    // 清空定位数据中第一径数据
                    for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
                    {
                        pstLctData = &pstLctDataList[i];
                        pstLctData->wAheadFrameCalPahtIndex = 0;
                        pstLctData->wAheadFrameTrackPathIndex = 0;
                    }

                    printf("minimum avalid\n");
                }
            }
            else
            {
                pstCurrentLctResult->dwCpltFlag = 1;    // 定位完成
                pstCurrentLctResult->dwSuccessFlag = 0; // 定位失败
                gg_stSysStatus.byBtsCaptureFlag = 0;

                // 清空定位数据中第一径数据
                for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
                {
                    pstLctData = &pstLctDataList[i];
                    pstLctData->wAheadFrameCalPahtIndex = 0;
                    pstLctData->wAheadFrameTrackPathIndex = 0;
                }

                printf("no minimum\n");
            }
        }
    }
    else
    {
        printf("error, sync flag:%d, bts num:%d\n", gg_stSysStatus.bySyncFlag, pstLctBtsList->byBtsNum);
    }

    return;
}

// 根据基站多边形再次校验定位坐标
void lct_main_dbcheck_coord_by_bts_polygon(LCT_FIRSTPATH_LIST_T *pstBtsFirstPathList, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord)
{
    uint8 byFirstPathListLen = pstBtsFirstPathList->byBtsNum; // 基站数量
    LCT_FIRSTPATH_T *pstFirstPathList = pstBtsFirstPathList->astFirstPath;

    int32 dwTemp = 0;
    int32 dwMinValueX = 2147483647;
    int32 dwMaxValueX = -2147483648;
    int32 dwMinValueY = 2147483647;
    int32 dwMaxValueY = -2147483648;

    uint32 dwGapX = 0;
    uint32 dwGapY = 0;

    int32 dwX = 0;
    int32 dwY = 0;

    ////计算端点坐标的最大值与最小值
    for (uint8 i = 0; i < byFirstPathListLen; i++)
    {
        dwTemp = pstFirstPathList[i].dwCoordX;
        if (dwTemp > dwMaxValueX)
        {
            dwMaxValueX = dwTemp;
        }

        if (dwTemp < dwMinValueX)
        {
            dwMinValueX = dwTemp;
        }

        dwTemp = pstFirstPathList[i].dwCoordY;
        if (dwTemp > dwMaxValueY)
        {
            dwMaxValueY = dwTemp;
        }
        if (dwTemp < dwMinValueY)
        {
            dwMinValueY = dwTemp;
        }
    }

    // 计算基站坐标间隔
    dwGapX = dwMaxValueX - dwMinValueX;
    dwGapY = dwMaxValueY - dwMinValueY;

    // 校验X坐标
    dwX = pstOptimumCoord->stCoord.dwX;
    if (dwGapX < 350)
    {
        dwX = util_distance_gen_rand_num((dwMaxValueX + dwMinValueX) / 2, 50);
    }
    else if (dwGapX >= 350 && dwGapX < 900)
    {
        dwX = pstOptimumCoord->stCoord.dwX * 0.85;
    }
    else if (dwGapX >= 900 && dwGapX < 1500)
    {
        dwX = pstOptimumCoord->stCoord.dwX * 0.95;
    }

    // 校验Y坐标
    dwY = pstOptimumCoord->stCoord.dwY;
    if (dwGapY < 350)
    {
        dwY = util_distance_gen_rand_num((dwMaxValueY + dwMinValueY) / 2, 50);
    }
    else if (dwGapY >= 350 && dwGapY < 900)
    {
        dwY = pstOptimumCoord->stCoord.dwY * 0.85;
    }
    else if (dwGapY >= 900 && dwGapY < 1500)
    {
        dwY = pstOptimumCoord->stCoord.dwY * 0.95;
    }

    // 修改
    pstOptimumCoord->stCoord.dwX = dwX;
    pstOptimumCoord->stCoord.dwY = dwY;

    return;
}

// 根据一个数，在该数左右两侧范围内生成一个随机数
// dwBaseData:基数
// wRange:单边偏移
int32 lct_main_gen_rand_num(int32 dwBaseData, uint16 wRange)
{
    int32 dwTotalRange = 2 * wRange + 1; // 总范围
    int32 dwRemainder = 0;
    int32 randomNum = 0; // 随机数

    // 初始化随机数生成器
    srand(time(NULL));

    // 生成随机数
    dwRemainder = rand() % dwTotalRange;           // 生成 [0, dwTotalRange) 范围内的随机数
    randomNum = dwBaseData - wRange + dwRemainder; // 生成所需的随机数

    return randomNum;
}

// 根据定位结果，更新基站第一径帧内跟踪索引
// byLctSourceFlag:定位使用的第一径来源
// pstBtsFirstPathList:基站第一径数据队列
// pstLctDataList:定位数据
void lct_main_update_bts_firstpath_trace_index(uint8 byLctSourceFlag, LCT_FIRSTPATH_LIST_T *pstBtsFirstPathList, PUB_LOCATION_DATA_T *pstLctDataList)
{
    uint8 byBtsNum = pstBtsFirstPathList->byBtsNum;
    LCT_FIRSTPATH_T *pstFirstPathList = NULL;
    uint16 wFrameTraceIndex = 0;
    uint8 byFreqIndex = 0;
    PUB_LOCATION_DATA_T *pstLctData = NULL;

    for (uint8 i = 0; i < byBtsNum; i++)
    {
        pstFirstPathList = &pstBtsFirstPathList->astFirstPath[i];

        // 最优定位坐标来自计算径
        if (byLctSourceFlag == 1)
        {
            wFrameTraceIndex = pstFirstPathList->wFrameCalIndex;
        }
        // 最优定位坐标来自跟踪径
        else if (byLctSourceFlag == 2)
        {
            wFrameTraceIndex = pstFirstPathList->wFrameTrackIndex;
        }
        // 最优定位坐标来自预测径
        else
        {
            wFrameTraceIndex = 0;
        }

        // 跟新"跟踪径"
        pstFirstPathList->wFrameTrackIndex = wFrameTraceIndex;
    }

    // 将跟踪索引更新到定位数据中
    for (uint8 j = 0; j < PUB_SYS_CARRIER_NUM; j++)
    {
        pstLctData = &pstLctDataList[j];
        pstLctData->wAheadFrameCalPahtIndex = 0;
        pstLctData->wAheadFrameTrackPathIndex = 0;
    }
    for (uint8 k = 0; k < byBtsNum; k++)
    {
        pstFirstPathList = &pstBtsFirstPathList->astFirstPath[k];
        byFreqIndex = pstFirstPathList->byFreqIndex;
        pstLctData = &pstLctDataList[byFreqIndex];

        pstLctData->wAheadFrameCalPahtIndex = pstFirstPathList->wFrameCalIndex;
        pstLctData->wAheadFrameTrackPathIndex = pstFirstPathList->wFrameTrackIndex;
    }

    return;
}

// 从第一径数据队列中查找主站信息
void lct_main_find_main_bts(LCT_FIRSTPATH_LIST_T *pstFirstPathList, uint32 *pdwBtsId, int32 *pwBuildId, int32 *pdwLayerId)
{
    LCT_FIRSTPATH_T *pstFirstPahtData = NULL;
    uint8 byExist = 0;

    for (uint8 i = 0; i < pstFirstPathList->byBtsNum; i++)
    {
        pstFirstPahtData = &pstFirstPathList->astFirstPath[i];
        if (pstFirstPahtData->byMainFlag == 1)
        {
            *pdwBtsId = pstFirstPahtData->dwBtsId;
            *pwBuildId = pstFirstPahtData->dwBuildId;
            *pdwLayerId = pstFirstPahtData->dwLayerId;

            byExist = 1;
            break;
        }
    }

    if (!byExist)
    {
        pstFirstPahtData = &pstFirstPathList->astFirstPath[0];
        *pdwBtsId = pstFirstPahtData->dwBtsId;
        *pwBuildId = pstFirstPahtData->dwBuildId;
        *pdwLayerId = pstFirstPahtData->dwLayerId;
    }

    return;
}

// 保存原始定位坐标
// stCoord:定位点坐标
// pstOriginalLctCoordList:原始定位坐标队列
void lct_main_save_original_coord(int32 dwBuildId, int32 dwLayerId, COORD_T stCoord, LCT_COORDSEL_ORIGINAL_COORD_T *pstOriginalLctCoordList)
{
    uint8 byCoordListLen = pstOriginalLctCoordList->byListLen; // 原始定位坐标队列长度

    // TODO（以后要修改成支持多个大楼和多个楼层） 保存大楼编号和楼层编号
    gg_stSysStatus.dwBuildId = dwBuildId;
    gg_stSysStatus.dwLayerId = dwLayerId;

    // 队列没有满
    if (byCoordListLen < LCT_COORDSEL_ORIGINAL_COORD_LIST_LEN)
    {
        pstOriginalLctCoordList->astCoordList[byCoordListLen] = stCoord;
        pstOriginalLctCoordList->byListLen++;
    }
    // 队列已满
    else
    {
        // 移动队列
        memmove(pstOriginalLctCoordList->astCoordList, &pstOriginalLctCoordList->astCoordList[1], sizeof(COORD_T) * (LCT_COORDSEL_ORIGINAL_COORD_LIST_LEN - 1));
        pstOriginalLctCoordList->astCoordList[LCT_COORDSEL_ORIGINAL_COORD_LIST_LEN - 1] = stCoord; // 将数据插入队列尾
    }

    return;
}

// 平滑原始定位坐标
// pstOriginalLctCoordList:原始定位坐标队列
// pfMeanWeightList:平滑权重
// pstMeanCoord:返回参数，平滑坐标
void lct_main_mean_original_coord(LCT_COORDSEL_ORIGINAL_COORD_T *pstOriginalLctCoordList, flt32 *pfMeanWeightList, LCT_COORDSEL_MEAN_COORD_T *pstMeanCoordList, COORD_T *pstMeanCoord)
{
    uint8 byListLen = pstOriginalLctCoordList->byListLen;
    flt32 fX = 0;
    flt32 fY = 0;
    uint8 byCoordListLen = pstMeanCoordList->byListLen; // 平滑坐标队列

    // 原始坐标队列长度不合法
    if ((byListLen < LCT_COORDSEL_ORIGINAL_COORD_LIST_LEN))
    {
        return;
    }

    //// 平滑坐标
    for (uint8 i = 0; i < byListLen; i++)
    {
        fX += pstOriginalLctCoordList->astCoordList[i].dwX * pfMeanWeightList[i];
        fY += pstOriginalLctCoordList->astCoordList[i].dwY * pfMeanWeightList[i];
    }
    pstMeanCoord->dwX = fX;
    pstMeanCoord->dwY = fY;
    pstMeanCoord->dwZ = 120;

    //// 保存平滑后的坐标
    // 队列没有满
    if (byCoordListLen < LCT_COORDSEL_MEAN_COORD_LIST_LEN)
    {
        pstMeanCoordList->astCoordList[byCoordListLen] = *pstMeanCoord;
        pstMeanCoordList->byListLen++;
    }
    // 队列已满
    else
    {
        // 移动队列
        memmove(pstMeanCoordList->astCoordList, &pstMeanCoordList->astCoordList[1], sizeof(COORD_T) * (LCT_COORDSEL_MEAN_COORD_LIST_LEN - 1));
        pstMeanCoordList->astCoordList[LCT_COORDSEL_MEAN_COORD_LIST_LEN - 1] = *pstMeanCoord; // 将数据插入队列尾
    }

    return;
}

// 组织定位结果
// dwBuildId:大楼编号
// dwLayerId:楼层编号
// pstFirstPathList:定位基站第一径队列
// pstMeanCoord:平滑坐标
// pstCurrentLctResult:定位结果
void lct_main_organize_lct_result(int32 dwBuildId, int32 dwLayerId, LCT_FIRSTPATH_LIST_T *pstFirstPathList, COORD_T *pstMeanCoord, uint32 dwImuMoveFlag, PUB_CURRENT_LCT_RESULT_T *pstCurrentLctResult)
{
    uint8 byBtsNum = pstFirstPathList->byBtsNum;
    PUB_LCT_BTS_T *pstLctBtsData = NULL;
    uint8 byExist = 0;

    // 1、 保存定位坐标坐标
    pstCurrentLctResult->stCoord = *pstMeanCoord;

    // 2、查找定位主站并保存大楼和楼层
    pstCurrentLctResult->dwBuildId = dwBuildId;
    pstCurrentLctResult->dwLayerId = dwLayerId;

    // 3、保存基站数据
    pstCurrentLctResult->dwLctBtsNum = byBtsNum; // 基站数量
    for (uint8 i = 0; i < byBtsNum; i++)
    {
        pstCurrentLctResult->adwBtsIdList[i] = pstFirstPathList->astFirstPath[i].dwBtsId; // 基站ID
    }

    // 4、保存IMU移动标识符
    pstCurrentLctResult->dwImuMoveFlag = dwImuMoveFlag;

    return;
}

// 保存平滑坐标
// stCoord：定位点坐标
// pstMeanCoordList:平滑队列
void lct_main_save_mean_coord(COORD_T stCoord, LCT_COORDSEL_MEAN_COORD_T *pstMeanCoordList)
{
    uint8 byCoordListLen = pstMeanCoordList->byListLen; // 平滑坐标队列

    // 队列没有满
    if (byCoordListLen < LCT_COORDSEL_MEAN_COORD_LIST_LEN)
    {
        pstMeanCoordList->astCoordList[byCoordListLen] = stCoord;
        pstMeanCoordList->byListLen++;
    }
    // 队列已满
    else
    {
        // 移动队列
        memmove(pstMeanCoordList->astCoordList, &pstMeanCoordList->astCoordList[1], sizeof(COORD_T) * (LCT_COORDSEL_MEAN_COORD_LIST_LEN - 1));
        pstMeanCoordList->astCoordList[LCT_COORDSEL_MEAN_COORD_LIST_LEN - 1] = stCoord; // 将数据插入队列尾
    }

    return;
}

// 根据定位坐标反算从站第一径相对主站第一径的偏移量
// pstAheadLctFirstPahtGroup:前一次基站第一径数据组PU
// stLctCoord:定位坐标
// byPathSource:第一径源，1：计算径、2：跟踪径：3：预测径
void lct_main_adjust_firstpath_index_offset(LCT_FIRSTPATH_LIST_T *pstAheadLctFirstPahtGroup, COORD_T stLctCoord, uint8 byPathSource, PUB_LOCATION_DATA_T *pstLctDataList)
{
    uint8 byBtsNum = pstAheadLctFirstPahtGroup->byBtsNum;
    LCT_FIRSTPATH_T stMainBtsFirstPath = {0};
    LCT_FIRSTPATH_T *pstSlaveBtsFirstPath = NULL;
    uint8 byMainFlag = 0;
    flt32 fMainTimeLen = 0;
    int32 dwMainAbsoluteIndex = 0;
    flt32 fSlaveTimeLen = 0;
    int32 dwSlaveAbsoluteIndex = 0;
    flt32 fDelayDiff = 0;

    int32 xx = 0;
    int32 yy = 0;
    int32 zz = 0;
    flt32 fDistance = 0;
    int32 dwTimeOffset = 0;
    int32 dwSampleOffset = 0;
    int32 dwIndexOffset = 0;

    // 找主站
    for (uint8 i = 0; i < byBtsNum; i++)
    {
        stMainBtsFirstPath = pstAheadLctFirstPahtGroup->astFirstPath[i];
        if (stMainBtsFirstPath.byMainFlag == 1)
        {
            byMainFlag = 1;
            break;
        }
    }
    // 未找到主站
    if (!byMainFlag)
    {
        return;
    }

    // 计算主站传输时长
    xx = stLctCoord.dwX - stMainBtsFirstPath.dwCoordX;
    yy = stLctCoord.dwY - stMainBtsFirstPath.dwCoordY;
    zz = stLctCoord.dwZ - stMainBtsFirstPath.dwCoordZ;
    fDistance = sqrt(xx * xx + yy * yy + zz * zz);
    fMainTimeLen = fDistance / 100 / PUB_AUDIO_SPEED * 1000; // 单位毫秒

    // 计算从站与主站间的时延差，并反算索引偏差
    for (uint8 j = 0; j < byBtsNum; j++)
    {
        pstSlaveBtsFirstPath = &pstAheadLctFirstPahtGroup->astFirstPath[j];

        // 当前站是从站
        if (pstSlaveBtsFirstPath->byMainFlag == 2)
        {
            // 计算定位点到从站间的传输时长
            xx = stLctCoord.dwX - pstSlaveBtsFirstPath->dwCoordX;
            yy = stLctCoord.dwY - pstSlaveBtsFirstPath->dwCoordY;
            zz = stLctCoord.dwZ - pstSlaveBtsFirstPath->dwCoordZ;
            fDistance = sqrt(xx * xx + yy * yy + zz * zz);
            fSlaveTimeLen = fDistance / 100 / PUB_AUDIO_SPEED * 1000; // 从站传输时长，单位毫秒

            // 定位点到主站与到从站间的时长差
            fDelayDiff = (fSlaveTimeLen - fMainTimeLen); // 毫秒

            // 计算从站第一径与主站第一径的索引偏差(绝对偏移)
            dwTimeOffset = (int32)(48 * fDelayDiff);

            if (byPathSource == 1)
            {
                dwMainAbsoluteIndex = stMainBtsFirstPath.dwCalPathIndex;
                dwSlaveAbsoluteIndex = pstSlaveBtsFirstPath->dwCalPathIndex;
            }
            else if (byPathSource == 2)
            {
                dwMainAbsoluteIndex = stMainBtsFirstPath.dwTrackPathIndex;
                dwSlaveAbsoluteIndex = pstSlaveBtsFirstPath->dwTrackPathIndex;
            }
            else
            {
                dwMainAbsoluteIndex = stMainBtsFirstPath.dwCalPathIndex * 0.4 + stMainBtsFirstPath.dwTrackPathIndex * 0.6;
                dwSlaveAbsoluteIndex = pstSlaveBtsFirstPath->dwCalPathIndex * 0.4 + pstSlaveBtsFirstPath->dwTrackPathIndex * 0.6;
            }

            // 采样索引偏移(通过第一径估算的偏移)
            dwSampleOffset = dwSlaveAbsoluteIndex - dwMainAbsoluteIndex;

            // 估算偏移与绝对偏移之差
            dwIndexOffset = dwSampleOffset - dwTimeOffset;

            // 调整定位数据队列索引
            lct_main_adjust_slave_bts_list_index(pstSlaveBtsFirstPath->byFreqIndex, pstLctDataList, pstSlaveBtsFirstPath->dwBtsId, dwIndexOffset);

            // 调整帧内第一径索引
            lct_main_adjust_frame_index(pstSlaveBtsFirstPath, dwIndexOffset);
        }
    }

    return;
}

// 调整从站定位数据队列索引
// byFreqIndex:从站频率索引
// dwBtsId:基站ID
// dwIndexOffset:从站相对与主站的偏移
void lct_main_adjust_slave_bts_list_index(uint8 byFreqIndex, PUB_LOCATION_DATA_T *pstLocationDataList, uint32 dwBtsId, int32 dwIndexOffset)
{
    PUB_LOCATION_DATA_T *pstLocationData = NULL;
    uint8 byFlag = 0;
    int32 dwIndex = 0;

    // 遍历队列查找从站
    for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
    {
        pstLocationData = &pstLocationDataList[i];
        if (pstLocationData->dwSetBtsId == dwBtsId && i == byFreqIndex)
        {
            byFlag = 1;
            break;
        }
    }

    // 从站存在，则调整队列索引，包括变频索引，定位开始索引和最后一帧定位数据索引
    if (byFlag)
    {
        // 队列往前调整(绝对索引)
        if (dwIndexOffset > 0)
        {
            // 调整变频索引
            pstLocationData->dwDdcIndex -= dwIndexOffset;

            // 调整定位索引
            dwIndex = pstLocationData->dwLocationIndex - dwIndexOffset;
            if (dwIndex >= 0)
            {
                pstLocationData->dwLocationIndex = dwIndex;
            }
            else
            {
                pstLocationData->dwLocationIndex = PUB_LCT_DATA_LIST_LEN + dwIndex;
            }

            // 调整定位数据最后一帧的开始索引
            dwIndex = pstLocationData->dwFourthLocationIndex - dwIndexOffset;
            if (dwIndex >= 0)
            {
                pstLocationData->dwFourthLocationIndex = dwIndex;
            }
            else
            {
                pstLocationData->dwFourthLocationIndex = PUB_LCT_DATA_LIST_LEN + dwIndex;
            }
        }
        // 队列往后调整
        else
        {
            dwIndexOffset = abs(dwIndexOffset);

            // 调整变频索引
            pstLocationData->dwDdcIndex += dwIndexOffset;

            // 调整定位索引
            dwIndex = pstLocationData->dwLocationIndex + dwIndexOffset;
            if (dwIndex >= PUB_LCT_DATA_LIST_LEN)
            {
                pstLocationData->dwLocationIndex = dwIndex - PUB_LCT_DATA_LIST_LEN;
            }
            else
            {
                pstLocationData->dwLocationIndex = dwIndex;
            }

            // 调整定位数据最后一帧的开始索引
            dwIndex = pstLocationData->dwFourthLocationIndex + dwIndexOffset;
            if (dwIndex >= PUB_LCT_DATA_LIST_LEN)
            {
                pstLocationData->dwFourthLocationIndex = dwIndex - PUB_LCT_DATA_LIST_LEN;
            }
            else
            {
                pstLocationData->dwFourthLocationIndex = dwIndex;
            }
        }
    }
}

// 调整帧内第一径索引
// pstSlaveBtsFirstPath:从站定位数据
// dwIndexOffset:索引偏移
void lct_main_adjust_frame_index(LCT_FIRSTPATH_T *pstSlaveBtsFirstPath, int32 dwIndexOffset)
{
    // 索引往左调整
    if (dwIndexOffset > 0)
    {
        pstSlaveBtsFirstPath->wFrameCalIndex -= dwIndexOffset;
        if (pstSlaveBtsFirstPath->wFrameTrackIndex > 0)
        {
            pstSlaveBtsFirstPath->wFrameTrackIndex -= dwIndexOffset;
        }
    }
    // 索引往右调整
    else
    {
        dwIndexOffset = abs(dwIndexOffset);
        pstSlaveBtsFirstPath->wFrameCalIndex += dwIndexOffset;
        if (pstSlaveBtsFirstPath->wFrameTrackIndex > 0)
        {
            pstSlaveBtsFirstPath->wFrameTrackIndex += dwIndexOffset;
        }
    }
}

// 把第一径写入文本文件中
void lct_main_write_text_file(LCT_FIRSTPATH_LIST_T *pstFirstPahtGroup)
{
    LCT_MAIN_TEST_FIRST_PATH_T stTmp = {0};
    LCT_FIRSTPATH_T stFirstPath = {0};
    char *pFilePaht = "E:\\work\\ips8000\\aplm8000sdk\\output\\firstpath\\firstpath.txt";

    for (uint8 i = 0; i < pstFirstPahtGroup->byBtsNum; i++)
    {
        stFirstPath = pstFirstPahtGroup->astFirstPath[i];
        stTmp.dwFrameNo = gg_dwFrameNo - 1;
        stTmp.dwFreqIndex = stFirstPath.byFreqIndex;
        stTmp.dwMaxValue = (uint32)stFirstPath.fMaxAmplValue;

        // stTmp.dwFrameCalIndex = stFirstPath.wFrameCalIndex;
        stTmp.dwCalDdcIndex = stFirstPath.dwCalPathIndex;

        // stTmp.dwFrameTrackIndex = stFirstPath.wFrameTrackIndex;
        stTmp.dwTrackDdcIndex = stFirstPath.dwTrackPathIndex;

        // 写入文件中
        util_write_real_number_to_txt_file(pFilePaht, sizeof(LCT_MAIN_TEST_FIRST_PATH_T) / 4, (uint32 *)(&stTmp));
    }

    return;
}

// 修改手机相对于基站的移动趋势
// pstMeanLctCoordList:定位坐标平滑队列
// pstPredictionDirectionDataList:预测手机移动方向数据队列
void lct_main_update_phone_tendency(LCT_COORDSEL_MEAN_COORD_T *pstMeanLctCoordList, LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList)
{
    LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstDirectionData = NULL;

    flt32 fDistanceC = 0; // 基站到当前一点的距离
    flt32 fDistanceA = 0; // 基站到前一点的距离

    uint8 byLen = pstMeanLctCoordList->byListLen;
    if (byLen < 2)
    {
        return;
    }

    COORD_T stPointC = pstMeanLctCoordList->astCoordList[byLen - 1];
    COORD_T stPointA = pstMeanLctCoordList->astCoordList[byLen - 2];
    for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
    {
        pstDirectionData = &pstPredictionDirectionDataList[i];
        if (pstDirectionData->byValidFlag)
        {
            fDistanceC = util_distance_cal_p2p_distance(pstDirectionData->dwCoordX, pstDirectionData->dwCoordY, stPointC.dwX, stPointC.dwY);
            fDistanceA = util_distance_cal_p2p_distance(pstDirectionData->dwCoordX, pstDirectionData->dwCoordY, stPointA.dwX, stPointA.dwY);
            if (fDistanceC > fDistanceA)
            {
                pstDirectionData->byDirectionTendency = 2;
            }
            else if (fDistanceC < fDistanceA)
            {
                pstDirectionData->byDirectionTendency = 1;
            }
            else
            {
                pstDirectionData->byDirectionTendency = 0;
            }
        }
    }
}