整理计步器
parent
5bd0ca97f2
commit
d20b742a36
|
|
@ -1,25 +1,10 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
/******************** (C) COPYRIGHT 2017 Geek************************************
|
/******************** (C) COPYRIGHT 2023 Geek************************************
|
||||||
* File Name : Steps_Algorithm.h
|
* File Name : Pedometer.h
|
||||||
* Department : Sensor Team
|
* Department : Sensor Team
|
||||||
* Current Version : V1.0
|
* Current Version : V1.0.0
|
||||||
* Author : Xiaoyong Li
|
* Author : logzhan
|
||||||
* Date of Issued : 2017.05.8
|
* Date of Issued : 2023.02.24
|
||||||
* 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
|
|
||||||
*******************************************************************************/
|
*******************************************************************************/
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
|
|
@ -29,66 +14,45 @@ extern "C" {
|
||||||
#define _STEPS_ALGORITHM_H_
|
#define _STEPS_ALGORITHM_H_
|
||||||
|
|
||||||
#include "pdr_sensor.h"
|
#include "pdr_sensor.h"
|
||||||
#define ACC_SAMPLE_RATE_100HZ
|
#define IMU_SAMPLE_FREQ 100 // IMU采样频率100Hz
|
||||||
//#define ACC_SAMPLE_RATE_25HZ
|
#define MS2S 1000 // 秒转毫秒单位
|
||||||
|
|
||||||
#ifdef ACC_SAMPLE_RATE_25HZ
|
#define DOWN_SAMPLE_NUM 4 // 降采样数量
|
||||||
#define DownResampleTimes 1
|
#define ACC_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // ACC采样间隔10ms
|
||||||
#define ACC_SAMPLE_TIME 40
|
#define GYRO_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // GYR采样间隔10ms
|
||||||
#define SAMPLE_TIME_MIN 36
|
#define SAMPLE_TIME_MIN 9
|
||||||
#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 SampleFrequency 25
|
#define PEDOMETER_FREQ IMU_SAMPLE_FREQ / DOWN_SAMPLE_NUM // 计步器的频率是
|
||||||
#define SampleFrequencyFour 100
|
#define SampleFrequencyFour 100
|
||||||
#define SampleFrequencydouble 50
|
#define SampleFrequencydouble 50
|
||||||
#define SampleFrequencyOPF (37.5l)
|
#define SampleFrequencyOPF (37.5l)
|
||||||
#define SampleFrequencySix 150
|
#define SampleFrequencySix 150
|
||||||
#define AvergeFilterOrder 5
|
#define AvergeFilterOrder 5
|
||||||
#define AxisNumber 4
|
#define AXIS_NUM 4 // 轴的数量有4个: x、y、z、xyz
|
||||||
#define FiltDifNumber 4
|
#define FILTER_NUM 4
|
||||||
#define AxisNumberFiltOut 16 //(AxisNumber * FiltDifNumber)
|
#define FiltDifNumber 4
|
||||||
#define AxisNumberStepSaveState 20 //(AxisNumberFiltOut + FiltDifNumber)
|
#define FILTER_AXIS_NUM AXIS_NUM * FILTER_NUM // 轴数量4，滤波器数量4，总共16轴滤波器输出
|
||||||
#define AccelerationNumber 39
|
#define AxisNumberStepSaveState 20 // AxisNumberFiltOut + FiltDifNumber
|
||||||
#define ORI_ACC_SAMPLE_RATE 100 //origin acc sampleFrequency is 100 hz
|
#define ACC_BUFF_NUM 39
|
||||||
#define WIN_LENGTH_AVE_DEVI 50 //
|
#define WIN_LENGTH_AVE_DEVI 50 //
|
||||||
#define PI2A (180/3.1415926)
|
#define PI2A (180/3.1415926)
|
||||||
//#define NSTOS 1000000000
|
|
||||||
#define MS2S 1000
|
|
||||||
|
|
||||||
|
#define STARTSTEPS 8
|
||||||
|
|
||||||
#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_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_WINDOWS
|
||||||
//#define PLATFORM_PC_LINUX
|
|
||||||
|
|
||||||
|
|
||||||
#if defined PLATFORM_ANDROID_QCOM_ADSP
|
#if defined PLATFORM_ANDROID_QCOM_ADSP
|
||||||
#define STEPLIB_MEMSET SNS_OS_MEMSET
|
#define STEPLIB_MEMSET SNS_OS_MEMSET
|
||||||
|
|
||||||
#if 1
|
#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
|
#endif
|
||||||
#elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845
|
#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
|
#define STEPLIB_MEMSET memset
|
||||||
|
|
@ -108,38 +72,39 @@ typedef struct {
|
||||||
double walk;
|
double walk;
|
||||||
double walkRealTime;
|
double walkRealTime;
|
||||||
double nD;
|
double nD;
|
||||||
}stepsDfine;
|
}StepsDefine_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
nonSteps = 0,
|
nonSteps = 0,
|
||||||
walk,
|
walk,
|
||||||
}enumState;
|
run,
|
||||||
|
}StepState_t;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
unsigned char sign;
|
unsigned char sign;
|
||||||
enumState state;
|
StepState_t state;
|
||||||
unsigned long long stateStartTime;
|
unsigned long long stateStartTime;
|
||||||
unsigned long long stepsStartTime;
|
unsigned long long stepsStartTime;
|
||||||
}stateNowDfine;
|
}stateNowDfine;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
enumState state;
|
StepState_t state;
|
||||||
unsigned long long startTimeStamp;
|
unsigned long long startTimeStamp;
|
||||||
}stateDfine;
|
}StateDef_t;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
int flag;
|
int flag;
|
||||||
double ax;
|
double ax;
|
||||||
double ay;
|
double ay;
|
||||||
double az;
|
double az;
|
||||||
}downSample;
|
}DownSample_t;
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : getPedometerLibVersion
|
* Function : Pedometer_GetVersion
|
||||||
* Description : 获取计步器版本
|
* Description : 获取计步器版本
|
||||||
* Date : 2020/2/20
|
* Date : 2023/02/24
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
const char* GetPedometerVersion(void);
|
const char* Pedometer_GetVersion(void);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : initPedometer
|
* Function : initPedometer
|
||||||
|
|
@ -149,28 +114,28 @@ const char* GetPedometerVersion(void);
|
||||||
void Pedometer_Init(void);
|
void Pedometer_Init(void);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : reinitPedometer
|
* Function : Pedometer_ReInit
|
||||||
* Description : PDR ÖØгõʼ»¯¼Æ²½Æ÷
|
* Description : 重新初始化计步器
|
||||||
* Date : 2020/2/20 logzhan
|
* Date : 2023/02/24 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void reinitPedometer(void);
|
void Pedometer_ReInit(void);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : updatePedometer
|
* Function : PedometerUpdate
|
||||||
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
|
* 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* ss_data, unsigned long* step);
|
||||||
|
|
||||||
void Steps_State_Detect(enumState* state);
|
|
||||||
|
|
||||||
|
void Pedometer_GetState(StepState_t* state);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : detVehicleMode
|
* Function : DetVehicleMode
|
||||||
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
|
* 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
|
#endif
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -134,7 +134,7 @@ int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) {
|
||||||
if (pdr.MotionType == DETECTOR_TYPE_HANDHELD)pdr.motionFreq = 0.1f;
|
if (pdr.MotionType == DETECTOR_TYPE_HANDHELD)pdr.motionFreq = 0.1f;
|
||||||
|
|
||||||
// 计步器输入传感器数据，占10%的运行时间
|
// 计步器输入传感器数据，占10%的运行时间
|
||||||
PedometerUpdate(ImuData, &pdr.steps);
|
Pedometer_Update(ImuData, &pdr.steps);
|
||||||
|
|
||||||
// ----- Filter Initialization ------- //
|
// ----- Filter Initialization ------- //
|
||||||
if (pdr.sysStatus == IS_INITIAL) {
|
if (pdr.sysStatus == IS_INITIAL) {
|
||||||
|
|
|
||||||
|
|
@ -14,62 +14,57 @@
|
||||||
|
|
||||||
const static char* Pedometer_Version = "Pedometer.1.0.0.20230224";
|
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 State_Detect(unsigned long long timeStamp);
|
||||||
static void Steps_Filt(void);
|
static void Pedometer_Filter(void);
|
||||||
static void Steps_Feature(void);
|
static void Steps_Feature(void);
|
||||||
static void Steps_Rcording(unsigned long long timeStamp);
|
static void Steps_Rcording(unsigned long long timeStamp);
|
||||||
|
|
||||||
|
static double AccBuffer[AXIS_NUM][ACC_BUFF_NUM];
|
||||||
static double stepsFiltAcc[AxisNumber][AccelerationNumber];
|
|
||||||
static float AccData[ACC_INSPECT_TIMEWIDTH] = { 0 };
|
static float AccData[ACC_INSPECT_TIMEWIDTH] = { 0 };
|
||||||
static float lastAccZ = 0;
|
static float lastAccZ = 0;
|
||||||
|
|
||||||
static unsigned int ReceivedHowManyAcc = 0;
|
static unsigned int ReceivedHowManyAcc = 0;
|
||||||
static double SumAcc = 0;
|
static double SumAcc = 0;
|
||||||
|
|
||||||
static double DifFiltCoefficientOne[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, 0.234567901234568 };
|
static double DifFiltCoefficient1[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 DifFiltCoefficient2[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 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 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 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 DifFiltOutput[FILTER_AXIS_NUM][5];
|
||||||
static double OutputFilt[AxisNumberFiltOut][2];
|
static double OutputFilt[FILTER_AXIS_NUM][2];
|
||||||
static double StepLcVar[AxisNumberFiltOut][3];
|
static double StepLcVar[FILTER_AXIS_NUM][3];
|
||||||
static char StepReFeature[AxisNumberFiltOut] = { 0 };
|
static char StepReFeature[FILTER_AXIS_NUM] = { 0 };
|
||||||
static double StepFetureAmp[AxisNumberFiltOut][8];
|
static double StepFetureAmp[FILTER_AXIS_NUM][8];
|
||||||
static unsigned short StepFetureTime[AxisNumberFiltOut][8];
|
static unsigned short StepFetureTime[FILTER_AXIS_NUM][8];
|
||||||
static double StepRecordeState[AxisNumberStepSaveState][4];
|
static double StepRecordeState[AxisNumberStepSaveState][4];
|
||||||
static double StepRecordeStateTime[AxisNumberStepSaveState][9];
|
static double StepRecordeStateTime[AxisNumberStepSaveState][9];
|
||||||
static unsigned long long timeStampSave[AxisNumberStepSaveState][2];
|
static unsigned long long timeStampSave[AxisNumberStepSaveState][2];
|
||||||
static unsigned short StepMaxTime = 100;
|
static unsigned short StepMaxTime = 100;
|
||||||
|
|
||||||
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ40ms,25HZ*/
|
|
||||||
static unsigned short StepMinTime = 10;
|
static unsigned short StepMinTime = 10;
|
||||||
static unsigned long long timeStamp = 0;
|
static unsigned long long timeStamp = 0;
|
||||||
static unsigned long long timeStamp_in_vehicle = 0;
|
static unsigned long long timeStamp_in_vehicle = 0;
|
||||||
static char Flag_whether_in_vehicle = 0;
|
static char Flag_whether_in_vehicle = 0;
|
||||||
static char Flag_step_exist = 0;
|
static char Flag_step_exist = 0;
|
||||||
|
|
||||||
static double AvergeFiltSave[AxisNumber - 1][AvergeFilterOrder]/* = { 0 }*/;
|
static double AvergeFiltSave[AXIS_NUM - 1][AvergeFilterOrder];
|
||||||
stepsDfine Steps = { 0, 0 };
|
StepsDefine_t Steps = { 0, 0 };
|
||||||
stepsRe ReSteps = { 0 };
|
stepsRe ReSteps = { 0 };
|
||||||
downSample DownSample = { 0, 0, 0, 0 };
|
DownSample_t DownSample = { 0, 0, 0, 0 };
|
||||||
|
|
||||||
static double StateDetectLcvar[5] = { 0 };
|
|
||||||
static enumState stateLcVar[2] = { nonSteps, nonSteps };
|
|
||||||
|
|
||||||
|
|
||||||
|
static double StateDetectLcVar[5] = { 0 };
|
||||||
|
static StepState_t stateLcVar[2] = { nonSteps, nonSteps };
|
||||||
stateNowDfine stateNow;
|
stateNowDfine stateNow;
|
||||||
stateDfine Restate;
|
StateDef_t Restate;
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : GetPedometerVersion
|
* Function : Pedometer_GetVersion
|
||||||
* Description : 获取计步器版本
|
* Description : 获取计步器版本
|
||||||
* Date : 2023/02/24
|
* Date : 2023/02/24
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
const char* GetPedometerVersion(void)
|
const char* Pedometer_GetVersion(void){
|
||||||
{
|
|
||||||
return Pedometer_Version;
|
return Pedometer_Version;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -80,7 +75,7 @@ const char* GetPedometerVersion(void)
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void Pedometer_Init(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(DifFiltOutput, 0, sizeof(DifFiltOutput));
|
||||||
STEPLIB_MEMSET(OutputFilt, 0, sizeof(OutputFilt));
|
STEPLIB_MEMSET(OutputFilt, 0, sizeof(OutputFilt));
|
||||||
STEPLIB_MEMSET(StepLcVar, 0, sizeof(StepLcVar));
|
STEPLIB_MEMSET(StepLcVar, 0, sizeof(StepLcVar));
|
||||||
|
|
@ -95,7 +90,7 @@ void Pedometer_Init(void)
|
||||||
timeStamp = 0;
|
timeStamp = 0;
|
||||||
STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
|
STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
|
||||||
STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
|
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(stateLcVar, 0, sizeof(stateLcVar));
|
||||||
STEPLIB_MEMSET(&stateNow, 0, sizeof(stateNow));
|
STEPLIB_MEMSET(&stateNow, 0, sizeof(stateNow));
|
||||||
STEPLIB_MEMSET(&Restate, 0, sizeof(Restate));
|
STEPLIB_MEMSET(&Restate, 0, sizeof(Restate));
|
||||||
|
|
@ -113,42 +108,41 @@ void Pedometer_Init(void)
|
||||||
* Description : PDR 重新初始化计步器
|
* Description : PDR 重新初始化计步器
|
||||||
* Date : 2020/2/20 logzhan
|
* Date : 2020/2/20 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void reinitPedometer(void)
|
void Pedometer_ReInit(void)
|
||||||
{
|
{
|
||||||
timeStamp = 0;
|
timeStamp = 0;
|
||||||
STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
|
STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
|
||||||
STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
|
STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : updatePedometer
|
* Function : Pedometer_Update
|
||||||
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
|
* 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;
|
unsigned char k = 0;
|
||||||
|
|
||||||
// 检测当前是否属于开车状态, 这个函数也是相对耗时的, 这个函数只用
|
// 检测当前是否属于开车状态, 这个函数也是相对耗时的, 这个函数只用
|
||||||
// 了加速度计的z轴, 是不是存在问题？
|
// 了加速度计的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;
|
timeStamp += ACC_SAMPLE_TIME;
|
||||||
DownSample.ax += ss_data->acc.s[0] / 9.8;
|
DownSample.ax += imu->acc.s[0] / 9.8;
|
||||||
DownSample.ay += ss_data->acc.s[1] / 9.8;
|
DownSample.ay += imu->acc.s[1] / 9.8;
|
||||||
DownSample.az += ss_data->acc.s[2] / 9.8;
|
DownSample.az += imu->acc.s[2] / 9.8;
|
||||||
DownSample.flag++;
|
DownSample.flag++;
|
||||||
|
|
||||||
if (DownSample.flag == DOWN_SAMPLE_TIMES)
|
if (DownSample.flag == DOWN_SAMPLE_NUM)
|
||||||
{
|
{
|
||||||
if (Restate.startTimeStamp == 0)
|
if (Restate.startTimeStamp == 0)
|
||||||
{
|
{
|
||||||
Restate.startTimeStamp = timeStamp;
|
Restate.startTimeStamp = timeStamp;
|
||||||
}
|
}
|
||||||
DownSample.ax /= DOWN_SAMPLE_TIMES;
|
DownSample.ax /= DOWN_SAMPLE_NUM;
|
||||||
DownSample.ay /= DOWN_SAMPLE_TIMES;
|
DownSample.ay /= DOWN_SAMPLE_NUM;
|
||||||
DownSample.az /= DOWN_SAMPLE_TIMES;
|
DownSample.az /= DOWN_SAMPLE_NUM;
|
||||||
for (k = 0; k < AvergeFilterOrder - 1; k++)
|
for (k = 0; k < AvergeFilterOrder - 1; k++)
|
||||||
{
|
{
|
||||||
AvergeFiltSave[0][k] = AvergeFiltSave[0][k + 1];
|
AvergeFiltSave[0][k] = AvergeFiltSave[0][k + 1];
|
||||||
|
|
@ -170,15 +164,15 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step)
|
||||||
DownSample.ay /= AvergeFilterOrder;
|
DownSample.ay /= AvergeFilterOrder;
|
||||||
DownSample.az /= AvergeFilterOrder;
|
DownSample.az /= AvergeFilterOrder;
|
||||||
|
|
||||||
// 计步器的加速度更新
|
// 计步器缓存Buffer更新
|
||||||
Steps_Acc_Save(DownSample.ax, DownSample.ay, DownSample.az);
|
AccBufferUpdate(DownSample.ax, DownSample.ay, DownSample.az);
|
||||||
// 计步器状态检测
|
// 计步器状态检测
|
||||||
State_Detect(timeStamp);
|
State_Detect(timeStamp);
|
||||||
|
|
||||||
if (StateDetectLcvar[4] < 0.5l)
|
if (StateDetectLcVar[4] < 0.5l)
|
||||||
{
|
{
|
||||||
// 滤波处理
|
// 滤波处理
|
||||||
Steps_Filt();
|
Pedometer_Filter();
|
||||||
// 计步特征
|
// 计步特征
|
||||||
Steps_Feature();
|
Steps_Feature();
|
||||||
// 步数记录
|
// 步数记录
|
||||||
|
|
@ -189,7 +183,8 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step)
|
||||||
}
|
}
|
||||||
// 如果计步标志位不存在，那么不计步数
|
// 如果计步标志位不存在，那么不计步数
|
||||||
if (Flag_step_exist == 0){
|
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)
|
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 检测汽车模式, 如果是检测到汽车模式，那么不计步数
|
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
|
||||||
* Date : 2020/2/1 logzhan
|
* Date : 2020/2/1 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void detVehicleMode(double ax, double ay, double az)
|
void DetVehicleMode(double ax, double ay, double az)
|
||||||
{
|
{
|
||||||
int t;
|
int t;
|
||||||
int index_move;
|
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的Buffer缓存
|
||||||
|
* Date : 2023/02/25
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
static void AccBufferUpdate(double ax, double ay, double az)
|
||||||
{
|
{
|
||||||
short i, j;
|
for (uint8_t i = 0; i < AXIS_NUM; i++){
|
||||||
for (i = 0; i < AxisNumber; i++){
|
for (uint8_t j = 0; j < (ACC_BUFF_NUM - 1); j++){
|
||||||
for (j = 0; j < (AccelerationNumber - 1); j++){
|
AccBuffer[i][j] = AccBuffer[i][j + 1];
|
||||||
stepsFiltAcc[i][j] = stepsFiltAcc[i][j + 1];
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
stepsFiltAcc[0][AccelerationNumber - 1] = ax;
|
AccBuffer[0][ACC_BUFF_NUM - 1] = ax;
|
||||||
stepsFiltAcc[1][AccelerationNumber - 1] = ay;
|
AccBuffer[1][ACC_BUFF_NUM - 1] = ay;
|
||||||
stepsFiltAcc[2][AccelerationNumber - 1] = az;
|
AccBuffer[2][ACC_BUFF_NUM - 1] = az;
|
||||||
stepsFiltAcc[3][AccelerationNumber - 1] = (sqrt(ax*ax + ay*ay + az*az));
|
AccBuffer[3][ACC_BUFF_NUM - 1] = sqrt(ax*ax + ay*ay + az*az);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/**---------------------------------------------------------------------
|
||||||
DifFiltOutput <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*5
|
* Function : Pedometer_Filter
|
||||||
OutputFilt <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*2
|
* Description : 计步器滤波器,一共有4个频段的低通滤波器。针对x,y,z,mod(x,y,z)
|
||||||
0 filter one and acc x
|
* 的数据进行滤波处理，一共得到16轴的输出数据。
|
||||||
1 filter one and acc y
|
* Date : 2023/02/25
|
||||||
2 filter one and acc z
|
*---------------------------------------------------------------------**/
|
||||||
3 filter one and acc xyz
|
static void Pedometer_Filter(void)
|
||||||
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
|
|
||||||
{
|
{
|
||||||
short i, j;
|
short i, j;
|
||||||
double FiltSum;
|
double FiltSum;
|
||||||
for (i = 0; i < AxisNumberFiltOut; i++)
|
for (i = 0; i < FILTER_AXIS_NUM; i++){
|
||||||
{
|
|
||||||
DifFiltOutput[i][0] = DifFiltOutput[i][1];
|
DifFiltOutput[i][0] = DifFiltOutput[i][1];
|
||||||
DifFiltOutput[i][1] = DifFiltOutput[i][2];
|
DifFiltOutput[i][1] = DifFiltOutput[i][2];
|
||||||
DifFiltOutput[i][2] = DifFiltOutput[i][3];
|
DifFiltOutput[i][2] = DifFiltOutput[i][3];
|
||||||
DifFiltOutput[i][3] = DifFiltOutput[i][4];
|
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;
|
FiltSum = 0;
|
||||||
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ5*/
|
|
||||||
for (j = 0; j < 4; j++)
|
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);
|
DifFiltOutput[i][4] = (FiltSum);
|
||||||
|
|
||||||
FiltSum = 0;
|
FiltSum = 0;
|
||||||
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ9*/
|
|
||||||
for (j = 0; j < 8; j++)
|
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];
|
FiltSum += DifFiltCoefficient2[8] * AccBuffer[i][19];
|
||||||
DifFiltOutput[i + AxisNumber][4] = (FiltSum);
|
DifFiltOutput[i + AXIS_NUM][4] = (FiltSum);
|
||||||
|
|
||||||
FiltSum = 0;
|
FiltSum = 0;
|
||||||
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ13*/
|
for (j = 0; j < 12; j++){
|
||||||
for (j = 0; j < 12; j++)
|
FiltSum += DifFiltCoefficient3[j] * (AccBuffer[i][7 + j] + AccBuffer[i][31 - j]);
|
||||||
{
|
|
||||||
FiltSum += DifFiltCoefficientThree[j] * (stepsFiltAcc[i][7 + j] + stepsFiltAcc[i][31 - j]);
|
|
||||||
}
|
}
|
||||||
FiltSum += DifFiltCoefficientThree[12] * stepsFiltAcc[i][19];
|
FiltSum += DifFiltCoefficient3[12] * AccBuffer[i][19];
|
||||||
DifFiltOutput[i + AxisNumber * 2][4] = (FiltSum);
|
DifFiltOutput[i + AXIS_NUM * 2][4] = (FiltSum);
|
||||||
|
|
||||||
FiltSum = 0;
|
FiltSum = 0;
|
||||||
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ17*/
|
for (j = 0; j < 16; j++){
|
||||||
for (j = 0; j < 16; j++)
|
FiltSum += DifFiltCoefficient4[j] * (AccBuffer[i][3 + j] + AccBuffer[i][35 - j]);
|
||||||
{
|
|
||||||
FiltSum += DifFiltCoefficientFour[j] * (stepsFiltAcc[i][3 + j] + stepsFiltAcc[i][35 - j]);
|
|
||||||
}
|
}
|
||||||
FiltSum += DifFiltCoefficientFour[16] * stepsFiltAcc[i][19];
|
FiltSum += DifFiltCoefficient4[16] * AccBuffer[i][19];
|
||||||
DifFiltOutput[i + AxisNumber * 3][4] = (FiltSum);
|
DifFiltOutput[i + AXIS_NUM * 3][4] = (FiltSum);
|
||||||
}
|
}
|
||||||
|
// 更新16个轴计步器的输出结果
|
||||||
for (i = 0; i < AxisNumberFiltOut; i++)
|
for (i = 0; i < FILTER_AXIS_NUM; i++){
|
||||||
{
|
|
||||||
OutputFilt[i][1] = DifFiltOutput[i][4];
|
OutputFilt[i][1] = DifFiltOutput[i][4];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
StepLcVar <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*3
|
|
||||||
StepLcVar[nA][0]<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.<EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|
||||||
StepLcVar[nA][1]<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD>ı<EFBFBD>־<EFBFBD><EFBFBD>1:<EFBFBD><EFBFBD><EFBFBD><EFBFBD> -1:<EFBFBD>½<EFBFBD> 0:<EFBFBD><EFBFBD>ʼֵ
|
|
||||||
StepLcVar[nA][2]<EFBFBD>洢<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵķ<EFBFBD>ֵ
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
StepFetureAmp <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*8 <EFBFBD><EFBFBD><EFBFBD>ڴ洢<EFBFBD><EFBFBD><EFBFBD>ֵ
|
|
||||||
StepFetureTime <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*8 <EFBFBD><EFBFBD><EFBFBD>ڴ洢<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
||||||
StepReFeature <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ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)
|
static void Steps_Feature(void)
|
||||||
{
|
{
|
||||||
short nA, nB;
|
short nA, nB;
|
||||||
for (nA = 0; nA < AxisNumberFiltOut; nA++)
|
for (nA = 0; nA < FILTER_AXIS_NUM; nA++)
|
||||||
{
|
{
|
||||||
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.ÿ<>μӣ<CEBC>*/
|
|
||||||
StepLcVar[nA][0] = StepLcVar[nA][0] + 1;
|
StepLcVar[nA][0] = StepLcVar[nA][0] + 1;
|
||||||
/*<2A>Ҳ<EFBFBD><D2B2>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(StepLcVar[nA][1] <1)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(OutputFilt[nA][1] > OutputFilt[nA][0])<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ұߵ<D2B1><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> StepLcVar[nA][1] = 1*/
|
if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) ||
|
||||||
if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) || ((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1))))
|
((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1))))
|
||||||
{
|
{
|
||||||
for (nB = 0; nB < 7; nB++)
|
for (nB = 0; nB < 7; nB++)
|
||||||
{
|
{
|
||||||
StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
|
StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
|
||||||
}
|
}
|
||||||
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ֵ=<3D><>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ-<2D><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ*/
|
|
||||||
StepFetureAmp[nA][7] = StepLcVar[nA][2] - OutputFilt[nA][0];
|
StepFetureAmp[nA][7] = StepLcVar[nA][2] - OutputFilt[nA][0];
|
||||||
/*<2A>洢<EFBFBD><E6B4A2>ǰ<EFBFBD><C7B0><EFBFBD>ȵķ<C8B5>ֵ*/
|
|
||||||
StepLcVar[nA][2] = OutputFilt[nA][0];
|
StepLcVar[nA][2] = OutputFilt[nA][0];
|
||||||
for (nB = 0; nB < 7; nB++)
|
for (nB = 0; nB < 7; nB++)
|
||||||
{
|
{
|
||||||
StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
|
StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
|
||||||
}
|
}
|
||||||
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ļ<EFBFBD><C4BC>ʱ<EFBFBD><CAB1>*/
|
|
||||||
StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
|
StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
|
||||||
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>Ⱥ<C8BA><F3A3ACBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|
||||||
StepLcVar[nA][0] = 0;
|
StepLcVar[nA][0] = 0;
|
||||||
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>Ⱥ<C8BA>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>״̬*/
|
|
||||||
StepLcVar[nA][1] = 1;
|
StepLcVar[nA][1] = 1;
|
||||||
StepReFeature[nA] = 1;
|
StepReFeature[nA] = 1;
|
||||||
}
|
}
|
||||||
|
|
@ -451,19 +391,14 @@ static void Steps_Feature(void)
|
||||||
{
|
{
|
||||||
StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
|
StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
|
||||||
}
|
}
|
||||||
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ֵ=<3D><>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ-<2D><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ*/
|
|
||||||
StepFetureAmp[nA][7] = OutputFilt[nA][0] - StepLcVar[nA][2];
|
StepFetureAmp[nA][7] = OutputFilt[nA][0] - StepLcVar[nA][2];
|
||||||
/*<2A>洢<EFBFBD><E6B4A2>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ķ<EFBFBD>ֵ*/
|
|
||||||
StepLcVar[nA][2] = OutputFilt[nA][0];
|
StepLcVar[nA][2] = OutputFilt[nA][0];
|
||||||
for (nB = 0; nB < 7; nB++)
|
for (nB = 0; nB < 7; nB++)
|
||||||
{
|
{
|
||||||
StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
|
StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
|
||||||
}
|
}
|
||||||
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ļ<EFBFBD><C4BC>ʱ<EFBFBD><CAB1>*/
|
|
||||||
StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
|
StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
|
||||||
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><F3A3ACBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|
||||||
StepLcVar[nA][0] = 0;
|
StepLcVar[nA][0] = 0;
|
||||||
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>Ⱥ<C8BA>¼<EFBFBD>Ͻ<EFBFBD>״̬*/
|
|
||||||
StepLcVar[nA][1] = -1;
|
StepLcVar[nA][1] = -1;
|
||||||
StepReFeature[nA] = 1;
|
StepReFeature[nA] = 1;
|
||||||
}
|
}
|
||||||
|
|
@ -477,10 +412,10 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
double StepFetureAmp12, StepFetureAmp34, StepFetureAmp56, StepFetureAmp78, StepFetureAmp14, StepFetureAmp58, StepFetureAmp18;
|
double StepFetureAmp12, StepFetureAmp34, StepFetureAmp56, StepFetureAmp78, StepFetureAmp14, StepFetureAmp58, StepFetureAmp18;
|
||||||
unsigned short StepFetureTime12, StepFetureTime34, StepFetureTime56, StepFetureTime78, StepFetureTime14, StepFetureTime58, StepFetureTime38, StepFetureTime18;
|
unsigned short StepFetureTime12, StepFetureTime34, StepFetureTime56, StepFetureTime78, StepFetureTime14, StepFetureTime58, StepFetureTime38, StepFetureTime18;
|
||||||
int j;
|
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];
|
nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
|
||||||
}
|
}
|
||||||
|
|
@ -488,23 +423,23 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
|
|
||||||
if (nD > 0)
|
if (nD > 0)
|
||||||
{
|
{
|
||||||
for (nC = 0; nC < AxisNumber - 1; nC++) // nC<AxisNumber;
|
for (nC = 0; nC < AXIS_NUM - 1; nC++) // nC<AxisNumber;
|
||||||
{
|
{
|
||||||
if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
|
if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
|
||||||
{
|
{
|
||||||
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]))
|
if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
|
||||||
{
|
{
|
||||||
nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
|
nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
Differ_Sum = 0;
|
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;
|
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)
|
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];
|
StepRecordeStateTime[nA][3] = StepRecordeStateTime[nA][3] + StepFetureTime[nA][7];
|
||||||
|
|
||||||
/*(nA % 4 == 3)Ϊ<>ϼ<EFBFBD><CFBC>ٶȾ<D9B6><C8BE>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
|
/*(nA % 4 == 3)Ϊ<>ϼ<EFBFBD><CFBC>ٶȾ<D9B6><C8BE>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
|
||||||
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;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] + 0.51;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] + StepFetureTime[nA][7];
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] + StepFetureTime[nA][7];
|
||||||
StepRecordeState[AxisNumberFiltOut + nA / 4][3] = StepRecordeState[AxisNumberFiltOut + nA / 4][3] + 0.51;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] + 0.51;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] + StepFetureTime[nA][7];
|
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*/
|
/*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
|
if (StepRecordeStateTime[nA][6] < 60) //25
|
||||||
{
|
{
|
||||||
|
|
@ -576,15 +511,15 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
/*(nA % 4 == 3)Ϊ<>ϼ<EFBFBD><CFBC>ٶȾ<D9B6><C8BE>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
|
/*(nA % 4 == 3)Ϊ<>ϼ<EFBFBD><CFBC>ٶȾ<D9B6><C8BE>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
|
||||||
if (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
|
else
|
||||||
{
|
{
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = 0;
|
||||||
}
|
}
|
||||||
if ((fabs(StepFetureAmp[nA][2] - StepFetureAmp[nA][3]) / (StepFetureAmp34) < 0.15l)
|
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)
|
&& (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 > StepMinTime)
|
||||||
&& (StepFetureTime58 < StepMaxTime))
|
&& (StepFetureTime58 < StepMaxTime))
|
||||||
{
|
{
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = SampleFrequency * 3.0 / StepFetureTime38;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][8] = PEDOMETER_FREQ * 3.0 / StepFetureTime38;
|
||||||
timeStampSave[AxisNumberFiltOut + nA / 4][1] = timeStamp;
|
timeStampSave[FILTER_AXIS_NUM + nA / 4][1] = timeStamp;
|
||||||
if (StepRecordeState[AxisNumberFiltOut + nA / 4][0] == 0)
|
if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] == 0)
|
||||||
{
|
{
|
||||||
StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + 3;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepFetureTime38;
|
||||||
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] + StepFetureTime38;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] + StepFetureTime38;
|
||||||
timeStampSave[AxisNumberFiltOut + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / SampleFrequency);
|
timeStampSave[FILTER_AXIS_NUM + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / PEDOMETER_FREQ);
|
||||||
}
|
}
|
||||||
else
|
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)))
|
&& ((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[FILTER_AXIS_NUM + nA / 4][4] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 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[FILTER_AXIS_NUM + nA / 4][4] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] + 3;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] + StepFetureTime38;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] + StepFetureTime38;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] = 0;
|
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];
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3];
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepRecordeStateTime[AxisNumberFiltOut + 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
|
else
|
||||||
{
|
{
|
||||||
StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + 3;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38;
|
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)
|
else if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] - StepFetureTime38 > StepMaxTime)
|
||||||
&& (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 11))
|
&& (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
|
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)
|
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
|
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
|
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
|
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;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] = 0;
|
||||||
StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + 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;
|
nB = AxisNumberStepSaveState - 1;
|
||||||
if (nD > 0)
|
if (nD > 0)
|
||||||
|
|
@ -704,15 +639,15 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
}
|
}
|
||||||
else
|
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;
|
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])
|
if (Steps.walkRealTime > StepRecordeState[nB][1])
|
||||||
{
|
{
|
||||||
|
|
@ -732,7 +667,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
//#endif
|
//#endif
|
||||||
for (nC = 0; nC < AxisNumberStepSaveState; nC++)
|
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;
|
StepRecordeState[nC][1] = 0;
|
||||||
StepRecordeStateTime[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;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = 0;
|
||||||
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = 0;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = 0;
|
||||||
StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0;
|
StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = 0;
|
||||||
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = 0;
|
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][8] = 0;
|
||||||
timeStampSave[AxisNumberFiltOut + nA / 4][0] = 0;
|
timeStampSave[FILTER_AXIS_NUM + nA / 4][0] = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -802,11 +737,11 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
if ((StepFetureTime58 >= StepMaxTime)
|
if ((StepFetureTime58 >= StepMaxTime)
|
||||||
|| ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l)))
|
|| ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l)))
|
||||||
{
|
{
|
||||||
StepRecordeStateTime[nA][8] = SampleFrequency * 8.0 / StepFetureTime18;
|
StepRecordeStateTime[nA][8] = PEDOMETER_FREQ * 8.0 / StepFetureTime18;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
StepRecordeStateTime[nA][8] = SampleFrequency * 4.0 / StepFetureTime18;
|
StepRecordeStateTime[nA][8] = PEDOMETER_FREQ * 4.0 / StepFetureTime18;
|
||||||
}
|
}
|
||||||
timeStampSave[nA][1] = timeStamp;
|
timeStampSave[nA][1] = timeStamp;
|
||||||
if (StepRecordeState[nA][0] == 0)
|
if (StepRecordeState[nA][0] == 0)
|
||||||
|
|
@ -823,7 +758,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
|
StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
|
||||||
}
|
}
|
||||||
StepRecordeStateTime[nA][1] = StepRecordeStateTime[nA][1] + StepFetureTime18;
|
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
|
else
|
||||||
{
|
{
|
||||||
|
|
@ -951,7 +886,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
StepRecordeState[nA][3] = 0;
|
StepRecordeState[nA][3] = 0;
|
||||||
StepRecordeStateTime[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;
|
nB = AxisNumberStepSaveState - 1;
|
||||||
if (nD > 0)
|
if (nD > 0)
|
||||||
|
|
@ -966,9 +901,9 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
}
|
}
|
||||||
else
|
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;
|
nB = nC;
|
||||||
}
|
}
|
||||||
|
|
@ -986,14 +921,10 @@ static void Steps_Rcording(unsigned long long timeStamp)
|
||||||
}
|
}
|
||||||
Steps.walkRealTime = 0;
|
Steps.walkRealTime = 0;
|
||||||
ReSteps.walkSteps = (unsigned long)(Steps.walk);
|
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++)
|
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;
|
StepRecordeState[nC][1] = 0;
|
||||||
StepRecordeStateTime[nC][1] = 0;
|
StepRecordeStateTime[nC][1] = 0;
|
||||||
|
|
@ -1204,119 +1135,114 @@ static void State_Detect(unsigned long long timeStamp)
|
||||||
double SumAccYZ = 0;
|
double SumAccYZ = 0;
|
||||||
double AccX = 0;
|
double AccX = 0;
|
||||||
double AccYZ = 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 (StateDetectLcVar[0] < StepMinTime)
|
||||||
#if defined(PLATFORM_ANDROID_QCOM_SLPI845)
|
|
||||||
(void)timeStamp;
|
|
||||||
#endif
|
|
||||||
if (StateDetectLcvar[0] < StepMinTime)
|
|
||||||
{
|
{
|
||||||
StateDetectLcvar[0] = StateDetectLcvar[0] + 1;
|
StateDetectLcVar[0] = StateDetectLcVar[0] + 1;
|
||||||
if (StateDetectLcvar[0] < 0.5l)
|
if (StateDetectLcVar[0] < 0.5l)
|
||||||
{
|
{
|
||||||
SumAcc = -1;
|
SumAcc = -1;
|
||||||
}
|
}else{
|
||||||
else
|
|
||||||
{
|
|
||||||
SumAcc = SumAcc - 1;
|
SumAcc = SumAcc - 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
SumAcc = 0;
|
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]);
|
SumAcc += fabs(AccBuffer[3][nA] - AccBuffer[3][nA + 1]);
|
||||||
SumAccX += fabs(stepsFiltAcc[0][nA] - stepsFiltAcc[0][nA + 1]);
|
SumAccX += fabs(AccBuffer[0][nA] - AccBuffer[0][nA + 1]);
|
||||||
SumAccYZ += fabs(sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA])
|
SumAccYZ += fabs(sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA])
|
||||||
- sqrt(stepsFiltAcc[1][nA + 1] * stepsFiltAcc[1][nA + 1] + stepsFiltAcc[2][nA + 1] * stepsFiltAcc[2][nA + 1]));
|
- sqrt(AccBuffer[1][nA + 1] * AccBuffer[1][nA + 1] + AccBuffer[2][nA + 1] * AccBuffer[2][nA + 1]));
|
||||||
}
|
}
|
||||||
SumAcc /= (AccelerationNumber - 1);
|
SumAcc /= (ACC_BUFF_NUM - 1);
|
||||||
SumAccX /= (AccelerationNumber - 1);
|
SumAccX /= (ACC_BUFF_NUM - 1);
|
||||||
SumAccYZ /= (AccelerationNumber - 1);
|
SumAccYZ /= (ACC_BUFF_NUM - 1);
|
||||||
for (nA = 0; nA < AccelerationNumber; nA++)
|
|
||||||
{
|
|
||||||
AccX += fabs(stepsFiltAcc[0][nA]);
|
|
||||||
}
|
|
||||||
AccX = AccX / AccelerationNumber;
|
|
||||||
|
|
||||||
for (nA = 0; nA < AccelerationNumber; nA++)
|
|
||||||
{
|
for (nA = 0; nA < ACC_BUFF_NUM; nA++){
|
||||||
AccYZ += sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][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 (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
|
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));
|
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 (((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{
|
}else{
|
||||||
StateDetectLcvar[1] = 0;
|
StateDetectLcVar[1] = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if ((StateDetectLcvar[1] > 4) || (SumAcc > 0.6l))
|
if ((StateDetectLcVar[1] > 4) || (SumAcc > 0.6l))
|
||||||
{
|
{
|
||||||
StateDetectLcvar[1] = 8;//10
|
StateDetectLcVar[1] = 8;//10
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if ((AccX < AccYZ) && (SumAcc > 0.25l))
|
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
|
else
|
||||||
{
|
{
|
||||||
StateDetectLcvar[1] = 0;
|
StateDetectLcVar[1] = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (StateDetectLcvar[1] > 4){
|
if (StateDetectLcVar[1] > 4){
|
||||||
stateLcVar[0] = 2;
|
stateLcVar[0] = 2;
|
||||||
}else{
|
}else{
|
||||||
stateLcVar[0] = walk;
|
stateLcVar[0] = walk;
|
||||||
}
|
}
|
||||||
StateDetectLcvar[0] = 0;
|
StateDetectLcVar[0] = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
void Steps_State_Detect(enumState* state){
|
void Pedometer_GetState(StepState_t* state){
|
||||||
*state = stateLcVar[0];
|
*state = stateLcVar[0];
|
||||||
}
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -0,0 +1,18 @@
|
||||||
|
#ifndef __PDR_FFT_H__
|
||||||
|
#define __PDR_FFT_H__
|
||||||
|
|
||||||
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <math.h>
|
||||||
|
#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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
||||||
|
|
@ -0,0 +1,43 @@
|
||||||
|
#pragma once
|
||||||
|
#ifndef _LOCATION_TOOL_H_
|
||||||
|
#define _LOCATION_TOOL_H_
|
||||||
|
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
#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_
|
||||||
|
|
@ -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 <string>
|
||||||
|
|
||||||
|
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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -0,0 +1,141 @@
|
||||||
|
#ifndef _PDR_UTIL_H_
|
||||||
|
#define _PDR_UTIL_H_
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
|
||||||
|
#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; /* 当地纬度（rad）*/
|
||||||
|
double wnie[3]; /* 地理系相对惯性系的角速度在导航系分量 */
|
||||||
|
} EarthData_t;
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : mean
|
||||||
|
* Description : 对输入double数组求均值
|
||||||
|
* 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 : 将字符串信息写入文本，主要是对fprintf封装一层
|
||||||
|
* Date : 2020/7/8 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void WriteCsvStr(FILE* fp_write, char* str);
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : pdr_utc2hms
|
||||||
|
* Description : 将UTC时间转换为时分秒
|
||||||
|
* 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
|
||||||
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -0,0 +1,15 @@
|
||||||
|
#ifndef _PDR_GPS_H_
|
||||||
|
#define _PDR_GPS_H_
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#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
|
||||||
|
|
@ -0,0 +1,111 @@
|
||||||
|
#ifndef __DETECTOR_H
|
||||||
|
#define __DETECTOR_H
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <time.h>
|
||||||
|
#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
|
||||||
|
|
@ -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 <stdint.h>
|
||||||
|
|
||||||
|
/* Macro Definition ----------------------------------------------------------*/
|
||||||
|
#define IMU_SENSOR_AXIS 3
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef _WIN32
|
||||||
|
#define PDR_LOGD printf
|
||||||
|
#define PDR_LOGE printf
|
||||||
|
#else
|
||||||
|
#include <android/log.h>
|
||||||
|
#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
|
||||||
|
|
||||||
|
|
@ -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__
|
||||||
|
|
@ -0,0 +1,207 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Debug|x64">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|x64">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\src\Buffer.c" />
|
||||||
|
<ClCompile Include="..\src\Complex.cpp" />
|
||||||
|
<ClCompile Include="..\src\CoordTrans.cpp" />
|
||||||
|
<ClCompile Include="..\src\Munkres.c" />
|
||||||
|
<ClCompile Include="..\src\AHRS.c" />
|
||||||
|
<ClCompile Include="..\src\Interface.c" />
|
||||||
|
<ClCompile Include="..\src\Detector.c" />
|
||||||
|
<ClCompile Include="..\src\Fft.c" />
|
||||||
|
<ClCompile Include="..\src\Filter.c" />
|
||||||
|
<ClCompile Include="..\src\Kalman.c" />
|
||||||
|
<ClCompile Include="..\src\LinearFit.c" />
|
||||||
|
<ClCompile Include="..\src\Location.c" />
|
||||||
|
<ClCompile Include="..\src\Main.cpp" />
|
||||||
|
<ClCompile Include="..\src\Matrix.c" />
|
||||||
|
<ClCompile Include="..\src\ParseData.c" />
|
||||||
|
<ClCompile Include="..\src\PDRBase.c" />
|
||||||
|
<ClCompile Include="..\src\Utils.c" />
|
||||||
|
<ClCompile Include="..\src\Quaternion.cpp" />
|
||||||
|
<ClCompile Include="..\src\SceneRecognition.c" />
|
||||||
|
<ClCompile Include="..\src\Pedometer.c" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\include\buffer.h" />
|
||||||
|
<ClInclude Include="..\include\Filter.h" />
|
||||||
|
<ClInclude Include="..\include\LapProcess.h" />
|
||||||
|
<ClInclude Include="..\include\LocationTool.h" />
|
||||||
|
<ClInclude Include="..\include\m_munkres.h" />
|
||||||
|
<ClInclude Include="..\include\ParseData.h" />
|
||||||
|
<ClInclude Include="..\include\AHRS.h" />
|
||||||
|
<ClInclude Include="..\include\pdr_api.h" />
|
||||||
|
<ClInclude Include="..\include\PDRBase.h" />
|
||||||
|
<ClInclude Include="..\include\pdr_detector.h" />
|
||||||
|
<ClInclude Include="..\include\Fft.h" />
|
||||||
|
<ClInclude Include="..\include\Kalman.h" />
|
||||||
|
<ClInclude Include="..\include\LinearFit.h" />
|
||||||
|
<ClInclude Include="..\include\Location.h" />
|
||||||
|
<ClInclude Include="..\include\Main.h" />
|
||||||
|
<ClInclude Include="..\include\Matrix.h" />
|
||||||
|
<ClInclude Include="..\include\pdr_sensor.h" />
|
||||||
|
<ClInclude Include="..\include\pdr_trackSmooth.h" />
|
||||||
|
<ClInclude Include="..\include\Utils.h" />
|
||||||
|
<ClInclude Include="..\include\scene_recognition.h" />
|
||||||
|
<ClInclude Include="..\include\Pedometer.h" />
|
||||||
|
<ClInclude Include="..\src\Complex.h" />
|
||||||
|
<ClInclude Include="..\src\CoordTrans.h" />
|
||||||
|
<ClInclude Include="..\src\Quaternion.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<ProjectGuid>{28D32532-309F-40EA-9E4A-2D162CC434D2}</ProjectGuid>
|
||||||
|
<Keyword>Win32Proj</Keyword>
|
||||||
|
<RootNamespace>mp4spliter</RootNamespace>
|
||||||
|
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||||
|
<ProjectName>PDR</ProjectName>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
<IncludePath>..\include\;$(IncludePath)</IncludePath>
|
||||||
|
<ReferencePath>$(ReferencePath)</ReferencePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
<IncludePath>..\3rdparty\Json\;..\3rdparty\onnx\;..\3rdparty\opencv\;..\include\;$(IncludePath)</IncludePath>
|
||||||
|
<ReferencePath>..\3rdparty\Json;$(ReferencePath)</ReferencePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
<IncludePath>..\include\;$(IncludePath)</IncludePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
<IncludePath>..\include\;.\3rdparty\libjson\;..\3rdparty\libcurl\;..\3rdparty\opencv\;$(IncludePath)</IncludePath>
|
||||||
|
<LibraryPath>..\3rdparty\onnx\lib\;..\3rdparty\opencv\lib\;..\3rdparty\libcurl\bin\;..\3rdparty\libjson\bin\;$(LibraryPath)</LibraryPath>
|
||||||
|
<ReferencePath>D:\Program Files\OpenSSL-Win64\include;$(ReferencePath)</ReferencePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<BufferSecurityCheck>true</BufferSecurityCheck>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<AdditionalDependencies>%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<BufferSecurityCheck>true</BufferSecurityCheck>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<AdditionalDependencies>%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
<LinkTimeCodeGeneration>UseLinkTimeCodeGeneration</LinkTimeCodeGeneration>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,159 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup>
|
||||||
|
<Filter Include="src">
|
||||||
|
<UniqueIdentifier>{7c7c488d-9b32-4de4-9ec8-b4bd38c3e7b0}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="inc">
|
||||||
|
<UniqueIdentifier>{a09dc878-ed5c-47e4-bdcc-c3e2c8f05882}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="src\Utils">
|
||||||
|
<UniqueIdentifier>{1d99ec5e-847f-4166-8f79-ddb0473a04dd}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="src\Math">
|
||||||
|
<UniqueIdentifier>{bd2be68d-d6d0-4b04-97d9-2ac99be07afe}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="src\Location">
|
||||||
|
<UniqueIdentifier>{74391251-b6de-45ec-88b1-0ee0171e44da}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="src\Ins">
|
||||||
|
<UniqueIdentifier>{7f1fc113-1a0a-488e-9f77-2b12c7280aed}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\src\Buffer.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Munkres.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Interface.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Detector.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Fft.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Filter.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Main.cpp">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\SceneRecognition.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Utils.c">
|
||||||
|
<Filter>src\Utils</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Matrix.c">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Kalman.c">
|
||||||
|
<Filter>src\Location</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\LinearFit.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Quaternion.cpp">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\CoordTrans.cpp">
|
||||||
|
<Filter>src\Utils</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Location.c">
|
||||||
|
<Filter>src\Location</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\ParseData.c">
|
||||||
|
<Filter>src\Utils</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\AHRS.c">
|
||||||
|
<Filter>src\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Pedometer.c">
|
||||||
|
<Filter>src\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Complex.cpp">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\PDRBase.c">
|
||||||
|
<Filter>src</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\include\buffer.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Filter.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\LapProcess.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\LocationTool.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\m_munkres.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\pdr_api.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\PDRBase.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\pdr_detector.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Fft.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\LinearFit.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Main.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\pdr_sensor.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\pdr_trackSmooth.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\scene_recognition.h">
|
||||||
|
<Filter>inc</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Utils.h">
|
||||||
|
<Filter>src\Utils</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Kalman.h">
|
||||||
|
<Filter>src\Location</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\src\Quaternion.h">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\src\CoordTrans.h">
|
||||||
|
<Filter>src\Utils</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Matrix.h">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Location.h">
|
||||||
|
<Filter>src\Location</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\ParseData.h">
|
||||||
|
<Filter>src\Utils</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\AHRS.h">
|
||||||
|
<Filter>src\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\include\Pedometer.h">
|
||||||
|
<Filter>src\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\src\Complex.h">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
</ItemGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -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 <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
@ -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 <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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 <math.h>
|
||||||
|
#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);
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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 <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#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);
|
||||||
|
}
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#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();
|
||||||
|
}
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <malloc.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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 <math.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <malloc.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <string>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#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, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
|
||||||
|
fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
|
||||||
|
fprintf(fid, "\t<Document>\n");
|
||||||
|
fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
|
||||||
|
fprintf(fid, "\t\t<name>GPS Fixs</name>\n");
|
||||||
|
|
||||||
|
// 写gps结果
|
||||||
|
for (int i = 0; i < resTracks.gpsLen; i++) {
|
||||||
|
fprintf(fid, "\t\t<Placemark>\n");
|
||||||
|
fprintf(fid, "\t\t\t<Style>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||||||
|
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||||||
|
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||||||
|
|
||||||
|
// 描述轨迹方向信息
|
||||||
|
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", resTracks.gpsTrack[i].heading);
|
||||||
|
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
|
||||||
|
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t</Style>\n");
|
||||||
|
|
||||||
|
// 写入时间信息
|
||||||
|
double h, m, s;
|
||||||
|
pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s);
|
||||||
|
fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||||||
|
time2str(m).c_str(), time2str(s).c_str());
|
||||||
|
fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||||||
|
|
||||||
|
// 写入描述信息
|
||||||
|
fprintf(fid, "\t\t\t<description>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dl>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||||||
|
resTracks.gpsTrack[i].hdop);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
|
||||||
|
resTracks.gpsTrack[i].accuracy);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
|
||||||
|
resTracks.gpsTrack[i].vel);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||||||
|
resTracks.gpsTrack[i].heading);
|
||||||
|
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||||||
|
fprintf(fid, "\t\t\t</description>\n");
|
||||||
|
// 写入坐标信息
|
||||||
|
fprintf(fid, "\t\t\t<Point>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||||||
|
resTracks.gpsTrack[i].lon,
|
||||||
|
resTracks.gpsTrack[i].lat);
|
||||||
|
fprintf(fid, "\t\t\t</Point>\n");
|
||||||
|
fprintf(fid, "\t\t</Placemark>\n");
|
||||||
|
}
|
||||||
|
fprintf(fid, "\t\t</Folder>\n");
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
|
||||||
|
fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
|
||||||
|
// 写pdr结果
|
||||||
|
for (int i = 0; i < resTracks.pdrLen; i++) {
|
||||||
|
fprintf(fid, "\t\t<Placemark>\n");
|
||||||
|
fprintf(fid, "\t\t\t<Style>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||||||
|
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||||||
|
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||||||
|
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n",
|
||||||
|
resTracks.pdrTrack[i].heading);
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
|
||||||
|
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t</Style>\n");
|
||||||
|
|
||||||
|
// 写入时间信息
|
||||||
|
double h, m, s;
|
||||||
|
pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
|
||||||
|
fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||||||
|
time2str(m).c_str(), time2str(s).c_str());
|
||||||
|
fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||||||
|
|
||||||
|
// description
|
||||||
|
fprintf(fid, "\t\t\t<description>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dl>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||||||
|
resTracks.pdrTrack[i].hdop);
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
|
||||||
|
Motion2TypeStr(resTracks.pdrTrack[i].motionType));
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
|
||||||
|
resTracks.pdrTrack[i].accuracy);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||||||
|
resTracks.pdrTrack[i].heading);
|
||||||
|
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||||||
|
fprintf(fid, "\t\t\t</description>\n");
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t<Point>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||||||
|
resTracks.pdrTrack[i].lon,
|
||||||
|
resTracks.pdrTrack[i].lat);
|
||||||
|
fprintf(fid, "\t\t\t</Point>\n");
|
||||||
|
fprintf(fid, "\t\t</Placemark>\n");
|
||||||
|
}
|
||||||
|
fprintf(fid, "\t\t</Folder>\n");
|
||||||
|
|
||||||
|
fprintf(fid, "\t</Document>\n");
|
||||||
|
fprintf(fid, "</kml>\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
|
||||||
|
|
@ -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 <N; j++){
|
||||||
|
s = 0;
|
||||||
|
for (k = 0; k < i; k++){
|
||||||
|
s += l[i][k] * u[k][j];
|
||||||
|
}
|
||||||
|
u[i][j] = a[i][j] - s;
|
||||||
|
}
|
||||||
|
for (j = i + 1; j < N; j++){
|
||||||
|
s = 0;
|
||||||
|
for (k = 0; k < i; k++){
|
||||||
|
s += l[j][k] * u[k][i];
|
||||||
|
}
|
||||||
|
l[j][i] = (a[j][i] - s) / u[i][i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++)l_inv[i][i] = 1;
|
||||||
|
|
||||||
|
for (i = 1; i < N; i++){
|
||||||
|
for (j = 0; j < i; j++){
|
||||||
|
s = 0;
|
||||||
|
for (k = 0; k < i; k++){
|
||||||
|
s += l[i][k] * l_inv[k][j];
|
||||||
|
}
|
||||||
|
l_inv[i][j] = -s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++){
|
||||||
|
u_inv[i][i] = 1 / u[i][i];
|
||||||
|
}
|
||||||
|
for (i = 1; i < N; i++)
|
||||||
|
{
|
||||||
|
for (j = i - 1; 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];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,295 @@
|
||||||
|
|
||||||
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -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 <math.h>
|
||||||
|
#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;
|
||||||
|
//}
|
||||||
|
}
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <malloc.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#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_number<gsv->satellites) //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_number<gsv->satellites)
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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];
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <math.h> /* fabs() */
|
||||||
|
#include <stdlib.h> /* malloc(), free() */
|
||||||
|
/* @Lai Zhilong added below code*/
|
||||||
|
#if !defined(LOCATION_PLATFORM_QCOM_MODEM) /* The Macro is in "custom.h" */
|
||||||
|
#include <malloc.h>
|
||||||
|
/* @Zhang Jingrui: The <malloc.h> header is depercated (and quite Linux specific, */
|
||||||
|
/* on which it defines non-standard functions like mallinfo(3)). Notice that the */
|
||||||
|
/* header <stdlib.h> is defined by C89 (and later) standards, but not <malloc.h>. */
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <string.h> /* memset() */
|
||||||
|
#include <float.h> /* 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 <sensors.h>. */
|
||||||
|
/* 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. */
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -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 <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <float.h>
|
||||||
|
#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;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,7 @@
|
||||||
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include "imu.h"
|
||||||
|
#include "ahrs.h"
|
||||||
|
#include "detector.h"
|
||||||
|
|
@ -0,0 +1,7 @@
|
||||||
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include "m_gps.h"
|
||||||
|
#include "pdr_util.h"
|
||||||
|
#include "pdr.h"
|
||||||
|
|
||||||
Loading…
Reference in New Issue