diff --git a/1.Software/PDR 1.04/include/pdr_fft.h b/1.Software/PDR 1.04/include/Fft.h similarity index 69% rename from 1.Software/PDR 1.04/include/pdr_fft.h rename to 1.Software/PDR 1.04/include/Fft.h index 2885703..21becc1 100644 --- a/1.Software/PDR 1.04/include/pdr_fft.h +++ b/1.Software/PDR 1.04/include/Fft.h @@ -4,22 +4,15 @@ /* Header File Including ------------------------------------------------------------------------ */ #include #include +#include "Complex.h" /* Macro Declaration ---------------------------------------------------------------------------- */ #define M_PI 3.14159265358979323846 /* pi */ -// #define MATLAB_DEBUG -// #define MATLAB_MAIN - /* Structure Declaration ------------------------------------------------------------------------ */ -typedef struct myComplex { - float real; - float image; -} myComplex; /* Function Declaration ------------------------------------------------------------------------- */ -extern int FFT_dft(myComplex *fft, float *signal, int length); - -extern int FFT_fft(myComplex *fft, float *signal, int length); +extern int FFT_Dft(Complex_t *fft, float *signal, int length); +extern int FFT_Fft(Complex_t *fft, float *signal, int length); #endif diff --git a/1.Software/PDR 1.04/include/Filter.h b/1.Software/PDR 1.04/include/Filter.h index 8998038..3c84732 100644 --- a/1.Software/PDR 1.04/include/Filter.h +++ b/1.Software/PDR 1.04/include/Filter.h @@ -1,43 +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_COEF_MAX_LENGTH 71 -#define FILTER_FIR_COEF_MAX_LENGTH 71 +/* Macro Declaration ----------------------------------------------------------*/ +#define FILTER_MAX_LENGTH 71 -/* Structure Declaration ------------------------------------------------------------------------ */ -typedef struct FILTER { // iir or fir filter - int reset; // reset flag - int order; // coefficient array length, not filter order - double H0; // dc signal gain - double b[FILTER_COEF_MAX_LENGTH]; // b coefficient - double a[FILTER_COEF_MAX_LENGTH]; // a coefficient - double yout[FILTER_COEF_MAX_LENGTH]; // yout buffer - double xin[FILTER_COEF_MAX_LENGTH]; // xin buffer -} FILTER; +/* 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; -typedef struct FILTER_FIR { // fir filter - int reset; // reset flag - int order; // coefficient array length, not filter order - double H0; // dc signal gain - double b[FILTER_FIR_COEF_MAX_LENGTH]; // b coefficient - double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer -} FILTER_FIR; +/* Function Declaration -------------------------------------------------------*/ -typedef struct DELAYER { // delayer - int reset; - int delay; // delay point number - double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer -} DELAYER; +/**--------------------------------------------------------------------- +* Function : FilterReset +* Description : 重置滤波器 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterReset(Filter_t *f); -/* Function Declaration ------------------------------------------------------------------------- */ -extern void FILTER_Reset(FILTER *f); -extern void FILTER_FIR_Reset(FILTER_FIR *f); -extern void FILTER_SetCoef(FILTER *f, int order, double *b, double *a, double H0); -extern void FILTER_FIR_setCoef(FILTER_FIR *f, int order, double *b, double H0); -extern double FILTER_filter(FILTER *f, double x); -extern double FILTER_FIR_filter(FILTER_FIR *f, double x); -extern void DELAY_reset(DELAYER *d); -extern void DELAY_setCoef(DELAYER *d, int delay); -extern double DELAY_delay(DELAYER *d, double x); +/**--------------------------------------------------------------------- +* Function : FilterSetCoef +* Description : 滤波器设置参数 +* Input : order coefficient array length, not filter order +* *b b coefficient +* *a a coefficient +* H0 dc signal gain +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterSetCoef(Filter_t *f, int order, double *b, double *a, double H0); + +/**--------------------------------------------------------------------- +* Function : FilterFilter +* Description : 滤波器滤波计算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +double FilterFilter(Filter_t *f, double x); #endif diff --git a/1.Software/PDR 1.04/include/Kalman.h b/1.Software/PDR 1.04/include/Kalman.h index 4fe93f8..2329ba4 100644 --- a/1.Software/PDR 1.04/include/Kalman.h +++ b/1.Software/PDR 1.04/include/Kalman.h @@ -37,14 +37,14 @@ void EKFStatePredict(EKFPara_t* kf, double step_length, PDR_t* g_pdr, int step); * kf，EKF数据结构体 * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void EKFCalQRMatrix(lct_nmea* nmea_data, PDR_t* g_pdr, classifer* sys, EKFPara_t* kf); +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* sys, PDR_t* g_pdr, +void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, Classifer_t* sys, PDR_t* g_pdr, int scene_type); /**---------------------------------------------------------------------- @@ -52,8 +52,8 @@ void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, classifer* sys, PDR_t* g_pdr, * Description : PDR的GNSS和INS融合定位 * Date : 2020/07/09 logzhan *---------------------------------------------------------------------**/ -void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, classifer* sys, double yaw_bias, - EKFPara_t* kf, lct_fs* res); +void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias, + EKFPara_t* kf, LctFs_t* res); #ifdef __cplusplus } diff --git a/1.Software/PDR 1.04/include/pdr_linearFit.h b/1.Software/PDR 1.04/include/LinearFit.h similarity index 100% rename from 1.Software/PDR 1.04/include/pdr_linearFit.h rename to 1.Software/PDR 1.04/include/LinearFit.h diff --git a/1.Software/PDR 1.04/include/Location.h b/1.Software/PDR 1.04/include/Location.h index 3ccd228..79e9548 100644 --- a/1.Software/PDR 1.04/include/Location.h +++ b/1.Software/PDR 1.04/include/Location.h @@ -32,14 +32,14 @@ int InsLocation(IMU_t *ss_data, EKFPara_t *kf); * Description : 在没有gps信息时预测GPS位置，最多预测10个点 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* g_pdr); +void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* g_pdr); /**---------------------------------------------------------------------- * Function : Nmea2Gnss * Description : nmea数据结构转gnss数据 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss); +void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss); /**---------------------------------------------------------------------- @@ -50,7 +50,7 @@ void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss); * 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高 * 代码的可读性 *---------------------------------------------------------------------**/ -int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, lct_fs* result); +int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result); /**---------------------------------------------------------------------- * Function : GnssCalHeading @@ -65,7 +65,7 @@ double GnssCalHeading(GNSS_t* pgnss); * Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR_t* p_pdr); +void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr); /**--------------------------------------------------------------------- * Function : pdr_resetSysStatus @@ -79,7 +79,7 @@ void ResetSystemStatus(EKFPara_t* kf); * Description : PDR GPS融合INS惯性定位 * Date : 2021/01/29 logzhan *---------------------------------------------------------------------**/ -int GnssInsFusionLocation(lct_nmea* nmea_data, EKFPara_t* kf, lct_fs* result); +int GnssInsFusionLocation(Nmea_t* nmea_data, EKFPara_t* kf, LctFs_t* result); /**--------------------------------------------------------------------- * Function : pdr_initGnssInfo @@ -88,7 +88,7 @@ int GnssInsFusionLocation(lct_nmea* nmea_data, EKFPara_t* kf, lct_fs* result); *---------------------------------------------------------------------**/ void InitGnssInfo(void); -void GnssUpdate(GNSS_t* gps, lct_nmea* nmea); +void GnssUpdate(GNSS_t* gps, Nmea_t* nmea); #ifdef __cplusplus diff --git a/1.Software/PDR 1.04/include/pdr_main.h b/1.Software/PDR 1.04/include/Main.h similarity index 97% rename from 1.Software/PDR 1.04/include/pdr_main.h rename to 1.Software/PDR 1.04/include/Main.h index d3f2f98..e5728cf 100644 --- a/1.Software/PDR 1.04/include/pdr_main.h +++ b/1.Software/PDR 1.04/include/Main.h @@ -76,7 +76,7 @@ const char* Motion2TypeStr(int type); * Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹 * Date : 2021/01/25 logzhan *---------------------------------------------------------------------**/ -void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs); +void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs); #endif diff --git a/1.Software/PDR 1.04/include/ParseData.h b/1.Software/PDR 1.04/include/ParseData.h index ab8e61b..6ee6676 100644 --- a/1.Software/PDR 1.04/include/ParseData.h +++ b/1.Software/PDR 1.04/include/ParseData.h @@ -34,7 +34,7 @@ extern "C" { ts， 手机开机时间戳，ms * Output : ln， NMEA数据结构体 **************************************************************************/ - void parseNMEA(char* pt, lct_nmea *ln, double ts); + void parseNMEA(char* pt, Nmea_t *ln, double ts); /************************************************************************** * Description : 解析GGA协议 @@ -42,7 +42,7 @@ extern "C" { ts， 手机开机时间戳，ms * Output : ln， NMEA数据结构体 **************************************************************************/ - void ParseGGA(char* pt, lct_nmea *ln, double ts); + void ParseGGA(char* pt, Nmea_t *ln, double ts); /************************************************************************** * Description : 解析RMC协议 @@ -50,7 +50,7 @@ extern "C" { ts， 手机开机时间戳，ms * Output : ln， NMEA数据结构体 **************************************************************************/ - void ParseRMC(char* pt, lct_nmea *ln, double ts); + void ParseRMC(char* pt, Nmea_t *ln, double ts); /************************************************************************** * Description : 解析GSV协议 @@ -58,7 +58,7 @@ extern "C" { ts， 手机开机时间戳，ms * Output : ln， NMEA数据结构体 **************************************************************************/ - void parseGSV(char* pt, lct_nmea *ln, double ts); + void parseGSV(char* pt, Nmea_t *ln, double ts); /************************************************************************** * Description : 解析GSA协议 @@ -66,22 +66,22 @@ extern "C" { ts， 手机开机时间戳，ms * Output : ln， NMEA数据结构体 **************************************************************************/ - void ParseGSA(char* pt, lct_nmea *ln, double ts); + 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, lct_nmea *ln, double ts); +void parseLocAccuracy(char* pt, Nmea_t *ln, double ts); /************************************************************************** * Description : 检测NMEA数据是否解析完整 * Input : ln， NMEA数据结构体 **************************************************************************/ -void preprocessNMEA(lct_nmea *ln); +void preprocessNMEA(Nmea_t *ln); -int ParseLineStr(char* line, IMU_t* imu_p, lct_nmea* ln); +int ParseLineStr(char* line, IMU_t* imu_p, Nmea_t* ln); /**---------------------------------------------------------------------- * Function : HexToDec diff --git a/1.Software/PDR 1.04/include/buffer.h b/1.Software/PDR 1.04/include/buffer.h index 416119c..5f9555f 100644 --- a/1.Software/PDR 1.04/include/buffer.h +++ b/1.Software/PDR 1.04/include/buffer.h @@ -18,6 +18,7 @@ extern "C" { /* Structure Declaration ------------------------------------------------------------------------ */ #pragma pack (4) + typedef struct BUFFER { char name[20]; int type; @@ -28,7 +29,7 @@ typedef struct BUFFER { double sum; double mean; float data[BUFFER_LONG_LENGTH + 1]; -} BUFFER; +}Buffer_t; typedef struct BUFFER_LONG { char name[20]; @@ -58,17 +59,17 @@ typedef struct BUFFER_SHORT { /* Global Variable Declaration ------------------------------------------------------------------ */ /* Function Declaration ------------------------------------------------------------------------- */ -int Buffer_initialize(BUFFER *buffer, const char *name, int type, int length); -int Buffer_clear(BUFFER *buffer); -int Buffer_count(BUFFER *buffer, int *count); -int Buffer_top(BUFFER *buffer, float *value); -int Buffer_bottom(BUFFER *buffer, float *value); -int Buffer_pop(BUFFER *buffer, float *value); -int buffer_push(BUFFER *buffer, float value); -int Buffer_get(BUFFER *buffer, float *value, int index); -int Buffer_mean(BUFFER *buffer, float *mean); -int Buffer_var(BUFFER *buffer, float *var); -int Buffer_std(BUFFER *buffer, float *std); +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 } diff --git a/1.Software/PDR 1.04/include/pdr_api.h b/1.Software/PDR 1.04/include/pdr_api.h index bac61d4..4052deb 100644 --- a/1.Software/PDR 1.04/include/pdr_api.h +++ b/1.Software/PDR 1.04/include/pdr_api.h @@ -36,15 +36,15 @@ typedef struct _PosFusion{ /* Function Declaration ------------------------------------------------------*/ /**--------------------------------------------------------------------- -* Function : pdr_algorithmInit +* Function : Algorithm_Init * Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨 * 迹平滑窗口的初始化 -* Date : 2020/7/3 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ void Algorithm_Init(void); /**--------------------------------------------------------------------- -* Function : pdr_locationMainLoop +* Function : LocationMainLoop * Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置 * 迹平滑窗口的初始化 * Input : ss_data， 传感器数据 @@ -53,7 +53,7 @@ void Algorithm_Init(void); * Output : int, 输出标志位 * Date : 2020/7/3 logzhan *---------------------------------------------------------------------**/ -int LocationMainLoop(IMU_t* ImuData, lct_nmea* NmeaData, lct_fs* LocFusion, +int LocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion, FILE *fp_gps); /**--------------------------------------------------------------------- @@ -63,7 +63,7 @@ int LocationMainLoop(IMU_t* ImuData, lct_nmea* NmeaData, lct_fs* LocFusion, * Date : 2020/07/03 logzhan * 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 *---------------------------------------------------------------------**/ -int ParseLineAndUpdate(char* line, lct_fs* result); +int ParseLineAndUpdate(char* line, LctFs_t* result); /**--------------------------------------------------------------------- * Function : pdr_initRmc @@ -71,7 +71,7 @@ int ParseLineAndUpdate(char* line, lct_fs* result); * Input : rmc， RMC结构体 * Date : 2020/7/3 logzhan *---------------------------------------------------------------------**/ -void RMC_Init(lct_nmea_rmc * rmc); +void RMC_Init(NmeaRMC_t * rmc); /**--------------------------------------------------------------------- * Function : pdr_initNmeaFlg @@ -79,28 +79,28 @@ void RMC_Init(lct_nmea_rmc * rmc); * Input : ln， NMEA结构体 * Date : 2020/7/3 logzhan *---------------------------------------------------------------------**/ -void NmeaFlag_Init(lct_nmea * ln); +void NmeaFlag_Init(Nmea_t * ln); /**--------------------------------------------------------------------- * Function : clearNmeaFlg * Description : 清空Nmea标志位,功能和init相同 * Date : 2020/7/4 logzhan *---------------------------------------------------------------------**/ -void ClearNmeaFlg(lct_nmea * ln); +void ClearNmeaFlg(Nmea_t * ln); /**--------------------------------------------------------------------- * Function : pdr_saveGnssInfo * Description : 保存GPS相关信息 * Date : 2020/7/3 logzhan *---------------------------------------------------------------------**/ -void SaveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps); +void SaveGnssInfo(Nmea_t* nmea_data, LctFs_t* result, FILE* fp_gps); /**---------------------------------------------------------------------- -* Function : GetLIBVersion -* Description : pdr库版本号 -* Date : 2020/8/3 logzhan +* Function : GetPDRVersion +* Description : 获取pdr版本号 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -const char* GetLIBVersion(void); +const char* GetPDRVersion(void); /**---------------------------------------------------------------------- * Function : pdr_closeAlgorithm diff --git a/1.Software/PDR 1.04/include/pdr_base.h b/1.Software/PDR 1.04/include/pdr_base.h index 0125d36..3c34da8 100644 --- a/1.Software/PDR 1.04/include/pdr_base.h +++ b/1.Software/PDR 1.04/include/pdr_base.h @@ -167,18 +167,17 @@ typedef struct PDR { typedef struct { int type; float accBuffer[PATTERN_RECOGNITION_LEN]; -}classifer; +}Classifer_t; // 用户运动类型分类器 typedef struct DETECTOR { uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动 uint32_t lastType; uint64_t tick; // 次数统计，用于调整检测器工作频率 -} DETECTOR_t; +}Detector_t; -DETECTOR_t *pdr_getDetector(void); - +Detector_t *GetDetectorObj(void); /* Function Declaration ------------------------------------------------------*/ @@ -200,16 +199,15 @@ void pdr_resetData(void); void ComputePCA(AHRS_t* ahrs); /**--------------------------------------------------------------------- -* Function : detectorUpdate -* Description : 根据新确定的手机携带方式更新g_pdr.status等 -* Date : 2020/7/20 logzhan +* Function : DetectorUpdate +* Description : 根据新确定的手机携带方式更新状态等 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void detectorUpdate(DETECTOR_t* detector); +void DetectorUpdate(Detector_t* detector); void gpsUpdateCb(GNSS_t* gps); - /************************************************************************** * Description : 计算每步所耗时间 * Input : stepPredict， 步数预测结构体 @@ -234,7 +232,7 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_ * Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 * Date : 2020/7/22 logzhan *---------------------------------------------------------------------**/ -void StateRecognition(float *acc, classifer *sys); +void StateRecognition(float *acc, Classifer_t *sys); /************************************************************************** * Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 @@ -245,7 +243,7 @@ void StateRecognition(float *acc, classifer *sys); ss_data， 传感器数据结构体 * Output : double，用户行走方向 **************************************************************************/ -double calPredAngle(PDR_t *g_pdr, classifer *sys, double gps_yaw, double G_yaw, IMU_t *ss_data); +double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw, IMU_t *ss_data); /************************************************************************** * Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 @@ -264,19 +262,18 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, GNSS_t *pgnss, EKFPara_t *kf); /**---------------------------------------------------------------------- -* Function : pdr_detStatic +* Function : DetUserStatic * Description : 检测用户是否处于静止状态 -* Date : 2021/01/28 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -int pdr_detStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps); - +int DetUserStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps); /**---------------------------------------------------------------------- -* Function : pdr_outputGpsPos +* Function : OutputRawGnssPos * Description : 输出GPS位置 -* Date : 2020/07/08 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result); +void OutputRawGnssPos(GNSS_t *pgnss, LctFs_t *result); /**---------------------------------------------------------------------- * Function : resetOutputResult @@ -288,7 +285,7 @@ void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result); * kf， EKF数据结构体 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result); +int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, LctFs_t *result); /**---------------------------------------------------------------------- * Function : detIsCarMode @@ -308,7 +305,7 @@ int DetectCarMode(GNSS_t *pgnss, PDR_t *g_pdr, unsigned long delSteps, int *time * 重置PDR到GPS的位置。 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ -int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t *g_pdr); +int DetectPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t *g_pdr); /************************************************************************** * Description : 初始化EKF滤波器 @@ -320,7 +317,7 @@ int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t * Output : int，初始化标志位 result，定位结果结构体 **************************************************************************/ -int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, lct_fs *result); +int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result); /************************************************************************** * Description : 设置EKF滤波器的Q和R diff --git a/1.Software/PDR 1.04/include/pdr_detector.h b/1.Software/PDR 1.04/include/pdr_detector.h index 59a6308..478c356 100644 --- a/1.Software/PDR 1.04/include/pdr_detector.h +++ b/1.Software/PDR 1.04/include/pdr_detector.h @@ -23,7 +23,7 @@ extern "C" { /* Struct Declaration --------------------------------------------------------------------------- */ -typedef void (*DETECTOR_update_callback)(DETECTOR_t *detector); +typedef void (*DETECTOR_update_callback)(Detector_t *detector); /* Global Variable Declaration ------------------------------------------------------------------ */ extern BUFFER_SHORT g_acc_frq_buf[3]; @@ -34,19 +34,18 @@ extern BUFFER_SHORT g_gyr_amp_buf[3]; /* Function Declaration ------------------------------------------------------------------------- */ /**--------------------------------------------------------------------- -* Function : pdr_getDetector -* Description : 获取PDR运动分类器的结构体指针 -* Date : 2020/02/16 logzhan & -* +* Function : GetDetectorObj +* Description : 获取PDR运动分类器对象 +* Date : 2020/02/16 logzhan *---------------------------------------------------------------------**/ -DETECTOR_t *pdr_getDetector(void); +Detector_t *GetDetectorObj(void); /**--------------------------------------------------------------------- * Function : Detector_Init * Description : 初始化运动分类器，识别用户处于哪一种运动模式 * Date : 2022/09/23 logzhan *---------------------------------------------------------------------**/ -DETECTOR_t* Detector_Init(void); +Detector_t* Detector_Init(void); /**--------------------------------------------------------------------- * Function : DetectorReset diff --git a/1.Software/PDR 1.04/include/pdr_sensor.h b/1.Software/PDR 1.04/include/pdr_sensor.h index bcf107e..26b14d5 100644 --- a/1.Software/PDR 1.04/include/pdr_sensor.h +++ b/1.Software/PDR 1.04/include/pdr_sensor.h @@ -154,7 +154,7 @@ typedef struct imu { Sensor_t mag; }IMU_t; -typedef struct lct_nmea_rmc { +typedef struct NmeaRMC_t { unsigned char update; char rmc_check_sum[9]; sensor type; @@ -171,9 +171,9 @@ typedef struct lct_nmea_rmc { double latitude; double longitudinal; double time; //ms -}lct_nmea_rmc; +}NmeaRMC_t; -typedef struct lct_nmea_gga { +typedef struct NmeaGGA{ unsigned char update; char gga_check_sum[9]; sensor type; @@ -191,9 +191,9 @@ typedef struct lct_nmea_gga { double latitude; double longitudinal; double time; //ms -}lct_nmea_gga; +}NmeaGGA_t; -typedef struct lct_nmea_gsa { +typedef struct NmeaGSA { unsigned char update; char check_sum[9]; sensor type; @@ -205,8 +205,7 @@ typedef struct lct_nmea_gsa { float hdop; float vdop; double time; //ms -} -lct_nmea_gsa; +}NmeaGSA_t; typedef struct satellite_status { int prn; @@ -215,7 +214,7 @@ typedef struct satellite_status { int snr; //snr >25 good }satellite_status; -typedef struct lct_nmea_gsv { +typedef struct NmeaGSV { unsigned char update; sensor type; int sentences; @@ -224,25 +223,25 @@ typedef struct lct_nmea_gsv { int satellite_number; satellite_status satellites_data[20]; double time; //ms -}lct_nmea_gsv; +}NmeaGSV_t; -typedef struct loc_accuracy{ +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; -}loc_accuracy; +}Accuracy_t; -typedef struct lct_nmea { +typedef struct Nmea_t { unsigned char update; - double minTime; - double maxTime; - lct_nmea_gga gga; - lct_nmea_rmc rmc[2]; // 0 - GNRMC, 1 - GPRMC - lct_nmea_gsa gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA - lct_nmea_gsv gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV - loc_accuracy accuracy; -}lct_nmea; + 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; @@ -263,14 +262,14 @@ typedef struct lct_fs { double last_lon; unsigned long step; uint8_t motionType; -}lct_fs; +}LctFs_t; typedef struct gnss { LL_NSEW lat_ns; LL_NSEW lon_ew; - int minElevation; - int satellites_num; // 当前卫星数量 - int quality; + int minElevation; + int satellites_num; // 当前卫星数量 + int quality; float yaw; // GNSS 航向角 float vel; // GNSS 速度 float HDOP; // GNSS 水平精度因子 @@ -295,17 +294,17 @@ typedef struct AHRS { uint8_t status; uint8_t stable; // 当前AHRS算法收敛稳定性 float error; - float qnb[4]; + float qnb[4]; // float gravity[3]; float x_axis[3]; float y_axis[3]; float z_axis[3]; - float kp; // mahony kp比例调节参数 - float yaw; - float pitch; - float roll; + float Kp; // mahony kp比例调节参数 + float Yaw; + float Pitch; + float Roll; float insHeading; -} AHRS_t; +}AHRS_t; #ifdef __cplusplus } diff --git a/1.Software/PDR 1.04/include/scene_recognition.h b/1.Software/PDR 1.04/include/scene_recognition.h index 73043c9..d0efd89 100644 --- a/1.Software/PDR 1.04/include/scene_recognition.h +++ b/1.Software/PDR 1.04/include/scene_recognition.h @@ -85,8 +85,8 @@ typedef enum _SIGNAL_QUALITY BAD } SIGNAL_QUALITY; -/* Usage: extract and save information from lct_nmea for the module. */ -/* This is the interface for the lct_nmea. */ +/* 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：不更新 @@ -132,14 +132,8 @@ SCENE_INIT_STATE scene_recognition_reset(void); * Input : PDR的nmea结构体 * Return : 场景识别结果 * Date : 2020/02/18 -* -* -* -* -* -* *---------------------------------------------------------------------**/ -SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea* nmea); +SCENE_RECOGNITION_RESULT sceneRecognitionProc(const Nmea_t* nmea); /* Function Name: scene_recognition_destroy() */ /* Usage: 场景识别处理结束后的资源释放操作，资源释放后无法继续进行场 */ @@ -156,7 +150,7 @@ SCENE_DESTROY_STATE scene_recognition_destroy(void); * Author : Zhang Jingrui, Geek ID: 11082826 * Date : 2020/02/18 logzhan *---------------------------------------------------------------------**/ -int isOpenArea(const lct_nmea* nmea); +int isOpenArea(const Nmea_t* nmea); #ifdef __cplusplus } diff --git a/1.Software/PDR 1.04/project/PDR.vcxproj b/1.Software/PDR 1.04/project/PDR.vcxproj index a1d98ef..6f144b9 100644 --- a/1.Software/PDR 1.04/project/PDR.vcxproj +++ b/1.Software/PDR 1.04/project/PDR.vcxproj @@ -19,24 +19,25 @@ - + + - + - - + + - + - + @@ -50,17 +51,18 @@ - + - + - + + diff --git a/1.Software/PDR 1.04/project/PDR.vcxproj.filters b/1.Software/PDR 1.04/project/PDR.vcxproj.filters index 31724d1..4a00583 100644 --- a/1.Software/PDR 1.04/project/PDR.vcxproj.filters +++ b/1.Software/PDR 1.04/project/PDR.vcxproj.filters @@ -16,15 +16,15 @@ {74391251-b6de-45ec-88b1-0ee0171e44da} - + {7f1fc113-1a0a-488e-9f77-2b12c7280aed} - + src - + src @@ -36,16 +36,16 @@ src - + src - + src src - + src @@ -57,7 +57,7 @@ src\Location - + src @@ -73,10 +73,13 @@ src\Utils - src\INS + src\Ins - src\INS + src\Ins + + + src\Math @@ -104,13 +107,13 @@ inc - + inc - + inc - + inc @@ -144,10 +147,13 @@ src\Utils - src\INS + src\Ins - src\INS + src\Ins + + + src\Math \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/AHRS.c b/1.Software/PDR 1.04/src/AHRS.c index 25e57b3..b1c83fc 100644 --- a/1.Software/PDR 1.04/src/AHRS.c +++ b/1.Software/PDR 1.04/src/AHRS.c @@ -106,8 +106,8 @@ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, f // 这两个函数占了大量的时间 if (g_ahrs.stable == PDR_FALSE) { - buffer_push((BUFFER*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez))); - Buffer_mean((BUFFER*)&g_erro_buf, &g_ahrs.error); + BufferPush((Buffer_t*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez))); + BufferMean((Buffer_t*)&g_erro_buf, &g_ahrs.error); } // 龙格库塔法更新四元数 @@ -121,9 +121,9 @@ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, f QuaternionNorm(&q0, &q1, &q2, &q3); // 四元数转欧拉角 - g_ahrs.pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (180.0f / 3.141592f); - g_ahrs.yaw = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (180.0f / 3.141592f); - g_ahrs.roll = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (180.0f / 3.141592f); + g_ahrs.Pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (180.0f / 3.141592f); + g_ahrs.Yaw = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (180.0f / 3.141592f); + g_ahrs.Roll = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (180.0f / 3.141592f); } /**--------------------------------------------------------------------- @@ -151,22 +151,22 @@ void AHRS_sensorFrame2EarthFrame(float e[], float s[]) { void AHRS_Init(void) { int i; g_ahrs.stable = PDR_FALSE; - Buffer_initialize((BUFFER *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); + BufferInit((Buffer_t *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); for (i = 0; i < 3; ++i) { - Buffer_initialize((BUFFER *)&g_grav_buf[i], GRV_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); - Buffer_initialize((BUFFER *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); + BufferInit((Buffer_t *)&g_grav_buf[i], GRV_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); + BufferInit((Buffer_t *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); + BufferInit((Buffer_t *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); } ResetARHS(); } void ResetARHS(void) { - Buffer_clear((BUFFER *)&g_erro_buf); + BufferClear((Buffer_t *)&g_erro_buf); for (uchar i = 0; i < 3; ++i) { - Buffer_clear((BUFFER *)&g_grav_buf[i]); - Buffer_clear((BUFFER *)&g_gyro_buf[i]); - Buffer_clear((BUFFER *)&g_magn_buf[i]); + BufferClear((Buffer_t *)&g_grav_buf[i]); + BufferClear((Buffer_t *)&g_gyro_buf[i]); + BufferClear((Buffer_t *)&g_magn_buf[i]); } g_ticker = 0; q0 = 1.0f; @@ -265,9 +265,9 @@ int UpdateAHRS(IMU_t* imu) { // ARHS BUF 中存储了256个sensor的缓存 if (g_ticker <= AHRS_BUF_LEN) { for (i = 0; i < 3; ++i) { - buffer_push((BUFFER *)&g_grav_buf[i], acce[i]); - buffer_push((BUFFER *)&g_gyro_buf[i], gyro[i]); - buffer_push((BUFFER *)&g_magn_buf[i], magn[i]); + BufferPush((Buffer_t *)&g_grav_buf[i], acce[i]); + BufferPush((Buffer_t *)&g_gyro_buf[i], gyro[i]); + BufferPush((Buffer_t *)&g_magn_buf[i], magn[i]); } } // 只有缓存满的时候才计算 @@ -275,7 +275,7 @@ int UpdateAHRS(IMU_t* imu) { //int index; index = (g_magn_buf[0]._top + AHRS_BUF_LEN + 1 - AHRS_BUF_LEN / 2) % (g_magn_buf[0].length + 1); for (i = 0; i < 3; ++i) { - Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]); + BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]); magn[i] = g_magn_buf[i].data[index]; g_grav_buf[i]._top = g_grav_buf[i]._bottom; } @@ -291,8 +291,8 @@ int UpdateAHRS(IMU_t* imu) { Kp = (float)(2 * 2); for (index = INC_PTR(g_gyro_buf[0], index); index != g_gyro_buf[0]._top; index = INC_PTR(g_gyro_buf[0], index)) { for (i = 0; i < 3; ++i) { - buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]); - Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]); + BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]); + BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]); gyro[i] = g_gyro_buf[i].data[index]; acce[i] = g_grav_buf[i].data[index]; magn[i] = g_magn_buf[i].data[index]; @@ -322,7 +322,7 @@ int UpdateAHRS(IMU_t* imu) { // 应该是判断是否进入PCA的条件 if (g_ahrs.error < 0.3) { - Buffer_count((BUFFER *)&g_erro_buf, &count); + BufferCount((Buffer_t *)&g_erro_buf, &count); memset((void *)mean, 0, sizeof(mean)); for (index = g_erro_buf._bottom, i = 0; index != g_erro_buf._top; index = INC_PTR(g_erro_buf, index), ++i) { int c = count >> 1; @@ -349,8 +349,8 @@ int UpdateAHRS(IMU_t* imu) { } for (i = 0; i < 3; ++i) { - buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]); - Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]); + BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]); + BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]); } estimate_sensor_vector(grav, grav, gyro); diff --git a/1.Software/PDR 1.04/src/Complex.cpp b/1.Software/PDR 1.04/src/Complex.cpp new file mode 100644 index 0000000..e1c0f92 --- /dev/null +++ b/1.Software/PDR 1.04/src/Complex.cpp @@ -0,0 +1,74 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Complex.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR复数运算库支持 +********************************************************************************/ +/* Header File Including -----------------------------------------------------*/ +#include "math.h" +#include "Complex.h" + +/**--------------------------------------------------------------------- +* Function : ComplexAbs +* Description : 复数球绝对值运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +float ComplexAbs(Complex_t c) { + return (float)sqrt(c.real * c.real + c.image * c.image); +} + +/**--------------------------------------------------------------------- +* Function : ComplexAdd +* Description : 复数加法运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexAdd(Complex_t a, Complex_t b) { + Complex_t c = { 0 }; + c.real = a.real + b.real; + c.image = a.image + b.image; + return c; +} + +/**--------------------------------------------------------------------- +* Function : ComplexSub +* Description : 复数减法运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexSub(Complex_t a, Complex_t b) { + Complex_t c = { 0 }; + + c.real = a.real - b.real; + c.image = a.image - b.image; + + return c; +} + +/**--------------------------------------------------------------------- +* Function : ComplexMul +* Description : 复数乘法运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexMul(Complex_t a, Complex_t b) { + Complex_t c; + c.real = a.real * b.real - a.image * b.image; + c.image = a.real * b.image + a.image * b.real; + return c; +} + +/**--------------------------------------------------------------------- +* Function : ComplexPow +* Description : 复数指数运算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +Complex_t ComplexPow(Complex_t c, int n) { + Complex_t p = { 1, 0 }; + int i; + float real; + for (i = 0; i < n; ++i) { + real = p.real * c.real - p.image * c.image; + p.image = p.real * c.image + p.image * c.real; + p.real = real; + } + return p; +} \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/Complex.h b/1.Software/PDR 1.04/src/Complex.h new file mode 100644 index 0000000..1479df4 --- /dev/null +++ b/1.Software/PDR 1.04/src/Complex.h @@ -0,0 +1,24 @@ +#ifndef _PDR_COMPLEX_H_ +#define _PDR_COMPLEX_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct Complex { + float real; + float image; +} Complex_t; + + +float ComplexAbs(Complex_t c); +Complex_t ComplexAdd(Complex_t a, Complex_t b); +Complex_t ComplexSub(Complex_t a, Complex_t b); +Complex_t ComplexMul(Complex_t a, Complex_t b); +Complex_t ComplexPow(Complex_t c, int n); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/Detector.c b/1.Software/PDR 1.04/src/Detector.c index b4f74db..65bffe6 100644 --- a/1.Software/PDR 1.04/src/Detector.c +++ b/1.Software/PDR 1.04/src/Detector.c @@ -15,7 +15,7 @@ #include "pdr_detector.h" #include "Filter.h" #include "buffer.h" -#include "pdr_fft.h" +#include "Fft.h" #include "m_munkres.h" @@ -42,9 +42,9 @@ static float ellipsoidPoint[3]; static float ellipsoidScale[3]; /* Global Variable Definition ------------------------------------------------*/ -static DETECTOR_t g_detector; +static Detector_t g_detector; static BUFFER_LONG g_mol_buf[2]; -static FILTER g_mol_flt[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, @@ -68,13 +68,11 @@ static int g_label[5] = { 0 }; /* Function Definition -------------------------------------------------------------------------- */ /**--------------------------------------------------------------------- -* Function : pdr_getDetector -* Description : 获取PDR运动分类器的结构体指针 -* Date : 2020/02/16 logzhan & -* -* +* Function : GetDetectorObj +* Description : 获取PDR运动分类器对象 +* Date : 2020/02/16 logzhan *---------------------------------------------------------------------**/ -DETECTOR_t* pdr_getDetector(void) { +Detector_t* GetDetectorObj(void) { return &g_detector; } @@ -83,7 +81,7 @@ DETECTOR_t* pdr_getDetector(void) { * Description : 初始化运动分类器，识别用户处于哪一种运动模式 * Date : 2020/02/16 logzhan & logzhan *---------------------------------------------------------------------**/ -DETECTOR_t* Detector_Init(void) { +Detector_t* Detector_Init(void) { DetectorReset(); return &g_detector; } @@ -98,28 +96,28 @@ void DetectorReset(void) { g_detector.tick = 0; g_detector.lastType = DETECTOR_TYPE_STATIC; - Buffer_initialize((BUFFER *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN); - Buffer_initialize((BUFFER *)&g_mol_buf[GYRO], "gyro_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_frq_buf[0], "acc_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_frq_buf[1], "acc_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_frq_buf[2], "acc_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_amp_buf[0], "acc_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_amp_buf[1], "acc_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_amp_buf[2], "acc_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_tmp_buf[0], "acc_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_tmp_buf[1], "acc_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); - Buffer_initialize((BUFFER *)&g_acc_tmp_buf[2], "acc_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_frq_buf[0], "gyr_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_frq_buf[1], "gyr_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_frq_buf[2], "gyr_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_amp_buf[0], "gyr_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_amp_buf[1], "gyr_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_amp_buf[2], "gyr_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_tmp_buf[0], "gyr_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_tmp_buf[1], "gyr_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); - Buffer_initialize((BUFFER *)&g_gyr_tmp_buf[2], "gyr_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); - FILTER_SetCoef(&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); - FILTER_SetCoef(&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); + 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(*mag_buff,0,sizeof(mag_buff)); mag_count = 0; mag_calibrate = CALIBRATE_OFF; @@ -142,8 +140,8 @@ void DetectorReset(void) { *---------------------------------------------------------------------**/ void DetectorUpdateIMU(IMU_t* imu) { - buffer_push((BUFFER *)&g_mol_buf[ACCE], (float)FILTER_filter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s))); - buffer_push((BUFFER *)&g_mol_buf[GYRO], (float)FILTER_filter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s))); + 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 ((g_detector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) { @@ -151,222 +149,26 @@ void DetectorUpdateIMU(IMU_t* imu) { int detectResult = DetectMotionType(); if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) { - detectorUpdate(&g_detector); + DetectorUpdate(&g_detector); g_detector.lastType = g_detector.type; } } ++g_detector.tick; } -/**--------------------------------------------------------------------- -* Function : mag_calibration -* Description : 收集手机晃动状态下，进行磁力计校准 -* Date : 2020/09/02 logzhan -*---------------------------------------------------------------------**/ -void mag_calibration(IMU *imu) -{ - if (DETECTOR_TYPE_SWINGING == g_detector.type && CALIBRATE_OFF == mag_calibrate) - { - mag_buff[mag_count][0] = imu->mag[0]; - mag_buff[mag_count][1] = imu->mag[1]; - mag_buff[mag_count][2] = imu->mag[2]; - mag_count++; - } - else{ - mag_count = 0; - } - if (MAG_BUF_LEN == mag_count && CALIBRATE_OFF == mag_calibrate) - { - /*处理代码(模拟退火)start*/ - float xmin = 0.0f; - float ymin = 0.0f; - float zmin = 0.0f; - float xmax = 0.0f; - float ymax = 0.0f; - float zmax = 0.0f; - float err_ori = 0.0; - float r[6] = { 0 }; - float mag_buff_T[3][MAG_BUF_LEN] = { { 0 } }; - mag_m_trans(mag_buff, mag_buff_T); - mag_min(mag_buff_T[0], &xmin); - mag_min(mag_buff_T[1], &ymin); - mag_min(mag_buff_T[2], &zmin); - mag_max(mag_buff_T[0], &xmax); - mag_max(mag_buff_T[1], &ymax); - mag_max(mag_buff_T[2], &zmax); - float xc = 0.5f*(xmax + xmin); - float yc = 0.5f*(ymax + ymin); - float zc = 0.5f*(zmax + zmin); - float a = 0.5f* (float)fabs(xmax - xmin); - float b = 0.5f* (float)fabs(ymax - ymin); - float c = 0.5f* (float)fabs(zmax - zmin); - for (int i = 0; i < MAG_BUF_LEN; i++) - { - err_ori = err_ori + (float)fabs((mag_buff[i][0] - xc)*(mag_buff[i][0] - xc) / a / a + \ - (mag_buff[i][1] - yc)*(mag_buff[i][1] - yc) / b / b + \ - (mag_buff[i][2] - zc)*(mag_buff[i][2] - zc) / c / c - 1); - } - - float xclast = xc; - float yclast = yc; - float zclast = zc; - float alast = a; - float blast = b; - float clast = c; - float err_last = err_ori; - float xcnew = 0; - float ycnew = 0; - float zcnew = 0; - float anew = 0; - float bnew = 0; - float cnew = 0; - float err_new = 0; - for (int i = 0; i < 1000; i++) - { - mag_rand(r); - xcnew = xclast + r[0] - 0.5f; - ycnew = yclast + r[1] - 0.5f; - zcnew = zclast + r[2] - 0.5f; - anew = (float)fabs(alast + r[3] - 0.5); - bnew = (float)fabs(blast + r[4] - 0.5); - cnew = (float)fabs(clast + r[5] - 0.5); - err_new = 0; - for (int j = 0; j < MAG_BUF_LEN; j++) - { - err_new = err_new + (float)fabs((mag_buff[j][0] - xcnew)*(mag_buff[j][0] - xcnew) / anew / anew + \ - (mag_buff[j][1] - ycnew)*(mag_buff[j][1] - ycnew) / bnew / bnew + \ - (mag_buff[j][2] - zcnew)*(mag_buff[j][2] - zcnew) / cnew / cnew - 1); - } - if (err_new < err_ori){ - xclast = xcnew; - yclast = ycnew; - zclast = zcnew; - alast = anew; - blast = bnew; - clast = cnew; - err_last = err_new; - } - } - float err_0 = 0.0; - for (int j = 0; j < MAG_BUF_LEN; j++) - { - err_0 = err_0 + (float)fabs((mag_buff[j][0])*(mag_buff[j][0]) + (mag_buff[j][1])*(mag_buff[j][1]) + (mag_buff[j][2])*(mag_buff[j][2]) - 1); - } - /*处理代码(模拟退火)end*/ - - if (err_0 > err_last) - { - ellipsoidPoint[0] = xclast; - ellipsoidPoint[1] = yclast; - ellipsoidPoint[2] = zclast; - ellipsoidScale[0] = alast; - ellipsoidScale[1] = blast; - ellipsoidScale[2] = clast; - mag_calibrate = CALIBRATE_ON; - } - else - { - ellipsoidPoint[0] = 0; - ellipsoidPoint[1] = 0; - ellipsoidPoint[2] = 0; - ellipsoidScale[0] = 1; - ellipsoidScale[1] = 1; - ellipsoidScale[2] = 1; - } - mag_count = 0; - memset(*mag_buff, 0, sizeof(mag_buff)); - } - - ellipsoidPoint[0] = 0; - ellipsoidPoint[1] = 0; - ellipsoidPoint[2] = 0; - ellipsoidScale[0] = 1; - ellipsoidScale[1] = 1; - ellipsoidScale[2] = 1; - - imu->mag[0] = (imu->mag[0] - ellipsoidPoint[0]) * ellipsoidScale[0]; - imu->mag[1] = (imu->mag[1] - ellipsoidPoint[1]) * ellipsoidScale[1]; - imu->mag[2] = (imu->mag[2] - ellipsoidPoint[2]) * ellipsoidScale[2]; -} - -/**--------------------------------------------------------------------- -* Function : mag_rand -* Description : 生成固定长度的0-1的随机数数组 -* Date : 2020/09/02 logzhan -*---------------------------------------------------------------------**/ -void mag_rand(float r[6]){ - float temp[6] = { 0.0 }; - srand((unsigned)time(NULL)); //读取系统时间，产生一个种子数值 - for (int i = 0; i < 6; i++) - { - temp[i] = rand() / (RAND_MAX + 1.0f); - } - for (int i = 0; i < 6; i++) - { - r[i] = temp[i]; - } -} - -/**--------------------------------------------------------------------- -* Function : mag_min -* Description : 返回数组中的最小值 -* Date : 2020/09/02 logzhan -*---------------------------------------------------------------------**/ -void mag_min(float a[MAG_BUF_LEN],float *mag_min){ - float temp; - temp = a[0]; - for (int i = 1; i < MAG_BUF_LEN; i++) - { - if (a[i] < temp) - temp = a[i]; - } - *mag_min = temp; -} - -/**--------------------------------------------------------------------- -* Function : mag_max -* Description : 返回数组中的最大值 -* Date : 2020/09/02 logzhan -*---------------------------------------------------------------------**/ -void mag_max(float a[MAG_BUF_LEN], float *mag_max){ - float temp; - temp = a[0]; - for (int i = 1; i < MAG_BUF_LEN; i++) - { - if (a[i] > temp) - temp = a[i]; - } - *mag_max = temp; -} - -/**--------------------------------------------------------------------- -* Function : mag_m_trans -* Description : 矩阵转置 -* Date : 2020/09/02 logzhan -*---------------------------------------------------------------------**/ -void mag_m_trans(float a[MAG_BUF_LEN][3], float r[3][MAG_BUF_LEN]) { - int i, j; - - for (i = 0; i < 3; i++) { - for (j = 0; j < MAG_BUF_LEN; j++) { - r[i][j] = a[j][i]; - } - } -} - /**--------------------------------------------------------------------- * Function : fft_buffer * Description : 对acc数据做傅里叶变换，频率信息保存在fft中 * Date : 2020/7/20 logzhan *---------------------------------------------------------------------**/ -static int fft_buffer(myComplex *fft, BUFFER *buffer) { +static int fft_buffer(Complex_t *fft, Buffer_t *buffer) { static float s_signal[1024] = { 0 }; float mean; int index; int i; - Buffer_mean(buffer, &mean); + 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新*/ } @@ -374,7 +176,7 @@ static int fft_buffer(myComplex *fft, BUFFER *buffer) { //序号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))) { + if (0 == FFT_Fft(fft, s_signal, ARR_LEN(s_signal))) { return DETECTOR_OUT_OF_MEMORY; } return DETECTOR_NO_ERROR; @@ -385,7 +187,8 @@ static int fft_buffer(myComplex *fft, BUFFER *buffer) { * Description : 获取前number个较大频点的频率frequency和幅度amplitude * Date : 2020/7/20 logzhan *---------------------------------------------------------------------**/ -static int find_peaks(float *frequency, float *amplitude, int number, myComplex *fft, int length, float cut_off_frequency, float min_threshold) { +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; @@ -482,8 +285,8 @@ static void copy_buffer(BUFFER_SHORT *dst, BUFFER_SHORT *src, int length) { int dst_idx; int src_idx; - Buffer_count((BUFFER *)dst, &dst_cnt); - Buffer_count((BUFFER *)src, &src_cnt); + 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; @@ -548,12 +351,12 @@ static int track_frequency(BUFFER_SHORT *frequency_buffer, BUFFER_SHORT *amplitu /* 4.push main frequency and main amplitude buffer */ for (i = 0; i < number; ++i) { if (1e3 == COST(i, assignment[i])) { - buffer_push((BUFFER *)&frequency_buffer[i], 0); - buffer_push((BUFFER *)&litude_buffer[i], 0); + BufferPush((Buffer_t *)&frequency_buffer[i], 0); + BufferPush((Buffer_t *)&litude_buffer[i], 0); } else { - buffer_push((BUFFER *)&frequency_buffer[i], frequency[assignment[i]]); - buffer_push((BUFFER *)&litude_buffer[i], amplitude[assignment[i]]); + 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; } @@ -584,7 +387,7 @@ static int track_frequency(BUFFER_SHORT *frequency_buffer, BUFFER_SHORT *amplitu /* 8.push temp buffer */ for (i = 0; i < number; ++i) { - buffer_push((BUFFER *)&temp_buffer[i], frequency[assignment[i]]); + BufferPush((Buffer_t *)&temp_buffer[i], frequency[assignment[i]]); } /* 9.caculate temp mean */ @@ -720,9 +523,9 @@ static void get_feature(float *feature, int length) { int j; float temp; - Buffer_mean((BUFFER *)&g_mol_buf[ACCE], &feature[0]); - Buffer_std( (BUFFER *)&g_mol_buf[ACCE], &feature[1]); - Buffer_std( (BUFFER *)&g_mol_buf[GYRO], &feature[2]); + 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; @@ -829,7 +632,7 @@ static int debounce(int *label, int length) { * Date : 2022/09/23 logzhan *---------------------------------------------------------------------**/ int DetectMotionType() { - static myComplex s_fft[1024] = { {0.0} }; + static Complex_t s_fft[1024] = { {0.0} }; static float s_feature[15] = { 0 }; float frq[3]; float amp[3]; @@ -837,7 +640,7 @@ int DetectMotionType() { int i; // 分析加速度计 - if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (BUFFER *)&g_mol_buf[ACCE]))) { + 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))) { @@ -846,8 +649,9 @@ int DetectMotionType() { 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 *)&g_mol_buf[GYRO]))) { + 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 == g_detector.type || diff --git a/1.Software/PDR 1.04/src/pdr_fft.c b/1.Software/PDR 1.04/src/Fft.c similarity index 55% rename from 1.Software/PDR 1.04/src/pdr_fft.c rename to 1.Software/PDR 1.04/src/Fft.c index c2df619..04a8f05 100644 --- a/1.Software/PDR 1.04/src/pdr_fft.c +++ b/1.Software/PDR 1.04/src/Fft.c @@ -1,6 +1,5 @@ /* Header File Including ------------------------------------------------------------------------ */ -#include "pdr_fft.h" - +#include "Fft.h" /* Macro Declaration ---------------------------------------------------------------------------- */ #define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01 @@ -8,72 +7,30 @@ /* Structure Declaration ------------------------------------------------------------------------ */ typedef struct FFT_MCB { - uint8_t type; - uint32_t parameter[4]; + uint8_t type; + uint32_t parameter[4]; struct FFT_MCB *next; struct FFT_MCB *prev; - uint8_t buffer[10240]; + uint8_t buffer[10240]; } FFT_MCB; /* Global Variable Definition ------------------------------------------------------------------- */ static FFT_MCB gMemery; /* Function Definition -------------------------------------------------------------------------- */ -float COMPLEX_abs(myComplex c) { - return (float)sqrt(c.real * c.real + c.image * c.image); -} - -myComplex COMPLEX_add(myComplex a, myComplex b) { - myComplex c = { 0 }; - - c.real = a.real + b.real; - c.image = a.image + b.image; - - return c; -} - -myComplex COMPLEX_sub(myComplex a, myComplex b) { - myComplex c = { 0 }; - - c.real = a.real - b.real; - c.image = a.image - b.image; - - return c; -} - -myComplex COMPLEX_mul(myComplex a, myComplex b) { - myComplex c; - c.real = a.real * b.real - a.image * b.image; - c.image = a.real * b.image + a.image * b.real; - return c; -} - -myComplex COMPLEX_pow(myComplex c, int n) { - myComplex 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; -} - /**---------------------------------------------------------------------- -* Function : FFT_init -* Description : 初始化gMemery -* Date : 2020/7/20 logzhan +* Function : FftInit +* Description : 初始化傅里叶运算库 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -static int FFT_init(int length) { - myComplex *omega; +static int FftInit(int length) { + Complex_t *omega; uint16_t *position; int bitLength; int i; int shift; int j; - if (length * (sizeof(myComplex) + sizeof(uint16_t)) > sizeof(gMemery.buffer)) { + if (length * (sizeof(Complex_t) + sizeof(uint16_t)) > sizeof(gMemery.buffer)) { return 0; } @@ -84,7 +41,7 @@ static int FFT_init(int length) { gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION; gMemery.parameter[0] = length; - omega = (myComplex *) gMemery.buffer; + 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); @@ -105,30 +62,30 @@ static int FFT_init(int length) { return length; } -int FFT_dft(myComplex *dft, float *signal, int length) { +int FFT_Dft(Complex_t *dft, float *signal, int length) { - if (0 == FFT_init(length)) return 0; + if (0 == FftInit(length)) return 0; for (int k = 0; k < length; ++k) { - dft[k].real = 0; + 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] * ((myComplex *)gMemery.buffer)[ik].real; - dft[k].image += signal[i] * ((myComplex *)gMemery.buffer)[ik].image; + 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(myComplex *fft, float *signal, int length) { +int FFT_Fft(Complex_t *fft, float *signal, int length) { int i,l,j,m; uint16_t *position; - myComplex *omega; + Complex_t *omega; //正常情况下，length正常、没溢出就不会进入return0 - if (length <= 0 || 0 != (length & (length - 1)) || 0 == FFT_init(length)) { + if (length <= 0 || 0 != (length & (length - 1)) || 0 == FftInit(length)) { return 0; } @@ -137,20 +94,20 @@ int FFT_fft(myComplex *fft, float *signal, int length) { fft[i].image = 0; } - position = (uint16_t *)(gMemery.buffer + length * sizeof(myComplex)); + position = (uint16_t *)(gMemery.buffer + length * sizeof(Complex_t)); for (i = 0; i < length; ++i) { if (i < position[i]) { - SWAP(myComplex, fft[i], fft[position[i]]); + SWAP(Complex_t, fft[i], fft[position[i]]); } } - omega = (myComplex *)gMemery.buffer; + 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) { - myComplex delta = COMPLEX_mul(omega[length / l * j], fft[i + m + j]); - fft[i + m + j] = COMPLEX_sub(fft[i + j], delta); - fft[i + j] = COMPLEX_add(fft[i + j], delta); + 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); } } } diff --git a/1.Software/PDR 1.04/src/Filter.c b/1.Software/PDR 1.04/src/Filter.c new file mode 100644 index 0000000..9f14924 --- /dev/null +++ b/1.Software/PDR 1.04/src/Filter.c @@ -0,0 +1,77 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Filter.c +* Current Version : V2.0 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR滤波器支持库 +********************************************************************************/ +/* Header File Including ------------------------------------------------------*/ +#include "stdio.h" +#include "Filter.h" + +/* Function Definition --------------------------------------------------------*/ +/**--------------------------------------------------------------------- +* Function : FilterReset +* Description : 重置滤波器 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterReset(Filter_t *f) { + f->reset = 1; +} + +/**--------------------------------------------------------------------- +* Function : FilterSetCoef +* Description : 滤波器设置参数 +* Input : order coefficient array length, not filter order +* *b b coefficient +* *a a coefficient +* H0 dc signal gain +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void FilterSetCoef(Filter_t *f, int order, double *b, double *a, double H0) { + for (int i = 0; i < order; ++i) { + f->b[i] = b[i]; + f->a[i] = a[i]; + } + f->order = order; + f->H0 = H0; + f->reset = 1; +} + +/**--------------------------------------------------------------------- +* Function : FilterFilter +* Description : 滤波器滤波计算 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +double FilterFilter(Filter_t *f, double x) { + + if (f->order < 1) { + printf("fileter order is set to incorrect order, pls chk \ + your code!\n"); + return 0; + } + + double y = 0.0; + if (f->reset) { + for (int i = 0; i < f->order; ++i) { + f->yout[i] = f->H0 * x; + f->xin[i] = x; + } + y = f->yout[f->order - 1]; + f->reset = 0; + } + // push x and y + for (int i = 0; i < f->order - 1; ++i) { + f->xin[i] = f->xin[i + 1]; + f->yout[i] = f->yout[i + 1]; + } + f->xin[f->order - 1] = x; + // filter + y = f->b[0] * f->xin[f->order - 1]; + for (int i = 1; i < f->order; ++i) { + y += f->b[i] * f->xin[f->order - 1 - i] - f->a[i] * f->yout[f->order - 1 - i]; + } + f->yout[f->order - 1] = y; + + return y / f->H0; +} \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/Interface.c b/1.Software/PDR 1.04/src/Interface.c index bf7bab5..ba7e97f 100644 --- a/1.Software/PDR 1.04/src/Interface.c +++ b/1.Software/PDR 1.04/src/Interface.c @@ -1,9 +1,8 @@ -/******************** (C) COPYRIGHT 2020 Geek************************************ -* File Name : pdr_api.c -* Department : Sensor Algorithm Team +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Interface.c * Current Version : V2.0 * Author : logzhan -* Date of Issued : 2020.7.3 +* Date of Issued : 2022.10.15 * Comments : PDR导航算法对外部接口 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ @@ -19,13 +18,13 @@ /* Global Variable Definition ------------------------------------------------*/ const char* PDR_Version = "2.0"; -Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}}; -LatLng_t llaLast = { 0 }; -lct_nmea NmeaData; -IMU_t ImuData; +Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}}; +LatLng_t llaLast = { 0 }; +Nmea_t NmeaData; +IMU_t ImuData; /* Extern Variable Definition ------------------------------------------------*/ -extern EKFPara_t g_kfPara; +extern EKFPara_t g_kfPara; extern AHRS_t g_ahrs; extern GNSS_t pgnss; @@ -36,7 +35,7 @@ extern GNSS_t pgnss; * Description : 初始化RMC协议 * Date : 2022/09/16 logzhan *---------------------------------------------------------------------**/ -void RMC_Init(lct_nmea_rmc * rmc) +void RMC_Init(NmeaRMC_t * rmc) { rmc->type = SENSOR_MIN; rmc->rmc_utc = ITEM_INVALID; @@ -62,7 +61,7 @@ void RMC_Init(lct_nmea_rmc * rmc) * Description : 初始化GPS的NMEA协议的相关标志位 * Date : 2020/7/3 logzhan *---------------------------------------------------------------------**/ -void NmeaFlag_Init(lct_nmea * ln) +void NmeaFlag_Init(Nmea_t * ln) { ln->gga.update = 0; for (char i = 0; i < 3; i++) { @@ -88,7 +87,7 @@ void NmeaFlag_Init(lct_nmea * ln) * Description : 清空Nmea标志位 * Date : 2020/7/4 logzhan *---------------------------------------------------------------------**/ -void ClearNmeaFlg(lct_nmea * ln){ +void ClearNmeaFlg(Nmea_t * ln){ NmeaFlag_Init(ln); } @@ -96,7 +95,7 @@ void ClearNmeaFlg(lct_nmea * ln){ * Function : Algorithm_Init * Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨 * 迹平滑窗口的初始化 -* Date : 2020/7/3 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ void Algorithm_Init(void) { // 导航相关初始化 @@ -115,7 +114,7 @@ void Algorithm_Init(void) { * Date : 2020/07/03 logzhan * 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 *---------------------------------------------------------------------**/ -int ParseLineAndUpdate(char* line, lct_fs* locFusion) { +int ParseLineAndUpdate(char* line, LctFs_t* locFusion) { ParseLineStr(line, &ImuData, &NmeaData); return LocationMainLoop(&ImuData, &NmeaData, locFusion, NULL); @@ -129,7 +128,7 @@ int ParseLineAndUpdate(char* line, lct_fs* locFusion) { * Date : 2020/07/03 logzhan * 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 *---------------------------------------------------------------------**/ -int LocationMainLoop(IMU_t *ImuData, lct_nmea *NmeaData, lct_fs *LocFusion, FILE *fp_gps) { +int LocationMainLoop(IMU_t *ImuData, Nmea_t *NmeaData, LctFs_t *LocFusion, FILE *fpGps) { int type = 0; if (ImuData->gyr.update) { @@ -141,7 +140,7 @@ int LocationMainLoop(IMU_t *ImuData, lct_nmea *NmeaData, lct_fs *LocFusion, FILE if (!NmeaData->update)return TYPE_FIX_NONE; // 写GPS相关LOG信息 - SaveGnssInfo(NmeaData, LocFusion, fp_gps); + SaveGnssInfo(NmeaData, LocFusion, fpGps); // 有GPS则采用GPS融合定位 int flag = GnssInsFusionLocation(NmeaData, &g_kfPara, LocFusion); @@ -190,9 +189,9 @@ int LocationMainLoop(IMU_t *ImuData, lct_nmea *NmeaData, lct_fs *LocFusion, FILE * Description : 保存GPS相关信息 * Date : 2020/7/3 logzhan *---------------------------------------------------------------------**/ -void SaveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps) +void SaveGnssInfo(Nmea_t* nmea_data, LctFs_t* result, FILE* fp_gps) { - lct_nmea_rmc rmc = nmea_data->rmc[1]; + NmeaRMC_t rmc = nmea_data->rmc[1]; // 选取更新时间的rmc if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) { rmc = nmea_data->rmc[0]; @@ -216,11 +215,11 @@ void SaveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps) } /**---------------------------------------------------------------------- -* Function : GetLIBVersion -* Description : 获取pdr库版本号 -* Date : 2020/8/3 logzhan +* Function : GetPDRVersion +* Description : 获取pdr版本号 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -const char* GetLIBVersion(void){ +const char* GetPDRVersion(void){ return PDR_Version; } diff --git a/1.Software/PDR 1.04/src/Kalman.c b/1.Software/PDR 1.04/src/Kalman.c index fbfa374..49be27e 100644 --- a/1.Software/PDR 1.04/src/Kalman.c +++ b/1.Software/PDR 1.04/src/Kalman.c @@ -1,9 +1,9 @@ /******************** (C) COPYRIGHT 2020 Geek************************************ -* File Name : pdr_kalman.c -* Current Version : V2.0(compare QCOM SAP 5.0) +* File Name : Kalman.c +* Current Version : V2.0 * Author : logzhan -* Date of Issued : 2020.7.3 -* Comments : PDR 卡尔曼滤波器相关 +* Date of Issued : 2022.10.15 +* Comments : PDR扩展卡尔曼滤波器融合 ********************************************************************************/ #include #include @@ -16,7 +16,7 @@ #include "Location.h" #include "Matrix.h" #include "Utils.h" -#include "pdr_linearFit.h" +#include "LinearFit.h" #include "CoordTrans.h" // 扩展卡尔曼向量缓存 @@ -29,14 +29,14 @@ extern double GpsPos[HIST_GPS_NUM][3]; extern int debugCount; extern double angle_mean[3]; -static int hold_type; -static double b_timestamp; -static double b_last_time; -static double attitude_yaw; -static DETECTOR_t *g_detector; -static int hold_type; -static double BUL_duration; -static double FUR_duration; +static int hold_type; +static double b_timestamp; +static double b_last_time; +static double attitude_yaw; +static Detector_t *g_detector; +static int hold_type; +static double BUL_duration; +static double FUR_duration; /**---------------------------------------------------------------------- * Function : KalmanFilter_Init @@ -53,13 +53,12 @@ void EKF_Init(void) hold_type = 0; } - /**---------------------------------------------------------------------- -* Function : pdr_ekfStatePredict +* Function : EKFStatePredict * Description : pdr卡尔曼滤波器的状态预测方程 -* Date : 2020/7/22 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { +void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) { double (*Phi)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]); double (*pvec1)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]); @@ -67,8 +66,8 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N); - double deltaHeading = g_pdr->heading - g_pdr->lastHeading; - double angle = g_pdr->heading; + double deltaHeading = pdr->heading - pdr->lastHeading; + double angle = pdr->heading; //printf("deltaHeading = %f\n", R2D(deltaHeading)); //if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) { @@ -77,27 +76,25 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { // angle = g_pdr->heading; //} - if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD) { + if (pdr->motionType != PDR_MOTION_TYPE_HANDHELD) { deltaHeading = 0; } - angle = deltaHeading + kf->pXk[3]; + angle = deltaHeading + ekf->pXk[3]; if (fabs(deltaHeading) > 5.5) { - //printf("%f %f\n", R2D(g_pdr->heading), R2D(g_pdr->lastHeading)); - //printf("d heading = %f\n", R2D(deltaHeading)); if (deltaHeading < 0) { deltaHeading = deltaHeading + D2R(360); }else { deltaHeading = deltaHeading - D2R(360); } deltaHeading = 0; - angle = deltaHeading + kf->pXk[3]; + angle = deltaHeading + ekf->pXk[3]; } if(step == 0){ - kf->pXk[3] = angle; + ekf->pXk[3] = angle; } // xk+1 = xk + step * cos(angle) PDR位置更新方程 @@ -116,21 +113,21 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { // phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性 // 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度 // 让预测的值更新，但是效果暂时没有感觉出明显的好 - if (fabs(kf->pXk[3]) > 0.000001 && - fabs(angle / kf->pXk[3]) < 3) + if (fabs(ekf->pXk[3]) > 0.000001 && + fabs(angle / ekf->pXk[3]) < 3) { - Phi[3][3] = angle / kf->pXk[3]; + Phi[3][3] = angle / ekf->pXk[3]; } // 卡尔曼状态更新方程 // pXk = phi * pXk - VecMatMultiply(kf->pXk, Phi, kf->pXk); + VecMatMultiply(ekf->pXk, Phi, ekf->pXk); // 卡尔曼更新协方差矩阵 : Pk = FPFt + Q - MatrixMultiply(Phi, kf->pPk, pvec1); // F*P + MatrixMultiply(Phi, ekf->pPk, pvec1); // F*P MatrixTrans(Phi, pvec2); // Ft MatrixMultiply(pvec1, pvec2, pvec1); // F*P*Ft - MatrixAdd(pvec1, kf->Q, kf->pPk); // F*P*Ft + Q + MatrixAdd(pvec1, ekf->Q, ekf->pPk); // F*P*Ft + Q } } @@ -139,7 +136,7 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { * Description : 扩展卡尔曼滤波器的状态更新方程 * Date : 2020/7/22 logzhan *---------------------------------------------------------------------**/ -void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, +void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, Classifer_t *sys, PDR_t *g_pdr, int scene_type) { double lambda = 0; @@ -157,7 +154,6 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, memset(EkfVecBuf, 0, sizeof(double) * 5 * N); // Nx1向量定义 - double* z = &(EkfVecBuf[0][0]); double(*pvec5) = &(EkfVecBuf[1][0]); double(*pvec6) = &(EkfVecBuf[2][0]); double(*pv1) = &(EkfVecBuf[3][0]); @@ -169,6 +165,7 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, I[i][i] = 1; } + double* z = &(EkfVecBuf[0][0]); // 获取观测向量 z[0] = pgnss->xNed; // 北向位置 z[1] = pgnss->yNed; // 东向位置 @@ -323,7 +320,7 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_ * Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void StateRecognition(float *acc, classifer *sys) { +void StateRecognition(float *acc, Classifer_t *sys) { } @@ -460,9 +457,8 @@ double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) /**---------------------------------------------------------------------- * Function : InsLocationPredict -* Description : PDR惯导位置更新, -* Date : 2020/07/08 logzhan -* 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 +* Description : PDR惯导位置更新 +* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 * 会造成PDR前后位置不够均匀 *---------------------------------------------------------------------**/ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data, @@ -513,7 +509,7 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data, * 2、GPS值赋值给result * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result) { +int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, LctFs_t *result) { double stepLength = 0.7; double ned[3] = { 0 }; @@ -543,12 +539,12 @@ int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result } } // 输出GPS位置结果 - OutputOriginGnssPos(pgnss, result); + OutputRawGnssPos(pgnss, result); g_pdr->NoGnssCount = 0; return PDR_TRUE; } -int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,lct_fs *result) +int InitProc(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) { @@ -583,14 +579,14 @@ int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,lct_fs *result) kf->R[i][l] = 0.0; } } - OutputOriginGnssPos(pgnss, result); + 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) { - OutputOriginGnssPos(pgnss, result); + OutputRawGnssPos(pgnss, result); return PDR_TRUE; } return PDR_FALSE; @@ -603,7 +599,7 @@ int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,lct_fs *result) * 信息精度有关。 * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void EKFCalQRMatrix(lct_nmea *nmea, PDR_t *g_pdr, classifer *sys, +void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys, EKFPara_t *kf) { if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD && @@ -650,25 +646,25 @@ void EKFCalQRMatrix(lct_nmea *nmea, PDR_t *g_pdr, classifer *sys, * Description : PDR的GNSS和INS融合定位 * Date : 2022/09/21 logzhan *---------------------------------------------------------------------**/ -void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *g_pdr, classifer *sys, double yaw_bias, - EKFPara_t *kf, lct_fs *res) { +void GnssInsLocFusion(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 Ned[3] = { 0 }; double yaw_thr = 6; - int scene_type = g_pdr->sceneType; + int scene_type = pdr->sceneType; plla[0] = pgnss->lat; plla[1] = pgnss->lon; plla[2] = 0; - WGS842NED(plla, g_pdr->pllaInit, ned); + WGS842NED(plla, pdr->pllaInit, Ned); - pgnss->xNed = ned[0]; - pgnss->yNed = ned[1]; + pgnss->xNed = Ned[0]; + pgnss->yNed = Ned[1]; //调整融合策略 - EKFStateUpdate(kf, pgnss, sys, g_pdr, scene_type); + EKFStateUpdate(kf, pgnss, sys, pdr, scene_type); for (uchar i = 0; i < N; i++) { for (uchar l = 0; l < N; l++) { @@ -677,11 +673,11 @@ void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *g_pdr, classifer *sys, double yaw_bi kf->pXk[i] = kf->Xk[i]; } - ned[0] = kf -> pXk[0] - g_pdr->x0; - ned[1] = kf -> pXk[1] - g_pdr->y0; - ned[2] = 0; + Ned[0] = kf -> pXk[0] - pdr->x0; + Ned[1] = kf -> pXk[1] - pdr->y0; + Ned[2] = 0; - NED2WGS84(g_pdr->pllaInit, ned, plla); + NED2WGS84(pdr->pllaInit, Ned, plla); res->latitude = plla[0]; res->latitude_ns = pgnss->lat_ns; @@ -689,5 +685,5 @@ void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *g_pdr, classifer *sys, double yaw_bi res->longitudinal_ew = pgnss->lon_ew; res->time = pgnss->timestamps; - g_pdr->NoGnssCount = 0; + pdr->NoGnssCount = 0; } diff --git a/1.Software/PDR 1.04/src/pdr_linearFit.c b/1.Software/PDR 1.04/src/LinearFit.c similarity index 99% rename from 1.Software/PDR 1.04/src/pdr_linearFit.c rename to 1.Software/PDR 1.04/src/LinearFit.c index 03f2cad..9586d7c 100644 --- a/1.Software/PDR 1.04/src/pdr_linearFit.c +++ b/1.Software/PDR 1.04/src/LinearFit.c @@ -9,7 +9,7 @@ /* Header File Including -----------------------------------------------------*/ #include #include -#include "pdr_linearFit.h" +#include "LinearFit.h" #include "Utils.h" /**---------------------------------------------------------------------- diff --git a/1.Software/PDR 1.04/src/Location.c b/1.Software/PDR 1.04/src/Location.c index f91aec0..926850c 100644 --- a/1.Software/PDR 1.04/src/Location.c +++ b/1.Software/PDR 1.04/src/Location.c @@ -25,7 +25,7 @@ /* Global Variable Definition ------------------------------------------------*/ PDR_t* pdr; // PDR全局信息 StepPredict stepPredict = { 0 }; -classifer sys; +Classifer_t sys; GNSS_t pgnss; // 定位相关信息 EKFPara_t g_kfPara; double GpsPos[HIST_GPS_NUM][3] = {{0}}; @@ -139,7 +139,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { //pdr->heading = CalPredHeading(...); - double imuYaw = - g_ahrs.yaw; + double imuYaw = - g_ahrs.Yaw; if (imuYaw < 0) { imuYaw += 360; } @@ -163,7 +163,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { * Description : PDR GPS融合INS惯性定位 * Date : 2021/01/29 logzhan *---------------------------------------------------------------------**/ -int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) +int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result) { // GPS数据更新 GnssUpdate(&pgnss, nmea_data); @@ -228,7 +228,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) // 不满足推算条件，直接输出原始GPS if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 15) { - OutputOriginGnssPos(&pgnss, result); + OutputRawGnssPos(&pgnss, result); pdr->NoGnssCount = 0; return TYPE_FIX_GPS; } @@ -248,7 +248,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) * Description : 在没有gps信息时预测GPS位置 * Date : 2022/09/16 *---------------------------------------------------------------------**/ -void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* p_pdr) +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; @@ -270,13 +270,13 @@ void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* p_pdr) * Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR_t* p_pdr) { +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) - && (pdr_detStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2)) + && (DetUserStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2)) { /*这里放姿态角与GPS对准part1 start*/ double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]), @@ -300,12 +300,12 @@ void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR_t* p_pdr) { * Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高 * 代码的可读性 *---------------------------------------------------------------------**/ -int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, lct_fs* result) { +int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result) { // 检测用户是否处于静止状态 - if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) { + if (DetUserStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) { if (pgnss->error > 0 && pgnss->error < 15 && pgnss->lastError >= pgnss->error) { - OutputOriginGnssPos(pgnss, result); + OutputRawGnssPos(pgnss, result); pgnss->lastError = pgnss->error; return TYPE_FIX_PDR; } @@ -318,7 +318,7 @@ int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, lct_fs* result) { ResetSystemStatus(kf); if (pgnss->error >= 0 || (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0)) { - OutputOriginGnssPos(pgnss, result); + OutputRawGnssPos(pgnss, result); return TYPE_FIX_GPS; } // 精度过差，不输出位置 @@ -348,9 +348,9 @@ double GnssCalHeading(GNSS_t* pgnss) { * Description : nmea数据结构转gnss数据 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss) +void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss) { - lct_nmea_rmc rmc = { 0 }; + NmeaRMC_t rmc = { 0 }; if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) { rmc = nmea_data->rmc[0]; }else { @@ -380,7 +380,7 @@ void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss) * 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没 * 什么用，还增加运算量 *---------------------------------------------------------------------**/ -void GnssUpdate(GNSS_t* gps, lct_nmea* nmea) { +void GnssUpdate(GNSS_t* gps, Nmea_t* nmea) { // nmea数据转gnss数据结构 Nmea2Gnss(nmea, &pgnss); // GPS回调更新，暂时不清楚函数功能 diff --git a/1.Software/PDR 1.04/src/Main.cpp b/1.Software/PDR 1.04/src/Main.cpp index 45ed90c..048d668 100644 --- a/1.Software/PDR 1.04/src/Main.cpp +++ b/1.Software/PDR 1.04/src/Main.cpp @@ -60,7 +60,7 @@ #include #include #include "pdr_api.h" -#include "pdr_main.h" +#include "Main.h" #include "Utils.h" #include "time.h" using namespace std; @@ -114,7 +114,7 @@ int main(int argc, char* argv[]) // PDR 仿真流程 Algorithm_Init(); - lct_fs locFusion; + LctFs_t locFusion; memset(&locFusion,0, sizeof(locFusion)); memset(&resTracks, 0, sizeof(resTracks)); @@ -190,7 +190,7 @@ double gpsYaw2GoogleYaw(double heading) { * Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹 * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs) +void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs) { resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude; resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal; @@ -387,10 +387,10 @@ PosFusion LibPDR_StepUpdateGPS(int useGpsFlg) PosFusion fusion; fusion.vaild = 0; - lct_nmea location_nmea; - lct_nmea* ln = &location_nmea; + Nmea_t location_nmea; + Nmea_t* ln = &location_nmea; imu imu_data; - lct_fs lct_fusion; + 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)); diff --git a/1.Software/PDR 1.04/src/m_munkres.c b/1.Software/PDR 1.04/src/Munkres.c similarity index 95% rename from 1.Software/PDR 1.04/src/m_munkres.c rename to 1.Software/PDR 1.04/src/Munkres.c index fff595e..101373b 100644 --- a/1.Software/PDR 1.04/src/m_munkres.c +++ b/1.Software/PDR 1.04/src/Munkres.c @@ -1,9 +1,9 @@ -/* Header File Including ------------------------------------------------------------------------ */#include +/* Header File Including ------------------------------------------------------------------------ */ +#include #include #include "m_munkres.h" -//#include "loc_vivo_pdr.h" /* Macro Definition ----------------------------------------------------------------------------- */ #define COST(i, j) cost[(i) + (j) * m] #define MARK(i, j) mark[(i) + (j) * m] @@ -20,7 +20,6 @@ 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); @@ -29,10 +28,6 @@ static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, u 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); -#ifdef __MATLAB_DBG -void display_cost_matrix_and_cover(float *cost, int m, int n, uint8_t row_cover, uint8_t col_cover); -void display_mark_matrix_and_cover(int *mark, int m, int n, uint8_t row_cover, uint8_t col_cover); -#endif /* Function Definition -------------------------------------------------------------------------- */ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) { @@ -40,14 +35,7 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) { int j = 0; int step; - #if PRINT_ALGO_RUN_TIME - uint64 time_algo_1 = 0l; - uint64 time_algo_2 = 0l; - uint64 time_algo_3 = 0l; - uint64 time_algo_4 = 0l; - #endif - if (m > 8 || n > 8) { return MUNKRES_EXCESSIVE_MATRIX; } diff --git a/1.Software/PDR 1.04/src/PDRBase.c b/1.Software/PDR 1.04/src/PDRBase.c index 99fa214..045b56d 100644 --- a/1.Software/PDR 1.04/src/PDRBase.c +++ b/1.Software/PDR 1.04/src/PDRBase.c @@ -17,7 +17,7 @@ #include "buffer.h" #include "Filter.h" #include "pdr_base.h" -#include "pdr_fft.h" +#include "Fft.h" /* Macro Definition ----------------------------------------------------------*/ #define DOWNSAMPLING_RATE 4 @@ -31,7 +31,7 @@ /* Global Variable Definition ------------------------------------------------*/ PDR_t g_pdr; -static DETECTOR_t *g_detector; +static Detector_t *g_detector; static uint64_t g_tick_p; static uint32_t g_motion_type; static uint32_t g_axis_ceof_counter; @@ -44,7 +44,7 @@ static int angle_count; static double p_angle[3][BUF_LEN]; double angle_mean[3] = { 0 }; -static FILTER g_grv_flt[2]; +static Filter_t g_grv_flt[2]; #if defined(__MATLAB_DBG) static int g_debug[2]; @@ -61,26 +61,26 @@ const double g_grv_flt_a[] = { 1, -8.39368255078839, 31.8158646581919, -7 const double g_grv_flt_h0 = 1; PDR_t* Base_Init() { - g_detector = pdr_getDetector(); - Buffer_initialize((BUFFER *)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[1], "x_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[2], "x_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[3], "y_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[4], "y_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[5], "y_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[6], "z_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[7], "z_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN); - Buffer_initialize((BUFFER *)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE); - Buffer_initialize((BUFFER *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60); - Buffer_initialize((BUFFER *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60); - Buffer_initialize((BUFFER *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60); - Buffer_initialize((BUFFER *)&g_posi[3], "posi_3", BUFFER_TYPE_QUEUE, 60); + g_detector = GetDetectorObj(); + BufferInit((Buffer_t *)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[1], "x_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[2], "x_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[3], "y_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[4], "y_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[5], "y_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[6], "z_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[7], "z_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN); + BufferInit((Buffer_t *)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE); + BufferInit((Buffer_t *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60); + BufferInit((Buffer_t *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60); + BufferInit((Buffer_t *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60); + BufferInit((Buffer_t *)&g_posi[3], "posi_3", BUFFER_TYPE_QUEUE, 60); - FILTER_SetCoef(&g_grv_flt[0], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0); - FILTER_SetCoef(&g_grv_flt[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0); + FilterSetCoef(&g_grv_flt[0], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0); + FilterSetCoef(&g_grv_flt[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0); pdr_resetData(); return &g_pdr; @@ -127,24 +127,24 @@ void pdr_resetData(void) { g_tick_p = 0; g_motion_type = g_detector->type; g_axis_ceof_counter = 0; - Buffer_clear((BUFFER *)&g_axis[0]); - Buffer_clear((BUFFER *)&g_axis[1]); - Buffer_clear((BUFFER *)&g_axis[2]); - Buffer_clear((BUFFER *)&g_axis[3]); - Buffer_clear((BUFFER *)&g_axis[4]); - Buffer_clear((BUFFER *)&g_axis[5]); - Buffer_clear((BUFFER *)&g_axis[6]); - Buffer_clear((BUFFER *)&g_axis[7]); - Buffer_clear((BUFFER *)&g_axis[8]); - Buffer_clear((BUFFER *)&g_grav[0]); - Buffer_clear((BUFFER *)&g_grav[1]); - Buffer_clear((BUFFER *)&g_erro); - Buffer_clear((BUFFER *)&g_posi[0]); - Buffer_clear((BUFFER *)&g_posi[1]); - Buffer_clear((BUFFER *)&g_posi[2]); - Buffer_clear((BUFFER *)&g_posi[3]); - FILTER_Reset(&g_grv_flt[0]); - FILTER_Reset(&g_grv_flt[1]); + BufferClear((Buffer_t *)&g_axis[0]); + BufferClear((Buffer_t *)&g_axis[1]); + BufferClear((Buffer_t *)&g_axis[2]); + BufferClear((Buffer_t *)&g_axis[3]); + BufferClear((Buffer_t *)&g_axis[4]); + BufferClear((Buffer_t *)&g_axis[5]); + BufferClear((Buffer_t *)&g_axis[6]); + BufferClear((Buffer_t *)&g_axis[7]); + BufferClear((Buffer_t *)&g_axis[8]); + BufferClear((Buffer_t *)&g_grav[0]); + BufferClear((Buffer_t *)&g_grav[1]); + BufferClear((Buffer_t *)&g_erro); + BufferClear((Buffer_t *)&g_posi[0]); + BufferClear((Buffer_t *)&g_posi[1]); + BufferClear((Buffer_t *)&g_posi[2]); + BufferClear((Buffer_t *)&g_posi[3]); + FilterReset(&g_grv_flt[0]); + FilterReset(&g_grv_flt[1]); angle_count = 0; memset(&p_angle, 0, sizeof(p_angle)); @@ -376,11 +376,11 @@ void ComputePCA(AHRS_t *ahrs) { } /**---------------------------------------------------------------------- -* Function : OutputOriginGnssPos -* Description : PDR输出原始GNSS定位结果 -* Date : 2022/09/16 logzhan +* Function : OutputRawGnssPos +* Description : 输出GPS位置 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void OutputOriginGnssPos(GNSS_t* pgnss, lct_fs* result) { +void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) { result->latitude = pgnss->lat; result->latitude_ns = pgnss->lat_ns; result->longitudinal = pgnss->lon; @@ -395,7 +395,7 @@ void OutputOriginGnssPos(GNSS_t* pgnss, lct_fs* result) { * 单运动频率计算量太大，可以考虑用加速度判断用户状态。 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ -int pdr_detStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) { +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)) { @@ -414,9 +414,9 @@ int pdr_detStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) { * Output : int，车载模式标志位 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ -int DetectCarMode(GNSS_t* pgnss, PDR_t* g_pdr, unsigned long delSteps, int* time) { +int DetectCarMode(GNSS_t* pgnss, PDR_t* pdr, unsigned long delSteps, int* time) { if ((pgnss->vel > 10 && delSteps == 0) || - (delSteps == 0 && g_pdr->motionFreq == 0.0 && pgnss->vel > 3 && pgnss->error < 30)) + (delSteps == 0 && pdr->motionFreq == 0.0 && pgnss->vel > 3 && pgnss->error < 30)) { (*time)++; }else { @@ -429,12 +429,12 @@ int DetectCarMode(GNSS_t* pgnss, PDR_t* g_pdr, unsigned long delSteps, int* time } /**---------------------------------------------------------------------- -* Function : detPdrToReset +* Function : DetectPdrToReset * Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况 * 重置PDR到GPS的位置。 -* Date : 2021/01/28 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -int detPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pdr) { +int DetectPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pdr) { if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (g_pdr->motionFreq == 0))) { (*gpscnt)++; @@ -445,11 +445,11 @@ int detPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pdr) } /**--------------------------------------------------------------------- -* Function : detectorUpdate -* Description : 根据新确定的手机携带方式更新g_pdr.status等 -* Date : 2020/7/20 logzhan +* Function : DetectorUpdate +* Description : 根据新确定的手机携带方式更新状态等 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void detectorUpdate(DETECTOR_t *detector) { +void DetectorUpdate(Detector_t *detector) { // 设置PDR的运动类型 g_pdr.motionType = detector->type; @@ -511,14 +511,14 @@ void gpsUpdateCb(GNSS_t* gps) } /* caculate position */ - Buffer_top((BUFFER *)&g_posi[0], &temp); - buffer_push((BUFFER *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI)); - Buffer_top((BUFFER *)&g_posi[1], &temp); - buffer_push((BUFFER *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI)); - Buffer_top((BUFFER *)&g_posi[2], &temp); - buffer_push((BUFFER *)&g_posi[2], temp + g_pdr.pca_direction[0]); - Buffer_top((BUFFER *)&g_posi[3], &temp); - buffer_push((BUFFER *)&g_posi[3], temp + g_pdr.pca_direction[1]); + BufferGetTop((Buffer_t *)&g_posi[0], &temp); + BufferPush((Buffer_t *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI)); + BufferGetTop((Buffer_t *)&g_posi[1], &temp); + BufferPush((Buffer_t *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI)); + BufferGetTop((Buffer_t *)&g_posi[2], &temp); + BufferPush((Buffer_t *)&g_posi[2], temp + g_pdr.pca_direction[0]); + BufferGetTop((Buffer_t *)&g_posi[3], &temp); + BufferPush((Buffer_t *)&g_posi[3], temp + g_pdr.pca_direction[1]); if ((g_posi[0]._top - g_posi[0]._bottom + g_posi[0].length + 1) % (g_posi[0].length + 1) >= 5) { count = 0; diff --git a/1.Software/PDR 1.04/src/ParseData.c b/1.Software/PDR 1.04/src/ParseData.c index 6451ec9..14170ef 100644 --- a/1.Software/PDR 1.04/src/ParseData.c +++ b/1.Software/PDR 1.04/src/ParseData.c @@ -158,7 +158,7 @@ void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp) #endif } -void parseNMEA(char* pt, lct_nmea *ln, double ts) +void parseNMEA(char* pt, Nmea_t *ln, double ts) { long i, result; long s = 0 ; @@ -213,7 +213,7 @@ void parseNMEA(char* pt, lct_nmea *ln, double ts) } } -void preprocessNMEA(lct_nmea *ln) +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)) @@ -226,7 +226,7 @@ void preprocessNMEA(lct_nmea *ln) } } -void ParseGGA(char* pt, lct_nmea *ln, double ts) +void ParseGGA(char* pt, Nmea_t *ln, double ts) { if (!strcmp(pt, GNGGA_STR)) { @@ -388,12 +388,12 @@ void ParseGGA(char* pt, lct_nmea *ln, double ts) ln->gga.time = ts; } -void ParseRMC(char* pt, lct_nmea *ln, double ts) +void ParseRMC(char* pt, Nmea_t *ln, double ts) { int i = 0; double temp = 0; double rmc_time = 0; - lct_nmea_rmc *rmc = NULL; + NmeaRMC_t *rmc = NULL; if (!strcmp(pt, GNRMC_STR)) { @@ -524,13 +524,13 @@ void ParseRMC(char* pt, lct_nmea *ln, double ts) rmc->time = ts; } -void parseGSV(char* pt, lct_nmea *ln, double 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; - lct_nmea_gsv* gsv = NULL; + NmeaGSV_t* gsv = NULL; if (!strcmp(pt, GPGSV_STR)) { gsv = &ln->gsv[0]; @@ -676,11 +676,11 @@ void parseGSV(char* pt, lct_nmea *ln, double ts) gsv->time = ts; } -void ParseGSA(char* pt, lct_nmea *ln, double ts) +void ParseGSA(char* pt, Nmea_t *ln, double ts) { int i = 0; - int ind = -1; - lct_nmea_gsa gsa = {0}; + int idx = -1; + NmeaGSA_t gsa = {0}; if (!strcmp(pt, GNGSA_STR)) { gsa.type = SENSOR_LOCATION_NMEA_GNGSA; @@ -688,19 +688,19 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts) } else if (!strcmp(pt, GPGSA_STR)) { - ind = 0; + idx = 0; gsa.type = SENSOR_LOCATION_NMEA_GPGSA; gsa.update = 1; } else if (!strcmp(pt, GLGSA_STR)) { - ind = 1; + idx = 1; gsa.type = SENSOR_LOCATION_NMEA_GLGSA; gsa.update = 1; } else if (!strcmp(pt, GAGSA_STR)) { - ind = 2; + idx = 2; gsa.type = SENSOR_LOCATION_NMEA_GAGSA; gsa.update = 1; } @@ -736,38 +736,38 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts) { gsa.prn[i - 2] = atoi(pt); - if (ind == 0 && gsa.prn[i - 2] > 32) + if (idx == 0 && gsa.prn[i - 2] > 32) { gsa.prn[i - 2] = (gsa.prn[i - 2] % 32)+1; } - if (ind == 1 && gsa.prn[i - 2] < 65 && gsa.prn[i - 2] > 99) + if (idx == 1 && gsa.prn[i - 2] < 65 && gsa.prn[i - 2] > 99) { gsa.prn[i - 2] = (gsa.prn[i - 2] % 35) + 65; } - if (ind == 2 && gsa.prn[i - 2] > 36) + 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 (ind < 0) + if (idx < 0) { - ind = 1; + idx = 1; } } else { gsa.prnflg |= (1 << gsa.prn[i - 2]); - if (ind < 0) + if (idx < 0) { if (ln->gsa[0].update && ln->gsa[1].update) { - ind = 2; + idx = 2; } else { - ind = 0; + idx = 0; } } } @@ -779,19 +779,19 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts) gsa.vdop = (float)atof(pt); }else if (17 == i) { - if (ind < 0 && pt[0] == '1') + if (idx < 0 && pt[0] == '1') { - ind = 0; + idx = 0; gsa.type = SENSOR_LOCATION_NMEA_GPGSA; } - else if (ind < 0 && pt[0] == '2') + else if (idx < 0 && pt[0] == '2') { - ind = 1; + idx = 1; gsa.type = SENSOR_LOCATION_NMEA_GLGSA; } - else if (ind < 0 && pt[0] == '3') + else if (idx < 0 && pt[0] == '3') { - ind = 2; + idx = 2; gsa.type = SENSOR_LOCATION_NMEA_GAGSA; } } @@ -804,9 +804,9 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts) } gsa.time = ts; - if (ind >=0 && ind <=2) + if (idx >=0 && idx <=2) { - memcpy(&ln->gsa[ind], &gsa, sizeof(lct_nmea_gsa)); + memcpy(&ln->gsa[idx], &gsa, sizeof(NmeaGSA_t)); } } @@ -816,7 +816,7 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts) * Description : 解析GPS的Accuracy精度参数 * Date : 2020/7/9 yuanlin@vivo.com &logzhan *---------------------------------------------------------------------**/ -void parseLocAccuracy(char* pt, lct_nmea *ln, double ts) +void parseLocAccuracy(char* pt, Nmea_t *ln, double ts) { int i = 0; while (pt && 8>i){ @@ -848,7 +848,7 @@ void parseLocAccuracy(char* pt, lct_nmea *ln, double ts) * Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析 * Date : 2020/7/9 yuanlin@vivo.com &logzhan *---------------------------------------------------------------------**/ -int ParseLineStr(char* line, IMU_t* imu_p, lct_nmea* ln) +int ParseLineStr(char* line, IMU_t* imu_p, Nmea_t* ln) { char* pt = NULL; int i = 0, sstp = -1; diff --git a/1.Software/PDR 1.04/src/Quaternion.cpp b/1.Software/PDR 1.04/src/Quaternion.cpp index e3338c7..e3dd82a 100644 --- a/1.Software/PDR 1.04/src/Quaternion.cpp +++ b/1.Software/PDR 1.04/src/Quaternion.cpp @@ -36,9 +36,9 @@ void QuaternConj(float _q[], float q[]) { * Description : 四元数乘法 * Date : 2022/09/21 logzhan *---------------------------------------------------------------------**/ -void QuaternProd(float ab[], float a[], float b[]) { - ab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]; - ab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]; - ab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1]; - ab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]; +void QuaternProd(float qab[], float a[], float b[]) { + qab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]; + qab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]; + qab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1]; + qab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]; } \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/scene_recognition.c b/1.Software/PDR 1.04/src/SceneRecognition.c similarity index 98% rename from 1.Software/PDR 1.04/src/scene_recognition.c rename to 1.Software/PDR 1.04/src/SceneRecognition.c index 302f4af..d922227 100644 --- a/1.Software/PDR 1.04/src/scene_recognition.c +++ b/1.Software/PDR 1.04/src/SceneRecognition.c @@ -79,7 +79,7 @@ static int alloc_resources (void); 0--success; -1--NULL param inputed; 1--lct_nmea _src_nmea is not valiable beacause of the update flag. */ -static int lctnmea2Gnssinfo (GnssInfo *, const lct_nmea *); +static int lctnmea2Gnssinfo (GnssInfo *, const Nmea_t *); /* Function name: static _detect_good_signal() */ /* Usage: judge the quality of positioning result between good and bad. */ @@ -182,7 +182,7 @@ SCENE_INIT_STATE scene_recognition_reset(void) * Author : logzhan * Date : 2020/02/18 *---------------------------------------------------------------------**/ -SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea *nmea) +SCENE_RECOGNITION_RESULT sceneRecognitionProc(const Nmea_t *nmea) { SCENE_RECOGNITION_RESULT res = RECOG_UNKNOWN; int transRes = 0; @@ -222,7 +222,7 @@ SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea *nmea) * Author : Zhang Jingrui, Geek ID: 11082826 * Date : 2020/02/18 logzhan *---------------------------------------------------------------------**/ -int isOpenArea(const lct_nmea *nmea) +int isOpenArea(const Nmea_t *nmea) { SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea); // 如果是开阔地则返回1 @@ -298,7 +298,7 @@ static int alloc_resources(void) } -static int lctnmea2Gnssinfo(GnssInfo *_dst, const lct_nmea * _src_nmea) +static int lctnmea2Gnssinfo(GnssInfo *_dst, const Nmea_t * _src_nmea) { int _parse_res_state = 0; /* Temporarily deprecated. */ diff --git a/1.Software/PDR 1.04/src/buffer.c b/1.Software/PDR 1.04/src/buffer.c index 4b89164..4b9846c 100644 --- a/1.Software/PDR 1.04/src/buffer.c +++ b/1.Software/PDR 1.04/src/buffer.c @@ -1,3 +1,11 @@ +/******************** (C) COPYRIGHT 2022 Geek************************************ +* File Name : Buffer.c +* Current Version : V1.2 +* Author : logzhan +* Date of Issued : 2022.10.15 +* Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取 +* 和计算 +********************************************************************************/ /* Header File Including ------------------------------------------------------------------------ */ #include #include @@ -9,27 +17,28 @@ #define DEC_PTR(ptr) ((ptr + buffer->length) % (buffer->length + 1)) /* Function Definition -------------------------------------------------------------------------- */ -int Buffer_initialize(BUFFER *buffer, const char *name, int type, int length) { + +int BufferInit(Buffer_t *buf, const char *name, int type, int len) { int i; - if (NULL == buffer) { + if (NULL == buf) { return BUFFER_WRONG_PARAMETER; } - memset(buffer->data, 0, buffer->length * sizeof(*(buffer->data))); + memset(buf->data, 0, buf->length * sizeof(*(buf->data))); for (i = 0; i < 20 - 1 && '\0' != name[i]; ++i) { - buffer->name[i] = name[i]; + buf->name[i] = name[i]; } - buffer->name[i] = '\0'; - buffer->type = type; - buffer->length = length; - buffer->_bottom = 0; - buffer->_top = 0; - buffer->mean = 0.0; - buffer->sum = 0.0; + 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 Buffer_clear(BUFFER *buffer) { +int BufferClear(Buffer_t *buffer) { if (NULL == buffer) { return BUFFER_WRONG_PARAMETER; } @@ -42,17 +51,15 @@ int Buffer_clear(BUFFER *buffer) { return BUFFER_NO_ERROR; } -int Buffer_count(BUFFER *buffer, int *count) { +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 Buffer_top(BUFFER *buffer, float *value) { +int BufferGetTop(Buffer_t *buffer, float *value) { *value = 0; if (NULL == buffer) { @@ -68,7 +75,7 @@ int Buffer_top(BUFFER *buffer, float *value) { return BUFFER_NO_ERROR; } -int Buffer_bottom(BUFFER *buffer, float *value) { +int BufferGetBottom(Buffer_t *buffer, float *value) { if (NULL == buffer) { return BUFFER_WRONG_PARAMETER; } @@ -82,7 +89,7 @@ int Buffer_bottom(BUFFER *buffer, float *value) { return BUFFER_NO_ERROR; } -int Buffer_pop(BUFFER *buffer, float *value) { +int BufferPop(Buffer_t *buffer, float *value) { if (NULL == buffer) { return BUFFER_WRONG_PARAMETER; } @@ -106,7 +113,7 @@ int Buffer_pop(BUFFER *buffer, float *value) { return BUFFER_NO_ERROR; } -int buffer_push(BUFFER *buffer, float value) { +int BufferPush(Buffer_t *buffer, float value) { if (NULL == buffer) { return BUFFER_WRONG_PARAMETER; /*BUFFER_WRONG_PARAMETER值为1*/ } @@ -123,7 +130,7 @@ int buffer_push(BUFFER *buffer, float value) { } } -int Buffer_get(BUFFER *buffer, float *value, int index) { +int BufferGet(Buffer_t *buffer, float *value, int index) { if (NULL == buffer) { return BUFFER_WRONG_PARAMETER; } @@ -137,7 +144,7 @@ int Buffer_get(BUFFER *buffer, float *value, int index) { return BUFFER_NO_ERROR; } -int Buffer_mean(BUFFER *buffer, float *mean) { +int BufferMean(Buffer_t *buffer, float *mean) { int index; int count; @@ -154,11 +161,10 @@ int Buffer_mean(BUFFER *buffer, float *mean) { if (0 != count) { *mean /= count; } - return BUFFER_NO_ERROR; } -int Buffer_var(BUFFER *buffer, float *var) { +int BufferVar(Buffer_t *buffer, float *var) { int index; int count; float mean; @@ -188,11 +194,9 @@ int Buffer_var(BUFFER *buffer, float *var) { return BUFFER_NO_ERROR; } -int Buffer_std(BUFFER *buffer, float *std) { +int BufferStd(Buffer_t *buffer, float *std) { int ret; - - ret = Buffer_var(buffer, std); + ret = BufferVar(buffer, std); *std = (float)sqrt(*std); - return ret; } \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/pdr_filter.c b/1.Software/PDR 1.04/src/pdr_filter.c deleted file mode 100644 index 947e586..0000000 --- a/1.Software/PDR 1.04/src/pdr_filter.c +++ /dev/null @@ -1,180 +0,0 @@ -/***************************************************************** -*Module Name: filter -*Module Date: 2018.10.24 -*Module Auth: chelimin11070377 -*****************************************************************/ - -/* Header File Including ------------------------------------------------------------------------ */ -#include "stdio.h" -#include "Filter.h" - -/* Function Definition -------------------------------------------------------------------------- */ - -/***************************************************************** -*Description: reset filter -*Input : *f filter -*Output: none -*Return: none -*****************************************************************/ -void FILTER_Reset(FILTER *f) { - f->reset = 1; -} - -/***************************************************************** -*Description: reset fir filter -*Input : *f fir filter -*Output: none -*Return: none -*****************************************************************/ -void FILTER_FIR_Reset(FILTER_FIR *f) { - f->reset = 1; -} - -/***************************************************************** -*Description: set filter coefficient -*Input : order coefficient array length, not filter order -* *b b coefficient -* *a a coefficient -* H0 dc signal gain -*Output: *f filter -*Return: none -*****************************************************************/ -void FILTER_SetCoef(FILTER *f, int order, double *b, double *a, double H0) { - int i; - - for (i = 0; i < order; ++i) { - f->b[i] = b[i]; - f->a[i] = a[i]; - } - f->order = order; - f->H0 = H0; - f->reset = 1; -} - -/***************************************************************** -*Description: set fir filter coefficient -*Input : order coefficient array length, not filter order -* *b b coefficient -* *a a coefficient -* H0 dc signal gain -*Output: *f fir filter -*Return: none -*****************************************************************/ -void FILTER_FIR_setCoef(FILTER_FIR *f, int order, double *b, double H0) { - int i; - - for (i = 0; i < order; ++i) { - f->b[i] = b[i]; - } - f->order = order; - f->H0 = H0; - f->reset = 1; -} - -/***************************************************************** -*Description: filter x signal -*Input : *f filter -* x signal -*Output: none -*Return: filter result -*****************************************************************/ -double FILTER_filter(FILTER *f, double x) { - int i; - double y; - - if (f->order < 1) { - printf("fileter order is set to incorrect order, pls chk \ - your code!\n"); - return 0; - } - - if (f->reset) { - for (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 (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 (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; -} - -/***************************************************************** -*Description: filter x signal -*Input : *f fir filter -* x signal -*Output: none -*Return: filter result -*****************************************************************/ -double FILTER_FIR_filter(FILTER_FIR *f, double x) { - int i; - double y; - - if (f->order < 1) { - printf("fileter order is set to incorrect order, pls chk \ - your code!\n"); - return 0; - } - - if (f->reset) { - for (i = 0; i < f->order; ++i) { - f->xin[i] = x; - } - f->reset = 0; - } - // push x - for (i = 0; i < f->order - 1; ++i) { - f->xin[i] = f->xin[i + 1]; - } - f->xin[f->order - 1] = x; - // filter - y = f->b[0] * f->xin[f->order - 1]; - for (i = 1; i < f->order; ++i) { - y += f->b[i] * f->xin[f->order - 1 - i]; - } - - return y / f->H0; -} - -void DELAY_reset(DELAYER *d) { - d->reset = 1; -} - -void DELAY_setCoef(DELAYER *d, int delay) { - d->delay = delay; - d->reset = 1; -} - -double DELAY_delay(DELAYER *d, double x) { - double yout = d->xin[0]; - int i; - - if (d->reset) { - for (i = 0; i < d->delay; ++i) { - d->xin[i] = x; - } - yout = x; - d->reset = 0; - } - else { - for (i = 0; i < d->delay - 1; ++i) { - d->xin[i] = d->xin[i + 1]; - } - d->xin[d->delay - 1] = x; - } - - return yout; -} \ No newline at end of file