From d20b742a36ef62ea92164b9bdf8b229c754a1062 Mon Sep 17 00:00:00 2001 From: logzhan <719901725@qq.com> Date: Sat, 25 Feb 2023 22:43:50 +0800 Subject: [PATCH] =?UTF-8?q?=E6=95=B4=E7=90=86=E8=AE=A1=E6=AD=A5=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 1.Software/PDR 1.05/include/Pedometer.h | 145 +- 1.Software/PDR 1.05/src/Location.c | 2 +- 1.Software/PDR 1.05/src/Pedometer.c | 498 +++---- 1.Software/PDR 1.06/gnssIns.sln | 31 + 1.Software/PDR 1.06/include/AHRS.h | 48 + 1.Software/PDR 1.06/include/Fft.h | 18 + 1.Software/PDR 1.06/include/Filter.h | 52 + 1.Software/PDR 1.06/include/Kalman.h | 54 + 1.Software/PDR 1.06/include/LinearFit.h | 66 + 1.Software/PDR 1.06/include/Location.h | 139 ++ 1.Software/PDR 1.06/include/LocationTool.h | 43 + 1.Software/PDR 1.06/include/Main.h | 85 ++ 1.Software/PDR 1.06/include/Matrix.h | 70 + 1.Software/PDR 1.06/include/PDRBase.h | 232 +++ 1.Software/PDR 1.06/include/ParseData.h | 110 ++ 1.Software/PDR 1.06/include/Pedometer.h | 140 ++ 1.Software/PDR 1.06/include/Utils.h | 141 ++ 1.Software/PDR 1.06/include/buffer.h | 78 ++ 1.Software/PDR 1.06/include/m_gps.h | 15 + 1.Software/PDR 1.06/include/m_munkres.h | 19 + 1.Software/PDR 1.06/include/pdr_api.h | 151 ++ 1.Software/PDR 1.06/include/pdr_detector.h | 111 ++ 1.Software/PDR 1.06/include/pdr_sensor.h | 314 +++++ .../PDR 1.06/include/scene_recognition.h | 158 +++ 1.Software/PDR 1.06/project/PDR.vcxproj | 207 +++ .../PDR 1.06/project/PDR.vcxproj.filters | 159 +++ 1.Software/PDR 1.06/src/AHRS.c | 114 ++ 1.Software/PDR 1.06/src/Buffer.c | 201 +++ 1.Software/PDR 1.06/src/Complex.cpp | 74 + 1.Software/PDR 1.06/src/Complex.h | 24 + 1.Software/PDR 1.06/src/CoordTrans.cpp | 74 + 1.Software/PDR 1.06/src/CoordTrans.h | 24 + 1.Software/PDR 1.06/src/Detector.c | 726 ++++++++++ 1.Software/PDR 1.06/src/Fft.c | 116 ++ 1.Software/PDR 1.06/src/Filter.c | 77 + 1.Software/PDR 1.06/src/Interface.c | 211 +++ 1.Software/PDR 1.06/src/Kalman.c | 267 ++++ 1.Software/PDR 1.06/src/LinearFit.c | 149 ++ 1.Software/PDR 1.06/src/Location.c | 564 ++++++++ 1.Software/PDR 1.06/src/Main.cpp | 460 ++++++ 1.Software/PDR 1.06/src/Matrix.c | 195 +++ 1.Software/PDR 1.06/src/Munkres.c | 295 ++++ 1.Software/PDR 1.06/src/PDRBase.c | 265 ++++ 1.Software/PDR 1.06/src/ParseData.c | 891 ++++++++++++ 1.Software/PDR 1.06/src/Pedometer.c | 1245 +++++++++++++++++ 1.Software/PDR 1.06/src/Quaternion.cpp | 44 + 1.Software/PDR 1.06/src/Quaternion.h | 33 + 1.Software/PDR 1.06/src/SceneRecognition.c | 492 +++++++ 1.Software/PDR 1.06/src/Utils.c | 465 ++++++ 1.Software/PDR 1.06/src/imu.c | 7 + 1.Software/PDR 1.06/src/m_gps.c | 7 + 51 files changed, 9729 insertions(+), 377 deletions(-) create mode 100644 1.Software/PDR 1.06/gnssIns.sln create mode 100644 1.Software/PDR 1.06/include/AHRS.h create mode 100644 1.Software/PDR 1.06/include/Fft.h create mode 100644 1.Software/PDR 1.06/include/Filter.h create mode 100644 1.Software/PDR 1.06/include/Kalman.h create mode 100644 1.Software/PDR 1.06/include/LinearFit.h create mode 100644 1.Software/PDR 1.06/include/Location.h create mode 100644 1.Software/PDR 1.06/include/LocationTool.h create mode 100644 1.Software/PDR 1.06/include/Main.h create mode 100644 1.Software/PDR 1.06/include/Matrix.h create mode 100644 1.Software/PDR 1.06/include/PDRBase.h create mode 100644 1.Software/PDR 1.06/include/ParseData.h create mode 100644 1.Software/PDR 1.06/include/Pedometer.h create mode 100644 1.Software/PDR 1.06/include/Utils.h create mode 100644 1.Software/PDR 1.06/include/buffer.h create mode 100644 1.Software/PDR 1.06/include/m_gps.h create mode 100644 1.Software/PDR 1.06/include/m_munkres.h create mode 100644 1.Software/PDR 1.06/include/pdr_api.h create mode 100644 1.Software/PDR 1.06/include/pdr_detector.h create mode 100644 1.Software/PDR 1.06/include/pdr_sensor.h create mode 100644 1.Software/PDR 1.06/include/scene_recognition.h create mode 100644 1.Software/PDR 1.06/project/PDR.vcxproj create mode 100644 1.Software/PDR 1.06/project/PDR.vcxproj.filters create mode 100644 1.Software/PDR 1.06/src/AHRS.c create mode 100644 1.Software/PDR 1.06/src/Buffer.c create mode 100644 1.Software/PDR 1.06/src/Complex.cpp create mode 100644 1.Software/PDR 1.06/src/Complex.h create mode 100644 1.Software/PDR 1.06/src/CoordTrans.cpp create mode 100644 1.Software/PDR 1.06/src/CoordTrans.h create mode 100644 1.Software/PDR 1.06/src/Detector.c create mode 100644 1.Software/PDR 1.06/src/Fft.c create mode 100644 1.Software/PDR 1.06/src/Filter.c create mode 100644 1.Software/PDR 1.06/src/Interface.c create mode 100644 1.Software/PDR 1.06/src/Kalman.c create mode 100644 1.Software/PDR 1.06/src/LinearFit.c create mode 100644 1.Software/PDR 1.06/src/Location.c create mode 100644 1.Software/PDR 1.06/src/Main.cpp create mode 100644 1.Software/PDR 1.06/src/Matrix.c create mode 100644 1.Software/PDR 1.06/src/Munkres.c create mode 100644 1.Software/PDR 1.06/src/PDRBase.c create mode 100644 1.Software/PDR 1.06/src/ParseData.c create mode 100644 1.Software/PDR 1.06/src/Pedometer.c create mode 100644 1.Software/PDR 1.06/src/Quaternion.cpp create mode 100644 1.Software/PDR 1.06/src/Quaternion.h create mode 100644 1.Software/PDR 1.06/src/SceneRecognition.c create mode 100644 1.Software/PDR 1.06/src/Utils.c create mode 100644 1.Software/PDR 1.06/src/imu.c create mode 100644 1.Software/PDR 1.06/src/m_gps.c diff --git a/1.Software/PDR 1.05/include/Pedometer.h b/1.Software/PDR 1.05/include/Pedometer.h index c333fa1..d9b5eec 100644 --- a/1.Software/PDR 1.05/include/Pedometer.h +++ b/1.Software/PDR 1.05/include/Pedometer.h @@ -1,25 +1,10 @@ #pragma once -/******************** (C) COPYRIGHT 2017 Geek************************************ -* File Name : Steps_Algorithm.h +/******************** (C) COPYRIGHT 2023 Geek************************************ +* File Name : Pedometer.h * Department : Sensor Team -* Current Version : V1.0 -* Author : Xiaoyong Li -* Date of Issued : 2017.05.8 -* Comments: step counter algorithm added -******************************************************************************** -* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR REFERENCE OR -* EDUCATION. AS A RESULT, Geek SOFTWARE SHALL NOT BE HELD LIABLE FOR ANY -* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING -* FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE -* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -******************************************************************************** -* Release Log -* -* Comments: step counter algorithm released -* Version Number : V1.0 -* Author : Xiaoyong Li -* Date of Issued : 2017.05.8 +* Current Version : V1.0.0 +* Author : logzhan +* Date of Issued : 2023.02.24 *******************************************************************************/ #ifdef __cplusplus @@ -29,66 +14,45 @@ extern "C" { #define _STEPS_ALGORITHM_H_ #include "pdr_sensor.h" -#define ACC_SAMPLE_RATE_100HZ - //#define ACC_SAMPLE_RATE_25HZ +#define IMU_SAMPLE_FREQ 100 // IMU采样频率100Hz +#define MS2S 1000 // 秒转毫秒单位 + +#define DOWN_SAMPLE_NUM 4 // 降采样数量 +#define ACC_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // ACC采样间隔10ms +#define GYRO_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // GYR采样间隔10ms +#define SAMPLE_TIME_MIN 9 + +#define PEDOMETER_FREQ IMU_SAMPLE_FREQ / DOWN_SAMPLE_NUM // 计步器的频率是 +#define SampleFrequencyFour 100 +#define SampleFrequencydouble 50 +#define SampleFrequencyOPF (37.5l) +#define SampleFrequencySix 150 +#define AvergeFilterOrder 5 +#define AXIS_NUM 4 // 轴的数量有4个: x、y、z、xyz +#define FILTER_NUM 4 +#define FiltDifNumber 4 +#define FILTER_AXIS_NUM AXIS_NUM * FILTER_NUM // 轴数量4，滤波器数量4，总共16轴滤波器输出 +#define AxisNumberStepSaveState 20 // AxisNumberFiltOut + FiltDifNumber +#define ACC_BUFF_NUM 39 +#define WIN_LENGTH_AVE_DEVI 50 // +#define PI2A (180/3.1415926) -#ifdef ACC_SAMPLE_RATE_25HZ -#define DownResampleTimes 1 -#define ACC_SAMPLE_TIME 40 -#define SAMPLE_TIME_MIN 36 -#else -#define DOWN_SAMPLE_TIMES 4 -#define ACC_SAMPLE_TIME 10 //origin acc sampleFrequency is 100 hz -#define GYRO_SAMPLE_TIME 10 -#define SAMPLE_TIME_MIN 9 -#endif +#define STARTSTEPS 8 -#define SampleFrequency 25 -#define SampleFrequencyFour 100 -#define SampleFrequencydouble 50 -#define SampleFrequencyOPF (37.5l) -#define SampleFrequencySix 150 -#define AvergeFilterOrder 5 -#define AxisNumber 4 -#define FiltDifNumber 4 -#define AxisNumberFiltOut 16 //(AxisNumber * FiltDifNumber) -#define AxisNumberStepSaveState 20 //(AxisNumberFiltOut + FiltDifNumber) -#define AccelerationNumber 39 -#define ORI_ACC_SAMPLE_RATE 100 //origin acc sampleFrequency is 100 hz -#define WIN_LENGTH_AVE_DEVI 50 // -#define PI2A (180/3.1415926) -//#define NSTOS 1000000000 -#define MS2S 1000 - - -#define ACC_INSPECT_TIMEWIDTH 200 //origin acc sampleFrequency is 100 hz +#define ACC_INSPECT_TIMEWIDTH 200 // Origin acc sampleFrequency is 100 hz #define WINDOW_LENGTH_MIN2MAX (ACC_INSPECT_TIMEWIDTH/2 - 1) //25hz , 1 second -//#define WINDOW_LENGTH_IN_VEHICLE -#define STARTSTEPS 8 + + - /*#define PLATFORM_ANDROID_QCOM_AP*/ - /*#define PLATFORM_ANDROID_QCOM_ADSP*/ #define PLATFORM_PC_WINDOWS - //#define PLATFORM_PC_LINUX + #if defined PLATFORM_ANDROID_QCOM_ADSP #define STEPLIB_MEMSET SNS_OS_MEMSET #if 1 -#define SAM_STEP_LIB_NAME "steplib" -#define SAM_STEP_LIB_MSG_0(msg) UMSG(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg) -#define SAM_STEP_LIB_MSG_1(msg,p1) UMSG_1(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1) -#define SAM_STEP_LIB_MSG_2(msg,p1,p2) UMSG_2(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2) -#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3) UMSG_3(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3) -#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4) UMSG_4(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3,p4) -#else -#define SAM_STEP_LIB_MSG_0(msg) -#define SAM_STEP_LIB_MSG_1(msg,p1) -#define SAM_STEP_LIB_MSG_2(msg,p1,p2) -#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3) -#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4) #endif #elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845 #define STEPLIB_MEMSET memset @@ -108,38 +72,39 @@ typedef struct { double walk; double walkRealTime; double nD; -}stepsDfine; +}StepsDefine_t; typedef enum { - nonSteps = 0, + nonSteps = 0, walk, -}enumState; + run, +}StepState_t; typedef struct { unsigned char sign; - enumState state; + StepState_t state; unsigned long long stateStartTime; unsigned long long stepsStartTime; }stateNowDfine; typedef struct { - enumState state; + StepState_t state; unsigned long long startTimeStamp; -}stateDfine; +}StateDef_t; typedef struct { int flag; double ax; double ay; double az; -}downSample; +}DownSample_t; /**--------------------------------------------------------------------- -* Function : getPedometerLibVersion +* Function : Pedometer_GetVersion * Description : 获取计步器版本 -* Date : 2020/2/20 +* Date : 2023/02/24 *---------------------------------------------------------------------**/ -const char* GetPedometerVersion(void); +const char* Pedometer_GetVersion(void); /**--------------------------------------------------------------------- * Function : initPedometer @@ -149,28 +114,28 @@ const char* GetPedometerVersion(void); void Pedometer_Init(void); /**--------------------------------------------------------------------- -* Function : reinitPedometer -* Description : PDR 重新初始化计步器 -* Date : 2020/2/20 logzhan +* Function : Pedometer_ReInit +* Description : 重新初始化计步器 +* Date : 2023/02/24 logzhan *---------------------------------------------------------------------**/ -void reinitPedometer(void); +void Pedometer_ReInit(void); /**--------------------------------------------------------------------- -* Function : updatePedometer +* Function : PedometerUpdate * Description : PDR 计步器的加速度输入, 并通过指针返回用户步数 -* Date : 2020/2/1 logzhan +* Date : 2023/02/25 logzhan 重构函数 *---------------------------------------------------------------------**/ -void PedometerUpdate(IMU_t* ss_data, unsigned long* step); - -void Steps_State_Detect(enumState* state); +void Pedometer_Update(IMU_t* ss_data, unsigned long* step); +void Pedometer_GetState(StepState_t* state); /**--------------------------------------------------------------------- -* Function : detVehicleMode +* Function : DetVehicleMode * Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数 -* Date : 2020/2/1 logzhan +* Date : 2023/02/24 logzhan *---------------------------------------------------------------------**/ -void detVehicleMode(double ax, double ay, double az); +void DetVehicleMode(double ax, double ay, double az); + #endif #ifdef __cplusplus } diff --git a/1.Software/PDR 1.05/src/Location.c b/1.Software/PDR 1.05/src/Location.c index 3a29841..314c959 100644 --- a/1.Software/PDR 1.05/src/Location.c +++ b/1.Software/PDR 1.05/src/Location.c @@ -134,7 +134,7 @@ int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) { if (pdr.MotionType == DETECTOR_TYPE_HANDHELD)pdr.motionFreq = 0.1f; // 计步器输入传感器数据，占10%的运行时间 - PedometerUpdate(ImuData, &pdr.steps); + Pedometer_Update(ImuData, &pdr.steps); // ----- Filter Initialization ------- // if (pdr.sysStatus == IS_INITIAL) { diff --git a/1.Software/PDR 1.05/src/Pedometer.c b/1.Software/PDR 1.05/src/Pedometer.c index e25aecf..f15ef7f 100644 --- a/1.Software/PDR 1.05/src/Pedometer.c +++ b/1.Software/PDR 1.05/src/Pedometer.c @@ -14,62 +14,57 @@ const static char* Pedometer_Version = "Pedometer.1.0.0.20230224"; -static void Steps_Acc_Save(double ax, double ay, double az); +static void AccBufferUpdate(double ax, double ay, double az); static void State_Detect(unsigned long long timeStamp); -static void Steps_Filt(void); +static void Pedometer_Filter(void); static void Steps_Feature(void); static void Steps_Rcording(unsigned long long timeStamp); - -static double stepsFiltAcc[AxisNumber][AccelerationNumber]; +static double AccBuffer[AXIS_NUM][ACC_BUFF_NUM]; static float AccData[ACC_INSPECT_TIMEWIDTH] = { 0 }; static float lastAccZ = 0; static unsigned int ReceivedHowManyAcc = 0; static double SumAcc = 0; -static double DifFiltCoefficientOne[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, 0.234567901234568 }; -static double DifFiltCoefficientTwo[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056, 0.0832, 0.1088, 0.128, 0.136 }; -static double DifFiltCoefficientThree[13] = { 0.000416493127863390, 0.00166597251145356, 0.00416493127863390, 0.00832986255726780, 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, 0.0962099125364431 }; -static double DifFiltCoefficientFour[17] = { 0.000152415790275873, 0.000609663161103490, 0.00152415790275873, 0.00304831580551745, 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, 0.0745313214449017 }; +static double DifFiltCoefficient1[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, 0.234567901234568 }; +static double DifFiltCoefficient2[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056, 0.0832, 0.1088, 0.128, 0.136 }; +static double DifFiltCoefficient3[13] = { 0.000416493127863390, 0.00166597251145356, 0.00416493127863390, 0.00832986255726780, 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, 0.0962099125364431 }; +static double DifFiltCoefficient4[17] = { 0.000152415790275873, 0.000609663161103490, 0.00152415790275873, 0.00304831580551745, 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, 0.0745313214449017 }; -static double DifFiltOutput[AxisNumberFiltOut][5]; -static double OutputFilt[AxisNumberFiltOut][2]; -static double StepLcVar[AxisNumberFiltOut][3]; -static char StepReFeature[AxisNumberFiltOut] = { 0 }; -static double StepFetureAmp[AxisNumberFiltOut][8]; -static unsigned short StepFetureTime[AxisNumberFiltOut][8]; +static double DifFiltOutput[FILTER_AXIS_NUM][5]; +static double OutputFilt[FILTER_AXIS_NUM][2]; +static double StepLcVar[FILTER_AXIS_NUM][3]; +static char StepReFeature[FILTER_AXIS_NUM] = { 0 }; +static double StepFetureAmp[FILTER_AXIS_NUM][8]; +static unsigned short StepFetureTime[FILTER_AXIS_NUM][8]; static double StepRecordeState[AxisNumberStepSaveState][4]; static double StepRecordeStateTime[AxisNumberStepSaveState][9]; static unsigned long long timeStampSave[AxisNumberStepSaveState][2]; static unsigned short StepMaxTime = 100; -/*锟斤拷锟斤拷锟斤拷锟节达拷锟斤拷锟斤拷锟斤拷锟姐法锟侥诧拷锟斤拷锟斤拷锟斤拷为40ms,25HZ*/ static unsigned short StepMinTime = 10; static unsigned long long timeStamp = 0; static unsigned long long timeStamp_in_vehicle = 0; static char Flag_whether_in_vehicle = 0; static char Flag_step_exist = 0; -static double AvergeFiltSave[AxisNumber - 1][AvergeFilterOrder]/* = { 0 }*/; -stepsDfine Steps = { 0, 0 }; -stepsRe ReSteps = { 0 }; -downSample DownSample = { 0, 0, 0, 0 }; - -static double StateDetectLcvar[5] = { 0 }; -static enumState stateLcVar[2] = { nonSteps, nonSteps }; - +static double AvergeFiltSave[AXIS_NUM - 1][AvergeFilterOrder]; +StepsDefine_t Steps = { 0, 0 }; +stepsRe ReSteps = { 0 }; +DownSample_t DownSample = { 0, 0, 0, 0 }; +static double StateDetectLcVar[5] = { 0 }; +static StepState_t stateLcVar[2] = { nonSteps, nonSteps }; stateNowDfine stateNow; -stateDfine Restate; +StateDef_t Restate; /**--------------------------------------------------------------------- -* Function : GetPedometerVersion +* Function : Pedometer_GetVersion * Description : 鑾峰彇璁℃鍣ㄧ増鏈 * Date : 2023/02/24 *---------------------------------------------------------------------**/ -const char* GetPedometerVersion(void) -{ +const char* Pedometer_GetVersion(void){ return Pedometer_Version; } @@ -80,7 +75,7 @@ const char* GetPedometerVersion(void) *---------------------------------------------------------------------**/ void Pedometer_Init(void) { - STEPLIB_MEMSET(stepsFiltAcc, 0, sizeof(stepsFiltAcc)); + STEPLIB_MEMSET(AccBuffer, 0, sizeof(AccBuffer)); STEPLIB_MEMSET(DifFiltOutput, 0, sizeof(DifFiltOutput)); STEPLIB_MEMSET(OutputFilt, 0, sizeof(OutputFilt)); STEPLIB_MEMSET(StepLcVar, 0, sizeof(StepLcVar)); @@ -95,7 +90,7 @@ void Pedometer_Init(void) timeStamp = 0; STEPLIB_MEMSET(&Steps, 0, sizeof(Steps)); STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps)); - STEPLIB_MEMSET(StateDetectLcvar, 0, sizeof(StateDetectLcvar)); + STEPLIB_MEMSET(StateDetectLcVar, 0, sizeof(StateDetectLcVar)); STEPLIB_MEMSET(stateLcVar, 0, sizeof(stateLcVar)); STEPLIB_MEMSET(&stateNow, 0, sizeof(stateNow)); STEPLIB_MEMSET(&Restate, 0, sizeof(Restate)); @@ -113,42 +108,41 @@ void Pedometer_Init(void) * Description : PDR 閲嶆柊鍒濆鍖栬姝ュ櫒 * Date : 2020/2/20 logzhan *---------------------------------------------------------------------**/ -void reinitPedometer(void) +void Pedometer_ReInit(void) { timeStamp = 0; STEPLIB_MEMSET(&Steps, 0, sizeof(Steps)); STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps)); } - /**--------------------------------------------------------------------- -* Function : updatePedometer +* Function : Pedometer_Update * Description : PDR 璁℃鍣ㄧ殑鍔犻熷害杈撳叆, 骞堕氳繃鎸囬拡杩斿洖鐢ㄦ埛姝ユ暟 -* Date : 2020/2/1 logzhan +* Date : 2023/02/25 logzhan *---------------------------------------------------------------------**/ -void PedometerUpdate(IMU_t* ss_data, unsigned long* step) +void Pedometer_Update(IMU_t* imu, unsigned long* step) { unsigned char k = 0; // 妫娴嬪綋鍓嶆槸鍚﹀睘浜庡紑杞︾姸鎬, 杩欎釜鍑芥暟涔熸槸鐩稿鑰楁椂鐨, 杩欎釜鍑芥暟鍙敤 // 浜嗗姞閫熷害璁＄殑z杞, 鏄笉鏄瓨鍦ㄩ棶棰橈紵 - detVehicleMode(ss_data->acc.s[0], ss_data->acc.s[1], ss_data->acc.s[2]); + DetVehicleMode(imu->acc.s[0], imu->acc.s[1], imu->acc.s[2]); timeStamp += ACC_SAMPLE_TIME; - DownSample.ax += ss_data->acc.s[0] / 9.8; - DownSample.ay += ss_data->acc.s[1] / 9.8; - DownSample.az += ss_data->acc.s[2] / 9.8; + DownSample.ax += imu->acc.s[0] / 9.8; + DownSample.ay += imu->acc.s[1] / 9.8; + DownSample.az += imu->acc.s[2] / 9.8; DownSample.flag++; - if (DownSample.flag == DOWN_SAMPLE_TIMES) + if (DownSample.flag == DOWN_SAMPLE_NUM) { if (Restate.startTimeStamp == 0) { Restate.startTimeStamp = timeStamp; } - DownSample.ax /= DOWN_SAMPLE_TIMES; - DownSample.ay /= DOWN_SAMPLE_TIMES; - DownSample.az /= DOWN_SAMPLE_TIMES; + DownSample.ax /= DOWN_SAMPLE_NUM; + DownSample.ay /= DOWN_SAMPLE_NUM; + DownSample.az /= DOWN_SAMPLE_NUM; for (k = 0; k < AvergeFilterOrder - 1; k++) { AvergeFiltSave[0][k] = AvergeFiltSave[0][k + 1]; @@ -170,15 +164,15 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step) DownSample.ay /= AvergeFilterOrder; DownSample.az /= AvergeFilterOrder; - // 璁℃鍣ㄧ殑鍔犻熷害鏇存柊 - Steps_Acc_Save(DownSample.ax, DownSample.ay, DownSample.az); + // 璁℃鍣ㄧ紦瀛楤uffer鏇存柊 + AccBufferUpdate(DownSample.ax, DownSample.ay, DownSample.az); // 璁℃鍣ㄧ姸鎬佹娴 State_Detect(timeStamp); - if (StateDetectLcvar[4] < 0.5l) + if (StateDetectLcVar[4] < 0.5l) { // 婊ゆ尝澶勭悊 - Steps_Filt(); + Pedometer_Filter(); // 璁℃鐗瑰緛 Steps_Feature(); // 姝ユ暟璁板綍 @@ -189,7 +183,8 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step) } // 濡傛灉璁℃鏍囧織浣嶄笉瀛樺湪锛岄偅涔堜笉璁℃鏁 if (Flag_step_exist == 0){ - timeStamp = timeStamp + 1; //vehicle for debug + // vehicle for debug + timeStamp = timeStamp + 1; } else if (llabs((long long)(timeStamp - timeStamp_in_vehicle)) > 500) { @@ -204,7 +199,7 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step) * Description : PDR 妫娴嬫苯杞︽ā寮, 濡傛灉鏄娴嬪埌姹借溅妯″紡锛岄偅涔堜笉璁℃鏁 * Date : 2020/2/1 logzhan *---------------------------------------------------------------------**/ -void detVehicleMode(double ax, double ay, double az) +void DetVehicleMode(double ax, double ay, double az) { int t; int index_move; @@ -291,156 +286,101 @@ void detVehicleMode(double ax, double ay, double az) } -static void Steps_Acc_Save(double ax, double ay, double az) +/**--------------------------------------------------------------------- +* Function : AccBufferUpdate +* Description : 鏇存柊Acc鐨凚uffer缂撳瓨 +* Date : 2023/02/25 +*---------------------------------------------------------------------**/ +static void AccBufferUpdate(double ax, double ay, double az) { - short i, j; - for (i = 0; i < AxisNumber; i++){ - for (j = 0; j < (AccelerationNumber - 1); j++){ - stepsFiltAcc[i][j] = stepsFiltAcc[i][j + 1]; + for (uint8_t i = 0; i < AXIS_NUM; i++){ + for (uint8_t j = 0; j < (ACC_BUFF_NUM - 1); j++){ + AccBuffer[i][j] = AccBuffer[i][j + 1]; } } - stepsFiltAcc[0][AccelerationNumber - 1] = ax; - stepsFiltAcc[1][AccelerationNumber - 1] = ay; - stepsFiltAcc[2][AccelerationNumber - 1] = az; - stepsFiltAcc[3][AccelerationNumber - 1] = (sqrt(ax*ax + ay*ay + az*az)); + AccBuffer[0][ACC_BUFF_NUM - 1] = ax; + AccBuffer[1][ACC_BUFF_NUM - 1] = ay; + AccBuffer[2][ACC_BUFF_NUM - 1] = az; + AccBuffer[3][ACC_BUFF_NUM - 1] = sqrt(ax*ax + ay*ay + az*az); } -/* -DifFiltOutput 锟斤拷锟斤拷为16*5 -OutputFilt 锟斤拷锟斤拷为16*2 -0 filter one and acc x -1 filter one and acc y -2 filter one and acc z -3 filter one and acc xyz -4 filter two and acc x -5 filter two and acc y -6 filter two and acc z -7 filter two and acc xyz -8 filter three and acc x -9 filter three and acc y -10 filter three and acc z -11 filter three and acc xyz -12 filter four and acc x -13 filter four and acc y -14 filter four and acc z -15 filter four and acc xyz - -*/ -static void Steps_Filt(void)//filter bank +/**--------------------------------------------------------------------- +* Function : Pedometer_Filter +* Description : 璁℃鍣ㄦ护娉㈠櫒,涓鍏辨湁4涓娈电殑浣庨氭护娉㈠櫒銆傞拡瀵箈,y,z,mod(x,y,z) +* 鐨勬暟鎹繘琛屾护娉㈠鐞嗭紝涓鍏卞緱鍒16杞寸殑杈撳嚭鏁版嵁銆 +* Date : 2023/02/25 +*---------------------------------------------------------------------**/ +static void Pedometer_Filter(void) { short i, j; double FiltSum; - for (i = 0; i < AxisNumberFiltOut; i++) - { + for (i = 0; i < FILTER_AXIS_NUM; i++){ DifFiltOutput[i][0] = DifFiltOutput[i][1]; DifFiltOutput[i][1] = DifFiltOutput[i][2]; DifFiltOutput[i][2] = DifFiltOutput[i][3]; DifFiltOutput[i][3] = DifFiltOutput[i][4]; - OutputFilt[i][0] = OutputFilt[i][1]; + OutputFilt[i][0] = OutputFilt[i][1]; } - for (i = 0; i < AxisNumber; i++) - { + + for (i = 0; i < AXIS_NUM; i++){ FiltSum = 0; - /*filter系锟斤拷锟斤拷锟斤拷为5*/ for (j = 0; j < 4; j++) { - FiltSum += DifFiltCoefficientOne[j] * (stepsFiltAcc[i][15 + j] + stepsFiltAcc[i][23 - j]); + FiltSum += DifFiltCoefficient1[j] * (AccBuffer[i][15 + j] + AccBuffer[i][23 - j]); } - FiltSum += DifFiltCoefficientOne[4] * stepsFiltAcc[i][19]; + FiltSum += DifFiltCoefficient1[4] * AccBuffer[i][19]; DifFiltOutput[i][4] = (FiltSum); FiltSum = 0; - /*filter系锟斤拷锟斤拷锟斤拷为9*/ for (j = 0; j < 8; j++) { - FiltSum += DifFiltCoefficientTwo[j] * (stepsFiltAcc[i][11 + j] + stepsFiltAcc[i][27 - j]); + FiltSum += DifFiltCoefficient2[j] * (AccBuffer[i][11 + j] + AccBuffer[i][27 - j]); } - FiltSum += DifFiltCoefficientTwo[8] * stepsFiltAcc[i][19]; - DifFiltOutput[i + AxisNumber][4] = (FiltSum); + FiltSum += DifFiltCoefficient2[8] * AccBuffer[i][19]; + DifFiltOutput[i + AXIS_NUM][4] = (FiltSum); FiltSum = 0; - /*filter系锟斤拷锟斤拷锟斤拷为13*/ - for (j = 0; j < 12; j++) - { - FiltSum += DifFiltCoefficientThree[j] * (stepsFiltAcc[i][7 + j] + stepsFiltAcc[i][31 - j]); + for (j = 0; j < 12; j++){ + FiltSum += DifFiltCoefficient3[j] * (AccBuffer[i][7 + j] + AccBuffer[i][31 - j]); } - FiltSum += DifFiltCoefficientThree[12] * stepsFiltAcc[i][19]; - DifFiltOutput[i + AxisNumber * 2][4] = (FiltSum); + FiltSum += DifFiltCoefficient3[12] * AccBuffer[i][19]; + DifFiltOutput[i + AXIS_NUM * 2][4] = (FiltSum); FiltSum = 0; - /*filter系锟斤拷锟斤拷锟斤拷为17*/ - for (j = 0; j < 16; j++) - { - FiltSum += DifFiltCoefficientFour[j] * (stepsFiltAcc[i][3 + j] + stepsFiltAcc[i][35 - j]); + for (j = 0; j < 16; j++){ + FiltSum += DifFiltCoefficient4[j] * (AccBuffer[i][3 + j] + AccBuffer[i][35 - j]); } - FiltSum += DifFiltCoefficientFour[16] * stepsFiltAcc[i][19]; - DifFiltOutput[i + AxisNumber * 3][4] = (FiltSum); + FiltSum += DifFiltCoefficient4[16] * AccBuffer[i][19]; + DifFiltOutput[i + AXIS_NUM * 3][4] = (FiltSum); } - - for (i = 0; i < AxisNumberFiltOut; i++) - { + // 鏇存柊16涓酱璁℃鍣ㄧ殑杈撳嚭缁撴灉 + for (i = 0; i < FILTER_AXIS_NUM; i++){ OutputFilt[i][1] = DifFiltOutput[i][4]; } } -/* -StepLcVar 锟斤拷锟斤拷为16*3 -StepLcVar[nA][0]锟斤拷锟斤拷锟斤拷.锟斤拷录锟斤拷锟斤拷时锟斤拷锟斤拷. -StepLcVar[nA][1]锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟铰斤拷锟侥憋拷志锟斤拷1:锟斤拷锟斤拷 -1:锟铰斤拷 0:锟斤拷始值 -StepLcVar[nA][2]锟芥储锟斤拷一锟斤拷锟斤拷锟斤拷虿ü鹊姆锟街 - - - -StepFetureAmp 锟斤拷锟斤拷为16*8 锟斤拷锟节存储锟斤拷锟街 -StepFetureTime 锟斤拷锟斤拷为16*8 锟斤拷锟节存储锟斤拷锟斤拷时锟斤拷锟斤拷 -StepReFeature 锟斤拷锟斤拷为16*1 - -0 filter one and acc x -1 filter one and acc y -2 filter one and acc z -3 filter one and acc xyz -4 filter two and acc x -5 filter two and acc y -6 filter two and acc z -7 filter two and acc xyz -8 filter three and acc x -9 filter three and acc y -10 filter three and acc z -11 filter three and acc xyz -12 filter four and acc x -13 filter four and acc y -14 filter four and acc z -15 filter four and acc xyz - - -*/ static void Steps_Feature(void) { short nA, nB; - for (nA = 0; nA < AxisNumberFiltOut; nA++) + for (nA = 0; nA < FILTER_AXIS_NUM; nA++) { - /*锟斤拷锟斤拷锟斤拷.每锟轿加ｏ拷*/ StepLcVar[nA][0] = StepLcVar[nA][0] + 1; - /*锟揭诧拷锟饺ｏ拷锟斤拷锟斤拷锟斤拷锟斤拷(StepLcVar[nA][1] <1)锟斤拷锟斤拷锟斤拷一锟斤拷锟斤拷锟斤拷(OutputFilt[nA][1] > OutputFilt[nA][0])锟斤拷锟斤拷锟斤拷锟揭边碉拷锟斤拷锟斤拷锟斤拷 StepLcVar[nA][1] = 1*/ - if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) || ((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1)))) + if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) || + ((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1)))) { for (nB = 0; nB < 7; nB++) { StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1]; } - /*锟芥储锟斤拷锟街=锟斤拷一锟轿诧拷锟斤拷值-锟斤拷前锟斤拷锟斤拷值*/ StepFetureAmp[nA][7] = StepLcVar[nA][2] - OutputFilt[nA][0]; - /*锟芥储锟斤拷前锟斤拷锟饺的凤拷值*/ + StepLcVar[nA][2] = OutputFilt[nA][0]; for (nB = 0; nB < 7; nB++) { StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1]; } - /*锟芥储锟斤拷锟侥硷拷锟绞憋拷锟*/ StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]); - /*锟揭碉拷锟斤拷锟饺后，硷拷锟斤拷锟斤拷锟斤拷锟斤拷*/ StepLcVar[nA][0] = 0; - /*锟揭碉拷锟斤拷锟饺后，硷拷录锟斤拷锟斤拷状态*/ StepLcVar[nA][1] = 1; StepReFeature[nA] = 1; } @@ -451,19 +391,14 @@ static void Steps_Feature(void) { StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1]; } - /*锟芥储锟斤拷锟街=锟斤拷一锟轿诧拷锟斤拷值-锟斤拷前锟斤拷锟斤拷值*/ StepFetureAmp[nA][7] = OutputFilt[nA][0] - StepLcVar[nA][2]; - /*锟芥储锟斤拷前锟斤拷锟斤拷姆锟街*/ StepLcVar[nA][2] = OutputFilt[nA][0]; for (nB = 0; nB < 7; nB++) { StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1]; } - /*锟芥储锟斤拷锟侥硷拷锟绞憋拷锟*/ StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]); - /*锟揭碉拷锟斤拷锟斤拷螅锟斤拷锟斤拷锟斤拷锟斤拷锟*/ StepLcVar[nA][0] = 0; - /*锟揭碉拷锟斤拷锟饺后，硷拷录锟较斤拷状态*/ StepLcVar[nA][1] = -1; StepReFeature[nA] = 1; } @@ -477,10 +412,10 @@ static void Steps_Rcording(unsigned long long timeStamp) double StepFetureAmp12, StepFetureAmp34, StepFetureAmp56, StepFetureAmp78, StepFetureAmp14, StepFetureAmp58, StepFetureAmp18; unsigned short StepFetureTime12, StepFetureTime34, StepFetureTime56, StepFetureTime78, StepFetureTime14, StepFetureTime58, StepFetureTime38, StepFetureTime18; int j; - for (nC = AxisNumber; nC < AxisNumberStepSaveState; nC++) + for (nC = AXIS_NUM; nC < AxisNumberStepSaveState; nC++) { - if ((StepRecordeStateTime[nC][4] > 5) && ((nD == 0) || (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) && (nC != AxisNumberFiltOut)) + if ((StepRecordeStateTime[nC][4] > 5) && ((nD == 0) || (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) && (nC != FILTER_AXIS_NUM)) { nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]; } @@ -488,23 +423,23 @@ static void Steps_Rcording(unsigned long long timeStamp) if (nD > 0) { - for (nC = 0; nC < AxisNumber - 1; nC++) // nC 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) { nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]; } } - nC = AxisNumberFiltOut; + nC = FILTER_AXIS_NUM; if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) { nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]; } } Differ_Sum = 0; - for (j = 0; j < AccelerationNumber; j++) + for (j = 0; j < ACC_BUFF_NUM; j++) { - Differ_Sum = Differ_Sum + fabs(stepsFiltAcc[3][j + 1] - stepsFiltAcc[3][j]); + Differ_Sum = Differ_Sum + fabs(AccBuffer[3][j + 1] - AccBuffer[3][j]); } nC = 3; //20180917,nC=4 to 3; @@ -522,7 +457,7 @@ static void Steps_Rcording(unsigned long long timeStamp) } } - for (nA = 0; nA < AxisNumberFiltOut; nA++) + for (nA = 0; nA < FILTER_AXIS_NUM; nA++) { if (StepReFeature[nA] == 1) { @@ -550,17 +485,17 @@ static void Steps_Rcording(unsigned long long timeStamp) StepRecordeStateTime[nA][3] = StepRecordeStateTime[nA][3] + StepFetureTime[nA][7]; /*(nA % 4 == 3)为锟较硷拷锟劫度撅拷锟剿诧拷锟斤拷锟斤拷锟街*/ - if ((nA % 4 == 3) && (StepRecordeState[AxisNumberFiltOut + nA / 4][2] < 30)) + if ((nA % 4 == 3) && (StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] < 30)) { - StepRecordeState[AxisNumberFiltOut + nA / 4][2] = StepRecordeState[AxisNumberFiltOut + nA / 4][2] + 0.51; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] + StepFetureTime[nA][7]; - StepRecordeState[AxisNumberFiltOut + nA / 4][3] = StepRecordeState[AxisNumberFiltOut + nA / 4][3] + 0.51; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] + StepFetureTime[nA][7]; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] + 0.51; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] + StepFetureTime[nA][7]; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] + 0.51; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] + StepFetureTime[nA][7]; } } /*0.1538 0.1445 0.1352 0.1260 0.1167 0.1075 0.0982 0.0890 0.0798 0.0705 0.0612 0.0520 0.0427 0.0335 0.0243 0.0150*/ - thred = 0.015 + ((AxisNumberFiltOut - nA - 1) / AxisNumber) * 0.037;//dynamic threshold + thred = 0.015 + ((FILTER_AXIS_NUM - nA - 1) / AXIS_NUM) * 0.037;//dynamic threshold if (StepRecordeStateTime[nA][6] < 60) //25 { @@ -576,15 +511,15 @@ static void Steps_Rcording(unsigned long long timeStamp) /*(nA % 4 == 3)为锟较硷拷锟劫度撅拷锟剿诧拷锟斤拷锟斤拷锟街*/ if (nA % 4 == 3) { - if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] < 25) + if (StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6] < 25) { - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6]++; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6]++; } else { - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = 0; } if ((fabs(StepFetureAmp[nA][2] - StepFetureAmp[nA][3]) / (StepFetureAmp34) < 0.15l) && (fabs(StepFetureAmp[nA][3] - StepFetureAmp[nA][4]) / (StepFetureAmp[nA][3] + StepFetureAmp[nA][4]) < 0.15l) @@ -600,15 +535,15 @@ static void Steps_Rcording(unsigned long long timeStamp) && (StepFetureTime58 > StepMinTime) && (StepFetureTime58 < StepMaxTime)) { - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = SampleFrequency * 3.0 / StepFetureTime38; - timeStampSave[AxisNumberFiltOut + nA / 4][1] = timeStamp; - if (StepRecordeState[AxisNumberFiltOut + nA / 4][0] == 0) + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][8] = PEDOMETER_FREQ * 3.0 / StepFetureTime38; + timeStampSave[FILTER_AXIS_NUM + nA / 4][1] = timeStamp; + if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] == 0) { - StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38; - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] + StepFetureTime38; - timeStampSave[AxisNumberFiltOut + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / SampleFrequency); + StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + 3; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepFetureTime38; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] + StepFetureTime38; + timeStampSave[FILTER_AXIS_NUM + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / PEDOMETER_FREQ); } else { @@ -618,78 +553,78 @@ static void Steps_Rcording(unsigned long long timeStamp) && ((ABS_INT(StepFetureTime[nA][5] - StepFetureTime[nA][7]) < 0.2 * (StepFetureTime[nA][5] + StepFetureTime[nA][7])) || (ABS_INT(StepFetureTime[nA][5] - StepFetureTime[nA][7]) < 5))) { - if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] > 0) && (fabs(StepFetureTime38 / 3.0 - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7]) > 0.2 * StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7])) + if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] > 0) && (fabs(StepFetureTime38 / 3.0 - StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7]) > 0.2 * StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7])) { - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0; } - if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] >= 6) + if (StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6] >= 6) { - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] + 3; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] + StepFetureTime38; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] + 3; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] + StepFetureTime38; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6] = 0; } - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = StepFetureTime38 / 3.0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = StepFetureTime38 / 3.0; } - if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] <= 3) + if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] <= 3) { - StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + StepRecordeState[AxisNumberFiltOut + nA / 4][3]; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3]; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3]; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3]; } else { - StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + 3; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepFetureTime38; } - if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] <= 3) + if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] <= 3) { - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + StepRecordeState[AxisNumberFiltOut + nA / 4][3]; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3]; } - else if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] - StepFetureTime38 > StepMaxTime) - && (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 11)) + else if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] - StepFetureTime38 > StepMaxTime) + && (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] > 11)) { - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3; } else { - if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] > 0) + if (StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] > 0) { - nE = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] * StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] / StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5]; + nE = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] * StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] / StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5]; } - if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] > 0) && (nE < StepRecordeState[AxisNumberFiltOut + nA / 4][3])) + if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] > 0) && (nE < StepRecordeState[FILTER_AXIS_NUM + nA / 4][3])) { if (nE > 4) { - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + nE; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + nE; } else { - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3; } } else { - if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 4) + if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] > 4) { - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + StepRecordeState[AxisNumberFiltOut + nA / 4][3]; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3]; } else { - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3; } } } - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] + StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3]; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3]; } - StepRecordeState[AxisNumberFiltOut + nA / 4][2] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = 0; - StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] = 0; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = 0; } - else if ((StepRecordeState[AxisNumberFiltOut + nA / 4][0] > 0) && ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency))) + else if ((StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] > 0) && ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ))) { nB = AxisNumberStepSaveState - 1; if (nD > 0) @@ -704,15 +639,15 @@ static void Steps_Rcording(unsigned long long timeStamp) } else { - for (nC = (AxisNumberStepSaveState - 1); nC >= AxisNumber; nC--) + for (nC = (AxisNumberStepSaveState - 1); nC >= AXIS_NUM; nC--) { - if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != AxisNumberFiltOut)) + if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != FILTER_AXIS_NUM)) { nB = nC; } } } - if ((nB == AxisNumberFiltOut + nA / 4) && (StepRecordeState[nB][0] >= 8)) + if ((nB == FILTER_AXIS_NUM + nA / 4) && (StepRecordeState[nB][0] >= 8)) { if (Steps.walkRealTime > StepRecordeState[nB][1]) { @@ -732,7 +667,7 @@ static void Steps_Rcording(unsigned long long timeStamp) //#endif for (nC = 0; nC < AxisNumberStepSaveState; nC++) { - if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency)) + if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ)) { StepRecordeState[nC][1] = 0; StepRecordeStateTime[nC][1] = 0; @@ -773,17 +708,17 @@ static void Steps_Rcording(unsigned long long timeStamp) } } } - StepRecordeState[AxisNumberFiltOut + nA / 4][0] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = 0; - StepRecordeState[AxisNumberFiltOut + nA / 4][1] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = 0; - StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0; - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = 0; - timeStampSave[AxisNumberFiltOut + nA / 4][0] = 0; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = 0; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = 0; + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = 0; + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][8] = 0; + timeStampSave[FILTER_AXIS_NUM + nA / 4][0] = 0; } } @@ -802,11 +737,11 @@ static void Steps_Rcording(unsigned long long timeStamp) if ((StepFetureTime58 >= StepMaxTime) || ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l))) { - StepRecordeStateTime[nA][8] = SampleFrequency * 8.0 / StepFetureTime18; + StepRecordeStateTime[nA][8] = PEDOMETER_FREQ * 8.0 / StepFetureTime18; } else { - StepRecordeStateTime[nA][8] = SampleFrequency * 4.0 / StepFetureTime18; + StepRecordeStateTime[nA][8] = PEDOMETER_FREQ * 4.0 / StepFetureTime18; } timeStampSave[nA][1] = timeStamp; if (StepRecordeState[nA][0] == 0) @@ -823,7 +758,7 @@ static void Steps_Rcording(unsigned long long timeStamp) StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4; } StepRecordeStateTime[nA][1] = StepRecordeStateTime[nA][1] + StepFetureTime18; - timeStampSave[nA][0] = timeStamp - (unsigned long)(StepFetureTime18 * 1000 / SampleFrequency); + timeStampSave[nA][0] = timeStamp - (unsigned long)(StepFetureTime18 * 1000 / PEDOMETER_FREQ); } else { @@ -951,7 +886,7 @@ static void Steps_Rcording(unsigned long long timeStamp) StepRecordeState[nA][3] = 0; StepRecordeStateTime[nA][3] = 0; } - else if ((StepRecordeState[nA][0] > 0) && ((StepRecordeStateTime[nA][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency))) + else if ((StepRecordeState[nA][0] > 0) && ((StepRecordeStateTime[nA][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ))) { nB = AxisNumberStepSaveState - 1; if (nD > 0) @@ -966,9 +901,9 @@ static void Steps_Rcording(unsigned long long timeStamp) } else { - for (nC = (AxisNumberStepSaveState - 1); nC >= AxisNumber; nC--) + for (nC = (AxisNumberStepSaveState - 1); nC >= AXIS_NUM; nC--) { - if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != AxisNumberFiltOut)) + if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != FILTER_AXIS_NUM)) { nB = nC; } @@ -986,14 +921,10 @@ static void Steps_Rcording(unsigned long long timeStamp) } Steps.walkRealTime = 0; ReSteps.walkSteps = (unsigned long)(Steps.walk); - //#if defined(PLATFORM_PC_WINDOWS) || defined(PLATFORM_PC_LINUX) - // steps_back(ReSteps); - //#elif defined(PLATFORM_ANDROID_QCOM_AP) || defined(PLATFORM_ANDROID_QCOM_ADSP) - // AutoDisplayStep_Event_Handler(ReSteps); - //#endif + for (nC = 0; nC < AxisNumberStepSaveState; nC++) { - if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency)) + if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ)) { StepRecordeState[nC][1] = 0; StepRecordeStateTime[nC][1] = 0; @@ -1204,119 +1135,114 @@ static void State_Detect(unsigned long long timeStamp) double SumAccYZ = 0; double AccX = 0; double AccYZ = 0; - /*double Mean_x = 0, Mean_y = 0, Mean_z = 0;*/ - /*double DeDC_x = 0, DeDC_y = 0, DeDC_z = 0, DeDC_xyz = 0;*/ -#if defined(PLATFORM_ANDROID_QCOM_SLPI845) - (void)timeStamp; -#endif - if (StateDetectLcvar[0] < StepMinTime) + + if (StateDetectLcVar[0] < StepMinTime) { - StateDetectLcvar[0] = StateDetectLcvar[0] + 1; - if (StateDetectLcvar[0] < 0.5l) + StateDetectLcVar[0] = StateDetectLcVar[0] + 1; + if (StateDetectLcVar[0] < 0.5l) { SumAcc = -1; - } - else - { + }else{ SumAcc = SumAcc - 1; } } else { SumAcc = 0; - for (nA = 0; nA < (AccelerationNumber - 1); nA++) + for (nA = 0; nA < (ACC_BUFF_NUM - 1); nA++) { - SumAcc += fabs(stepsFiltAcc[3][nA] - stepsFiltAcc[3][nA + 1]); - SumAccX += fabs(stepsFiltAcc[0][nA] - stepsFiltAcc[0][nA + 1]); - SumAccYZ += fabs(sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA]) - - sqrt(stepsFiltAcc[1][nA + 1] * stepsFiltAcc[1][nA + 1] + stepsFiltAcc[2][nA + 1] * stepsFiltAcc[2][nA + 1])); + SumAcc += fabs(AccBuffer[3][nA] - AccBuffer[3][nA + 1]); + SumAccX += fabs(AccBuffer[0][nA] - AccBuffer[0][nA + 1]); + SumAccYZ += fabs(sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA]) + - sqrt(AccBuffer[1][nA + 1] * AccBuffer[1][nA + 1] + AccBuffer[2][nA + 1] * AccBuffer[2][nA + 1])); } - SumAcc /= (AccelerationNumber - 1); - SumAccX /= (AccelerationNumber - 1); - SumAccYZ /= (AccelerationNumber - 1); - for (nA = 0; nA < AccelerationNumber; nA++) - { - AccX += fabs(stepsFiltAcc[0][nA]); - } - AccX = AccX / AccelerationNumber; + SumAcc /= (ACC_BUFF_NUM - 1); + SumAccX /= (ACC_BUFF_NUM - 1); + SumAccYZ /= (ACC_BUFF_NUM - 1); - for (nA = 0; nA < AccelerationNumber; nA++) - { - AccYZ += sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA]); + + for (nA = 0; nA < ACC_BUFF_NUM; nA++){ + AccX += fabs(AccBuffer[0][nA]); } - AccYZ /= AccelerationNumber; + AccX = AccX / ACC_BUFF_NUM; + + for (nA = 0; nA < ACC_BUFF_NUM; nA++) + { + AccYZ += sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA]); + } + AccYZ /= ACC_BUFF_NUM; if (SumAcc > 3 || SumAcc < 0.002l) { - if (StateDetectLcvar[2] < SampleFrequencyFour) + if (StateDetectLcVar[2] < SampleFrequencyFour) { - StateDetectLcvar[2] = StateDetectLcvar[2] + StepMinTime; + StateDetectLcVar[2] = StateDetectLcVar[2] + StepMinTime; } - StateDetectLcvar[3] = 0; + StateDetectLcVar[3] = 0; } else { - if (StateDetectLcvar[3] < SampleFrequencyFour) + if (StateDetectLcVar[3] < SampleFrequencyFour) { - StateDetectLcvar[3] = StateDetectLcvar[3] + StepMinTime; + StateDetectLcVar[3] = StateDetectLcVar[3] + StepMinTime; } - StateDetectLcvar[2] = 0; + StateDetectLcVar[2] = 0; } - if (StateDetectLcvar[2] > SampleFrequencydouble) + if (StateDetectLcVar[2] > SampleFrequencydouble) { - StateDetectLcvar[4] = 1; + StateDetectLcVar[4] = 1; } - else if (StateDetectLcvar[2] > SampleFrequencyOPF) + else if (StateDetectLcVar[2] > SampleFrequencyOPF) { STEPLIB_MEMSET(StepReFeature, 1, sizeof(StepReFeature)); } - if (StateDetectLcvar[3] > StepMinTime) + if (StateDetectLcVar[3] > StepMinTime) { - StateDetectLcvar[4] = 0; + StateDetectLcVar[4] = 0; } if (((AccX * 1.3l > AccYZ) || (SumAccX * 1.3l > SumAccYZ)) && (SumAcc < 0.8l)) { - if (StateDetectLcvar[1] > 0) + if (StateDetectLcVar[1] > 0) { - StateDetectLcvar[1] = StateDetectLcvar[1] - 1; + StateDetectLcVar[1] = StateDetectLcVar[1] - 1; }else{ - StateDetectLcvar[1] = 0; + StateDetectLcVar[1] = 0; } } else { - if ((StateDetectLcvar[1] > 4) || (SumAcc > 0.6l)) + if ((StateDetectLcVar[1] > 4) || (SumAcc > 0.6l)) { - StateDetectLcvar[1] = 8;//10 + StateDetectLcVar[1] = 8;//10 } else { if ((AccX < AccYZ) && (SumAcc > 0.25l)) { - if (StateDetectLcvar[1] < 8)//10 + if (StateDetectLcVar[1] < 8)//10 { - StateDetectLcvar[1] = StateDetectLcvar[1] + 1; + StateDetectLcVar[1] = StateDetectLcVar[1] + 1; } } else { - StateDetectLcvar[1] = 0; + StateDetectLcVar[1] = 0; } } } - if (StateDetectLcvar[1] > 4){ + if (StateDetectLcVar[1] > 4){ stateLcVar[0] = 2; }else{ stateLcVar[0] = walk; } - StateDetectLcvar[0] = 0; + StateDetectLcVar[0] = 0; } } -void Steps_State_Detect(enumState* state){ +void Pedometer_GetState(StepState_t* state){ *state = stateLcVar[0]; } \ No newline at end of file diff --git a/1.Software/PDR 1.06/gnssIns.sln b/1.Software/PDR 1.06/gnssIns.sln new file mode 100644 index 0000000..a3249dd --- /dev/null +++ b/1.Software/PDR 1.06/gnssIns.sln @@ -0,0 +1,31 @@ +锘 +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.30804.86 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PDR", "project\PDR.vcxproj", "{28D32532-309F-40EA-9E4A-2D162CC434D2}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x64.ActiveCfg = Debug|x64 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x64.Build.0 = Debug|x64 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x86.ActiveCfg = Debug|Win32 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x86.Build.0 = Debug|Win32 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x64.ActiveCfg = Release|x64 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x64.Build.0 = Release|x64 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x86.ActiveCfg = Release|Win32 + {28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {B6378B77-5AB0-4A12-ACE8-2B45F292D7EE} + EndGlobalSection +EndGlobal diff --git a/1.Software/PDR 1.06/include/AHRS.h b/1.Software/PDR 1.06/include/AHRS.h new file mode 100644 index 0000000..53e919f --- /dev/null +++ b/1.Software/PDR 1.06/include/AHRS.h @@ -0,0 +1,48 @@ +#ifndef _PDR_AHRS_H_ +#define _PDR_AHRS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Header File Including ------------------------------------------------------------------------ */ + +#include "pdr_sensor.h" + +/* Macro Declaration ---------------------------------------------------------------------------- */ +#define AHRS_SAMPLE_FREQ 100 // ARHS算法的传感器采样频率 +#define IMU_SAMPLING_FREQUENCY 100 +#define AHRS_KP 0.500 +#define AHRS_TYPE_GYRO 4 +#define AHRS_TYPE_ACCE 1 +#define AHRS_TYPE_MAGN 2 +#define AHRS_STATUS_RESET_PHASE_0 0x80 +#define AHRS_STATUS_RESET_PHASE_1 0x40 +#define AHRS_STATUS_RESET (AHRS_STATUS_RESET_PHASE_0|AHRS_STATUS_RESET_PHASE_1) +#define AHRS_STATUS_STABLE 0x20 + +/* Struct Declaration --------------------------------------------------------------------------- */ + + +/* Function Declaration ------------------------------------------------------------------------- */ + +void AHRS_Init(void); +void ResetARHS(void); + +/**--------------------------------------------------------------------- +* Function : fv3Norm +* Description : 三维浮点数向量归一化 +* Date : 2021/01/26 yuanlin@vivocom && +* +* +*---------------------------------------------------------------------**/ +void fv3Norm(float* vx, float* vy, float* vz); + +int UpdateAHRS(IMU_t* imu); + + +#ifdef __cplusplus +} +#endif + +#endif // for __AHRS_H diff --git a/1.Software/PDR 1.06/include/Fft.h b/1.Software/PDR 1.06/include/Fft.h new file mode 100644 index 0000000..21becc1 --- /dev/null +++ b/1.Software/PDR 1.06/include/Fft.h @@ -0,0 +1,18 @@ +#ifndef __PDR_FFT_H__ +#define __PDR_FFT_H__ + +/* Header File Including ------------------------------------------------------------------------ */ +#include +#include +#include "Complex.h" + +/* Macro Declaration ---------------------------------------------------------------------------- */ +#define M_PI 3.14159265358979323846 /* pi */ + +/* Structure Declaration ------------------------------------------------------------------------ */ + +/* Function Declaration ------------------------------------------------------------------------- */ +extern int FFT_Dft(Complex_t *fft, float *signal, int length); +extern int FFT_Fft(Complex_t *fft, float *signal, int length); + +#endif diff --git a/1.Software/PDR 1.06/include/Filter.h b/1.Software/PDR 1.06/include/Filter.h new file mode 100644 index 0000000..3c84732 --- /dev/null +++ b/1.Software/PDR 1.06/include/Filter.h @@ -0,0 +1,52 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Filter.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR滤波器支持库 +********************************************************************************/ +/* Header File Including ------------------------------------------------------*/ +#ifndef __FILTER_H__ +#define __FILTER_H__ +/* Macro Declaration ----------------------------------------------------------*/ +#define FILTER_MAX_LENGTH 71 + +/* Structure Declaration ------------------------------------------------------*/ +typedef struct Filter_t { // iir or fir filter + int reset; // reset flag + int order; // coefficient array length, not filter order + double H0; // dc signal gain + double b[FILTER_MAX_LENGTH]; // b coefficient + double a[FILTER_MAX_LENGTH]; // a coefficient + double yout[FILTER_MAX_LENGTH]; // yout buffer + double xin[FILTER_MAX_LENGTH]; // xin buffer +} Filter_t; + +/* Function Declaration -------------------------------------------------------*/ + +/**--------------------------------------------------------------------- +* Function : FilterReset +* Description : 重置滤波器 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterReset(Filter_t *f); + +/**--------------------------------------------------------------------- +* Function : FilterSetCoef +* Description : 滤波器设置参数 +* Input : order coefficient array length, not filter order +* *b b coefficient +* *a a coefficient +* H0 dc signal gain +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterSetCoef(Filter_t *f, int order, double *b, double *a, double H0); + +/**--------------------------------------------------------------------- +* Function : FilterFilter +* Description : 滤波器滤波计算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +double FilterFilter(Filter_t *f, double x); + +#endif diff --git a/1.Software/PDR 1.06/include/Kalman.h b/1.Software/PDR 1.06/include/Kalman.h new file mode 100644 index 0000000..3333183 --- /dev/null +++ b/1.Software/PDR 1.06/include/Kalman.h @@ -0,0 +1,54 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_kalman.h +* Department : Sensor Algorithm Team +* Current Version : V2.0(compare QCOM SAP 5.0) +* Author : logzhan +* Date of Issued : 2020.7.3 +* Comments : PDR 卡尔曼滤波器头文件函数声明以及相关结构体定义 +********************************************************************************/ +#ifndef _PDR_KALMAN_H_ +#define _PDR_KALMAN_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/**---------------------------------------------------------------------- +* Function : EKF_Init +* Description : 初始化卡尔曼滤波器相关 +* Date : 2022/09/19 +*---------------------------------------------------------------------**/ +void EKF_Init(void); + +/**---------------------------------------------------------------------- +* Function : EKFStatePredict +* Description : pdr卡尔曼滤波器的状态预测方程 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +void EKFStatePredict(EKFPara_t* kf, double step_length, PDR_t* g_pdr, int step); + +/**---------------------------------------------------------------------- +* Function : EKFCalQRMatrix +* Description : 计算卡尔曼滤波噪声矩阵 +* scane_type, 用户所处场景（开阔和非开阔） +* nmea_data，NMEA数据结构体 +* g_pdr，PDR结构体 +* sys，运动幅度数据结构体 +* kf，EKF数据结构体 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +void EKFCalQRMatrix(Nmea_t* nmea_data, PDR_t* g_pdr, Classifer_t* sys, EKFPara_t* kf); + +/**---------------------------------------------------------------------- +* Function : pdr_ekfStateUpdate +* Description : pdr卡尔曼滤波器的状态更新方程 +* Date : 2020/7/22 logzhan +*---------------------------------------------------------------------**/ +void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, Classifer_t* sys, PDR_t* g_pdr, + int scene_type); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/1.Software/PDR 1.06/include/LinearFit.h b/1.Software/PDR 1.06/include/LinearFit.h new file mode 100644 index 0000000..93cdcfb --- /dev/null +++ b/1.Software/PDR 1.06/include/LinearFit.h @@ -0,0 +1,66 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_linearFit.h +* Department : Sensor Algorithm Team +* Current Version : V2.0(compare QCOM SAP 5.0) +* Author : + + + +logzhan +* Date of Issued : 2020.7.4 +* Comments : PDR 线性拟合相关函数支持 +********************************************************************************/ +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _PDR_LINEAR_FIT_H_ +#define _PDR_LINEAR_FIT_H_ + +/**---------------------------------------------------------------------- +* Function : pdr_linearLeastSquaresFitting +* Description : 线性最小二乘拟合 Y = c0 + c1 x + 目标函数为sum| nDataY - (a*nDataX+b)|^2最小 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +int linearLeastSquaresFit(const double *nDataX, const double *nDataY, + const int nLength, double *a, double *b); + +/************************************************************************** +* Description : 二维坐标系点的线性拟合，ax+by+c=0, 目标函数为点到拟合直线的距离 + 平方和最小 +* Input : x， y，二维坐标X和Y + num, 二维点个数 +* Output : para， 拟合直线参数, para[0]:a, para[0]:b, para[0]:c +**************************************************************************/ +int leastDistanceLinearFit(double x[], double y[], int num, double para[]); + + +/**---------------------------------------------------------------------- +* Function : gsl_fit_linear +* Description : Fit the data (x_i, y_i) to the linear relationship + Y = c0 + c1 x + returning, + c0, c1 -- coefficients + cov00, cov01, cov11 -- variance-covariance matrix of + c0 and c1, + sumsq -- sum of squares of residuals + This fit can be used in the case where the errors for + the data are uknown, but assumed equal for all points. + The resulting variance-covariance matrix estimates the + error in the coefficientsfrom the observed variance of + the points around the best fit line. +* Date : +*---------------------------------------------------------------------**/ +static int gslFitLinear(const double* x, const size_t xstride, + const double* y, const size_t ystride, + const size_t n, + double* c0, double* c1, + double* cov_00, double* cov_01, double* cov_11, double* sumsq); + + +#endif // !_LINEAR_FITTING_H_ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/include/Location.h b/1.Software/PDR 1.06/include/Location.h new file mode 100644 index 0000000..9166fc3 --- /dev/null +++ b/1.Software/PDR 1.06/include/Location.h @@ -0,0 +1,139 @@ +#ifndef __PDR_LOCATION_H__ +#define __PDR_LOCATION_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "PDRBase.h" + +#define HIST_GPS_NUM 8 +#define ACCURACY_THRES 0.6f // 精度阈值 +#define YAW_THRES 10.0f // Yaw角范围阈值 + + +/**--------------------------------------------------------------------- +* Function : NavSys_Init +* Description : PDR导航系统初始化 +* Date : 2022/9/16 +*---------------------------------------------------------------------**/ +void PDR_Init(void); + +void PDRInfoInit(void); + +/**--------------------------------------------------------------------- +* Function : InsLocation +* Description : PDR 惯性导航定位 +* Date : 2022/09/16 logzhan +*---------------------------------------------------------------------**/ +int InsLocationUpdate(IMU_t *ss_data, EKFPara_t *kf); + + +/**---------------------------------------------------------------------- +* Function : pdr_noGpsPredict +* Description : 在没有gps信息时预测GPS位置，最多预测10个点 +* Date : 2020/07/08 logzhan +*---------------------------------------------------------------------**/ +void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* g_pdr); + +/**---------------------------------------------------------------------- +* Function : Nmea2Gnss +* Description : nmea数据结构转gnss数据 +* Date : 2020/07/08 logzhan +*---------------------------------------------------------------------**/ +void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss); + + +/**---------------------------------------------------------------------- +* Function : pdr_detectFixMode +* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出 +* 原始GPS或者不输出 +* Date : 2020/07/08 logzhan +* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高 +* 代码的可读性 +*---------------------------------------------------------------------**/ +int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result); + +/**---------------------------------------------------------------------- +* Function : GnssCalHeading +* Description : 获取GPS偏航角(0 - 360°) +* Date : 2022/09/16 logzhan +* 代码的可读性 +*---------------------------------------------------------------------**/ +double GnssCalHeading(GNSS_t* pgnss); + +/**---------------------------------------------------------------------- +* Function : calPdrHeadingOffset +* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移 +* Date : 2020/07/08 logzhan +*---------------------------------------------------------------------**/ +void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr); + +/**--------------------------------------------------------------------- +* Function : pdr_resetSysStatus +* Description : PDS 重设系统状态 +* Date : 2020/2/1 logzhan +*---------------------------------------------------------------------**/ +void ResetSystemStatus(EKFPara_t* kf); + +/**---------------------------------------------------------------------- +* Function : pdr_gnssInsLocation +* Description : PDR GPS融合INS惯性定位 +* Date : 2021/01/29 logzhan +*---------------------------------------------------------------------**/ +int GnssInsFusionLocation(Nmea_t* nmea_data, EKFPara_t* kf, LctFs_t* result); + +/**--------------------------------------------------------------------- +* Function : pdr_initGnssInfo +* Description : PDS GNSS定位信息初始化 +* Date : 2020/2/1 logzhan +*---------------------------------------------------------------------**/ +void InitGnssInfo(void); + +/**---------------------------------------------------------------------- +* Function : GnssUpdate +* Description : nmea数据结构转gnss数据 +* Date : 2020/07/08 logzhan +* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数， +* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没 +* 什么用，还增加运算量 +*---------------------------------------------------------------------**/ +void GnssUpdate(GNSS_t* gps, Nmea_t* nmea); + +/**---------------------------------------------------------------------- +* Function : OutputRawGnssPos +* Description : 输出GPS位置 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result); + +/**---------------------------------------------------------------------- +* Function : ResetOutputLoction +* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数 +* 2、GPS值赋值给result +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result); + + +/**---------------------------------------------------------------------- +* Function : EkfGnssInsLocFusion +* Description : 采用EKF对GNSS和INS融合定位 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias, + EKFPara_t* kf, LctFs_t* res); + +/**---------------------------------------------------------------------- +* Function : PDRInitialAlignment +* Description : PDR初始对准 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/1.Software/PDR 1.06/include/LocationTool.h b/1.Software/PDR 1.06/include/LocationTool.h new file mode 100644 index 0000000..b601b39 --- /dev/null +++ b/1.Software/PDR 1.06/include/LocationTool.h @@ -0,0 +1,43 @@ +#pragma once +#ifndef _LOCATION_TOOL_H_ +#define _LOCATION_TOOL_H_ + +#include + +#ifndef PI +//#define PI 3.14159265358979323846 +#endif // !PI + +#ifndef EARTH_RADIUS +#define EARTH_RADIUS 6371008.8 +#endif + +typedef struct DoublePair { + double x; + double y; +} DoublePair; +typedef DoublePair PlaneCoordinate; + +typedef struct LatLng{ + double lat; + double lon; +} LatLng_t; + +typedef struct DoublePairList{ + double *x; + double *y; + size_t length; +} DoublePairList; +typedef struct LocationList{ + double *lat; + double *lon; + size_t length; +} LocationList; + +LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB); +DoublePair ProjPoint(DoublePair point, DoublePair linePointA, DoublePair linePointB); + +//PlaneCoordinate WGS84_XYZ(const LatLng lla); +double CalculateDistance(LatLng pointA, LatLng pointB); + +#endif // !_LOCATION_TOOL_H_ diff --git a/1.Software/PDR 1.06/include/Main.h b/1.Software/PDR 1.06/include/Main.h new file mode 100644 index 0000000..e5728cf --- /dev/null +++ b/1.Software/PDR 1.06/include/Main.h @@ -0,0 +1,85 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_main.h +* Department : Sensor Algorithm Team +* Current Version : V2.0 +* Author : + +& yuanlin@vivo.cm +* Date of Issued : 2020.7.18 +* Comments : PDR导航算法平台配置文件 +********************************************************************************/ +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _PDR_MAIN_H_ +#define _PDR_MAIN_H_ +#include + +using namespace std; + +#define PATH_MAX 256 +#define TRACK_MAX 100000 + +typedef struct LatLngd { + double lat; + double lon; + double heading; + double hdop; + double accuracy; + double vel; + double time; + int motionType; +} LatLngd; + +typedef struct ResultTracks { + LatLngd gpsTrack[TRACK_MAX]; + LatLngd pdrTrack[TRACK_MAX]; + int gpsLen; + int pdrLen; +}ResultTracks; + +/**---------------------------------------------------------------------- +* Function : pdr_writeKml +* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式 +* path : kml文件的输出文件路径 +* name : kml文件主体名称 +* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本 +* Date : 2020/11/1 logzhan +*---------------------------------------------------------------------**/ +void KmlWrite(string path, string name, string postfix); + +/**---------------------------------------------------------------------- +* Function : getSimulateFileFp +* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针 +* Date : 2021/01/25 logzhan +*---------------------------------------------------------------------**/ +FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead); + +/**---------------------------------------------------------------------- +* Function : gpsYaw2GoogleYaw +* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换 +* 为谷歌支持的Yaw规则 +* Date : 2021/01/25 logzhan +*---------------------------------------------------------------------**/ +double gpsYaw2GoogleYaw(double heading); + +/**---------------------------------------------------------------------- +* Function : Motion2TypeStr +* Description : 把用户运动类型转换为字符串输出 +* Date : 2022/9/16 logzhan +*---------------------------------------------------------------------**/ +const char* Motion2TypeStr(int type); + +/**---------------------------------------------------------------------- +* Function : updateResTrack +* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹 +* Date : 2021/01/25 logzhan +*---------------------------------------------------------------------**/ +void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs); + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/1.Software/PDR 1.06/include/Matrix.h b/1.Software/PDR 1.06/include/Matrix.h new file mode 100644 index 0000000..404e77e --- /dev/null +++ b/1.Software/PDR 1.06/include/Matrix.h @@ -0,0 +1,70 @@ +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _PDR_MATRIX_H_ +#define _PDR_MATRIX_H_ + +#define N 4 + +/**--------------------------------------------------------------------- +* Function : MatrixTrans +* Description : 矩阵转置 +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixTrans(double a[N][N], double r[N][N]); + +/**--------------------------------------------------------------------- +* Function : VecMatMultiply +* Description : 向量和矩阵相乘 r = b * a +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void VecMatMultiply(double a[N], double b[N][N], double r[N]); + +/**--------------------------------------------------------------------- +* Function : MatrixMultiply +* Description : 矩阵和矩阵相乘 r = a * b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]); + +/**--------------------------------------------------------------------- +* Function : MatrixAdd +* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]); + +/**--------------------------------------------------------------------- +* Function : VectorAdd +* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void VectorAdd(double a[N], double b[N], double r[N]); + +/**--------------------------------------------------------------------- +* Function : MatrixSub +* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixSub(double a[N][N], double b[N][N], double r[N][N]); + +/**--------------------------------------------------------------------- +* Function : VectorSub +* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void VectorSub(double a[N], double b[N], double r[N]); + +/**--------------------------------------------------------------------- +* Function : MatrixInverse +* Description : 求矩阵的逆矩阵 +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixInverse(double(*a)[N], double(*a_inv)[N]); + +#endif + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/include/PDRBase.h b/1.Software/PDR 1.06/include/PDRBase.h new file mode 100644 index 0000000..99e2ed6 --- /dev/null +++ b/1.Software/PDR 1.06/include/PDRBase.h @@ -0,0 +1,232 @@ +/******************** (C) COPYRIGHT 2021 Geek************************************ +* File Name : pdr_base.h +* Department : Sensor Algorithm Team +* Current Version : V2.0 +* Author : yuanlin@vivocom & +* +* +* Date of Issued : 2021.02.01 +* Comments : PDR 算法基本定义头文件 +********************************************************************************/ +#ifndef _PDR_BASE_H_ +#define _PDR_BASE_H_ +/* Header File Including -----------------------------------------------------*/ +#include "stdint.h" +#include "pdr_sensor.h" + +/* PDR SYS CFG ---------------------------------------------------------------*/ +#define PDR_OUTPUT_SMOOTH 0 // 是否开启轨迹平滑 + +/* Macro Definition ----------------------------------------------------------*/ +#define PCA_NUM 200 +#define ACCURACY_ERR_MAX 1000 // GPS的accuracy最大值,一般用于初始化用 +#define N 4 // 矩阵维数 +#define OPEN_SKY 1 +#define MAX_NO_GPS_PREDICT 10 // 无GPS信息状态，最大位置推算数量 +#define UN_UPDATE 0 // 非更新状态 +#define DATA_UPDATE 1 // 数据更新 +#define ON 1 +#define OFF 0 +#define PDR_TRUE 1 +#define PDR_FALSE 0 + +#define TYPE_FIX_NONE 0 // PDR修正结果为不输出 +#define TYPE_FIX_GPS 2 // PDR修正结果为输出原始GPS +#define TYPE_FIX_PDR 1 // PDR修正结果为PDR融合 + +// PDR携带方式 +#define UNKNOWN 0 +#define FORWARD_UP_RIGHT 1 // 臂包：右臂，头朝上，屏朝外；or左臂，头朝上，屏朝内 +#define BACKWARD_UP_LEFT 2 // 臂包：左臂，头朝上，屏朝外；or右臂，头朝上，屏朝内 +#define BACKWARD_DOWN_RIGHT 3 // 臂包：右臂，头朝下，屏朝外；or左臂，头朝下，屏朝内 +#define FORWARD_DOWN_LEFT 4 // 臂包：左臂，头朝下，屏朝外；or右臂，头朝下，屏朝内 +#define LEFT_UP_FORWARD 5 // 裤兜：头朝上，屏朝前 +#define RIGHT_UP_BACKWARD 6 // 裤兜：头朝上，屏朝后 +#define RIGHT_DOWN_FORWARD 7 // 裤兜：头朝下，屏朝前 +#define LEFT_DOWN_BACKWARD 8 // 裤兜：头朝下，屏朝后 + + +// PDR相关状态 +#define PDR_NO_ERROR 0 +#define PDR_OUT_OF_MEMORY 1 +#define PDR_MOTION_TYPE_STATIC 0 +#define PDR_MOTION_TYPE_IRREGULAR 1 +#define PDR_MOTION_TYPE_HANDHELD 2 +#define PDR_MOTION_TYPE_SWINGING 3 +#define PDR_STATUS_RESET 0x80 +#define PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40 +#define PDR_STATUS_PCA_STABLE 0x20 +#define PDR_STATUS_BIAS_STABLE 0x10 +#define PDR_STATUS_STABLE (PDR_STATUS_PCA_STABLE | PDR_STATUS_BIAS_STABLE) + +// 地球相关常数 +#define WGS84_RE 6378137 // WGS-84椭球体长半轴 +#define WGS84_ECCENTR2 6.69437999014e-3 // 第一偏向率的平方，e^2(WGS-84) +#define WGS84_OMEGA_E_DOT 7.2921151467e-5 // 地球自转角速度 rad/sec +#define WGS84_GRAVITY 9.7803267714 // 地球平均重力加速度 m/s^2 + +#define DEG_PER_RADIAN 57.295779513082323 // degrees per radian +#define RADIAN_PER_DEG 0.017453292519943 // radians per degree + +#define GPS_SPEED_UNIT 0.5144444 // GPS原始速度转m/s的缩放系数 +#define PI 3.1415926 +#define DOUBLE_ZERO 1e-10 +#define TWO_PI (2.0*PI) +#define R2D(x) (x*57.2957795130823) +#define D2R(x) (x*3.14159265/180.0) +#define USE_BUG_FOR_LOCAL_PARA 1 +#define BUF_DMT_1 1 +#define BUF_DMT_2 2 + +#define PRINT_ALGO_RUN_TIME 0 +#define PRINT_CLCT 1 +#define PATTERN_RECOGNITION_LEN 256 // 缓存256 / SAMPLE_RATE的数量 + +#define OPEN_AREA 1 // 开阔区域(GPS信号较好) +#define UN_OPEN_AREA 0 // 非开阔区域(GPS信号较弱) + + +#define DETECTOR_RUN_FREQ 1280 // 用户运动类型检测器检测周期ms +#define DETECTOR_TYPE_STATIC 0 // 用户静止 +#define DETECTOR_TYPE_IRREGULAR 1 // 无规律运动 +#define DETECTOR_TYPE_HANDHELD 2 // 手持运动 +#define DETECTOR_TYPE_SWINGING 3 // 摆手运动 + +#define ulong unsigned long +#define uchar unsigned char + +typedef enum { + IS_INITIAL = 0, + IS_NORMAL = 1, +}pdrStatus; + + +typedef struct { + double Xk[N]; // 状态变量Xk xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 + double pXk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 + double Zk[N]; + double pPk[N][N]; + double Pk[N][N]; + double Phi[N][N]; + double hk[N][N]; + double Q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声) + double R[N][N]; // 卡尔曼滤波R矩阵(观测噪声) + double Kk[N][N]; + double Lambda; + double pLat; + double pLon; + double initHeading; +}EKFPara_t; + +typedef struct{ + int fnum; + int deltaStep; + float fsum; + float meanTime; + double lastTime; +}StepPredict; + +typedef struct PDR { + uint32_t status; // PDR当前状态 + uint32_t MotionType; // 用户运动类型 + uint32_t NoGnssCount; // 没有GNSS信息次数 + pdrStatus sysStatus; // PDR系统状态 + int sceneType; // 场景类型：1：开阔区域，0：非开阔区域(信号较弱) + int fusionPdrFlg; // 融合PDR位置标志位, 当flg为0时纯输出GPS + double PllaInit[3]; // 初始plla坐标 + double ts; // 时间戳 + double x0; // 初始北向坐标 + double y0; // 初始东向坐标 + // 航向角相关 + double heading; // PDR方向角 + double lastHeading; // 上一次PDR方向角 + double deltaHeading; // 偏航角变化量 + double insHeadingOffset; // 惯导方向角偏移 + double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯 + double GpsHeading; // GPS航向角 + double LastGpsHeading; // 上一次GPS航向角 + double trackHeading; // GPS轨迹航向角 + // 速度相关 + double GpsSpeed; // GPS速度 + // 步数相关 + ulong steps; // 当前步数信息 + ulong lastSteps; // 上一次的步数 + ulong lastGpsSteps; // 上一次GPS步数 + ulong deltaStepsPerGps; // 两次GPS更新之间，步数的变化量 + float motionFreq; // 运动频率 + double gyroTime; // 陀螺仪时间 + float axis_ceofficient[3]; + float axis_direction[2]; + float pca_direction[2]; + float pcaAccuracy; + float bias_direction[2]; + float biasAccuracy; + float cal_direction[2]; +} PDR_t; + +typedef struct { + int type; + float accBuffer[PATTERN_RECOGNITION_LEN]; +}Classifer_t; + +// 用户运动类型分类器 +typedef struct DETECTOR { + uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动 + uint32_t lastType; + uint64_t tick; // 次数统计，用于调整检测器工作频率 +}Detector_t; + + +/* Function Declaration ------------------------------------------------------*/ + + +/************************************************************************** +* Description : 计算每步所耗时间 +* Input : stepPredict， 步数预测结构体 + timestamp，时间戳 + steps_last，上一次步数 + steps，本次步数 +**************************************************************************/ +void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps); + +/************************************************************************** +* Description : 预测步数 +* Input : stepPredict， 步数预测结构体 + timestamp，时间戳 + steps_last，上一次步数 + gnssVel，gps速度 +* Output : int， 步数 +**************************************************************************/ +int PredictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel); + +/**---------------------------------------------------------------------- +* Function : stateRecognition +* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 +* Date : 2020/7/22 logzhan +*---------------------------------------------------------------------**/ +void StateRecognition(float *acc, Classifer_t *sys); + + +/**---------------------------------------------------------------------- +* Function : InsLocationPredict +* Description : PDR惯导位置更新 +* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 +* 会造成PDR前后位置不够均匀 +*---------------------------------------------------------------------**/ +void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu, + GNSS_t* pgnss, EKFPara_t* kf); + + + +/************************************************************************** +* Description : 计算GPS轨迹角 +* Input : gpsPos，GPS历史定位点在ECEF下的三维坐标 + pgnss，GPS数据结构体 +* Output : double，轨迹角 +**************************************************************************/ +double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss); + + +#endif + +#pragma once diff --git a/1.Software/PDR 1.06/include/ParseData.h b/1.Software/PDR 1.06/include/ParseData.h new file mode 100644 index 0000000..9631850 --- /dev/null +++ b/1.Software/PDR 1.06/include/ParseData.h @@ -0,0 +1,110 @@ +#ifdef __cplusplus +extern "C" { +#endif +#ifndef _PARSEDATA_H_ +#define _PARSEDATA_H_ + +#include "pdr_sensor.h" + +#define NAEA_LAST_TIME 500 +#define IMU_LAST_COUNT 10 + +#define FLOAT_TO_INT 1000000 + +/************************************************************************** +* Description : NMEA数据和传感器数据解析 +* : NMEA数据包括RMC,GGA,GSA,GSV,以及安卓定位信息语句 + 传感器数据包括加速度计，陀螺仪，磁力计 +**************************************************************************/ + + char * strtok_ct(char * s, const char * delim); + +/************************************************************************** +* Description : 解析惯性传感器数据 +* Input : pt， 传感器数据字符串 + ts， 手机开机时间戳，ms + sstp，安卓中传感器类型标识符 +* Output : imu_p， 惯性传感器结构体 +**************************************************************************/ + void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp); + +/************************************************************************** +* Description : 解析NMEA数据 +* Input : pt， NMEA数据字符串 + ts， 手机开机时间戳，ms +* Output : ln， NMEA数据结构体 +**************************************************************************/ + void parseNMEA(char* pt, Nmea_t *ln, double ts); + +/************************************************************************** +* Description : 解析GGA协议 +* Input : pt， NMEA数据字符串 + ts， 手机开机时间戳，ms +* Output : ln， NMEA数据结构体 +**************************************************************************/ + void ParseGGA(char* pt, Nmea_t *ln, double ts); + +/************************************************************************** +* Description : 解析RMC协议 +* Input : pt， NMEA数据字符串 + ts， 手机开机时间戳，ms +* Output : ln， NMEA数据结构体 +**************************************************************************/ + void ParseRMC(char* pt, Nmea_t *ln, double ts); + +/************************************************************************** +* Description : 解析GSV协议 +* Input : pt， NMEA数据字符串 + ts， 手机开机时间戳，ms +* Output : ln， NMEA数据结构体 +**************************************************************************/ + void parseGSV(char* pt, Nmea_t *ln, double ts); + +/************************************************************************** +* Description : 解析GSA协议 +* Input : pt， NMEA数据字符串 + ts， 手机开机时间戳，ms +* Output : ln， NMEA数据结构体 +**************************************************************************/ + void ParseGSA(char* pt, Nmea_t *ln, double ts); + +/**---------------------------------------------------------------------- +* Function : parseLocAccuracy +* Description : 解析GPS的Accuracy精度参数 +* Date : 2020/7/9 yuanlin@vivo.com &logzhan +*---------------------------------------------------------------------**/ +void parseLocAccuracy(char* pt, Nmea_t *ln, double ts); + +/************************************************************************** +* Description : 检测NMEA数据是否解析完整 +* Input : ln， NMEA数据结构体 +**************************************************************************/ +void preprocessNMEA(Nmea_t *ln); + +int ParseGnssInsData(char* line, IMU_t* imu_p, Nmea_t* ln); + +/**---------------------------------------------------------------------- +* Function : HexToDec +* Description : 十六进制数转换为十进制数 +* Date : 2022-09-15 logzhan +*---------------------------------------------------------------------**/ +long HexToDec(char *source); + +/**---------------------------------------------------------------------- +* Function : pdr_int2Hex +* Description : 十六进制数转换为十进制数 +* Date : 2020/7/9 yuanlin@vivo.com & logzhan +*---------------------------------------------------------------------**/ +char * Int2Hex(int a, char *buffer); + + +/**---------------------------------------------------------------------- +* Function : pdr_getIndexOfSigns +* Description : 返回ch字符在sign数组中的序号 +* Date : 2020/7/9 yuanlin@vivo.com &logzhan +*---------------------------------------------------------------------**/ + int pdr_getIndexOfSigns(char ch); +#endif +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/include/Pedometer.h b/1.Software/PDR 1.06/include/Pedometer.h new file mode 100644 index 0000000..7706a4c --- /dev/null +++ b/1.Software/PDR 1.06/include/Pedometer.h @@ -0,0 +1,140 @@ +#pragma once +/******************** (C) COPYRIGHT 2023 Geek************************************ +* File Name : Pedometer.h +* Department : Sensor Team +* Current Version : V1.0.0 +* Author : logzhan +* Date of Issued : 2023.02.24 +*******************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif +#ifndef _STEPS_ALGORITHM_H_ +#define _STEPS_ALGORITHM_H_ + +#include "pdr_sensor.h" +#define IMU_SAMPLE_FREQ 100 // IMU采样频率100Hz +#define MS2S 1000 // 秒转毫秒单位 + +#define DOWN_SAMPLE_NUM 4 // 降采样数量 +#define ACC_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // ACC采样间隔10ms +#define GYRO_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // GYR采样间隔10ms +#define SAMPLE_TIME_MIN 9 + +#define PEDOMETER_FREQ IMU_SAMPLE_FREQ / DOWN_SAMPLE_NUM // 计步器的频率是 +#define SampleFrequencyFour 100 +#define SampleFrequencydouble 50 +#define SampleFrequencyOPF (37.5l) +#define SampleFrequencySix 150 +#define AVG_FILTER_BUFF_LEN 5 // 均值滤波器长度，默认为5 +#define AXIS_NUM 4 // 轴的数量有4个: x、y、z、xyz +#define FILTER_NUM 4 // 滤波器数量4个 +#define FILTER_AXIS_NUM AXIS_NUM * FILTER_NUM // 4*4=16, 轴数量4，滤波器数量4，总共16轴滤波器输出 +#define AXIS_ALL 20 // AxisNumberFiltOut + FiltDifNumber +#define ACC_BUFF_NUM 39 // ACC缓存长度39 +#define WIN_LENGTH_AVE_DEVI 50 // +#define PI2A (180/3.1415926) + +#define STARTSTEPS 8 + +#define DET_VEHICLE_ACC_BUFF_LEN 200 // Origin acc sampleFrequency is 100 hz + +#define WINDOW_LENGTH_MIN2MAX (DET_VEHICLE_ACC_BUFF_LEN/2 - 1) // 25hz , 1 second +#define PLATFORM_PC_WINDOWS + +#define STEP_TRUE 1 +#define STEP_FALSE 0 +#define AXIS_XYZ 3 + +#if defined PLATFORM_ANDROID_QCOM_ADSP +#define STEPLIB_MEMSET SNS_OS_MEMSET + +#if 1 +#endif +#elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845 +#define STEPLIB_MEMSET memset +#endif +#if defined PLATFORM_ANDROID_MTK +#define ABS_INT abs_value +#else +#define ABS_INT abs +#endif + + +typedef struct { + unsigned long walkSteps; +}stepsRe; + +typedef struct { + double walk; + double walkRealTime; + double nD; +}StepsDefine_t; + +typedef enum { + NonSteps = 0, + walk, + run, +}StepState_t; + +typedef struct { + unsigned char sign; + StepState_t state; + unsigned long long stateStartTime; + unsigned long long stepsStartTime; +}stateNowDefine; + +typedef struct { + StepState_t state; + unsigned long long startTimeStamp; +}StateDef_t; + +typedef struct { + int flag; + double ax; + double ay; + double az; +}DownSample_t; + +/**--------------------------------------------------------------------- +* Function : Pedometer_GetVersion +* Description : 获取计步器版本 +* Date : 2023/02/24 +*---------------------------------------------------------------------**/ +const char* Pedometer_GetVersion(void); + +/**--------------------------------------------------------------------- +* Function : initPedometer +* Description : PDR 初始化计步器 +* Date : 2020/2/20 logzhan +*---------------------------------------------------------------------**/ +void Pedometer_Init(void); + +/**--------------------------------------------------------------------- +* Function : Pedometer_ReInit +* Description : 重新初始化计步器 +* Date : 2023/02/24 logzhan +*---------------------------------------------------------------------**/ +void Pedometer_ReInit(void); + +/**--------------------------------------------------------------------- +* Function : PedometerUpdate +* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数 +* Date : 2023/02/25 logzhan 重构函数 +*---------------------------------------------------------------------**/ +void Pedometer_Update(IMU_t* imu, uint32_t* step); + +void Pedometer_GetState(StepState_t* state); + +/**--------------------------------------------------------------------- +* Function : DetVehicleMode +* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数 +* Date : 2023/02/24 logzhan +*---------------------------------------------------------------------**/ +void DetVehicleMode(double ax, double ay, double az); + +#endif +#ifdef __cplusplus +} +#endif diff --git a/1.Software/PDR 1.06/include/Utils.h b/1.Software/PDR 1.06/include/Utils.h new file mode 100644 index 0000000..0702015 --- /dev/null +++ b/1.Software/PDR 1.06/include/Utils.h @@ -0,0 +1,141 @@ +锘#ifndef _PDR_UTIL_H_ +#define _PDR_UTIL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + + +#define TRAJ_POINT_COUNT 1000 +#define MIN_POINT_COUNT 15 +#define OPT_TRAJ_TIMEOUT 1000 + +#define FALIED_LINE 0 +#define IN_LINE 1 +#define CLOSE_START 2 +#define CLOSE_END 3 + +typedef struct LatLng{ + double lat; + double lon; +} LatLng_t; + +typedef struct TrajSign +{ + int count; + double lastTime; + double weight; + double yaw[TRAJ_POINT_COUNT]; + double error[TRAJ_POINT_COUNT]; + double points[TRAJ_POINT_COUNT][2]; +}TrajSign; + +/**鍦扮悆鐩稿叧鍙傛暟*******************************************************************/ +typedef struct +{ + double rmh; /* 瀛愬崍鍦堟洸鐜囧崐寰 */ + double rnh; /* 鍗厜鍦堟洸鐜囧崐寰 */ + double grav; /* 褰撳湴閲嶅姏鍔犻熷害 */ + double lat; /* 褰撳湴绾害锛坮ad锛*/ + double wnie[3]; /* 鍦扮悊绯荤浉瀵规儻鎬х郴鐨勮閫熷害鍦ㄥ鑸郴鍒嗛噺 */ +} EarthData_t; + +/**---------------------------------------------------------------------- +* Function : mean +* Description : 瀵硅緭鍏ouble鏁扮粍姹傚潎鍊 +* Review : 姹傚潎鍊艰繃绋嬫瘡娆￠兘鍋氶櫎娉曪紝鏈変紭鍖栫┖闂达紝鍙互閽堝姹傝В鍧囧肩殑鏁板煎ぇ灏忛噰鐢 + * 涓嶅悓鍑芥暟 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +double mean(double *x, int n); + +float fmean(float *data, int len); + +float stdf(float* data, int len); + +double meanAngle(double* angle, int len); + +void modAngle(double * angle, double min, double max); + +double pow_i(double num, long long n); + +double pow_f(double num, double m); + +double pow(double x, double y); + +void centralization(double *x, double *x_out, int n); + +void GetCovMatrix(double *x, double *y, double cov[2][2], int n); + +int Jacobi(double a[][2], double p[][2], int n, double eps, int T); + + +LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB); + +double CalDistance(LatLng_t pointA, LatLng_t pointB); + +void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]); +/**---------------------------------------------------------------------- +* Function : pdr_invSqrt +* Description : 璁＄畻1/sqrt(x)鐨勫揩閫熺殑绠楁硶 +* Date : 2020/6/30 logzhan +*---------------------------------------------------------------------**/ +float InvSqrt(float x); + +/**--------------------------------------------------------------------- +* Function : pdr_v3Norm +* Description : 涓夌淮娴偣鏁板悜閲忓綊涓鍖 +* Date : 2021/01/26 yuanlin@vivocom && +* +* +*---------------------------------------------------------------------**/ +void Vec3Norm(float* vx, float* vy, float* vz); + +/**---------------------------------------------------------------------- +* Function : pdr_writeCsvStr +* Description : 灏嗗瓧绗︿覆淇℃伅鍐欏叆鏂囨湰锛屼富瑕佹槸瀵筬printf灏佽涓灞 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +void WriteCsvStr(FILE* fp_write, char* str); + +/**---------------------------------------------------------------------- +* Function : pdr_utc2hms +* Description : 灏哢TC鏃堕棿杞崲涓烘椂鍒嗙 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +void pdr_utc2hms(double utc, double* h, double* m, double* s); + +/**---------------------------------------------------------------------- +* Function : qnb2att +* Description : 鍥涘厓鏁拌浆娆ф媺瑙 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +void Qnb2Att(float* q, double* attitude); + +double CalRadianDifferent(double s_dir, double d_dir); + +/**---------------------------------------------------------------------- +* Function : pdr_earthParameter +* Description : 鏍规嵁杈撳叆绾害杩斿洖璇ョ含搴﹀湴鐞冪浉鍏冲弬鏁 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +EarthData_t CalEarthParameter(double oriLat); + + +/**---------------------------------------------------------------------- +* Function : Motion2TypeStr +* Description : 鎶婄敤鎴疯繍鍔ㄧ被鍨嬭浆鎹负瀛楃涓茶緭鍑 +* Date : 2022/9/16 logzhan +*---------------------------------------------------------------------**/ +const char* Motion2TypeStr(int type); + +int pdr_min(int param_a, int param_b); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/1.Software/PDR 1.06/include/buffer.h b/1.Software/PDR 1.06/include/buffer.h new file mode 100644 index 0000000..5f9555f --- /dev/null +++ b/1.Software/PDR 1.06/include/buffer.h @@ -0,0 +1,78 @@ +#ifndef _PDR_BUFFER_H_ +#define _PDR_BUFFER_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Header File Including ------------------------------------------------------------------------ */ + +/* Macro Declaration ---------------------------------------------------------------------------- */ +#define BUFFER_LONG_LENGTH 256 +#define BUFFER_SHORT_LENGTH 10 +#define BUFFER_TYPE_STACK 0 +#define BUFFER_TYPE_QUEUE 1 +#define BUFFER_NO_ERROR 0 +#define BUFFER_WRONG_PARAMETER 1 +#define BUFFER_OUT_OF_MEMORY 2 + +/* Structure Declaration ------------------------------------------------------------------------ */ +#pragma pack (4) + +typedef struct BUFFER { + char name[20]; + int type; + int length; + int _bottom; + int _top; + int reserved; + double sum; + double mean; + float data[BUFFER_LONG_LENGTH + 1]; +}Buffer_t; + +typedef struct BUFFER_LONG { + char name[20]; + int type; + int length; + int _bottom; + int _top; + int reserved; + double sum; + double mean; + float data[BUFFER_LONG_LENGTH + 1]; +} BUFFER_LONG; + +typedef struct BUFFER_SHORT { + char name[20]; + int type; + int length; + int _bottom; + int _top; + int reserved; + double sum; + double mean; + float data[BUFFER_SHORT_LENGTH + 1]; +} BUFFER_SHORT; +#pragma pack () + +/* Global Variable Declaration ------------------------------------------------------------------ */ + +/* Function Declaration ------------------------------------------------------------------------- */ +int BufferInit(Buffer_t *buffer, const char *name, int type, int length); +int BufferClear(Buffer_t *buffer); +int BufferCount(Buffer_t *buffer, int *count); +int BufferGetTop(Buffer_t *buffer, float *value); +int BufferGetBottom(Buffer_t *buffer, float *value); +int BufferPop(Buffer_t *buffer, float *value); +int BufferPush(Buffer_t *buffer, float value); +int BufferGet(Buffer_t *buffer, float *value, int index); +int BufferMean(Buffer_t *buffer, float *mean); +int BufferVar(Buffer_t *buffer, float *var); +int BufferStd(Buffer_t *buffer, float *std); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/include/m_gps.h b/1.Software/PDR 1.06/include/m_gps.h new file mode 100644 index 0000000..7f4548c --- /dev/null +++ b/1.Software/PDR 1.06/include/m_gps.h @@ -0,0 +1,15 @@ +#ifndef _PDR_GPS_H_ +#define _PDR_GPS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/1.Software/PDR 1.06/include/m_munkres.h b/1.Software/PDR 1.06/include/m_munkres.h new file mode 100644 index 0000000..458ea16 --- /dev/null +++ b/1.Software/PDR 1.06/include/m_munkres.h @@ -0,0 +1,19 @@ +#ifndef __MUNKRES_H +#define __MUNKRES_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Macro Definition ----------------------------------------------------------------------------- */ +#define MUNKRES_NO_ERROR 0 +#define MUNKRES_EXCESSIVE_MATRIX 1 + +/* Function Declaration ------------------------------------------------------------------------- */ +extern int MUNKRES_get_assignment(int *assignment, float* cost, int m, int n); + +#ifdef __cplusplus +} +#endif + +#endif // for __MUNKRES_H diff --git a/1.Software/PDR 1.06/include/pdr_api.h b/1.Software/PDR 1.06/include/pdr_api.h new file mode 100644 index 0000000..a348876 --- /dev/null +++ b/1.Software/PDR 1.06/include/pdr_api.h @@ -0,0 +1,151 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_api.h +* Department : Sensor Algorithm Team +* Current Version : V2.0(compare QCOM SAP 5.0) +* Author : logzhan +* Date of Issued : 2021.01.26 +* Comments : PDR 对外部接口 +********************************************************************************/ +#ifndef _PDR_API_H_ +#define _PDR_API_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include +#include "pdr_sensor.h" +#include "PDRBase.h" +#include "Utils.h" + +/* Macro Definition ----------------------------------------------------------*/ +#define IMU_LAST_COUNT 10 + +typedef struct _PosFusion{ + double lat; + double lon; + double gpsLat; + double gpsLon; + double t; + int vaild; +}PosFusion; + +/* Function Declaration ------------------------------------------------------*/ + +/**--------------------------------------------------------------------- +* Function : Algorithm_Init +* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨 +* 迹平滑窗口的初始化 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void Algorithm_Init(void); + +/**--------------------------------------------------------------------- +* Function : PDRLocationMainLoop +* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置 +* 迹平滑窗口的初始化 +* Date : 2022/10/16 logzhan +*---------------------------------------------------------------------**/ +int PDRLocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion); + +/**--------------------------------------------------------------------- +* Function : pdr_locationMainLoopStr +* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置 +* 迹平滑窗口的初始化 +* Date : 2020/07/03 logzhan +* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 +*---------------------------------------------------------------------**/ +int ParseDataAndUpdate(char* line, LctFs_t* result); + +/**--------------------------------------------------------------------- +* Function : pdr_initRmc +* Description : 初始化RMC结构体 +* Input : rmc， RMC结构体 +* Date : 2020/7/3 logzhan +*---------------------------------------------------------------------**/ +void RMC_Init(NmeaRMC_t * rmc); + +/**--------------------------------------------------------------------- +* Function : pdr_initNmeaFlg +* Description : 初始化GPS的NMEA协议的相关标志位 +* Input : ln， NMEA结构体 +* Date : 2020/7/3 logzhan +*---------------------------------------------------------------------**/ +void NmeaFlag_Init(Nmea_t * ln); + +/**--------------------------------------------------------------------- +* Function : clearNmeaFlg +* Description : 清空Nmea标志位,功能和init相同 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +void ClearNmeaFlg(Nmea_t * ln); + +/**--------------------------------------------------------------------- +* Function : pdr_saveGnssInfo +* Description : 保存GPS相关信息 +* Date : 2020/7/3 logzhan +*---------------------------------------------------------------------**/ +void SaveGnssInfo(Nmea_t* nmea_data, LctFs_t* result, FILE* fp_gps); + +/**---------------------------------------------------------------------- +* Function : GetPDRVersion +* Description : 获取pdr版本号 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +const char* GetPDRVersion(void); + +/**---------------------------------------------------------------------- +* Function : pdr_closeAlgorithm +* Description : 关闭PDR算法,释放资源 +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +void Algorithm_DeInit(); + + +#ifdef _WIN32 // 主要用于支持win32调试使用，用于dll接口导出 + +/**---------------------------------------------------------------------- +* Function : LibPDR_Init +* Description : 初始化PDR算法(DLL调用) +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +__declspec(dllexport) void LibPDR_Init(); + +/**---------------------------------------------------------------------- +* Function : PDR_StepUpdateGPS +* Description : 仿真器按照GPS单步执行(DLL调用) +* Date : 2022/9/16 logzhan +*---------------------------------------------------------------------**/ +__declspec(dllexport) PosFusion LibPDR_StepUpdateGPS(int useGpsFlg); + +/**---------------------------------------------------------------------- +* Function : setSimulatorFileCsvPath +* Description : 设置仿真文件的路径(DLL调用) +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +__declspec(dllexport) void setSimulatorFileCsvPath(char* path); + +/**---------------------------------------------------------------------- +* Function : LibPDR_DeInit +* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用) +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +__declspec(dllexport) void LibPDR_DeInit(); + +/**---------------------------------------------------------------------- +* Function : setRefGpsYaw +* Description : 设置GPS参考角度 +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +__declspec(dllexport) void setRefGpsYaw(); + +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/1.Software/PDR 1.06/include/pdr_detector.h b/1.Software/PDR 1.06/include/pdr_detector.h new file mode 100644 index 0000000..9346d30 --- /dev/null +++ b/1.Software/PDR 1.06/include/pdr_detector.h @@ -0,0 +1,111 @@ +#ifndef __DETECTOR_H +#define __DETECTOR_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Header File Including ------------------------------------------------------------------------ */ +#include +#include +#include +#include "PDRBase.h" +#include "buffer.h" +#include "AHRS.h" + +/* Macro Declaration ---------------------------------------------------------------------------- */ +#define DETECTOR_NO_ERROR 0 +#define DETECTOR_OUT_OF_MEMORY 1 + +#define MAG_BUF_LEN 256 +#define MAG_LEN 6 + +/* Struct Declaration --------------------------------------------------------------------------- */ + + +typedef void (*DETECTOR_update_callback)(Detector_t *detector); + +/* Global Variable Declaration ------------------------------------------------------------------ */ +extern BUFFER_SHORT g_acc_frq_buf[3]; +extern BUFFER_SHORT g_acc_amp_buf[3]; +extern BUFFER_SHORT g_gyr_frq_buf[3]; +extern BUFFER_SHORT g_gyr_amp_buf[3]; + +/* Function Declaration ------------------------------------------------------------------------- */ + +/**--------------------------------------------------------------------- +* Function : GetDetectorObj +* Description : 获取PDR运动分类器对象 +* Date : 2020/02/16 logzhan +*---------------------------------------------------------------------**/ +Detector_t *GetDetectorObj(void); + +/**--------------------------------------------------------------------- +* Function : Detector_Init +* Description : 初始化运动分类器，识别用户处于哪一种运动模式 +* Date : 2022/09/23 logzhan +*---------------------------------------------------------------------**/ +Detector_t* Detector_Init(void); + +/**--------------------------------------------------------------------- +* Function : DetectorReset +* Description : 重置PDR运动分类器 +* Date : 2022/09/23 logzhan +*---------------------------------------------------------------------**/ +void DetectorReset(void); + +/**--------------------------------------------------------------------- +* Function : DetectMotionType +* Description : pdr运动类型检测 +* Date : 2020/7/20 +*---------------------------------------------------------------------**/ +int DetectMotionType(void); + +/**--------------------------------------------------------------------- +* Function : predict +* Description : 判断手机携带方式（静止、手持、摆手等） +* Date : 2020/7/20 logzhan +*---------------------------------------------------------------------**/ +int pdr_detectorPredict(float* feature); + + +/**---------------------------------------------------------------------- +* Function : DetUserStatic +* Description : 检测用户是否处于静止状态 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps); + +/**---------------------------------------------------------------------- +* Function : detIsCarMode +* Description : 识别是否是车载模式 +* Input : pgnss，GPS数据结构体 + g_pdr，PDR结构体 + delSteps， 与上次相比步数的变化量 + time，计数器 +* Output : int，车载模式标志位 +* Date : 2021/01/28 logzhan +*---------------------------------------------------------------------**/ +int DetectCarMode(GNSS_t* pgnss, PDR_t* g_pdr, unsigned long delSteps, int* time); + +/**---------------------------------------------------------------------- +* Function : detPdrToReset +* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况 +* 重置PDR到GPS的位置。 +* Date : 2021/01/28 logzhan +*---------------------------------------------------------------------**/ +int DetectPdrToReset(double pdr_angle, int* gpscnt, unsigned long deltsteps, PDR_t* g_pdr); + +/**--------------------------------------------------------------------- +* Function : DetectorUpdateIMU +* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测 +* 用户的运动类型 +* Date : 2022/09/23 +*---------------------------------------------------------------------**/ +void DetectorUpdateIMU(IMU_t* imu, PDR_t* pdr); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/1.Software/PDR 1.06/include/pdr_sensor.h b/1.Software/PDR 1.06/include/pdr_sensor.h new file mode 100644 index 0000000..ecb8717 --- /dev/null +++ b/1.Software/PDR 1.06/include/pdr_sensor.h @@ -0,0 +1,314 @@ +/******************** (C) COPYRIGHT 2021 Geek************************************ +* File Name : pdr_sensor.h +* Department : Sensor Algorithm Team +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2021.02.01 +* Comments : PDR算法传感器基本定义 +********************************************************************************/ +#ifndef _SENSOR_H_ +#define _SENSOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* Macro Definition ----------------------------------------------------------*/ +#define IMU_SENSOR_AXIS 3 + + +#ifdef _WIN32 +#define PDR_LOGD printf +#define PDR_LOGE printf +#else +#include +#define LOG_TAG "Loc_PDR" +#define PDR_LOGD(...) __android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__) +#define PDR_LOGE(...) __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__) +#endif + +#define ADD_POINT_NUM 10 + +#define GGA_STR "GGA" +#define RMC_STR "RMC" +#define GSA_STR "GSA" +#define GSV_STR "GSV" + +#define GNGGA_STR "$GNGGA" +#define GPGGA_STR "$GPGGA" +#define GNRMC_STR "$GNRMC" +#define GPRMC_STR "$GPRMC" +#define GNGSA_STR "$GNGSA" +#define GPGSA_STR "$GPGSA" +#define GAGSA_STR "$GAGSA" +#define GLGSA_STR "$GLGSA" +#define GPGSV_STR "$GPGSV" +#define GAGSV_STR "$GAGSV" +#define GLGSV_STR "$GLGSV" + +#define NORTH "N" +#define SOUTH "S" +#define EAST "E" +#define WEST "W" +#define ITEM_INVALID (-1) +#define INVAILD_GPS_YAW (-1) // 无效GPS航向角定义 +/* Struct Definition ---------------------------------------------------------*/ +typedef enum { + SENSOR_MIN, + SENSOR_ACC = 1, + SENSOR_MAG = 2, + SENSOR_GYRO = 4, + SENSOR_LIGHT = 5, + SENSOR_PROXIMITY = 8, + SENSOR_GAME_ROTATION_VECTOR = 15, + SENSOR_LOCATION_STATELLITE = 70, + SENSOR_LOCATION_NETWORK = 71, + SENSOR_LOCATION_FUSION = 72, + SENSOR_LOCATION_NMEA = 73, + SENSOR_LOCATION_NMEA_GNGGA = 74, + SENSOR_LOCATION_NMEA_GPGGA = 75, + SENSOR_LOCATION_NMEA_GNRMC = 76, + SENSOR_LOCATION_NMEA_GPRMC = 77, + SENSOR_LOCATION_NMEA_GNGSA = 78, + SENSOR_LOCATION_NMEA_GPGSA = 79, + SENSOR_LOCATION_NMEA_GPGSV = 80, + SENSOR_LOCATION_NMEA_GLGSV = 81, + SENSOR_LOCATION_NMEA_GAGSV = 82, + SENSOR_LOCATION_NMEA_GLGSA = 83, + SENSOR_LOCATION_NMEA_GAGSA = 84, + SENSOR_MAX, +} sensor; + +typedef enum { + POSITIONING_N = 0, + POSITIONING_Y = 1, +}lct_status; + +typedef enum { + POSITIONING_MODE_A = 0, + POSITIONING_MODE_M = 1, +}lct_mode; + +typedef enum PDR_Mode { + RAW_GPS = 1, + STOP = 2, + WALK_PDR = 3, + WALK_GPS = 4, + CAR = 5 +} PDR_Mode; + +typedef enum DIR_Mode { + GPS_DATASHEET = 0, + IMU_PCA = 1, + DELTA_LOCATION = 2 +} DIR_Mode; + +/*for GSA*/ +typedef enum { + PSTN_GSA_N = 1, + PSTN_GSA_Y_2D = 2, + PSTN_GSA_Y_3D = 3, +}lct_type; + +typedef enum { + LATITUDE_N = 0, + LATITUDE_S = 1, + LONGITUDINAL_E = 2, + LONGITUDINAL_W = 3, +}LL_NSEW; + +typedef enum { + MAG_E = 0, + MAG_W = 1, +}mag_heading; + +typedef enum { + MODE_A = 0, + MODE_D = 1, + MODE_E = 2, + MODE_N = 3, +}gnrmc_mode; + +typedef enum { + invalid = 0, + fix = 1, + dgps_fix = 2, + invalid_pps = 3, + rtk_fix = 4, + rtk_float = 5, + estimating = 6, +}gngga_quality; + +typedef struct Sensor { + unsigned char update; + int type; + double time; + float s[IMU_SENSOR_AXIS]; +}Sensor_t; + +typedef struct imu { + Sensor_t acc; + Sensor_t gyr; + Sensor_t mag; +}IMU_t; + +typedef struct NmeaRMC_t { + unsigned char update; + char rmc_check_sum[9]; + sensor type; + lct_status status; + LL_NSEW rmc_latitude_ns; + LL_NSEW longitudinal_ew; + mag_heading mag_h; + gnrmc_mode mode; + float speed; + float cog; + float utc_ddmmyy; + float magnetic; + double rmc_utc; + double latitude; + double longitudinal; + double time; //ms +}NmeaRMC_t; + +typedef struct NmeaGGA{ + unsigned char update; + char gga_check_sum[9]; + sensor type; + LL_NSEW gga_latitude_ns; + LL_NSEW longitudinal_ew; + gngga_quality status; + int satellites_nb; + int unit_a; + int unit_h; + int dgps; + float hdop; + float altitude; + float height; + double gga_utc; + double latitude; + double longitudinal; + double time; //ms +}NmeaGGA_t; + +typedef struct NmeaGSA { + unsigned char update; + char check_sum[9]; + sensor type; + lct_mode mode; + lct_type p_type; + int prnflg; + int prn[12]; + float pdop; + float hdop; + float vdop; + double time; //ms +}NmeaGSA_t; + +typedef struct satellite_status { + int prn; + int elevation; // elevation >10 + int azimuth; + int snr; //snr >25 good +}satellite_status; + +typedef struct NmeaGSV { + unsigned char update; + sensor type; + int sentences; + int sentence_number; + int satellites; + int satellite_number; + satellite_status satellites_data[20]; + double time; //ms +}NmeaGSV_t; + +typedef struct Accuracy{ + unsigned char update; + int status; //-1,:Status Unknown; 0: Out of Service; 1: Temporarily Unavailable; 2: Available + float err; // m + double time; +}Accuracy_t; + +typedef struct Nmea_t { + unsigned char update; + double minTime; + double maxTime; + NmeaGGA_t gga; + NmeaRMC_t rmc[2]; // 0 - GNRMC, 1 - GPRMC + NmeaGSA_t gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA + NmeaGSV_t gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV + Accuracy_t accuracy; +}Nmea_t; + +typedef struct lct_fs { + LL_NSEW latitude_ns; + LL_NSEW longitudinal_ew; + double latitude; + double longitudinal; + double gpsLat; + double gpsLon; + double gpsHeading; + double pdrHeading; + double hdop; + double gpsSpeed; + double accuracy; + double time; + double yaw; + double lambda; + double last_lat; + double last_lon; + unsigned long step; + uint8_t motionType; +}LctFs_t; + +typedef struct gnss { + LL_NSEW lat_ns; + LL_NSEW lon_ew; + int minElevation; + int satellites_num; // 当前卫星数量 + int quality; + float yaw; // GNSS 航向角 + float vel; // GNSS 速度 + float HDOP; // GNSS 水平精度因子 + float error; // 当前GPS的Accuracy + float lastError; // 上一次GPS的Accuracy + int overVelCount; // 速度过快数量统计, 记录速度过快的数量，用于判断手机是否处于车载模式 + double timestamps; + double lat; + double lon; + double xNed; + double yNed; +}GNSS_t; + +typedef struct IMU { + double time; + float gyr[3]; + float acc[3]; + float mag[3]; +} IMU; + +typedef struct AHRS { + uint8_t status; + uint8_t stable; // 当前AHRS算法收敛稳定性 + float error; + float q[4]; // + float gravity[3]; + float x_axis[3]; + float y_axis[3]; + float z_axis[3]; + float Dt; + float Kp; // mahony kp比例调节参数 + float Yaw; + float Pitch; + float Roll; + float insHeading; +}AHRS_t; + +#ifdef __cplusplus +} +#endif +#endif + diff --git a/1.Software/PDR 1.06/include/scene_recognition.h b/1.Software/PDR 1.06/include/scene_recognition.h new file mode 100644 index 0000000..d0efd89 --- /dev/null +++ b/1.Software/PDR 1.06/include/scene_recognition.h @@ -0,0 +1,158 @@ +/*********************************************************************************/ +/* FILE NAME : scene_recognition.h */ +/* AUTHOR : Zhang Jingrui, ID: 11082826, Version: V0.1_Beta, Data:2019-03-28 */ +/* DESCRIPTION: This header file conterns the prototype description of the external */ +/* interface funtions of the scene recognition module and the definit- */ +/* ion of related resources. */ +/* HISTORY : (NONE) */ +/********************************************************************************/ + +#ifndef _PDR_SCENE_RECOGNITION_H_ +#define _PDR_SCENE_RECOGNITION_H_ + +/* Header Files Including -----------------------------------------*/ +/*=================================================================*/ +#include "pdr_sensor.h" + +/* Macro Declaration ----------------------------------------------*/ +/*=================================================================*/ + +#define _TIME_BUFFER_SIZE 10 /* Buffer size for the time calculation. */ +#define _ACCURACY_BUFFER_SIZE 9 /* Accuracy time-based buffer size. */ +#define _GNSS_INDEX_LEN 103 /* Structural array size in _GnssInfo. */ + +/* Extern Variable Declaration ------------------------------------*/ +/*=================================================================*/ +//extern int scene_recognition_debug_flag ; +//extern int scene_recognition_log_debug_flag ; + +/* Status Declaration ---------------------------------------------*/ +/*=================================================================*/ +/* Usage: 场景识别模块初始化、重置操作结果状态标志位 */ +typedef enum SCENE_INIT_STATE +{ + // 场景识别模块初始化相关状态 + INIT_SUCCESS = 0, + INIT_NOT_PERFORMED, + + INIT_ERROR_MEMORY_OCCUPIED, // 模块资源未提前释放，需先释放资源 + INIT_ERROR_MEMORY_INIT_FAILED, // 初始化内存申请失败 + INIT_ERROR_REDUNDANCY, // 模块重复初始化，此时初始化无效。若需要重启模块，可以： + // 方法1：先关闭模块，关闭成功后，重新初始化模块。 + // 方法2：重启模块，效果同方法1。 + // 场景识别模块重置相关状态(for debug) + INIT_RESET_SUCCESS = 0, + INIT_RESET_FAILED +} SCENE_INIT_STATE; + +/* Usage: 场景分类结果 */ +typedef enum SCENE_RECOGNITION_RESULT +{ + RECOG_UNKNOWN, // 未能确定场景的状态(默认状态) + RECOG_OPEN_AREA, // 场景open-area + RECOG_OTHERS, // 除了上述确定场景外的其他场景 + RECOG_ERROR_PARAM_INCRECT, // 输入参数检查错误 + RECOG_ERROR_NMEA_INFO_MISS, // 输入参数正确但是内容有误 + /* 预研检测结果(不稳定) */ + RECOG_MULTIPATH // 检测多径 +} SCENE_RECOGNITION_RESULT; + +/* Usage: 场景识别模块资源释放操作结果 */ +typedef enum SCENE_DESTROY_STATE +{ + DESTROY_SUCCESS = 0, + DESTROY_FIALED, + DESTROY_ERROR, + DESTROY_INVALID // 无效释放操作，表示没有对应的初始化或重置操作 +} SCENE_DESTROY_STATE; + + +/* Status Declaration--------------------------------------------*/ +/*===============================================================*/ + +// 描述当前场景识别模块的工作状态 +typedef enum _SCENE_MODEL_WORK_STATE +{ + MODEL_OFF, + MODEL_ON +} _SCENE_MODEL_WORK_STATE; + +/* Usage: Identifier of the quality of the satellites' signal source. */ +typedef enum _SIGNAL_QUALITY +{ + SIG_UNKNOWN, + GOOD, + BAD +} SIGNAL_QUALITY; + +/* Usage: extract and save information from Nmea_t for the module. */ +/* This is the interface for the Nmea_t. */ +typedef struct _GnssInfo +{ + int update; // GNSS更新标志位 1：更新 0：不更新 + double local_timestamp; // 当地时间 + float accuracy; // GPS 输出的Accuracy参数，Accuracy > 0 当其为负数表示无效 + int sat_visible_number; // 当前时间可见卫星数量 + float snr_list[_GNSS_INDEX_LEN]; // 当前时间可见卫星的SNR列表 + int sat_used_list[_GNSS_INDEX_LEN]; // 当前使用卫星列表 +} GnssInfo; + +/* Usage: 标记GNSS中的各个卫星系统 */ + +/* Struct Declaration -------------------------------------------*/ +/*===============================================================*/ + + +/* Function Declaration -------------------------------------------*/ +/*=================================================================*/ +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Name: sceneRecognitionInit() */ +/* Usage: Initialization module for configuring related resources */ +/* and must be executed once first beforing using the scene */ +/* recognition module funcition. */ +/* Param @ (NONE) */ +/* Return @ init_res: Result state of the initialization process. */ +SCENE_INIT_STATE SceneRecog_Init(void); + +/* Function Name: scene_recognition_reset() */ +/* Usage: 初始化模块，配置相关资源，在使用模块前必须首先被执行。 */ +/* Param @ (NONE) */ +/* Return @ init_res: 重置执行结果。 */ +SCENE_INIT_STATE scene_recognition_reset(void); + +/**---------------------------------------------------------------------- +* Function : sceneRecognitionProc +* Description : GNSS场景识别处理流程： +* 1）将nmea转换为gnss +* 2) 通过accuracy、snr、卫星数量分析GNSS信号质量 +* 3) 返回结果 +* Input : PDR的nmea结构体 +* Return : 场景识别结果 +* Date : 2020/02/18 +*---------------------------------------------------------------------**/ +SCENE_RECOGNITION_RESULT sceneRecognitionProc(const Nmea_t* nmea); + +/* Function Name: scene_recognition_destroy() */ +/* Usage: 场景识别处理结束后的资源释放操作，资源释放后无法继续进行场 */ +/* 景识别过程。 */ +/* Param @ (NONE) */ +/* Return @ destroy_state: 资源释放操作结果。 */ +SCENE_DESTROY_STATE scene_recognition_destroy(void); + +/**---------------------------------------------------------------------- +* Function : isOpenArea +* Description : GNSS开阔地检测，通过输入GNSS信号，分析当前信号质量，从而确定 +* 目标是否处于开阔场地。 +* Return : 0 : 非开阔地 1：开阔地(信号质量较好) +* Author : Zhang Jingrui, Geek ID: 11082826 +* Date : 2020/02/18 logzhan +*---------------------------------------------------------------------**/ +int isOpenArea(const Nmea_t* nmea); + +#ifdef __cplusplus +} +#endif +#endif //__SCENE_RECOGNITION_H__ \ No newline at end of file diff --git a/1.Software/PDR 1.06/project/PDR.vcxproj b/1.Software/PDR 1.06/project/PDR.vcxproj new file mode 100644 index 0000000..9484f69 --- /dev/null +++ b/1.Software/PDR 1.06/project/PDR.vcxproj @@ -0,0 +1,207 @@ +锘 + + + + Debug + Win32 + + + Debug + x64 + + + Release + Win32 + + + Release + x64 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + {28D32532-309F-40EA-9E4A-2D162CC434D2} + Win32Proj + mp4spliter + 10.0 + PDR + + + + Application + true + v142 + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + true + ..\include\;$(IncludePath) + $(ReferencePath) + + + true + ..\3rdparty\Json\;..\3rdparty\onnx\;..\3rdparty\opencv\;..\include\;$(IncludePath) + ..\3rdparty\Json;$(ReferencePath) + + + false + ..\include\;$(IncludePath) + + + false + ..\include\;.\3rdparty\libjson\;..\3rdparty\libcurl\;..\3rdparty\opencv\;$(IncludePath) + ..\3rdparty\onnx\lib\;..\3rdparty\opencv\lib\;..\3rdparty\libcurl\bin\;..\3rdparty\libjson\bin\;$(LibraryPath) + D:\Program Files\OpenSSL-Win64\include;$(ReferencePath) + + + + NotUsing + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions) + true + true + + + Console + true + %(AdditionalDependencies) + + + + + NotUsing + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions) + true + true + + + Console + true + + + + + Level3 + NotUsing + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + NotUsing + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + %(AdditionalDependencies) + UseLinkTimeCodeGeneration + + + + + + \ No newline at end of file diff --git a/1.Software/PDR 1.06/project/PDR.vcxproj.filters b/1.Software/PDR 1.06/project/PDR.vcxproj.filters new file mode 100644 index 0000000..b705454 --- /dev/null +++ b/1.Software/PDR 1.06/project/PDR.vcxproj.filters @@ -0,0 +1,159 @@ +锘 + + + + {7c7c488d-9b32-4de4-9ec8-b4bd38c3e7b0} + + + {a09dc878-ed5c-47e4-bdcc-c3e2c8f05882} + + + {1d99ec5e-847f-4166-8f79-ddb0473a04dd} + + + {bd2be68d-d6d0-4b04-97d9-2ac99be07afe} + + + {74391251-b6de-45ec-88b1-0ee0171e44da} + + + {7f1fc113-1a0a-488e-9f77-2b12c7280aed} + + + + + src + + + src + + + src + + + src + + + src + + + src + + + src + + + src + + + src\Utils + + + src\Math + + + src\Location + + + src + + + src\Math + + + src\Utils + + + src\Location + + + src\Utils + + + src\Ins + + + src\Ins + + + src\Math + + + src + + + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + inc + + + src\Utils + + + src\Location + + + src\Math + + + src\Utils + + + src\Math + + + src\Location + + + src\Utils + + + src\Ins + + + src\Ins + + + src\Math + + + \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/AHRS.c b/1.Software/PDR 1.06/src/AHRS.c new file mode 100644 index 0000000..8654bc0 --- /dev/null +++ b/1.Software/PDR 1.06/src/AHRS.c @@ -0,0 +1,114 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_ahrs.c +* Department : Sensor Algorithm Team +* Current Version : V1.2 +* Author : logzhan & +* +* +* Date of Issued : 2020.7.3 +* Comments : PDR AHRS 姿态解算相关 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include "PDRBase.h" +#include "AHRS.h" +#include "pdr_detector.h" +#include "Filter.h" +#include "buffer.h" +#include "Kalman.h" +#include "Utils.h" +#include "Quaternion.h" + +/* Macro Definition ----------------------------------------------------------*/ +#define FLT_THRES 0.000001f // 浮点数最小阈值 +#define ENABLE_MAG 0 +/* Global Variable Definition ------------------------------------------------*/ +AHRS_t Ahrs; + +/**---------------------------------------------------------------------- +* Function : MahonyUpdateAHRS +* Description : Mahony姿态更新算法 +* Date : 2020/02/18 logzhan : +* 增加了AHRS稳定性标志位，如果AHRS稳定后不再计算error, 降低运算 + 量。能提升PDR仿真10%的运行速度 +*---------------------------------------------------------------------**/ +static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az, + float mx, float my, float mz) { + + float* q = Ahrs.q; + // 归一化磁力计和加速度计 + Vec3Norm(&ax, &ay, &az); + + float wx = 0.0f, wy = 0.0f, wz = 0.0f; + +#if ENABLE_MAG + if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { + + pdr_v3Norm(&mx, &my, &mz); + + // 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz) + float hx = 2.0f * (mx * (0.5f - q2 * q2 - q[3] * q[3]) + my * (q[1] * q2 - q[0] * q[3]) + mz * (q[1] * q[3] + q[0] * q2)); + float hy = 2.0f * (mx * (q[1] * q2 + q[0] * q[3]) + my * (0.5f - q[1] * q[1] - q[3] * q[3]) + mz * (q2 * q[3] - q[0] * q[1])); + float hz = 2.0f * (mx * (q[1] * q[3] - q[0] * q2) + my * (q2 * q[3] + q[0] * q[1]) + mz * (0.5f - q[1] * q[1] - q2 * q2)); + + // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系 + float by = (float)sqrt(hx * hx + hy * hy); + float bz = hz; + + wx = by * (q[0] * q[3] + q[1] * q2) + bz * (q[1] * q[3] - q[0] * q2); + wy = by * (0.5f - q[1] * q[1] - q[3] * q[3]) + bz * (q[0] * q[1] + q2 * q[3]); + wz = by * (q2 * q[3] - q[0] * q[1]) + bz * (0.5f - q[1] * q[1] - q2 * q2); + } +#endif + // 把重力转换到地理坐标系 v = R * (0,0,1) + float vx = q[1]*q[3] - q[0]*q[2]; + float vy = q[0]*q[1] + q[2]*q[3]; + float vz = q[0]*q[0] - 0.5f + q[3]*q[3]; + + // 计算矢量叉乘误差 + float ex = ay*vz - az*vy + my*wz - mz*wy; + float ey = az*vx - ax*vz + mz*wx - mx*wz; + float ez = ax*vy - ay*vx + mx*wy - my*wx; + + // Apply proportional feedback + gx += Ahrs.Kp * ex; + gy += Ahrs.Kp * ey; + gz += Ahrs.Kp * ez; + + // 龙格库塔法更新四元数 + float qa = q[0]; float qb = q[1]; float qc = q[2]; + q[0] += (-qb * gx - qc * gy - q[3] * gz) * 0.5f * (Ahrs.Dt); + q[1] += ( qa * gx + qc * gz - q[3] * gy) * 0.5f * (Ahrs.Dt); + q[2] += ( qa * gy - qb * gz + q[3] * gx) * 0.5f * (Ahrs.Dt); + q[3] += ( qa * gz + qb * gy - qc * gx) * 0.5f * (Ahrs.Dt); + + // 四元数归一化 + QuaternionNorm(&q[0], &q[1], &q[2], &q[3]); + + // 四元数转欧拉角 + Ahrs.Pitch = asinf(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f); + Ahrs.Yaw = atan2f(q[2]*q[1] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * (180.0f / 3.141592f); + Ahrs.Roll = atan2f(q[2]*q[3] + q[0]*q[1], 0.5f - q[2]*q[2] - q[1]*q[1]) * (180.0f / 3.141592f); +} + +void AHRS_Init(void) { + memset(&Ahrs, 0, sizeof(Ahrs)); + Ahrs.Kp = AHRS_KP; + Ahrs.Dt = 1.0f / AHRS_SAMPLE_FREQ; + Ahrs.q[0] = 1.0f; +} + +/**---------------------------------------------------------------------- +* Function : UpdateAHRS +* Description : AHRS融合解算 +* Date : 2022/10/18 logzhan +*---------------------------------------------------------------------**/ +int UpdateAHRS(IMU_t* imu) { + + // logzhan 21/02/06 : 感觉是acc输入精度会高一些 + MahonyUpdateAHRS(imu->gyr.s[0], imu->gyr.s[1], imu->gyr.s[2], + imu->acc.s[0], imu->acc.s[1], imu->acc.s[2], + imu->mag.s[0], imu->mag.s[1], imu->mag.s[2]); + return PDR_TRUE; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Buffer.c b/1.Software/PDR 1.06/src/Buffer.c new file mode 100644 index 0000000..ea53172 --- /dev/null +++ b/1.Software/PDR 1.06/src/Buffer.c @@ -0,0 +1,201 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Buffer.c +* Current Version : V1.2 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取 +* 和计算 +********************************************************************************/ +/* Header File Including ------------------------------------------------------*/ +#include +#include +#include "buffer.h" + +/* Macro Declaration ----------------------------------------------------------*/ +#define ADD_PTR(ptr, ofs) ((ptr + ofs) % (buffer->length + 1)) +#define INC_PTR(ptr) ((ptr + 1) % (buffer->length + 1)) +#define DEC_PTR(ptr) ((ptr + buffer->length) % (buffer->length + 1)) + +/* Function Definition --------------------------------------------------------*/ + +int BufferInit(Buffer_t *buf, const char *name, int type, int len) { + int i; + if (NULL == buf) { + return BUFFER_WRONG_PARAMETER; + } + + memset(buf->data, 0, buf->length * sizeof(*(buf->data))); + for (i = 0; i < 20 - 1 && '\0' != name[i]; ++i) { + buf->name[i] = name[i]; + } + buf->name[i] = '\0'; + buf->type = type; + buf->length = len; + buf->_bottom = 0; + buf->_top = 0; + buf->mean = 0.0; + buf->sum = 0.0; + return BUFFER_NO_ERROR; +} + +int BufferClear(Buffer_t *buffer) { + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + memset(buffer->data, 0, buffer->length * sizeof(*(buffer->data))); + buffer->_bottom = 0; + buffer->_top = 0; + buffer->mean = 0.0; + buffer->sum = 0.0; + return BUFFER_NO_ERROR; +} + +int BufferCount(Buffer_t *buffer, int *count) { + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + *count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1); + return BUFFER_NO_ERROR; +} + +int BufferGetTop(Buffer_t *buffer, float *value) { + *value = 0; + + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + if (buffer->_top == buffer->_bottom) { + return BUFFER_OUT_OF_MEMORY; + } + + *value = buffer->data[DEC_PTR(buffer->_top)]; + + return BUFFER_NO_ERROR; +} + +int BufferGetBottom(Buffer_t *buffer, float *value) { + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + if (buffer->_top == buffer->_bottom) { + return BUFFER_OUT_OF_MEMORY; + } + + *value = buffer->data[buffer->_bottom]; + + return BUFFER_NO_ERROR; +} + +int BufferPop(Buffer_t *buffer, float *value) { + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + if (buffer->_top == buffer->_bottom) { + return BUFFER_OUT_OF_MEMORY; + } + + switch (buffer->type) { + case BUFFER_TYPE_STACK: + buffer->_top = DEC_PTR(buffer->_top); + *value = buffer->data[buffer->_top]; + break; + case BUFFER_TYPE_QUEUE: + default: + *value = buffer->data[buffer->_bottom]; + buffer->_bottom = INC_PTR(buffer->_bottom); + break; + } + + return BUFFER_NO_ERROR; +} + +int BufferPush(Buffer_t *buffer, float value) { + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; /* BUFFER_WRONG_PARAMETER值为1 */ + } + + buffer->data[buffer->_top] = value; + buffer->_top = INC_PTR(buffer->_top); /* INC_PTR为加1（超过buffer长度就循环加1）*/ + + if (buffer->_top == buffer->_bottom) { + buffer->_bottom = INC_PTR(buffer->_bottom); + return BUFFER_OUT_OF_MEMORY; /* BUFFER_OUT_OF_MEMORY值为2 */ + } + else { + return BUFFER_NO_ERROR; /* BUFFER_NO_ERROR值为0 */ + } +} + +int BufferGet(Buffer_t *buffer, float *value, int index) { + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + if (index >= buffer->length) { + return BUFFER_OUT_OF_MEMORY; + } + + *value = buffer->data[ADD_PTR(buffer->_bottom, index)]; + + return BUFFER_NO_ERROR; +} + +int BufferMean(Buffer_t *buffer, float *mean) { + int index; + int count; + + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + *mean = 0; + count = 0; + for (index = buffer->_bottom; index != buffer->_top; index = INC_PTR(index)) { + *mean += buffer->data[index]; + ++count; + } + if (0 != count) { + *mean /= count; + } + return BUFFER_NO_ERROR; +} + +int BufferVar(Buffer_t *buffer, float *var) { + int index; + int count; + float mean; + + if (NULL == buffer) { + return BUFFER_WRONG_PARAMETER; + } + + mean = 0; + count = 0; + for (index = buffer->_bottom; index != buffer->_top; index = INC_PTR(index)) { + mean += buffer->data[index]; + ++count; + } + if (0 != count) { + mean /= count; + } + + *var = 0; + for (index = buffer->_bottom; index != buffer->_top; index = INC_PTR(index)) { + *var += (buffer->data[index] - mean) * (buffer->data[index] - mean); + } + if (0 != count) { + *var /= count; + } + + return BUFFER_NO_ERROR; +} + +int BufferStd(Buffer_t *buffer, float *std) { + int ret = BufferVar(buffer, std); + *std = sqrtf(*std); + return ret; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Complex.cpp b/1.Software/PDR 1.06/src/Complex.cpp new file mode 100644 index 0000000..1061770 --- /dev/null +++ b/1.Software/PDR 1.06/src/Complex.cpp @@ -0,0 +1,74 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Complex.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR复数运算库支持 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include "math.h" +#include "Complex.h" + +/**--------------------------------------------------------------------- +* Function : ComplexAbs +* Description : 复数球绝对值运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +float ComplexAbs(Complex_t c) { + return sqrtf(c.real * c.real + c.image * c.image); +} + +/**--------------------------------------------------------------------- +* Function : ComplexAdd +* Description : 复数加法运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexAdd(Complex_t a, Complex_t b) { + Complex_t c = { 0 }; + c.real = a.real + b.real; + c.image = a.image + b.image; + return c; +} + +/**--------------------------------------------------------------------- +* Function : ComplexSub +* Description : 复数减法运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexSub(Complex_t a, Complex_t b) { + Complex_t c = { 0 }; + + c.real = a.real - b.real; + c.image = a.image - b.image; + + return c; +} + +/**--------------------------------------------------------------------- +* Function : ComplexMul +* Description : 复数乘法运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexMul(Complex_t a, Complex_t b) { + Complex_t c; + c.real = a.real * b.real - a.image * b.image; + c.image = a.real * b.image + a.image * b.real; + return c; +} + +/**--------------------------------------------------------------------- +* Function : ComplexPow +* Description : 复数指数运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexPow(Complex_t c, int n) { + Complex_t p = { 1, 0 }; + int i; + float real; + for (i = 0; i < n; ++i) { + real = p.real * c.real - p.image * c.image; + p.image = p.real * c.image + p.image * c.real; + p.real = real; + } + return p; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Complex.h b/1.Software/PDR 1.06/src/Complex.h new file mode 100644 index 0000000..1479df4 --- /dev/null +++ b/1.Software/PDR 1.06/src/Complex.h @@ -0,0 +1,24 @@ +#ifndef _PDR_COMPLEX_H_ +#define _PDR_COMPLEX_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct Complex { + float real; + float image; +} Complex_t; + + +float ComplexAbs(Complex_t c); +Complex_t ComplexAdd(Complex_t a, Complex_t b); +Complex_t ComplexSub(Complex_t a, Complex_t b); +Complex_t ComplexMul(Complex_t a, Complex_t b); +Complex_t ComplexPow(Complex_t c, int n); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/CoordTrans.cpp b/1.Software/PDR 1.06/src/CoordTrans.cpp new file mode 100644 index 0000000..789c3a1 --- /dev/null +++ b/1.Software/PDR 1.06/src/CoordTrans.cpp @@ -0,0 +1,74 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : CoordTrans.cpp +* Current Version : V1.0 +* Author : logzhan +* Date of Issued : 2022.09.20 +* Comments : PDR 坐标系转换相关函数支持 +********************************************************************************/ +#include +#include "CoordTrans.h" + +void WGS842ECEF(double* plla, double* ecef) { + double f = 1 / 298.257223563; + double a = 6378136.49; + double b, e, roc; + + b = a * (1 - f); + e = sqrt(pow(a, 2) - pow(b, 2)) / a; + roc = a / sqrt(1 - pow(e * sin(plla[0]), 2)); + ecef[0] = (roc + plla[2]) * cos(plla[0]) * cos(plla[1]); + ecef[1] = (roc + plla[2]) * cos(plla[0]) * sin(plla[1]); + ecef[2] = (roc * (1 - e * e) + plla[2]) * sin(plla[0]); + +} + +void ECEF2WGS84(double* ecef, double* plla) +{ + double a = 6378137; + double b = 6356752.314245179; + double e, ep, p, th, roc; + + e = sqrt(pow(a, 2) - pow(b, 2)) / a; + b = sqrt(pow(a, 2) * (1 - pow(e, 2))); + ep = sqrt((pow(a, 2) - pow(b, 2)) / pow(b, 2)); + p = sqrt(ecef[0] * ecef[0] + ecef[1] * ecef[1]); + th = atan2(a * ecef[2], b * p); + + plla[1] = atan2(ecef[1], ecef[0]); + plla[0] = atan2((ecef[2] + pow(ep, 2) * b * pow(sin(th), 3)), (p - pow(e, 2) * a * pow(cos(th), 3))); + roc = a / sqrt(1 - pow(e * sin(plla[0]), 2)); + plla[2] = p / cos(plla[0]) - roc; + +} + +void ECEF2NED(double* ecef, double* plla, double* ned) +{ + ned[0] = -sin(plla[0]) * cos(plla[1]) * ecef[0] - sin(plla[0]) * sin(plla[1]) * ecef[1] + cos(plla[0]) * ecef[2]; + ned[1] = -sin(plla[1]) * ecef[0] + cos(plla[1]) * ecef[1]; + ned[2] = -cos(plla[0]) * cos(plla[1]) * ecef[0] - cos(plla[0]) * sin(plla[1]) * ecef[1] - sin(plla[0]) * ecef[2]; +} + +void NED2ECEF(double* plla, double* ned, double* ecef0, double* ecef) +{ + ecef[0] = -sin(plla[0]) * cos(plla[1]) * ned[0] - sin(plla[1]) * ned[1] - cos(plla[0]) * cos(plla[1]) * ned[2] + ecef0[0]; + ecef[1] = -sin(plla[0]) * sin(plla[1]) * ned[0] + cos(plla[1]) * ned[1] - cos(plla[0]) * sin(plla[1]) * ned[2] + ecef0[1]; + ecef[2] = cos(plla[0]) * ned[0] - sin(plla[0]) * ned[2] + ecef0[2];; +} + +void WGS842NED(double* plla, double* ref_lla, double* ned) +{ + double ecef[3] = { 0 }; + + WGS842ECEF(plla, ecef); + ECEF2NED(ecef, ref_lla, ned); +} + +void NED2WGS84(double* ref_plla, double* ned, double* plla) +{ + double ecef[3] = { 0 }; + double ref_ecef[3] = { 0 }; + WGS842ECEF(ref_plla, ref_ecef); + + NED2ECEF(ref_plla, ned, ref_ecef, ecef); + ECEF2WGS84(ecef, plla); +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/CoordTrans.h b/1.Software/PDR 1.06/src/CoordTrans.h new file mode 100644 index 0000000..1720b35 --- /dev/null +++ b/1.Software/PDR 1.06/src/CoordTrans.h @@ -0,0 +1,24 @@ +#ifndef _PDR_COORD_SUPPORT_H +#define _PDR_COORD_SUPPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +void WGS842ECEF(double* plla, double* ecef); + +void ECEF2WGS84(double* ecef, double* plla); + +void ECEF2NED(double* ecef, double* plla, double* ned); + +void NED2ECEF(double* plla, double* ned, double* ecef0, double* ecef); + +void WGS842NED(double* plla, double* ref_lla, double* ned); + +void NED2WGS84(double* ref_plla, double* ned, double* plla); + +#ifdef __cplusplus +} +#endif + +#endif // _PDR_KML_SUPPORT_H \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Detector.c b/1.Software/PDR 1.06/src/Detector.c new file mode 100644 index 0000000..c5f3aab --- /dev/null +++ b/1.Software/PDR 1.06/src/Detector.c @@ -0,0 +1,726 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_detector.c +* Department : Sensor Algorithm Team +* Current Version : V1.2 +* Author : ZhangJingrui@vivo.com +* Date of Issued : 2020.7.4 +* Comments : PDR检测器，主要用于区分用户的运动类型，用于PDR算法进行下一步的 +* 决策。 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include +#include "PDRBase.h" +#include "pdr_detector.h" +#include "Filter.h" +#include "buffer.h" +#include "Fft.h" +#include "m_munkres.h" + + +/* Macro Definition ----------------------------------------------------------*/ +#define FLT_BUF_LEN 256 +#define FRQ_BUF_LEN 10 +#define TMP_BUF_LEN 5 +#define FRQ_RAT 0.3f +#define ACCE 0 +#define GYRO 1 +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 +#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr)) +#define GET_MOL(x) (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2])) +#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1)) +#define DEC_PTR(buf, ptr) (((ptr) + (buf)->length) % ((buf)->length + 1)) +#define COST(i, j) cost[(i) + (j) * number] + + +/* Global Variable Definition ------------------------------------------------*/ +static Detector_t pDetector; +static BUFFER_LONG g_mol_buf[2]; +static Filter_t g_mol_flt[2]; + +const double g_mol_flt_b[] = { 4.96135120696701e-05, 0.000496135120696701, 0.00223260804313515, 0.00595362144836041, + 0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515, + 0.000496135120696701, 4.96135120696701e-05 }; +const double g_mol_flt_a[] = { 1, -3.98765436730188, 8.09440659266207, -10.4762753571126, 9.42333716215302, -6.08421408355183, + 2.83526165426630, -0.936403462626094, 0.208912324670401, -0.0283358586690797, 0.00176963186900233 }; +const double g_mol_flt_h0 = 1; + BUFFER_SHORT g_acc_frq_buf[3]; + BUFFER_SHORT g_acc_amp_buf[3]; +static BUFFER_SHORT g_acc_tmp_buf[3]; + BUFFER_SHORT g_gyr_frq_buf[3]; + BUFFER_SHORT g_gyr_amp_buf[3]; +static BUFFER_SHORT g_gyr_tmp_buf[3]; + + +static int g_label[5] = { 0 }; + +/* Function Definition -------------------------------------------------------------------------- */ + +/**--------------------------------------------------------------------- +* Function : GetDetectorObj +* Description : 获取PDR运动分类器对象 +* Date : 2020/02/16 logzhan +*---------------------------------------------------------------------**/ +Detector_t* GetDetectorObj(void) { + return &pDetector; +} + +/**--------------------------------------------------------------------- +* Function : Detector_Init +* Description : 初始化运动分类器，识别用户处于哪一种运动模式 +* Date : 2020/02/16 logzhan & logzhan +*---------------------------------------------------------------------**/ +Detector_t* Detector_Init(void) { + DetectorReset(); + return &pDetector; +} + +/**--------------------------------------------------------------------- +* Function : DetectorReset +* Description : 重置PDR运动分类器 +* Date : 2022/09/23 logzhan +*---------------------------------------------------------------------**/ +void DetectorReset(void) { + + pDetector.tick = 0; + pDetector.lastType = DETECTOR_TYPE_STATIC; + + BufferInit((Buffer_t *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN); + BufferInit((Buffer_t *)&g_mol_buf[GYRO], "gyro_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN); + + BufferInit((Buffer_t *)&g_acc_frq_buf[0], "acc_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_frq_buf[1], "acc_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_frq_buf[2], "acc_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + + BufferInit((Buffer_t *)&g_acc_amp_buf[0], "acc_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_amp_buf[1], "acc_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_amp_buf[2], "acc_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + + BufferInit((Buffer_t *)&g_acc_tmp_buf[0], "acc_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_tmp_buf[1], "acc_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_tmp_buf[2], "acc_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + + BufferInit((Buffer_t *)&g_gyr_frq_buf[0], "gyr_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_frq_buf[1], "gyr_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_frq_buf[2], "gyr_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + + BufferInit((Buffer_t *)&g_gyr_amp_buf[0], "gyr_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_amp_buf[1], "gyr_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_amp_buf[2], "gyr_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + + BufferInit((Buffer_t *)&g_gyr_tmp_buf[0], "gyr_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_tmp_buf[1], "gyr_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_tmp_buf[2], "gyr_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + + FilterSetCoef(&g_mol_flt[ACCE], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0); + FilterSetCoef(&g_mol_flt[GYRO], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0); + + + memset((void *)g_label, 0, sizeof(g_label)); +} + + +/**--------------------------------------------------------------------- +* Function : DetectorUpdateIMU +* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测 +* 用户的运动类型 +* Date : 2022/09/23 +*---------------------------------------------------------------------**/ +void DetectorUpdateIMU(IMU_t* imu, PDR_t* pdr) { + + BufferPush((Buffer_t *)&g_mol_buf[ACCE], (float)FilterFilter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s))); + BufferPush((Buffer_t *)&g_mol_buf[GYRO], (float)FilterFilter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s))); + + // g_tick_d 实际上是1.28s检测一次，不然检测的频率过高 + if ((pDetector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) { + // 检测用户的运动类型 + int detectResult = DetectMotionType(); + + if (detectResult == DETECTOR_NO_ERROR && pDetector.lastType != pDetector.type) { + pdr->MotionType = pDetector.type; + pDetector.lastType = pDetector.type; + } + } + ++pDetector.tick; +} + +/**--------------------------------------------------------------------- +* Function : fft_buffer +* Description : 对acc数据做傅里叶变换，频率信息保存在fft中 +* Date : 2020/7/20 logzhan +*---------------------------------------------------------------------**/ +static int fft_buffer(Complex_t *fft, Buffer_t *buffer) { + static float s_signal[1024] = { 0 }; + + float mean; + int index; + int i; + + BufferMean(buffer, &mean); + for (i = 0, index = buffer->_bottom; index != buffer->_top; ++i, index = INC_PTR(buffer, index)) { + s_signal[i] = buffer->data[index] - mean; /*时序上255比0新*/ + } + + //序号i之后的长度里元素全部置为0 + memset(s_signal + i, 0, (ARR_LEN(s_signal) - i) * sizeof(*s_signal)); + + if (0 == FFT_Fft(fft, s_signal, ARR_LEN(s_signal))) { + return DETECTOR_OUT_OF_MEMORY; + } + return DETECTOR_NO_ERROR; +} + +/**---------------------------------------------------------------------- +* Function : find_peaks +* Description : 获取前number个较大频点的频率frequency和幅度amplitude +* Date : 2020/7/20 logzhan +*---------------------------------------------------------------------**/ +static int find_peaks(float *frequency, float *amplitude, int number, Complex_t *fft, + int length, float cut_off_frequency, float min_threshold) { + float amp_2[2]; + int min_ind; + int dir; + int n; + int i; + int j; + + min_ind = 0; + memset(frequency, 0, number * sizeof(*frequency)); + memset(amplitude, 0, number * sizeof(*amplitude)); + + dir = 1; + n = (int)(cut_off_frequency * length / IMU_SAMPLING_FREQUENCY); + amp_2[1] = fft[0].real * fft[0].real + fft[0].image * fft[0].image; + for (i = 1; i < n; ++i) { + amp_2[0] = amp_2[1]; + amp_2[1] = fft[i].real * fft[i].real + fft[i].image * fft[i].image; + if (1 == dir) { + if (amp_2[1] < amp_2[0]) { + if (amp_2[0] > amplitude[min_ind]) { + amplitude[min_ind] = amp_2[0]; + frequency[min_ind] = (float)i; + min_ind = 0; + for (j = 1; j < number; ++j) { + if (amplitude[min_ind] > amplitude[j]) { + min_ind = j; + } + } + } + dir = -1; + } + } + else { + if (amp_2[1] > amp_2[0]) { + dir = 1; + } + } + } + + for (i = 0; i < number; ++i) { + amplitude[i] = (float)sqrt(amplitude[i]); + if (amplitude[i] < min_threshold) { + frequency[i] = 0; + amplitude[i] = 0; + } + else { + frequency[i] *= (float)IMU_SAMPLING_FREQUENCY / length; + } + } + + return DETECTOR_NO_ERROR; +} + + +static void get_nonzero_weighted_mean(float *mean, BUFFER_SHORT *buffer, int length) { + int weight_sum; + int weight; + int index; + + if (buffer->_bottom == buffer->_top) { + *mean = 0; + return; + } + + *mean = 0; + weight_sum = 0; + index = buffer->_top; + for (weight = length; weight > 0 && index != buffer->_bottom; --weight) { + index = DEC_PTR(buffer, index); + if (0 != buffer->data[index]) { + *mean += weight * buffer->data[index]; + weight_sum += weight; + } + } + if (0 != weight_sum) { + *mean /= weight_sum; + } +} + +static void count_zeros(int *count, BUFFER_SHORT *buffer, int length) { + int index; + + *count = 0; + for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) { + if (0 == buffer->data[index]) { + ++*count; + } + } +} + +static void copy_buffer(BUFFER_SHORT *dst, BUFFER_SHORT *src, int length) { + int dst_cnt; + int src_cnt; + int dst_idx; + int src_idx; + + BufferCount((Buffer_t *)dst, &dst_cnt); + BufferCount((Buffer_t *)src, &src_cnt); + length = length < dst_cnt ? length : dst_cnt; + length = length < src_cnt ? length : src_cnt; + + dst_idx = (dst->_top + dst->length + 1 - length) % (dst->length + 1); + src_idx = (src->_top + src->length + 1 - length) % (src->length + 1); + while (dst_idx != dst->_top) { + dst->data[dst_idx] = src->data[src_idx]; + dst_idx = INC_PTR(dst, dst_idx); + src_idx = INC_PTR(src, src_idx); + } +} + +static void clear_buffer(BUFFER_SHORT *buffer) { + int index; + + index = buffer->_bottom; + while (index != buffer->_top) { + buffer->data[index] = 0; + index = INC_PTR(buffer, index); + } +} + +static int track_frequency(BUFFER_SHORT *frequency_buffer, BUFFER_SHORT *amplitude_buffer, BUFFER_SHORT *temp_buffer, + float *frequency, float *amplitude, int number) { + static float mean[3]; + static float cost[3 * 3]; + static int assignment[3]; + float distance; + float prev; + int i; + int j; + int transform_flag; + int count; + + /* 0.detect argument */ + if (number > ARR_LEN(mean)) { + return DETECTOR_OUT_OF_MEMORY; + } + + /* 1.caculate main mean */ + for (i = 0; i < number; ++i) { + get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN); + } + + /* 2.caculate main cost */ + for (i = 0; i < number; ++i) { + for (j = 0; j < number; ++j) { + distance = (float)fabs(mean[i] - frequency[j]); + + if (0 == mean[i] || 0 == frequency[j] || distance > FRQ_RAT) { + COST(i, j) = 1e3; + } + else { + COST(i, j) = distance; + } + } + } + + /* 3.get main assignment */ + MUNKRES_get_assignment(assignment, cost, number, number); + + /* 4.push main frequency and main amplitude buffer */ + for (i = 0; i < number; ++i) { + if (1e3 == COST(i, assignment[i])) { + BufferPush((Buffer_t *)&frequency_buffer[i], 0); + BufferPush((Buffer_t *)&litude_buffer[i], 0); + } + else { + BufferPush((Buffer_t *)&frequency_buffer[i], frequency[assignment[i]]); + BufferPush((Buffer_t *)&litude_buffer[i], amplitude[assignment[i]]); + frequency[assignment[i]] = 0; + amplitude[assignment[i]] = 0; + } + } + + /* 5.caculate temp mean */ + for (i = 0; i < number; ++i) { + get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN); + } + + /* 6.caculate temp cost */ + for (i = 0; i < number; ++i) { + for (j = 0; j < number; ++j) { + if (0 == frequency[j]) { + COST(i, j) = 1e3; + } + else if (0 == mean[i]) { + COST(i, j) = FRQ_RAT; + } + else { + COST(i, j) = (float)fabs(mean[i] - frequency[j]); + } + } + } + + /* 7.get main assignment */ + MUNKRES_get_assignment(assignment, cost, number, number); + + /* 8.push temp buffer */ + for (i = 0; i < number; ++i) { + BufferPush((Buffer_t *)&temp_buffer[i], frequency[assignment[i]]); + } + + /* 9.caculate temp mean */ + for (i = 0; i < number; ++i) { + get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN); + } + + /* 10.check temp buffer */ + transform_flag = 0; + for (i = 0; i < number; ++i) { + + prev = temp_buffer[i].data[temp_buffer[i]._bottom]; + for (j = temp_buffer[i]._bottom; j != temp_buffer[i]._top; j = INC_PTR(&temp_buffer[i], j)) { + if (0 == temp_buffer[i].data[j] || fabs(temp_buffer[i].data[j] - prev) > FRQ_RAT) { + break; + } + prev = temp_buffer[i].data[j]; + } + if (j == temp_buffer[i]._top) { + frequency[i] = mean[i]; + transform_flag = 1; + } + else { + frequency[i] = 0; + } + } + + if (transform_flag) { + /* 11.caculate main mean */ + for (i = 0; i < number; ++i) { + get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN); + } + + /* 12.caculate main cost */ + for (i = 0; i < number; ++i) { + + count_zeros(&count, &frequency_buffer[i], TMP_BUF_LEN); + for (j = 0; j < number; ++j) { + if (0 == frequency[j]) { + COST(i, j) = 1e3; + } + else if (0 == mean[i]) { + COST(i, j) = -5; + } + else if (count >= 3) { + // COST(i, j) = 3 - count; + COST(i, j) = (float)(-5 + fabs(mean[i] - frequency[j])); + } + else { + COST(i, j) = (float)fabs(mean[i] - frequency[j]); + } + } + } + + /* 13.get main assignment */ + MUNKRES_get_assignment(assignment, cost, number, number); + + /* 14.push main frequency and main amplitude buffer */ + for (i = 0; i < number; ++i) { + if (0 != frequency[assignment[i]]) { + // Buffer_push(&frequency_buffer[i], frequency[assignment[i]]); + // Buffer_push(&litude_buffer[i], 0); + copy_buffer(&frequency_buffer[i], &temp_buffer[assignment[i]], TMP_BUF_LEN); + clear_buffer(&temp_buffer[assignment[i]]); + } + } + } + + return DETECTOR_NO_ERROR; +} + +static void get_nonzero_mean(float *mean, BUFFER_SHORT *buffer, int length) { + int count; + int index; + + count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1); + length = count < length ? count : length; + *mean = 0; + count = 0; + for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) { + if (0 != buffer->data[index]) { + *mean += buffer->data[index]; + ++count; + } + } + if (count >= 0.5 * length) { + *mean /= count; + } +} + +static void get_nonzero_std(float *std, BUFFER_SHORT *buffer, int length) { + float mean; + float delta; + int count; + int index; + + count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1); + length = count < length ? count : length; + mean = 0; + count = 0; + for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) { + if (0 != buffer->data[index]) { + mean += buffer->data[index]; + ++count; + } + } + if (0 != count) { + mean /= count; + } + + *std = 0; + for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) { + if (0 != buffer->data[index]) { + delta = buffer->data[index] - mean; + *std += delta * delta; + } + } + if (count >= 0.5 * length) { + *std = (float)sqrt(*std / count); + } + else { + *std = -1; // default; + } +} + +/**--------------------------------------------------------------------- +* Function : get_feature +* Description : 获取用于判断手机携带方式（静止、手持、摆手等）的特征 +* Date : 2020/7/20 logzhan +*---------------------------------------------------------------------**/ +static void get_feature(float *feature, int length) { + int i; + int j; + float temp; + + BufferMean((Buffer_t *)&g_mol_buf[ACCE], &feature[0]); + BufferStd( (Buffer_t *)&g_mol_buf[ACCE], &feature[1]); + BufferStd( (Buffer_t *)&g_mol_buf[GYRO], &feature[2]); + get_nonzero_std( &feature[3] , &g_acc_frq_buf[0], length); if (feature[3] < 0) feature[3] = 10; + get_nonzero_std( &feature[4] , &g_acc_frq_buf[1], length); if (feature[4] < 0) feature[4] = 10; + get_nonzero_std( &feature[5] , &g_acc_frq_buf[2], length); if (feature[5] < 0) feature[5] = 10; + get_nonzero_mean(&feature[6] , &g_acc_amp_buf[0], length); + get_nonzero_mean(&feature[7] , &g_acc_amp_buf[1], length); + get_nonzero_mean(&feature[8] , &g_acc_amp_buf[2], length); + get_nonzero_std( &feature[9] , &g_gyr_frq_buf[0], length); if (feature[9] < 0) feature[9] = 10; + get_nonzero_std( &feature[10], &g_gyr_frq_buf[1], length); if (feature[10] < 0) feature[10] = 10; + get_nonzero_std( &feature[11], &g_gyr_frq_buf[2], length); if (feature[11] < 0) feature[11] = 10; + get_nonzero_mean(&feature[12], &g_gyr_amp_buf[0], length); + get_nonzero_mean(&feature[13], &g_gyr_amp_buf[1], length); + get_nonzero_mean(&feature[14], &g_gyr_amp_buf[2], length); + + /* sort by std - bubble sort */ + for (i = 0; i < 2; ++i) { + for (j = 0; j < 2 - i; ++j) { + if (feature[3 + j] > feature[3 + j + 1]) { + + temp = feature[3 + j]; + feature[3 + j] = feature[3 + j + 1]; + feature[3 + j + 1] = temp; + temp = feature[6 + j]; + feature[6 + j] = feature[6 + j + 1]; + feature[6 + j + 1] = temp; + } + } + } + for (i = 0; i < 2; ++i) { + for (j = 0; j < 2 - i; ++j) { + if (feature[9 + j] > feature[9 + j + 1]) { + + temp = feature[9 + j]; + feature[9 + j] = feature[9 + j + 1]; + feature[9 + j + 1] = temp; + temp = feature[12 + j]; + feature[12 + j] = feature[12 + j + 1]; + feature[12 + j + 1] = temp; + } + } + } +} + +/**--------------------------------------------------------------------- +* Function : predict +* Description : 判断手机携带方式（静止、手持、摆手等） +* Date : 2020/7/20 logzhan +*---------------------------------------------------------------------**/ +int pdr_detectorPredict(float *feature) +{ + if (feature[1] < 0.932921) { + return DETECTOR_TYPE_STATIC; + }else if ((feature[3] < 0.10) + (feature[4] < 0.10) + (feature[5] < 0.10) > 0) { + if (feature[12] < 49.9795) { + if (feature[0] < 10.7220) { + return DETECTOR_TYPE_HANDHELD; + } + else { + return DETECTOR_TYPE_SWINGING; + } + } + else { + if (feature[2] < 0.312002) { + return DETECTOR_TYPE_HANDHELD; + } + else { + return DETECTOR_TYPE_SWINGING; + } + } + } + return DETECTOR_TYPE_IRREGULAR; +} + +/**--------------------------------------------------------------------- +* Function : debounce +* Description : 去抖动，判断之前length个label，根据较多的携带方式最终确定手 + 机携带方式 +* Date : 2020/7/20 logzhan +*---------------------------------------------------------------------**/ +static int debounce(int *label, int length) { + int cnt[4]; + int def; + int i; + + memset(cnt, 0, sizeof(cnt)); + for (i = 0; i < length; ++i) { + ++cnt[label[i]]; + } + + def = 0; + for (i = 0; i < ARR_LEN(cnt); ++i) { + if (cnt[i] > length * 3 / 4) { + return i; + } + if (cnt[i] > 0) { + def = i; + } + } + return def; +} + +/**--------------------------------------------------------------------- +* Function : DetectMotionType +* Description : pdr运动类型检测器 +* Date : 2022/09/23 logzhan +*---------------------------------------------------------------------**/ +int DetectMotionType() { + static Complex_t s_fft[1024] = { {0.0} }; + static float s_feature[15] = { 0 }; + float frq[3]; + float amp[3]; + int ret; + int i; + + // 分析加速度计 + if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[ACCE]))) { + return ret; + } + if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, 0))) { + return ret; + } + if (DETECTOR_NO_ERROR != (ret = track_frequency(g_acc_frq_buf, g_acc_amp_buf, g_acc_tmp_buf, frq, amp, 3))) { + return ret; + } + + // 分析陀螺仪 + if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[GYRO]))) { + return ret; + } + if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == pDetector.type || + DETECTOR_TYPE_HANDHELD == pDetector.type) ? 20.0f : 100.0f))) { + return ret; + } + if (DETECTOR_NO_ERROR != (ret = track_frequency(g_gyr_frq_buf, g_gyr_amp_buf, g_gyr_tmp_buf, frq, amp, 3))) { + return ret; + } + + get_feature(s_feature, 5); + + pDetector.type = pdr_detectorPredict(s_feature); + + /* debounce */ + for (i = 0; i < ARR_LEN(g_label) - 1; ++i) { + g_label[i] = g_label[i + 1]; + } + g_label[i] = pDetector.type; + pDetector.type = debounce(g_label, ARR_LEN(g_label)); + + return DETECTOR_NO_ERROR; +} + +/**---------------------------------------------------------------------- +* Function : DetUserStatic +* Description : 检测用户是否处于静止状态, 目前主要采用计步器判断和运动频率分析 +* 单运动频率计算量太大，可以考虑用加速度判断用户状态。 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) { + + if (pgnss->vel != -1 && pgnss->vel < 1 && (delSteps == 0 && + g_pdr->motionFreq == 0.0)) { + return PDR_TRUE; + } + return PDR_FALSE; +} + +/**---------------------------------------------------------------------- +* Function : DetectCarMode +* Description : 识别是否是车载模式 +* Input : pgnss，GPS数据结构体 + g_pdr，PDR结构体 + delSteps， 与上次相比步数的变化量 + time，计数器 +* Output : int，车载模式标志位 +* Date : 2021/01/28 logzhan +*---------------------------------------------------------------------**/ +int DetectCarMode(GNSS_t* gnss, PDR_t* pdr, unsigned long delSteps, int* time) +{ + if ((gnss->vel > 10 && delSteps == 0) || + (delSteps == 0 && pdr->motionFreq == 0.0 && gnss->vel > 3 && gnss->error < 30)) + { + (*time)++; + } + else { + *time = 0; + } + if (*time > 5) { + return PDR_TRUE; + } + return PDR_FALSE; +} + +/**---------------------------------------------------------------------- +* Function : DetectPdrToReset +* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况 +* 重置PDR到GPS的位置。 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +int DetectPdrToReset(double pdr_angle, int* gpsCnt, ulong deltsteps, PDR_t* pdr) { + + if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (pdr->motionFreq == 0))) { + (*gpsCnt)++; + } + else { + *gpsCnt = 0; + } + return (*gpsCnt > 6); +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Fft.c b/1.Software/PDR 1.06/src/Fft.c new file mode 100644 index 0000000..04a8f05 --- /dev/null +++ b/1.Software/PDR 1.06/src/Fft.c @@ -0,0 +1,116 @@ +/* Header File Including ------------------------------------------------------------------------ */ +#include "Fft.h" + +/* Macro Declaration ---------------------------------------------------------------------------- */ +#define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01 +#define SWAP(type, a, b) { type t = a; a = b; b = t; } + +/* Structure Declaration ------------------------------------------------------------------------ */ +typedef struct FFT_MCB { + uint8_t type; + uint32_t parameter[4]; + struct FFT_MCB *next; + struct FFT_MCB *prev; + uint8_t buffer[10240]; +} FFT_MCB; + +/* Global Variable Definition ------------------------------------------------------------------- */ +static FFT_MCB gMemery; + +/* Function Definition -------------------------------------------------------------------------- */ +/**---------------------------------------------------------------------- +* Function : FftInit +* Description : 初始化傅里叶运算库 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +static int FftInit(int length) { + Complex_t *omega; + uint16_t *position; + int bitLength; + int i; + int shift; + int j; + if (length * (sizeof(Complex_t) + sizeof(uint16_t)) > sizeof(gMemery.buffer)) { + return 0; + } + + if (FFT_MCB_TYPE_OMEGA_AND_POSITION == gMemery.type && length == (int)gMemery.parameter[0]) { + return length; + } + + gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION; + gMemery.parameter[0] = length; + + omega = (Complex_t *) gMemery.buffer; + for (i = 0; i < length; ++i) { + omega[i].real = (float)cos(-2 * M_PI * i / length); + omega[i].image = (float)sin(-2 * M_PI * i / length); + } + + position = (uint16_t *)(omega + length); + for (bitLength = 0, i = length; (i >> bitLength); ++bitLength); + for (i = 0; i < length; ++i) { + j = 0; + for (shift = 0; shift < bitLength - 1; ++shift) { + if((i >> shift) & 1) { + j |= (1 << (bitLength - shift - 2)); + } + } + position[i] = (uint16_t)j; + } + + return length; +} + +int FFT_Dft(Complex_t *dft, float *signal, int length) { + + if (0 == FftInit(length)) return 0; + + for (int k = 0; k < length; ++k) { + dft[k].real = 0; + dft[k].image = 0; + for(int i = 0; i < length; ++i) { + int ik = i * k % length; + dft[k].real += signal[i] * ((Complex_t *)gMemery.buffer)[ik].real; + dft[k].image += signal[i] * ((Complex_t *)gMemery.buffer)[ik].image; + } + } + return length; +} + + +int FFT_Fft(Complex_t *fft, float *signal, int length) { + int i,l,j,m; + uint16_t *position; + Complex_t *omega; + + //正常情况下，length正常、没溢出就不会进入return0 + if (length <= 0 || 0 != (length & (length - 1)) || 0 == FftInit(length)) { + return 0; + } + + for (i = 0; i < length; ++i) { + fft[i].real = signal[i]; + fft[i].image = 0; + } + + position = (uint16_t *)(gMemery.buffer + length * sizeof(Complex_t)); + for (i = 0; i < length; ++i) { + if (i < position[i]) { + SWAP(Complex_t, fft[i], fft[position[i]]); + } + } + + omega = (Complex_t *)gMemery.buffer; + for (l = 2, m = 1; l <= length; l <<= 1, m <<= 1) { + for (i = 0; i < length; i += l) { + for (j = 0; j < m; ++j) { + Complex_t delta = ComplexMul(omega[length / l * j], fft[i + m + j]); + fft[i + m + j] = ComplexSub(fft[i + j], delta); + fft[i + j] = ComplexAdd(fft[i + j], delta); + } + } + } + + return length; +} diff --git a/1.Software/PDR 1.06/src/Filter.c b/1.Software/PDR 1.06/src/Filter.c new file mode 100644 index 0000000..9f14924 --- /dev/null +++ b/1.Software/PDR 1.06/src/Filter.c @@ -0,0 +1,77 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Filter.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR滤波器支持库 +********************************************************************************/ +/* Header File Including ------------------------------------------------------*/ +#include "stdio.h" +#include "Filter.h" + +/* Function Definition --------------------------------------------------------*/ +/**--------------------------------------------------------------------- +* Function : FilterReset +* Description : 重置滤波器 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterReset(Filter_t *f) { + f->reset = 1; +} + +/**--------------------------------------------------------------------- +* Function : FilterSetCoef +* Description : 滤波器设置参数 +* Input : order coefficient array length, not filter order +* *b b coefficient +* *a a coefficient +* H0 dc signal gain +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterSetCoef(Filter_t *f, int order, double *b, double *a, double H0) { + for (int i = 0; i < order; ++i) { + f->b[i] = b[i]; + f->a[i] = a[i]; + } + f->order = order; + f->H0 = H0; + f->reset = 1; +} + +/**--------------------------------------------------------------------- +* Function : FilterFilter +* Description : 滤波器滤波计算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +double FilterFilter(Filter_t *f, double x) { + + if (f->order < 1) { + printf("fileter order is set to incorrect order, pls chk \ + your code!\n"); + return 0; + } + + double y = 0.0; + if (f->reset) { + for (int i = 0; i < f->order; ++i) { + f->yout[i] = f->H0 * x; + f->xin[i] = x; + } + y = f->yout[f->order - 1]; + f->reset = 0; + } + // push x and y + for (int i = 0; i < f->order - 1; ++i) { + f->xin[i] = f->xin[i + 1]; + f->yout[i] = f->yout[i + 1]; + } + f->xin[f->order - 1] = x; + // filter + y = f->b[0] * f->xin[f->order - 1]; + for (int i = 1; i < f->order; ++i) { + y += f->b[i] * f->xin[f->order - 1 - i] - f->a[i] * f->yout[f->order - 1 - i]; + } + f->yout[f->order - 1] = y; + + return y / f->H0; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Interface.c b/1.Software/PDR 1.06/src/Interface.c new file mode 100644 index 0000000..b8a6a43 --- /dev/null +++ b/1.Software/PDR 1.06/src/Interface.c @@ -0,0 +1,211 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Interface.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR导航算法对外部接口 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include +#include "pdr_api.h" +#include "PDRBase.h" +#include "Pedometer.h" +#include "Location.h" +#include "parseData.h" +#include "scene_recognition.h" + +/* Global Variable Definition ------------------------------------------------*/ +const char* Pedometer_Version = "2.0"; +Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}}; +LatLng_t llaLast = { 0 }; +Nmea_t Nmea; +IMU_t Imu; + +/* Extern Variable Definition ------------------------------------------------*/ +extern EKFPara_t EkfPara; +extern AHRS_t Ahrs; +extern GNSS_t pgnss; + +/* Function Declaration ------------------------------------------------------*/ + +/**--------------------------------------------------------------------- +* Function : RMC_Init +* Description : 初始化RMC协议 +* Date : 2022/09/16 logzhan +*---------------------------------------------------------------------**/ +void RMC_Init(NmeaRMC_t * rmc) +{ + rmc->type = SENSOR_MIN; + rmc->rmc_utc = ITEM_INVALID; + rmc->status = POSITIONING_N; + rmc->latitude = ITEM_INVALID; + rmc->rmc_latitude_ns = LATITUDE_N; + rmc->longitudinal = ITEM_INVALID; + rmc->longitudinal_ew = LATITUDE_N; + rmc->speed = ITEM_INVALID; + rmc->cog = ITEM_INVALID; + rmc->utc_ddmmyy = ITEM_INVALID; + rmc->magnetic = ITEM_INVALID; + rmc->mag_h = MAG_E; + rmc->mode = MODE_A; + rmc->time = ITEM_INVALID; + rmc->update = UN_UPDATE; + + memset(rmc->rmc_check_sum, 0, sizeof(rmc->rmc_check_sum)); +} + +/**--------------------------------------------------------------------- +* Function : NmeaFlag_Init +* Description : 初始化GPS的NMEA协议的相关标志位 +* Date : 2020/7/3 logzhan +*---------------------------------------------------------------------**/ +void NmeaFlag_Init(Nmea_t * ln) +{ + ln->gga.update = 0; + for (char i = 0; i < 3; i++) { + ln->gsa[i].update = UN_UPDATE; + ln->gsv[i].update = UN_UPDATE; + } + + ln->minTime = 0; + ln->maxTime = 0; + ln->update = UN_UPDATE; + + RMC_Init(&ln->rmc[0]); + RMC_Init(&ln->rmc[1]); + + ln->accuracy.time = ITEM_INVALID; + ln->accuracy.update = UN_UPDATE; + ln->accuracy.status = ITEM_INVALID; + ln->accuracy.err = ITEM_INVALID; +} + +/**--------------------------------------------------------------------- +* Function : clearNmeaFlg +* Description : 清空Nmea标志位 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +void ClearNmeaFlg(Nmea_t * ln){ + NmeaFlag_Init(ln); +} + +/**--------------------------------------------------------------------- +* Function : Algorithm_Init +* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨 +* 迹平滑窗口的初始化 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void Algorithm_Init(void) { + // 导航相关初始化 + PDR_Init(); + // 计步器初始化 + Pedometer_Init(); + // 初始化sensor历史缓存 + memset(SensorDataHist, 0, sizeof(SensorDataHist)); + memset(&Nmea, 0, sizeof(Nmea)); + memset(&Imu, 0, sizeof(Imu)); +} + +/**--------------------------------------------------------------------- +* Function : ParseDataAndUpdate +* Description : 解析字符串信息并更新 +* Date : 2022/10/16 logzhan +*---------------------------------------------------------------------**/ +int ParseDataAndUpdate(char* line, LctFs_t* LocFusion) +{ + ParseGnssInsData(line, &Imu, &Nmea); + // 给定Imu和Nmea结构体，返回融合位置 + return PDRLocationMainLoop(&Imu, &Nmea, LocFusion); +} + + +/**--------------------------------------------------------------------- +* Function : PDRLocationMainLoop +* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置 +* 迹平滑窗口的初始化 +* Date : 2020/07/03 logzhan +* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 +*---------------------------------------------------------------------**/ +int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) { + int type = 0; + + if (imu->gyr.update) { + // 如果陀螺仪更新则采用惯性传感器计算 + InsLocationUpdate(imu,&EkfPara); + imu->gyr.update = UN_UPDATE; + } + // 如果GPS不更新，返回 + if (!nmea->update)return TYPE_FIX_NONE; + + // 写GPS相关LOG信息 + SaveGnssInfo(nmea, LocFusion, NULL); + + // 有GPS则采用GPS融合定位 + int flag = GnssInsFusionLocation(nmea, &EkfPara, LocFusion); + + if (flag != TYPE_FIX_NONE) { + // 如果是开车这种，直接输出GPS，不进行平滑处理 + LocFusion->latitude = R2D(LocFusion->latitude); + LocFusion->longitudinal = R2D(LocFusion->longitudinal); + + LocFusion->last_lat = LocFusion->latitude; + LocFusion->last_lon = LocFusion->longitudinal; + type = 1; + }else if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0) { + LocFusion->latitude = LocFusion->last_lat; + LocFusion->longitudinal = LocFusion->last_lon; + type = 1; + } + ClearNmeaFlg(nmea); + return type; +} + +/**--------------------------------------------------------------------- +* Function : SaveGnssInfo +* Description : 保存GPS相关信息 +* Date : 2022/10/16 logzhan +*---------------------------------------------------------------------**/ +void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps) +{ + NmeaRMC_t rmc = nmea->rmc[1]; + // 选取更新时间的rmc + if (nmea->rmc[0].time > nmea->rmc[1].time) { + rmc = nmea->rmc[0]; + } + if (rmc.update && (rmc.status == POSITIONING_Y)) { + char str[256]; + memset(str, 0, sizeof(str)); + float accuracy = -1.0f; + if (fabs(rmc.time - nmea->accuracy.time) < 500){ + accuracy = nmea->accuracy.err; + } + sprintf(str, "%f,%.10f,%d,%.10f,%d,%.10f,%f,%f", rmc.time, rmc.latitude, + rmc.rmc_latitude_ns,rmc.longitudinal, rmc.longitudinal_ew, rmc.cog, accuracy, + rmc.speed); + + WriteCsvStr(fp_gps, str); + + result->gpsLat = rmc.latitude; + result->gpsLon = rmc.longitudinal; + } +} + +/**---------------------------------------------------------------------- +* Function : GetPDRVersion +* Description : 获取pdr版本号 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +const char* GetPDRVersion(void){ + return Pedometer_Version; +} + +/**---------------------------------------------------------------------- +* Function : pdr_closeAlgorithm +* Description : 关闭PDR算法,释放资源 +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +void Algorithm_DeInit() { + scene_recognition_destroy(); +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Kalman.c b/1.Software/PDR 1.06/src/Kalman.c new file mode 100644 index 0000000..b8d5263 --- /dev/null +++ b/1.Software/PDR 1.06/src/Kalman.c @@ -0,0 +1,267 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : Kalman.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR扩展卡尔曼滤波器融合 +********************************************************************************/ +#include +#include +#include +#include +#include "PDRBase.h" +#include "Kalman.h" +#include "Location.h" +#include "Matrix.h" +#include "Utils.h" + +// 扩展卡尔曼向量缓存 +static double EkfVecBuf[5][N] = { {0.0} }; +// 扩展卡尔曼矩阵缓存 +static double EkfMatBuf[7][N][N] = { { {0.0} } }; + +/**---------------------------------------------------------------------- +* Function : KalmanFilter_Init +* Description : 初始化卡尔曼滤波器相关 +* Date : 2022/09/16 logzhan +*---------------------------------------------------------------------**/ +void EKF_Init(void) +{ + memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N); + memset(EkfVecBuf, 0, sizeof(double) * 5 * N); +} + +/**---------------------------------------------------------------------- +* Function : EKFStatePredict +* Description : pdr卡尔曼滤波器的状态预测方程 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) { + + double angle = pdr->deltaHeading + ekf->pXk[3]; + + if(step == 0){ + ekf->pXk[3] = angle; + return; + } + + double(*Phi)[N] = (double(*)[N]) & (EkfMatBuf[0][0][0]); + double(*pvec1)[N] = (double(*)[N]) & (EkfMatBuf[1][0][0]); + double(*pvec2)[N] = (double(*)[N]) & (EkfMatBuf[2][0][0]); + + memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N); + + // xk+1 = xk + step * cos(angle) PDR位置更新方程 + // yk+1 = yk + step * sin(angle) PDR位置更新方程 + // sk+1 = sk 步长和上一次相同 + // ak+1 = ak * (angle / ak) 或 ak+1 = ak + + Phi[0][0] = 1; Phi[1][0] = 0; Phi[2][0] = 0; Phi[3][0] = 0; + Phi[0][1] = 0; Phi[1][1] = 1; Phi[2][1] = 0; Phi[3][1] = 0; + Phi[0][2] = cos(angle); Phi[1][2] = sin(angle); Phi[2][2] = 1; Phi[3][2] = 0; + Phi[0][3] = 0; Phi[1][3] = 0; Phi[2][3] = 0; Phi[3][3] = 1; + + + for (uchar i = 0; i < step; i++) { + // logzhan 21/02/06: + // phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性 + // 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度 + // 让预测的值更新，但是效果暂时没有感觉出明显的好 + if (fabs(ekf->pXk[3]) > 0.000001 && + fabs(angle / ekf->pXk[3]) < 3) + { + Phi[3][3] = angle / ekf->pXk[3]; + } + + // 卡尔曼状态更新方程 + // pXk = phi * pXk + VecMatMultiply(ekf->pXk, Phi, ekf->pXk); + + // 卡尔曼更新协方差矩阵 : Pk = FPFt + Q + MatrixMultiply(Phi, ekf->pPk, pvec1); // F*P + MatrixTrans(Phi, pvec2); // Ft + MatrixMultiply(pvec1, pvec2, pvec1); // F*P*Ft + MatrixAdd(pvec1, ekf->Q, ekf->pPk); // F*P*Ft + Q + } +} + +/**---------------------------------------------------------------------- +* Function : EKFStateUpdate +* Description : 扩展卡尔曼滤波器的状态更新方程 +* Date : 2020/7/22 logzhan +*---------------------------------------------------------------------**/ +void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, Classifer_t *sys, PDR_t *pdr, + int scene_type) { + + double lambda = 0; + + // NxN矩阵定义 + double(*H)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]); + double(*I)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]); + double(*pvec1)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]); + double(*Ht)[N] = (double(*)[N])&(EkfMatBuf[3][0][0]); + double(*pvec3)[N] = (double(*)[N])&(EkfMatBuf[4][0][0]); + double(*pvec4)[N] = (double(*)[N])&(EkfMatBuf[5][0][0]); + double(*pv3)[N] = (double(*)[N])&(EkfMatBuf[6][0][0]); + + memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N); + memset(EkfVecBuf, 0, sizeof(double) * 5 * N); + + // Nx1向量定义 + double(*pvec5) = &(EkfVecBuf[1][0]); + double(*pvec6) = &(EkfVecBuf[2][0]); + double(*pv1) = &(EkfVecBuf[3][0]); + double(*pv2) = &(EkfVecBuf[4][0]); + + // 创建单位矩阵 + for (char i = 0; i < 4; i++) { + H[i][i] = 1; + I[i][i] = 1; + } + + double* z = &(EkfVecBuf[0][0]); + // 获取观测向量 + z[0] = pgnss->xNed; // 北向位置 + z[1] = pgnss->yNed; // 东向位置 + z[2] = 0.5; // 步长默认0.5 + + // 这个if条件实际是使用GPS的位置变化量和步数变化量估计步长, 这部分代码还没 + // 调好，实际类似于步长不变 + //if (pgnss->error > 0 && pgnss->error < 3.8 && pgnss->HDOP <= 0.4 && + // g_pdr->deltaStepsPerGps != 0 && g_pdr->deltaStepsPerGps < 5) { + // // 利用GPS速度和步频估计步长 + // //if (g_pdr->motionFreq > 2.5 && (sys->type == 1)) { + // // + // // //printf("z[2] = %f\n", z[2]); + // //} + // z[2] = pgnss->vel * 0.514 / (g_pdr->deltaStepsPerGps); + //}else { + // z[2] = kf->pXk[2]; + //} + + z[3] = pgnss->yaw * PI / 180; // 观测的航向角为GPS的航向角 + + // 计算 K = K = PHt / (R + H*P*H^T) + MatrixTrans(H, Ht); // 计算Ht + MatrixMultiply(H, kf->pPk, pvec1); // 计算HP + MatrixMultiply(pvec1, Ht, pvec3); // pvec3 = H*P*H^T + MatrixAdd(pvec3, kf->R, pvec4); // pvec4 = R + H*P*H^T + MatrixInverse(pvec4, pvec1); // 计算 1 / (R + H*P*H^T) + + for (char i = 0; i < N; i++) { + for (uchar j = 0; j < N; j++) { + pv3[i][j] = pvec3[i][j]; + } + } + MatrixMultiply(kf->pPk, Ht, pvec3); // 计算PHt + MatrixMultiply(pvec3, pvec1, kf->Kk); // 计算增益K = PHt / (R + H*P*H^T) + + + VecMatMultiply(kf->pXk, H, pvec5); // pvec5 = Hx' + VectorSub(z, pvec5, pvec6); // pvec6 = Z - Hx' + modAngle(&pvec6[3], -PI, PI); + for (char i = 0; i < N; i++) { + pv1[i] = pvec6[i]; + } + + VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx') + // Get the optimally updated state estimation + VectorAdd(kf->pXk, pvec5, kf->Xk); + + // ---- prepare the matrix for updated estimate covariance ------ + // pvec1 = K*H + MatrixMultiply(kf->Kk, H, pvec1); + // pvec2 = (I - K*H) + MatrixSub(I, pvec1, Ht); + // pvec3 = (I - K*H)*P + MatrixMultiply(Ht, kf->pPk, pvec3); + // pvec4 = (I- K*H)^T + MatrixTrans(Ht, pvec4); + // pvec1 = (I - K*H)*P*(I- K*H)^T + MatrixMultiply(pvec3, pvec4, pvec1); + + // pvec2 = K*R + MatrixMultiply(kf->Kk, kf->R, Ht); + // pvec3 = K^T + MatrixTrans(kf->Kk, pvec3); + // pvec4 = K*R*K^T + MatrixMultiply(Ht, pvec3, pvec4); + // Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T + MatrixAdd(pvec1, pvec4, kf->Pk); + + // pv2 = (z - Hx')*( H*P*H^T ) + VecMatMultiply(pv1, pv3, pv2); + + // inner product the pre-fit residual and the transitioned residual: lamda = (z - Hx')*( H*P*H^T ) .* (z-Hx') + // Calculate the difference between original differenc and the transitioned, + // if the value is close to zero, it shows that there is a big gap in the + // oprimization. Thus, we believe our PDR algorithm + for (char i = 0; i < N; i++) { + lambda += pv2[i] * pv1[i]; + } + + kf->Lambda = lambda; + kf->pLat = pgnss->lat; + kf->pLon = pgnss->lon; + + if (lambda >= 200)return; + + for (char i = 0; i < N; i++) { + kf->Xk[i] = kf->pXk[i]; + for (int j = 0; j < N; j++) { + kf->Pk[i][j] = kf->pPk[i][j]; + } + } + +} + +/**---------------------------------------------------------------------- +* Function : EKFCalQRMatrix +* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵， +* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的 +* 信息精度有关。 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *pdr, Classifer_t *sys, + EKFPara_t *kf) { + + if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD && + pdr->MotionType != PDR_MOTION_TYPE_STATIC) { + kf->Q[0][0] = 25; // PDR预测位置噪声 + kf->Q[1][1] = 25; // PDR预测位置噪声 + kf->Q[2][2] = 1; // PDR步长噪声 + kf->Q[3][3] = 100; // PDR方向噪声 + + kf->R[0][0] = 50; // GPS观测位置噪声 + kf->R[1][1] = 50; // GPS观测位置噪声 + kf->R[2][2] = 0.1; // GPS步长噪声 + kf->R[3][3] = 50; // GPS方向噪声 + return; + } + + kf->Q[0][0] = 0.1; // PDR预测位置噪声 + kf->Q[1][1] = 0.1; // PDR预测位置噪声 + kf->Q[2][2] = 1; // PDR步长噪声 + kf->Q[3][3] = 1; // PDR方向噪声 + + kf->R[0][0] = 100; // GPS预测位置噪声 + kf->R[1][1] = 100; // GPS预测位置噪声 + kf->R[2][2] = 0.1; // GPS步长噪声 + kf->R[3][3] = 1000; // GPS方向噪声 + + if (nmea->accuracy.err > 0 && + nmea->accuracy.err < 3.80 && + nmea->gga.hdop <= 0.4) + { + kf->R[0][0] = 50; // GPS观测位置噪声 + kf->R[1][1] = 50; // GPS观测位置噪声 + kf->R[2][2] = 0.1; // GPS步长噪声 + kf->R[3][3] = 50; // GPS方向噪声 + } + + if (fabs(R2D((pdr->GpsHeading - pdr->LastGpsHeading))) > 50 || + pdr->GpsSpeed < 0.7 || pdr->GpsHeading == -1) { + kf->R[3][3] = 10000; + } +} + diff --git a/1.Software/PDR 1.06/src/LinearFit.c b/1.Software/PDR 1.06/src/LinearFit.c new file mode 100644 index 0000000..cf09331 --- /dev/null +++ b/1.Software/PDR 1.06/src/LinearFit.c @@ -0,0 +1,149 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_linearFit.c +* Department : Sensor Algorithm Team +* Current Version : V2.0(compare QCOM SAP 5.0) +* Author : logzhan +* Date of Issued : 2020.7.4 +* Comments : PDR 线性拟合相关函数支持, 主要是用于轨迹平滑 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include "LinearFit.h" +#include "Utils.h" + +/**---------------------------------------------------------------------- +* Function : pdr_linearLeastSquaresFitting +* Description : 线性最小二乘拟合 Y = c0 + c1 x + 目标函数为sum| nDataY - (a*nDataX+b)|^2最小 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +int linearLeastSquaresFit(const double *nDataX, const double *nDataY, const int nLength, double *a, double *b) +{ + int fittingRes = 0; + if (nDataX == NULL || nDataY == NULL || a == NULL || b == NULL){ + fittingRes = -1; + } else if (nLength <= 0){ + fittingRes = 1; + } else { + fittingRes = gslFitLinear(nDataX, 1, nDataY, 1, nLength, b, a, NULL, NULL, NULL, NULL); + } + return fittingRes; +} + +/**---------------------------------------------------------------------- +* Function : gsl_fit_linear +* Description : Fit the data (x_i, y_i) to the linear relationship + Y = c0 + c1 x + returning, + c0, c1 -- coefficients + cov00, cov01, cov11 -- variance-covariance matrix of + c0 and c1, + sumsq -- sum of squares of residuals + This fit can be used in the case where the errors for + the data are uknown, but assumed equal for all points. + The resulting variance-covariance matrix estimates the + error in the coefficientsfrom the observed variance of + the points around the best fit line. +* Date : +*---------------------------------------------------------------------**/ +static int gslFitLinear(const double *x, const size_t xstride, + const double *y, const size_t ystride, + const size_t n, + double *c0, double *c1, + double *cov_00, double *cov_01, double *cov_11, double *sumsq) +{ + double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0; + double dx; + double dy; + size_t i; + double s2 = 0, d2 = 0; + double b = 0; + double a = 0; + double d = 0; + for (i = 0; i < n; i++) + { + m_x += (x[i * xstride] - m_x) / (i + 1.0); + m_y += (y[i * ystride] - m_y) / (i + 1.0); + } + + for (i = 0; i < n; i++) + { + dx = x[i * xstride] - m_x; + dy = y[i * ystride] - m_y; + + m_dx2 += (dx * dx - m_dx2) / (i + 1.0); + m_dxdy += (dx * dy - m_dxdy) / (i + 1.0); + } + + /* In terms of y = a + b x */ + if (fabs(m_dx2) < 1e-20)return -1; + + b = m_dxdy / m_dx2; + a = m_y - m_x * b; + + *c0 = a; + *c1 = b; + + /* Compute chi^2 = \sum (y_i - (a + b * x_i))^2 */ + for (i = 0; i < n; i++) + { + dx = x[i * xstride] - m_x; + dy = y[i * ystride] - m_y; + d = dy - b * dx; + d2 += d * d; + } + + s2 = d2 / (n - 2.0); /* chisq per degree of freedom */ + + /* 为函数增加空指针检查以使函数可以将一下参数设置为空指针 */ + if (cov_00 != NULL) + *cov_00 = s2 * (1.0 / n) * (1 + m_x * m_x / m_dx2); + if (cov_11 != NULL) + *cov_11 = s2 * 1.0 / (n * m_dx2); + if (cov_01 != NULL) + *cov_01 = s2 * (-m_x) / (n * m_dx2); + + if (sumsq != NULL) + *sumsq = d2; + + return 0; +} + +/**---------------------------------------------------------------------- +* Function : leastDistanceLinearFit +* Description : 二维坐标系点的线性拟合，ax+by+c=0, 目标函数为点到拟合直线的距离 + 平方和最小， 这个函数的计算量还是相对较大的，主要注意这个函数不 + 适合频繁调用 +* Input : x， y，二维坐标X和Y + num, 二维点个数 +* Output : para， 拟合直线参数, para[0]:a, para[0]:b, para[0]:c +*---------------------------------------------------------------------**/ +int leastDistanceLinearFit(double x[], double y[], int num, double para[]) +{ + int i; + double covMatrix[2][2] = { { 0.0 } }; + double p[2][2] = { { 0.0 } }; + + if (num < 2) + return -1; + + GetCovMatrix(x, y, covMatrix, num); + + Jacobi(covMatrix, p, 2, 1e-10, 1000); + + if (covMatrix[0][0] > covMatrix[1][1]) { + para[0] = p[0][1]; + para[1] = p[1][1]; + } + else { + para[0] = p[0][0]; + para[1] = p[1][0]; + } + + para[2] = 0; + for (i = num - 1; i >= 0; i--) { + para[2] -= (para[0] * x[i] + para[1] * y[i]) / num; + } + return 0; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Location.c b/1.Software/PDR 1.06/src/Location.c new file mode 100644 index 0000000..314c959 --- /dev/null +++ b/1.Software/PDR 1.06/src/Location.c @@ -0,0 +1,564 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_location.c +* Current Version : V1.2 +* Author : logzhan +* Date of Issued : 2020.7.3 +* Comments : PDR导航算法主流程 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include +#include +#include +#include +#include +#include "Location.h" +#include "Kalman.h" +#include "Pedometer.h" +#include "AHRS.h" +#include "pdr_detector.h" +#include "Utils.h" +#include "scene_recognition.h" +#include "CoordTrans.h" + +/* Global Variable Definition ------------------------------------------------*/ +PDR_t pdr; // PDR全局信息 +StepPredict stepPredict = { 0 }; +Classifer_t sys; +GNSS_t pgnss; // 定位相关信息 +EKFPara_t EkfPara; +double GpsPos[HIST_GPS_NUM][3] = {{0}}; +static uint32_t g_status; +extern AHRS_t Ahrs; + +double refGpsYaw = -1000; +double yaw_bias = -1; + +float dir[2] = { 0 }; +float bias_dir[2] = { 0 }; + +/**--------------------------------------------------------------------- +* Function : PDRNav_Init +* Description : PDR导航系统初始化 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void PDR_Init(void) { + + memset(&EkfPara, 0, sizeof(EkfPara)); + memset(&pgnss, 0, sizeof(pgnss)); + memset(GpsPos, 0, sizeof(GpsPos)); + memset(&stepPredict, 0, sizeof(stepPredict)); + memset(&sys, 0, sizeof(sys)); + memset(dir, 0, sizeof(dir)); + memset(bias_dir, 0, sizeof(bias_dir)); + + // GNSS定位初始化 + InitGnssInfo(); + // 初始化AHRS姿态解算相关 + AHRS_Init(); + // 检测器初始化 + Detector_Init(); + // PDR初始化 + PDRInfoInit(); + //base文件中姿态角计算初始化 + EKF_Init(); + // 场景识别模块初始化 + SceneRecog_Init(); + + yaw_bias = -1; +} + +void PDRInfoInit() +{ + memset(&pdr, 0, sizeof(pdr)); + pdr.status = PDR_STATUS_RESET; + pdr.axis_ceofficient[0] = 0; + pdr.axis_ceofficient[1] = 1; + pdr.axis_ceofficient[2] = 0; + pdr.bias_direction[0] = 1; + pdr.bias_direction[1] = 0; + pdr.pcaAccuracy = ITEM_INVALID; + pdr.biasAccuracy = ITEM_INVALID; + pdr.sysStatus = IS_INITIAL; + pdr.sceneType = UN_OPEN_AREA; + pdr.fusionPdrFlg = PDR_FALSE; + pdr.trackHeading = ITEM_INVALID; + pdr.motionFreq = 0.1f; + memset(pdr.PllaInit, 0, sizeof(pdr.PllaInit)); +} + +/**--------------------------------------------------------------------- +* Function : InitGnssInfo +* Description : PDS GNSS定位信息初始化 +* Date : 2020/02/01 logzhan: +* 创建初始版本 +* 2020/02/08: logzhan 修改initGps为initGnssInfo，更 +* 加符合函数功能 +*---------------------------------------------------------------------**/ +void InitGnssInfo(void) { + pgnss.lastError = ACCURACY_ERR_MAX; + pgnss.overVelCount = 0; +} + +/**--------------------------------------------------------------------- +* Function : ResetSystemStatus +* Description : PDR 重设系统状态 +* Date : 2022/09/16 logzhan +*---------------------------------------------------------------------**/ +void ResetSystemStatus(EKFPara_t *kf) +{ + memset(kf, 0, sizeof(EKFPara_t)); + pdr.fusionPdrFlg = PDR_FALSE; + pdr.sysStatus = IS_INITIAL; + pgnss.overVelCount = 0; +} + +/**--------------------------------------------------------------------- +* Function : InsLocationUpdate +* Description : PDR 惯性导航定位 +* Date : 2022/09/16 logzhan +*---------------------------------------------------------------------**/ +int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) { + + // AHRS姿态更新占2%的运行时间 + if (UpdateAHRS(ImuData)) { + // 完成姿态估计后计算PCA， 占26%的运行时间 + //ComputePCA(&g_ahrs); + } + // 更新用户运动类型检测器IMU信息以及用户运动类型检测, 占21%的运行时间 + DetectorUpdateIMU(ImuData, &pdr); + + pdr.gyroTime = ImuData->gyr.time; + // 为什么手持类型是0.1的运动频率？ + if (pdr.MotionType == DETECTOR_TYPE_HANDHELD)pdr.motionFreq = 0.1f; + + // 计步器输入传感器数据，占10%的运行时间 + Pedometer_Update(ImuData, &pdr.steps); + + // ----- Filter Initialization ------- // + if (pdr.sysStatus == IS_INITIAL) { + pdr.ts = ImuData->gyr.time; + pdr.lastSteps = pdr.steps; + stepPredict.lastTime = 0; + stepPredict.fnum = 0; + stepPredict.fsum = 0; + return 0; + } + + pdr.ts = ImuData->gyr.time; + + if ((0 == ImuData->acc.s[0] && 0 == ImuData->acc.s[1] && 0 == ImuData->acc.s[2]) || + (0 == ImuData->mag.s[0] && 0 == ImuData->mag.s[1] && 0 == ImuData->mag.s[2])) { + return 0; + } + + // 识别当前状态是否平稳，这个函数计算量到达整个系统10%的计算量, 以后根据情况进行优化 + StateRecognition(ImuData->acc.s, &sys); + + //pdr.heading = CalPredHeading(...); + + //if (pdr.MotionType == PDR_MOTION_TYPE_HANDHELD) + { + double imuHeading = -Ahrs.Yaw; + if (imuHeading < 0) { + imuHeading += 360; + } + + pdr.heading = D2R(imuHeading); + if (fabs(pdr.insHeadingOffset) > 0.000001) { + imuHeading = imuHeading - pdr.insHeadingOffset * (180 / 3.14159265); + pdr.heading = imuHeading / (180 / 3.14159265); + } + pdr.deltaHeading = pdr.heading - pdr.lastHeading; + } + + // 角度增量约束(100Hz * 5.5° = 550°/sec) + if (pdr.deltaHeading > D2R(5.5)) { + pdr.deltaHeading = 0.0f; + } + + // PDR 惯性位置预测 + InsLocationPredict(&pdr, &stepPredict, ImuData, &pgnss, Ekf); + + pdr.lastSteps = pdr.steps; + pdr.lastHeading = pdr.heading; + return 0; +} + +/**---------------------------------------------------------------------- +* Function : GnssInsFusionLocation +* Description : PDR GPS融合INS惯性定位 +* Date : 2021/01/29 logzhan +*---------------------------------------------------------------------**/ +int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result) +{ + Nmea2Gnss(nmea, &pgnss); + // 检查时间差是否满足条件 + if (fabs(pgnss.timestamps - pdr.ts) >= 1000)return TYPE_FIX_NONE; + + // 根据nmea数据识别场景是否是开阔天空场景 + pdr.sceneType = isOpenArea(nmea); + // 根据HDOP、速度、卫星数量等判断gps yaw是否可用 + + pdr.GpsSpeed = pgnss.vel * GPS_SPEED_UNIT; + pdr.LastGpsHeading = pdr.GpsHeading; + pdr.GpsHeading = GnssCalHeading(&pgnss); + + // 如果在win32平台下仿真调试,则添加下面代码 +#ifdef _WIN32 + result->pdrHeading = kf->pXk[3]; + result->gpsHeading = pdr.GpsHeading; + result->hdop = pgnss.HDOP; + result->accuracy = nmea->accuracy.err; + result->gpsSpeed = pdr.GpsSpeed; + result->motionType = pdr.MotionType; +#endif + + // 利用Nmea信息修正Ins航向角 + CalPdrHeadingOffset(nmea, &pdr); + + // 计算两次GPS时间之间，步数变化量 + pdr.deltaStepsPerGps = pdr.steps - pdr.lastGpsSteps; + pdr.lastGpsSteps = pdr.steps; + + int mode = DetectFixMode(&pgnss, kf, &pdr, result); + if (mode != TYPE_FIX_PDR)return mode; + + //根据历史GPS拟合输出预估的GPS轨迹航向角 + pdr.trackHeading = CalGnssTrackHeading(GpsPos, pgnss); + + // 如果系统没有初始化，则先初始化 + if (pdr.sysStatus == IS_INITIAL) { + // Gnss系统初始对准 + return GnssInsInitialAlignment(&pgnss, kf, &pdr, result); + } + // 如果惯导没有达到输出条件，则不预测轨迹 + if (!pdr.fusionPdrFlg) + { + return ResetOutputLoction(&pgnss, &pdr, kf, result); + } + + // 根据GPS信息, 计算kf的过程噪声和观测噪声 + EKFCalQRMatrix(nmea, &pdr, &sys, kf); + + // 运行卡尔曼位置融合 + if (pgnss.satellites_num > 4){ + // 如果GPS航向角处于无效状态且速度也是无效状态 + if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) { + //if(pdr.heading != INVAILD_GPS_YAW){ + // pgnss.yaw = (float)(R2D(pdr.heading)); + //}else if (pdr.trackHeading != INVAILD_GPS_YAW) { + // pgnss.yaw = (float)(R2D(pdr.trackHeading)); + //} + pgnss.yaw = (float)kf->pXk[3]; + } + if (pgnss.yaw != INVAILD_GPS_YAW) { + EkfGnssInsLocFusion(&pgnss, &pdr, &sys, yaw_bias, kf, result); + return TYPE_FIX_PDR; + } + } + + // 不满足推算条件，直接输出原始GPS + if (pgnss.satellites_num > 4 && nmea->accuracy.err > 0 && + nmea->accuracy.err < 15) { + OutputRawGnssPos(&pgnss, result); + pdr.NoGnssCount = 0; + return TYPE_FIX_GPS; + } + + // 如果没有GPS信号，那么PDR也会接着推算位置, 但是最多推算10个点 + if (pdr.NoGnssCount < MAX_NO_GPS_PREDICT) { + // 无GPS状态位置推算 + NoGnssInfoPredict(kf, result, &pdr); + return TYPE_FIX_PDR; + } + pdr.NoGnssCount++; + return 0; +} + +/**---------------------------------------------------------------------- +* Function : NoGnssInfoPredict +* Description : 在没有gps信息时预测GPS位置 +* Date : 2022/09/16 +*---------------------------------------------------------------------**/ +void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* p_pdr) +{ + double ned[3] = {0}, lla[3] = {0}; + ned[0] = kf->pXk[0] - p_pdr->x0; + ned[1] = kf->pXk[1] - p_pdr->y0; + ned[2] = 0; + + NED2WGS84(p_pdr->PllaInit, ned, lla); + + result->latitude = lla[0]; + result->latitude_ns = pgnss.lat_ns; + result->longitudinal = lla[1]; + result->longitudinal_ew = pgnss.lon_ew; + result->time = pgnss.timestamps; + p_pdr->NoGnssCount++; +} + +/**---------------------------------------------------------------------- +* Function : calPdrHeadingOffset +* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移 +* Date : 2020/07/08 logzhan +*---------------------------------------------------------------------**/ +void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) { + //惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳） + if (refGpsYaw != -1000)return; + + if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0) + && (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4) + && (DetUserStatic(&pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr.MotionType == 2)) + { + /*这里放姿态角与GPS对准part1 start*/ + double imuHeading = atan2(2 * (Ahrs.q[0] * Ahrs.q[3] + Ahrs.q[1] * Ahrs.q[2]), + 1 - 2 * (Ahrs.q[2] * Ahrs.q[2] + Ahrs.q[3] * Ahrs.q[3])); + + if (imuHeading < 0) { + imuHeading += TWO_PI; + } + + if (fabs(imuHeading - p_pdr->GpsHeading) < PI / 18) + { + //printf("offset 标定\n"); + //p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading; + } + } +} + +/**---------------------------------------------------------------------- +* Function : DetectFixMode +* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出 +* 原始GPS或者不输出 +* Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高 +* 代码的可读性 +*---------------------------------------------------------------------**/ +int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* pdr, LctFs_t* result) { + // 检测用户是否处于静止状态 + if (DetUserStatic(pdr, pgnss, pdr->deltaStepsPerGps)) { + if (pgnss->error > 0 && pgnss->error < 15 && + pgnss->lastError >= pgnss->error) { + OutputRawGnssPos(pgnss, result); + pgnss->lastError = pgnss->error; + return TYPE_FIX_PDR; + } + // 否则不输出位置 + return TYPE_FIX_NONE; + } + // 如果是检测到是开车状态则输出GPS位置 + if (DetectCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) { + // 如果检测到车载模式，则重置PDR状态，需要重新初始化 + ResetSystemStatus(kf); + if (pgnss->error >= 0 || (pgnss->satellites_num > 4 + && pgnss->HDOP < 2.0)) { + OutputRawGnssPos(pgnss, result); + return TYPE_FIX_GPS; + } + // 精度过差，不输出位置 + return TYPE_FIX_NONE; + } + return TYPE_FIX_PDR; +} + + +/**---------------------------------------------------------------------- +* Function : GnssCalHeading +* Description : 获取GPS偏航角(0 - 360°) +* Date : 2022/09/16 logzhan +* 代码的可读性 +*---------------------------------------------------------------------**/ +double GnssCalHeading(GNSS_t* pgnss) { + double gnssHeading = INVAILD_GPS_YAW; + if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0 + && pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) { + gnssHeading = pgnss->yaw * RADIAN_PER_DEG; + } + return gnssHeading; +} + +/**---------------------------------------------------------------------- +* Function : pdr_nmea2Gnss +* Description : nmea数据结构转gnss数据 +* Date : 2020/07/08 logzhan +*---------------------------------------------------------------------**/ +void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss) +{ + NmeaRMC_t rmc = { 0 }; + if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) { + rmc = nmea_data->rmc[0]; + }else { + rmc = nmea_data->rmc[1]; + } + // 更新上一次的Accuracy Error + pgnss->lastError = pgnss->error; + pgnss->lat = rmc.latitude * PI / 180; + pgnss->lat_ns = rmc.rmc_latitude_ns; + pgnss->lon = rmc.longitudinal * PI / 180; + pgnss->lon_ew = rmc.longitudinal_ew; + //pgnss->vel = rmc.speed * GPS_SPEED_UNIT; + pgnss->vel = rmc.speed; + pgnss->HDOP = nmea_data->gsa[0].hdop; + pgnss->yaw = rmc.cog; + pgnss->quality = 0; + pgnss->error = nmea_data->accuracy.err; + pgnss->timestamps = rmc.time; + pgnss->satellites_num = nmea_data->gga.satellites_nb; + +} + +/**---------------------------------------------------------------------- +* Function : OutputRawGnssPos +* Description : 输出GPS位置 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) { + result->latitude = pgnss->lat; + result->latitude_ns = pgnss->lat_ns; + result->longitudinal = pgnss->lon; + result->longitudinal_ew = pgnss->lon_ew; + result->time = pgnss->timestamps; +} + + +/**---------------------------------------------------------------------- +* Function : ResetOutputLoction +* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数 +* 2、GPS值赋值给result +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result) { + + double stepLength = 0.7; + double ned[3] = { 0 }; + + g_pdr->PllaInit[0] = pgnss->lat; + g_pdr->PllaInit[1] = pgnss->lon; + g_pdr->PllaInit[2] = 0; + WGS842NED(g_pdr->PllaInit, g_pdr->PllaInit, ned); + + g_pdr->x0 = ned[0]; + g_pdr->y0 = ned[1]; + + kf->pXk[0] = ned[0]; + kf->pXk[1] = ned[1]; + kf->pXk[2] = stepLength; + // 重置输出GPS时，GPS的航向角不一定准 + kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; + + for (uchar i = 0; i < N; i++) { + kf->Xk[i] = kf->pXk[i]; + } + // 清除卡尔曼Q矩阵和R矩阵 + for (int i = 0; i < N; i++) { + for (int l = 0; l < N; l++) { + kf->Q[i][l] = 0.0; + kf->R[i][l] = 0.0; + } + } + // 输出GPS位置结果 + OutputRawGnssPos(pgnss, result); + g_pdr->NoGnssCount = 0; + return PDR_TRUE; +} + +/**---------------------------------------------------------------------- +* Function : EkfGnssInsLocFusion +* Description : 采用EKF对GNSS和INS融合定位 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* pdr, Classifer_t* sys, double yaw_bias, + EKFPara_t* kf, LctFs_t* res) { + double plla[3] = { 0 }; + double Ned[3] = { 0 }; + double yaw_thr = 6; + + int scene_type = pdr->sceneType; + + plla[0] = pgnss->lat; + plla[1] = pgnss->lon; + plla[2] = 0; + + WGS842NED(plla, pdr->PllaInit, Ned); + + pgnss->xNed = Ned[0]; + pgnss->yNed = Ned[1]; + + //调整融合策略 + EKFStateUpdate(kf, pgnss, sys, pdr, scene_type); + + for (uchar i = 0; i < N; i++) { + for (uchar l = 0; l < N; l++) { + kf->pPk[i][l] = kf->Pk[i][l]; + } + kf->pXk[i] = kf->Xk[i]; + } + + Ned[0] = kf->pXk[0] - pdr->x0; + Ned[1] = kf->pXk[1] - pdr->y0; + Ned[2] = 0; + + NED2WGS84(pdr->PllaInit, Ned, plla); + + res->latitude = plla[0]; + res->latitude_ns = pgnss->lat_ns; + res->longitudinal = plla[1]; + res->longitudinal_ew = pgnss->lon_ew; + res->time = pgnss->timestamps; + + pdr->NoGnssCount = 0; +} + +/**---------------------------------------------------------------------- +* Function : PDRInitialAlignment +* Description : PDR初始对准 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result) +{ + double stepLen = 0.7; // 初始运动步长为0.7m + if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) { + // plla 类似位置坐标的原点(单位是经纬度) + + g_pdr->PllaInit[0] = pgnss->lat; + g_pdr->PllaInit[1] = pgnss->lon; + g_pdr->PllaInit[2] = 0; + + // plla 转 ned + double ned[3] = { 0 }; + WGS842NED(g_pdr->PllaInit, g_pdr->PllaInit, ned); + + // 在NED坐标系的计算, 单位是米 + g_pdr->x0 = ned[0]; + g_pdr->y0 = ned[1]; + + // 初始化卡尔曼滤波器的观测量 + kf->pXk[0] = ned[0]; // 状态量1: 北向x + kf->pXk[1] = ned[1]; // 状态量2：东向y + kf->pXk[2] = stepLen; // 状态量3：步长 + kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角 + + for (uchar i = 0; i < N; i++) { + kf->Xk[i] = kf->pXk[i]; + } + + kf->initHeading = kf->pXk[3]; + for (int i = 0; i < N; i++) { + for (int l = 0; l < N; l++) { + kf->Q[i][l] = 0.0; + kf->R[i][l] = 0.0; + } + } + OutputRawGnssPos(pgnss, result); + g_pdr->sysStatus = IS_NORMAL; + + return PDR_TRUE; + } + + if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) { + OutputRawGnssPos(pgnss, result); + return PDR_TRUE; + } + return PDR_FALSE; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Main.cpp b/1.Software/PDR 1.06/src/Main.cpp new file mode 100644 index 0000000..54971db --- /dev/null +++ b/1.Software/PDR 1.06/src/Main.cpp @@ -0,0 +1,460 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_main.cpp +* Department : Sensor Algorithm Team +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2021.1.28 +* Comments : PDR PC端仿真主文件 +* Modify : 2021/02/04 logzhan : 优化kml显示，kml增加航向角图形 +* 显示，hdop、heading、accuracy信息在Google Map的显示，方便调 +* 试代码。 +* 2021/02/18 logzhan : 增加kml的时间显示，能够拖动进 +* 度条显示轨迹。可以对比查看轨迹效果。 +* +* 一、优化方向和工作内容 +* 1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外， +* 其他地方都是用的实际的GPS速度，包括步数预测 +* 2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化 +* 时间分析发现，buffer mean耗费了大量时间 +* 2021/02/10 logzhan: +* 1) 需要跟品质谈高通对比版本，防止最后效果和高通SAP5.0持平后， +* 高通出了新版本，品质不认可效果改进。 +* 2) 目前GPS的速度是没有乘以0.51444的，许多地方计算没有统一，需 +* 要统一。 +* 3）惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化 +* 时间分析发现，buffer mean耗费了大量时间 +* 4)*目前在不依赖磁力计的版本也能达到比以前有磁力计效果还好。但是 +* 磁力计依然是一个重要的辅助确定方向的手段。所以利用GPS航向作为 +* 观测，磁力计启动动态校准，校准完成后才启动辅助磁力计校准方向， +* 或者作为一个观测量，用于提升预测方向的准确程度。 +* 5)*动态偏移对准，后续会开始开发PCA方向估计，那么PCA估计方向肯定 +* 和真实方向存在一定的偏差，需要动态对准偏移。 +* 6）目前的计步器，输出不够实时，对于步长的估计不够准确。目前测试 +* 的情况是，加入动态步长估计后貌似轨迹会偏大，是不是计步偏少？ +* 7）这个步频计算用到了fft，感觉没有太大作用，可以考虑在新版本中 +* 去除，然后以后开发时如果需要可以参考原理重新优化。 +* 8) 赖工提到路网信息，目前比较确定的是能拿到路的方向信息，考虑 +* 在选定的场景标定一下经纬度和最近的道路方向信息，观察如何提升 +* 定位精度(方向辅助，减少方向上的漂移)。 +* 9) PDR 目前一定概率初始对准方向会偏，所以初始对准时，检测是否 +* 是直线路径，然后考虑用轨迹计算平均方向角，辅助初始化。 +* 10) 跟张经睿讨论过，场景识别模块以前主要是针对以前的单频手机， +* 所以这个模块考虑重构，因为以前单频手机的参数和现在双频手机不同。 +* 需要重新优化。 +* 11) 目前PDR算法在观测方向时，还是一定概率会被带偏，所以导致转弯 +* 转向不够，然而又不够相信GPS方向，考虑通过多次GPS方向角方差不大时 +* 调整GPS方向角的噪声策略。 +* 12) kml支持时间信息，可以拖动进度条比较轨迹同时运行的情况 ---- logzhan finish +* +* 二、调整规划为： + 第一期：手持稳定优化 + 摆手(平滑模块主要生效) + 多圈优化 => 不会比之前效果差，部分场景提升 + 第二期：手持稳定优化 + 摆手(PCA + 平滑) + 多圈 => 在摆手场景优化，覆盖一大半场景 + 第三期：手持稳定优化 + 摆手 + 任意动作 => 鲁棒性优化，覆盖全部使用场景 + 第四期：手持稳定优化 + 摆手 + 任意动作 + 多圈 + 真值仪 => 保证轨迹优化和方向优化条件下，精度提升，可以方便对外 +********************************************************************************/ +#include +#include +#include +#include +#include "pdr_api.h" +#include "Main.h" +#include "Utils.h" +#include "time.h" +using namespace std; + +/* Global Variable Definition ------------------------------------------------*/ +ResultTracks resTracks; +FILE* SimFile = NULL; +/* Extern Variable Definition ------------------------------------------------*/ +extern "C" PDR pdr; +extern "C" double refGpsYaw; + +/**---------------------------------------------------------------------- +* Function : main +* Description : pdr算法仿真用的main函数 +* Date : 2021/01/25 logzhan +*---------------------------------------------------------------------**/ +int main(int argc, char* argv[]) +{ + + string logPath = "E:\\SoftwareProject\\GnssIns\\GnssIns-PDR-Private\\1.Software\\PDR 1.06\\Release\\"; + string logDate = "shenzhen"; + string logLabel = "data\\"; + string fileHead; + char line[256] = { 0 }; + + // 解析命令行, 用于解析参数 + for (int i = 0; i < argc; i++) { + string s = argv[i]; + if(i == 1)logPath = s; + if(i == 2)logDate = s; + } + + clock_t t = clock(); + // 获取kml输出路径 + string kmlPath = logPath + logDate + "\\output\\"; + // 仿真文件夹路径和Catalog.txt路径 + string filePath = logPath + logDate + "\\" + logLabel + "\\"; + string catalogPath = filePath + "catalog.txt"; + + FILE* catalogFp = fopen(catalogPath.c_str(), "r"); + if (catalogFp == NULL){ + printf("open catalog.txt file %s error %s\n", catalogPath.c_str(), + strerror(errno)); + system("pause"); + return -1; + } + + while (!feof(catalogFp)) + { + FILE* fileFp = getSimulateFile(catalogFp, filePath, fileHead); + // PDR 仿真流程 + Algorithm_Init(); + + LctFs_t locFusion; + memset(&locFusion,0, sizeof(locFusion)); + memset(&resTracks, 0, sizeof(resTracks)); + + // 解析文本仿真 + while (fgets(line, 256, fileFp)){ + // 解析文本数据仿真 + if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){ + // 更新结果轨迹，用于最后的kml生成 + UpdateResTrack(resTracks, locFusion); + } + } + // 关闭PDR算法，释放资源 + Algorithm_DeInit(); + // 写结果kml + KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1c")); + // 关闭文件 + fclose(fileFp); + } + // 打印仿真时间信息 + printf("PDR simulate spends %f s\n", ((double)(clock() - t) / CLOCKS_PER_SEC)); + system("pause"); + return 0; +} + +/**---------------------------------------------------------------------- +* Function : getSimulateFile +* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针 +* catalogFp : 目录文件的文件指针 +* path_file ：仿真文件的文件目录 +* fileHead : 通过C++引用返回的去除了扩展名的文件名称 +* Date : 2021/01/25 logzhan +*---------------------------------------------------------------------**/ +FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead) { + + char file_name[256] = { 0 }; + FILE* fp = NULL; + + if (fscanf(catalogFp, "%s\n", file_name) == -1){ + printf("get data file name failed\n"); + system("pause"); + exit(-1); + } + string fileNamePath = path_file + string(file_name); + fp = fopen(fileNamePath.c_str(), "rb"); + if (fp == NULL){ + printf("open file %s failed error %s\n", fileNamePath.c_str(), strerror(errno)); + system("pause"); + exit(-1); + } + // 去除文件扩展名.csv + fileHead = file_name; + fileHead = fileHead.substr(0, fileHead.length() - 4); + + printf("%s\n", fileNamePath.c_str()); + return fp; +} + +/**---------------------------------------------------------------------- +* Function : gpsYaw2GoogleYaw +* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换 +* 为谷歌支持的Yaw规则 +* Date : 2021/01/25 logzhan +*---------------------------------------------------------------------**/ +double gpsYaw2GoogleYaw(double heading) { + double gh = R2D(heading) - 180; + while (gh > 360)gh -= 360; + while (gh < 0)gh += 360; + return gh; +} + +/**---------------------------------------------------------------------- +* Function : UpdateResTrack +* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs) +{ + resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude; + resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal; + resTrack.pdrTrack[resTrack.pdrLen].heading = + gpsYaw2GoogleYaw(lctfs.pdrHeading); + resTrack.pdrTrack[resTrack.pdrLen].hdop = lctfs.hdop; + resTrack.pdrTrack[resTrack.pdrLen].accuracy = lctfs.accuracy; + resTrack.pdrTrack[resTrack.pdrLen].time = lctfs.time; + resTrack.pdrTrack[resTrack.pdrLen].motionType = lctfs.motionType; + resTrack.pdrLen++; + + if (lctfs.gpsLat > 0 && lctfs.gpsLon > 0) { + resTrack.gpsTrack[resTrack.gpsLen].lat = lctfs.gpsLat; + resTrack.gpsTrack[resTrack.gpsLen].lon = lctfs.gpsLon; + resTrack.gpsTrack[resTrack.gpsLen].heading = + gpsYaw2GoogleYaw(lctfs.gpsHeading); + + resTrack.gpsTrack[resTrack.gpsLen].vel = lctfs.gpsSpeed; + resTrack.gpsTrack[resTrack.gpsLen].hdop = lctfs.hdop; + resTrack.gpsTrack[resTrack.gpsLen].accuracy = lctfs.accuracy; + resTrack.gpsTrack[resTrack.gpsLen].time = lctfs.time; + resTrack.gpsLen++; + } +} + +string time2str(double t) { + long time = (long)t; + if (t < 10) { + string str = "0" + std::to_string(time); + return str; + }else { + string str = std::to_string(time); + return str; + } +} + +/**---------------------------------------------------------------------- +* Function : KmlWrite +* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式 +* path : kml文件的输出文件路径 +* name : kml文件主体名称 +* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本 +* Date : 2020/11/1 logzhan +*---------------------------------------------------------------------**/ +void KmlWrite(string path, string name, string postfix) +{ + string pdrColor = "ff0000ff"; + string gpsColor = "ff00ffff"; + + if (path == "") { + return; + } + + string kmlPath = path + name + postfix + ".kml"; + string kmlName = name + postfix; + FILE* fid = fopen(kmlPath.c_str(), "w"); + + fprintf(fid, "\n"); + fprintf(fid, "\n"); + fprintf(fid, "\t\n"); + fprintf(fid, "\t\t%s\n", kmlName.c_str()); + + fprintf(fid, "\t\t\n"); + fprintf(fid, "\t\tGPS Fixs\n"); + + // 写gps结果 + for (int i = 0; i < resTracks.gpsLen; i++) { + fprintf(fid, "\t\t\n"); + fprintf(fid, "\t\t\t\n"); + + // 写入时间信息 + double h, m, s; + pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s); + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\t\t2021-01-22T%s:%s:%s\n", time2str(h).c_str(), + time2str(m).c_str(), time2str(s).c_str()); + fprintf(fid, "\t\t\t\n"); + + // 写入描述信息 + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\t\t\n"); + fprintf(fid, "\t\t\t\t
East : 0.0 (m/s)
\n"); + fprintf(fid, "\t\t\t\t
HDOP : %.2f (m/s)
\n", + resTracks.gpsTrack[i].hdop); + fprintf(fid, "\t\t\t\t
accuracy : %.2f (m)
\n", + resTracks.gpsTrack[i].accuracy); + fprintf(fid, "\t\t\t\t
speed : %.2f (m/s)
\n", + resTracks.gpsTrack[i].vel); + fprintf(fid, "\t\t\t\t
Heading : %.2f (degrees)
\n", + resTracks.gpsTrack[i].heading); + fprintf(fid, "\t\t\t\t
]]>\n"); + fprintf(fid, "\t\t\t\n"); + // 写入坐标信息 + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\t\t%.10f,%.10f\n", + resTracks.gpsTrack[i].lon, + resTracks.gpsTrack[i].lat); + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\n"); + } + fprintf(fid, "\t\t\n"); + + fprintf(fid, "\t\t\n"); + fprintf(fid, "\t\tPDR Fixs\n"); + // 写pdr结果 + for (int i = 0; i < resTracks.pdrLen; i++) { + fprintf(fid, "\t\t\n"); + fprintf(fid, "\t\t\t\n"); + + // 写入时间信息 + double h, m, s; + pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s); + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\t\t2021-01-22T%s:%s:%s\n", time2str(h).c_str(), + time2str(m).c_str(), time2str(s).c_str()); + fprintf(fid, "\t\t\t\n"); + + // description + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\t\t\n"); + fprintf(fid, "\t\t\t\t
East : 0.0 (m/s)
\n"); + fprintf(fid, "\t\t\t\t
HDOP : %.2f (m/s)
\n", + resTracks.pdrTrack[i].hdop); + + fprintf(fid, "\t\t\t\t
MOTION TYPE : %s (m/s)
\n", + Motion2TypeStr(resTracks.pdrTrack[i].motionType)); + + fprintf(fid, "\t\t\t\t
accuracy : %.2f (m/s)
\n", + resTracks.pdrTrack[i].accuracy); + fprintf(fid, "\t\t\t\t
Heading : %.2f (degrees)
\n", + resTracks.pdrTrack[i].heading); + fprintf(fid, "\t\t\t\t
]]>\n"); + fprintf(fid, "\t\t\t\n"); + + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\t\t%.10f,%.10f\n", + resTracks.pdrTrack[i].lon, + resTracks.pdrTrack[i].lat); + fprintf(fid, "\t\t\t\n"); + fprintf(fid, "\t\t\n"); + } + fprintf(fid, "\t\t\n"); + + fprintf(fid, "\t\n"); + fprintf(fid, "\n"); + + fclose(fid); +} + + +#ifdef _WIN32 +/**---------------------------------------------------------------------- +* Function : PDR_Init +* Description : 初始化PDR算法(DLL调用) +* Date : 2022/09/15 logzhan +*---------------------------------------------------------------------**/ +void LibPDR_Init() { + Algorithm_Init(); +} + +/**---------------------------------------------------------------------- +* Function : PDR_StepUpdateGPS +* Description : 仿真器按照GPS单步执行(DLL调用) +* Date : 2022/9/16 logzhan +*---------------------------------------------------------------------**/ +PosFusion LibPDR_StepUpdateGPS(int useGpsFlg) +{ + PosFusion fusion; + fusion.vaild = 0; + + Nmea_t location_nmea; + Nmea_t* ln = &location_nmea; + imu imu_data; + LctFs_t lct_fusion; + memset(&lct_fusion, 0, sizeof(lct_fusion)); + //memset(&location_nmea, 0, sizeof(location_nmea)); + memset(&imu_data, 0, sizeof(imu_data)); + /*memset(&kf, 0, sizeof(kf));*/ + char line[256] = { 0 }; + while (fgets(line, 256, SimFile)) { + + int flag = ParseDataAndUpdate(line, &lct_fusion); + + if (flag) { + + fusion.gpsLat = lct_fusion.gpsLat; + fusion.gpsLon = lct_fusion.gpsLon; + } + + // 写PDR融合结果 + if (flag) { + fusion.lat = lct_fusion.latitude; + fusion.lon = lct_fusion.longitudinal; + + fusion.t = lct_fusion.time; + fusion.vaild = 1; + + break; + } + } + return fusion; +} + +/**---------------------------------------------------------------------- +* Function : setSimulatorFileCsvPath +* Description : 设置仿真文件的路径(DLL调用) +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +void setSimulatorFileCsvPath(char* path) { + if (SimFile == NULL) { + SimFile = fopen(path, "rb"); + } + else { + fclose(SimFile); + SimFile = NULL; + } +} + +/**---------------------------------------------------------------------- +* Function : setRefGpsYaw +* Description : 设置GPS参考角度 +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +void setRefGpsYaw() { + refGpsYaw = pdr.GpsHeading; + printf("ref Gps Yaw = %f\n", refGpsYaw); + //g_pdr.sysStatus = IS_INITIAL; + //g_pdr.fusionPdrFlg = ON; +} + +/**---------------------------------------------------------------------- +* Function : closePdrAlgo +* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用) +* Date : 2020/8/3 logzhan +*---------------------------------------------------------------------**/ +void LibPDR_DeInit() { + Algorithm_DeInit(); + + if (SimFile != NULL) { + fclose(SimFile); + SimFile = NULL; + } +} +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Matrix.c b/1.Software/PDR 1.06/src/Matrix.c new file mode 100644 index 0000000..2f255f8 --- /dev/null +++ b/1.Software/PDR 1.06/src/Matrix.c @@ -0,0 +1,195 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : matrix.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.09.14 +* Comments : PDR矩阵运算库支持 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include "Matrix.h" + +/**--------------------------------------------------------------------- +* Function : MatrixTrans +* Description : 矩阵转置 +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixTrans(double a[N][N], double r[N][N]) { + int i, j; + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + r[i][j] = a[j][i]; + } + } +} + +/**--------------------------------------------------------------------- +* Function : VecMatMultiply +* Description : 向量和矩阵相乘 r = b * a +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void VecMatMultiply(double a[N], double b[N][N], double r[N]) { + int i, j; + double temp[N] = { 0.0 }; + //for (i = 0; i < N; i++) { + // temp[i] = a[i]; + //} + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + temp[i] += ((b[i][j] * 100) * (a[j] * 100)) / 10000; + } + } + for (i = 0; i < N; i++) { + r[i] = temp[i]; + } + +} + +/**--------------------------------------------------------------------- +* Function : MatrixMultiply +* Description : 矩阵和矩阵相乘 r = a * b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]) { + int i, j, m; + double temp[N][N] = { { 0.0 } }; + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + for (m = 0; m < N; m++) { + //temp[i][j] += a[j][m] * b[m][j]; + temp[i][j] += a[i][m] * b[m][j]; + } + } + } + + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + r[i][j] = temp[i][j]; + } + } +} + +/**--------------------------------------------------------------------- +* Function : MatrixAdd +* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]) { + int i, j; + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + r[i][j] = a[i][j] + b[i][j]; + } + } +} + + +/**--------------------------------------------------------------------- +* Function : VectorAdd +* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void VectorAdd(double a[N], double b[N], double r[N]) { + int i; + for (i = 0; i < N; i++) { + r[i] = a[i] + b[i]; + } +} + +/**--------------------------------------------------------------------- +* Function : MatrixSub +* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixSub(double a[N][N], double b[N][N], double r[N][N]) { + int i, j; + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + r[i][j] = a[i][j] - b[i][j]; + } + } +} +/**--------------------------------------------------------------------- +* Function : VectorSub +* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void VectorSub(double a[N], double b[N], double r[N]) { + int i; + for (i = 0; i < N; i++) { + r[i] = a[i] - b[i]; + } +} + +/**--------------------------------------------------------------------- +* Function : MatrixInverse +* Description : 求矩阵的逆矩阵 +* Date : 2022/09/14 logzhan +*---------------------------------------------------------------------**/ +void MatrixInverse(double(*a)[N], double (*a_inv)[N]) { + + double l[N][N] = { { 0.0 } }; + double u[N][N] = { { 0.0 } }; + double l_inv[N][N] = { { 0.0 } }; + double u_inv[N][N] = { { 0.0 } }; + double temp[N][N] = { { 0.0 } }; + + int i, j, k; + double s; + + for (i = 0; i < N; i++)l[i][i] = 1; + + for (i = 0; i < N; i++){ + for (j = i; j = 0; j--) + { + s = 0; + for (k = j + 1; k <= i; k++) + { + s += u[j][k] * u_inv[k][i]; + } + u_inv[j][i] = -s / u[j][j]; + } + } + for (i = 0; i < N; i++){ + for (j = 0; j < N; j++){ + for (k = 0; k < N; k++){ + temp[i][j] += u_inv[i][k] * l_inv[k][j]; + } + } + } + for (i = 0; i < N; i++) { + for (j = 0; j < N; j++) { + a_inv[i][j] = temp[i][j]; + } + } +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Munkres.c b/1.Software/PDR 1.06/src/Munkres.c new file mode 100644 index 0000000..4a0c206 --- /dev/null +++ b/1.Software/PDR 1.06/src/Munkres.c @@ -0,0 +1,295 @@ + +/* Header File Including ------------------------------------------------------------------------ */ +#include +#include +#include "m_munkres.h" + +/* Macro Definition ----------------------------------------------------------------------------- */ +#define COST(i, j) cost[(i) + (j) * m] +#define MARK(i, j) mark[(i) + (j) * m] +#define PATH(i, j) path[(i) + (j) * 64] +#define CLR_ALL(flag) (*flag##_cover = 0) +#define SET_FLG(flag, index) (*flag##_cover |= (1 << (index))) +#define CLR_FLG(flag, index) (*flag##_cover &= ~(1 << (index))) +#define TST_FLG(flag, index) (*flag##_cover & (1 << (index))) + +/* Global Variable Definition ------------------------------------------------------------------- */ +static float g_cost[64]; +static int g_mark[64]; +static uint8_t g_row_cover; +static uint8_t g_col_cover; + + +/* Function Declaration ------------------------------------------------------------------------- */ +static int step_one(float *cost, int m, int n); +static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover); +static int step_three(int *mark, int m, int n, uint8_t *col_cover); +static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j); +static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j); +static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover); + + +/* Function Definition -------------------------------------------------------------------------- */ +int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) { + int i = 0; + int j = 0; + int step; + + + if (m > 8 || n > 8) { + return MUNKRES_EXCESSIVE_MATRIX; + } + + memcpy(g_cost, cost, m * n * sizeof(*g_cost)); + memset(g_mark, 0, m * n * sizeof(*g_mark)); + g_row_cover = 0; + g_col_cover = 0; + + step = 0; + #if PRINT_ALGO_RUN_TIME + time_algo_1 = uTimetick_Get(); + #endif + while (7 != step) { + + switch (step) { + case 0: + case 1: + step = step_one(g_cost, m, n); + break; + case 2: + step = step_two(g_cost, g_mark, m, n, &g_col_cover); + break; + case 3: + step = step_three(g_mark, m, n, &g_col_cover); + break; + case 4: + step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j); + + break; + case 5: + step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j); + break; + case 6: + step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover); + break; + default: + step = 7; + break; + } + } + + for (i = 0; i < m; ++i) { + for (j = 0; j < n; ++j) { + if (0 != g_mark[(i) + (j) * m]) { + assignment[i] = j; + break; + } + } + } + + return MUNKRES_NO_ERROR; +} + +static int step_one(float *cost, int m, int n) { + int i; + int j; + float min; + for (i = 0; i < m; ++i) { + min = COST(i, 0); + for (j = 1; j < n; ++j) { + if (min > COST(i, j)) { + min = COST(i, j); + } + } + for (j = 0; j < n; ++j) { + COST(i, j) -= min; + } + } + + return 2; +} + +static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover) { + int i; + int j; + + for (i = 0; i < m; ++i) { + for (j = 0; j < n; ++j) { + if (0 == COST(i, j) && 0 == TST_FLG(col, j)) { + MARK(i, j) = 1; + SET_FLG(col, j); + break; + } + } + } + + CLR_ALL(col); + + return 3; +} + +static int step_three(int *mark, int m, int n, uint8_t *col_cover) { + int i; + int j; + int count; + + CLR_ALL(col); + for (i = 0; i < m; ++i) { + for (j = 0; j < n; ++j) { + if (1 == MARK(i, j)) { + SET_FLG(col, j); + } + } + } + + count = 0; + for (j = 0; j < n; ++j) { + count += (0 != TST_FLG(col, j) ? 1 : 0); + } + + return (count >= m || count >= n) ? 7 : 4; +} + +static void find_a_noncovered_zero(int *i, int *j, float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) { + for (*i = 0; *i < m; ++*i) { + if (0 != TST_FLG(row, *i)) { + continue; + } + for (*j = 0; *j < n; ++*j) { + if (0 == COST(*i, *j) && 0 == TST_FLG(col, *j)) { + break; + } + } + if (n != *j) { + break; + } + } +} + +static void find_a_star_zero_in_row(int *j, int i, int *mark, int m, int n) { + for (*j = 0; *j < n; ++*j) { + if (1 == MARK(i, *j)) { + break; + } + } +} + +static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j) { + int i; + int j; + int jj; + while (1) { + find_a_noncovered_zero(&i, &j, cost, m, n, row_cover, col_cover); + if (m == i) { + return 6; + } + else { + MARK(i, j) = 2; + find_a_star_zero_in_row(&jj, i, mark, m, n); + if (m != jj) { + SET_FLG(row, i); + CLR_FLG(col, jj); + j = jj; + } + else { + *path_start_i = i; + *path_start_j = j; + return 5; + } + } + } +} + +static void find_a_star_zero_in_col(int *i, int j, int *mark, int m) { + for (*i = 0; *i < m; ++*i) { + if (1 == MARK(*i, j)) { + break; + } + } +} + +static void find_a_prime_zero_in_row(int *j, int i, int *mark, int m, int n) { + for (*j = 0; *j < n; ++*j) { + if (2 == MARK(i, *j)) { + break; + } + } +} + +static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j) { + static int path[64 * 2]; + int i; + int j; + int k; + + k = 0; + PATH(k, 0) = path_start_i; + PATH(k, 1) = path_start_j; + while (1) { + find_a_star_zero_in_col(&i, PATH(k, 1), mark, m); + if (m == i) { + goto STEP_FIVE_EXIT; + } + else { + PATH(k + 1, 0) = i; + PATH(k + 1, 1) = PATH(k, 1); + ++k; + find_a_prime_zero_in_row(&j, PATH(k, 0), mark, m, n); + PATH(k + 1, 0) = PATH(k, 0); + PATH(k + 1, 1) = j; + ++k; + } + } + +STEP_FIVE_EXIT: + /* augment path */ + for(; k >= 0; --k) { + MARK(PATH(k, 0), PATH(k, 1)) = (1 == MARK(PATH(k, 0), PATH(k, 1)) ? 0 : 1); + } + /* clear covers */ + CLR_ALL(row); + CLR_ALL(col); + /* erease primes */ + for (i = 0; i < m; ++i) { + for (j = 0; j < n; ++j) { + if (2 == MARK(i, j)) { + MARK(i, j) = 0; + break; + } + } + } + + return 3; +} + +static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) { + float min; + int i; + int j; + + min = 1e6; + for (i = 0; i < m; ++i) { + for (j = 0; j < n; ++j) { + if (0 == TST_FLG(row, i) && 0 == TST_FLG(col, j)) { + if (min > COST(i, j)) { + min = COST(i, j); + } + } + } + } + + for (i = 0; i < m; ++i) { + for (j = 0; j < n; ++j) { + if (0 != TST_FLG(row, i)) { + COST(i, j) += min; + } + if (0 == TST_FLG(col, j)) { + COST(i, j) -= min; + } + } + } + + return 4; +} + + diff --git a/1.Software/PDR 1.06/src/PDRBase.c b/1.Software/PDR 1.06/src/PDRBase.c new file mode 100644 index 0000000..7b1c107 --- /dev/null +++ b/1.Software/PDR 1.06/src/PDRBase.c @@ -0,0 +1,265 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : PDRBase.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR基础功能函数 +********************************************************************************/ +#include +#include "PDRBase.h" +#include "Location.h" +#include "LinearFit.h" +#include "Utils.h" +#include "CoordTrans.h" +#include "Kalman.h" + +extern double GpsPos[HIST_GPS_NUM][3]; + +int PredictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel) +{ + int step = 0; + float mean_time = 400; + double dis = 0; + + if (stepPredict->meanTime > 0) + mean_time = stepPredict->meanTime; + + if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0)) + { + stepPredict->fsum = stepPredict->fsum / stepPredict->fnum; + dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0] - GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2) + + pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2)); + + if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0))) + { + step = (int)((timestamp - stepPredict->lastTime) / mean_time); + stepPredict->deltaStep += step; + } + + stepPredict->lastTime = timestamp; + stepPredict->fnum = 0; + stepPredict->fsum = 0; + } + + return step; +} + + +double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) +{ + int i; + double lla[3] = { 0.0 }; + double ned1[3] = { 0.0 }; + double temp[2][HIST_GPS_NUM] = { { 0.0 } }; + double angle[HIST_GPS_NUM - 1] = { 0.0 }; + + double ned2[3] = { 0.0 }; + double maxn = 0; + double maxe = 0; + double minn = 0; + double mine = 0; + double meanx = 0; + double meany = 0; + + float error = 0; + float sst = 0; + float r2 = 0; + + double range = 0; + double yaw = 0; + double tempYaw = 0; + double diffAngle = 0; + + double para[3] = { 0 }; + int flg = 0; + + lla[0] = pgnss.lat; + lla[1] = pgnss.lon; + + for (i = 0; i < HIST_GPS_NUM - 1; i++) + { + gpsPos[i][0] = gpsPos[i + 1][0]; + gpsPos[i][1] = gpsPos[i + 1][1]; + gpsPos[i][2] = gpsPos[i + 1][2]; + } + // 经纬度WGS84转地心地固坐标系ECEF + if ((lla[0] > 0) && (lla[1] > 0)){ + WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]); + }else{ + gpsPos[HIST_GPS_NUM - 1][0] = 0; + gpsPos[HIST_GPS_NUM - 1][1] = 0; + gpsPos[HIST_GPS_NUM - 1][2] = 0; + return -1; + } + + if (pgnss.error > 40){ + return -1; + } + + for (i = 0; i < HIST_GPS_NUM - 1; i++) + { + if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6)) + { + return -1; + } + } + //地心地固坐标系ECEF转东北天坐标系NED + ECEF2NED(gpsPos[HIST_GPS_NUM - 1], lla, ned1); + + for (i = HIST_GPS_NUM - 2; i >= 0; i--) { + + ECEF2NED(gpsPos[i], lla, ned2); + + temp[0][i] = ned1[0] - ned2[0]; + temp[1][i] = ned1[1] - ned2[1]; + + meanx += temp[0][i] / HIST_GPS_NUM; + meany += temp[1][i] / HIST_GPS_NUM; + + if (maxn > temp[0][i]) + { + maxn = temp[0][i]; + } + if (minn < temp[0][i]) + { + minn = temp[0][i]; + } + if (maxe > temp[1][i]) + { + maxe = temp[1][i]; + } + if (mine < temp[1][i]) + { + mine = temp[1][i]; + } + + angle[i] = atan2(temp[1][i], temp[0][i]); + if (angle[i] < 0) + angle[i] += TWO_PI; + } + flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para); + if (flg < 0) + return -1; + + for (i = HIST_GPS_NUM - 1; i >= 0; i--) { + error += (float)(pow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (pow(para[0], 2) + pow(para[1], 2))); + sst += (float)(pow(meanx - temp[0][i], 2) + pow(meany - temp[1][i], 2)); + } + + if (sst > 0) + { + r2 = 1 - error / sst; + } + range = sqrt(pow(maxn - minn, 2) + pow(maxe - mine, 2)); + //printf("%f,%f,%f\n", r2, error, range); + if (r2 < 0.95 || range < 1.5 || error > 3.0) + return -1; + + yaw = atan(-para[0] / para[1]); + modAngle(&yaw, 0, TWO_PI); + + tempYaw = meanAngle(angle, HIST_GPS_NUM - 1); + modAngle(&tempYaw, 0, TWO_PI); + + diffAngle = tempYaw - yaw; + modAngle(&diffAngle, -PI, PI); + + if (fabs(diffAngle) > PI / 2) + yaw += PI; + modAngle(&yaw, 0, TWO_PI); + + return yaw; +} + +/**---------------------------------------------------------------------- +* Function : StateRecognition +* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +void StateRecognition(float* acc, Classifer_t* sys) { + +} + + + +void calStepLastTime(StepPredict* stepPredict, double timestamp, unsigned long steps_last, + unsigned long steps) +{ + float tmpTime = 0; + if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0) + { + tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last)); + if (tmpTime > 300 && tmpTime < 800) + { + if (stepPredict->meanTime > 0) { + stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f; + } + else { + stepPredict->meanTime = tmpTime; + } + } + } + + stepPredict->fnum = 0; + stepPredict->fsum = 0; + + stepPredict->lastTime = timestamp; + stepPredict->deltaStep = 0; + +} + +/**---------------------------------------------------------------------- +* Function : InsLocationPredict +* Description : PDR惯导位置更新 +* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 +* 会造成PDR前后位置不够均匀 +*---------------------------------------------------------------------**/ +void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu, + GNSS_t* pgnss, EKFPara_t* kf) { + int step; + long deltaStep; + double stepLength = 0.7; + + stepPredict->fnum++; + stepPredict->fsum += pdr->motionFreq; + + ulong steps = pdr->steps; + ulong lastSteps = pdr->lastSteps; + + if (steps - lastSteps > 0 && pdr->motionFreq != 0.0) { + deltaStep = steps - lastSteps - stepPredict->deltaStep; + + // 应该是防止计步器异常情况，一次计步特别多 + if (deltaStep > 5)deltaStep = 5; + + if (stepPredict->meanTime > 0) + { + step = (int)((imu->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5); + + if (step > deltaStep && step <= 3)deltaStep = step; + } + if (lastSteps == 0)deltaStep = 0; + + if (pdr->heading > 0) { + EKFStatePredict(kf, stepLength, pdr, deltaStep); + pdr->fusionPdrFlg = PDR_TRUE; + } + calStepLastTime(stepPredict, imu->gyr.time, lastSteps, steps); + } + else { + // 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到 + // 惯导更新位置的目的 + int PreStep = PredictStep(stepPredict, imu->gyr.time, lastSteps, pgnss->vel); + if (pdr->heading > 0) { + EKFStatePredict(kf, stepLength, pdr, PreStep); + if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE; + } + } + + // 如果计步器可以保持较高的实时性，就可以采用下面的方法直接计算 + //if (pdr->heading > 0) { + // deltaStep = steps - lastSteps; + // EKFStatePredict(kf, stepLength, pdr, deltaStep); + // pdr->fusionPdrFlg = PDR_TRUE; + //} +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/ParseData.c b/1.Software/PDR 1.06/src/ParseData.c new file mode 100644 index 0000000..196b8f2 --- /dev/null +++ b/1.Software/PDR 1.06/src/ParseData.c @@ -0,0 +1,891 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_parseData.c +* Department : Sensor Algorithm Team +* Current Version : V1.0 +* Author : +* Date of Issued : 2021.01.29 +* Comments : GNSS数据解析，解析来自上层传入算法的字符串数据。主要是两种类 + 型。IMU字符串和NMEA字符串。 + IMU格式 : time_ms, type, data_x, data_y, data_z + GPS格式 ：time_ms, type, utc_time(second unit), $GPXXX,... + 其中time_ms是时间戳单位，时间单位要求是ms，不是ms需要转换为ms + type是传感器类型码，代表传入字符串所代表的传感器类型。 + NMEA协议一般是$G开头。NMEA协议一般会附带utc时间，单位是秒(s) +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include +#include +#include +#include +#include +#include "parseData.h" +#include "pdr_sensor.h" +#include "pdr_api.h" + +extern Sensor_t SensorDataHist[IMU_LAST_COUNT]; + + +char * strtok_ct(char * s, const char * delim) +{ + static char* lasts; + if (s == 0)s = lasts; + if (*s == '\0')return 0; + lasts = s + strcspn(s, delim); + if (*lasts != 0)*lasts++ = 0; + return s; +} + +/**---------------------------------------------------------------------- +* Function : Int2Hex +* Description : 十六进制数转换为十进制数 +* Date : 2020/7/9 yuanlin@vivo.com & logzhan +*---------------------------------------------------------------------**/ +char* Int2Hex(int a, char *buffer) +{ + sprintf(buffer, "%X", a); + return (buffer); +} + +/**---------------------------------------------------------------------- +* Function : HexToDec +* Description : 十六进制数转换为十进制数 +* Date : 2022-09-15 logzhan +*---------------------------------------------------------------------**/ +long HexToDec(char *source) +{ + long sum = 0, t = 1; + int len = (int)strlen(source); + for (int i = len - 1; i >= 0; i--) { + sum += t * pdr_getIndexOfSigns(*(source + i)); + t *= 16; + } + return sum; +} + +/**---------------------------------------------------------------------- +* Function : pdr_getIndexOfSigns +* Description : 返回ch字符在sign数组中的序号 +* Date : 2020/7/9 yuanlin@vivo.com &logzhan +*---------------------------------------------------------------------**/ +int pdr_getIndexOfSigns(char ch) +{ + if (ch >= '0' && ch <= '9')return ch - '0'; + if (ch >= 'A' && ch <= 'F')return ch - 'A' + 10; + if (ch >= 'a' && ch <= 'f')return ch - 'a' + 10; + return -1; +} + +void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp) +{ + int i = 0; + int j = 0; + Sensor_t data_xyz = { 0 }; + Sensor_t outputData = { 0 }; + data_xyz.update = 1; + while (pt && 3 > i) + { + if (0 == i) + { + data_xyz.s[0] = (float)atof(pt); + } + else if (1 == i) + { + data_xyz.s[1] = (float)atof(pt); + } + else if (2 == i) + { + data_xyz.s[2] = (float)atof(pt); + } + data_xyz.time = ts; + + pt = strtok_ct(NULL, ","); + i++; + } + +#if 0 + if (SENSOR_ACC == sstp) + { + memcpy(&imu_p->acc, &data_xyz, sizeof(sensor_agm)); + } + else if (SENSOR_MAG == sstp) + { + memcpy(&imu_p->mag, &data_xyz, sizeof(sensor_agm)); + } + else if (SENSOR_GYRO == sstp) + { + memcpy(&imu_p->gyro, &data_xyz, sizeof(sensor_agm)); + } +#else + data_xyz.type = sstp; + //Hist_SensorData第0位为时间上最靠前的数据 + if (SensorDataHist[0].time > data_xyz.time) + { + memcpy(&outputData, &data_xyz, sizeof(Sensor_t)); + } + else + { + memcpy(&outputData, &SensorDataHist[0], sizeof(Sensor_t)); + + for (i = IMU_LAST_COUNT - 1; i >= 0; i--) + { + if ((SensorDataHist[i].time <= data_xyz.time)) + { + for (j = 1; j <= i ; j++) + { + memcpy(&SensorDataHist[j - 1], &SensorDataHist[j], sizeof(Sensor_t)); + } + memcpy(&SensorDataHist[i], &data_xyz, sizeof(Sensor_t)); + break; + } + } + } + + if (SENSOR_ACC == outputData.type) + { + memcpy(&imu_p->acc, &outputData, sizeof(Sensor_t)); + } + else if (SENSOR_MAG == outputData.type) + { + memcpy(&imu_p->mag, &outputData, sizeof(Sensor_t)); + } + else if (SENSOR_GYRO == outputData.type) + { + memcpy(&imu_p->gyr, &outputData, sizeof(Sensor_t)); + } + +#endif +} + +void parseNMEA(char* pt, Nmea_t *ln, double ts) +{ + long i, result; + long s = 0 ; + char buffer[3] = {'0','0','\0'}; + char* line_p = pt + strlen(pt) + 1; + //char sumBuf[3] + if (strchr(line_p, '*') == NULL) + { + return; + } + + for (result = line_p[1], i = 2; line_p[i] != '*'; i++){ + result ^= line_p[i]; + } + + buffer[0] = line_p[i + 1]; + buffer[1] = line_p[i + 2]; + s = HexToDec(buffer); + + if (result == s) { // + pt = strtok_ct(NULL, ","); + /*printf("pt nmea l %s %u %u\n", pt,i, strcmp(pt, GNRMC_STR));*/ + if (strstr(pt, GGA_STR)) + { + ParseGGA(pt, ln, ts); + } + else if (strstr(pt, RMC_STR)) + { + ParseRMC(pt, ln, ts); + } + else if (strstr(pt, GSV_STR)) + { + parseGSV(pt, ln, ts); + } + else if (strstr(pt, GSA_STR)) + { + ParseGSA(pt, ln, ts); + } + //minTime：应用程序接收GPS更新的最短时间（即只有超过这个时间设定，系统才可能通知我们的程序说“GPS"——单位毫秒 + if ((ln->minTime < 1000) || (ln->minTime > ts)) + { + ln->minTime = ts; + } + + if ((ln->maxTime < 1000) || (ln->maxTime < ts)) + { + ln->maxTime = ts; + } + }// + else { + printf("unpass %s", line_p); + } +} + +void preprocessNMEA(Nmea_t *ln) +{ + if (ln->gga.update && (ln->rmc[0].update || ln->rmc[1].update) && (ln->gsa[0].update || ln->gsa[1].update || ln->gsa[2].update) + && (ln->gsv[0].update || ln->gsv[1].update || ln->gsv[2].update)) + { + ln->update = 1; + } + else + { + NmeaFlag_Init(ln); + } +} + +void ParseGGA(char* pt, Nmea_t *ln, double ts) +{ + if (!strcmp(pt, GNGGA_STR)) + { + ln->gga.type = SENSOR_LOCATION_NMEA_GNGGA; + ln->gga.update = 1; + } + else if (!strcmp(pt, GPGGA_STR)) + { + ln->gga.type = SENSOR_LOCATION_NMEA_GPGGA; + ln->gga.update = 1; + } + else + { + return; + } + int i = 0; + pt = strtok_ct(NULL, ","); + + while (pt && i < 13) + { + if (pt && '\0' == *pt) + { + if (0 == i) + { + ln->gga.gga_utc = ITEM_INVALID; + } + else if (1 == i) + { + ln->gga.latitude = ITEM_INVALID; + } + else if (2 == i) + { + ln->gga.gga_latitude_ns = LATITUDE_N; + } + else if (3 == i) + { + ln->gga.longitudinal = ITEM_INVALID; + + } + else if (4 == i) + { + ln->gga.longitudinal_ew = LATITUDE_N; + } + else if (5 == i) + { + ln->gga.status = invalid; + } + else if (6 == i) + { + ln->gga.satellites_nb = ITEM_INVALID; + } + else if (7 == i) + { + ln->gga.hdop = ITEM_INVALID; + } + else if (8 == i) + { + ln->gga.altitude = ITEM_INVALID; + } + else if (9 == i) + { + ln->gga.unit_a = ITEM_INVALID; + } + else if (10 == i) + { + ln->gga.height = ITEM_INVALID; + } + else if (11 == i) + { + ln->gga.unit_h = ITEM_INVALID; + + } + else if (12 == i) + { + ln->gga.dgps = ITEM_INVALID; + } + else if (13 == i) + { + ln->gga.gga_check_sum[0] = '\0'; + } + } + else + { + if (0 == i) + { + ln->gga.gga_utc = atof(pt); + } + else if (1 == i) + { + ln->gga.latitude = atof(pt); + } + else if (2 == i) + { + if (!strcmp(pt, NORTH)) + { + ln->gga.gga_latitude_ns = LATITUDE_N; + } + else if (!strcmp(pt, SOUTH)) + { + ln->gga.gga_latitude_ns = LATITUDE_S; + } + } + else if (3 == i) + { + ln->gga.longitudinal = atof(pt); + + } + else if (4 == i) + { + if (!strcmp(pt, EAST)) + { + ln->gga.longitudinal_ew = LONGITUDINAL_E; + } + else if (!strcmp(pt, WEST)) + { + ln->gga.longitudinal_ew = LONGITUDINAL_W; + } + } + else if (5 == i) + { + ln->gga.status = (gngga_quality)atoll(pt); + } + else if (6 == i) + { + ln->gga.satellites_nb = atoi(pt); + } + else if (7 == i) + { + ln->gga.hdop = (float)atof(pt); + } + else if (8 == i) + { + ln->gga.altitude = (float)atof(pt); + } + else if (9 == i) + { + if (!strcmp(pt, "M")) + { + ln->gga.unit_a = 1; + } + } + else if (10 == i) + { + ln->gga.height = (float)atof(pt); + } + else if (11 == i) + { + if (!strcmp(pt, "M")) + { + ln->gga.unit_h = 1; + } + + } + } + pt = strtok_ct(NULL, ","); + i++; + /*printf("pt %s %u\n", pt,i);*/ + } + ln->gga.time = ts; +} + +void ParseRMC(char* pt, Nmea_t *ln, double ts) +{ + int i = 0; + double temp = 0; + double rmc_time = 0; + NmeaRMC_t *rmc = NULL; + + if (!strcmp(pt, GNRMC_STR)) + { + rmc = &ln->rmc[0]; + RMC_Init(rmc); + rmc->type = SENSOR_LOCATION_NMEA_GNRMC; + rmc->update = 1; + } + else if (!strcmp(pt, GPRMC_STR)) + { + rmc = &ln->rmc[1]; + RMC_Init(rmc); + rmc->type = SENSOR_LOCATION_NMEA_GPRMC; + rmc->update = 1; + } + else + { + return; + } + + pt = strtok_ct(NULL, ","); + + while (pt && 12>i) + { + if (strlen(pt) > 0) + { + if (0 == i) + { + rmc_time = atof(pt); + rmc->rmc_utc = rmc_time; + } + else if (1 == i) + { + if (!strcmp(pt, "V")) + { + rmc->status = POSITIONING_N; + } + else if (!strcmp(pt, "A")) + { + rmc->status = POSITIONING_Y; + } + } + else if (2 == i) + { + temp = atof(pt); + rmc->latitude = (floor(temp / 100) + (temp - floor(temp / 100) * 100) / 60); + } + else if (3 == i) + { + if (!strcmp(pt, NORTH)) + { + rmc->rmc_latitude_ns = LATITUDE_N; + } + else if (!strcmp(pt, SOUTH)) + { + rmc->rmc_latitude_ns = LATITUDE_S; + } + } + else if (4 == i) + { + temp = atof(pt); + rmc->longitudinal = (floor(temp / 100) + (temp - floor(temp / 100) * 100) / 60); + } + else if (5 == i) + { + if (!strcmp(pt, EAST)) + { + rmc->longitudinal_ew = LONGITUDINAL_E; + } + else if (!strcmp(pt, WEST)) + { + rmc->longitudinal_ew = LONGITUDINAL_W; + } + } + else if (6 == i) + { + rmc->speed = (float)atof(pt); + } + else if (7 == i) + { + rmc->cog = (float)atof(pt); + } + else if (8 == i) + { + rmc->utc_ddmmyy = (float)atof(pt); + } + else if (9 == i) + { + rmc->magnetic = (float)atof(pt); + } + else if (10 == i) + { + if (!strcmp(pt, EAST)) + { + rmc->mag_h = MAG_E; + } + else if (!strcmp(pt, WEST)) + { + rmc->mag_h = MAG_W; + } + } + else if (11 == i) + { + if (!strcmp(pt, "A")) + { + rmc->mode = MODE_A; + } + else if (!strcmp(pt, "D")) + { + rmc->mode = MODE_D; + } + else if (!strcmp(pt, "E")) + { + rmc->mode = MODE_E; + } + else if (!strcmp(pt, "N")) + { + rmc->mode = MODE_N; + } + } + else if (12 == i) + { + } + } + pt = strtok_ct(NULL, ","); + i++; + } + rmc->time = ts; +} + +void parseGSV(char* pt, Nmea_t *ln, double ts) +{ + char *pt_gsv = NULL; + + //memset(&ln->gsv, ITEM_INVALID, sizeof(ln->gsv)); + int i = 0; + NmeaGSV_t* gsv = NULL; + if (!strcmp(pt, GPGSV_STR)) + { + gsv = &ln->gsv[0]; + gsv->type = SENSOR_LOCATION_NMEA_GPGSV; + } + else if (!strcmp(pt, GLGSV_STR)) + { + gsv = &ln->gsv[1]; + gsv->type = SENSOR_LOCATION_NMEA_GLGSV; + } + else if (!strcmp(pt, GAGSV_STR)) + { + gsv = &ln->gsv[2]; + gsv->type = SENSOR_LOCATION_NMEA_GAGSV; + } + else + { + return; + } + + /*printf("gsv %s %d\n", pt, gsv->type);*/ + /*printf("nmea rmc %s %u %u\n", pt, !strcmp(pt, GNRMC_STR), rmc->type);*/ + pt = strtok_ct(NULL, ","); + while (pt && 3>i) + { + if (0 == i) + { + gsv->sentences = atoi(pt); //total + } + else if (1 == i) + { + gsv->sentence_number = atoi(pt); // Which term of + if (1 == gsv->sentence_number) + { + gsv->satellite_number = 0; + } + } + else if (2 == i) + { + gsv->satellites = atoi(pt); //total + break; + } + + pt = strtok_ct(NULL, ","); + i++; + } + + pt_gsv = strtok_ct(NULL, "*"); + + pt = strtok_ct(pt_gsv, ","); + i = 0; + + while (pt && 16>i && gsv->satellites > 0) + { + if (pt && '\0' == *pt) + { + if (0 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].prn = ITEM_INVALID; + } + else if (1 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].elevation = ITEM_INVALID; + } + else if (2 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].azimuth = ITEM_INVALID; + } + else if (3 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].snr = ITEM_INVALID; + if (gsv->satellite_number < gsv->satellites) //satellite_number是第几个;satellites是总共的个数 + { + gsv->satellite_number++; + if (gsv->satellite_number == gsv->satellites) + { + break; + } + } + } + } + else + { + if (0 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].prn = atoi(pt); + if (gsv->type == SENSOR_LOCATION_NMEA_GPGSV && gsv->satellites_data[gsv->satellite_number].prn > 32) + { + gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 32) + 1; + } + if (gsv->type == SENSOR_LOCATION_NMEA_GLGSV && gsv->satellites_data[gsv->satellite_number].prn < 65 && gsv->satellites_data[gsv->satellite_number].prn > 99) + { + gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 35) + 65; + } + if (gsv->type == SENSOR_LOCATION_NMEA_GAGSV && gsv->satellites_data[gsv->satellite_number].prn > 36) + { + gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 36) + 1; + } + } + else if (1 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].elevation = atoi(pt); //仰角 + } + else if (2 == i % 4) + { + + gsv->satellites_data[gsv->satellite_number].azimuth = atoi(pt); + } + else if (3 == i % 4) + { + gsv->satellites_data[gsv->satellite_number].snr = atoi(pt); + // printf("2.gsv->satellites_data[gsv->satellite_number]: %d %d %d %d\n", gsv->satellites_data[gsv->satellite_number].prn, gsv->satellites_data[gsv->satellite_number].elevation, gsv->satellites_data[gsv->satellite_number].azimuth, gsv->satellites_data[gsv->satellite_number].snr); + if (gsv->satellite_numbersatellites) //satellite_number是第几个;satellites是总共的个数 + { + gsv->satellite_number++; + if (gsv->satellite_number == gsv->satellites) + { + break; + } + } + } + //printf("2.gsv->satellites_data[gsv->satellite_number]: %d %d %d %d\n", gsv->satellites_data[gsv->satellite_number].prn, gsv->satellites_data[gsv->satellite_number].elevation, gsv->satellites_data[gsv->satellite_number].azimuth, gsv->satellites_data[gsv->satellite_number].snr); + } + + pt = strtok_ct(NULL, ","); + i++; + } + if (3 == i % 4 && !pt) + { + gsv->satellites_data[gsv->satellite_number].snr = ITEM_INVALID; + if (gsv->satellite_numbersatellites) + { + gsv->satellite_number++; + } + } + + if (gsv->satellites == gsv->satellite_number) + { + gsv->update = 1; + //printf_nmea(gsv->type,ln,PrintLOG_file_name); + } + + gsv->time = ts; +} + +void ParseGSA(char* pt, Nmea_t *ln, double ts) +{ + int i = 0; + int idx = -1; + NmeaGSA_t gsa = {0}; + if (!strcmp(pt, GNGSA_STR)) + { + gsa.type = SENSOR_LOCATION_NMEA_GNGSA; + gsa.update = 1; + } + else if (!strcmp(pt, GPGSA_STR)) + { + idx = 0; + gsa.type = SENSOR_LOCATION_NMEA_GPGSA; + gsa.update = 1; + } + else if (!strcmp(pt, GLGSA_STR)) + { + idx = 1; + gsa.type = SENSOR_LOCATION_NMEA_GLGSA; + gsa.update = 1; + } + else if (!strcmp(pt, GAGSA_STR)) + { + idx = 2; + gsa.type = SENSOR_LOCATION_NMEA_GAGSA; + gsa.update = 1; + } + else + { + return; + } + + // printf("gsa %s %d\n", pt, ln->gsa.type); + pt = strtok_ct(NULL, ","); + /*printf("nmea gngsa %s %u\n", pt, i);*/ + while (pt && 17>=i) + { + /*printf("%u gsa pt %s\n", i, pt);*/ + if (strlen(pt) > 0) + { + if (0 == i) + { + if (!strcmp(pt, "A")) + { + gsa.mode = POSITIONING_MODE_A; + } + else if (!strcmp(pt, "M")) + { + gsa.mode = POSITIONING_MODE_M; + } + } + else if (1 == i) + { + gsa.p_type = (lct_type)atoll(pt); + } + else if (( i >= 2 ) && ( i <= 13 )) + { + gsa.prn[i - 2] = atoi(pt); + + if (idx == 0 && gsa.prn[i - 2] > 32) + { + gsa.prn[i - 2] = (gsa.prn[i - 2] % 32)+1; + } + if (idx == 1 && gsa.prn[i - 2] < 65 && gsa.prn[i - 2] > 99) + { + gsa.prn[i - 2] = (gsa.prn[i - 2] % 35) + 65; + } + if (idx == 2 && gsa.prn[i - 2] > 36) + { + gsa.prn[i - 2] = (gsa.prn[i - 2] % 36)+1; + } + if (gsa.prn[i - 2] > 64) + { + gsa.prnflg |= (1 << (gsa.prn[i - 2] - 64)); + if (idx < 0) + { + idx = 1; + } + } + else + { + gsa.prnflg |= (1 << gsa.prn[i - 2]); + if (idx < 0) + { + if (ln->gsa[0].update && ln->gsa[1].update) + { + idx = 2; + } + else + { + idx = 0; + } + } + } + }else if (i == 14){ + gsa.pdop = (float)atof(pt); + }else if (i == 15){ + gsa.hdop = (float)atof(pt); + }else if (i == 16){ + gsa.vdop = (float)atof(pt); + }else if (17 == i) + { + if (idx < 0 && pt[0] == '1') + { + idx = 0; + gsa.type = SENSOR_LOCATION_NMEA_GPGSA; + } + else if (idx < 0 && pt[0] == '2') + { + idx = 1; + gsa.type = SENSOR_LOCATION_NMEA_GLGSA; + } + else if (idx < 0 && pt[0] == '3') + { + idx = 2; + gsa.type = SENSOR_LOCATION_NMEA_GAGSA; + } + } + + } + + pt = strtok_ct(NULL, ","); + i++; + /*printf("pt %s %u\n", pt,i);*/ + } + + gsa.time = ts; + if (idx >=0 && idx <=2) + { + memcpy(&ln->gsa[idx], &gsa, sizeof(NmeaGSA_t)); + } + +} + +/**---------------------------------------------------------------------- +* Function : parseLocAccuracy +* Description : 解析GPS的Accuracy精度参数 +* Date : 2020/7/9 yuanlin@vivo.com &logzhan +*---------------------------------------------------------------------**/ +void parseLocAccuracy(char* pt, Nmea_t *ln, double ts) +{ + int i = 0; + while (pt && 8>i){ + if ((i == 0) && (!strcmp(pt, "Status"))){ + pt = strtok_ct(NULL, ","); + ln->accuracy.status = atoi(pt); + return; + } + if (i == 7){ + ln->accuracy.update = 1; + ln->accuracy.time = ts; + if ((pt && '\0' == *pt)) + { + ln->accuracy.err = -1; + } + else if (ln->accuracy.status == -1 || ln->accuracy.status == 2) + { + ln->accuracy.err = (float)atof(pt); + } + return; + } + pt = strtok_ct(NULL, ","); + i++; + } +} + +/**---------------------------------------------------------------------- +* Function : ParseLineStr +* Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析 +* Date : 2020/7/9 yuanlin@vivo.com &logzhan +*---------------------------------------------------------------------**/ +int ParseGnssInsData(char* line, IMU_t* imu_p, Nmea_t* ln) +{ + char* pt = NULL; + int i = 0, sstp = -1; + double ts = 0; + pt = strtok_ct(line, ","); + + while (pt && i < 2){ + if (i == 0)ts = atoll(pt) * 0.000001; + else if (i == 1)sstp = atoi(pt); + pt = strtok_ct(NULL, ","); + i++; + } + + switch (sstp) + { + case SENSOR_ACC: + case SENSOR_MAG: + case SENSOR_GYRO: + ParseIMU(pt, imu_p, ts, sstp); + break; + case SENSOR_LOCATION_STATELLITE: + break; + case SENSOR_LOCATION_NETWORK: + break; + case SENSOR_LOCATION_NMEA: + parseNMEA(pt, ln, ts); + if ((ln->maxTime - ln->minTime) > NAEA_LAST_TIME) + { + preprocessNMEA(ln); + } + break; + case SENSOR_LOCATION_FUSION: + parseLocAccuracy(pt, ln, ts); + break; + } + if ((ln->minTime > 0) && (ts - ln->minTime) > NAEA_LAST_TIME) { + preprocessNMEA(ln); + } + return 0; +} diff --git a/1.Software/PDR 1.06/src/Pedometer.c b/1.Software/PDR 1.06/src/Pedometer.c new file mode 100644 index 0000000..f728fa6 --- /dev/null +++ b/1.Software/PDR 1.06/src/Pedometer.c @@ -0,0 +1,1245 @@ +锘/******************** (C) COPYRIGHT 2023 Geek************************************ +* File Name : Pedometer.c +* Department : Geek Technology +* Current Version : V1.0.0 +* Author : zhanli +* Date of Issued : 2023.02.24 +* Comments : PDR璁℃鍣ㄧ浉鍏 +********************************************************************************/ +#include +#include +#include +#include +#include "Pedometer.h" + +const static char* Pedometer_Version = "Pedometer.1.0.0.20230224"; + +static void AccBufferUpdate(double ax, double ay, double az); +static void StateDetector(unsigned long long timeStamp); +static void Pedometer_Filter(void); +static void Steps_Feature(void); +static void Steps_Recording(unsigned long long timeStamp); + +static float DetVehicleAccBuff[DET_VEHICLE_ACC_BUFF_LEN] = { 0 }; +static float lastAccZ = 0; +static uint32_t ReceivedHowManyAcc = 0; +static double SumAcc = 0; + +static double DifFiltCoefficient1[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, + 0.234567901234568 }; + +static double DifFiltCoefficient2[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056, 0.0832, 0.1088, 0.128, 0.136 }; +static double DifFiltCoefficient3[13] = { 0.00041649312786339, 0.00166597251145356, 0.0041649312786339, 0.0083298625572678, + 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, + 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, + 0.0962099125364431 }; + +static double DifFiltCoefficient4[17] = { 0.000152415790275873, 0.00060966316110349, 0.00152415790275873, 0.00304831580551745, + 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, + 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, + 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, + 0.0745313214449017 }; + +static double DifFiltOutput[FILTER_AXIS_NUM][5]; +static double OutputFilt[FILTER_AXIS_NUM][2]; +static double StepLcVar[FILTER_AXIS_NUM][3]; // 浠呯敤浜庤姝ュ櫒鐗瑰緛鎻愬彇 + +static double AccBuffer[AXIS_NUM][ACC_BUFF_NUM]; // 缁忚繃闄嶉噰鏍峰拰5鐐瑰潎鍊兼护娉㈠悗鐨凙cc缂撳瓨,闀垮害涓39 + +// 璁℃鍣ㄧ壒寰 +static char StepReFeature[FILTER_AXIS_NUM] = { 0 }; +static double FeatureAmp[FILTER_AXIS_NUM][8]; // Acc鐨勬尟骞呯壒寰 +static uint16_t FeatureTime[FILTER_AXIS_NUM][8]; + +static double StepRecordeState[AXIS_ALL][4]; +static double StepRecordTime[AXIS_ALL][9]; // 璁板綍璁℃鍣ㄧ殑鐘舵佹椂闂达紝璁板綍娉㈠嘲鍜屾尝璋 +static uint64_t TimeStampSave[AXIS_ALL][2]; + +static uint16_t StepMaxTime = 100; +static uint16_t StepMinTime = 10; + +static uint64_t timeStamp = 0; +static uint64_t timeStamp_in_vehicle = 0; +static char Flag_whether_in_vehicle = 0; +static char Flag_step_exist = 0; +static double AvergeFiltSave[AXIS_NUM - 1][AVG_FILTER_BUFF_LEN]; +static double StateDetectLcVar[5] = { 0 }; +static StepsDefine_t Steps = { 0, 0 }; +static stepsRe ReSteps = { 0 }; +static DownSample_t DownSample = { 0, 0, 0, 0 }; +static StepState_t StateLcVar[2] = { NonSteps, NonSteps }; +stateNowDefine stateNow; +StateDef_t Restate; + +/**--------------------------------------------------------------------- +* Function : Pedometer_GetVersion +* Description : 鑾峰彇璁℃鍣ㄧ増鏈 +* Date : 2023/02/24 +*---------------------------------------------------------------------**/ +const char* Pedometer_GetVersion(void){ + return Pedometer_Version; +} + +/**--------------------------------------------------------------------- +* Function : initPedometer +* Description : PDR 鍒濆鍖栬姝ュ櫒 +* Date : 2020/2/20 logzhan +*---------------------------------------------------------------------**/ +void Pedometer_Init(void) +{ + STEPLIB_MEMSET(AccBuffer, 0, sizeof(AccBuffer)); + STEPLIB_MEMSET(DifFiltOutput, 0, sizeof(DifFiltOutput)); + STEPLIB_MEMSET(OutputFilt, 0, sizeof(OutputFilt)); + STEPLIB_MEMSET(StepLcVar, 0, sizeof(StepLcVar)); + STEPLIB_MEMSET(StepReFeature, 0, sizeof(StepReFeature)); + STEPLIB_MEMSET(FeatureAmp, 0, sizeof(FeatureAmp)); + STEPLIB_MEMSET(FeatureTime, 0, sizeof(FeatureTime)); + STEPLIB_MEMSET(StepRecordeState, 0, sizeof(StepRecordeState)); + STEPLIB_MEMSET(StepRecordTime, 0, sizeof(StepRecordTime)); + STEPLIB_MEMSET(TimeStampSave, 0, sizeof(TimeStampSave)); + STEPLIB_MEMSET(&DownSample, 0, sizeof(DownSample)); + SumAcc = 0; + timeStamp = 0; + STEPLIB_MEMSET(&Steps, 0, sizeof(Steps)); + STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps)); + STEPLIB_MEMSET(StateDetectLcVar, 0, sizeof(StateDetectLcVar)); + STEPLIB_MEMSET(StateLcVar, 0, sizeof(StateLcVar)); + STEPLIB_MEMSET(&stateNow, 0, sizeof(stateNow)); + STEPLIB_MEMSET(&Restate, 0, sizeof(Restate)); + STEPLIB_MEMSET(AvergeFiltSave, 0, sizeof(AvergeFiltSave)); + ReceivedHowManyAcc = 0; + STEPLIB_MEMSET(DetVehicleAccBuff, 0, sizeof(DetVehicleAccBuff)); + timeStamp_in_vehicle = 0; + Flag_whether_in_vehicle = 0; + Flag_step_exist = 0; + +} + +/**--------------------------------------------------------------------- +* Function : reinitPedometer +* Description : PDR 閲嶆柊鍒濆鍖栬姝ュ櫒 +* Date : 2020/2/20 logzhan +*---------------------------------------------------------------------**/ +void Pedometer_ReInit(void) +{ + timeStamp = 0; + STEPLIB_MEMSET(&Steps, 0, sizeof(Steps)); + STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps)); +} + +/**--------------------------------------------------------------------- +* Function : Pedometer_Update +* Description : PDR 璁℃鍣ㄧ殑鍔犻熷害杈撳叆, 骞堕氳繃鎸囬拡杩斿洖鐢ㄦ埛姝ユ暟 +* Date : 2023/02/25 logzhan +*---------------------------------------------------------------------**/ +void Pedometer_Update(IMU_t* imu, uint32_t* step) +{ + unsigned char k = 0; + + // 妫娴嬪綋鍓嶆槸鍚﹀睘浜庡紑杞︾姸鎬, 杩欎釜鍑芥暟涔熸槸鐩稿鑰楁椂鐨, 杩欎釜鍑芥暟鍙敤 + // 浜嗗姞閫熷害璁＄殑z杞, 鏄笉鏄瓨鍦ㄩ棶棰橈紵 + DetVehicleMode(imu->acc.s[0], imu->acc.s[1], imu->acc.s[2]); + + timeStamp += ACC_SAMPLE_TIME; + DownSample.ax += imu->acc.s[0] / 9.8; + DownSample.ay += imu->acc.s[1] / 9.8; + DownSample.az += imu->acc.s[2] / 9.8; + DownSample.flag++; + + if (DownSample.flag == DOWN_SAMPLE_NUM) + { + if (Restate.startTimeStamp == 0) + { + Restate.startTimeStamp = timeStamp; + } + // 闄嶉噰鏍风殑鏁版嵁鐢ㄥ洓涓偣鐨勫潎鍊兼护娉 + DownSample.ax /= DOWN_SAMPLE_NUM; + DownSample.ay /= DOWN_SAMPLE_NUM; + DownSample.az /= DOWN_SAMPLE_NUM; + + // 鍧囧兼护娉㈠櫒缂撳瓨鏇存柊锛屽緱鍒25Hz鐨凙cc缂撳瓨 + for (k = 0; k < AVG_FILTER_BUFF_LEN - 1; k++) + { + AvergeFiltSave[0][k] = AvergeFiltSave[0][k + 1]; + AvergeFiltSave[1][k] = AvergeFiltSave[1][k + 1]; + AvergeFiltSave[2][k] = AvergeFiltSave[2][k + 1]; + } + AvergeFiltSave[0][k] = DownSample.ax; + AvergeFiltSave[1][k] = DownSample.ay; + AvergeFiltSave[2][k] = DownSample.az; + + // 瀵瑰潎鍊兼护娉㈠櫒涓殑5涓偣鍐嶆眰鍧囧煎鐞嗭紝闄嶄綆鍣０ + STEPLIB_MEMSET(&DownSample, 0, sizeof(DownSample)); + for (k = 0; k < AVG_FILTER_BUFF_LEN; k++) + { + DownSample.ax += AvergeFiltSave[0][k]; + DownSample.ay += AvergeFiltSave[1][k]; + DownSample.az += AvergeFiltSave[2][k]; + } + DownSample.ax /= AVG_FILTER_BUFF_LEN; + DownSample.ay /= AVG_FILTER_BUFF_LEN; + DownSample.az /= AVG_FILTER_BUFF_LEN; + + // 璁℃鍣ㄧ紦瀛楤uffer鏇存柊 + AccBufferUpdate(DownSample.ax, DownSample.ay, DownSample.az); + // 璁℃鍣ㄧ姸鎬佹娴 + StateDetector(timeStamp); + + if (StateDetectLcVar[4] < 0.5l) + { + // 璁℃鍣ㄥ閫氶亾婊ゆ尝澶勭悊 + Pedometer_Filter(); + // 璁℃鍣ㄦ尝宄版尝璋蜂互鍙婃尟骞呬俊鎭彁鍙 + Steps_Feature(); + // 姝ユ暟璁板綍 + Steps_Recording(timeStamp); + } + // 閲嶇疆DownSample鏍囧織浣 + STEPLIB_MEMSET(&DownSample, 0, sizeof(DownSample)); + } + // 濡傛灉璁℃鏍囧織浣嶄笉瀛樺湪锛岄偅涔堜笉璁℃鏁 + if (Flag_step_exist == 0){ + // vehicle for debug + timeStamp = timeStamp + 1; + } + else if (llabs((long long)(timeStamp - timeStamp_in_vehicle)) > 500) + { + // 鏇存柊姝ユ暟 + *step = ReSteps.walkSteps; + Flag_step_exist = 1; + } +} + +/**--------------------------------------------------------------------- +* Function : detVehicleMode +* Description : PDR 妫娴嬫苯杞︽ā寮, 濡傛灉鏄娴嬪埌姹借溅妯″紡锛岄偅涔堜笉璁℃鏁 +* Date : 2020/2/1 logzhan +*---------------------------------------------------------------------**/ +void DetVehicleMode(double ax, double ay, double az) +{ + int t; + int index_move; + + int num_SumAcc_Ascend_Descend[DET_VEHICLE_ACC_BUFF_LEN / 2] = { 0 }; + int index_SumAcc_Ascend_Descend = 0; + int num_SumAcc_Ascend_Descend_occur = 0; + char flag_1_ascend_0_descend = 0; + + + if (ReceivedHowManyAcc < DET_VEHICLE_ACC_BUFF_LEN) + { + ReceivedHowManyAcc++; + DetVehicleAccBuff[ReceivedHowManyAcc - 1] = (float)az - lastAccZ; + lastAccZ = (float)az; + } + else + { + for (index_move = 0; index_move < DET_VEHICLE_ACC_BUFF_LEN - 1; index_move++) + { + DetVehicleAccBuff[index_move] = DetVehicleAccBuff[index_move + 1]; + } + + DetVehicleAccBuff[DET_VEHICLE_ACC_BUFF_LEN - 1] = (float)az - lastAccZ; + lastAccZ = (float)az; + } + + if (ReceivedHowManyAcc >= DET_VEHICLE_ACC_BUFF_LEN) + { + // The Number of Sign Change of First Order Difference,to find the window_length of window_min2max + + for (t = 1; t < WINDOW_LENGTH_MIN2MAX; t++) + { + if (DetVehicleAccBuff[t] > 0 && DetVehicleAccBuff[t - 1] < 0) + { + flag_1_ascend_0_descend = 1; + index_SumAcc_Ascend_Descend++; + } + else if (DetVehicleAccBuff[t - 1] > 0 && DetVehicleAccBuff[t] < 0) + { + flag_1_ascend_0_descend = 0; + index_SumAcc_Ascend_Descend++; + } + + if (flag_1_ascend_0_descend == 1) + { + num_SumAcc_Ascend_Descend[index_SumAcc_Ascend_Descend]++; + } + else if (flag_1_ascend_0_descend == 0) + { + num_SumAcc_Ascend_Descend[index_SumAcc_Ascend_Descend]--; + } + } + + + for (t = 0; t < DET_VEHICLE_ACC_BUFF_LEN / 2 - 2; t++) + { + index_move = abs(num_SumAcc_Ascend_Descend[t]); + + if (index_move >= 6) + { + num_SumAcc_Ascend_Descend_occur++; + if (num_SumAcc_Ascend_Descend_occur >= 3) + { + break; + } + } + } + + if (num_SumAcc_Ascend_Descend_occur < 3) + { + timeStamp_in_vehicle = timeStamp; + Flag_step_exist = 0; //in vehicle + } + else + { + Flag_step_exist = 1; //count step + } + } + else + { + Flag_step_exist = 1; // start 2 seconds + } + +} + +/**--------------------------------------------------------------------- +* Function : AccBufferUpdate +* Description : 鏇存柊Acc鐨凚uffer缂撳瓨 +* Date : 2023/02/25 +*---------------------------------------------------------------------**/ +static void AccBufferUpdate(double ax, double ay, double az) +{ + for (uint8_t i = 0; i < AXIS_NUM; i++){ + for (uint8_t j = 0; j < (ACC_BUFF_NUM - 1); j++){ + AccBuffer[i][j] = AccBuffer[i][j + 1]; + } + } + AccBuffer[0][ACC_BUFF_NUM - 1] = ax; + AccBuffer[1][ACC_BUFF_NUM - 1] = ay; + AccBuffer[2][ACC_BUFF_NUM - 1] = az; + AccBuffer[3][ACC_BUFF_NUM - 1] = sqrt(ax*ax + ay*ay + az*az); +} + +/**--------------------------------------------------------------------- +* Function : Pedometer_Filter +* Description : 璁℃鍣ㄦ护娉㈠櫒,涓鍏辨湁4涓娈电殑浣庨氭护娉㈠櫒銆傞拡瀵箈,y,z,mod(x,y,z) +* 鐨勬暟鎹繘琛屾护娉㈠鐞嗭紝涓鍏卞緱鍒16杞寸殑杈撳嚭鏁版嵁銆 +* Date : 2023/02/25 +*---------------------------------------------------------------------**/ +static void Pedometer_Filter(void) +{ + short i, j; + for (i = 0; i < FILTER_AXIS_NUM; i++){ + DifFiltOutput[i][0] = DifFiltOutput[i][1]; + DifFiltOutput[i][1] = DifFiltOutput[i][2]; + DifFiltOutput[i][2] = DifFiltOutput[i][3]; + DifFiltOutput[i][3] = DifFiltOutput[i][4]; + OutputFilt[i][0] = OutputFilt[i][1]; + } + + for (i = 0; i < AXIS_NUM; i++){ + double FiltSum = 0; + for (j = 0; j < 4; j++) + { + FiltSum += DifFiltCoefficient1[j] * (AccBuffer[i][15 + j] + AccBuffer[i][23 - j]); + } + FiltSum += DifFiltCoefficient1[4] * AccBuffer[i][19]; + DifFiltOutput[i][4] = (FiltSum); + + FiltSum = 0; + for (j = 0; j < 8; j++) + { + FiltSum += DifFiltCoefficient2[j] * (AccBuffer[i][11 + j] + AccBuffer[i][27 - j]); + } + FiltSum += DifFiltCoefficient2[8] * AccBuffer[i][19]; + DifFiltOutput[i + AXIS_NUM][4] = (FiltSum); + + FiltSum = 0; + for (j = 0; j < 12; j++){ + FiltSum += DifFiltCoefficient3[j] * (AccBuffer[i][7 + j] + AccBuffer[i][31 - j]); + } + FiltSum += DifFiltCoefficient3[12] * AccBuffer[i][19]; + DifFiltOutput[i + AXIS_NUM * 2][4] = (FiltSum); + + FiltSum = 0; + for (j = 0; j < 16; j++){ + FiltSum += DifFiltCoefficient4[j] * (AccBuffer[i][3 + j] + AccBuffer[i][35 - j]); + } + FiltSum += DifFiltCoefficient4[16] * AccBuffer[i][19]; + DifFiltOutput[i + AXIS_NUM * 3][4] = (FiltSum); + } + // 鏇存柊16涓酱璁℃鍣ㄧ殑杈撳嚭缁撴灉 + for (i = 0; i < FILTER_AXIS_NUM; i++){ + OutputFilt[i][1] = DifFiltOutput[i][4]; + } +} +/**--------------------------------------------------------------------- +* Function : Steps_Feature +* Description : 鎻愬彇璁℃鍣ㄧ壒寰侊紝鍒嗘瀽璁℃鍣ㄧ殑娉㈠嘲娉㈣胺浠ュ強鎸箙鎯呭喌 +* Date : 2023/02/25 +*---------------------------------------------------------------------**/ +static void Steps_Feature(void) +{ + // 閬嶅巻16涓护娉㈠櫒閫氶亾杈撳嚭 + for (short axis = 0; axis < FILTER_AXIS_NUM; axis++) + { + StepLcVar[axis][0] = StepLcVar[axis][0] + 1.0; + + int8_t FeatureType = 0; + // 鎺ㄦ祴鏄垽鏂亣鍒版尝宄版垨鑰呮尝璋 + if ((StepLcVar[axis][1] < 1) && ((OutputFilt[axis][1] > OutputFilt[axis][0]) || + ((OutputFilt[axis][1] == OutputFilt[axis][0]) && (StepLcVar[axis][1] == 1)))){ + FeatureType = 1; + } + + // 鎺ㄦ祴鏄垽鏂亣鍒版尝宄版垨鑰呮尝璋 + if ((StepLcVar[axis][1] > -1) && ((OutputFilt[axis][1] < OutputFilt[axis][0]) || + ((OutputFilt[axis][1] == OutputFilt[axis][0]) && (StepLcVar[axis][1] == -1)))){ + FeatureType = -1; + } + + // 濡傛灉閬囧埌鐨勪簡娉㈠嘲鍜屾尝璋 + if (FeatureType != 0) { + // 鏇存柊鎸箙鐗瑰緛鍜屾椂闂寸壒寰(璁板綍8涓偣)鏁扮粍 + for (uint8_t ch = 0; ch < 7; ch++) { + FeatureAmp[axis][ch] = FeatureAmp[axis][ch + 1]; + FeatureTime[axis][ch] = FeatureTime[axis][ch + 1]; + } + // 鏇存柊鏃堕棿鐗瑰緛鍜屾尟骞呯壒寰佺殑鏈鏂板硷紝杩欓噷鏍规嵁娉㈠嘲鍜屾尝璋峰仛鍙栧弽澶勭悊 + FeatureAmp[axis][7] = (StepLcVar[axis][2] - OutputFilt[axis][0]) * FeatureType; + FeatureTime[axis][7] = (uint16_t)(StepLcVar[axis][0]); + + StepLcVar[axis][2] = OutputFilt[axis][0]; + // 淇濆瓨缁撴灉 + StepLcVar[axis][0] = 0; + StepLcVar[axis][1] = FeatureType; + StepReFeature[axis] = STEP_TRUE; + } + } +} + +static void Steps_Recording(unsigned long long timeStamp) +{ + short nB, nC; + double thred, nD = 0, nE = 0, Differ_Sum = 0; + double StepAmp12, StepAmp34, StepAmp56, StepAmp78, StepAmp14, StepAmp58, StepAmp18; + unsigned short StepTime12, StepTime34, StepTime56, StepTime78, StepTime14, StepTime58, StepTime38, StepTime18; + + // 浠庣4涓酱鍒扮20涓酱杩涜閬嶅巻 + for (short axis = AXIS_NUM; axis < AXIS_ALL; axis++){ + // StepRecordTime鐨勫噯纭惈涔夋槸浠涔堬紵 + if (StepRecordTime[axis][4] > 5 && axis != FILTER_AXIS_NUM){ + nD = StepRecordTime[axis][5] / StepRecordTime[axis][4]; + } + } + // 濡傛灉nD澶т簬0 + if (nD > 0) + { + // 浠0鍒2杞(x,y,z)杩涜鐩稿悓鐨勯亶鍘 + for (nC = 0; nC < AXIS_NUM - 1; nC++) + { + if ((StepRecordTime[nC][4] > 5) && (nD > StepRecordTime[nC][5] / StepRecordTime[nC][4])) + { + nD = StepRecordTime[nC][5] / StepRecordTime[nC][4]; + } + } + // 鏈鍚庢墠璁＄畻FILTER_AXIS_NUM鐨勭粨鏋 + nC = FILTER_AXIS_NUM; + if ((StepRecordTime[nC][4] > 5) && (nD > StepRecordTime[nC][5] / StepRecordTime[nC][4])) + { + nD = StepRecordTime[nC][5] / StepRecordTime[nC][4]; + } + } + + // 姹傚姞閫熷害璁＄殑宸垎鍜 + Differ_Sum = 0; + for (uint8_t j = 0; j < ACC_BUFF_NUM; j++){ + Differ_Sum = Differ_Sum + fabs(AccBuffer[3][j + 1] - AccBuffer[3][j]); + } + + nC = 3; + if ((StepRecordTime[0][4] > 5) && (StepRecordTime[1][4] > 5) && (StepRecordTime[2][4] > 5) + && (StepRecordTime[0][5] / StepRecordTime[0][4] + StepRecordTime[1][5] / StepRecordTime[1][4] + StepRecordTime[2][5] / StepRecordTime[2][4] + > 5.7 * StepRecordTime[nC][5] / StepRecordTime[nC][4]) && (Differ_Sum < 1.2)) // >5 + { + + }else{ + if ((StepRecordTime[nC][4] > 5) && (nD > StepRecordTime[nC][5] / StepRecordTime[nC][4])){ + nD = StepRecordTime[nC][5] / StepRecordTime[nC][4]; + } + } + // 瀵规护娉㈠櫒杈撳嚭鐨16涓酱杩涜澶勭悊 + for (uint8_t axis = 0; axis < FILTER_AXIS_NUM; axis++){ + if (StepReFeature[axis] == STEP_FALSE) { + continue; + } + + // 璁＄畻璁℃鍣ㄧ殑鎸箙鐗瑰緛鍜屾椂闂寸壒寰 + StepAmp12 = FeatureAmp[axis][0] + FeatureAmp[axis][1]; + StepAmp34 = FeatureAmp[axis][2] + FeatureAmp[axis][3]; + StepAmp56 = FeatureAmp[axis][4] + FeatureAmp[axis][5]; + StepAmp78 = FeatureAmp[axis][6] + FeatureAmp[axis][7]; + StepAmp14 = StepAmp12 + StepAmp34; + StepAmp58 = StepAmp56 + StepAmp78; + StepAmp18 = StepAmp14 + StepAmp58; + + StepTime12 = FeatureTime[axis][0] + FeatureTime[axis][1]; + StepTime34 = FeatureTime[axis][2] + FeatureTime[axis][3]; + StepTime56 = FeatureTime[axis][4] + FeatureTime[axis][5]; + StepTime78 = FeatureTime[axis][6] + FeatureTime[axis][7]; + StepTime14 = StepTime12 + StepTime34; + StepTime58 = StepTime56 + StepTime78; + StepTime38 = StepTime34 + StepTime58; + StepTime18 = StepTime14 + StepTime58; + + if (StepRecordeState[axis][2] < 30) + { + StepRecordeState[axis][2] = StepRecordeState[axis][2] + 0.51; + StepRecordTime[axis][2] = StepRecordTime[axis][2] + FeatureTime[axis][7]; + StepRecordeState[axis][3] = StepRecordeState[axis][3] + 0.51; + StepRecordTime[axis][3] = StepRecordTime[axis][3] + FeatureTime[axis][7]; + + if ((axis % 4 == 3) && (StepRecordeState[FILTER_AXIS_NUM + axis / 4][2] < 30)) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][2] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][2] + 0.51; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][2] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][2] + FeatureTime[axis][7]; + StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] + 0.51; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][3] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][3] + FeatureTime[axis][7]; + } + } + + // 鍔ㄦ侀槇鍊 + thred = 0.015 + ((FILTER_AXIS_NUM - axis - 1) / AXIS_NUM) * 0.037; + + if (StepRecordTime[axis][6] < 60) { + StepRecordTime[axis][6]++; + }else{ + StepRecordTime[axis][4] = 0; + StepRecordTime[axis][5] = 0; + StepRecordTime[axis][7] = 0; + } + + // 濡傛灉杞存槸XYZ鍜屽悎鎴愯酱 + if (axis % 4 == AXIS_XYZ) + { + if (StepRecordTime[FILTER_AXIS_NUM + axis / 4][6] < 25){ + StepRecordTime[FILTER_AXIS_NUM + axis / 4][6]++; + }else{ + StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][5] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][7] = 0; + } + + if ((fabs(FeatureAmp[axis][2] - FeatureAmp[axis][3]) / (StepAmp34) < 0.15l) + && (fabs(FeatureAmp[axis][3] - FeatureAmp[axis][4]) / (FeatureAmp[axis][3] + FeatureAmp[axis][4]) < 0.15l) + && (fabs(FeatureAmp[axis][4] - FeatureAmp[axis][5]) / (StepAmp56) < 0.15l) + && (fabs(FeatureAmp[axis][5] - FeatureAmp[axis][6]) / (FeatureAmp[axis][5] + FeatureAmp[axis][6]) < 0.15l) + && (fabs(FeatureAmp[axis][6] - FeatureAmp[axis][7]) / (StepAmp78) < 0.15l) + && (FeatureAmp[axis][2] > thred) && (FeatureAmp[axis][3] > thred) && (FeatureAmp[axis][4] > thred) && (FeatureAmp[axis][5] > thred) && (FeatureAmp[axis][6] > thred) && (FeatureAmp[axis][7] > thred) + && (ABS_INT(StepTime34 - StepTime56) / (StepTime34 + StepTime56) < 0.15l) + && (ABS_INT(StepTime56 - StepTime78) / (StepTime56 + StepTime78) < 0.15) + && (((2 * FeatureTime[axis][2] > FeatureTime[axis][3]) && (FeatureTime[axis][2] < 2 * FeatureTime[axis][3])) || (ABS_INT(FeatureTime[axis][2] - FeatureTime[axis][3]) <= 3)) + && (((2 * FeatureTime[axis][4] > FeatureTime[axis][5]) && (FeatureTime[axis][4] < 2 * FeatureTime[axis][5])) || (ABS_INT(FeatureTime[axis][4] - FeatureTime[axis][5]) <= 3)) + && (((2 * FeatureTime[axis][6] > FeatureTime[axis][7]) && (FeatureTime[axis][6] < 2 * FeatureTime[axis][7])) || (ABS_INT(FeatureTime[axis][6] - FeatureTime[axis][7]) <= 3)) + && (StepTime58 > StepMinTime) + && (StepTime58 < StepMaxTime)) + { + StepRecordTime[FILTER_AXIS_NUM + axis / 4][8] = PEDOMETER_FREQ * 3.0 / StepTime38; + TimeStampSave[FILTER_AXIS_NUM + axis / 4][1] = timeStamp; + if (StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] == 0) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] + 3; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] + StepTime38; + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + 3; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][1] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][1] + StepTime38; + TimeStampSave[FILTER_AXIS_NUM + axis / 4][0] = timeStamp - (unsigned long)(StepTime38 * 1000 / PEDOMETER_FREQ); + } + else + { + if (((ABS_INT(FeatureTime[axis][2] - FeatureTime[axis][4]) < 0.2 * (FeatureTime[axis][2] + FeatureTime[axis][4])) || (ABS_INT(FeatureTime[axis][2] - FeatureTime[axis][4]) < 5)) + && ((ABS_INT(FeatureTime[axis][3] - FeatureTime[axis][5]) < 0.2 * (FeatureTime[axis][3] + FeatureTime[axis][5])) || (ABS_INT(FeatureTime[axis][3] - FeatureTime[axis][5]) < 5)) + && ((ABS_INT(FeatureTime[axis][4] - FeatureTime[axis][6]) < 0.2 * (FeatureTime[axis][4] + FeatureTime[axis][6])) || (ABS_INT(FeatureTime[axis][4] - FeatureTime[axis][6]) < 5)) + && ((ABS_INT(FeatureTime[axis][5] - FeatureTime[axis][7]) < 0.2 * (FeatureTime[axis][5] + FeatureTime[axis][7])) || (ABS_INT(FeatureTime[axis][5] - FeatureTime[axis][7]) < 5))) + { + + if ((StepRecordTime[FILTER_AXIS_NUM + axis / 4][7] > 0) && (fabs(StepTime38 / 3.0 - StepRecordTime[FILTER_AXIS_NUM + axis / 4][7]) > 0.2 * StepRecordTime[FILTER_AXIS_NUM + axis / 4][7])) + { + StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][5] = 0; + } + if (StepRecordTime[FILTER_AXIS_NUM + axis / 4][6] >= 6) + { + StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] + 3; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][5] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][5] + StepTime38; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][6] = 0; + } + StepRecordTime[FILTER_AXIS_NUM + axis / 4][7] = StepTime38 / 3.0; + } + if (StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] <= 3) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] + StepRecordeState[FILTER_AXIS_NUM + axis / 4][3]; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] + StepRecordTime[FILTER_AXIS_NUM + axis / 4][3]; + } + else + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] + 3; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] + StepTime38; + } + + + if (StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] <= 3) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + StepRecordeState[FILTER_AXIS_NUM + axis / 4][3]; + } + else if ((StepRecordTime[FILTER_AXIS_NUM + axis / 4][3] - StepTime38 > StepMaxTime) + && (StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] > 11)) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + 3; + } + else + { + if (StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] > 0) + { + nE = StepRecordTime[FILTER_AXIS_NUM + axis / 4][3] * StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] / StepRecordTime[FILTER_AXIS_NUM + axis / 4][5]; + } + if ((StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] > 0) && (nE < StepRecordeState[FILTER_AXIS_NUM + axis / 4][3])) + { + if (nE > 4) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + nE; + } + else + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + 3; + } + } + else + { + if (StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] > 4) + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + StepRecordeState[FILTER_AXIS_NUM + axis / 4][3]; + } + else + { + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] + 3; + } + } + } + + StepRecordTime[FILTER_AXIS_NUM + axis / 4][1] = StepRecordTime[FILTER_AXIS_NUM + axis / 4][1] + StepRecordTime[FILTER_AXIS_NUM + axis / 4][3]; + } + StepRecordeState[FILTER_AXIS_NUM + axis / 4][2] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][2] = 0; + StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][3] = 0; + } + else if ((StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] > 0) && ((StepRecordTime[FILTER_AXIS_NUM + axis / 4][2] >= StepMaxTime * 2) || (StepAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ))) + { + nB = AXIS_ALL - 1; + if (nD > 0) + { + for (nC = (AXIS_ALL - 1); nC >= 0; nC--) + { + if (StepRecordTime[nB][0] < StepRecordTime[nC][0]) + { + nB = nC; + } + } + } + else + { + for (nC = (AXIS_ALL - 1); nC >= AXIS_NUM; nC--) + { + if ((StepRecordTime[nB][0] < StepRecordTime[nC][0]) && (nC != FILTER_AXIS_NUM)) + { + nB = nC; + } + } + } + if ((nB == FILTER_AXIS_NUM + axis / 4) && (StepRecordeState[nB][0] >= 8)) + { + if (Steps.walkRealTime > StepRecordeState[nB][1]) + { + Steps.walk = Steps.walk + Steps.walkRealTime; + } + else + { + Steps.walk = Steps.walk + StepRecordeState[nB][1]; + } + Steps.walkRealTime = 0; + + ReSteps.walkSteps = (unsigned long)(Steps.walk); + for (nC = 0; nC < AXIS_ALL; nC++) + { + if ((StepAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ)) + { + StepRecordeState[nC][1] = 0; + StepRecordTime[nC][1] = 0; + if (StepRecordTime[nC][3] > StepRecordTime[nB][2]) + { + StepRecordTime[nC][3] = StepRecordTime[nB][2]; + if (StepRecordeState[nC][3] > StepRecordeState[nB][2]) + { + StepRecordeState[nC][3] = StepRecordeState[nB][2]; + } + } + } + else if (StepRecordTime[nC][3] >= StepRecordTime[nB][2]) + { + StepRecordeState[nC][1] = 0; + StepRecordTime[nC][1] = 0; + StepRecordTime[nC][3] = StepRecordTime[nB][2]; + if (StepRecordeState[nC][3] > StepRecordeState[nB][2]) + { + StepRecordeState[nC][3] = StepRecordeState[nB][2]; + } + } + else if (StepRecordTime[nC][1] + StepRecordTime[nC][3] >= StepRecordTime[nB][2]) + { + StepRecordTime[nC][1] = StepRecordTime[nB][2] - StepRecordTime[nC][3]; + if (StepRecordTime[nC][4] > 0) + { + StepRecordeState[nC][1] = StepRecordTime[nC][1] * StepRecordTime[nC][4] / StepRecordTime[nC][5]; + } + else if (StepRecordeState[nC][1] + StepRecordeState[nC][3] >= StepRecordeState[nB][2]) + { + StepRecordeState[nC][1] = StepRecordeState[nB][2] - StepRecordeState[nC][3]; + } + else + { + StepRecordeState[nC][1] = 0; + } + } + } + } + StepRecordeState[FILTER_AXIS_NUM + axis / 4][0] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][0] = 0; + StepRecordeState[FILTER_AXIS_NUM + axis / 4][1] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][1] = 0; + StepRecordeState[FILTER_AXIS_NUM + axis / 4][3] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][3] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][4] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][5] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][7] = 0; + StepRecordTime[FILTER_AXIS_NUM + axis / 4][8] = 0; + TimeStampSave[FILTER_AXIS_NUM + axis / 4][0] = 0; + } + } + + if ((fabs(-FeatureAmp[axis][0] + FeatureAmp[axis][1] - FeatureAmp[axis][2] + FeatureAmp[axis][3]) < FeatureAmp[axis][3] * 0.3l) + && (fabs(-FeatureAmp[axis][4] + FeatureAmp[axis][5] - FeatureAmp[axis][6] + FeatureAmp[axis][7]) < FeatureAmp[axis][7] * 0.3l) + && (FeatureAmp[axis][3] > thred) && (FeatureAmp[axis][3] > 0.7l * FeatureAmp[axis][2]) && (FeatureAmp[axis][3] > 0.7l * FeatureAmp[axis][1]) && (FeatureAmp[axis][3] > 0.7l * FeatureAmp[axis][0]) + && (FeatureAmp[axis][7] > thred) && (FeatureAmp[axis][7] > 0.7l * FeatureAmp[axis][6]) && (FeatureAmp[axis][7] > 0.7l * FeatureAmp[axis][5]) && (FeatureAmp[axis][7] > 0.7l * FeatureAmp[axis][4]) + && (fabs(StepAmp14 - StepAmp58) < 0.15l * StepAmp18) + && (ABS_INT(StepTime12 - StepTime56) < 0.15l * StepTime58) + && (ABS_INT(StepTime34 - StepTime78) < 0.15l * StepTime58) + && (StepTime14 >= StepMinTime) + && (StepTime14 < 2 * StepMaxTime) + && (StepTime58 > StepMinTime) + && (StepTime58 < 2 * StepMaxTime)) + { + if ((StepTime58 >= StepMaxTime) + || ((nD != 0) && (nD < 0.7l * StepTime18 / 4.0l))) + { + StepRecordTime[axis][8] = PEDOMETER_FREQ * 8.0 / StepTime18; + } + else + { + StepRecordTime[axis][8] = PEDOMETER_FREQ * 4.0 / StepTime18; + } + TimeStampSave[axis][1] = timeStamp; + if (StepRecordeState[axis][0] == 0) + { + StepRecordeState[axis][0] = StepRecordeState[axis][0] + 4; + StepRecordTime[axis][0] = StepRecordTime[axis][0] + StepTime18; + if ((StepTime58 >= StepMaxTime) + || ((nD != 0) && (nD < 0.7l * StepTime18 / 4.0l))) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 8; + } + else + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 4; + } + StepRecordTime[axis][1] = StepRecordTime[axis][1] + StepTime18; + TimeStampSave[axis][0] = timeStamp - (unsigned long)(StepTime18 * 1000 / PEDOMETER_FREQ); + } + else + { + // 鍒ゆ柇璁℃鍣ㄧ殑Amp鎸箙鐗瑰緛鍜屾椂闂寸壒寰佹槸鍚︽弧瓒宠姹 + if (((fabs(FeatureAmp[axis][0] - FeatureAmp[axis][4]) < 0.2l * (FeatureAmp[axis][0] + FeatureAmp[axis][4])) || (fabs(FeatureAmp[axis][0] - FeatureAmp[axis][4]) < 0.075l * StepAmp18)) + && ((fabs(FeatureAmp[axis][1] - FeatureAmp[axis][5]) < 0.2l * (FeatureAmp[axis][1] + FeatureAmp[axis][5])) || (fabs(FeatureAmp[axis][1] - FeatureAmp[axis][5]) < 0.075l * StepAmp18)) + && ((fabs(FeatureAmp[axis][2] - FeatureAmp[axis][6]) < 0.2l * (FeatureAmp[axis][2] + FeatureAmp[axis][6])) || (fabs(FeatureAmp[axis][2] - FeatureAmp[axis][6]) < 0.075l * StepAmp18)) + && ((fabs(FeatureAmp[axis][3] - FeatureAmp[axis][7]) < 0.2l * (FeatureAmp[axis][3] + FeatureAmp[axis][7])) || (fabs(FeatureAmp[axis][3] - FeatureAmp[axis][7]) < 0.075l * StepAmp18)) + && ((ABS_INT(FeatureTime[axis][0] - FeatureTime[axis][4]) < 0.2l * (FeatureTime[axis][0] + FeatureTime[axis][4])) || (ABS_INT(FeatureTime[axis][0] - FeatureTime[axis][4]) < 5)) + && ((ABS_INT(FeatureTime[axis][1] - FeatureTime[axis][5]) < 0.2l * (FeatureTime[axis][1] + FeatureTime[axis][5])) || (ABS_INT(FeatureTime[axis][1] - FeatureTime[axis][5]) < 5)) + && ((ABS_INT(FeatureTime[axis][2] - FeatureTime[axis][6]) < 0.2l * (FeatureTime[axis][2] + FeatureTime[axis][6])) || (ABS_INT(FeatureTime[axis][2] - FeatureTime[axis][6]) < 5)) + && ((ABS_INT(FeatureTime[axis][3] - FeatureTime[axis][7]) < 0.2l * (FeatureTime[axis][3] + FeatureTime[axis][7])) || (ABS_INT(FeatureTime[axis][3] - FeatureTime[axis][7]) < 5))) + { + if ((StepRecordTime[axis][7] > 0) && (fabs(StepTime18 / 4.0 - StepRecordTime[axis][7]) > 0.2 * StepRecordTime[axis][7])) + { + StepRecordTime[axis][4] = 0; + StepRecordTime[axis][5] = 0; + } + if (StepRecordTime[axis][6] >= 8) + { + StepRecordTime[axis][4] = StepRecordTime[axis][4] + 4; + StepRecordTime[axis][5] = StepRecordTime[axis][5] + StepTime18; + StepRecordTime[axis][6] = 0; + } + StepRecordTime[axis][7] = StepTime18 / 4.0; + } + if (StepRecordeState[axis][3] <= 4) + { + StepRecordeState[axis][0] = StepRecordeState[axis][0] + StepRecordeState[axis][3]; + StepRecordTime[axis][0] = StepRecordTime[axis][0] + StepRecordTime[axis][3]; + } + else + { + StepRecordeState[axis][0] = StepRecordeState[axis][0] + 4; + StepRecordTime[axis][0] = StepRecordTime[axis][0] + StepTime18; + } + if ((StepTime58 >= StepMaxTime) + || ((nD != 0) && (nD < 0.7l * StepTime18 / 4.0l))) + { + if (StepRecordeState[axis][3] <= 4) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + StepRecordeState[axis][3] * 2; + } + else if ((StepRecordTime[axis][3] - StepTime18 > SampleFrequencySix) + && (StepRecordeState[axis][3] > 12)) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 8;//10 + } + else + { + if (StepRecordTime[axis][4] > 0) + { + nE = StepRecordTime[axis][3] * StepRecordTime[axis][4] / StepRecordTime[axis][5]; + } + if ((StepRecordTime[axis][4] > 0) && (nE < StepRecordeState[axis][3])) + { + if (nE > 2.5l) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + nE * 2; + } + else + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 8;//10 + } + } + else + { + if (StepRecordeState[axis][3] > 2.5l) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + StepRecordeState[axis][3] * 2; + } + else + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 8;//10 + } + } + } + + } + else + { + if (StepRecordeState[axis][3] <= 4) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + StepRecordeState[axis][3]; + } + else if ((StepRecordTime[axis][3] - StepTime18 > SampleFrequencySix) + && (StepRecordeState[axis][3] > 12)) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 4; + } + else + { + if (StepRecordTime[axis][4] > 0) + { + nE = StepRecordTime[axis][3] * StepRecordTime[axis][4] / StepRecordTime[axis][5]; + } + if ((StepRecordTime[axis][4] > 0) && (nE < StepRecordeState[axis][3])) + { + if (nE > 5) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + nE; + } + else + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 4; + } + } + else + { + if (StepRecordeState[axis][3] > 5) + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + StepRecordeState[axis][3]; + } + else + { + StepRecordeState[axis][1] = StepRecordeState[axis][1] + 4; + } + } + } + + } + StepRecordTime[axis][1] = StepRecordTime[axis][1] + StepRecordTime[axis][3]; + } + StepRecordeState[axis][2] = 0; + StepRecordTime[axis][2] = 0; + StepRecordeState[axis][3] = 0; + StepRecordTime[axis][3] = 0; + } + else if ((StepRecordeState[axis][0] > 0) && ((StepRecordTime[axis][2] >= StepMaxTime * 2) || (StepAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ))) + { + nB = AXIS_ALL - 1; + if (nD > 0) + { + for (nC = (AXIS_ALL - 1); nC >= 0; nC--) + { + if (StepRecordTime[nB][0] < StepRecordTime[nC][0]) + { + nB = nC; + } + } + } + else + { + for (nC = (AXIS_ALL - 1); nC >= AXIS_NUM; nC--) + { + if ((StepRecordTime[nB][0] < StepRecordTime[nC][0]) && (nC != FILTER_AXIS_NUM)) + { + nB = nC; + } + } + } + if ((nB == axis) && (StepRecordeState[nB][0] >= 10)) + { + if (Steps.walkRealTime > StepRecordeState[nB][1]) + { + Steps.walk = Steps.walk + Steps.walkRealTime; + } + else + { + Steps.walk = Steps.walk + StepRecordeState[nB][1]; + } + Steps.walkRealTime = 0; + ReSteps.walkSteps = (unsigned long)(Steps.walk); + + for (nC = 0; nC < AXIS_ALL; nC++) + { + if ((StepAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ)) + { + StepRecordeState[nC][1] = 0; + StepRecordTime[nC][1] = 0; + if (StepRecordTime[nC][3] > StepRecordTime[nB][2]) + { + StepRecordTime[nC][3] = StepRecordTime[nB][2]; + if (StepRecordeState[nC][3] > StepRecordeState[nB][2]) + { + StepRecordeState[nC][3] = StepRecordeState[nB][2]; + } + } + } + else if (StepRecordTime[nC][3] >= StepRecordTime[nB][2]) + { + StepRecordeState[nC][1] = 0; + StepRecordTime[nC][1] = 0; + StepRecordTime[nC][3] = StepRecordTime[nB][2]; + if (StepRecordeState[nC][3] > StepRecordeState[nB][2]) + { + StepRecordeState[nC][3] = StepRecordeState[nB][2]; + } + } + else if (StepRecordTime[nC][1] + StepRecordTime[nC][3] >= StepRecordTime[nB][2]) + { + StepRecordTime[nC][1] = StepRecordTime[nB][2] - StepRecordTime[nC][3]; + if (StepRecordTime[nC][4] > 0) + { + StepRecordeState[nC][1] = StepRecordTime[nC][1] * StepRecordTime[nC][4] / StepRecordTime[nC][5]; + } + else if (StepRecordeState[nC][1] + StepRecordeState[nC][3] >= StepRecordeState[nB][2]) + { + StepRecordeState[nC][1] = StepRecordeState[nB][2] - StepRecordeState[nC][3]; + } + else + { + StepRecordeState[nC][1] = 0; + } + } + } + + + StepRecordTime[axis][4] = 0; + StepRecordTime[axis][5] = 0; + + + } + StepRecordeState[axis][0] = 0; + StepRecordTime[axis][0] = 0; + StepRecordeState[axis][1] = 0; + StepRecordTime[axis][1] = 0; + StepRecordeState[axis][3] = 0; + StepRecordTime[axis][3] = 0; + // StepRecordeStateTime[nA][4] = 0; + // StepRecordeStateTime[nA][5] = 0; + StepRecordTime[axis][7] = 0; + StepRecordTime[axis][8] = 0; + TimeStampSave[axis][0] = 0; + } + + StepReFeature[axis] = STEP_FALSE; + } + + nB = AXIS_ALL - 1; + for (nC = (AXIS_ALL - 1); nC >= 0; nC--){ + if (StepRecordTime[nB][0] < StepRecordTime[nC][0]){ + nB = nC; + } + } + if (StepRecordeState[nB][0] == 0) + { + StateLcVar[0] = NonSteps; + if (Steps.walkRealTime > 0) + { + Steps.walk = Steps.walk + Steps.walkRealTime; + Steps.walkRealTime = 0; + } + } + + if (StateLcVar[1] == StateLcVar[0]) + { + if ((StepRecordTime[nB][1] > StepMinTime) && (StepRecordeState[nB][0] >= 8) && (StepRecordeState[nB][3] == 0)) + { + if (StateLcVar[1] == walk) + { + if (Steps.walkRealTime < StepRecordeState[nB][1]) + { + Steps.walkRealTime = StepRecordeState[nB][1]; + + ReSteps.walkSteps = (unsigned long)(Steps.walk + Steps.walkRealTime); + } + } + else if (Steps.walkRealTime < StepRecordeState[nB][1]) + { + Steps.walkRealTime = StepRecordeState[nB][1]; + ReSteps.walkSteps = (unsigned long)(Steps.walk + Steps.walkRealTime); + } + } + } + else + { + if ((StepRecordeState[nB][0] >= 8) && (StateLcVar[1] != NonSteps)) + { + + if (Steps.walkRealTime > StepRecordeState[nB][1]) + { + Steps.walk = Steps.walk + Steps.walkRealTime; + } + else + { + Steps.walk = Steps.walk + StepRecordeState[nB][1]; + } + Steps.walkRealTime = 0; + ReSteps.walkSteps = (unsigned long)(Steps.walk + Steps.walkRealTime); + + for (nC = 0; nC < AXIS_ALL; nC++) + { + if (StepRecordTime[nC][3] >= StepRecordTime[nB][2]) + { + StepRecordeState[nC][1] = 0; + StepRecordTime[nC][1] = 0; + StepRecordTime[nC][3] = StepRecordTime[nB][2]; + if (StepRecordeState[nC][3] > StepRecordeState[nB][2]) + { + StepRecordeState[nC][3] = StepRecordeState[nB][2]; + } + } + else if (StepRecordTime[nC][1] + StepRecordTime[nC][3] >= StepRecordTime[nB][2]) + { + StepRecordTime[nC][1] = StepRecordTime[nB][2] - StepRecordTime[nC][3]; + if (StepRecordTime[nC][4] > 0) + { + StepRecordeState[nC][1] = StepRecordTime[nC][1] * StepRecordTime[nC][4] / StepRecordTime[nC][5]; + } + else if (StepRecordeState[nC][1] + StepRecordeState[nC][3] >= StepRecordeState[nB][2]) + { + StepRecordeState[nC][1] = StepRecordeState[nB][2] - StepRecordeState[nC][3]; + } + else + { + StepRecordeState[nC][1] = 0; + } + } + } + StepRecordeState[nB][1] = 0; + StepRecordTime[nB][1] = 0; + } + } + + stateNow.sign = 0; + if ((StateLcVar[1] != StateLcVar[0]) && ((StateLcVar[0] == NonSteps) || (StepRecordeState[nB][0] >= 8))) + { + stateNow.state = StateLcVar[0]; + if (StateLcVar[1] == NonSteps) + { + nB = AXIS_ALL - 1; + for (nC = (AXIS_ALL - 1); nC >= 0; nC--) + { + if (((TimeStampSave[nB][0] == 0) || (TimeStampSave[nB][0] > TimeStampSave[nC][0])) && (TimeStampSave[nC][0] > 0)) + { + nB = nC; + } + } + if (stateNow.stateStartTime < TimeStampSave[nB][0]) + { + stateNow.stateStartTime = TimeStampSave[nB][0]; + } + stateNow.stepsStartTime = TimeStampSave[nB][0]; + stateNow.sign = 1; + } + else if (StateLcVar[0] == NonSteps) + { + nB = AXIS_ALL - 1; + for (nC = (AXIS_ALL - 1); nC >= 0; nC--) + { + if (TimeStampSave[nB][1] < TimeStampSave[nC][1]) + { + nB = nC; + } + } + if (stateNow.stateStartTime < TimeStampSave[nB][1]) + { + stateNow.stateStartTime = TimeStampSave[nB][1]; + } + stateNow.sign = 2; + } + else + { + stateNow.stateStartTime = timeStamp; + stateNow.sign = 3; + } + StateLcVar[1] = StateLcVar[0]; + } + +} + +static void StateDetector(unsigned long long timeStamp) +{ + int nA; + double SumAccX = 0; + double SumAccYZ = 0; + double AccX = 0; + double AccYZ = 0; + + if (StateDetectLcVar[0] < StepMinTime) + { + StateDetectLcVar[0] = StateDetectLcVar[0] + 1; + if (StateDetectLcVar[0] < 0.5l) + { + SumAcc = -1; + }else{ + SumAcc = SumAcc - 1; + } + } + else + { + SumAcc = 0; + for (nA = 0; nA < (ACC_BUFF_NUM - 1); nA++) + { + SumAcc += fabs(AccBuffer[3][nA] - AccBuffer[3][nA + 1]); + SumAccX += fabs(AccBuffer[0][nA] - AccBuffer[0][nA + 1]); + SumAccYZ += fabs(sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA]) + - sqrt(AccBuffer[1][nA + 1] * AccBuffer[1][nA + 1] + AccBuffer[2][nA + 1] * AccBuffer[2][nA + 1])); + } + SumAcc /= (ACC_BUFF_NUM - 1); + SumAccX /= (ACC_BUFF_NUM - 1); + SumAccYZ /= (ACC_BUFF_NUM - 1); + + + for (nA = 0; nA < ACC_BUFF_NUM; nA++){ + AccX += fabs(AccBuffer[0][nA]); + } + AccX = AccX / ACC_BUFF_NUM; + + for (nA = 0; nA < ACC_BUFF_NUM; nA++) + { + AccYZ += sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA]); + } + AccYZ /= ACC_BUFF_NUM; + + if (SumAcc > 3 || SumAcc < 0.002l) + { + if (StateDetectLcVar[2] < SampleFrequencyFour) + { + StateDetectLcVar[2] = StateDetectLcVar[2] + StepMinTime; + } + StateDetectLcVar[3] = 0; + } + else + { + if (StateDetectLcVar[3] < SampleFrequencyFour) + { + StateDetectLcVar[3] = StateDetectLcVar[3] + StepMinTime; + } + StateDetectLcVar[2] = 0; + } + + if (StateDetectLcVar[2] > SampleFrequencydouble) + { + + StateDetectLcVar[4] = 1; + } + else if (StateDetectLcVar[2] > SampleFrequencyOPF) + { + STEPLIB_MEMSET(StepReFeature, 1, sizeof(StepReFeature)); + } + + if (StateDetectLcVar[3] > StepMinTime) + { + StateDetectLcVar[4] = 0; + } + + if (((AccX * 1.3l > AccYZ) || (SumAccX * 1.3l > SumAccYZ)) && (SumAcc < 0.8l)) + { + if (StateDetectLcVar[1] > 0) + { + StateDetectLcVar[1] = StateDetectLcVar[1] - 1; + }else{ + StateDetectLcVar[1] = 0; + } + } + else + { + if ((StateDetectLcVar[1] > 4) || (SumAcc > 0.6l)) + { + StateDetectLcVar[1] = 8;//10 + } + else + { + if ((AccX < AccYZ) && (SumAcc > 0.25l)) + { + if (StateDetectLcVar[1] < 8)//10 + { + StateDetectLcVar[1] = StateDetectLcVar[1] + 1; + } + } + else + { + StateDetectLcVar[1] = 0; + } + } + } + + if (StateDetectLcVar[1] > 4){ + StateLcVar[0] = 2; + }else{ + StateLcVar[0] = walk; + } + StateDetectLcVar[0] = 0; + } +} +void Pedometer_GetState(StepState_t* state){ + *state = StateLcVar[0]; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Quaternion.cpp b/1.Software/PDR 1.06/src/Quaternion.cpp new file mode 100644 index 0000000..e3dd82a --- /dev/null +++ b/1.Software/PDR 1.06/src/Quaternion.cpp @@ -0,0 +1,44 @@ +#include "math.h" +#include "Quaternion.h" + +const float FLT_THRES = 0.000001f; // 浮点数最小阈值 + +/**--------------------------------------------------------------------- +* Function : QuaternionNorm +* Description : 四元数归一化 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void QuaternionNorm(float* q0, float* q1, float* q2, float* q3) { + + float norm = sqrtf(((*q0) * (*q0) + (*q1) * (*q1) + + (*q2) * (*q2) + (*q3) * (*q3))); + if (norm > FLT_THRES) { + norm = 1 / norm; + (*q0) *= norm; (*q1) *= norm; + (*q2) *= norm; (*q3) *= norm; + } +} + +/**--------------------------------------------------------------------- +* Function : QuaternConj +* Description : 四元数求共轭四元数 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void QuaternConj(float _q[], float q[]) { + _q[0] = q[0]; + _q[1] = -q[1]; + _q[2] = -q[2]; + _q[3] = -q[3]; +} + +/**--------------------------------------------------------------------- +* Function : QuaternProd +* Description : 四元数乘法 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void QuaternProd(float qab[], float a[], float b[]) { + qab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]; + qab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]; + qab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1]; + qab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]; +} \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/Quaternion.h b/1.Software/PDR 1.06/src/Quaternion.h new file mode 100644 index 0000000..157edfe --- /dev/null +++ b/1.Software/PDR 1.06/src/Quaternion.h @@ -0,0 +1,33 @@ +#ifndef _PDR_QUATERNION_H_ +#define _PDR_QUATERNION_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/**--------------------------------------------------------------------- +* Function : QuaternionNorm +* Description : 四元数归一化 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void QuaternionNorm(float* q0, float* q1, float* q2, float* q3); + +/**--------------------------------------------------------------------- +* Function : QuaternConj +* Description : 四元数求共轭四元数 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void QuaternConj(float _q[], float q[]); + +/**--------------------------------------------------------------------- +* Function : QuaternProd +* Description : 四元数乘法 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void QuaternProd(float ab[], float a[], float b[]); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.06/src/SceneRecognition.c b/1.Software/PDR 1.06/src/SceneRecognition.c new file mode 100644 index 0000000..d922227 --- /dev/null +++ b/1.Software/PDR 1.06/src/SceneRecognition.c @@ -0,0 +1,492 @@ +/********************************************************************/ +/* FILE NAME : scene_recognition.c */ +/* AUTHOR : ZhangJingrui, ID: 11082826 */ +/* VERSION : V0.85_Beta (Doing) */ +/* DESCRIPTION: The definations of functions in scene_recognition.h */ +/********************************************************************/ + +#include "scene_recognition.h" +#include +#include /* fabs() */ +#include /* malloc(), free() */ +/* @Lai Zhilong added below code*/ +#if !defined(LOCATION_PLATFORM_QCOM_MODEM) /* The Macro is in "custom.h" */ + #include + /* @Zhang Jingrui: The header is depercated (and quite Linux specific, */ + /* on which it defines non-standard functions like mallinfo(3)). Notice that the */ + /* header is defined by C89 (and later) standards, but not . */ +#endif + +#include /* memset() */ +#include /* FLT_EPSILON */ + +/* Macro Defination----------------------------------------------*/ +/*===============================================================*/ + + /* Compatible with the value in header file "sensor.h" */ +#define _CONSTELLATION_USED_CAPACITY 12 /* Size of array defined as lct_nmea_gsa::prn. */ +#define _CONSTELLATION_VISIBLE_CAPACITY 20 /* Size of array defined as lct_nmea_gsv::satellites_data. */ + +/* Satellite PRN index in _GnssInfo list. */ +/* These index are planed with _GNSS_INDEX_LEN. */ +/* GP: index in _GnssInfo's list is 0~31, and the PRN index is 1~32 */ +#define _GP_INDEX_L 0 +#define _GP_INDEX_R 31 +#define _GP_INDEX_OFFSET ( _GP_INDEX_L ) +#define _GP_SVID_OFFSET 1 +/* GA: index in _GnssInfo's list is 32~67, and the PRN index is 1~36 */ +#define _GA_INDEX_L 32 +#define _GA_INDEX_R 67 +#define _GA_INDEX_OFFSET ( _GA_INDEX_L ) +#define _GA_SVID_OFFSET 1 +/* GL: index in _GnssInfo's list is 68~102, and the PRN index is 65~99 */ +#define _GL_INDEX_L 68 +#define _GL_INDEX_R 102 +#define _GL_INDEX_OFFSET ( _GL_INDEX_L ) +#define _GL_SVID_OFFSET 65 + + +/* Global Variable-----------------------------------------------*/ +/*===============================================================*/ + + +/* g_gnss_info_buffer: the newnest value is in the 0th sub-stucture. */ +static GnssInfo* g_gnss_info_buffer = NULL; +static const int g_gnss_info_buffer_size = _TIME_BUFFER_SIZE; +/* g_gnss_info_buffer_trail: 0 ~ gnss_info_buffer_size */ +static int g_gnss_info_buffer_trail = 0; +static _SCENE_MODEL_WORK_STATE g_model_state = MODEL_OFF; + + +/* static function Declaration-----------------------------------*/ +/*===============================================================*/ + +/* Return @ _is_occupied: 0 -- non resource occupied. */ +static int check_resources_occupied(void); + +/* Return @ _is_failed: 0 -- process of free res is successful. */ +static int free_resources (void); + +/* Return @ _is_failed: 0 -- process of allocating res is successful. */ +static int alloc_resources (void); + +/* Function Name: static _lctnmea_to_gnssinfo() */ +/* Usage: Transform from the updated lct_nmea structure to the _GnssInfo structure. */ +/* The defination of lct_nmea is in . */ +/* Param @ _dst : The pointer to the target _GnssInfo structure variable.*/ +/* Param @ _src_nmea : The pointer to the source lct_nmea structure varible. */ +/* Return @ _parse_res_state: The transformation process result. + 0--success; + -1--NULL param inputed; + 1--lct_nmea _src_nmea is not valiable beacause of the update flag. */ +static int lctnmea2Gnssinfo (GnssInfo *, const Nmea_t *); + +/* Function name: static _detect_good_signal() */ +/* Usage: judge the quality of positioning result between good and bad. */ +/* Param @ _src: The pointer to the _GnssInfo structure in the positioning epoch. */ +/* The value in the structure should be avaliable. */ +/* Return @ _process_res: the quality of positioning result. */ +/* _SIGNAL_QUALITY::GOOD -- the signal is good. */ +/* _SIGNAL_QUALITY::BAD -- the signal is not good. */ +static SIGNAL_QUALITY detGnssSignal (const GnssInfo *); + +/* Function name: static _gnss_info_st_cpy() */ +/* Usage: Deep copy from one _GnssInfo to another one. */ +/* The rule of copy operation: */ +/* _dst | _src | Operation behavior */ +/* -------------------------------------- */ +/* NULL | variable | Nothing done. */ +/* NULL | NULL | Nothing done. */ +/* val A | NULL | Set the value of A to the initialization value. */ +/* val A | val B | Deep copy B to A. */ +/* Val A | val A | Nothing done. */ +/* Param @ _src: Point to the source _GnssInfo structure. */ +/* Param @ _dst: Point to the target _GnssInfo structure . */ +/* Return @ (void) */ +static void gnss_info_st_cpy (GnssInfo *_dst, const GnssInfo *_src); + + +/* external function defination ---------------------------------*/ +/*===============================================================*/ + +SCENE_INIT_STATE SceneRecog_Init(void) +{ + SCENE_INIT_STATE res = INIT_NOT_PERFORMED; + int _is_occupied = -1; + int is_failed = 0; + /* Check the legality of initialization process. */ + if (g_model_state == MODEL_ON) + { + return INIT_ERROR_REDUNDANCY; /* 重复初始化 */ + } + + /* Initialization of resources */ + g_model_state = MODEL_OFF; + _is_occupied = check_resources_occupied(); + if (_is_occupied) + { + free_resources(); + } + is_failed = alloc_resources(); + if (is_failed) + { + res = INIT_ERROR_MEMORY_INIT_FAILED; + }else{ + res = INIT_SUCCESS; + g_model_state = MODEL_ON; + } + return res; +} + + +SCENE_INIT_STATE scene_recognition_reset(void) +{ + SCENE_INIT_STATE _process_res = INIT_RESET_FAILED; + int _is_occupied = 0; + int _is_failed = 0; + if (g_model_state == MODEL_OFF) + { + _process_res = INIT_RESET_FAILED; + } + else + { + _is_occupied = check_resources_occupied(); + if (_is_occupied) + { + free_resources(); + } + g_model_state = MODEL_OFF; + _is_failed = alloc_resources(); + if (_is_failed) + { + _process_res = INIT_RESET_FAILED; + g_model_state = MODEL_OFF; + } + else + { + _process_res = INIT_RESET_SUCCESS; + g_model_state = MODEL_ON; + } + } + return _process_res; +} + +/**---------------------------------------------------------------------- +* Function : sceneRecognitionProc +* Description : GNSS场景识别处理流程： +* 1）将nmea转换为gnss +* 2) 通过accuracy、snr、卫星数量分析GNSS信号质量 +* 3) 返回结果 +* Input : PDR的nmea结构体 +* Return : 场景识别结果 +* Author : logzhan +* Date : 2020/02/18 +*---------------------------------------------------------------------**/ +SCENE_RECOGNITION_RESULT sceneRecognitionProc(const Nmea_t *nmea) +{ + SCENE_RECOGNITION_RESULT res = RECOG_UNKNOWN; + int transRes = 0; + SIGNAL_QUALITY detRes = SIG_UNKNOWN; + GnssInfo gnssInfo; + // 将nmea转换为gnss + transRes = lctnmea2Gnssinfo(&gnssInfo, nmea); + switch (transRes) + { + case -1: + res = RECOG_ERROR_PARAM_INCRECT; + break; + case 1: + res = RECOG_ERROR_NMEA_INFO_MISS; + break; + case 0: + // GNSS信号检测结果 + detRes = detGnssSignal(&gnssInfo); + if (detRes == GOOD) + { res = RECOG_OPEN_AREA; } + else if (detRes == BAD) + { res = RECOG_MULTIPATH; } + else /*UNKOWN but is not exist*/ + { res = RECOG_UNKNOWN; } + break; + default: + res = RECOG_UNKNOWN; + } + return res; +} + +/**---------------------------------------------------------------------- +* Function : isOpenArea +* Description : GNSS开阔地检测，通过输入GNSS信号，分析当前信号质量，从而确定 +* 目标是否处于开阔场地。 +* Return : 0 : 非开阔地 1：开阔地(信号质量较好) +* Author : Zhang Jingrui, Geek ID: 11082826 +* Date : 2020/02/18 logzhan +*---------------------------------------------------------------------**/ +int isOpenArea(const Nmea_t *nmea) +{ + SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea); + // 如果是开阔地则返回1 + return (type == RECOG_OPEN_AREA); +} + + +SCENE_DESTROY_STATE scene_recognition_destroy(void) +{ + SCENE_DESTROY_STATE res = DESTROY_INVALID; + if (g_model_state == MODEL_OFF){ + res = DESTROY_INVALID; + }else{ + // 释放资源 + free_resources(); + g_model_state = MODEL_OFF; + res = DESTROY_SUCCESS; + } + return res; + +} + + +/* static function defination -----------------------------------*/ +/*===============================================================*/ + +static int check_resources_occupied(void) +{ + int res = 0; + if (g_gnss_info_buffer != NULL){ + ++res; + } + return res; +} + + +static int free_resources(void) +{ + int res = 0; + if (g_gnss_info_buffer != NULL) + { + free(g_gnss_info_buffer); + } + g_gnss_info_buffer = NULL; /* The pointer should be set NULL after free() for memory safety. */ + g_gnss_info_buffer_trail = 0; /* 0 because of the memory of g_gnss_info_buffer is NULL now. */ + return res; +} + + +static int alloc_resources(void) +{ + int _process_res = 0; + if (g_gnss_info_buffer != NULL) + { + /* Check the buffer's occupied. This branch is generally because of the */ + /* functions flow call errors. */ + _process_res = -1; + return _process_res; + } + GnssInfo *ptr = (GnssInfo *)malloc(sizeof(GnssInfo) * g_gnss_info_buffer_size); + if (ptr != NULL) + { + g_gnss_info_buffer = ptr; + g_gnss_info_buffer_trail = 0; + _process_res = 0; + } + else + { + /* Failed to apply for memory.*/ + _process_res = 1; + } + return _process_res; +} + + +static int lctnmea2Gnssinfo(GnssInfo *_dst, const Nmea_t * _src_nmea) +{ + int _parse_res_state = 0; + /* Temporarily deprecated. */ + /* int _gsa_used_prn[3][_CONSTELLATION_USED_CAPACITY] = {0}; */ /* GPGSA, GLGSA, GAGSA */ + + int _i_first = 0; + int _i_second = 0; + int _offset[6] = { _GP_SVID_OFFSET, _GP_INDEX_OFFSET, + _GL_SVID_OFFSET, _GL_INDEX_OFFSET, + _GA_SVID_OFFSET, _GA_INDEX_OFFSET }; + int _index = -1; + /* Detecting the NULL pamrameters inputed. */ + if (_src_nmea == NULL || _dst == NULL) + { + _parse_res_state = -1; + } + /* Detecting the _src_nmea if it wasn't updated. */ + else if (_src_nmea->update == 0) + { + _parse_res_state = 1; + } + /*****************************************************************/ + /* DELETE: abandon this branch for the stability of the program. */ + /* Detecting the incorrect accuracy value. */ + /*else if ((_src_nmea->accuracy.update == 0) || + (_src_nmea->accuracy.update == 0) ) + { + _parse_res_state = 2; + }*/ + /****************************************************************/ + /* update the the _GnssInfo. */ + else + { + /* initialize the memory space pointed by _dst to all 0. */ + gnss_info_st_cpy(_dst, NULL); + + /* pick the raw accuracy from lct_nmea::loc_accuracy. Therefore _dst->accuracy + could be equal with ITEM_INVALID. */ + _dst->accuracy = _src_nmea->accuracy.err; + + /* pick the aviable satellites PRN from GxGSA and match the SNR value in + GxGSV where Gx is the presented to GP, GA or GL. */ + for (_i_first = 0 ; _i_first < 3 ; ++ _i_first) /* 0 - GP, 1 - GL, 2 - GA */ + { + /* make sure the GxGSA and GxGSV has been update in the epoch */ + if (_src_nmea->gsa[_i_first].update == 0 || _src_nmea->gsv[_i_first].update == 0) + { + continue; + } + /* copy all the PRN number in GSA to the _dst pointed _GnssInfo */ + for (_i_second = 0 ; + _i_second < _CONSTELLATION_USED_CAPACITY && + _src_nmea->gsa[_i_first].prn[_i_second] != 0 && + _src_nmea->gsa[_i_first].prn[_i_second] != ITEM_INVALID ; + ++ _i_second) + { + _index = _src_nmea->gsa[_i_first].prn[_i_second] + - _offset[_i_first*2] + _offset[_i_first*2 + 1]; + if (_index > 0) + { + _dst->sat_used_list[_index] = 1; + } + } + /* copy all the SNR values in GSV to the _dst pointed _GnssInfo */ + for (_i_second = 0 ; + (_i_second < _CONSTELLATION_VISIBLE_CAPACITY) && + _src_nmea->gsv[_i_first].satellites_data[_i_second].snr != ITEM_INVALID ; + ++ _i_second) + { + _index = _src_nmea->gsv[_i_first].satellites_data[_i_second].prn + - _offset[_i_first*2] + _offset[_i_first*2 + 1]; + if (_index > 0) + { + _dst->snr_list[_index] = (float)(_src_nmea->gsv[_i_first].satellites_data[_i_second].snr); + } + } + } + _dst->update = 1; + _parse_res_state = 0; + } + return _parse_res_state; +} + + +/**--------------------------------------------------------------------- +* Function : detGnssSignal +* Description : 检测GPS信号是否良好，原理： +* 1）当Accuracy在0到5之间，那么是较好的信号 +* 2）如果Accuracy在5到10之间，那么利用卫星总数和使用数量判断 +* GPS质量好坏 +* 3）如果Accuracy在10以外，那么说明GPS信号不好 +* Date : 2020/02/18 logzhan +*---------------------------------------------------------------------**/ +static SIGNAL_QUALITY detGnssSignal(const GnssInfo* _src) +{ + SIGNAL_QUALITY procRes = SIG_UNKNOWN; + int i = 0; + float accuracy = 0; + float snr_large_rate = 0; + int sat_used_num = 0; // 卫星使用数量 + /* Simulating a Queue data structure which can be optimazated in the future. */ + /* move all the elements to the right one memory and abandon the rightmost one.*/ + for (i = 0 ; i < g_gnss_info_buffer_size - 1 ; ++ i) + { + gnss_info_st_cpy((g_gnss_info_buffer + i + 1) , (g_gnss_info_buffer + i) ); + } + /* Enqueue where the new element struct is put in the leftmost gpsFusionLocation of the array. */ + gnss_info_st_cpy((g_gnss_info_buffer), _src); + if (g_gnss_info_buffer_trail < g_gnss_info_buffer_size) + { + ++ g_gnss_info_buffer_trail; + } + + /* make sure _src->accuracy is available otherwise the quality of signal is not correct. */ + if (fabs(_src->accuracy - ITEM_INVALID) < FLT_EPSILON ) + { + procRes = SIG_UNKNOWN; + return procRes; + } + + /* features extraction */ + for (i = 0 ; i < _GNSS_INDEX_LEN ; ++ i) + { + if (_src->sat_used_list[i] == 1 && _src->snr_list[i] > 20) + { + ++ snr_large_rate; + } + } + sat_used_num = _src->sat_visible_number; + accuracy = _src->accuracy; + snr_large_rate = sat_used_num > 0 ? (snr_large_rate / sat_used_num) : 0 ; + /* classification process BEGIN */ + if (accuracy > 0 && accuracy < (5 + FLT_EPSILON) ) + { procRes = GOOD; } + else if (accuracy > (10 + FLT_EPSILON) ) + { procRes = BAD; } + else + { + /* 5 <= _accuracy <= 10 */ + if (snr_large_rate < 0.878676) + { + if (snr_large_rate < 0.781746) + { procRes = BAD; } + else + { + if (sat_used_num < 18.5) + { procRes = BAD; } + else + { procRes = GOOD; } + } + }else{ + if (accuracy > 0 && accuracy < 9.2) + { procRes = GOOD; } + else + { procRes = BAD; } + } + } + /* classification process END */ + return procRes; +} + + +static void gnss_info_st_cpy(GnssInfo *_dst, const GnssInfo *_src) +{ + if (_dst != NULL && _src == NULL) + { + /* initialize _dst if _src is a empty pointer. */ + _dst->update = 0; + _dst->local_timestamp = 0; + _dst->accuracy = 0; + _dst->sat_visible_number = 0; + memset(_dst->snr_list, 0, sizeof(_dst->snr_list) ); + memset(_dst->sat_used_list, 0, sizeof(_dst->sat_used_list) ); + } + else if (_dst != NULL && _src != NULL && _dst != _src) + { + /* deep copy from _src to _dst if both of them isn't empty and not equal. */ + _dst->update = _src->update; + _dst->local_timestamp = _src->local_timestamp; + _dst->accuracy = _src->accuracy; + _dst->sat_visible_number = _src->sat_visible_number; + memcpy(_dst->snr_list, _src->snr_list, sizeof(_src->snr_list) ); + memcpy(_dst->sat_used_list, _src->sat_used_list, sizeof(_src->sat_used_list) ); + } + else + { + /* _dst and _src are both NULL pointer, then PASS. */ + /* _dst is NULL pointer, then PASS. */ + /* _dst and _src are both pointer to the same memory, then PASS. */ + } +} diff --git a/1.Software/PDR 1.06/src/Utils.c b/1.Software/PDR 1.06/src/Utils.c new file mode 100644 index 0000000..1aeb5fb --- /dev/null +++ b/1.Software/PDR 1.06/src/Utils.c @@ -0,0 +1,465 @@ +/******************** (C) COPYRIGHT 2020 Geek************************************ +* File Name : pdr_util.c +* Department : Sensor Algorithm Team +* Current Version : V1.2 +* Author : logzhan +* Date of Issued : 2020.7.4 +* Comments : PDR 工具类函数支持 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include +#include +#include +#include "PDRBase.h" +#include "Utils.h" +#include "CoordTrans.h" + +const float FLT_THRES = 0.000001f; // 浮点数最小阈值 + +/* Function Declaration ------------------------------------------------------*/ +/**---------------------------------------------------------------------- +* Function : mean +* Description : 对输入double数组求均值 +* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用 + * 不同函数 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +double mean(double *data, int len) { + int i; + double value = 0; + for (i = 0; i < len; i++) { + value += data[i] / len; + } + return value; +} +/**---------------------------------------------------------------------- +* Function : fmean +* Description : 对输入float数组求均值 +* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用 + * 不同函数 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +float fmean(float *data, int len) +{ + float value = 0; + for (int i = 0; i < len; i++){ + value += data[i] / len; + } + return value; +} + +double meanAngle(double* angle, int len) +{ + double baseAngle = angle[0]; + double diff = 0.0; + double sum = 0.0; + double value = 0; + + if (len <= 0)return 0; + + for (int i = 0; i < len; i++) + { + diff = angle[i] - baseAngle; + if (diff > PI) + { + diff = diff - TWO_PI; + } + else if (diff < -PI) + { + diff = diff + TWO_PI; + } + + sum += (baseAngle + diff); + } + + value = sum / len; + + modAngle(&value, 0, TWO_PI); + return value; +} + +void modAngle(double * angle, double min, double max) +{ + double value = *angle; + double range = max - min; + while (value > max)value = value - range; + while (value < min)value = value + range; + *angle = value; +} + +float stdf(float* data, int len) +{ + int i; + float meanValue = fmean(data, len); + float value = 0; + for (i = 0; i < len; i++) + { + value += (data[i] - meanValue) * (data[i] - meanValue) / len; + } + return (float)sqrt(value); +} + +/**---------------------------------------------------------------------- +* Function : pdr_invSqrt +* Description : 计算1/sqrt(x)的快速的算法 +* Date : 2020/6/30 logzhan +*---------------------------------------------------------------------**/ +float InvSqrt(float x) { + float xHalf = 0.5f * x; + int i = *(int *)&x; + i = 0x5f3759df - (i >> 1); + x = *(float *)&i; + x = x * (1.5f - xHalf * x * x); + return x; +} + +/**--------------------------------------------------------------------- +* Function : Vec3Norm +* Description : 三维浮点数向量归一化 +* Date : 2022/11/1 logzhan +*---------------------------------------------------------------------**/ +void Vec3Norm(float* vx, float* vy, float* vz) +{ + float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) + + (*vz) * (*vz))); + // 防止出现模为0的情况 + if (norm > FLT_THRES) { + norm = 1 / norm; + (*vx) *= norm; (*vy) *= norm; (*vz) *= norm; + } +} + + + +/**---------------------------------------------------------------------- +* Function : Motion2TypeStr +* Description : 把用户运动类型转换为字符串输出 +* Date : 2022/9/16 logzhan +*---------------------------------------------------------------------**/ +const char* Motion2TypeStr(int type) +{ + if (type == PDR_MOTION_TYPE_STATIC) { + return "STATIC"; + } + else if (type == PDR_MOTION_TYPE_IRREGULAR) { + return "IRREGULAR"; + } + else if (type == PDR_MOTION_TYPE_HANDHELD) { + return "HANDLED"; + } + else if (type == PDR_MOTION_TYPE_SWINGING) { + return "SWINGING"; + } + return "UNKOWN"; +} + +/**---------------------------------------------------------------------- +* Function : qnb2att +* Description : 四元数转欧拉角 +* Date : 2020/7/4 logzhan +*---------------------------------------------------------------------**/ +void Qnb2Att(float* q, double* attitude) +{ + double q11 = q[0] * q[0]; double q13 = q[0] * q[2]; + double q12 = q[0] * q[1]; double q14 = q[0] * q[3]; + double q22 = q[1] * q[1]; double q24 = q[1] * q[3]; + double q23 = q[1] * q[2]; double q33 = q[2] * q[2]; + double q34 = q[2] * q[3]; double q44 = q[3] * q[3]; + + if (fabs(q34 + q12) <= 0.5) { + attitude[0] = asin(2 * (q34 + q12)); + } + attitude[1] = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44); + attitude[2] = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44); +} + + + +void GetCovMatrix(double *x, double *y, double cov[2][2], int n) { + int i; + double sum_x = 0, sum_y = 0, sum_xy = 0; + double m_x = 0, m_y = 0; + m_x = mean(x, n); + m_y = mean(y, n); + for (i = 0; i < n; i++) { + sum_xy += (x[i] - m_x)*(y[i] - m_y); + sum_x += (x[i] - m_x)*(x[i] - m_x); + sum_y += (y[i] - m_y)*(y[i] - m_y); + } + cov[0][0] = sum_x / n; + cov[0][1] = sum_xy / n; + cov[1][0] = sum_xy / n; + cov[1][1] = sum_y / n; +} + +int Jacobi(double a[][2], double p[][2], int n, double eps, int T) +{ + int i, j, k; + double max = a[0][1]; + int row = 0; + int col = 0; + int ite_num = 0; + double theta = 0; + double aii = 0; + double ajj = 0; + double aij = 0; + double sin_theta = 0; + double cos_theta = 0; + double sin_2theta = 0; + double cos_2theta = 0; + double arowk = 0; + double acolk = 0; + double pki = 0; + double pkj = 0; + + for (i = 0; i < n; i++) + p[i][i] = 1; + while (1) + { + max = fabs(a[0][1]); + row = 0; + col = 1; + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) + if (i != j && fabs(a[i][j])>max) + { + max = fabs(a[i][j]); + row = i; + col = j; + } + if (max < eps) + { + //printf("max : %lf \n", max); + //printf("t : %lf \n", ite_num); + + return 1; + } + + if (ite_num>T) + { + //printf("max : %lf \n", max); + //printf("t : %lf \n", ite_num); + return 0; + } + + if (a[row][row] == a[col][col]) + theta = PI / 4; + else + theta = 0.5*atan(2 * a[row][col] / (a[row][row] - a[col][col])); + aii = a[row][row]; + ajj = a[col][col]; + aij = a[row][col]; + sin_theta = sin(theta); + cos_theta = cos(theta); + sin_2theta = sin(2 * theta); + cos_2theta = cos(2 * theta); + a[row][row] = aii * cos_theta*cos_theta + ajj * sin_theta*sin_theta + aij * sin_2theta; + a[col][col] = aii * sin_theta*sin_theta + ajj * cos_theta*cos_theta - aij * sin_2theta; + a[row][col] = 0.5*(ajj - aii)*sin_2theta + aij * cos_2theta; + a[col][row] = a[row][col]; + for (k = 0; k < n; k++) + { + if (k != row && k != col) + { + arowk = a[row][k]; + acolk = a[col][k]; + a[row][k] = arowk * cos_theta + acolk * sin_theta; + a[k][row] = a[row][k]; + a[col][k] = acolk * cos_theta - arowk * sin_theta; + a[k][col] = a[col][k]; + } + } + if (ite_num == 0) + { + p[row][row] = cos_theta; + p[row][col] = -sin_theta; + p[col][row] = sin_theta; + p[col][col] = cos_theta; + } + else + { + for (k = 0; k < n; k++) + { + pki = p[k][row]; + pkj = p[k][col]; + p[k][row] = pki * cos_theta + pkj * sin_theta; + p[k][col] = pkj * cos_theta - pki * sin_theta; + } + } + ite_num++; + } +} + +static int isRealLat(double lat){ return (lat < 90) && (lat > -90); } +static int isRealLon(double lon){ return (lon < 180) && (lon > -180); } + +/* +function: 计算地图上两个由经纬度表示点间的直线距离。 +param @ pointA : 点A的经纬度 +param @ pointB : 点B的经纬度 +return @ dis : A、B点间的距离；若小于0则说明A或B点的经纬度输入有误。 +*/ +double CalDistance(LatLng_t pointA, LatLng_t pointB) +{ + double dis = -1; + double lla1[3] = { 0 }; + double lla2[3] = { 0 }; + double ned1[3] = { 0 }; + double ned2[3] = { 0 }; + double diff_ned[3] = { 0 }; + + lla1[0] = pointA.lat * RADIAN_PER_DEG; + lla1[1] = pointA.lon * RADIAN_PER_DEG; + lla2[0] = pointB.lat * RADIAN_PER_DEG; + lla2[1] = pointB.lon * RADIAN_PER_DEG; + + if (isRealLat(pointA.lat) && isRealLon(pointA.lon) + && isRealLat(pointB.lat) && isRealLon(pointB.lon)) + { + WGS842NED(lla1, lla1, ned1); + WGS842NED(lla2, lla1, ned2); + diff_ned[0] = ned1[0] - ned2[0]; + diff_ned[1] = ned1[1] - ned2[1]; + diff_ned[2] = ned1[2] - ned2[2]; + + dis = sqrt(pow(diff_ned[0], 2) + pow(diff_ned[1], 2) + pow(diff_ned[2], 2)); + } + return dis; +} + + + +LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB) +{ + + /* local variables declaration */ + double gradxB_P; /* linePointB 向 point 的 x 梯度 */ + double gradxB_A; /* linePointA 向 linePointB 的 x 梯度 */ + double gradyB_A; /* linePointA 向 linePointB 的 y 梯度 */ + double normPow; /* linePointA 向 linePointB 梯度的平方和 */ + double scalarMultip; /* gradxB_A * gradyB_A 的标量乘法 */ + LatLng_t resProjPoint; /* 结果点坐标 */ + + /* block process */ + gradxB_A = linePointA.lat - linePointB.lat; + gradyB_A = linePointA.lon - linePointB.lon; + if ((fabs(gradxB_A) < DBL_EPSILON) && (fabs(gradyB_A) < DBL_EPSILON)) + { + /* 当linePointA 和linePointB是同一点时，返回的point坐标 */ + resProjPoint.lat = point.lat; + resProjPoint.lon = point.lon; + } + else + { + gradxB_P = point.lat - linePointB.lat; + normPow = pow(gradxB_A, 2) + pow(gradyB_A, 2); + resProjPoint.lon = (double)( + (gradxB_P * gradxB_A * gradyB_A + point.lon * pow(gradyB_A, 2) + linePointB.lon * pow(gradxB_A, 2)) / normPow); + if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON)) + { + /* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/ + scalarMultip = (fabs(gradxB_A) < DBL_EPSILON) ? gradyB_A : gradxB_A; + } + else + { + scalarMultip = gradxB_A * gradyB_A; + } + resProjPoint.lat = (double)( + (gradxB_A * linePointB.lat * gradyB_A + gradxB_A * gradxB_A * (resProjPoint.lon - linePointB.lon)) / scalarMultip); + if (fabs(gradxB_A) < DBL_EPSILON) + { + resProjPoint.lat = linePointA.lat; + } + if (fabs(gradyB_A) < DBL_EPSILON) + { + resProjPoint.lon = point.lon; + } + } + + return resProjPoint; +} + +/**---------------------------------------------------------------------- +* Function : CalEarthParameter +* Description : 根据输入纬度返回该纬度地球相关参数 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +EarthData_t CalEarthParameter(double lat) +{ + double temp_value = 0; + EarthData_t earthData = { 0 }; + + if (fabs(lat) >= PI / 2)lat = 0.0f; + + temp_value = sqrt(1 - WGS84_ECCENTR2 * (sin(lat)*sin(lat))); + + earthData.lat = lat; + earthData.rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (temp_value*temp_value*temp_value); + earthData.rnh = WGS84_RE * cos(lat) / temp_value; + return earthData; +} + +/**---------------------------------------------------------------------- +* Function : pdr_CalRadianDifferent +* Description : 计算弧度之差 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +double CalRadianDifferent(double s_dir, double d_dir) +{ + double dirDiff = d_dir - s_dir; + if (fabs(dirDiff) < PI)return dirDiff; + return dirDiff > 0 ? dirDiff - 2 * PI : dirDiff + 2 * PI; +} + +void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]) +{ + if (fabs(fabs(yaw) - PI / 2) < DOUBLE_ZERO) + { + result[0] = point1[0]; + result[1] = point2[1]; + return; + } + + EarthData_t earthData = CalEarthParameter(point1[0]); + double k = -tan(yaw); + double lonDiff = (point2[1] - point1[1]) * earthData.rnh; + double latDiff = (point2[0] - point1[0]) * earthData.rmh; + + double x = (k*latDiff + k * k * lonDiff) / (1 + k * k); + double y = (latDiff + k * lonDiff) / (1 + k * k); + + result[1] = point1[1] + x / earthData.rnh; + result[0] = point1[0] + y / earthData.rmh; +} + +/**---------------------------------------------------------------------- +* Function : pdr_WriteCsvStr +* Description : 将字符串信息写入文本，主要是对fprintf封装一层 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +void WriteCsvStr(FILE* fp_write, char* str) +{ + if (NULL == fp_write)return; + fprintf(fp_write, "%s\n", str); +} + +int pdr_min(int a, int b){ + return (a > b ? b : a); +} + +/**---------------------------------------------------------------------- +* Function : pdr_utc2hms +* Description : 将UTC时间转换为时分秒 +* Date : 2020/7/8 logzhan +*---------------------------------------------------------------------**/ +void pdr_utc2hms(double utc, double* h, double* m, double* s) { + + long lutc = (long)(utc / 1000); + + lutc = lutc % (3600 * 24); + + *h = floor((double)(lutc / (3600))); + *m = floor((lutc - (*h) * 3600) / 60); + *s = lutc - (*h) * 3600 - (*m) * 60; + +} diff --git a/1.Software/PDR 1.06/src/imu.c b/1.Software/PDR 1.06/src/imu.c new file mode 100644 index 0000000..b86f63c --- /dev/null +++ b/1.Software/PDR 1.06/src/imu.c @@ -0,0 +1,7 @@ +锘/* Header File Including ------------------------------------------------------------------------ */ +#include +#include +#include +#include "imu.h" +#include "ahrs.h" +#include "detector.h" diff --git a/1.Software/PDR 1.06/src/m_gps.c b/1.Software/PDR 1.06/src/m_gps.c new file mode 100644 index 0000000..71b154a --- /dev/null +++ b/1.Software/PDR 1.06/src/m_gps.c @@ -0,0 +1,7 @@ +/* Header File Including ------------------------------------------------------------------------ */ +#include +#include +#include "m_gps.h" +#include "pdr_util.h" +#include "pdr.h" +