完善代码，修改了当前坐标的预测方法，修改一些注释

.vscode/launch.json

@@ -1,14 +1,10 @@
 {
-    // Use IntelliSense to learn about possible attributes.
-    // Hover to view descriptions of existing attributes.
-    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
     "version": "0.2.0",
     "configurations": [
         {
             "name": "(gdb) Launch",
             "type": "cppdbg",
             "request": "launch",
-            //"program": "enter program name, for example ${workspaceFolder}/a.exe",
             "program": "${fileDirname}\\bin\\aplm8000sdk.exe",
             "args": [],
             "stopAtEntry": false,
@@ -16,7 +12,6 @@
             "environment": [],
             "externalConsole": false,
             "MIMode": "gdb",
-            //"miDebuggerPath": "/path/to/gdb",
             "miDebuggerPath": "D:\\Qt\\Tools\\mingw1120_64\\bin\\gdb.exe",
             "setupCommands": [
                 {

aplm8000sdk.code-workspace

@@ -41,7 +41,10 @@
             "lctdata.h": "c",
             "limits": "c",
             "inttypes.h": "c",
-            "route_main.h": "c"
+            "route_main.h": "c",
+            "lct_public.h": "c",
+            "imu.h": "c",
+            "lct_prediction.h": "c"
         }
     }
 }

compile.bat

@@ -34,20 +34,21 @@ set "DEMODULATION=%DEMODU_MAIN% %DEMODU_DDC% %DEMODU_OVERLAP% %DEMODU_FIR_REAL%
 set "BTS_MAIN=src\modules\basestation\bts_main.c"
 set "BTS_AMPL=src\modules\basestation\bts_ampl.c"
 set "BTS_SYNC=src\modules\basestation\bts_sync.c"
-set "BTS_ID=src\modules\basestation\bts_id.c"
-set "BTS_IDCHECK=src\modules\basestation\bts_idcheck.c"
+::set "BTS_ID=src\modules\basestation\bts_id.c"
+::set "BTS_IDCHECK=src\modules\basestation\bts_idcheck.c"
 set "BTS_TRACK=src\modules\basestation\bts_track.c"
-set "BASESTATION=%BTS_MAIN% %BTS_AMPL% %BTS_SYNC% %BTS_ID% %BTS_IDCHECK% %BTS_TRACK%"
+set "BASESTATION=%BTS_MAIN% %BTS_AMPL% %BTS_SYNC% %BTS_TRACK%"
 
 ::3、定位模块
 set "LCT_MAIN=src\modules\location\lct_main.c"
 set "LCT_FIRSTPATH=src\modules\location\lct_firstpath.c"
 set "LCT_COORDCAL=src\modules\location\lct_coordcal.c"
+set "LCT_PREDICTION=src\modules\location\lct_prediction.c"
 set "LCT_COORDCAL_ONE=src\modules\location\lct_coordcal_one.c"
 set "LCT_COORDCAL_TWO=src\modules\location\lct_coordcal_two.c"
 set "LCT_COORDSEL=src\modules\location\lct_coordsel.c"
 set "LCT_SINC=src\modules\location\lct_sinc.c"
-set "LOCATION=%LCT_MAIN% %LCT_FIRSTPATH% %LCT_COORDCAL% %LCT_COORDCAL_ONE% %LCT_COORDCAL_TWO% %LCT_COORDSEL% %LCT_SINC%"
+set "LOCATION=%LCT_MAIN% %LCT_FIRSTPATH% %LCT_COORDCAL% %LCT_PREDICTION% %LCT_COORDCAL_ONE% %LCT_COORDCAL_TWO% %LCT_COORDSEL% %LCT_SINC%"
 
 ::4、路由模块
 set "ROUTE_MAIN=src\modules\route\route_main.c"

doc/二维方向预测.docx

@@ -0,0 +1,527 @@
+--------------------------------------------------------------------------------
+这里有一个需求，即"我在设计一套基于声波适用于手机的室内定位系统，系统框架是声波基站发射声信号，手机接收多个基站的声信号并在本地解算手机位置。系统采用TDOA原理，在进行二维定位时，需要根据基站幅度（连续3秒钟幅度，即前前一秒，前一秒，当前一秒）、基站第一径索引（连续3秒钟第一径索引，即前前一秒，前一秒，当前一秒）和4个历史定位坐标，预测手机的移动方向，方向用与X轴的夹角表示。 可用的参数如下： 1、每个定位基站的坐标(x,y,z)，在每次定位中最少可用3个基站最多可用9个基站，每个基站的载波频率均不同。 2、每个基站三秒最大幅度（前前一秒，前一秒，当前一秒）。 3、每个基站三秒第一径索引（前前一秒，前一秒，当前一秒）。 4、该定位系统每一秒输出一个定位坐标。 5、4个历史定位点坐标。 注意： 1、算法一定要考虑把每个基站的幅度和第一径索引用上。 2、用C代码写，并且独立的功能要写成单独的函数。 3、基站坐标数据类型是int32，单位是cm 4、第一径索引数据类型是uint32 5、所有结构体定位都要写上注释 6、所有函数的输入参数和输出参数都要写上注释 7、函数内的功能块也要写上注释 8、基站频率数据类型为uint32，单位hz 9、是否需要就行多普勒补偿，请根据算法考虑 10、算法是在手机端运行。 11、要充分考虑遮挡带来的幅度与第一径的不稳定情况。 假如你是一名算法工程，请针对这个需求写一个算法
+--------------------------------------------------------------------------------
+已深度思考（用时 368 秒）
+好的，我现在需要帮用户设计一个基于声波的室内定位系统算法，采用TDOA原理，二维定位，还要利用基站的幅度、第一径索引和四个历史坐标来预测移动方向。用户的要求挺详细的，我得仔细分析每个点，确保不遗漏。
+首先，用户提到系统使用TDOA，也就是到达时间差。TDOA通常需要至少三个基站来计算位置，但这里可能更多，最多九个。不过用户需要的是二维定位，所以可能需要处理三维坐标投影到二维的情况，或者基站都在同一平面上？可能需要确认，但用户没说，暂时假设是二维，忽略z轴。
+关键点在于预测移动方向，用与X轴的夹角表示。需要结合基站的幅度和第一径索引，还有四个历史坐标。这四个坐标应该是用来计算移动趋势的，比如速度、方向的变化。但用户强调算法必须使用每个基站的幅度和第一径索引，所以这两个参数需要被整合到方向预测中。
+接下来，参数方面，每个基站有坐标（x,y,z，int32 cm单位），载波频率不同。幅度是连续三秒的最大幅度，第一径索引也是三秒的数据。定位系统每秒输出一个坐标，所以历史坐标是前四秒的数据。
+需要考虑的问题包括多普勒效应是否需要补偿。因为手机在移动，声波的频率可能会因多普勒效应而改变，特别是当手机快速移动时。载波频率不同的基站可能需要不同的处理，但用户提到是否需要补偿由算法决定。所以得判断在TDOA中是否需要补偿多普勒带来的时间差误差。如果手机移动速度快，可能需要补偿，否则可能忽略。但这里可能需要结合第一径索引的变化来判断速度，进而决定是否需要补偿。
+遮挡问题会影响幅度和第一径的稳定性。比如，当有遮挡时，幅度可能突然下降，第一径索引可能跳变。算法需要处理这些异常情况，可能采用滤波或加权方法，减少不稳定数据的影响。
+接下来，如何将幅度和第一径索引用于方向预测。幅度可能反映信号的强度，可能与距离相关，或者遮挡情况。第一径索引可能反映信号的到达时间，即路径延迟。在TDOA中，第一径通常是直射路径，所以第一径索引的变化可能反映手机移动导致的路径变化。但如何将这些参数与方向预测结合？
+可能的思路是：使用历史坐标计算移动方向的变化趋势，例如通过加权平均或卡尔曼滤波预测当前方向。同时，利用幅度和第一径索引的变化来调整这个预测。例如，某个基站的幅度连续增加，可能表示手机正在接近该基站，从而影响方向预测。或者，第一径索引的突然变化可能意味着多径效应或遮挡，需要调整权重。
+结构体方面，需要定义基站的信息，包括坐标、频率、三秒的幅度和第一径索引。历史坐标是四个点，可能存储为数组。预测方向的结果是角度。
+函数设计方面，可能需要几个独立函数：
+1. 预处理数据，处理幅度和第一径的异常，比如检测遮挡情况，进行滤波。
+2. 计算多普勒补偿，如果需要的话。
+3. 基于历史坐标计算初步移动方向（比如，线性回归或平均位移向量）。
+4. 使用幅度和第一径索引调整方向，比如通过加权平均，根据各个基站的变化情况调整方向向量的权重。
+5. 综合所有因素，输出最终方向。
+关于多普勒补偿，计算手机相对于每个基站的速度，可能需要历史坐标的速度估计。然后根据速度方向和基站的方向，计算频率偏移，调整TDOA计算。不过用户提到算法在手机端运行，可能计算资源有限，所以是否实现需要权衡。如果用户的历史坐标足够频繁，可能可以估计速度，进而计算多普勒效应的影响，进行补偿。
+遮挡处理方面，当某个基站的幅度突然下降，或第一径索引突变，可能认为该基站的数据不可靠，降低其权重或排除在外。或者使用移动平均或其他滤波方法平滑数据。
+现在，如何将幅度和第一径索引的变化转换为方向调整？例如，幅度增加可能意味着靠近基站，而幅度减少可能远离。但多个基站的情况，需要综合考虑。可能需要计算每个基站的信号强度变化趋势，结合其位置，推断移动方向。例如，如果三个基站中，A的幅度增加，B的幅度减少，C的幅度稳定，可能手机在朝A移动，远离B，从而方向可能偏向A的方向。
+第一径索引的变化可能反映路径延迟的变化。如果第一径索引增加，可能意味着路径变长（比如手机远离基站），反之则缩短。但需要考虑声波的速度，以及采样率，将索引转换为时间差，进而计算距离变化。这可能比较复杂，需要知道系统的具体参数，比如采样率，但用户没有给出，可能需要假设。
+另外，结合TDOA定位的结果，可能已经使用了第一径索引来确定到达时间差，所以这里可能需要用这些索引的变化率来估计速度向量，进而辅助方向预测。但用户的需求是在定位之后，用这些参数预测方向，所以可能需要将第一径索引的变化与历史位置的变化结合，例如，比较两者的趋势是否一致，若不一致可能需要调整。
+现在，具体的步骤可能如下：
+1. 预处理每个基站的幅度和第一径索引，检测异常，进行滤波处理（如移动平均，或剔除异常值）。
+2. 计算历史坐标的移动向量，例如用最后两个坐标计算当前速度方向。
+3. 对于每个基站，分析幅度和第一径索引的趋势，判断手机是靠近还是远离该基站。
+4. 根据基站的分布和趋势，调整移动方向。例如，如果多数基站的幅度趋势显示手机向东移动，则方向可能向东。
+5. 综合历史坐标的趋势和基站的信号趋势，得到最终方向。
+在代码结构上，需要定义结构体来存储基站的信息，历史坐标。函数包括滤波处理、方向预测、多普勒补偿判断等。
+需要注意的是，用户要求用C代码，函数独立，结构体注释详细。所以需要先定义结构体，如基站信息结构体，包含坐标、频率、三秒的幅度和第一径索引。历史坐标数组。
+函数可能包括：
+- 初始化结构体
+- 预处理数据（滤波处理）
+- 计算历史移动方向
+- 分析基站信号趋势
+- 综合预测方向
+- 多普勒补偿计算（如果需要）
+现在，如何处理遮挡情况？例如，某个基站的幅度突然下降，可能该基站的数据不可靠，需要降低其权重。或者在计算趋势时，使用鲁棒的方法，如中位数而不是平均值。
+关于多普勒补偿，假设手机移动速度v，载波频率f，则多普勒频移Δf = (v·cosθ)/c * f，其中θ是手机移动方向与基站方向的夹角，c是声速。补偿的话可能需要调整到达时间差的估计，但具体如何整合到方向预测中还不清楚。可能如果多普勒效应明显，会影响第一径索引的测量，因此需要进行补偿，以提高TDOA的准确性。但用户的需求是在定位之后的移动方向预测，所以可能多普勒补偿是在定位解算阶段，而方向预测阶段可能不需要。但用户要求算法考虑是否需要多普勒补偿，所以可能在处理第一径索引时需要考虑多普勒带来的误差。
+不过这可能比较复杂，可能需要先估计手机的速度，然后计算多普勒频移，调整载波频率，进而影响第一径的检测。这部分可能在信号处理阶段，但用户提供的参数中，第一径索引已经是处理后的数据，所以可能不需要在方向预测阶段处理多普勒补偿，但需要确认。
+或者，是否在第一径索引的变化中已经反映了多普勒效应的影响？例如，移动导致接收信号的时间压缩或扩展，从而影响第一径的检测。这种情况下，可能需要通过多普勒补偿来纠正第一径索引的测量，以提高TDOA的准确性。但用户的问题是，在预测移动方向时是否需要考虑多普勒补偿，可能是在定位解算阶段，而方向预测可能基于已经补偿过的位置数据。不过用户的需求是，在方向预测的算法中要考虑是否需要多普勒补偿，所以可能需要结合速度估计来判断。
+例如，在方向预测中，如果检测到手机移动速度较高，可能需要启用多普勒补偿，修正第一径索引的值，从而更准确地计算TDOA，得到更准确的历史坐标，进而预测方向。但用户给的参数中，可能已经包含了经过多普勒补偿后的第一径索引，或者需要在这里处理。
+这部分可能需要进一步分析。但根据用户的问题，可能需要在算法中考虑是否需要多普勒补偿，因此在代码中可能需要有相应的函数来判断，比如根据历史坐标估计速度，如果速度超过阈值，则进行补偿。
+现在，综合以上思路，开始组织代码结构。
+首先定义结构体：
+1. 基站信息结构体：包含坐标、频率、三秒幅度数组、三秒第一径索引数组。
+2. 定位参数结构体：包含多个基站信息，数量，历史坐标数组。
+然后函数：
+- 预处理数据：对每个基站的幅度和第一径索引进行滤波，处理遮挡情况。
+- 计算移动方向：基于历史坐标，计算初步方向。
+- 分析基站信号：根据幅度和第一径索引的变化，调整方向。
+- 多普勒补偿判断：根据速度判断是否需要补偿，并调整第一径索引。
+- 综合预测方向：结合历史方向和基站信号分析，得到最终角度。
+但用户要求函数独立，所以每个功能要分开。
+现在，处理遮挡带来的不稳定情况，比如幅度突然下降，可能使用滑动窗口的方差检测，或者判断连续变化是否合理。例如，如果当前幅度比前两秒的平均值低很多，可能认为被遮挡，降低其权重。
+对于第一径索引，如果突然变大或变小，可能路径发生变化，同样需要处理。
+在预测方向时，可能需要给每个基站一个权重，根据其信号的稳定性，然后综合这些权重调整方向。
+例如，历史坐标计算的方向是基础方向，然后每个基站根据其幅度趋势和第一径索引趋势，给出一个方向调整的建议，最终加权平均。
+或者，幅度增加的基站可能指示手机靠近该基站，因此移动方向可能与该基站的方向有关联。
+例如，假设基站位于东边，如果手机向东移动，该基站的幅度可能增加，第一径索引可能减少（因为距离缩短，到达时间更早）。因此，可以将每个基站的趋势转换为一个方向向量，然后综合这些向量得到调整后的方向。
+具体来说，对于每个基站，计算幅度的变化率（当前秒与前两秒的比较），以及第一径索引的变化率。结合这些变化率，判断手机相对于该基站的移动方向（靠近或远离），然后结合基站的方位，得到一个方向向量。所有基站的方向向量加权平均，得到总调整方向，再与历史坐标计算的方向结合，得到最终方向。
+这似乎比较复杂，但可能需要这样的处理。
+另外，多普勒补偿可能需要根据手机的速度和基站的方向来计算频偏，进而调整第一径的检测。但如何整合到当前系统中？例如，如果检测到手机移动速度较快，则对每个基站的第一径索引进行补偿，以纠正多普勒效应带来的误差。这部分可能需要知道信号的采样率，将第一径索引转换为时间，再计算实际的时间差。
+例如，第一径索引是检测到的信号到达的采样点索引，假设采样率为fs，则时间t = index / fs。多普勒效应会导致实际传播时间的变化，从而影响索引值。因此，补偿时需要估计手机的速度，计算传播时间的改变量，调整索引。
+不过这部分可能在TDOA定位解算时就需要处理，而方向预测是基于已经解算的位置数据，因此可能多普勒补偿在定位阶段处理，而方向预测阶段不需要。但用户的问题要求算法考虑是否需要多普勒补偿，因此可能需要在方向预测时，如果检测到高速移动，则对第一径索引进行调整，以更准确地反映实际距离变化。
+但具体实现可能比较复杂，需要更多的参数，比如采样率、声速等，用户没有给出，可能需要假设或忽略。
+现在，开始编写代码的大纲。
+首先定义结构体：
+// 基站信息结构体
+typedef struct {
+int32_t x; // cm
+int32_t y;
+int32_t z;
+uint32_t frequency; // Hz
+uint32_t amplitude[3]; // 前前一秒，前一秒，当前一秒
+uint32_t fp_index[3]; // 第一径索引，同上
+} BeaconInfo;
+// 定位参数结构体
+typedef struct {
+BeaconInfo *beacons; // 基站数组指针
+uint8_t num_beacons; // 基站数量，3-9
+int32_t history_x[4]; // 历史x坐标，最新最后
+int32_t history_y[4]; // 历史y坐标
+} PositioningParams;
+然后，函数：
+预处理函数：处理幅度和第一径索引，检测遮挡，进行平滑处理。
+void preprocessBeaconData(BeaconInfo *beacons, uint8_t num_beacons) {
+// 对每个基站的幅度和第一径索引进行滤波处理
+// 例如，移动平均或中值滤波，检测异常
+}
+计算历史移动方向：
+float calculateHistoricalDirection(int32_t *x, int32_t *y) {
+// 根据四个历史坐标，计算平均移动方向
+// 例如，取最后两个点的位移向量，或线性回归
+}
+分析基站信号趋势：
+void analyzeBeaconTrends(BeaconInfo *beacons, uint8_t num_beacons, float *trend_angle) {
+// 根据每个基站的幅度和第一径索引变化，分析趋势
+// 转换为方向调整
+}
+多普勒补偿判断：
+bool needsDopplerCompensation(int32_t *history_x, int32_t *history_y) {
+// 根据历史坐标计算速度，判断是否需要补偿
+// 返回true或false
+}
+综合预测方向：
+float predictMovementDirection(PositioningParams *params) {
+// 综合历史方向和基站趋势，得到最终角度
+}
+主函数可能调用这些函数，并返回方向角度。
+接下来，详细处理每个函数。
+预处理函数可能需要检测每个基站的幅度是否有突变。例如，当前秒的幅度比前两秒的平均值低很多，可能认为被遮挡。可以使用阈值或标准差来判断。例如，如果当前幅度小于前两秒的平均值的一半，视为遮挡，标记该基站的数据不可靠。
+处理方式可能包括：使用滑动平均滤波，或者剔除异常值后用剩余数据计算趋势。
+例如，对于每个基站的幅度数组，计算当前值与之前值的差异，若差异超过阈值，则替换为前值的平均值。
+类似地处理第一径索引。如果索引突变，可能路径变化，但如何判断？比如连续三秒的索引是否有较大波动，可以使用差分检测。
+然后，计算历史方向。例如，用四个历史点，可以计算三个位移向量（从t-3到t-2，t-2到t-1，t-1到t），然后取平均方向。或者使用加权平均，最近的位移权重更大。
+例如，最新的位移向量（history[3] - history[2]）的权重较大，而较早的位移权重较小。
+然后，分析基站趋势。对于每个基站，计算幅度变化率和第一径索引变化率。幅度增加可能表示靠近，反之远离。第一径索引增加可能表示距离增加，因为到达时间推迟。
+结合基站的坐标，可以推断移动方向。例如，假设基站位于东边，幅度增加，可能手机向东移动。每个基站的这种趋势可以生成一个方向向量，所有基站的向量加权平均得到总趋势方向。
+权重可以根据幅度变化的显著性和基站的可靠性（如是否被遮挡）来决定。
+例如，幅度变化越大，权重越高；被遮挡的基站权重降低。
+最后，综合历史方向和基站趋势方向，可能取加权平均或向量相加，得到最终方向。
+多普勒补偿的判断可能需要计算手机的速度。例如，根据历史坐标计算速度，如果速度超过某个阈值（例如1m/s），则启用补偿。
+如果需要补偿，如何调整第一径索引？可能需要计算每个基站的预期多普勒频移，然后调整索引值。例如，假设手机移动导致接收频率变化，从而影响信号的处理，进而影响第一径的检测。这部分可能需要信号处理的知识，但用户可能希望简化处理，比如根据速度调整第一径索引的值。
+例如，计算手机相对于基站的速度分量，使用多普勒公式计算时间拉伸或压缩，从而调整第一径索引。但这需要知道信号的波长和采样率。
+假设载波频率为f，声速c=343m/s，波长λ = c/f。手机移动速度v在基站方向的分量为v_dir，则多普勒频移Δf = (v_dir / c) * f。这会导致接收信号的频率变化，进而影响信号处理中的相关峰位置，即第一径索引。
+因此，可能需要根据速度估计来调整第一径索引，以补偿多普勒效应的影响。例如，如果手机正朝向基站移动，接收信号频率变高，导致相关峰提前，第一径索引可能变小。补偿时，应调整索引值以反映实际的时间差。
+这部分的具体实现可能需要详细的信号处理模型，但在此问题中可能需要简化。例如，计算速度后，对每个基站的第一径索引进行补偿，然后再用于TDOA计算。但用户的问题是在方向预测阶段使用这些参数，所以可能需要将补偿后的第一径索引用于分析趋势。
+但代码中，用户可能希望将多普勒补偿作为一个函数，判断是否需要，然后调整第一径索引的值。
+综上，整个算法的步骤大致如下：
+1. 预处理每个基站的幅度和第一径索引，处理遮挡带来的不稳定。
+2. 判断是否需要多普勒补偿，如果需要，调整第一径索引。
+3. 使用历史坐标计算初步的移动方向。
+4. 分析基站的幅度和第一径索引趋势，生成方向调整向量。
+5. 综合历史方向和基站趋势，得到最终的移动方向角度。
+现在，将这些步骤转化为代码。
+预处理函数可能包括对每个基站的幅度和第一径进行滤波，例如中值滤波：
+void preprocessBeaconData(BeaconInfo *beacons, uint8_t num_beacons) {
+for (int i = 0; i < num_beacons; i++) {
+// 幅度处理：检测当前值是否异常
+uint32_t curr_amp = beacons[i].amplitude[2];
+uint32_t prev_amp_avg = (beacons[i].amplitude[0] + beacons[i].amplitude[1]) / 2;
+if (curr_amp < prev_amp_avg * 0.5) { // 假设当前幅度比前两秒平均低一半，视为遮挡
+// 使用前两秒的平均值替代当前值
+beacons[i].amplitude[2] = prev_amp_avg;
+}
+// 类似处理第一径索引
+uint32_t curr_fp = beacons[i].fp_index[2];
+uint32_t prev_fp_avg = (beacons[i].fp_index[0] + beacons[i].fp_index[1]) / 2;
+if (abs((int)curr_fp - (int)prev_fp_avg) > 10) { // 假设索引变化超过10为异常
+beacons[i].fp_index[2] = prev_fp_avg;
+}
+}
+}
+但需要根据实际情况调整阈值。
+多普勒补偿判断：
+bool needsDopplerCompensation(int32_t *history_x, int32_t *history_y) {
+// 计算最近两个点的速度
+int32_t dx = history_x[3] - history_x[2];
+int32_t dy = history_y[3] - history_y[2];
+float speed = sqrtf(dx*dx + dy*dy) / 1.0; // 单位cm/s，因为每秒一个点
+// 如果速度超过阈值，如100 cm/s（1 m/s），则需要补偿
+return (speed > 100.0f);
+}
+如果启用补偿，可能需要调整第一径索引：
+void applyDopplerCompensation(BeaconInfo *beacon, float speed_x, float speed_y) {
+// 计算手机相对于基站的速度方向
+int32_t dx = beacon->x - history_x[3]; // 最新坐标到基站的方向
+int32_t dy = beacon->y - history_y[3];
+float distance = sqrtf(dx*dx + dy*dy);
+float dir_x = dx / distance;
+float dir_y = dy / distance;
+float radial_speed = speed_x * dir_x + speed_y * dir_y; // 径向速度，单位cm/s
+float delta_f = (radial_speed / 34300.0f) * beacon->frequency; // 多普勒频移
+// 计算对第一径索引的影响，假设采样率为fs，索引变化与时间差有关
+// 例如，时间差 delta_t = (delta_f / beacon->frequency) * (1 / fs) * N?
+// 这部分需要更多系统参数，可能无法具体实现，此处仅示例
+// 假设采样率fs=48000Hz，每个样本时间约20.8微秒
+// 时间差 delta_t = radial_speed / 34300.0f * (1 / beacon->frequency) ?
+// 这部分可能需要更准确的模型，这里仅示意调整
+int32_t index_adjust = (int32_t)(delta_f * 0.001); // 假设调整量，具体根据实际情况
+beacon->fp_index[2] += index_adjust;
+}
+但用户没有提供足够的参数，这部分可能需要省略或简化。
+计算历史方向：
+float calculateHistoricalDirection(int32_t *x, int32_t *y) {
+// 取最近两个点的位移向量
+int32_t dx = x[3] - x[2];
+int32_t dy = y[3] - y[2];
+if (dx == 0 && dy == 0) {
+// 没有移动，返回之前的方向或0
+// 如果历史点更多，可以取更早的位移
+dx = x[3] - x[1];
+dy = y[3] - y[1];
+}
+float angle = atan2f(dy, dx) * 180 / M_PI;
+return angle;
+}
+分析基站趋势：
+void analyzeBeaconTrends(PositioningParams *params, float *trend_angle) {
+float sum_weight = 0.0f;
+float sum_x = 0.0f;
+float sum_y = 0.0f;
+for (int i = 0; i < params->num_beacons; i++) {
+BeaconInfo *beacon = &params->beacons[i];
+// 计算幅度趋势：当前秒与前两秒的平均比较
+uint32_t amp_prev_avg = (beacon->amplitude[0] + beacon->amplitude[1]) / 2;
+int32_t amp_trend = (int32_t)beacon->amplitude[2] - amp_prev_avg;
+// 计算第一径索引趋势
+uint32_t fp_prev_avg = (beacon->fp_index[0] + beacon->fp_index[1]) / 2;
+int32_t fp_trend = (int32_t)beacon->fp_index[2] - fp_prev_avg;
+// 根据趋势判断移动方向：假设幅度增加和fp减少表示靠近
+float weight = 0.0f;
+if (amp_trend > 0 && fp_trend < 0) {
+weight = 1.0f; // 靠近，高权重
+} else if (amp_trend < 0 && fp_trend > 0) {
+weight = -1.0f; // 远离，负权重
+} else {
+// 趋势不明显，权重低
+weight = 0.5f * ((amp_trend != 0) ? (amp_trend / 100.0f) : 0) + 0.5f * ((fp_trend != 0) ? (-fp_trend / 10.0f) : 0);
+}
+// 基站的方向向量
+int32_t beacon_dx = beacon->x - params->history_x[3];
+int32_t beacon_dy = beacon->y - params->history_y[3];
+float distance = sqrtf(beacon_dx*beacon_dx + beacon_dy*beacon_dy);
+if (distance < 1.0f) distance = 1.0f; // 防止除以零
+float dir_x = beacon_dx / distance;
+float dir_y = beacon_dy / distance;
+// 综合权重和方向
+sum_x += weight * dir_x;
+sum_y += weight * dir_y;
+sum_weight += fabs(weight);
+}
+if (sum_weight == 0) {
+*trend_angle = 0.0f;
+return;
+}
+// 平均趋势方向
+sum_x /= sum_weight;
+sum_y /= sum_weight;
+*trend_angle = atan2f(sum_y, sum_x) * 180.0f / M_PI;
+}
+然后，综合历史方向和趋势方向：
+float predictMovementDirection(PositioningParams *params) {
+// 预处理数据
+preprocessBeaconData(params->beacons, params->num_beacons);
+// 检查多普勒补偿
+bool need_comp = needsDopplerCompensation(params->history_x, params->history_y);
+if (need_comp) {
+// 计算速度
+int32_t dx = params->history_x[3] - params->history_x[2];
+int32_t dy = params->history_y[3] - params->history_y[2];
+for (int i = 0; i < params->num_beacons; i++) {
+applyDopplerCompensation(&params->beacons[i], dx, dy);
+}
+}
+// 计算历史方向
+float historical_angle = calculateHistoricalDirection(params->history_x, params->history_y);
+// 分析基站趋势方向
+float trend_angle;
+analyzeBeaconTrends(params, &trend_angle);
+// 综合两个方向，例如加权平均
+float final_angle = 0.7f * historical_angle + 0.3f * trend_angle;
+return final_angle;
+}
+这只是一个简化的示例，实际可能需要更复杂的加权或向量相加。
+需要注意的几点：
+1. 预处理函数中的遮挡检测和滤波需要根据实际数据调整阈值。
+2. 多普勒补偿的具体实现需要更多系统参数，这里仅示例。
+3. 方向综合的权重可能需要根据实际情况调整，或者动态计算。
+4. 处理浮点运算时要注意单位转换，例如坐标是cm，速度是cm/s等。
+5. 历史坐标的处理需要考虑可能的静止情况，避免除以零。
+此外，用户要求所有结构体和函数都有注释，输入输出参数也要注释。代码中需要确保这一点。
+最后，测试各种情况，如遮挡、高速移动、不同基站分布等，确保算法的鲁棒性。
+以下是根据需求设计的算法实现，包含详细注释和结构体定义：
+--------------------------------------------------------------------------------
+c
+--------------------------------------------------------------------------------
+复制
+#include <stdio.h>#include <stdint.h>#include <math.h>#include <stdbool.h>
+// 基站信息结构体typedef struct {
+    int32_t x;          // X坐标(cm)
+    int32_t y;          // Y坐标(cm)
+    int32_t z;          // Z坐标(cm)
+    uint32_t frequency; // 载波频率(Hz)
+    uint32_t amplitude[3]; // 幅度序列[前前秒, 前秒, 当前秒]
+    uint32_t fp_index[3];  // 第一径索引序列[前前秒, 前秒, 当前秒]} BeaconInfo;
+// 定位参数结构体typedef struct {
+    BeaconInfo* beacons;    // 基站数组指针
+    uint8_t num_beacons;    // 有效基站数量(3-9)
+    int32_t history_x[4];   // 历史X坐标[最早,...,最新]
+    int32_t history_y[4];   // 历史Y坐标[最早,...,最新]} PositioningParams;
+// 移动向量结构体typedef struct {
+    float dx;       // X方向分量
+    float dy;       // Y方向分量
+    float weight;   // 向量权重} MotionVector;
+/* 预处理基站数据（滤波与异常处理） */void preprocessBeaconData(BeaconInfo* beacons, uint8_t num_beacons) {
+    /* 输入参数：
+       - beacons: 基站信息数组
+       - num_beacons: 基站数量
+       处理逻辑：
+       1. 对幅度和第一径索引进行中值滤波
+       2. 检测并处理遮挡引起的异常值
+    */
+    for(int i=0; i<num_beacons; i++) {
+        // 幅度中值滤波
+        uint32_t amp[3] = {beacons[i].amplitude[0], beacons[i].amplitude[1], beacons[i].amplitude[2]};
+        if((amp[2] < amp[0]*0.4) && (amp[2] < amp[1]*0.4)) { // 幅度骤降超过60%
+            // 使用前两秒平均值替代当前值
+            beacons[i].amplitude[2] = (amp[0] + amp[1]) / 2;
+        }
+
+        // 第一径索引突变检测
+        uint32_t fp[3] = {beacons[i].fp_index[0], beacons[i].fp_index[1], beacons[i].fp_index[2]};
+        if(abs((int)fp[2] - (int)fp[1]) > 10 && abs((int)fp[2] - (int)fp[0]) > 10) {
+            // 使用线性预测值替代异常值
+            beacons[i].fp_index[2] = 2*fp[1] - fp[0];
+        }
+    }}
+/* 计算历史移动向量 */
+MotionVector calculateHistoricalMotion(const PositioningParams* params) {
+    /* 输入参数：
+       - params: 定位参数
+       输出参数：
+       - MotionVector: 历史移动向量
+       处理逻辑：
+       1. 使用加权最小二乘法计算移动趋势
+    */
+    float sum_x=0, sum_y=0, weight_sum=0;
+    
+    // 时间衰减权重（最新点权重最大）
+    const float weights[4] = {0.1, 0.2, 0.3, 0.4}; 
+    
+    // 计算加权平均位移
+    for(int i=1; i<4; i++) {
+        float w = weights[i];
+        sum_x += w * (params->history_x[i] - params->history_x[i-1]);
+        sum_y += w * (params->history_y[i] - params->history_y[i-1]);
+        weight_sum += w;
+    }
+    
+    return (MotionVector){
+        .dx = sum_x / weight_sum,
+        .dy = sum_y / weight_sum,
+        .weight = 0.6f  // 历史数据权重
+    };}
+/* 生成基站特征向量 */
+MotionVector generateBeaconVector(const BeaconInfo* beacon, int32_t curr_x, int32_t curr_y) {
+    /* 输入参数：
+       - beacon: 单个基站信息
+       - curr_x: 当前X坐标
+       - curr_y: 当前Y坐标
+       处理逻辑：
+       1. 根据幅度和第一径索引变化生成方向向量
+       2. 计算信号质量权重
+    */
+    // 幅度变化趋势
+    float amp_trend = (2*beacon->amplitude[2] - beacon->amplitude[1] - beacon->amplitude[0]) / 100.0f;
+    
+    // 第一径索引变化趋势（转换为时间变化，假设采样率48kHz）
+    float time_trend = (2*beacon->fp_index[2] - beacon->fp_index[1] - beacon->fp_index[0]) / 48000.0f;
+    
+    // 基站方向向量（从手机指向基站）
+    int32_t dx = beacon->x - curr_x;
+    int32_t dy = beacon->y - curr_y;
+    float distance = sqrtf(dx*dx + dy*dy);
+    float dir_x = dx / (distance + 1e-6); // 防止除零
+    float dir_y = dy / (distance + 1e-6);
+    
+    // 综合特征（幅度增加+时间减少=靠近）
+    float feature = amp_trend - time_trend*1000; // 时间趋势系数调节
+    
+    // 信号质量权重（抑制突变信号）
+    float amp_stability = fabs(beacon->amplitude[2] - (beacon->amplitude[0]+beacon->amplitude[1])/2);
+    float weight = 1.0f / (1 + amp_stability/1000); // 幅度稳定性权重
+    
+    return (MotionVector){
+        .dx = dir_x * feature,
+        .dy = dir_y * feature,
+        .weight = weight
+    };}
+/* 多普勒效应补偿判断 */
+bool needsDopplerCompensation(const MotionVector* hist_motion, uint32_t frequency) {
+    /* 输入参数：
+       - hist_motion: 历史移动向量
+       - frequency: 基站载波频率
+       处理逻辑：
+       1. 根据移动速度和载波频率判断是否需要补偿
+    */
+    float speed = sqrtf(hist_motion->dx*hist_motion->dx + hist_motion->dy*hist_motion->dy);
+    float speed_threshold = 100.0f * frequency / 20000.0f; // 动态速度阈值
+    
+    return (speed > speed_threshold); // cm/s}
+/* 主预测函数 */float predictMovementDirection(PositioningParams* params) {
+    /* 输入参数：
+       - params: 定位参数
+       输出参数：
+       - float: 预测方向角度（度数）
+       处理逻辑：
+       1. 多传感器数据融合
+       2. 鲁棒向量合成
+    */
+    // 数据预处理
+    preprocessBeaconData(params->beacons, params->num_beacons);
+
+    // 获取当前最新位置
+    int32_t curr_x = params->history_x[3];
+    int32_t curr_y = params->history_y[3];
+
+    // 计算历史运动趋势
+    MotionVector hist_vec = calculateHistoricalMotion(params);
+
+    // 合成基站特征向量
+    MotionVector total_vec = {0};
+    for(int i=0; i<params->num_beacons; i++) {
+        MotionVector bv = generateBeaconVector(&params->beacons[i], curr_x, curr_y);
+        
+        // 多普勒补偿判断
+        if(needsDopplerCompensation(&hist_vec, params->beacons[i].frequency)) {
+            // 简化补偿：削弱高速移动时的反向信号影响
+            bv.dx *= 0.7;
+            bv.dy *= 0.7;
+        }
+
+        // 加权合成
+        total_vec.dx += bv.dx * bv.weight;
+        total_vec.dy += bv.dy * bv.weight;
+        total_vec.weight += bv.weight;
+    }
+
+    // 归一化基站向量
+    if(total_vec.weight > 1e-6) {
+        total_vec.dx /= total_vec.weight;
+        total_vec.dy /= total_vec.weight;
+    }
+
+    // 最终向量合成（历史趋势+基站特征）
+    float final_dx = hist_vec.dx * hist_vec.weight + total_vec.dx * (1 - hist_vec.weight);
+    float final_dy = hist_vec.dy * hist_vec.weight + total_vec.dy * (1 - hist_vec.weight);
+
+    // 计算角度（-180~180度）
+    float angle = atan2f(final_dy, final_dx) * 180.0f / M_PI;
+    return angle;}
+/* 示例用法 */int main() {
+    // 初始化基站信息（示例数据）
+    BeaconInfo beacons[3] = {
+        {1000, 2000, 3000, 18000, {500, 520, 480}, {100, 98, 120}},
+        {3000, 1000, 3000, 19000, {480, 500, 510}, {105, 103, 101}},
+        {2000, 3000, 3000, 20000, {510, 490, 530}, {102, 101, 99}}
+    };
+
+    // 初始化定位参数
+    PositioningParams params = {
+        .beacons = beacons,
+        .num_beacons = 3,
+        .history_x = {100, 120, 135, 150},
+        .history_y = {200, 210, 225, 240}
+    };
+
+    // 执行预测
+    float angle = predictMovementDirection(&params);
+    printf("Predicted movement angle: %.2f degrees\n", angle);
+
+    return 0;}
+关键算法说明：
+  1.      
+   数据预处理：
+  2.      
+     1.            
+      使用中值滤波处理幅度异常（如遮挡导致的信号骤降）
+     2.            
+     3.            
+      采用线性预测校正第一径索引突变
+     4.            
+  3.      
+   历史运动分析：
+  4.      
+     1.            
+      采用加权最小二乘法计算移动趋势
+     2.            
+     3.            
+      时间衰减权重赋予最新数据更高权重
+     4.            
+  5.      
+   多普勒补偿：
+  6.      
+     1.            
+      动态速度阈值根据载波频率调整
+     2.            
+     3.            
+      通过削弱反向信号影响实现简化补偿
+     4.            
+  7.      
+   传感器融合：
+  8.      
+     1.            
+      将基站特征向量分解为方向分量和信号质量权重
+     2.            
+     3.            
+      采用归一化加权合成保证数值稳定性
+     4.            
+  9.      
+   鲁棒性设计：
+  10.      
+     1.            
+      幅度稳定性检测抑制低质量信号
+     2.            
+     3.            
+      向量合成避免单基站主导
+     4.            
+实现特点：
+  1.      
+   模块化设计，各功能独立成函数
+  2.      
+  3.      
+   采用cm单位制，避免浮点运算精度损失
+  4.      
+  5.      
+   动态权重分配适应复杂环境
+  6.      
+  7.      
+   轻量级实现适合移动端运行
+  8.      
+该算法综合考虑了信号特征、历史轨迹和环境干扰因素，在保证实时性的同时实现了较好的鲁棒性。实际部署时需要根据具体环境参数调整滤波阈值和权重系数。
+

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\\zcq(20241217-03).wav";
+    const char *pFileName = "E:\\work\\ips8000\\aplm8000sdk\\wave\\zhoutao_6bts_rect_0327.wav";
     int16 *pwData = NULL; // 音频数据存储地址
     uint8 byIsOver = 0;
     uint16 wCounter = 0;

src/exchange/exchange.c

@@ -97,7 +97,10 @@ exchange_js_notice_wasm_free_memory()
 
     free(pExch_pdwAccAddress);
 
-    imu_free_memory();     // 释放imu模块内存
+    imu_free_memory(); // 释放imu模块内存
+
+    route_main_free_memory(); // 释放route模块内存
+
     sysmain_free_memory(); // 释放sysmain模块内存
 }
 

src/includes/includes.h

@@ -31,13 +31,13 @@
 #include "../modules/demodulation/dmdl_modulus.h" //取模
 
 // 基站模块
-#include "../modules/basestation/bts_main.h"    //主控
-#include "../modules/basestation/bts_mirror.h"  //数据镜像模块
-#include "../modules/basestation/bts_ampl.h"    //信号强度
-#include "../modules/basestation/bts_sync.h"    //信号同步
-#include "../modules/basestation/bts_id.h"      //计算基站ID
-#include "../modules/basestation/bts_idcheck.h" //ID二次校验
-#include "../modules/basestation/bts_track.h"   //基站跟踪
+#include "../modules/basestation/bts_main.h"   //主控
+#include "../modules/basestation/bts_mirror.h" //数据镜像模块
+#include "../modules/basestation/bts_ampl.h"   //信号强度
+#include "../modules/basestation/bts_sync.h"   //信号同步
+// #include "../modules/basestation/bts_id.h"      //计算基站ID
+// #include "../modules/basestation/bts_idcheck.h" //ID二次校验
+#include "../modules/basestation/bts_track.h" //基站跟踪
 
 // 定位数据模块
 #include "../modules/lctdata/lctdata.h" //定位数据
@@ -46,6 +46,7 @@
 #include "../modules/location/lct_main.h"         //主控
 #include "../modules/location/lct_firstpath.h"    //计算第一径
 #include "../modules/location/lct_coordcal.h"     //计算坐标
+#include "../modules/location/lct_prediction.h"   //预测坐标
 #include "../modules/location/lct_coordcal_one.h" //计算坐标(1维和0维)
 #include "../modules/location/lct_coordcal_two.h" //计算坐标(2维)
 #include "../modules/location/lct_coordsel.h"     //选择坐标

src/modules/basestation/bts_main.c

@@ -36,9 +36,13 @@ void bts_main_init()
     memset(gBts_pstInstallBtsList, 0, sizeof(BTS_INSTALL_BTS_LIST_T));
 
     // 基站ID模块初始化
-    bts_id_init();
+    // bts_id_init();
 
-    bts_main_gen_test_bts();
+    // TODO组织测试基站数据
+    if (PUB_DEBUG_ENABLE)
+    {
+        bts_main_gen_test_bts();
+    }
 }
 
 // 模块反初始化
@@ -251,83 +255,84 @@ void bts_main_gen_test_bts()
     pstHead->astLocalBtsList[0].dwBuildId = 1;
     pstHead->astLocalBtsList[0].dwLayerId = 15;
     pstHead->astLocalBtsList[0].dwBtsId = 10001;
-    pstHead->astLocalBtsList[0].dwFreqIndex = 0;
+    pstHead->astLocalBtsList[0].dwFreqIndex = 3;
     pstHead->astLocalBtsList[0].dwSlot = 0;
-    pstHead->astLocalBtsList[0].dwLctType = 1;
-    pstHead->astLocalBtsList[0].dwTwoBtsNum = 1;
-    pstHead->astLocalBtsList[0].adwTwoBtsList[0] = 10002;
+    pstHead->astLocalBtsList[0].dwLctType = 2;
+    pstHead->astLocalBtsList[0].dwTwoBtsNum = 0;
+    pstHead->astLocalBtsList[0].adwTwoBtsList[0] = 0;
     pstHead->astLocalBtsList[0].dwCoordX = 0;
     pstHead->astLocalBtsList[0].dwCoordY = 0;
-    pstHead->astLocalBtsList[0].dwCoordZ = 200;
+    pstHead->astLocalBtsList[0].dwCoordZ = 300;
 
     // 2号基站
     pstHead->astLocalBtsList[1].dwBuildId = 1;
     pstHead->astLocalBtsList[1].dwLayerId = 15;
     pstHead->astLocalBtsList[1].dwBtsId = 10002;
-    pstHead->astLocalBtsList[1].dwFreqIndex = 1;
+    pstHead->astLocalBtsList[1].dwFreqIndex = 4;
     pstHead->astLocalBtsList[1].dwSlot = 1;
-    pstHead->astLocalBtsList[1].dwLctType = 1;
-    pstHead->astLocalBtsList[1].dwTwoBtsNum = 2;
-    pstHead->astLocalBtsList[1].adwTwoBtsList[0] = 10001;
-    pstHead->astLocalBtsList[1].adwTwoBtsList[1] = 10003;
+    pstHead->astLocalBtsList[1].dwLctType = 2;
+    pstHead->astLocalBtsList[1].dwTwoBtsNum = 0;
+    pstHead->astLocalBtsList[1].adwTwoBtsList[0] = 0;
     pstHead->astLocalBtsList[1].dwCoordX = 2500;
     pstHead->astLocalBtsList[1].dwCoordY = 0;
-    pstHead->astLocalBtsList[1].dwCoordZ = 200;
+    pstHead->astLocalBtsList[1].dwCoordZ = 300;
+    pstHead->astLocalBtsList[1].adwBluetoothMac[0] = 1; // 蓝牙mac0
+    pstHead->astLocalBtsList[1].adwBluetoothMac[1] = 2; // 蓝牙mac1
+    pstHead->astLocalBtsList[1].adwBluetoothMac[2] = 3; // 蓝牙mac2
 
-    // 4号基站
+    // 3号基站
     pstHead->astLocalBtsList[2].dwBuildId = 1;
     pstHead->astLocalBtsList[2].dwLayerId = 15;
     pstHead->astLocalBtsList[2].dwBtsId = 10003;
-    pstHead->astLocalBtsList[2].dwFreqIndex = 2;
+    pstHead->astLocalBtsList[2].dwFreqIndex = 5;
     pstHead->astLocalBtsList[2].dwSlot = 2;
-    pstHead->astLocalBtsList[2].dwLctType = 1;
-    pstHead->astLocalBtsList[2].dwTwoBtsNum = 2;
+    pstHead->astLocalBtsList[2].dwLctType = 2;
+    pstHead->astLocalBtsList[2].dwTwoBtsNum = 0;
     pstHead->astLocalBtsList[2].adwTwoBtsList[0] = 10002;
-    pstHead->astLocalBtsList[2].adwTwoBtsList[0] = 10004;
     pstHead->astLocalBtsList[2].dwCoordX = 5000;
     pstHead->astLocalBtsList[2].dwCoordY = 0;
-    pstHead->astLocalBtsList[2].dwCoordZ = 200;
-    pstHead->astLocalBtsList[2].adwBluetoothMac[0] = 1; // 蓝牙mac0
-    pstHead->astLocalBtsList[2].adwBluetoothMac[1] = 2; // 蓝牙mac1
-    pstHead->astLocalBtsList[2].adwBluetoothMac[2] = 3; // 蓝牙mac2
+    pstHead->astLocalBtsList[2].dwCoordZ = 300;
 
-    // 5号基站
+    // 4号基站
     pstHead->astLocalBtsList[3].dwBuildId = 1;
     pstHead->astLocalBtsList[3].dwLayerId = 15;
     pstHead->astLocalBtsList[3].dwBtsId = 10004;
-    pstHead->astLocalBtsList[3].dwFreqIndex = 3;
+    pstHead->astLocalBtsList[3].dwFreqIndex = 6;
     pstHead->astLocalBtsList[3].dwSlot = 3;
-    pstHead->astLocalBtsList[3].dwCoordX = 7500;
-    pstHead->astLocalBtsList[3].dwCoordY = 0;
-    pstHead->astLocalBtsList[3].dwCoordZ = 200;
-    pstHead->astLocalBtsList[3].dwLctType = 1;
-    pstHead->astLocalBtsList[3].dwTwoBtsNum = 1;
+    pstHead->astLocalBtsList[3].dwCoordX = 5000;
+    pstHead->astLocalBtsList[3].dwCoordY = 2500;
+    pstHead->astLocalBtsList[3].dwCoordZ = 300;
+    pstHead->astLocalBtsList[3].dwLctType = 2;
+    pstHead->astLocalBtsList[3].dwTwoBtsNum = 0;
     pstHead->astLocalBtsList[3].adwTwoBtsList[0] = 10003;
 
-    // // 7号基站
-    // pstHead->astLocalBtsList[4].dwBuildId = 1;
-    // pstHead->astLocalBtsList[4].dwLayerId = 15;
-    // pstHead->astLocalBtsList[4].dwBtsId = 2097114;
-    // pstHead->astLocalBtsList[4].dwFreqIndex = 6;
-    // pstHead->astLocalBtsList[4].dwSlot = 0;
-    // pstHead->astLocalBtsList[4].dwCoordX = 500;
-    // pstHead->astLocalBtsList[4].dwCoordY = 180;
-    // pstHead->astLocalBtsList[4].dwCoordZ = 0;
-    // pstHead->astLocalBtsList[4].dwLctType = 2;
-    // pstHead->astLocalBtsList[4].dwTwoBtsNum = 1;
-    // pstHead->astLocalBtsList[4].adwTwoBtsList[0] = 0;
-
-    // // // 8号基站
-    // // pstHead->astLocalBtsList[7].dwBuildId = 1;
-    // // pstHead->astLocalBtsList[7].dwLayerId = 15;
-    // // pstHead->astLocalBtsList[7].dwBtsId = 2097110;
-    // // pstHead->astLocalBtsList[7].dwFreqIndex = 7;
-    // // pstHead->astLocalBtsList[7].dwSlot = 0;
-    // // pstHead->astLocalBtsList[7].dwCoordX = 7500;
-    // // pstHead->astLocalBtsList[7].dwCoordY = 2400;
-    // // pstHead->astLocalBtsList[7].dwCoordZ = 270;
-
-    pstHead->dwBtsNum = 5;
+    // 5号基站
+    pstHead->astLocalBtsList[4].dwBuildId = 1;
+    pstHead->astLocalBtsList[4].dwLayerId = 15;
+    pstHead->astLocalBtsList[4].dwBtsId = 10005;
+    pstHead->astLocalBtsList[4].dwFreqIndex = 7;
+    pstHead->astLocalBtsList[4].dwSlot = 0;
+    pstHead->astLocalBtsList[4].dwCoordX = 2500;
+    pstHead->astLocalBtsList[4].dwCoordY = 2500;
+    pstHead->astLocalBtsList[4].dwCoordZ = 300;
+    pstHead->astLocalBtsList[4].dwLctType = 2;
+    pstHead->astLocalBtsList[4].dwTwoBtsNum = 0;
+    pstHead->astLocalBtsList[4].adwTwoBtsList[0] = 10003;
+
+    // 6号基站
+    pstHead->astLocalBtsList[5].dwBuildId = 1;
+    pstHead->astLocalBtsList[5].dwLayerId = 15;
+    pstHead->astLocalBtsList[5].dwBtsId = 10006;
+    pstHead->astLocalBtsList[5].dwFreqIndex = 2;
+    pstHead->astLocalBtsList[5].dwSlot = 1;
+    pstHead->astLocalBtsList[5].dwCoordX = 0;
+    pstHead->astLocalBtsList[5].dwCoordY = 2500;
+    pstHead->astLocalBtsList[5].dwCoordZ = 300;
+    pstHead->astLocalBtsList[5].dwLctType = 2;
+    pstHead->astLocalBtsList[5].dwTwoBtsNum = 0;
+    pstHead->astLocalBtsList[5].adwTwoBtsList[0] = 10003;
+
+    pstHead->dwBtsNum = 6;
 }
 
 // 显示安装基站

src/modules/lctdata/lctdata.c

@@ -15,6 +15,7 @@ extern PUB_SYS_STATUS_T gg_stSysStatus;
 // pstLctResult:定位结果数据队列
 void lctdata_update_lct_data_btsid_and_slot(PUB_LOCATION_DATA_T *pstLctDataList, ROUTE_NEARBY_BTS_LIST *pstNearbyBtsList, PUB_CURRENT_LCT_RESULT_T *pstLctResult)
 {
+    // 系统处在基站捕获阶段
     if (!gg_stSysStatus.byBtsCaptureFlag)
     {
         lctdata_update_lct_data_by_bts_data(pstLctDataList, pstNearbyBtsList);
@@ -27,7 +28,7 @@ void lctdata_update_lct_data_btsid_and_slot(PUB_LOCATION_DATA_T *pstLctDataList,
     return;
 }
 
-// 根据基站队列更新定位数据，包括基站ID和时隙
+// 根据周边基站队列更新定位数据，包括基站ID和时隙
 // pstLctDataList:定位数据队列
 // pstNearbyBtsList:周边基站数据队列
 void lctdata_update_lct_data_by_bts_data(PUB_LOCATION_DATA_T *pstLctDataList, ROUTE_NEARBY_BTS_LIST *pstNearbyBtsList)
@@ -48,7 +49,7 @@ void lctdata_update_lct_data_by_bts_data(PUB_LOCATION_DATA_T *pstLctDataList, RO
     byRemainder = (gg_dwFrameNo - 1) % PUBLIC_SYNC_FRAMES;
     if (byRemainder == 0)
     {
-        // 遍历定位数据队列
+        // 各频点定位数据复位
         for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++)
         {
             pstLctData = &pstLctDataList[i];
@@ -57,16 +58,16 @@ void lctdata_update_lct_data_by_bts_data(PUB_LOCATION_DATA_T *pstLctDataList, RO
             pstLctData->byLctBtsFlag = 0;
         }
 
-        // 遍历周边基站队列
+        // 以周边基站数据，配置定位数据
         for (uint8 j = 0; j < byBtsNum; j++)
         {
-            pstBtsData = &pstNearbyBtsList->astBtsList[j];
-            byFreqIndex = pstBtsData->dwFreqIndex;
+            pstBtsData = &pstNearbyBtsList->astBtsList[j]; // 基站数据
+            byFreqIndex = pstBtsData->dwFreqIndex;         // 基站频率
 
-            pstLctData = &pstLctDataList[byFreqIndex];
-            pstLctData->dwSetBtsId = pstBtsData->dwBtsId;
-            pstLctData->bySetSlot = (uint8)pstBtsData->dwSlot;
-            pstLctData->byLctBtsFlag = 1;
+            pstLctData = &pstLctDataList[byFreqIndex];         // 定位数据
+            pstLctData->dwSetBtsId = pstBtsData->dwBtsId;      // 基站编号
+            pstLctData->bySetSlot = (uint8)pstBtsData->dwSlot; // 基站时隙
+            pstLctData->byLctBtsFlag = 1;                      // 定位基站标识符，0：非定位基站，1：定位基站
         }
     }
 
@@ -98,10 +99,6 @@ void lctdata_update_lct_data_by_lct_coord(PUB_LOCATION_DATA_T *pstLctDataList, P
     byRemainder = (gg_dwFrameNo - 1) % PUBLIC_SYNC_FRAMES;
     if (byRemainder == 0)
     {
-        if (gg_dwFrameNo > 32)
-        {
-            uint8 byTmp = 0;
-        }
         // 获得安装基站队列
         pstInstallBtsList = (BTS_INSTALL_BTS_LIST_T *)bts_main_get_install_bts_list();
 
@@ -115,7 +112,7 @@ void lctdata_update_lct_data_by_lct_coord(PUB_LOCATION_DATA_T *pstLctDataList, P
             {
                 // 计算定位坐标点到安装基站之间的距离
                 fDistance = util_distance_cal_p2p_distance(pstLctResult->stCoord.dwX, pstLctResult->stCoord.dwY, pstOneInstallBts->dwCoordX, pstOneInstallBts->dwCoordY);
-                if (fDistance < PUB_BTS_VALID_DISTANCE)
+                if (fDistance <= PUB_BTS_VALID_DISTANCE)
                 { // 距离有效
                     stLctBtsList.astLocationBts[byBtsCounter].dwFreqIndex = pstOneInstallBts->dwFreqIndex;
                     stLctBtsList.astLocationBts[byBtsCounter].dwBtsId = pstOneInstallBts->dwBtsId;
@@ -125,7 +122,7 @@ void lctdata_update_lct_data_by_lct_coord(PUB_LOCATION_DATA_T *pstLctDataList, P
             }
         }
 
-        // 遍历定位数据队列
+        // 各频点定位数据复位
         for (uint8 j = 0; j < PUB_SYS_CARRIER_NUM; j++)
         {
             pstLctData = &pstLctDataList[j];
@@ -137,13 +134,13 @@ void lctdata_update_lct_data_by_lct_coord(PUB_LOCATION_DATA_T *pstLctDataList, P
         // 遍历定位基站队列
         for (uint8 k = 0; k < byBtsCounter; k++)
         {
-            pstLctBtsData = &stLctBtsList.astLocationBts[k];
-            byFreqIndex = pstLctBtsData->dwFreqIndex;
+            pstLctBtsData = &stLctBtsList.astLocationBts[k]; // 基站数据
+            byFreqIndex = pstLctBtsData->dwFreqIndex;        // 基站频率
 
-            pstLctData = &pstLctDataList[byFreqIndex];
-            pstLctData->dwSetBtsId = pstLctBtsData->dwBtsId;
-            pstLctData->bySetSlot = (uint8)pstLctBtsData->dwSlot;
-            pstLctData->byLctBtsFlag = 1;
+            pstLctData = &pstLctDataList[byFreqIndex];            // 定位数据
+            pstLctData->dwSetBtsId = pstLctBtsData->dwBtsId;      // 基站编号
+            pstLctData->bySetSlot = (uint8)pstLctBtsData->dwSlot; // 基站时隙
+            pstLctData->byLctBtsFlag = 1;                         // 定位基站标识符，0：非定位基站，1：定位基站
         }
     }
 

src/modules/location/lct_coordcal.c

@@ -78,7 +78,7 @@ void lct_coordcal_main(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T
     COORD_T stAheadPointCoord = {0};
 
     LCT_MINIMUM_VALUE_GROUP_T stMinmumValueGroup = {0}; // 极小值点组
-    uint16 wMinmumValueNum = 0;                         // 极小值数据
+    uint16 wMinmumValueNum = 0;                         // 极小值个数
 
     // 选择有效基站(过滤第一径跳变和信噪比差的基站)
     lct_coordcal_sel_valid_bts(pstLctDataList, pstFirstPathDataList, byFirstPathDataListLen, astValidCalPathBtsList, &byValidCalPathBtsListLen, astValidTrackPathBtsList, &byValidTrackPathBtsListLen, &dwBuildId, &dwLayerId);
@@ -86,7 +86,8 @@ void lct_coordcal_main(PUB_LOCATION_DATA_T *pstLctDataList, LCT_FIRSTPATH_LIST_T
     // 预测定位坐标
     gLct_dwSpeedX = 0;
     gLct_dwSpeedY = 0;
-    lct_coordcal_prediction_coord(&stPredictionCoord, &byPredictionFlag, &stAheadPointCoord, dwBuildId, dwLayerId);
+    // lct_coordcal_prediction_coord(&stPredictionCoord, &byPredictionFlag, &stAheadPointCoord, dwBuildId, dwLayerId);
+    lct_prediction_current_coord_by_history(dwBuildId, dwLayerId, &stPredictionCoord, &byPredictionFlag, &stAheadPointCoord);
 
     // // 修正预测坐标
     // if (byPredictionFlag)

src/modules/location/lct_coordcal_one.c

@@ -1232,7 +1232,7 @@ void lct_coordcal_adjust_first_path_index(PUB_LOCATION_DATA_T *pstLctDataList, u
     fSlaveBtsDist = util_distance_cal_p2p_distance(pstLctCoord->dwX, pstLctCoord->dwY, pstSlaveBtsFirstPath->dwCoordX, pstSlaveBtsFirstPath->dwCoordY);
 
     // 基站从站帧内索引
-    fTimeDelay = ((fMainBtsDist - fSlaveBtsDist) / 340) * 1000 * 1000; // 单位微妙
+    fTimeDelay = ((fMainBtsDist - fSlaveBtsDist) / 343) * 1000 * 1000; // 单位微妙
     dwIndexDiff = (fTimeDelay * 48) / 1000;                            // 相位差
 
     // 修正从站第一径索引

src/modules/location/lct_firstpath.c

@@ -77,14 +77,11 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
     flt32 fMaxValue; // 最大值
     uint16 wAnchorIndex = 0;
     flt32 fNoise = 0;
-    flt32 fSnr = 0;
     flt32 fPeakThreshold = 0;
     FIRST_PATH_T stFirstPath = {0};
     uint8 byCounter = 0;
     uint16 wFrameTraceIndex = 0; // 帧内跟踪索引
 
-    uint8 byBtsIdBitIndex = 0; // 基站ID的bit索引
-    uint8 byBtsIdBitValue = 0; // 基站ID的bit表征值
     LCT_FIRSTPATH_T *pstFirstPath = NULL;
 
     // 复位预测方向数据有效标识符
@@ -94,8 +91,8 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
     for (uint8 i = 0; i < byLctBtsNum; i++)
     {
         pstLctBts = &pstLctBtsList->astLocationBts[i]; // 定位基站
-        byFreqIndex = pstLctBts->dwFreqIndex;          // 基站频率索引
-        pstLctData = &pstLctDataList[byFreqIndex];     // 定位数据
+        byFreqIndex = pstLctBts->dwFreqIndex;          // 基站的频率索引
+        pstLctData = &pstLctDataList[byFreqIndex];     // 基站的定位数据
 
         // 获取该基站一帧定位数据
         lct_firstpath_get_lct_data(pstLctData, gg_afOneFrameLocationData);
@@ -103,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 == 0 || pstLctData->byFreqIndex == 1 || pstLctData->byFreqIndex == 2 || pstLctData->byFreqIndex == 3) && pstLctData->dwFrameNo > 0)
+            if (pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 0)
             {
                 char *pfileName = "E:\\work\\ips8000\\aplm8000sdk\\output\\lctdata\\firstpath.bin";
                 util_write_flt32_to_bin_file(pfileName, PUB_SLOT_FRAME_LEN, gg_afOneFrameLocationData, 1);
@@ -111,51 +108,47 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
             }
         }
 
-        // 当前一秒数据有效
+        // 当前一秒数据有效(是定位基站、定位基站有效、当前幅度值有效)
         if (pstLctData->byLctBtsFlag && pstLctData->byBtsValidFlag && pstLctData->byCurrentAmplValidFlag)
         {
-            wFrameTraceIndex = pstLctData->wAheadFrameTrackPathIndex; // 帧内跟踪索引
+            // 帧内跟踪索引
+            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)
+            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];                                                            // 计算径值
-                fSnr = stFirstPath.fSnr;
-                pstFirstPath->fCalPathIndexSnr = fSnr; // 计算径信噪比
+                pstFirstPath->fCalPathIndexSnr = stFirstPath.fSnr;                                                                                                    // 计算径信噪比
                 fMaxValue = gg_afOneFrameLocationData[wMaxValueIndex];
                 pstFirstPath->fMaxAmplValue = fMaxValue; // 帧内最大值
 
                 // TODO测试
                 if (PUB_DEBUG_ENABLE)
                 {
-                    if (pstLctData->byFreqIndex < 6 && pstLctData->dwFrameNo > 0)
+                    if (pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 0)
                     {
                         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;
@@ -180,7 +173,7 @@ void lct_firstpath_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *
                     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;
+                    pstFirstPath->fTrackPathIndexSnr = stFirstPath.fSnr;                                                                                                    // 跟踪径信噪比
                 }
                 // 跟踪失败，则使用计算径的数据
                 else
@@ -202,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 < 6 && pstLctData->dwFrameNo > 0)
+                if (pstFirstPath->wFrameTrackIndex > 0 && pstLctData->byFreqIndex < 8 && pstLctData->dwFrameNo > 0)
                 {
                     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_main.c

@@ -44,6 +44,7 @@ void lct_main_reset()
 
         gLct_dwPredictionTimes = 0;
         gg_stSysStatus.byBtsCaptureFlag = 0; // 系统处于捕获基站状态
+        gg_stSysStatus.byReLocationFlag = 0; // 系统进入重新定位状态
     }
 
     return;
@@ -145,6 +146,7 @@ void lct_main(PUB_LCT_BTS_LIST_T *pstLctBtsList, PUB_LOCATION_DATA_T *pstLctData
                             gg_stSysStatus.byBtsCaptureFlag = 1; // 系统进入基站跟踪阶段
                         }
 
+                        // 进入正常定位状态
                         if (gg_stSysStatus.byReLocationFlag)
                         {
                             gg_stSysStatus.byReLocationFlag = 0;
@@ -592,7 +594,7 @@ void lct_main_adjust_firstpath_index_offset(LCT_FIRSTPATH_LIST_T *pstAheadLctFir
     yy = stLctCoord.dwY - stMainBtsFirstPath.dwCoordY;
     zz = stLctCoord.dwZ - stMainBtsFirstPath.dwCoordZ;
     fDistance = sqrt(xx * xx + yy * yy + zz * zz);
-    fMainTimeLen = fDistance / 100 / 340 * 1000; // 单位毫秒
+    fMainTimeLen = fDistance / 100 / PUB_AUDIO_SPEED * 1000; // 单位毫秒
 
     // 计算从站与主站间的时延差，并反算索引偏差
     for (uint8 j = 0; j < byBtsNum; j++)
@@ -607,7 +609,7 @@ void lct_main_adjust_firstpath_index_offset(LCT_FIRSTPATH_LIST_T *pstAheadLctFir
             yy = stLctCoord.dwY - pstSlaveBtsFirstPath->dwCoordY;
             zz = stLctCoord.dwZ - pstSlaveBtsFirstPath->dwCoordZ;
             fDistance = sqrt(xx * xx + yy * yy + zz * zz);
-            fSlaveTimeLen = fDistance / 100 / 340 * 1000; // 从站传输时长，单位毫秒
+            fSlaveTimeLen = fDistance / 100 / PUB_AUDIO_SPEED * 1000; // 从站传输时长，单位毫秒
 
             // 定位点到主站与到从站间的时长差
             fDelayDiff = (fSlaveTimeLen - fMainTimeLen); // 毫秒

src/modules/location/lct_prediction.c

@@ -0,0 +1,96 @@
+// 秒寻科技
+// 预测坐标模块
+// zt
+// 2025-02-24
+
+#include "../../includes/includes.h"
+#include "./lct_public.h"
+
+// 导入全局变量
+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];
+
+// 根据历史坐标，预测当前一秒的坐标
+// 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)
+{
+    LCT_COORDSEL_MEAN_COORD_T *pstData = (LCT_COORDSEL_MEAN_COORD_T *)&gLct_stMeanLctCoordList; // 平滑定位点队列
+    uint8 byMeanListLen = pstData->byListLen;                                                   // 平滑定位点队列长度
+
+    LCT_COORDSEL_ORIGINAL_COORD_T *pstOriginalLctCoordList = &gLct_stOriginalLctCoordList; // 原始定位点队列
+    uint8 byOriginalListLen = pstOriginalLctCoordList->byListLen;                          // 原始定位点队列长度
+
+    COORD_T stPrediction = {0};
+
+    // 处于同一大楼和楼层
+    if (dwBulidId == gg_stSysStatus.dwBuildId && dwLayerId == gg_stSysStatus.dwLayerId)
+    {
+        // 系统处于重定位阶段
+        if (gg_stSysStatus.byReLocationFlag)
+        {
+            memset(&gLct_stOriginalLctCoordList, 0, sizeof(LCT_COORDSEL_ORIGINAL_COORD_T)); // 原始坐标队列复位
+            memset(&gLct_stMeanLctCoordList, 0, sizeof(LCT_COORDSEL_MEAN_COORD_T));         // 平滑坐标队列复位
+            gg_stSysStatus.byBtsCaptureFlag = 0;                                            // 系统进入基站捕获状态
+
+            // 预测失败
+            *pbyPredictionFlag = 0;
+            return;
+        }
+
+        // 平滑队列中的定位点不够
+        if (byMeanListLen < LCT_COORDSEL_MEAN_COORD_LIST_LEN)
+        {
+            // 预测失败
+            *pbyPredictionFlag = 0;
+            return;
+        }
+
+        // 计算基础速度
+        // 最近两次位移的加权平均（3:1权重）
+        int32 dx = (3 * (pstData->astCoordList[3].dwX - pstData->astCoordList[2].dwX) + (pstData->astCoordList[2].dwX - pstData->astCoordList[1].dwX)) / 4;
+        int32 dy = (3 * (pstData->astCoordList[3].dwY - pstData->astCoordList[2].dwY) + (pstData->astCoordList[2].dwY - pstData->astCoordList[1].dwY)) / 4;
+
+        // 计算加速度
+        // 计算二阶差分（加速度）
+        int32 ddx = (pstData->astCoordList[3].dwX - 2 * pstData->astCoordList[2].dwX + pstData->astCoordList[1].dwX);
+        int32 ddy = (pstData->astCoordList[3].dwY - 2 * pstData->astCoordList[2].dwY + pstData->astCoordList[1].dwY);
+
+        // 计算预测坐标（包括加速度补偿）
+        stPrediction.dwX = pstData->astCoordList[3].dwX + dx + ddx / 2; // 加速度补偿
+        stPrediction.dwY = pstData->astCoordList[3].dwY + dy + ddy / 2; // 加速度补偿
+        stPrediction.dwZ = 120;
+
+        // 在X轴和Y轴上的位移
+        gLct_dwSpeedX = (int32)dx + ddx / 2; // 位移速度
+        gLct_dwSpeedY = (int32)dy + ddy / 2; // 位移速度
+
+        // 保存预测坐标
+        *pstPredictionCoord = stPrediction;
+
+        // 保存上一个定位点坐标
+        *pstAheadPointCoord = pstData->astCoordList[byMeanListLen - 1];
+
+        // 预测成功
+        *pbyPredictionFlag = 1;
+    }
+    // 不在同一大楼和楼层，系统进入基站捕获状态
+    else
+    {
+        memset(&gLct_stOriginalLctCoordList, 0, sizeof(LCT_COORDSEL_ORIGINAL_COORD_T)); // 原始坐标队列复位
+        memset(&gLct_stMeanLctCoordList, 0, sizeof(LCT_COORDSEL_MEAN_COORD_T));         // 平滑坐标队列复位
+        gg_stSysStatus.byBtsCaptureFlag = 0;                                            // 系统进入基站捕获状态
+
+        // 预测失败
+        *pbyPredictionFlag = 0;
+    }
+
+    return;
+}

src/modules/location/lct_prediction.h

@@ -0,0 +1,15 @@
+// 秒寻科技
+// 预测坐标模块
+// zt
+// 2025-02-24
+
+#ifndef __LCT_PREDICTION_H__
+#define __LCT_PREDICTION_H__
+
+#include "../../sysdefine/sysdef.h"
+#include "./lct_public.h"
+
+// 供外部使用接口
+void lct_prediction_current_coord_by_history(int32 dwBulidId, int32 dwLayerId, COORD_T *pstPredictionCoord, uint8 *pbyPredictionFlag, COORD_T *pstAheadPointCoord);
+
+#endif

src/modules/route/route_bluetooth.c

@@ -5,7 +5,12 @@
 
 #include "../../includes/includes.h"
 
-// 基于蓝牙基站路由
+// 基于蓝牙基站路由，找出主站(即用户当前在哪个基站附近)
+// pstBluetoothBtsList:蓝牙站队列
+// byBleBtsNum:蓝牙站数量
+// pstInstallBtsList:安装站列表
+// pstMainBtsData:返回参数，主站数据
+// 返回值：1，成功；0，失败
 uint8 route_bluetooth_route(ROUTE_BLUETOOTH_BTS_T *pstBluetoothBtsList, uint32 dwBtsNum, BTS_INSTALL_BTS_LIST_T *pstInstallBtsList, BTS_INSTALL_BTS_T *pstMainBts)
 {
     uint8 byFlag = 0;
@@ -13,17 +18,15 @@ uint8 route_bluetooth_route(ROUTE_BLUETOOTH_BTS_T *pstBluetoothBtsList, uint32 d
     // 按照接收电平倒排序
     route_bluetooth_bts_sort(pstBluetoothBtsList, dwBtsNum);
 
-    // 从安装基站队列中查改蓝牙基站
+    // 从安装基站队列中查找蓝牙基站
     byFlag = route_bluetooth_find_bluetooth_bts_from_install_bts(pstBluetoothBtsList, (uint8)dwBtsNum, pstInstallBtsList, pstMainBts);
-    if (byFlag == RET_OK)
-    {
-        return RET_OK;
-    }
 
-    return RET_FAIL;
+    return byFlag;
 }
 
 // 按照接收电平排序
+// pstBluetoothBtsList:蓝牙站队列
+// dwBtsNum:蓝牙站数量
 void route_bluetooth_bts_sort(ROUTE_BLUETOOTH_BTS_T *pstBluetoothBtsList, uint32 dwBtsNum)
 {
     ROUTE_BLUETOOTH_BTS_T stTmp = {0};
@@ -45,6 +48,11 @@ void route_bluetooth_bts_sort(ROUTE_BLUETOOTH_BTS_T *pstBluetoothBtsList, uint32
 }
 
 // 从本地安装基站中查找蓝牙基站
+// pstBluetoothBtsList:蓝牙站队列
+// byBleBtsNum:蓝牙站数量
+// pstInstallBtsList:安装站列表
+// pstMainBtsData:返回参数，主站数据
+// 返回值:0，失败；1，成功
 uint8 route_bluetooth_find_bluetooth_bts_from_install_bts(ROUTE_BLUETOOTH_BTS_T *pstBluetoothBtsList, uint8 byBleBtsNum, BTS_INSTALL_BTS_LIST_T *pstInstallBtsList, BTS_INSTALL_BTS_T *pstMainBtsData)
 {
     ROUTE_BLUETOOTH_BTS_T stBluetoothBts = {0};
@@ -87,11 +95,12 @@ uint8 route_bluetooth_find_bluetooth_bts_from_install_bts(ROUTE_BLUETOOTH_BTS_T
         }
     }
 
+    // 找到了主站
     if (byValidFlag)
     {
         *pstMainBtsData = stInstallBts;
-        return RET_OK;
+        return 1;
     }
 
-    return RET_FAIL;
+    return 0;
 }

src/modules/route/route_main.c

@@ -39,8 +39,8 @@ int32 *route_main_get_route_bts_list_save_address()
     return (int32 *)gRoute_pdwRouteBtsList;
 }
 
-// 模块反向初始化
-void route_main_deinit()
+// 释放内存
+void route_main_free_memory()
 {
     // 释放内存
     free(gRoute_pdwRouteBtsList);
@@ -50,7 +50,7 @@ void route_main_deinit()
 }
 
 // 路由主控，根据蓝牙或者wifi标识符生成周边基站队列
-// pstOutNearbyBtsList:周边的基站队列
+// pstOutNearbyBtsList:周边的基站队列（返回参数）
 void route_main(ROUTE_NEARBY_BTS_LIST *pstOutNearbyBtsList)
 {
     uint8 byFlag = 0;
@@ -68,7 +68,7 @@ void route_main(ROUTE_NEARBY_BTS_LIST *pstOutNearbyBtsList)
     pstBluetoothBtsList = pstRouteBtsList->astBluetoothBts; // 蓝牙站数据
     pstWifiBtsList = pstRouteBtsList->astWifiBts;           // wifi站数据
 
-    // 当前帧不需要维护周边站数据
+    // 当前帧不需要维护周边站数据（每一秒维护一次周边站队列）
     if ((gg_dwFrameNo < 2) || ((gg_dwFrameNo - 1) % PUB_SYS_SLOT_NUM != 0))
     {
         return;
@@ -101,7 +101,7 @@ void route_main(ROUTE_NEARBY_BTS_LIST *pstOutNearbyBtsList)
         if (pstRouteBtsList->dwBluetoothNum)
         {
             byFlag = route_bluetooth_route(pstBluetoothBtsList, pstRouteBtsList->dwBluetoothNum, pstInstallBtsList, &stMainBtsData);
-            if (byFlag == RET_OK)
+            if (byFlag) // 找到了主站
             {
                 //  基于主站，组织定位基站队列
                 route_main_get_bts_list_by_main_bts(stMainBtsData, pstInstallBtsList, pstOutNearbyBtsList);
@@ -115,11 +115,15 @@ void route_main(ROUTE_NEARBY_BTS_LIST *pstOutNearbyBtsList)
         else if (pstRouteBtsList->dwWifiNum)
         {
             byFlag = route_wifi_route(pstWifiBtsList, (uint8)pstRouteBtsList->dwWifiNum, pstInstallBtsList, &stMainBtsData);
-            if (byFlag == RET_OK)
+            if (byFlag)
             {
                 // 基于主站，组织定位基站队列
                 route_main_get_bts_list_by_main_bts(stMainBtsData, pstInstallBtsList, pstOutNearbyBtsList);
             }
+            else
+            {
+                printf("route no find main bts\n");
+            }
         }
 
         // 有2个基站，标识周边站有效
@@ -161,8 +165,8 @@ void route_main_get_bts_list_by_main_bts(BTS_INSTALL_BTS_T stMinBts, BTS_INSTALL
             // 计算主从站之间的距离
             fDistance = util_distance_cal_p2p_distance(stMinBts.dwCoordX, stMinBts.dwCoordY, stSlaveBts.dwCoordX, stSlaveBts.dwCoordY);
 
-            // 从站之间的距离有效
-            if (fDistance < PUB_BTS_VALID_DISTANCE)
+            // 主从站之间的距离有效
+            if (fDistance <= PUB_BTS_VALID_DISTANCE)
             {
                 pstOutBtsList->astBtsList[byCounter] = stSlaveBts;
                 byCounter++;

src/modules/route/route_main.h

@@ -10,7 +10,7 @@
 
 // 外部接口
 void route_main_init();
-void route_main_deinit();
+void route_main_free_memory();
 void route_main(ROUTE_NEARBY_BTS_LIST *pstOutNearbyBtsList);
 int32 *route_main_get_route_bts_list_save_address();
 void route_main_show_route_bts();

src/modules/route/route_wifi.c

@@ -20,12 +20,8 @@ uint8 route_wifi_route(ROUTE_WIFI_BTS_T *pstWifiBtsList, uint8 byBtsNum, BTS_INS
 
     // 从安装基站队列中查改Wifi基站
     byFlag = route_wifi_find_wifi_bts_from_install_bts(pstWifiBtsList, byBtsNum, pstInstallBtsList, pstMainBts);
-    if (byFlag == RET_OK)
-    {
-        return RET_OK;
-    }
 
-    return RET_FAIL;
+    return byFlag;
 }
 
 // 按照接收电平排序
@@ -96,11 +92,12 @@ uint8 route_wifi_find_wifi_bts_from_install_bts(ROUTE_WIFI_BTS_T *pstWifiBtsList
         }
     }
 
+    // 找到了主站
     if (byValidFlag)
     {
         *pstMainBtsData = stInstallBts;
-        return RET_OK;
+        return 1;
     }
 
-    return RET_FAIL;
+    return 0;
 }

src/sysdefine/struct.h

@@ -19,7 +19,7 @@ typedef struct
     uint8 byShiftTimes;     // 基站ID右移次数
 
     uint8 byLocationTimes;  // 定位次数，定位次数超过21表示定位初始化阶段完成
-    uint8 byReLocationFlag; // 重新定位标识符
+    uint8 byReLocationFlag; // 重新定位标识符，0：正常定位状态，1：重新定位状态
     int32 dwBuildId;        // 基站归属大楼编号
     int32 dwLayerId;        // 基站归属楼层编号
 
@@ -50,9 +50,6 @@ typedef struct
     uint8 byLctBtsFlag;   // 定位基站标识符，0：不是定位基站，1：是定位基站
     uint8 byBtsValidFlag; // 基站有效标识符，0：无效，1：有效
 
-    uint8 abyBtsIdBitList[PUBLIC_BTS_ID_BITS_NUM]; // 基站ID的bit列表
-    uint8 byBtsIdBitIndex;                         // 基站ID的bit列表索引
-
     flt32 fCurrentMaxAmpl;        // 当前最大幅度
     uint8 byCurrentAmplValidFlag; // 当前幅度有效标识符
 

src/sysdefine/sysmacro.h

@@ -27,14 +27,17 @@
 // 系统时隙数
 #define PUB_SYS_SLOT_NUM 4
 
+// 声速，每秒343米
+#define PUB_AUDIO_SPEED 343
+
 // 信号采样率
 #define PUB_SIGNAL_SAMPLE_RATIO 48000
 
-// 信号同步向左偏移量
+// 信号同步向左偏移量，6000约42米（5000约35米）
 #define PUB_SYNC_INDEX_OFFSET_NUM 5000
 
-// TODO 正式版本修改为35米 基站有效距离，单位米
-#define PUB_BTS_VALID_DISTANCE 40
+// TODO 基站有效距离，单位米
+#define PUB_BTS_VALID_DISTANCE ((PUB_SYNC_INDEX_OFFSET_NUM * PUB_AUDIO_SPEED) / PUB_SIGNAL_SAMPLE_RATIO)
 
 // 参与定位的基站，最多9个
 #define PUB_LCT_BTS_MAX_NUM PUB_SYS_CARRIER_NUM
@@ -42,20 +45,19 @@
 // FFT变换帧长（点数）
 #define PUB_FFT_FRAME_LEN 16384
 
-// 定位相关
-#define PUB_SLOT_FRAME_LEN 12000                       // 每次定位处理的信号长度
-#define PUB_LCT_DATA_LIST_LEN (6 * PUB_SLOT_FRAME_LEN) // 定位数据队列长度，缓存6帧数据
-
-// 同步镜像
-#define PUBLIC_SYNC_FRAMES BTS_AMPL_MIRROR_FRAMES // 同步帧数（与最强幅度镜像帧数相同）
-#define PUBLIC_SYNC_SLAVE_SEARCH_WIDTH 4300       // 从站搜索窗（30米）
-
 // 基站ID模块
 #define PUB_BTS_LCT_BTS_DATA_USE_MAX_TIMES 5              // 定位基站数据最多使用次数
 #define PUBLIC_BTS_ID_BITS_NUM 21                         // 基站ID比特位数
 #define PUBLIC_BTS_ID_FRAMES (4 * PUBLIC_BTS_ID_BITS_NUM) // 基站ID比特帧数，4帧表征每比特
 #define PUBLIC_BTS_ID_JUDGEMENT_FACTOR 15                 // 基站ID判决因子，放大100倍保存
-#define PUB_BTS_ID_NUM 1164                               // 基站ID个数
+#define PUB_BTS_ID_NUM 1164
+
+// 定位相关
+#define PUB_SLOT_FRAME_LEN 12000                       // 每次定位处理的信号长度
+#define PUB_LCT_DATA_LIST_LEN (6 * PUB_SLOT_FRAME_LEN) // 定位数据队列长度，缓存6帧数据
+
+// // 同步镜像
+#define PUBLIC_SYNC_FRAMES BTS_AMPL_MIRROR_FRAMES // 同步帧数（与最强幅度镜像帧数相同）
 
 // 定位模块
 #define PUB_TWO_BTS_MAX_NUM 6               // 与本站参与两站定位的基站最大数量
@@ -68,7 +70,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

@@ -7,7 +7,7 @@
 #include "./globaldata.h"
 
 // 定义本模块使用的全局变量
-int32 *gSysmain_pdwCurrentLctCoord = NULL;
+int32 *gSysmain_pdwCurrentLctCoord = NULL;  // 当前定位坐标
 uint8 *gSysmain_pbySoftNameVsersion = NULL; // 软件名称与版本
 
 // 导入全局变量
@@ -74,7 +74,7 @@ void sysmain_init_lct()
 
     // 计算坐标点集参数
     LCT_COORDCAL_PARAM_T stLctCoordcalParam = {0};
-    stLctCoordcalParam.dwAudioSpeed = 340;
+    stLctCoordcalParam.dwAudioSpeed = PUB_AUDIO_SPEED; // 声速
     stLctCoordcalParam.dwPhoneCoordZ = 120;
 
     // 初始化阶段搜索框参数
@@ -117,7 +117,7 @@ uint8 sysmain_audio_signal_process()
     dmdl_main(gg_astLocationDataList);
     // dmdl_main_old(gg_astLocationDataList);//老变频方案
 
-    ////查找用户周边的基站
+    ////查找用户周边的基站，并生成周边基站列表
     route_main(&gg_stNearbyBtsList);
 
     //// 更新定位数据队列中的基站ID和时隙