logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

重构部分函数命名和结构体，运行结果不变

PDRHistoryBranch
詹力 2022-10-15 16:07:19 +08:00
parent 050fb4b3d1
commit 868421702a
33 changed files with 683 additions and 940 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
1.Software/PDR 1.04/include/pdr_fft.h → 1.Software/PDR 1.04/include/Fft.h
View File

@ -4,22 +4,15 @@
/* Header File Including ------------------------------------------------------------------------ */
#include <stdint.h>
#include <math.h>
#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

79
1.Software/PDR 1.04/include/Filter.h
View File

@ -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

8
1.Software/PDR 1.04/include/Kalman.h
View File

@ -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
}

0
1.Software/PDR 1.04/include/pdr_linearFit.h → 1.Software/PDR 1.04/include/LinearFit.h
View File

12
1.Software/PDR 1.04/include/Location.h
View File

@ -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

2
1.Software/PDR 1.04/include/pdr_main.h → 1.Software/PDR 1.04/include/Main.h
View File

@ -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

16
1.Software/PDR 1.04/include/ParseData.h
View File

@ -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

25
1.Software/PDR 1.04/include/buffer.h
View File

@ -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
}

26
1.Software/PDR 1.04/include/pdr_api.h
View File

@ -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

39
1.Software/PDR 1.04/include/pdr_base.h
View File

@ -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

13
1.Software/PDR 1.04/include/pdr_detector.h
View File

@ -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

59
1.Software/PDR 1.04/include/pdr_sensor.h
View File

@ -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
}

14
1.Software/PDR 1.04/include/scene_recognition.h
View File

@ -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
}

20
1.Software/PDR 1.04/project/PDR.vcxproj
View File

@ -19,24 +19,25 @@
</ProjectConfiguration>
</ItemGroup>
<ItemGroup>
<ClCompile Include="..\src\buffer.c" />
<ClCompile Include="..\src\Buffer.c" />
<ClCompile Include="..\src\Complex.cpp" />
<ClCompile Include="..\src\CoordTrans.cpp" />
<ClCompile Include="..\src\m_munkres.c" />
<ClCompile Include="..\src\Munkres.c" />
<ClCompile Include="..\src\AHRS.c" />
<ClCompile Include="..\src\Interface.c" />
<ClCompile Include="..\src\PDRBase.c" />
<ClCompile Include="..\src\Detector.c" />
<ClCompile Include="..\src\pdr_fft.c" />
<ClCompile Include="..\src\pdr_filter.c" />
<ClCompile Include="..\src\Fft.c" />
<ClCompile Include="..\src\Filter.c" />
<ClCompile Include="..\src\Kalman.c" />
<ClCompile Include="..\src\pdr_linearFit.c" />
<ClCompile Include="..\src\LinearFit.c" />
<ClCompile Include="..\src\Location.c" />
<ClCompile Include="..\src\Main.cpp" />
<ClCompile Include="..\src\Matrix.c" />
<ClCompile Include="..\src\ParseData.c" />
<ClCompile Include="..\src\Utils.c" />
<ClCompile Include="..\src\Quaternion.cpp" />
<ClCompile Include="..\src\scene_recognition.c" />
<ClCompile Include="..\src\SceneRecognition.c" />
<ClCompile Include="..\src\Pedometer.c" />
</ItemGroup>
<ItemGroup>
@ -50,17 +51,18 @@
<ClInclude Include="..\include\pdr_api.h" />
<ClInclude Include="..\include\pdr_base.h" />
<ClInclude Include="..\include\pdr_detector.h" />
<ClInclude Include="..\include\pdr_fft.h" />
<ClInclude Include="..\include\Fft.h" />
<ClInclude Include="..\include\Kalman.h" />
<ClInclude Include="..\include\pdr_linearFit.h" />
<ClInclude Include="..\include\LinearFit.h" />
<ClInclude Include="..\include\Location.h" />
<ClInclude Include="..\include\pdr_main.h" />
<ClInclude Include="..\include\Main.h" />
<ClInclude Include="..\include\Matrix.h" />
<ClInclude Include="..\include\pdr_sensor.h" />
<ClInclude Include="..\include\pdr_trackSmooth.h" />
<ClInclude Include="..\include\Utils.h" />
<ClInclude Include="..\include\scene_recognition.h" />
<ClInclude Include="..\include\Pedometer.h" />
<ClInclude Include="..\src\Complex.h" />
<ClInclude Include="..\src\CoordTrans.h" />
<ClInclude Include="..\src\Quaternion.h" />
</ItemGroup>

34
1.Software/PDR 1.04/project/PDR.vcxproj.filters
View File

@ -16,15 +16,15 @@
<Filter Include="src\Location">
<UniqueIdentifier>{74391251-b6de-45ec-88b1-0ee0171e44da}</UniqueIdentifier>
</Filter>
<Filter Include="src\INS">
<Filter Include="src\Ins">
<UniqueIdentifier>{7f1fc113-1a0a-488e-9f77-2b12c7280aed}</UniqueIdentifier>
</Filter>
</ItemGroup>
<ItemGroup>
<ClCompile Include="..\src\buffer.c">
<ClCompile Include="..\src\Buffer.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\m_munkres.c">
<ClCompile Include="..\src\Munkres.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Interface.c">
@ -36,16 +36,16 @@
<ClCompile Include="..\src\Detector.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\pdr_fft.c">
<ClCompile Include="..\src\Fft.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\pdr_filter.c">
<ClCompile Include="..\src\Filter.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Main.cpp">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\scene_recognition.c">
<ClCompile Include="..\src\SceneRecognition.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Utils.c">
@ -57,7 +57,7 @@
<ClCompile Include="..\src\Kalman.c">
<Filter>src\Location</Filter>
</ClCompile>
<ClCompile Include="..\src\pdr_linearFit.c">
<ClCompile Include="..\src\LinearFit.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Quaternion.cpp">
@ -73,10 +73,13 @@
<Filter>src\Utils</Filter>
</ClCompile>
<ClCompile Include="..\src\AHRS.c">
<Filter>src\INS</Filter>
<Filter>src\Ins</Filter>
</ClCompile>
<ClCompile Include="..\src\Pedometer.c">
<Filter>src\INS</Filter>
<Filter>src\Ins</Filter>
</ClCompile>
<ClCompile Include="..\src\Complex.cpp">
<Filter>src\Math</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
@ -104,13 +107,13 @@
<ClInclude Include="..\include\pdr_detector.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_fft.h">
<ClInclude Include="..\include\Fft.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_linearFit.h">
<ClInclude Include="..\include\LinearFit.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_main.h">
<ClInclude Include="..\include\Main.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_sensor.h">
@ -144,10 +147,13 @@
<Filter>src\Utils</Filter>
</ClInclude>
<ClInclude Include="..\include\AHRS.h">
<Filter>src\INS</Filter>
<Filter>src\Ins</Filter>
</ClInclude>
<ClInclude Include="..\include\Pedometer.h">
<Filter>src\INS</Filter>
<Filter>src\Ins</Filter>
</ClInclude>
<ClInclude Include="..\src\Complex.h">
<Filter>src\Math</Filter>
</ClInclude>
</ItemGroup>
</Project>

44
1.Software/PDR 1.04/src/AHRS.c
View File

@ -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);

74
1.Software/PDR 1.04/src/Complex.cpp Normal file
View File

@ -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;
}

24
1.Software/PDR 1.04/src/Complex.h Normal file
View File

@ -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

300
1.Software/PDR 1.04/src/Detector.c
View File

@ -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 *)&amplitude_buffer[i], 0);
BufferPush((Buffer_t *)&frequency_buffer[i], 0);
BufferPush((Buffer_t *)&amplitude_buffer[i], 0);
}
else {
buffer_push((BUFFER *)&frequency_buffer[i], frequency[assignment[i]]);
buffer_push((BUFFER *)&amplitude_buffer[i], amplitude[assignment[i]]);
BufferPush((Buffer_t *)&frequency_buffer[i], frequency[assignment[i]]);
BufferPush((Buffer_t *)&amplitude_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 ||

93
1.Software/PDR 1.04/src/pdr_fft.c → 1.Software/PDR 1.04/src/Fft.c
View File

@ -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);
}
}
}

77
1.Software/PDR 1.04/src/Filter.c Normal file
View File

@ -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;
}

43
1.Software/PDR 1.04/src/Interface.c
View File

@ -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;
}

108
1.Software/PDR 1.04/src/Kalman.c
View File

@ -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 <stdio.h>
#include <string.h>
@ -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;
}

2
1.Software/PDR 1.04/src/pdr_linearFit.c → 1.Software/PDR 1.04/src/LinearFit.c
View File

@ -9,7 +9,7 @@
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <stdlib.h>
#include "pdr_linearFit.h"
#include "LinearFit.h"
#include "Utils.h"
/**----------------------------------------------------------------------

28
1.Software/PDR 1.04/src/Location.c
View File

@ -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回调更新，暂时不清楚函数功能

12
1.Software/PDR 1.04/src/Main.cpp
View File

@ -60,7 +60,7 @@
#include <errno.h>
#include <stdlib.h>
#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));

16
1.Software/PDR 1.04/src/m_munkres.c → 1.Software/PDR 1.04/src/Munkres.c
View File

@ -1,9 +1,9 @@
/* Header File Including ------------------------------------------------------------------------ */#include <stdint.h>
/* Header File Including ------------------------------------------------------------------------ */
#include <stdint.h>
#include <string.h>
#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;
}

124
1.Software/PDR 1.04/src/PDRBase.c
View File

@ -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;

62
1.Software/PDR 1.04/src/ParseData.c
View File

@ -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;

10
1.Software/PDR 1.04/src/Quaternion.cpp
View File

@ -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];
}

8
1.Software/PDR 1.04/src/scene_recognition.c → 1.Software/PDR 1.04/src/SceneRecognition.c
View File

@ -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. */

58
1.Software/PDR 1.04/src/buffer.c
View File

@ -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 <string.h>
#include <math.h>
@ -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;
}

180
1.Software/PDR 1.04/src/pdr_filter.c
View File

@ -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;
}