// 秒寻科技 // 系统主控 // zt // 2023-03-04 #include "../includes/includes.h" #include "./globaldata.h" // 定义本模块使用的全局变量 int32 *gSysmain_pdwCurrentLctCoord = NULL; // 当前定位坐标 uint8 *gSysmain_pbySoftNameVsersion = NULL; // 软件名称与版本 // 导入全局变量 extern PUB_SYS_STATUS_T gg_stSysStatus; // 系统状态 extern PUB_LOCATION_DATA_T gg_astLocationDataList[PUB_SYS_CARRIER_NUM]; // 定位数据队列 extern ROUTE_NEARBY_BTS_LIST gg_stNearbyBtsList; // 周边基站队列 extern PUB_LCT_BTS_LIST_T gg_stLctBtsList; // 定位基站队列 extern PUB_CURRENT_LCT_RESULT_T gg_stCurrentLctCoord; // 当前定位坐标 // 模块初始化 void sysmain_init() { // fft模块初始化 util_fft_init(); // 申请一片内存 gSysmain_pdwCurrentLctCoord = malloc(sizeof(PUB_CURRENT_LCT_RESULT_T)); gSysmain_pbySoftNameVsersion = malloc(sizeof(PUB_SOFT_NAME_VERSION_T)); // 组织软件名称与版本 sysmain_organize_soft_name_and_version(gSysmain_pbySoftNameVsersion); // 初始化全局数据 globaldata_init(); // 路由模块初始化 route_main_init(); // 初始化解调模块 dmdl_main_init(); // 初始化基站模块 bts_main_init(); // 初始化定位模块 sysmain_init_lct(); return; } // 初始化定位模块 void sysmain_init_lct() { // 第一径参数 LCT_FIRSTPATH_PARAM_T stFirstPathParam = {0}; stFirstPathParam.byMinValueFactor = 11; // 最大值的10%作为选峰门限 stFirstPathParam.byAnchorFactor = 21; stFirstPathParam.byAnchorThreshold = 30; // 35; stFirstPathParam.byPeakPeakDistance = 50; // 峰间距 stFirstPathParam.byNoiseSignalFactor = 7; stFirstPathParam.byNoiseLowScale = 25; // 底噪放大低倍数，放大10倍保存 stFirstPathParam.byNoiseHightScale = 50; // 底噪放大高倍数，放大10倍保存 stFirstPathParam.byCfarLeftFactor = 120; // CFAR左侧门限 stFirstPathParam.bySincFactor = 94; // 95; stFirstPathParam.byFirstPathContactFactor = 75; stFirstPathParam.wFirstPathContactDistance = 1200; // 8米 // 跟踪径 stFirstPathParam.wFirstPathTrackWindowWidth = 320; // 280; // 跟踪径搜索范围 stFirstPathParam.wFirstPathTrackPeakGuardWidth = 80; // 160; // 跟踪保护贷款 stFirstPathParam.byFirstPathTrackPeakGainFactor = 120; // 跟踪补偿因子 stFirstPathParam.wFirstPathTrackNoiseFindWidth = 1000; // 跟踪径噪声搜索范围 // 计算坐标点集参数 LCT_COORDCAL_PARAM_T stLctCoordcalParam = {0}; stLctCoordcalParam.dwAudioSpeed = PUB_AUDIO_SPEED; // 声速 stLctCoordcalParam.dwPhoneCoordZ = 120; // 初始化阶段搜索框参数 stLctCoordcalParam.dwCoordExpendLen = 300; // 初始搜索框向外扩大5米 stLctCoordcalParam.dwIntervalDistance = 50; // 初始搜索格子间隔50厘米 // 跟踪阶段搜索框参数 stLctCoordcalParam.dwTrackCoordExpendLen = 300; // 跟踪阶段搜索框向外扩大3米 stLctCoordcalParam.dwTrackIntervalDistance = 50; // 跟踪阶段搜索格子间隔厘米 stLctCoordcalParam.dwMinimumValueNum = 5; // 判断二维基站第一径是否有效参数 stLctCoordcalParam.dwFirstPahtValueThres = 150; // 二维第一径幅度有效门限 stLctCoordcalParam.dwFirstPahtSnrThres = 35; // 二维第一径信噪有效门限(放大10倍) stLctCoordcalParam.dwFirstPathIndexJumpThres = 720; // 二维第一径跳变门限 // 判断一维基站第一径是否有效参数，相比于二维门限要求有所提高 stLctCoordcalParam.dwOneFirstPahtValueThres = 350; // 一维第一径幅度有效门限 stLctCoordcalParam.dwOneFirstPahtSnrThres = 50; // 一维第一径信噪有效门限(放大10倍) stLctCoordcalParam.dwOneFirstPathIndexJumpThres = 500; // 一维第一径跳变门限 stLctCoordcalParam.dwBtsEdgeExpendLen = 150; // 基站围成的矩形框向外扩展长度，默认值150 stLctCoordcalParam.dwOneDimensionExpendLen = 200; // 一维定位时向外扩展长度，默认值200 stLctCoordcalParam.dwOneDimensionSquareExpendLen = 500; // 一维定位时向外扩展长度，默认值500 // 选择最优定位坐标模块参数 LCT_COORDSEL_PARAM_T stLctCoordselParam = {0}; stLctCoordselParam.dwValidDistanceThres = 400; // 极小值点到前一个点的有效距离门限 stLctCoordselParam.dwPredictionRadius = 250; // 预测半径（极小值点必须在以预测点为圆心的圆内） //// 定位模块初始化 lct_main_init(stFirstPathParam, stLctCoordcalParam, stLctCoordselParam); return; } // 音频信号处理入口 // 返回值：1：成功，0：失败 uint8 sysmain_audio_signal_process() { ////信号解调，输出定位数据 dmdl_main(gg_astLocationDataList); // 新变频方案 // dmdl_main_old(gg_astLocationDataList);//老变频方案 ////查找用户周边的基站，并生成周边基站列表 route_main(&gg_stNearbyBtsList); //// 更新定位数据队列中的基站ID和时隙 lctdata_update_lct_data_btsid_and_slot(gg_astLocationDataList, &gg_stNearbyBtsList, &gg_stCurrentLctCoord); //// 计算主站ID，输出定位基站队列 bts_main(gg_astLocationDataList, &gg_stNearbyBtsList, &gg_stLctBtsList); ////定位计算，输出当前定位点 lct_main(&gg_stLctBtsList, gg_astLocationDataList, &gg_stCurrentLctCoord); ////保存当前定位坐标 sysmain_save_current_location_coord(&gg_stCurrentLctCoord); // 消费定位数据 sysmain_consume_location_data(gg_astLocationDataList); return 1; } // 消费定位数据 void sysmain_consume_location_data(PUB_LOCATION_DATA_T *pstLocationDataList) { PUB_LOCATION_DATA_T *pstLocationData = NULL; int32 dwIndex = 0; for (uint8 i = 0; i < PUB_SYS_CARRIER_NUM; i++) { pstLocationData = (PUB_LOCATION_DATA_T *)&pstLocationDataList[i]; // 生产第2帧后，开始维护当前帧的前一帧数据索引 if (pstLocationData->dwDataListLen >= (2 * PUB_SLOT_FRAME_LEN)) { // 前一帧定位数据索引 dwIndex = pstLocationData->dwFourthLocationIndex + PUB_SLOT_FRAME_LEN; if (dwIndex >= PUB_LCT_DATA_LIST_LEN) { dwIndex -= PUB_LCT_DATA_LIST_LEN; } pstLocationData->dwFourthLocationIndex = dwIndex; } // 生产第5帧后，开始维护定位数据索引和变频索引 if (pstLocationData->dwDataListLen >= (5 * PUB_SLOT_FRAME_LEN)) { // 定位数据索引 dwIndex = pstLocationData->dwLocationIndex + PUB_SLOT_FRAME_LEN; if (dwIndex >= PUB_LCT_DATA_LIST_LEN) { dwIndex -= PUB_LCT_DATA_LIST_LEN; } pstLocationData->dwLocationIndex = dwIndex; // 变频索引 pstLocationData->dwDdcIndex += PUB_SLOT_FRAME_LEN; // 队列长度 pstLocationData->dwDataListLen -= PUB_SLOT_FRAME_LEN; } } return; } // 保存当前定位坐标 // pstLctResult:定位结果 void sysmain_save_current_location_coord(PUB_CURRENT_LCT_RESULT_T *pstLctResult) { int32 *pdwData = gSysmain_pdwCurrentLctCoord; // 本次定位已完成 if (pstLctResult->dwCpltFlag) { memcpy(pdwData, pstLctResult, sizeof(PUB_CURRENT_LCT_RESULT_T)); pstLctResult->dwCpltFlag = 0; // 定位完成标识符置0，定位尚未完成 pstLctResult->dwSuccessFlag = 0; // 定位成功标识符置0，定位失败 } return; } // 查询当前定位坐标 // 返回值是存储当前定位点的内存地址，若为NULL则标识暂未输出定位坐标 int32 *sysmain_get_current_location_coord() { return (int32 *)gSysmain_pdwCurrentLctCoord; } // 组织软件名称与版本 void sysmain_organize_soft_name_and_version(uint8 *pbyData) { PUB_SOFT_NAME_VERSION_T *pstData = (PUB_SOFT_NAME_VERSION_T *)pbyData; memcpy(pstData->abySoftName, PUB_SOFTWARE_NAME, PUB_SOFTWARE_NAME_LEN); memcpy(pstData->abySoftVersion, PUB_SOFTWARE_VERSION, PUB_SOFTWARE_VERSION_LEN); return; } // 查询软件名称与版本的存储地址 uint8 *sysmain_get_soft_name_and_version_data_address() { return gSysmain_pbySoftNameVsersion; } // 释放模块内存 void sysmain_free_memory() { free(gSysmain_pdwCurrentLctCoord); free(gSysmain_pbySoftNameVsersion); }