修改代码，完善预测手机移动方向相关代码

main.c

@@ -45,7 +45,7 @@ void main_read_wav()
 {
     // const char *pFileName = "E:\\work\\ips8000\\aplm8000sdk\\wave\\zcq(20251203-09).wav";
     //  const char *pFileName = "E:\\work\\ips8000\\aplm8000sdk\\wave\\rec5-0-5(0245)-btsx.wav";
-    const char *pFileName = "E:\\work\\ips8000\\aplm8000sdk\\wave\\zhoutao_6bts_rect_0327.wav";
+    const char *pFileName = "E:\\work\\ips8000\\aplm8000sdk\\wave\\zcq_6bts_rect_0327.wav";
     int16 *pwData = NULL; // 音频数据存储地址
     uint8 byIsOver = 0;
     uint16 wCounter = 0;

src/modules/location/lct_coordcal.c

@@ -84,10 +84,10 @@ void lct_coordcal_main(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T
     lct_coordcal_sel_valid_bts(pstLctDataList, pstFirstPathDataList, byFirstPathDataListLen, astValidCalPathBtsList, &byValidCalPathBtsListLen, astValidTrackPathBtsList, &byValidTrackPathBtsListLen, &dwBuildId, &dwLayerId);
 
     // 预测定位坐标
-    gLct_dwSpeedX = 0;
-    gLct_dwSpeedY = 0;
+    gLct_dwSpeedX = 0; // x轴移动速度
+    gLct_dwSpeedY = 0; // y轴移动速度
     // lct_coordcal_prediction_coord(&stPredictionCoord, &byPredictionFlag, &stAheadPointCoord, dwBuildId, dwLayerId);
-    lct_prediction_current_coord_by_history(dwBuildId, dwLayerId, &stPredictionCoord, &byPredictionFlag, &stAheadPointCoord);
+    lct_prediction_current_coord_by_history(dwBuildId, dwLayerId, &stPredictionCoord, &byPredictionFlag, &stAheadPointCoord, &gLct_dwSpeedX, &gLct_dwSpeedY);
 
     // // 修正预测坐标
     // if (byPredictionFlag)
@@ -97,6 +97,9 @@ void lct_coordcal_main(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T
     //     *pstPredictionCoord = stPredictionCoord;
     // }
 
+    // TODO 预测手机相对于基站的运动趋势(未调试成功)
+    //  lct_prediction_prediction_phone_move_tendency(gLct_astPredictionDirectionDataList);
+
     // 预测定位坐标失败
     if (!byPredictionFlag)
     {
@@ -115,16 +118,20 @@ void lct_coordcal_main(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T
     // 预测定位坐标成功
     else
     {
+        *pbyPredictionFlag = byPredictionFlag;
+        *pstPredictionCoord = stPredictionCoord;
+
         // 校验手机与基站间的位置变化趋势
         lct_coordcal_check_move_tendency(pstFirstPathDataList, byFirstPathDataListLen, gLct_astPredictionDirectionDataList, &stPredictionCoord, &stAheadPointCoord);
 
-        //  预测手机移动方向
-        gLct_byDirectionX = 0;
-        gLct_byDirectionY = 0;
+        // 预测手机分别在x轴和y轴上的移动方向
+        gLct_byDirectionX = 0; // x轴移动方向，1：向右移动，-1向左移动
+        gLct_byDirectionY = 0; // y轴移动方向，1：向上移动，-1向下移动
         lct_coordcal_prediction_x_y_move_direction(pstFirstPathDataList, byFirstPathDataListLen, gLct_astPredictionDirectionDataList, &gLct_byDirectionX, &gLct_byDirectionY);
+        // lct_prediction_prediction_phone_x_y_move_direction(gLct_astPredictionDirectionDataList, stAheadPointCoord, &gLct_byDirectionX, &gLct_byDirectionY);
 
-        // 校验预测坐标
-        lct_coordcal_check_prediction_coord_by_xy_move_tendency(&stPredictionCoord, &stAheadPointCoord);
+        // 根据手机移动方向修正预测坐标
+        lct_coordcal_check_prediction_coord_by_xy_move_tendency(&stPredictionCoord, &stAheadPointCoord, gLct_byDirectionX, gLct_byDirectionY, gLct_dwSpeedX, gLct_dwSpeedY);
 
         ////一维定位
         // 计算径
@@ -137,6 +144,9 @@ void lct_coordcal_main(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T
         lct_coordcal_two_dimension_location(astValidCalPathBtsList, byValidCalPathBtsListLen, byPredictionFlag, stPredictionCoord, 0, &stMinmumValueGroup);
         // 跟踪径
         lct_coordcal_two_dimension_location(astValidTrackPathBtsList, byValidTrackPathBtsListLen, byPredictionFlag, stPredictionCoord, 1, &stMinmumValueGroup);
+
+        // 校验二维定位的极小值
+        lct_coordcal_two_dimension_check(&stMinmumValueGroup, &stPredictionCoord, &stAheadPointCoord, gLct_byDirectionX, gLct_byDirectionY);
     }
 
     // 没有极小值，则使用预测坐标
@@ -423,7 +433,12 @@ void lct_coordcal_prediction_coord(COORD_T *pstPredictionCoord, uint8 *pbyPredic
     return;
 }
 
-// 检验手机移动趋势
+// 检验手机移动趋势，用基站到预测点与上一个定位点的距离来校验手机的移动趋势
+// pstFirstPathList:
+// byFirstPathListLen:
+// pstPredictionDirectionDataList:
+// pstPredictionCoord:
+// pstAheadCoord:
 void lct_coordcal_check_move_tendency(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList, COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord)
 {
     LCT_FIRSTPATH_T *pstFirstPathData = NULL; // 主站
@@ -790,22 +805,22 @@ void lct_coordcal_prediction_x_y_move_direction(LCT_FIRSTPATH_T *pstFirstPathLis
 }
 
 // 根据X轴和Y轴移动趋势，校验预测坐标
-void lct_coordcal_check_prediction_coord_by_xy_move_tendency(COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord)
+void lct_coordcal_check_prediction_coord_by_xy_move_tendency(COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord, int8 byDirectionX, int8 byDirectionY, int32 dwSpeedX, int32 dwSpeedY)
 {
     // X轴向右移动
-    if (gLct_byDirectionX == 1)
+    if (byDirectionX == 1)
     {
-        if (gLct_dwSpeedX <= 0)
+        if (dwSpeedX <= 0)
         {
-            pstPredictionCoord->dwX = pstAheadCoord->dwX - gLct_dwSpeedX;
+            pstPredictionCoord->dwX = pstAheadCoord->dwX - dwSpeedX;
         }
     }
     // X轴向左移动
-    else if (gLct_byDirectionX == -1)
+    else if (byDirectionX == -1)
     {
-        if (gLct_dwSpeedX >= 0)
+        if (dwSpeedX >= 0)
         {
-            pstPredictionCoord->dwX = pstAheadCoord->dwX - gLct_dwSpeedX;
+            pstPredictionCoord->dwX = pstAheadCoord->dwX - dwSpeedX;
         }
     }
     else
@@ -814,18 +829,18 @@ void lct_coordcal_check_prediction_coord_by_xy_move_tendency(COORD_T *pstPredict
     }
 
     // Y轴向上移动
-    if (gLct_byDirectionY == 1)
+    if (byDirectionY == 1)
     {
-        if (gLct_dwSpeedY <= 0)
+        if (dwSpeedY <= 0)
         {
-            pstPredictionCoord->dwY = pstAheadCoord->dwY - gLct_dwSpeedY;
+            pstPredictionCoord->dwY = pstAheadCoord->dwY - dwSpeedY;
         }
     }
-    else if (gLct_byDirectionY == -1)
+    else if (byDirectionY == -1)
     {
-        if (gLct_dwSpeedY >= 0)
+        if (dwSpeedY >= 0)
         {
-            pstPredictionCoord->dwY = pstAheadCoord->dwY - gLct_dwSpeedY;
+            pstPredictionCoord->dwY = pstAheadCoord->dwY - dwSpeedY;
         }
     }
     else

src/modules/location/lct_coordcal.h

@@ -18,7 +18,7 @@ void lct_coordcal_sel_valid_bts(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPA
 void lct_coordcal_check_move_tendency(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList, COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord);
 void lct_coordcal_prediction_x_y_move_direction(LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList, int8 *pbyDirectionX, int8 *pbyDirectionY);
 void lct_coordcal_prediction_coord(COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord, int32 dwBulidId, int32 dwLayerId);
-void lct_coordcal_check_prediction_coord_by_xy_move_tendency(COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord);
+void lct_coordcal_check_prediction_coord_by_xy_move_tendency(COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord, int8 byDirectionX, int8 byDirectionY, int32 dwSpeedX, int32 dwSpeedY);
 void lct_coordcal_check_prediction_coord_by_bts_coord(COORD_T *pstPredicCoord, LCT_FIRSTPATH_T *pstCalBtsList, uint8 byCalBtsListLen, LCT_FIRSTPATH_T *pstTrackBtsList, uint8 byTrackBtsListLen);
 
 // 二维定位

src/modules/location/lct_coordcal_one.c

@@ -19,6 +19,7 @@ extern int32 gLct_dwSpeedX; // X位移速度
 extern int32 gLct_dwSpeedY; // Y位移速度
 
 // 一维计算
+// pstLctDataList:定位数据队列
 // byPredictionFlag:预测标识符，0：预测失败，1:预测成功
 // stPredictionCoord:预测坐标
 // pstValidBtsList:有效基站列表
@@ -516,7 +517,7 @@ void lct_coordcal_check_one_dimension_bts_firstpath_valid(uint8 byPredictionFlag
         }
         else
         {
-            // 检查跟踪径基站Snr是否符合定位要求
+            // 检查跟踪径基站Snr和ampl value是否符合定位要求
             if (pstFirstPath->fTrackPathIndexSnr > fFirstPathSnrThres && pstFirstPath->fFrameTrackPathValue > fFirstPathValueThres)
             {
                 if (pstLctData->wAheadFrameTrackPathIndex > 0 && !gg_stSysStatus.byReLocationFlag)

src/modules/location/lct_coordcal_two.c

@@ -419,135 +419,154 @@ void lct_coordcal_sel_minimum_value(flt32 *pfDelayDeviationSumMatrix, uint16 wMa
     LCT_MINIMUM_VALUE_T astMinmumValueList[128] = {0};                        // 极小值点列表
     uint8 byMinimumValueNum = (uint8)gg_stLctCoordcalParam.dwMinimumValueNum; // 极小值个数
 
-    // 遍历时延队列，查找极小值时延参考点
-    for (uint16 wRow = 0; wRow < wMatrixRow; wRow++)
+    uint8 byWhildFlag = 1; //
+    int8 byValidThres = 5; // 有效校验点数门限
+
+    do
     {
-        for (uint16 wCol = 0; wCol < wMatrixCol; wCol++)
+        // 最小需要2个点来校验
+        if (byValidThres < 3)
         {
-            wIndex = wRow * wMatrixCol + wCol;
-            fDeviation = pfDelayDeviationSumMatrix[wIndex]; // 当前点误差
+            break;
+        }
+        byValidThres--; // 门限减1
 
-            // 判断四周各点是否合法(若1点不合法则退出)
-            byValidFlag = 1;
-            for (uint8 i = 0; i < 1; i++)
+        // 遍历时延队列，查找极小值时延参考点
+        for (uint16 wRow = 0; wRow < wMatrixRow; wRow++)
+        {
+            for (uint16 wCol = 0; wCol < wMatrixCol; wCol++)
             {
-                // 1东方
-                if ((wCol + 1) < wMatrixCol)
+                wIndex = wRow * wMatrixCol + wCol;
+                fDeviation = pfDelayDeviationSumMatrix[wIndex]; // 当前点误差
+
+                // 判断四周各点是否合法(若1点不合法则退出)
+                byValidFlag = 0;
+                for (uint8 i = 0; i < 1; i++)
                 {
-                    wTmpIndex = wRow * wMatrixCol + wCol + 1;
-                    fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                    if (fDeviation >= fTmpDeviation)
-                    { // 不合法
-                        byValidFlag = 0;
-                        break;
+                    // 1东方
+                    if ((wCol + 1) < wMatrixCol)
+                    {
+                        wTmpIndex = wRow * wMatrixCol + wCol + 1;
+                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                        if (fDeviation >= fTmpDeviation)
+                        { // 不合法
+                            byValidFlag = 0;
+                            break;
+                        }
+                        byValidFlag++;
                     }
-                }
 
-                // // 2东北角
-                // if ((wRow + 1) < wMatrixRow && (wCol + 1) < wMatrixCol)
-                // {
-                //     wTmpIndex = (wRow + 1) * wMatrixCol + wCol + 1;
-                //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                //     if (fDeviation >= fTmpDeviation)
-                //     { // 不合法
-                //         byValidFlag = 0;
-                //         break;
-                //     }
-                // }
-
-                // 3北方
-                if ((wRow + 1) < wMatrixRow)
-                {
-                    wTmpIndex = (wRow + 1) * wMatrixCol + wCol;
-                    fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                    if (fDeviation >= fTmpDeviation)
-                    { // 不合法
-                        byValidFlag = 0;
-                        break;
+                    // // 2东北角
+                    // if ((wRow + 1) < wMatrixRow && (wCol + 1) < wMatrixCol)
+                    // {
+                    //     wTmpIndex = (wRow + 1) * wMatrixCol + wCol + 1;
+                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                    //     if (fDeviation >= fTmpDeviation)
+                    //     { // 不合法
+                    //         byValidFlag = 0;
+                    //         break;
+                    //     }
+                    // }
+
+                    // 3北方
+                    if ((wRow + 1) < wMatrixRow)
+                    {
+                        wTmpIndex = (wRow + 1) * wMatrixCol + wCol;
+                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                        if (fDeviation >= fTmpDeviation)
+                        { // 不合法
+                            byValidFlag = 0;
+                            break;
+                        }
+                        byValidFlag++;
                     }
-                }
 
-                // // 4西北角
-                // if ((wRow + 1) < wMatrixRow && (wCol - 1) >= 0)
-                // {
-                //     wTmpIndex = (wRow + 1) * wMatrixCol + wCol - 1;
-                //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                //     if (fDeviation >= fTmpDeviation)
-                //     { // 不合法
-                //         byValidFlag = 0;
-                //         break;
-                //     }
-                // }
-
-                // 5西方
-                if ((wCol - 1) >= 0)
-                {
-                    wTmpIndex = wRow * wMatrixCol + wCol - 1;
-                    fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                    if (fDeviation >= fTmpDeviation)
-                    { // 不合法
-                        byValidFlag = 0;
-                        break;
+                    // // 4西北角
+                    // if ((wRow + 1) < wMatrixRow && (wCol - 1) >= 0)
+                    // {
+                    //     wTmpIndex = (wRow + 1) * wMatrixCol + wCol - 1;
+                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                    //     if (fDeviation >= fTmpDeviation)
+                    //     { // 不合法
+                    //         byValidFlag = 0;
+                    //         break;
+                    //     }
+                    // }
+
+                    // 5西方
+                    if ((wCol - 1) >= 0)
+                    {
+                        wTmpIndex = wRow * wMatrixCol + wCol - 1;
+                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                        if (fDeviation >= fTmpDeviation)
+                        { // 不合法
+                            byValidFlag = 0;
+                            break;
+                        }
+                        byValidFlag++;
+                    }
+
+                    // // 6西南角
+                    // if ((wRow - 1) >= 0 && (wCol - 1) >= 0)
+                    // {
+                    //     wTmpIndex = (wRow - 1) * wMatrixCol + wCol - 1;
+                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                    //     if (fDeviation >= fTmpDeviation)
+                    //     { // 不合法
+                    //         byValidFlag = 0;
+                    //         break;
+                    //     }
+                    // }
+
+                    // 7南方
+                    if ((wRow - 1) >= 0)
+                    {
+                        wTmpIndex = (wRow - 1) * wMatrixCol + wCol;
+                        fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                        if (fDeviation >= fTmpDeviation)
+                        { // 不合法
+                            byValidFlag = 0;
+                            break;
+                        }
+                        byValidFlag++;
                     }
+
+                    // // 8东南角
+                    // if ((wRow - 1) >= 0 && (wCol + 1) < wMatrixCol)
+                    // {
+                    //     wTmpIndex = (wRow - 1) * wMatrixCol + wCol + 1;
+                    //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
+                    //     if (fDeviation >= fTmpDeviation)
+                    //     { // 不合法
+                    //         byValidFlag = 0;
+                    //         break;
+                    //     }
+                    // }
                 }
 
-                // // 6西南角
-                // if ((wRow - 1) >= 0 && (wCol - 1) >= 0)
-                // {
-                //     wTmpIndex = (wRow - 1) * wMatrixCol + wCol - 1;
-                //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                //     if (fDeviation >= fTmpDeviation)
-                //     { // 不合法
-                //         byValidFlag = 0;
-                //         break;
-                //     }
-                // }
-
-                // 7南方
-                if ((wRow - 1) >= 0)
+                // 当前点合法，保存该点
+                if (byValidFlag == byValidThres)
                 {
-                    wTmpIndex = (wRow - 1) * wMatrixCol + wCol;
-                    fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                    if (fDeviation >= fTmpDeviation)
-                    { // 不合法
-                        byValidFlag = 0;
+                    byWhildFlag = 0;
+                    stMinimumValue.stCoord = pstRefPointMatrix[wIndex];
+                    stMinimumValue.fDelayDeviationSum = fDeviation;
+                    astMinmumValueList[wCounter] = stMinimumValue;
+
+                    wCounter++;
+                    if (wCounter >= 128)
+                    {
                         break;
                     }
                 }
-
-                // // 8东南角
-                // if ((wRow - 1) >= 0 && (wCol + 1) < wMatrixCol)
-                // {
-                //     wTmpIndex = (wRow - 1) * wMatrixCol + wCol + 1;
-                //     fTmpDeviation = pfDelayDeviationSumMatrix[wTmpIndex];
-                //     if (fDeviation >= fTmpDeviation)
-                //     { // 不合法
-                //         byValidFlag = 0;
-                //         break;
-                //     }
-                // }
             }
 
-            // 当前点合法，保存该点
-            if (byValidFlag)
+            if (wCounter >= 128)
             {
-                stMinimumValue.stCoord = pstRefPointMatrix[wIndex];
-                stMinimumValue.fDelayDeviationSum = fDeviation;
-                astMinmumValueList[wCounter] = stMinimumValue;
-
-                wCounter++;
-                if (wCounter >= 128)
-                {
-                    break;
-                }
+                byWhildFlag = 0;
+                break;
             }
         }
-
-        if (wCounter >= 128)
-        {
-            break;
-        }
-    }
+    } while (byWhildFlag);
 
     // 有极小值存在
     if (wCounter > 0)
@@ -702,7 +721,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
         dwX = pstMiminumValueData->stCoord.dwX;
         if (dwGapX < 350)
         {
-            dwX = util_distance_gen_rand_num((dwMaxValueX + dwMinValueX) / 2, 50);
+            dwX = util_distance_gen_rand_num((dwMaxValueX + dwMinValueX) / 2, dwGapX / 2);
         }
         else if (dwGapX >= 350 && dwGapX < 900)
         {
@@ -718,7 +737,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
         dwY = pstMiminumValueData->stCoord.dwY;
         if (dwGapY < 350)
         {
-            dwY = util_distance_gen_rand_num((dwMaxValueY + dwMinValueY) / 2, 50);
+            dwY = util_distance_gen_rand_num((dwMaxValueY + dwMinValueY) / 2, dwGapY / 10);
         }
         else if (dwGapY >= 350 && dwGapY < 900)
         {
@@ -731,7 +750,7 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
         pstMiminumValueData->stCoord.dwY = dwY;
 
         // 再次检查坐标是否合法
-        if ((dwX >= dwMinValueX) && (dwX <= dwMaxValueX) && (dwY >= dwMinValueY) && (dwY <= dwMaxValueY))
+        if ((dwX >= (dwMinValueX - 300)) && (dwX <= (dwMaxValueX + 300)) && (dwY >= (dwMinValueY - 300)) && (dwY <= (dwMaxValueY + 300)))
         {
             if (byFirstPathFlag == CAL_PATH)
             {
@@ -763,3 +782,90 @@ void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *
 
     return;
 }
+
+// 根据手机在x轴和y轴上的移动方向，校验二维定位结果
+// pstMinmumValueGroup:极小值组
+// pstPredictionCoord:预测坐标
+// pstAheadCoord:前一点坐标
+// byDirectionX:手机在x轴的移动方向，1：向右，-1：向左
+// byDirectionY:手机在y轴的移动方向，1：向上，-1：向下
+void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup, COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord, int8 byDirectionX, int8 byDirectionY)
+{
+    // 校验计算径
+    for (uint8 i = 0; i < pstMinmumValueGroup->byCalMinimumListLen; i++)
+    {
+        // x轴向右移动
+        if (byDirectionX == 1)
+        {
+            if (pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX <= pstAheadCoord->dwX)
+            {
+                pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+            }
+        }
+        // x轴向左移动
+        else if (byDirectionX == -1)
+        {
+            if (pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX >= pstAheadCoord->dwX)
+            {
+                pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+            }
+        }
+
+        // y轴向上移动
+        if (byDirectionY == 1)
+        {
+            if (pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY <= pstAheadCoord->dwY)
+            {
+                pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+            }
+        }
+        // y轴向下移动
+        else if (byDirectionY == -1)
+        {
+            if (pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY >= pstAheadCoord->dwY)
+            {
+                pstMinmumValueGroup->astCalMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+            }
+        }
+    }
+
+    // 校验跟踪径
+    for (uint8 i = 0; i < pstMinmumValueGroup->byTrackMinimumListLen; i++)
+    {
+        // x轴向右移动
+        if (byDirectionX == 1)
+        {
+            if (pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX <= pstAheadCoord->dwX)
+            {
+                pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+            }
+        }
+        // x轴向左移动
+        else if (byDirectionX == -1)
+        {
+            if (pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX >= pstAheadCoord->dwX)
+            {
+                pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwX = pstPredictionCoord->dwX;
+            }
+        }
+
+        // y轴向上移动
+        if (byDirectionY == 1)
+        {
+            if (pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY <= pstAheadCoord->dwY)
+            {
+                pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+            }
+        }
+        // y轴向下移动
+        else if (byDirectionY == -1)
+        {
+            if (pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY >= pstAheadCoord->dwY)
+            {
+                pstMinmumValueGroup->astTrackMinimumList[i].stCoord.dwY = pstPredictionCoord->dwY;
+            }
+        }
+    }
+
+    return;
+}

src/modules/location/lct_coordcal_two.h

@@ -20,4 +20,5 @@ void lct_coordcal_cal_two_point_transmit_times_by_coord(COORD_T stFirstPoint, CO
 void lct_coordcal_cal_delay_by_first_path_index(uint32 dwMainBtsIndex, uint32 dwSlaveBtsIndex, flt32 *pfTwoBtsDelay);
 void lct_coordcal_sel_minimum_value(flt32 *pfDelayDeviationSumMatrix, uint16 wMatrixRow, uint16 wMatrixCol, COORD_T *pstRefPointMatrix, uint8 byFirstPathFlag, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList);
 void lct_coordcal_dbcheck_minimum_value(uint8 byFirstPathFlag, LCT_FIRSTPATH_T *pstFirstPathList, uint8 byFirstPathListLen, LCT_MINIMUM_VALUE_GROUP_T *pstMinimumValueList);
+void lct_coordcal_two_dimension_check(LCT_MINIMUM_VALUE_GROUP_T *pstMinmumValueGroup, COORD_T *pstPredictionCoord, COORD_T *pstAheadCoord, int8 byDirectionX, int8 byDirectionY);
 #endif

src/modules/location/lct_coordsel.c

@@ -364,7 +364,7 @@ void lct_coordsel_organization_minimum_value_list(uint8 byLctType, LCT_MINIMUM_V
 void lct_coordsel_select_final_coord_by_variance(LCT_COORDSEL_OPTIMUM_COORD_T *pstMinimumValueList, uint8 byMinimumValueListLen, COORD_T stPredictionCoord, LCT_COORDSEL_OPTIMUM_COORD_T *pstOptimumCoord, uint8 *pbyResult)
 {
     uint8 byCounter = byMinimumValueListLen; // 极小值数
-    uint8 byOldCounter = 0;
+    uint8 byOldCounter = byMinimumValueListLen;
 
     // 1、过滤极小值跳变点
     if (byMinimumValueListLen > 0)

src/modules/location/lct_firstpath.c

@@ -100,7 +100,7 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
         // TODO测试
         if (PUB_DEBUG_ENABLE)
         {
-            if (pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 0)
+            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);
@@ -148,7 +148,7 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
                 // TODO测试
                 if (PUB_DEBUG_ENABLE)
                 {
-                    if (pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 0)
+                    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;
@@ -195,7 +195,7 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
             // TODO测试
             if (PUB_DEBUG_ENABLE)
             {
-                if (pstFirstPath->wFrameTrackIndex > 0 && pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 0)
+                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;

src/modules/location/lct_prediction.c

@@ -10,17 +10,17 @@
 extern PUB_SYS_STATUS_T gg_stSysStatus;
 extern LCT_COORDSEL_MEAN_COORD_T gLct_stMeanLctCoordList;
 extern LCT_COORDSEL_ORIGINAL_COORD_T gLct_stOriginalLctCoordList;
-extern int32 gLct_dwSpeedX;
-extern int32 gLct_dwSpeedY;
 extern LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T gLct_astPredictionDirectionDataList[PUB_SYS_CARRIER_NUM];
 
 // 根据历史坐标，预测当前一秒的坐标
+// dwBulidId:大楼编号
+// dwLayerId:楼层编号
 // pstPredictionCoord:预测坐标（返回参数）
 // pbyPredictionFlag:预测标识（返回参数），0：预测失败，1：预测成功
 // pstAheadPointCoord:前一个点坐标（返回参数）
-// dwBulidId:大楼编号
-// dwLayerId:楼层编号
-void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord)
+// pdwSpeedX:x轴位移速度（返回参数）
+// pdwSpeedY:y轴位移速度（返回参数）
+void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord, int32 *pdwSpeedX, int32 *pdwSpeedY)
 {
     LCT_COORDSEL_MEAN_COORD_T *pstData = (LCT_COORDSEL_MEAN_COORD_T *)&gLct_stMeanLctCoordList; // 平滑定位点队列
     uint8 byMeanListLen = pstData->byListLen;                                                   // 平滑定位点队列长度
@@ -69,8 +69,8 @@ void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, C
         stPrediction.dwZ = 120;
 
         // 在X轴和Y轴上的位移
-        gLct_dwSpeedX = (int32)dx + ddx / 2; // 位移速度
-        gLct_dwSpeedY = (int32)dy + ddy / 2; // 位移速度
+        *pdwSpeedX = (int32)dx + ddx / 2; // 位移速度
+        *pdwSpeedY = (int32)dy + ddy / 2; // 位移速度
 
         // 保存预测坐标
         *pstPredictionCoord = stPrediction;
@@ -94,3 +94,180 @@ void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, C
 
     return;
 }
+
+// 预测手机相对于各基站的移动趋势，包括是远离、靠近和不确定三种趋势，1：靠近，2：远离，0：不确定
+// pstPredictionDirectionDataList:预测方向的数据队列
+void lct_prediction_prediction_phone_move_tendency(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;                         // 帧间隔，间隔为8方才合法
+
+    // 运动趋势，1：靠近，2：远离，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 >= (1.5 * pstAheadData->fMaxAmplValue))
+                        {
+                            pstPredictionData->byDirectionTendency = 1; // 靠近
+                        }
+                        else
+                        {
+                            pstPredictionData->byDirectionTendency = 0; // 方向不确定
+                        }
+                    }
+                }
+                // "第一径维度"远离该基站
+                else
+                {
+                    pstPredictionData->byDirectionTendency = 2; // 远离
+
+                    // 上一次是靠近该基站
+                    if (byAheadTendency == 1)
+                    {
+                        // 幅度维度远离该基站
+                        if (pstCurrentData->fMaxAmplValue <= (0.5 * pstAheadData->fMaxAmplValue))
+                        {
+                            pstPredictionData->byDirectionTendency = 2; // 远离
+                        }
+                        else
+                        {
+                            pstPredictionData->byDirectionTendency = 0; // 方向不确定
+                        }
+                    }
+                    // 上一次是远离该基站
+                    else if (byAheadTendency == 2)
+                    {
+                        pstPredictionData->byDirectionTendency = 2; // 远离
+                    }
+                }
+            }
+        }
+    }
+}
+
+// 预测手机相对于每一个基站在X/Y轴移动方向
+// pstPredictionDirectionDataList:预测移动方向的数据队列
+// pbyDirectionX:X轴移动方向
+// pbyDirectionY:Y轴移动方向
+void lct_prediction_prediction_phone_x_y_move_direction(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList, COORD_T stAheadCoord, int8 *pbyDirectionX, int8 *pbyDirectionY)
+{
+    LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstCurrentBtsDirectionData = NULL; // 当前基站方向预测数据
+    int8 byDirectionX = 0;
+    int8 byDirectionY = 0;
+
+    // 结合前一个定位点坐标判断手机在x轴和y轴上的移动方向
+    for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
+    {
+        // 当前基站
+        pstCurrentBtsDirectionData = &pstPredictionDirectionDataList[i];
+        if (pstCurrentBtsDirectionData->byValidFlag && pstCurrentBtsDirectionData->byDirectionTendency)
+        {
+            // 靠近该基站
+            if (pstCurrentBtsDirectionData->byDirectionTendency == 1)
+            {
+                // 前一个点的X轴坐标在基站右侧
+                if (stAheadCoord.dwX > pstCurrentBtsDirectionData->dwCoordX)
+                {
+                    pstCurrentBtsDirectionData->byDirectionX = -1; // 向左移动
+                }
+                else
+                {
+                    pstCurrentBtsDirectionData->byDirectionX = 1; // 向右移动
+                }
+
+                // 前一个点的Y轴坐标在基站上方
+                if (stAheadCoord.dwY > pstCurrentBtsDirectionData->dwCoordY)
+                {
+                    pstCurrentBtsDirectionData->byDirectionY = -1; // 向下移动
+                }
+                else
+                {
+                    pstCurrentBtsDirectionData->byDirectionY = 1; // 向上移动
+                }
+            }
+            // 远离该基站
+            else if (pstCurrentBtsDirectionData->byDirectionTendency == 2)
+            {
+                // 前一个点的X轴坐标在基站右侧
+                if (stAheadCoord.dwX > pstCurrentBtsDirectionData->dwCoordX)
+                {
+                    pstCurrentBtsDirectionData->byDirectionX = 1; // 向右移动
+                }
+                else
+                {
+                    pstCurrentBtsDirectionData->byDirectionX = -1; // 向左移动
+                }
+
+                // 前一个点的Y轴坐标在基站上方
+                if (stAheadCoord.dwY > pstCurrentBtsDirectionData->dwCoordY)
+                {
+                    pstCurrentBtsDirectionData->byDirectionY = 1; // 向上移动
+                }
+                else
+                {
+                    pstCurrentBtsDirectionData->byDirectionY = -1; // 向下移动
+                }
+            }
+
+            byDirectionX += pstCurrentBtsDirectionData->byDirectionX;
+            byDirectionY += pstCurrentBtsDirectionData->byDirectionY;
+        }
+    }
+
+    if (byDirectionX >= 1)
+    {
+        byDirectionX = 1;
+    }
+    else if (byDirectionX <= -1)
+    {
+        byDirectionX = -1;
+    }
+
+    if (byDirectionY >= 1)
+    {
+        byDirectionY = 1;
+    }
+    else if (byDirectionY <= -1)
+    {
+        byDirectionY = -1;
+    }
+
+    *pbyDirectionX = byDirectionX;
+    *pbyDirectionY = byDirectionY;
+
+    return;
+}

src/modules/location/lct_prediction.h

@@ -10,6 +10,8 @@
 #include "./lct_public.h"
 
 // 供外部使用接口
-void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord);
+void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord, int32 *pdwSpeedX, int32 *pdwSpeedY);
+void lct_prediction_prediction_phone_move_tendency(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList);
+void lct_prediction_prediction_phone_x_y_move_direction(LCT_FIRSTPATH_PREDICTION_DIRECTION_DATA_T *pstPredictionDirectionDataList, COORD_T stAheadCoord, int8 *pbyDirectionX, int8 *pbyDirectionY);
 
 #endif

src/sysdefine/struct_lct.h

@@ -132,8 +132,10 @@ typedef struct
     // 第一径备份数据队列
     uint32 dwFirstPathBackupIndex;                                            // 第一径备份数据索引
     LCT_FIRSTPATH_BACKUP_T astFirstPathBackupList[LCT_FIRST_PATH_BACKUP_NUM]; // 第一径备份数据队列，前两秒数据，前一秒数据，当前数据
-    uint8 byAheadDirectionTendency;                                           // 上一秒方向趋势，0:趋势不一致， 1：趋势一致靠近，2：趋势一致远离
-    uint8 byDirectionTendency;                                                // 当前一秒方向趋势，0:趋势不一致， 1：趋势一致靠近，2：趋势一致远离
+    // uint8 byAheadDirectionTendency;                                           // 上一秒方向趋势，0:趋势不一致， 1：趋势一致靠近，2：趋势一致远离
+    uint8 byDirectionTendency; // 当前一秒方向趋势，0:趋势不一致， 1：趋势一致靠近，2：趋势一致远离
+    int8 byDirectionX;         // X轴方向，1：向右、-1：向左，0：未知
+    int8 byDirectionY;         // Y轴方向，1：向上、-1：向下，0：未知
 
     // 基站坐标
     int32 dwCoordX; // 基站x坐标

src/sysdefine/sysmacro.h

@@ -10,9 +10,10 @@
 #define PUB_SOFTWARE_NAME_LEN 11
 #define PUB_SOFTWARE_NAME "APLM8000SDK" // 软件名称：基于音频的手机定位模块开发包
 
-// 2025-2-8 by zt 增加软件版本接口
+// V2.1.00 2025-02-08 by zt 增加软件版本接口
+// V2.1.01 2025-04-14 by zt 修改代码缺陷（修复轨迹跳变，修改极小值选择方法、用运动方向二次校验极小值）
 #define PUB_SOFTWARE_VERSION_LEN 7
-#define PUB_SOFTWARE_VERSION "V2.1.00" // 软件版本
+#define PUB_SOFTWARE_VERSION "V2.1.01" // 软件版本
 
 #define PUB_M_PI 3.14159265358979323846
 #define PUB_2PI (2 * 3.14159265358979323846)
@@ -34,9 +35,9 @@
 #define PUB_SIGNAL_SAMPLE_RATIO 48000
 
 // 信号同步向左偏移量，6000约42米（5000约35米）
-#define PUB_SYNC_INDEX_OFFSET_NUM 5000
+#define PUB_SYNC_INDEX_OFFSET_NUM 5600 // 40米
 
-// TODO 基站有效距离，单位米
+// TODO 基站有效距离，单位米，28米
 #define PUB_BTS_VALID_DISTANCE ((PUB_SYNC_INDEX_OFFSET_NUM * PUB_AUDIO_SPEED) / PUB_SIGNAL_SAMPLE_RATIO)
 
 // 参与定位的基站，最多9个
@@ -70,7 +71,7 @@
 #define PUB_MAC_ADDRESS_LEN 12 // MAC地址字节数
 
 // 测试宏定义
-#define PUB_DEBUG_ENABLE 1 // 打开调试开关
-// #define PUB_DEBUG_ENABLE 0 // 关闭调试开关
+// #define PUB_DEBUG_ENABLE 1 // 打开调试开关
+#define PUB_DEBUG_ENABLE 0 // 关闭调试开关
 
 #endif

src/sysmain/sysmain.c

@@ -61,8 +61,8 @@ void sysmain_init_lct()
     stFirstPathParam.byNoiseSignalFactor = 7;
     stFirstPathParam.byNoiseLowScale = 25;   // 底噪放大低倍数，放大10倍保存
     stFirstPathParam.byNoiseHightScale = 50; // 底噪放大高倍数，放大10倍保存
-    stFirstPathParam.byCfarLeftFactor = 120;
-    stFirstPathParam.bySincFactor = 94; // 95;
+    stFirstPathParam.byCfarLeftFactor = 135; // 120; //update 135 by zt 2025-01-09
+    stFirstPathParam.bySincFactor = 94;      // 95;
     stFirstPathParam.byFirstPathContactFactor = 75;
     stFirstPathParam.wFirstPathContactDistance = 1200; // 8米