重构部分函数命名和结构体，运行结果不变
parent
050fb4b3d1
commit
868421702a
|
|
@ -4,22 +4,15 @@
|
||||||
/* Header File Including ------------------------------------------------------------------------ */
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
#include "Complex.h"
|
||||||
|
|
||||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||||
#define M_PI 3.14159265358979323846 /* pi */
|
#define M_PI 3.14159265358979323846 /* pi */
|
||||||
|
|
||||||
// #define MATLAB_DEBUG
|
|
||||||
// #define MATLAB_MAIN
|
|
||||||
|
|
||||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||||
typedef struct myComplex {
|
|
||||||
float real;
|
|
||||||
float image;
|
|
||||||
} myComplex;
|
|
||||||
|
|
||||||
/* Function Declaration ------------------------------------------------------------------------- */
|
/* Function Declaration ------------------------------------------------------------------------- */
|
||||||
extern int FFT_dft(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);
|
||||||
extern int FFT_fft(myComplex *fft, float *signal, int length);
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
@ -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__
|
#ifndef __FILTER_H__
|
||||||
#define __FILTER_H__
|
#define __FILTER_H__
|
||||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
/* Macro Declaration ----------------------------------------------------------*/
|
||||||
#define FILTER_COEF_MAX_LENGTH 71
|
#define FILTER_MAX_LENGTH 71
|
||||||
#define FILTER_FIR_COEF_MAX_LENGTH 71
|
|
||||||
|
|
||||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
/* Structure Declaration ------------------------------------------------------*/
|
||||||
typedef struct FILTER { // iir or fir filter
|
typedef struct Filter_t { // iir or fir filter
|
||||||
int reset; // reset flag
|
int reset; // reset flag
|
||||||
int order; // coefficient array length, not filter order
|
int order; // coefficient array length, not filter order
|
||||||
double H0; // dc signal gain
|
double H0; // dc signal gain
|
||||||
double b[FILTER_COEF_MAX_LENGTH]; // b coefficient
|
double b[FILTER_MAX_LENGTH]; // b coefficient
|
||||||
double a[FILTER_COEF_MAX_LENGTH]; // a coefficient
|
double a[FILTER_MAX_LENGTH]; // a coefficient
|
||||||
double yout[FILTER_COEF_MAX_LENGTH]; // yout buffer
|
double yout[FILTER_MAX_LENGTH]; // yout buffer
|
||||||
double xin[FILTER_COEF_MAX_LENGTH]; // xin buffer
|
double xin[FILTER_MAX_LENGTH]; // xin buffer
|
||||||
} FILTER;
|
} Filter_t;
|
||||||
|
|
||||||
typedef struct FILTER_FIR { // fir filter
|
/* Function Declaration -------------------------------------------------------*/
|
||||||
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;
|
|
||||||
|
|
||||||
typedef struct DELAYER { // delayer
|
/**---------------------------------------------------------------------
|
||||||
int reset;
|
* Function : FilterReset
|
||||||
int delay; // delay point number
|
* Description : 重置滤波器
|
||||||
double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer
|
* Date : 2022/10/15 logzhan
|
||||||
} DELAYER;
|
*---------------------------------------------------------------------**/
|
||||||
|
void FilterReset(Filter_t *f);
|
||||||
|
|
||||||
/* Function Declaration ------------------------------------------------------------------------- */
|
/**---------------------------------------------------------------------
|
||||||
extern void FILTER_Reset(FILTER *f);
|
* Function : FilterSetCoef
|
||||||
extern void FILTER_FIR_Reset(FILTER_FIR *f);
|
* Description : 滤波器设置参数
|
||||||
extern void FILTER_SetCoef(FILTER *f, int order, double *b, double *a, double H0);
|
* Input : order coefficient array length, not filter order
|
||||||
extern void FILTER_FIR_setCoef(FILTER_FIR *f, int order, double *b, double H0);
|
* *b b coefficient
|
||||||
extern double FILTER_filter(FILTER *f, double x);
|
* *a a coefficient
|
||||||
extern double FILTER_FIR_filter(FILTER_FIR *f, double x);
|
* H0 dc signal gain
|
||||||
extern void DELAY_reset(DELAYER *d);
|
* Date : 2022/10/15 logzhan
|
||||||
extern void DELAY_setCoef(DELAYER *d, int delay);
|
*---------------------------------------------------------------------**/
|
||||||
extern double DELAY_delay(DELAYER *d, double x);
|
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
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -37,14 +37,14 @@ void EKFStatePredict(EKFPara_t* kf, double step_length, PDR_t* g_pdr, int step);
|
||||||
* kf，EKF数据结构体
|
* kf，EKF数据结构体
|
||||||
* Date : 2022/09/19 logzhan
|
* 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
|
* Function : pdr_ekfStateUpdate
|
||||||
* Description : pdr卡尔曼滤波器的状态更新方程
|
* Description : pdr卡尔曼滤波器的状态更新方程
|
||||||
* Date : 2020/7/22 logzhan
|
* 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);
|
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融合定位
|
* Description : PDR的GNSS和INS融合定位
|
||||||
* Date : 2020/07/09 logzhan
|
* Date : 2020/07/09 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, classifer* sys, double yaw_bias,
|
void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias,
|
||||||
EKFPara_t* kf, lct_fs* res);
|
EKFPara_t* kf, LctFs_t* res);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -32,14 +32,14 @@ int InsLocation(IMU_t *ss_data, EKFPara_t *kf);
|
||||||
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
|
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
|
||||||
* Date : 2020/07/08 logzhan
|
* 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
|
* Function : Nmea2Gnss
|
||||||
* Description : nmea数据结构转gnss数据
|
* Description : nmea数据结构转gnss数据
|
||||||
* Date : 2020/07/08 logzhan
|
* 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，提高
|
* 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
|
* Function : GnssCalHeading
|
||||||
|
|
@ -65,7 +65,7 @@ double GnssCalHeading(GNSS_t* pgnss);
|
||||||
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
||||||
* Date : 2020/07/08 logzhan
|
* 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
|
* Function : pdr_resetSysStatus
|
||||||
|
|
@ -79,7 +79,7 @@ void ResetSystemStatus(EKFPara_t* kf);
|
||||||
* Description : PDR GPS融合INS惯性定位
|
* Description : PDR GPS融合INS惯性定位
|
||||||
* Date : 2021/01/29 logzhan
|
* 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
|
* Function : pdr_initGnssInfo
|
||||||
|
|
@ -88,7 +88,7 @@ int GnssInsFusionLocation(lct_nmea* nmea_data, EKFPara_t* kf, lct_fs* result);
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void InitGnssInfo(void);
|
void InitGnssInfo(void);
|
||||||
|
|
||||||
void GnssUpdate(GNSS_t* gps, lct_nmea* nmea);
|
void GnssUpdate(GNSS_t* gps, Nmea_t* nmea);
|
||||||
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
|
|
|
||||||
|
|
@ -76,7 +76,7 @@ const char* Motion2TypeStr(int type);
|
||||||
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
||||||
* Date : 2021/01/25 logzhan
|
* Date : 2021/01/25 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs);
|
void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
@ -34,7 +34,7 @@ extern "C" {
|
||||||
ts， 手机开机时间戳，ms
|
ts， 手机开机时间戳，ms
|
||||||
* Output : ln， NMEA数据结构体
|
* Output : ln， NMEA数据结构体
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
void parseNMEA(char* pt, lct_nmea *ln, double ts);
|
void parseNMEA(char* pt, Nmea_t *ln, double ts);
|
||||||
|
|
||||||
/**************************************************************************
|
/**************************************************************************
|
||||||
* Description : 解析GGA协议
|
* Description : 解析GGA协议
|
||||||
|
|
@ -42,7 +42,7 @@ extern "C" {
|
||||||
ts， 手机开机时间戳，ms
|
ts， 手机开机时间戳，ms
|
||||||
* Output : ln， NMEA数据结构体
|
* Output : ln， NMEA数据结构体
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
void ParseGGA(char* pt, lct_nmea *ln, double ts);
|
void ParseGGA(char* pt, Nmea_t *ln, double ts);
|
||||||
|
|
||||||
/**************************************************************************
|
/**************************************************************************
|
||||||
* Description : 解析RMC协议
|
* Description : 解析RMC协议
|
||||||
|
|
@ -50,7 +50,7 @@ extern "C" {
|
||||||
ts， 手机开机时间戳，ms
|
ts， 手机开机时间戳，ms
|
||||||
* Output : ln， NMEA数据结构体
|
* Output : ln， NMEA数据结构体
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
void ParseRMC(char* pt, lct_nmea *ln, double ts);
|
void ParseRMC(char* pt, Nmea_t *ln, double ts);
|
||||||
|
|
||||||
/**************************************************************************
|
/**************************************************************************
|
||||||
* Description : 解析GSV协议
|
* Description : 解析GSV协议
|
||||||
|
|
@ -58,7 +58,7 @@ extern "C" {
|
||||||
ts， 手机开机时间戳，ms
|
ts， 手机开机时间戳，ms
|
||||||
* Output : ln， NMEA数据结构体
|
* Output : ln， NMEA数据结构体
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
void parseGSV(char* pt, lct_nmea *ln, double ts);
|
void parseGSV(char* pt, Nmea_t *ln, double ts);
|
||||||
|
|
||||||
/**************************************************************************
|
/**************************************************************************
|
||||||
* Description : 解析GSA协议
|
* Description : 解析GSA协议
|
||||||
|
|
@ -66,22 +66,22 @@ extern "C" {
|
||||||
ts， 手机开机时间戳，ms
|
ts， 手机开机时间戳，ms
|
||||||
* Output : ln， NMEA数据结构体
|
* Output : ln， NMEA数据结构体
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
void ParseGSA(char* pt, lct_nmea *ln, double ts);
|
void ParseGSA(char* pt, Nmea_t *ln, double ts);
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : parseLocAccuracy
|
* Function : parseLocAccuracy
|
||||||
* Description : 解析GPS的Accuracy精度参数
|
* Description : 解析GPS的Accuracy精度参数
|
||||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
* 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数据是否解析完整
|
* Description : 检测NMEA数据是否解析完整
|
||||||
* Input : ln， 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
|
* Function : HexToDec
|
||||||
|
|
|
||||||
|
|
@ -18,6 +18,7 @@ extern "C" {
|
||||||
|
|
||||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||||
#pragma pack (4)
|
#pragma pack (4)
|
||||||
|
|
||||||
typedef struct BUFFER {
|
typedef struct BUFFER {
|
||||||
char name[20];
|
char name[20];
|
||||||
int type;
|
int type;
|
||||||
|
|
@ -28,7 +29,7 @@ typedef struct BUFFER {
|
||||||
double sum;
|
double sum;
|
||||||
double mean;
|
double mean;
|
||||||
float data[BUFFER_LONG_LENGTH + 1];
|
float data[BUFFER_LONG_LENGTH + 1];
|
||||||
} BUFFER;
|
}Buffer_t;
|
||||||
|
|
||||||
typedef struct BUFFER_LONG {
|
typedef struct BUFFER_LONG {
|
||||||
char name[20];
|
char name[20];
|
||||||
|
|
@ -58,17 +59,17 @@ typedef struct BUFFER_SHORT {
|
||||||
/* Global Variable Declaration ------------------------------------------------------------------ */
|
/* Global Variable Declaration ------------------------------------------------------------------ */
|
||||||
|
|
||||||
/* Function Declaration ------------------------------------------------------------------------- */
|
/* Function Declaration ------------------------------------------------------------------------- */
|
||||||
int Buffer_initialize(BUFFER *buffer, const char *name, int type, int length);
|
int BufferInit(Buffer_t *buffer, const char *name, int type, int length);
|
||||||
int Buffer_clear(BUFFER *buffer);
|
int BufferClear(Buffer_t *buffer);
|
||||||
int Buffer_count(BUFFER *buffer, int *count);
|
int BufferCount(Buffer_t *buffer, int *count);
|
||||||
int Buffer_top(BUFFER *buffer, float *value);
|
int BufferGetTop(Buffer_t *buffer, float *value);
|
||||||
int Buffer_bottom(BUFFER *buffer, float *value);
|
int BufferGetBottom(Buffer_t *buffer, float *value);
|
||||||
int Buffer_pop(BUFFER *buffer, float *value);
|
int BufferPop(Buffer_t *buffer, float *value);
|
||||||
int buffer_push(BUFFER *buffer, float value);
|
int BufferPush(Buffer_t *buffer, float value);
|
||||||
int Buffer_get(BUFFER *buffer, float *value, int index);
|
int BufferGet(Buffer_t *buffer, float *value, int index);
|
||||||
int Buffer_mean(BUFFER *buffer, float *mean);
|
int BufferMean(Buffer_t *buffer, float *mean);
|
||||||
int Buffer_var(BUFFER *buffer, float *var);
|
int BufferVar(Buffer_t *buffer, float *var);
|
||||||
int Buffer_std(BUFFER *buffer, float *std);
|
int BufferStd(Buffer_t *buffer, float *std);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -36,15 +36,15 @@ typedef struct _PosFusion{
|
||||||
/* Function Declaration ------------------------------------------------------*/
|
/* Function Declaration ------------------------------------------------------*/
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_algorithmInit
|
* Function : Algorithm_Init
|
||||||
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
||||||
* 迹平滑窗口的初始化
|
* 迹平滑窗口的初始化
|
||||||
* Date : 2020/7/3 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void Algorithm_Init(void);
|
void Algorithm_Init(void);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_locationMainLoop
|
* Function : LocationMainLoop
|
||||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||||
* 迹平滑窗口的初始化
|
* 迹平滑窗口的初始化
|
||||||
* Input : ss_data， 传感器数据
|
* Input : ss_data， 传感器数据
|
||||||
|
|
@ -53,7 +53,7 @@ void Algorithm_Init(void);
|
||||||
* Output : int, 输出标志位
|
* Output : int, 输出标志位
|
||||||
* Date : 2020/7/3 logzhan
|
* 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);
|
FILE *fp_gps);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
|
|
@ -63,7 +63,7 @@ int LocationMainLoop(IMU_t* ImuData, lct_nmea* NmeaData, lct_fs* LocFusion,
|
||||||
* Date : 2020/07/03 logzhan
|
* Date : 2020/07/03 logzhan
|
||||||
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
int ParseLineAndUpdate(char* line, lct_fs* result);
|
int ParseLineAndUpdate(char* line, LctFs_t* result);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_initRmc
|
* Function : pdr_initRmc
|
||||||
|
|
@ -71,7 +71,7 @@ int ParseLineAndUpdate(char* line, lct_fs* result);
|
||||||
* Input : rmc， RMC结构体
|
* Input : rmc， RMC结构体
|
||||||
* Date : 2020/7/3 logzhan
|
* Date : 2020/7/3 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void RMC_Init(lct_nmea_rmc * rmc);
|
void RMC_Init(NmeaRMC_t * rmc);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_initNmeaFlg
|
* Function : pdr_initNmeaFlg
|
||||||
|
|
@ -79,28 +79,28 @@ void RMC_Init(lct_nmea_rmc * rmc);
|
||||||
* Input : ln， NMEA结构体
|
* Input : ln， NMEA结构体
|
||||||
* Date : 2020/7/3 logzhan
|
* Date : 2020/7/3 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void NmeaFlag_Init(lct_nmea * ln);
|
void NmeaFlag_Init(Nmea_t * ln);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : clearNmeaFlg
|
* Function : clearNmeaFlg
|
||||||
* Description : 清空Nmea标志位,功能和init相同
|
* Description : 清空Nmea标志位,功能和init相同
|
||||||
* Date : 2020/7/4 logzhan
|
* Date : 2020/7/4 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void ClearNmeaFlg(lct_nmea * ln);
|
void ClearNmeaFlg(Nmea_t * ln);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_saveGnssInfo
|
* Function : pdr_saveGnssInfo
|
||||||
* Description : 保存GPS相关信息
|
* Description : 保存GPS相关信息
|
||||||
* Date : 2020/7/3 logzhan
|
* 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
|
* Function : GetPDRVersion
|
||||||
* Description : pdr¿â°æ±¾ºÅ
|
* Description : »ñÈ¡pdr°æ±¾ºÅ
|
||||||
* Date : 2020/8/3 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
const char* GetLIBVersion(void);
|
const char* GetPDRVersion(void);
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : pdr_closeAlgorithm
|
* Function : pdr_closeAlgorithm
|
||||||
|
|
|
||||||
|
|
@ -167,18 +167,17 @@ typedef struct PDR {
|
||||||
typedef struct {
|
typedef struct {
|
||||||
int type;
|
int type;
|
||||||
float accBuffer[PATTERN_RECOGNITION_LEN];
|
float accBuffer[PATTERN_RECOGNITION_LEN];
|
||||||
}classifer;
|
}Classifer_t;
|
||||||
|
|
||||||
// 用户运动类型分类器
|
// 用户运动类型分类器
|
||||||
typedef struct DETECTOR {
|
typedef struct DETECTOR {
|
||||||
uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
|
uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
|
||||||
uint32_t lastType;
|
uint32_t lastType;
|
||||||
uint64_t tick; // 次数统计，用于调整检测器工作频率
|
uint64_t tick; // 次数统计，用于调整检测器工作频率
|
||||||
} DETECTOR_t;
|
}Detector_t;
|
||||||
|
|
||||||
|
|
||||||
DETECTOR_t *pdr_getDetector(void);
|
Detector_t *GetDetectorObj(void);
|
||||||
|
|
||||||
|
|
||||||
/* Function Declaration ------------------------------------------------------*/
|
/* Function Declaration ------------------------------------------------------*/
|
||||||
|
|
||||||
|
|
@ -200,16 +199,15 @@ void pdr_resetData(void);
|
||||||
void ComputePCA(AHRS_t* ahrs);
|
void ComputePCA(AHRS_t* ahrs);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : detectorUpdate
|
* Function : DetectorUpdate
|
||||||
* Description : 根据新确定的手机携带方式更新g_pdr.status等
|
* Description : 根据新确定的手机携带方式更新状态等
|
||||||
* Date : 2020/7/20 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void detectorUpdate(DETECTOR_t* detector);
|
void DetectorUpdate(Detector_t* detector);
|
||||||
|
|
||||||
|
|
||||||
void gpsUpdateCb(GNSS_t* gps);
|
void gpsUpdateCb(GNSS_t* gps);
|
||||||
|
|
||||||
|
|
||||||
/**************************************************************************
|
/**************************************************************************
|
||||||
* Description : 计算每步所耗时间
|
* Description : 计算每步所耗时间
|
||||||
* Input : stepPredict， 步数预测结构体
|
* Input : stepPredict， 步数预测结构体
|
||||||
|
|
@ -234,7 +232,7 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_
|
||||||
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
||||||
* Date : 2020/7/22 logzhan
|
* Date : 2020/7/22 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void StateRecognition(float *acc, classifer *sys);
|
void StateRecognition(float *acc, Classifer_t *sys);
|
||||||
|
|
||||||
/**************************************************************************
|
/**************************************************************************
|
||||||
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
|
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
|
||||||
|
|
@ -245,7 +243,7 @@ void StateRecognition(float *acc, classifer *sys);
|
||||||
ss_data， 传感器数据结构体
|
ss_data， 传感器数据结构体
|
||||||
* Output : double，用户行走方向
|
* 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角度，计算用户行走方向
|
* 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);
|
GNSS_t *pgnss, EKFPara_t *kf);
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : pdr_detStatic
|
* Function : DetUserStatic
|
||||||
* Description : 检测用户是否处于静止状态
|
* 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位置
|
* 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
|
* Function : resetOutputResult
|
||||||
|
|
@ -288,7 +285,7 @@ void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result);
|
||||||
* kf， EKF数据结构体
|
* kf， EKF数据结构体
|
||||||
* Date : 2020/07/08 logzhan
|
* 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
|
* Function : detIsCarMode
|
||||||
|
|
@ -308,7 +305,7 @@ int DetectCarMode(GNSS_t *pgnss, PDR_t *g_pdr, unsigned long delSteps, int *time
|
||||||
* 重置PDR到GPS的位置。
|
* 重置PDR到GPS的位置。
|
||||||
* Date : 2021/01/28 logzhan
|
* 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滤波器
|
* Description : 初始化EKF滤波器
|
||||||
|
|
@ -320,7 +317,7 @@ int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t
|
||||||
* Output : int，初始化标志位
|
* Output : int，初始化标志位
|
||||||
result，定位结果结构体
|
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
|
* Description : 设置EKF滤波器的Q和R
|
||||||
|
|
|
||||||
|
|
@ -23,7 +23,7 @@ extern "C" {
|
||||||
/* Struct Declaration --------------------------------------------------------------------------- */
|
/* Struct Declaration --------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
|
||||||
typedef void (*DETECTOR_update_callback)(DETECTOR_t *detector);
|
typedef void (*DETECTOR_update_callback)(Detector_t *detector);
|
||||||
|
|
||||||
/* Global Variable Declaration ------------------------------------------------------------------ */
|
/* Global Variable Declaration ------------------------------------------------------------------ */
|
||||||
extern BUFFER_SHORT g_acc_frq_buf[3];
|
extern BUFFER_SHORT g_acc_frq_buf[3];
|
||||||
|
|
@ -34,19 +34,18 @@ extern BUFFER_SHORT g_gyr_amp_buf[3];
|
||||||
/* Function Declaration ------------------------------------------------------------------------- */
|
/* Function Declaration ------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_getDetector
|
* Function : GetDetectorObj
|
||||||
* Description : 获取PDR运动分类器的结构体指针
|
* Description : 获取PDR运动分类器对象
|
||||||
* Date : 2020/02/16 logzhan &
|
* Date : 2020/02/16 logzhan
|
||||||
*
|
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
DETECTOR_t *pdr_getDetector(void);
|
Detector_t *GetDetectorObj(void);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : Detector_Init
|
* Function : Detector_Init
|
||||||
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||||
* Date : 2022/09/23 logzhan
|
* Date : 2022/09/23 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
DETECTOR_t* Detector_Init(void);
|
Detector_t* Detector_Init(void);
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : DetectorReset
|
* Function : DetectorReset
|
||||||
|
|
|
||||||
|
|
@ -154,7 +154,7 @@ typedef struct imu {
|
||||||
Sensor_t mag;
|
Sensor_t mag;
|
||||||
}IMU_t;
|
}IMU_t;
|
||||||
|
|
||||||
typedef struct lct_nmea_rmc {
|
typedef struct NmeaRMC_t {
|
||||||
unsigned char update;
|
unsigned char update;
|
||||||
char rmc_check_sum[9];
|
char rmc_check_sum[9];
|
||||||
sensor type;
|
sensor type;
|
||||||
|
|
@ -171,9 +171,9 @@ typedef struct lct_nmea_rmc {
|
||||||
double latitude;
|
double latitude;
|
||||||
double longitudinal;
|
double longitudinal;
|
||||||
double time; //ms
|
double time; //ms
|
||||||
}lct_nmea_rmc;
|
}NmeaRMC_t;
|
||||||
|
|
||||||
typedef struct lct_nmea_gga {
|
typedef struct NmeaGGA{
|
||||||
unsigned char update;
|
unsigned char update;
|
||||||
char gga_check_sum[9];
|
char gga_check_sum[9];
|
||||||
sensor type;
|
sensor type;
|
||||||
|
|
@ -191,9 +191,9 @@ typedef struct lct_nmea_gga {
|
||||||
double latitude;
|
double latitude;
|
||||||
double longitudinal;
|
double longitudinal;
|
||||||
double time; //ms
|
double time; //ms
|
||||||
}lct_nmea_gga;
|
}NmeaGGA_t;
|
||||||
|
|
||||||
typedef struct lct_nmea_gsa {
|
typedef struct NmeaGSA {
|
||||||
unsigned char update;
|
unsigned char update;
|
||||||
char check_sum[9];
|
char check_sum[9];
|
||||||
sensor type;
|
sensor type;
|
||||||
|
|
@ -205,8 +205,7 @@ typedef struct lct_nmea_gsa {
|
||||||
float hdop;
|
float hdop;
|
||||||
float vdop;
|
float vdop;
|
||||||
double time; //ms
|
double time; //ms
|
||||||
}
|
}NmeaGSA_t;
|
||||||
lct_nmea_gsa;
|
|
||||||
|
|
||||||
typedef struct satellite_status {
|
typedef struct satellite_status {
|
||||||
int prn;
|
int prn;
|
||||||
|
|
@ -215,7 +214,7 @@ typedef struct satellite_status {
|
||||||
int snr; //snr >25 good
|
int snr; //snr >25 good
|
||||||
}satellite_status;
|
}satellite_status;
|
||||||
|
|
||||||
typedef struct lct_nmea_gsv {
|
typedef struct NmeaGSV {
|
||||||
unsigned char update;
|
unsigned char update;
|
||||||
sensor type;
|
sensor type;
|
||||||
int sentences;
|
int sentences;
|
||||||
|
|
@ -224,25 +223,25 @@ typedef struct lct_nmea_gsv {
|
||||||
int satellite_number;
|
int satellite_number;
|
||||||
satellite_status satellites_data[20];
|
satellite_status satellites_data[20];
|
||||||
double time; //ms
|
double time; //ms
|
||||||
}lct_nmea_gsv;
|
}NmeaGSV_t;
|
||||||
|
|
||||||
typedef struct loc_accuracy{
|
typedef struct Accuracy{
|
||||||
unsigned char update;
|
unsigned char update;
|
||||||
int status; //-1,:Status Unknown; 0: Out of Service; 1: Temporarily Unavailable; 2: Available
|
int status; //-1,:Status Unknown; 0: Out of Service; 1: Temporarily Unavailable; 2: Available
|
||||||
float err; // m
|
float err; // m
|
||||||
double time;
|
double time;
|
||||||
}loc_accuracy;
|
}Accuracy_t;
|
||||||
|
|
||||||
typedef struct lct_nmea {
|
typedef struct Nmea_t {
|
||||||
unsigned char update;
|
unsigned char update;
|
||||||
double minTime;
|
double minTime;
|
||||||
double maxTime;
|
double maxTime;
|
||||||
lct_nmea_gga gga;
|
NmeaGGA_t gga;
|
||||||
lct_nmea_rmc rmc[2]; // 0 - GNRMC, 1 - GPRMC
|
NmeaRMC_t rmc[2]; // 0 - GNRMC, 1 - GPRMC
|
||||||
lct_nmea_gsa gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA
|
NmeaGSA_t gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA
|
||||||
lct_nmea_gsv gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV
|
NmeaGSV_t gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV
|
||||||
loc_accuracy accuracy;
|
Accuracy_t accuracy;
|
||||||
}lct_nmea;
|
}Nmea_t;
|
||||||
|
|
||||||
typedef struct lct_fs {
|
typedef struct lct_fs {
|
||||||
LL_NSEW latitude_ns;
|
LL_NSEW latitude_ns;
|
||||||
|
|
@ -263,14 +262,14 @@ typedef struct lct_fs {
|
||||||
double last_lon;
|
double last_lon;
|
||||||
unsigned long step;
|
unsigned long step;
|
||||||
uint8_t motionType;
|
uint8_t motionType;
|
||||||
}lct_fs;
|
}LctFs_t;
|
||||||
|
|
||||||
typedef struct gnss {
|
typedef struct gnss {
|
||||||
LL_NSEW lat_ns;
|
LL_NSEW lat_ns;
|
||||||
LL_NSEW lon_ew;
|
LL_NSEW lon_ew;
|
||||||
int minElevation;
|
int minElevation;
|
||||||
int satellites_num; // 当前卫星数量
|
int satellites_num; // 当前卫星数量
|
||||||
int quality;
|
int quality;
|
||||||
float yaw; // GNSS 航向角
|
float yaw; // GNSS 航向角
|
||||||
float vel; // GNSS 速度
|
float vel; // GNSS 速度
|
||||||
float HDOP; // GNSS 水平精度因子
|
float HDOP; // GNSS 水平精度因子
|
||||||
|
|
@ -295,17 +294,17 @@ typedef struct AHRS {
|
||||||
uint8_t status;
|
uint8_t status;
|
||||||
uint8_t stable; // 当前AHRS算法收敛稳定性
|
uint8_t stable; // 当前AHRS算法收敛稳定性
|
||||||
float error;
|
float error;
|
||||||
float qnb[4];
|
float qnb[4]; //
|
||||||
float gravity[3];
|
float gravity[3];
|
||||||
float x_axis[3];
|
float x_axis[3];
|
||||||
float y_axis[3];
|
float y_axis[3];
|
||||||
float z_axis[3];
|
float z_axis[3];
|
||||||
float kp; // mahony kp比例调节参数
|
float Kp; // mahony kp比例调节参数
|
||||||
float yaw;
|
float Yaw;
|
||||||
float pitch;
|
float Pitch;
|
||||||
float roll;
|
float Roll;
|
||||||
float insHeading;
|
float insHeading;
|
||||||
} AHRS_t;
|
}AHRS_t;
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -85,8 +85,8 @@ typedef enum _SIGNAL_QUALITY
|
||||||
BAD
|
BAD
|
||||||
} SIGNAL_QUALITY;
|
} SIGNAL_QUALITY;
|
||||||
|
|
||||||
/* Usage: extract and save information from lct_nmea for the module. */
|
/* Usage: extract and save information from Nmea_t for the module. */
|
||||||
/* This is the interface for the lct_nmea. */
|
/* This is the interface for the Nmea_t. */
|
||||||
typedef struct _GnssInfo
|
typedef struct _GnssInfo
|
||||||
{
|
{
|
||||||
int update; // GNSS更新标志位 1：更新 0：不更新
|
int update; // GNSS更新标志位 1：更新 0：不更新
|
||||||
|
|
@ -132,14 +132,8 @@ SCENE_INIT_STATE scene_recognition_reset(void);
|
||||||
* Input : PDR的nmea结构体
|
* Input : PDR的nmea结构体
|
||||||
* Return : 场景识别结果
|
* Return : 场景识别结果
|
||||||
* Date : 2020/02/18
|
* 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() */
|
/* Function Name: scene_recognition_destroy() */
|
||||||
/* Usage: 场景识别处理结束后的资源释放操作，资源释放后无法继续进行场 */
|
/* Usage: 场景识别处理结束后的资源释放操作，资源释放后无法继续进行场 */
|
||||||
|
|
@ -156,7 +150,7 @@ SCENE_DESTROY_STATE scene_recognition_destroy(void);
|
||||||
* Author : Zhang Jingrui, Geek ID: 11082826
|
* Author : Zhang Jingrui, Geek ID: 11082826
|
||||||
* Date : 2020/02/18 logzhan
|
* Date : 2020/02/18 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
int isOpenArea(const lct_nmea* nmea);
|
int isOpenArea(const Nmea_t* nmea);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -19,24 +19,25 @@
|
||||||
</ProjectConfiguration>
|
</ProjectConfiguration>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<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\CoordTrans.cpp" />
|
||||||
<ClCompile Include="..\src\m_munkres.c" />
|
<ClCompile Include="..\src\Munkres.c" />
|
||||||
<ClCompile Include="..\src\AHRS.c" />
|
<ClCompile Include="..\src\AHRS.c" />
|
||||||
<ClCompile Include="..\src\Interface.c" />
|
<ClCompile Include="..\src\Interface.c" />
|
||||||
<ClCompile Include="..\src\PDRBase.c" />
|
<ClCompile Include="..\src\PDRBase.c" />
|
||||||
<ClCompile Include="..\src\Detector.c" />
|
<ClCompile Include="..\src\Detector.c" />
|
||||||
<ClCompile Include="..\src\pdr_fft.c" />
|
<ClCompile Include="..\src\Fft.c" />
|
||||||
<ClCompile Include="..\src\pdr_filter.c" />
|
<ClCompile Include="..\src\Filter.c" />
|
||||||
<ClCompile Include="..\src\Kalman.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\Location.c" />
|
||||||
<ClCompile Include="..\src\Main.cpp" />
|
<ClCompile Include="..\src\Main.cpp" />
|
||||||
<ClCompile Include="..\src\Matrix.c" />
|
<ClCompile Include="..\src\Matrix.c" />
|
||||||
<ClCompile Include="..\src\ParseData.c" />
|
<ClCompile Include="..\src\ParseData.c" />
|
||||||
<ClCompile Include="..\src\Utils.c" />
|
<ClCompile Include="..\src\Utils.c" />
|
||||||
<ClCompile Include="..\src\Quaternion.cpp" />
|
<ClCompile Include="..\src\Quaternion.cpp" />
|
||||||
<ClCompile Include="..\src\scene_recognition.c" />
|
<ClCompile Include="..\src\SceneRecognition.c" />
|
||||||
<ClCompile Include="..\src\Pedometer.c" />
|
<ClCompile Include="..\src\Pedometer.c" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
|
|
@ -50,17 +51,18 @@
|
||||||
<ClInclude Include="..\include\pdr_api.h" />
|
<ClInclude Include="..\include\pdr_api.h" />
|
||||||
<ClInclude Include="..\include\pdr_base.h" />
|
<ClInclude Include="..\include\pdr_base.h" />
|
||||||
<ClInclude Include="..\include\pdr_detector.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\Kalman.h" />
|
||||||
<ClInclude Include="..\include\pdr_linearFit.h" />
|
<ClInclude Include="..\include\LinearFit.h" />
|
||||||
<ClInclude Include="..\include\Location.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\Matrix.h" />
|
||||||
<ClInclude Include="..\include\pdr_sensor.h" />
|
<ClInclude Include="..\include\pdr_sensor.h" />
|
||||||
<ClInclude Include="..\include\pdr_trackSmooth.h" />
|
<ClInclude Include="..\include\pdr_trackSmooth.h" />
|
||||||
<ClInclude Include="..\include\Utils.h" />
|
<ClInclude Include="..\include\Utils.h" />
|
||||||
<ClInclude Include="..\include\scene_recognition.h" />
|
<ClInclude Include="..\include\scene_recognition.h" />
|
||||||
<ClInclude Include="..\include\Pedometer.h" />
|
<ClInclude Include="..\include\Pedometer.h" />
|
||||||
|
<ClInclude Include="..\src\Complex.h" />
|
||||||
<ClInclude Include="..\src\CoordTrans.h" />
|
<ClInclude Include="..\src\CoordTrans.h" />
|
||||||
<ClInclude Include="..\src\Quaternion.h" />
|
<ClInclude Include="..\src\Quaternion.h" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
|
||||||
|
|
@ -16,15 +16,15 @@
|
||||||
<Filter Include="src\Location">
|
<Filter Include="src\Location">
|
||||||
<UniqueIdentifier>{74391251-b6de-45ec-88b1-0ee0171e44da}</UniqueIdentifier>
|
<UniqueIdentifier>{74391251-b6de-45ec-88b1-0ee0171e44da}</UniqueIdentifier>
|
||||||
</Filter>
|
</Filter>
|
||||||
<Filter Include="src\INS">
|
<Filter Include="src\Ins">
|
||||||
<UniqueIdentifier>{7f1fc113-1a0a-488e-9f77-2b12c7280aed}</UniqueIdentifier>
|
<UniqueIdentifier>{7f1fc113-1a0a-488e-9f77-2b12c7280aed}</UniqueIdentifier>
|
||||||
</Filter>
|
</Filter>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ClCompile Include="..\src\buffer.c">
|
<ClCompile Include="..\src\Buffer.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\m_munkres.c">
|
<ClCompile Include="..\src\Munkres.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\Interface.c">
|
<ClCompile Include="..\src\Interface.c">
|
||||||
|
|
@ -36,16 +36,16 @@
|
||||||
<ClCompile Include="..\src\Detector.c">
|
<ClCompile Include="..\src\Detector.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\pdr_fft.c">
|
<ClCompile Include="..\src\Fft.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\pdr_filter.c">
|
<ClCompile Include="..\src\Filter.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\Main.cpp">
|
<ClCompile Include="..\src\Main.cpp">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\scene_recognition.c">
|
<ClCompile Include="..\src\SceneRecognition.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\Utils.c">
|
<ClCompile Include="..\src\Utils.c">
|
||||||
|
|
@ -57,7 +57,7 @@
|
||||||
<ClCompile Include="..\src\Kalman.c">
|
<ClCompile Include="..\src\Kalman.c">
|
||||||
<Filter>src\Location</Filter>
|
<Filter>src\Location</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\pdr_linearFit.c">
|
<ClCompile Include="..\src\LinearFit.c">
|
||||||
<Filter>src</Filter>
|
<Filter>src</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\Quaternion.cpp">
|
<ClCompile Include="..\src\Quaternion.cpp">
|
||||||
|
|
@ -73,10 +73,13 @@
|
||||||
<Filter>src\Utils</Filter>
|
<Filter>src\Utils</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\AHRS.c">
|
<ClCompile Include="..\src\AHRS.c">
|
||||||
<Filter>src\INS</Filter>
|
<Filter>src\Ins</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
<ClCompile Include="..\src\Pedometer.c">
|
<ClCompile Include="..\src\Pedometer.c">
|
||||||
<Filter>src\INS</Filter>
|
<Filter>src\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\src\Complex.cpp">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
|
|
@ -104,13 +107,13 @@
|
||||||
<ClInclude Include="..\include\pdr_detector.h">
|
<ClInclude Include="..\include\pdr_detector.h">
|
||||||
<Filter>inc</Filter>
|
<Filter>inc</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
<ClInclude Include="..\include\pdr_fft.h">
|
<ClInclude Include="..\include\Fft.h">
|
||||||
<Filter>inc</Filter>
|
<Filter>inc</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
<ClInclude Include="..\include\pdr_linearFit.h">
|
<ClInclude Include="..\include\LinearFit.h">
|
||||||
<Filter>inc</Filter>
|
<Filter>inc</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
<ClInclude Include="..\include\pdr_main.h">
|
<ClInclude Include="..\include\Main.h">
|
||||||
<Filter>inc</Filter>
|
<Filter>inc</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
<ClInclude Include="..\include\pdr_sensor.h">
|
<ClInclude Include="..\include\pdr_sensor.h">
|
||||||
|
|
@ -144,10 +147,13 @@
|
||||||
<Filter>src\Utils</Filter>
|
<Filter>src\Utils</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
<ClInclude Include="..\include\AHRS.h">
|
<ClInclude Include="..\include\AHRS.h">
|
||||||
<Filter>src\INS</Filter>
|
<Filter>src\Ins</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
<ClInclude Include="..\include\Pedometer.h">
|
<ClInclude Include="..\include\Pedometer.h">
|
||||||
<Filter>src\INS</Filter>
|
<Filter>src\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\src\Complex.h">
|
||||||
|
<Filter>src\Math</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
</Project>
|
</Project>
|
||||||
|
|
@ -106,8 +106,8 @@ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, f
|
||||||
|
|
||||||
// 这两个函数占了大量的时间
|
// 这两个函数占了大量的时间
|
||||||
if (g_ahrs.stable == PDR_FALSE) {
|
if (g_ahrs.stable == PDR_FALSE) {
|
||||||
buffer_push((BUFFER*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez)));
|
BufferPush((Buffer_t*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez)));
|
||||||
Buffer_mean((BUFFER*)&g_erro_buf, &g_ahrs.error);
|
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);
|
QuaternionNorm(&q0, &q1, &q2, &q3);
|
||||||
|
|
||||||
// 四元数转欧拉角
|
// 四元数转欧拉角
|
||||||
g_ahrs.pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (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.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.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) {
|
void AHRS_Init(void) {
|
||||||
int i;
|
int i;
|
||||||
g_ahrs.stable = PDR_FALSE;
|
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) {
|
for (i = 0; i < 3; ++i) {
|
||||||
Buffer_initialize((BUFFER *)&g_grav_buf[i], GRV_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);
|
||||||
Buffer_initialize((BUFFER *)&g_gyro_buf[i], GYR_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);
|
||||||
Buffer_initialize((BUFFER *)&g_magn_buf[i], MAG_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();
|
ResetARHS();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ResetARHS(void) {
|
void ResetARHS(void) {
|
||||||
|
|
||||||
Buffer_clear((BUFFER *)&g_erro_buf);
|
BufferClear((Buffer_t *)&g_erro_buf);
|
||||||
for (uchar i = 0; i < 3; ++i) {
|
for (uchar i = 0; i < 3; ++i) {
|
||||||
Buffer_clear((BUFFER *)&g_grav_buf[i]);
|
BufferClear((Buffer_t *)&g_grav_buf[i]);
|
||||||
Buffer_clear((BUFFER *)&g_gyro_buf[i]);
|
BufferClear((Buffer_t *)&g_gyro_buf[i]);
|
||||||
Buffer_clear((BUFFER *)&g_magn_buf[i]);
|
BufferClear((Buffer_t *)&g_magn_buf[i]);
|
||||||
}
|
}
|
||||||
g_ticker = 0;
|
g_ticker = 0;
|
||||||
q0 = 1.0f;
|
q0 = 1.0f;
|
||||||
|
|
@ -265,9 +265,9 @@ int UpdateAHRS(IMU_t* imu) {
|
||||||
// ARHS BUF 中存储了256个sensor的缓存
|
// ARHS BUF 中存储了256个sensor的缓存
|
||||||
if (g_ticker <= AHRS_BUF_LEN) {
|
if (g_ticker <= AHRS_BUF_LEN) {
|
||||||
for (i = 0; i < 3; ++i) {
|
for (i = 0; i < 3; ++i) {
|
||||||
buffer_push((BUFFER *)&g_grav_buf[i], acce[i]);
|
BufferPush((Buffer_t *)&g_grav_buf[i], acce[i]);
|
||||||
buffer_push((BUFFER *)&g_gyro_buf[i], gyro[i]);
|
BufferPush((Buffer_t *)&g_gyro_buf[i], gyro[i]);
|
||||||
buffer_push((BUFFER *)&g_magn_buf[i], magn[i]);
|
BufferPush((Buffer_t *)&g_magn_buf[i], magn[i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// 只有缓存满的时候才计算
|
// 只有缓存满的时候才计算
|
||||||
|
|
@ -275,7 +275,7 @@ int UpdateAHRS(IMU_t* imu) {
|
||||||
//int index;
|
//int index;
|
||||||
index = (g_magn_buf[0]._top + AHRS_BUF_LEN + 1 - AHRS_BUF_LEN / 2) % (g_magn_buf[0].length + 1);
|
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) {
|
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];
|
magn[i] = g_magn_buf[i].data[index];
|
||||||
g_grav_buf[i]._top = g_grav_buf[i]._bottom;
|
g_grav_buf[i]._top = g_grav_buf[i]._bottom;
|
||||||
}
|
}
|
||||||
|
|
@ -291,8 +291,8 @@ int UpdateAHRS(IMU_t* imu) {
|
||||||
Kp = (float)(2 * 2);
|
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 (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) {
|
for (i = 0; i < 3; ++i) {
|
||||||
buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||||||
Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
|
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
|
||||||
gyro[i] = g_gyro_buf[i].data[index];
|
gyro[i] = g_gyro_buf[i].data[index];
|
||||||
acce[i] = g_grav_buf[i].data[index];
|
acce[i] = g_grav_buf[i].data[index];
|
||||||
magn[i] = g_magn_buf[i].data[index];
|
magn[i] = g_magn_buf[i].data[index];
|
||||||
|
|
@ -322,7 +322,7 @@ int UpdateAHRS(IMU_t* imu) {
|
||||||
// 应该是判断是否进入PCA的条件
|
// 应该是判断是否进入PCA的条件
|
||||||
if (g_ahrs.error < 0.3) {
|
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));
|
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) {
|
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;
|
int c = count >> 1;
|
||||||
|
|
@ -349,8 +349,8 @@ int UpdateAHRS(IMU_t* imu) {
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i = 0; i < 3; ++i) {
|
for (i = 0; i < 3; ++i) {
|
||||||
buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||||||
Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
|
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
|
||||||
}
|
}
|
||||||
estimate_sensor_vector(grav, grav, gyro);
|
estimate_sensor_vector(grav, grav, gyro);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
@ -15,7 +15,7 @@
|
||||||
#include "pdr_detector.h"
|
#include "pdr_detector.h"
|
||||||
#include "Filter.h"
|
#include "Filter.h"
|
||||||
#include "buffer.h"
|
#include "buffer.h"
|
||||||
#include "pdr_fft.h"
|
#include "Fft.h"
|
||||||
#include "m_munkres.h"
|
#include "m_munkres.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -42,9 +42,9 @@ static float ellipsoidPoint[3];
|
||||||
static float ellipsoidScale[3];
|
static float ellipsoidScale[3];
|
||||||
|
|
||||||
/* Global Variable Definition ------------------------------------------------*/
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
static DETECTOR_t g_detector;
|
static Detector_t g_detector;
|
||||||
static BUFFER_LONG g_mol_buf[2];
|
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,
|
const double g_mol_flt_b[] = { 4.96135120696701e-05, 0.000496135120696701, 0.00223260804313515, 0.00595362144836041,
|
||||||
0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515,
|
0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515,
|
||||||
|
|
@ -68,13 +68,11 @@ static int g_label[5] = { 0 };
|
||||||
/* Function Definition -------------------------------------------------------------------------- */
|
/* Function Definition -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : pdr_getDetector
|
* Function : GetDetectorObj
|
||||||
* Description : 获取PDR运动分类器的结构体指针
|
* Description : »ñÈ¡PDRÔ˶¯·ÖÀàÆ÷¶ÔÏó
|
||||||
* Date : 2020/02/16 logzhan &
|
* Date : 2020/02/16 logzhan
|
||||||
*
|
|
||||||
*
|
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
DETECTOR_t* pdr_getDetector(void) {
|
Detector_t* GetDetectorObj(void) {
|
||||||
return &g_detector;
|
return &g_detector;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -83,7 +81,7 @@ DETECTOR_t* pdr_getDetector(void) {
|
||||||
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||||
* Date : 2020/02/16 logzhan & logzhan
|
* Date : 2020/02/16 logzhan & logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
DETECTOR_t* Detector_Init(void) {
|
Detector_t* Detector_Init(void) {
|
||||||
DetectorReset();
|
DetectorReset();
|
||||||
return &g_detector;
|
return &g_detector;
|
||||||
}
|
}
|
||||||
|
|
@ -98,28 +96,28 @@ void DetectorReset(void) {
|
||||||
g_detector.tick = 0;
|
g_detector.tick = 0;
|
||||||
g_detector.lastType = DETECTOR_TYPE_STATIC;
|
g_detector.lastType = DETECTOR_TYPE_STATIC;
|
||||||
|
|
||||||
Buffer_initialize((BUFFER *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
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);
|
||||||
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);
|
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));
|
memset(*mag_buff,0,sizeof(mag_buff));
|
||||||
mag_count = 0;
|
mag_count = 0;
|
||||||
mag_calibrate = CALIBRATE_OFF;
|
mag_calibrate = CALIBRATE_OFF;
|
||||||
|
|
@ -142,8 +140,8 @@ void DetectorReset(void) {
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void DetectorUpdateIMU(IMU_t* imu) {
|
void DetectorUpdateIMU(IMU_t* imu) {
|
||||||
|
|
||||||
buffer_push((BUFFER *)&g_mol_buf[ACCE], (float)FILTER_filter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s)));
|
BufferPush((Buffer_t *)&g_mol_buf[ACCE], (float)FilterFilter(&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[GYRO], (float)FilterFilter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s)));
|
||||||
|
|
||||||
// g_tick_d 实际上是1.28s检测一次，不然检测的频率过高
|
// g_tick_d 实际上是1.28s检测一次，不然检测的频率过高
|
||||||
if ((g_detector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
|
if ((g_detector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
|
||||||
|
|
@ -151,222 +149,26 @@ void DetectorUpdateIMU(IMU_t* imu) {
|
||||||
int detectResult = DetectMotionType();
|
int detectResult = DetectMotionType();
|
||||||
|
|
||||||
if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) {
|
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.lastType = g_detector.type;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
++g_detector.tick;
|
++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
|
* Function : fft_buffer
|
||||||
* Description : 对acc数据做傅里叶变换，频率信息保存在fft中
|
* Description : 对acc数据做傅里叶变换，频率信息保存在fft中
|
||||||
* Date : 2020/7/20 logzhan
|
* 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 };
|
static float s_signal[1024] = { 0 };
|
||||||
|
|
||||||
float mean;
|
float mean;
|
||||||
int index;
|
int index;
|
||||||
int i;
|
int i;
|
||||||
|
|
||||||
Buffer_mean(buffer, &mean);
|
BufferMean(buffer, &mean);
|
||||||
for (i = 0, index = buffer->_bottom; index != buffer->_top; ++i, index = INC_PTR(buffer, index)) {
|
for (i = 0, index = buffer->_bottom; index != buffer->_top; ++i, index = INC_PTR(buffer, index)) {
|
||||||
s_signal[i] = buffer->data[index] - mean; /*时序上255比0新*/
|
s_signal[i] = buffer->data[index] - mean; /*时序上255比0新*/
|
||||||
}
|
}
|
||||||
|
|
@ -374,7 +176,7 @@ static int fft_buffer(myComplex *fft, BUFFER *buffer) {
|
||||||
//序号i之后的长度里元素全部置为0
|
//序号i之后的长度里元素全部置为0
|
||||||
memset(s_signal + i, 0, (ARR_LEN(s_signal) - i) * sizeof(*s_signal));
|
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_OUT_OF_MEMORY;
|
||||||
}
|
}
|
||||||
return DETECTOR_NO_ERROR;
|
return DETECTOR_NO_ERROR;
|
||||||
|
|
@ -385,7 +187,8 @@ static int fft_buffer(myComplex *fft, BUFFER *buffer) {
|
||||||
* Description : 获取前number个较大频点的频率frequency和幅度amplitude
|
* Description : 获取前number个较大频点的频率frequency和幅度amplitude
|
||||||
* Date : 2020/7/20 logzhan
|
* 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];
|
float amp_2[2];
|
||||||
int min_ind;
|
int min_ind;
|
||||||
int dir;
|
int dir;
|
||||||
|
|
@ -482,8 +285,8 @@ static void copy_buffer(BUFFER_SHORT *dst, BUFFER_SHORT *src, int length) {
|
||||||
int dst_idx;
|
int dst_idx;
|
||||||
int src_idx;
|
int src_idx;
|
||||||
|
|
||||||
Buffer_count((BUFFER *)dst, &dst_cnt);
|
BufferCount((Buffer_t *)dst, &dst_cnt);
|
||||||
Buffer_count((BUFFER *)src, &src_cnt);
|
BufferCount((Buffer_t *)src, &src_cnt);
|
||||||
length = length < dst_cnt ? length : dst_cnt;
|
length = length < dst_cnt ? length : dst_cnt;
|
||||||
length = length < src_cnt ? length : src_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 */
|
/* 4.push main frequency and main amplitude buffer */
|
||||||
for (i = 0; i < number; ++i) {
|
for (i = 0; i < number; ++i) {
|
||||||
if (1e3 == COST(i, assignment[i])) {
|
if (1e3 == COST(i, assignment[i])) {
|
||||||
buffer_push((BUFFER *)&frequency_buffer[i], 0);
|
BufferPush((Buffer_t *)&frequency_buffer[i], 0);
|
||||||
buffer_push((BUFFER *)&litude_buffer[i], 0);
|
BufferPush((Buffer_t *)&litude_buffer[i], 0);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
buffer_push((BUFFER *)&frequency_buffer[i], frequency[assignment[i]]);
|
BufferPush((Buffer_t *)&frequency_buffer[i], frequency[assignment[i]]);
|
||||||
buffer_push((BUFFER *)&litude_buffer[i], amplitude[assignment[i]]);
|
BufferPush((Buffer_t *)&litude_buffer[i], amplitude[assignment[i]]);
|
||||||
frequency[assignment[i]] = 0;
|
frequency[assignment[i]] = 0;
|
||||||
amplitude[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 */
|
/* 8.push temp buffer */
|
||||||
for (i = 0; i < number; ++i) {
|
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 */
|
/* 9.caculate temp mean */
|
||||||
|
|
@ -720,9 +523,9 @@ static void get_feature(float *feature, int length) {
|
||||||
int j;
|
int j;
|
||||||
float temp;
|
float temp;
|
||||||
|
|
||||||
Buffer_mean((BUFFER *)&g_mol_buf[ACCE], &feature[0]);
|
BufferMean((Buffer_t *)&g_mol_buf[ACCE], &feature[0]);
|
||||||
Buffer_std( (BUFFER *)&g_mol_buf[ACCE], &feature[1]);
|
BufferStd( (Buffer_t *)&g_mol_buf[ACCE], &feature[1]);
|
||||||
Buffer_std( (BUFFER *)&g_mol_buf[GYRO], &feature[2]);
|
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[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[4] , &g_acc_frq_buf[1], length); if (feature[4] < 0) feature[4] = 10;
|
||||||
get_nonzero_std( &feature[5] , &g_acc_frq_buf[2], length); if (feature[5] < 0) feature[5] = 10;
|
get_nonzero_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
|
* Date : 2022/09/23 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
int DetectMotionType() {
|
int DetectMotionType() {
|
||||||
static myComplex s_fft[1024] = { {0.0} };
|
static Complex_t s_fft[1024] = { {0.0} };
|
||||||
static float s_feature[15] = { 0 };
|
static float s_feature[15] = { 0 };
|
||||||
float frq[3];
|
float frq[3];
|
||||||
float amp[3];
|
float amp[3];
|
||||||
|
|
@ -837,7 +640,7 @@ int DetectMotionType() {
|
||||||
int i;
|
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;
|
return ret;
|
||||||
}
|
}
|
||||||
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, 0))) {
|
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))) {
|
if (DETECTOR_NO_ERROR != (ret = track_frequency(g_acc_frq_buf, g_acc_amp_buf, g_acc_tmp_buf, frq, amp, 3))) {
|
||||||
return ret;
|
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;
|
return ret;
|
||||||
}
|
}
|
||||||
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == g_detector.type ||
|
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == g_detector.type ||
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,5 @@
|
||||||
/* Header File Including ------------------------------------------------------------------------ */
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
#include "pdr_fft.h"
|
#include "Fft.h"
|
||||||
|
|
||||||
|
|
||||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||||
#define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01
|
#define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01
|
||||||
|
|
@ -8,72 +7,30 @@
|
||||||
|
|
||||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||||
typedef struct FFT_MCB {
|
typedef struct FFT_MCB {
|
||||||
uint8_t type;
|
uint8_t type;
|
||||||
uint32_t parameter[4];
|
uint32_t parameter[4];
|
||||||
struct FFT_MCB *next;
|
struct FFT_MCB *next;
|
||||||
struct FFT_MCB *prev;
|
struct FFT_MCB *prev;
|
||||||
uint8_t buffer[10240];
|
uint8_t buffer[10240];
|
||||||
} FFT_MCB;
|
} FFT_MCB;
|
||||||
|
|
||||||
/* Global Variable Definition ------------------------------------------------------------------- */
|
/* Global Variable Definition ------------------------------------------------------------------- */
|
||||||
static FFT_MCB gMemery;
|
static FFT_MCB gMemery;
|
||||||
|
|
||||||
/* Function Definition -------------------------------------------------------------------------- */
|
/* 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
|
* Function : FftInit
|
||||||
* Description : ³õʼ»¯gMemery
|
* Description : ³õʼ»¯¸µÀïÒ¶ÔËËã¿â
|
||||||
* Date : 2020/7/20 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
static int FFT_init(int length) {
|
static int FftInit(int length) {
|
||||||
myComplex *omega;
|
Complex_t *omega;
|
||||||
uint16_t *position;
|
uint16_t *position;
|
||||||
int bitLength;
|
int bitLength;
|
||||||
int i;
|
int i;
|
||||||
int shift;
|
int shift;
|
||||||
int j;
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -84,7 +41,7 @@ static int FFT_init(int length) {
|
||||||
gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION;
|
gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION;
|
||||||
gMemery.parameter[0] = length;
|
gMemery.parameter[0] = length;
|
||||||
|
|
||||||
omega = (myComplex *) gMemery.buffer;
|
omega = (Complex_t *) gMemery.buffer;
|
||||||
for (i = 0; i < length; ++i) {
|
for (i = 0; i < length; ++i) {
|
||||||
omega[i].real = (float)cos(-2 * M_PI * i / length);
|
omega[i].real = (float)cos(-2 * M_PI * i / length);
|
||||||
omega[i].image = (float)sin(-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;
|
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) {
|
for (int k = 0; k < length; ++k) {
|
||||||
dft[k].real = 0;
|
dft[k].real = 0;
|
||||||
dft[k].image = 0;
|
dft[k].image = 0;
|
||||||
for(int i = 0; i < length; ++i) {
|
for(int i = 0; i < length; ++i) {
|
||||||
int ik = i * k % length;
|
int ik = i * k % length;
|
||||||
dft[k].real += signal[i] * ((myComplex *)gMemery.buffer)[ik].real;
|
dft[k].real += signal[i] * ((Complex_t *)gMemery.buffer)[ik].real;
|
||||||
dft[k].image += signal[i] * ((myComplex *)gMemery.buffer)[ik].image;
|
dft[k].image += signal[i] * ((Complex_t *)gMemery.buffer)[ik].image;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return length;
|
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;
|
int i,l,j,m;
|
||||||
uint16_t *position;
|
uint16_t *position;
|
||||||
myComplex *omega;
|
Complex_t *omega;
|
||||||
|
|
||||||
//正常情况下，length正常、没溢出就不会进入return0
|
//正常情况下，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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -137,20 +94,20 @@ int FFT_fft(myComplex *fft, float *signal, int length) {
|
||||||
fft[i].image = 0;
|
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) {
|
for (i = 0; i < length; ++i) {
|
||||||
if (i < position[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 (l = 2, m = 1; l <= length; l <<= 1, m <<= 1) {
|
||||||
for (i = 0; i < length; i += l) {
|
for (i = 0; i < length; i += l) {
|
||||||
for (j = 0; j < m; ++j) {
|
for (j = 0; j < m; ++j) {
|
||||||
myComplex delta = COMPLEX_mul(omega[length / l * j], fft[i + m + j]);
|
Complex_t delta = ComplexMul(omega[length / l * j], fft[i + m + j]);
|
||||||
fft[i + m + j] = COMPLEX_sub(fft[i + j], delta);
|
fft[i + m + j] = ComplexSub(fft[i + j], delta);
|
||||||
fft[i + j] = COMPLEX_add(fft[i + j], delta);
|
fft[i + j] = ComplexAdd(fft[i + j], delta);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -1,9 +1,8 @@
|
||||||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
* File Name : pdr_api.c
|
* File Name : Interface.c
|
||||||
* Department : Sensor Algorithm Team
|
|
||||||
* Current Version : V2.0
|
* Current Version : V2.0
|
||||||
* Author : logzhan
|
* Author : logzhan
|
||||||
* Date of Issued : 2020.7.3
|
* Date of Issued : 2022.10.15
|
||||||
* Comments : PDR导航算法对外部接口
|
* Comments : PDR导航算法对外部接口
|
||||||
********************************************************************************/
|
********************************************************************************/
|
||||||
/* Header File Including -----------------------------------------------------*/
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
|
@ -19,13 +18,13 @@
|
||||||
|
|
||||||
/* Global Variable Definition ------------------------------------------------*/
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
const char* PDR_Version = "2.0";
|
const char* PDR_Version = "2.0";
|
||||||
Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}};
|
Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}};
|
||||||
LatLng_t llaLast = { 0 };
|
LatLng_t llaLast = { 0 };
|
||||||
lct_nmea NmeaData;
|
Nmea_t NmeaData;
|
||||||
IMU_t ImuData;
|
IMU_t ImuData;
|
||||||
|
|
||||||
/* Extern Variable Definition ------------------------------------------------*/
|
/* Extern Variable Definition ------------------------------------------------*/
|
||||||
extern EKFPara_t g_kfPara;
|
extern EKFPara_t g_kfPara;
|
||||||
extern AHRS_t g_ahrs;
|
extern AHRS_t g_ahrs;
|
||||||
extern GNSS_t pgnss;
|
extern GNSS_t pgnss;
|
||||||
|
|
||||||
|
|
@ -36,7 +35,7 @@ extern GNSS_t pgnss;
|
||||||
* Description : 初始化RMC协议
|
* Description : 初始化RMC协议
|
||||||
* Date : 2022/09/16 logzhan
|
* Date : 2022/09/16 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void RMC_Init(lct_nmea_rmc * rmc)
|
void RMC_Init(NmeaRMC_t * rmc)
|
||||||
{
|
{
|
||||||
rmc->type = SENSOR_MIN;
|
rmc->type = SENSOR_MIN;
|
||||||
rmc->rmc_utc = ITEM_INVALID;
|
rmc->rmc_utc = ITEM_INVALID;
|
||||||
|
|
@ -62,7 +61,7 @@ void RMC_Init(lct_nmea_rmc * rmc)
|
||||||
* Description : 初始化GPS的NMEA协议的相关标志位
|
* Description : 初始化GPS的NMEA协议的相关标志位
|
||||||
* Date : 2020/7/3 logzhan
|
* Date : 2020/7/3 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void NmeaFlag_Init(lct_nmea * ln)
|
void NmeaFlag_Init(Nmea_t * ln)
|
||||||
{
|
{
|
||||||
ln->gga.update = 0;
|
ln->gga.update = 0;
|
||||||
for (char i = 0; i < 3; i++) {
|
for (char i = 0; i < 3; i++) {
|
||||||
|
|
@ -88,7 +87,7 @@ void NmeaFlag_Init(lct_nmea * ln)
|
||||||
* Description : 清空Nmea标志位
|
* Description : 清空Nmea标志位
|
||||||
* Date : 2020/7/4 logzhan
|
* Date : 2020/7/4 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void ClearNmeaFlg(lct_nmea * ln){
|
void ClearNmeaFlg(Nmea_t * ln){
|
||||||
NmeaFlag_Init(ln);
|
NmeaFlag_Init(ln);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -96,7 +95,7 @@ void ClearNmeaFlg(lct_nmea * ln){
|
||||||
* Function : Algorithm_Init
|
* Function : Algorithm_Init
|
||||||
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
||||||
* 迹平滑窗口的初始化
|
* 迹平滑窗口的初始化
|
||||||
* Date : 2020/7/3 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void Algorithm_Init(void) {
|
void Algorithm_Init(void) {
|
||||||
// 导航相关初始化
|
// 导航相关初始化
|
||||||
|
|
@ -115,7 +114,7 @@ void Algorithm_Init(void) {
|
||||||
* Date : 2020/07/03 logzhan
|
* Date : 2020/07/03 logzhan
|
||||||
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
int ParseLineAndUpdate(char* line, lct_fs* locFusion) {
|
int ParseLineAndUpdate(char* line, LctFs_t* locFusion) {
|
||||||
|
|
||||||
ParseLineStr(line, &ImuData, &NmeaData);
|
ParseLineStr(line, &ImuData, &NmeaData);
|
||||||
return LocationMainLoop(&ImuData, &NmeaData, locFusion, NULL);
|
return LocationMainLoop(&ImuData, &NmeaData, locFusion, NULL);
|
||||||
|
|
@ -129,7 +128,7 @@ int ParseLineAndUpdate(char* line, lct_fs* locFusion) {
|
||||||
* Date : 2020/07/03 logzhan
|
* Date : 2020/07/03 logzhan
|
||||||
* 2020/02/02 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;
|
int type = 0;
|
||||||
|
|
||||||
if (ImuData->gyr.update) {
|
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;
|
if (!NmeaData->update)return TYPE_FIX_NONE;
|
||||||
|
|
||||||
// 写GPS相关LOG信息
|
// 写GPS相关LOG信息
|
||||||
SaveGnssInfo(NmeaData, LocFusion, fp_gps);
|
SaveGnssInfo(NmeaData, LocFusion, fpGps);
|
||||||
|
|
||||||
// 有GPS则采用GPS融合定位
|
// 有GPS则采用GPS融合定位
|
||||||
int flag = GnssInsFusionLocation(NmeaData, &g_kfPara, LocFusion);
|
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相关信息
|
* Description : 保存GPS相关信息
|
||||||
* Date : 2020/7/3 logzhan
|
* 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
|
// 选取更新时间的rmc
|
||||||
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
|
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
|
||||||
rmc = nmea_data->rmc[0];
|
rmc = nmea_data->rmc[0];
|
||||||
|
|
@ -216,11 +215,11 @@ void SaveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps)
|
||||||
}
|
}
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : GetLIBVersion
|
* Function : GetPDRVersion
|
||||||
* Description : »ñÈ¡pdr¿â°æ±¾ºÅ
|
* Description : »ñÈ¡pdr°æ±¾ºÅ
|
||||||
* Date : 2020/8/3 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
const char* GetLIBVersion(void){
|
const char* GetPDRVersion(void){
|
||||||
return PDR_Version;
|
return PDR_Version;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,9 +1,9 @@
|
||||||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||||
* File Name : pdr_kalman.c
|
* File Name : Kalman.c
|
||||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
* Current Version : V2.0
|
||||||
* Author : logzhan
|
* Author : logzhan
|
||||||
* Date of Issued : 2020.7.3
|
* Date of Issued : 2022.10.15
|
||||||
* Comments : PDR 卡尔曼滤波器相关
|
* Comments : PDR扩展卡尔曼滤波器融合
|
||||||
********************************************************************************/
|
********************************************************************************/
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
@ -16,7 +16,7 @@
|
||||||
#include "Location.h"
|
#include "Location.h"
|
||||||
#include "Matrix.h"
|
#include "Matrix.h"
|
||||||
#include "Utils.h"
|
#include "Utils.h"
|
||||||
#include "pdr_linearFit.h"
|
#include "LinearFit.h"
|
||||||
#include "CoordTrans.h"
|
#include "CoordTrans.h"
|
||||||
|
|
||||||
// 扩展卡尔曼向量缓存
|
// 扩展卡尔曼向量缓存
|
||||||
|
|
@ -29,14 +29,14 @@ extern double GpsPos[HIST_GPS_NUM][3];
|
||||||
extern int debugCount;
|
extern int debugCount;
|
||||||
extern double angle_mean[3];
|
extern double angle_mean[3];
|
||||||
|
|
||||||
static int hold_type;
|
static int hold_type;
|
||||||
static double b_timestamp;
|
static double b_timestamp;
|
||||||
static double b_last_time;
|
static double b_last_time;
|
||||||
static double attitude_yaw;
|
static double attitude_yaw;
|
||||||
static DETECTOR_t *g_detector;
|
static Detector_t *g_detector;
|
||||||
static int hold_type;
|
static int hold_type;
|
||||||
static double BUL_duration;
|
static double BUL_duration;
|
||||||
static double FUR_duration;
|
static double FUR_duration;
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : KalmanFilter_Init
|
* Function : KalmanFilter_Init
|
||||||
|
|
@ -53,13 +53,12 @@ void EKF_Init(void)
|
||||||
hold_type = 0;
|
hold_type = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : pdr_ekfStatePredict
|
* Function : EKFStatePredict
|
||||||
* Description : pdr卡尔曼滤波器的状态预测方程
|
* 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 (*Phi)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]);
|
||||||
double (*pvec1)[N] = (double(*)[N])&(EkfMatBuf[1][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);
|
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
|
||||||
|
|
||||||
double deltaHeading = g_pdr->heading - g_pdr->lastHeading;
|
double deltaHeading = pdr->heading - pdr->lastHeading;
|
||||||
double angle = g_pdr->heading;
|
double angle = pdr->heading;
|
||||||
|
|
||||||
//printf("deltaHeading = %f\n", R2D(deltaHeading));
|
//printf("deltaHeading = %f\n", R2D(deltaHeading));
|
||||||
//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
|
//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;
|
// angle = g_pdr->heading;
|
||||||
//}
|
//}
|
||||||
|
|
||||||
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD) {
|
if (pdr->motionType != PDR_MOTION_TYPE_HANDHELD) {
|
||||||
deltaHeading = 0;
|
deltaHeading = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
angle = deltaHeading + kf->pXk[3];
|
angle = deltaHeading + ekf->pXk[3];
|
||||||
|
|
||||||
|
|
||||||
if (fabs(deltaHeading) > 5.5) {
|
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) {
|
if (deltaHeading < 0) {
|
||||||
deltaHeading = deltaHeading + D2R(360);
|
deltaHeading = deltaHeading + D2R(360);
|
||||||
}else {
|
}else {
|
||||||
deltaHeading = deltaHeading - D2R(360);
|
deltaHeading = deltaHeading - D2R(360);
|
||||||
}
|
}
|
||||||
deltaHeading = 0;
|
deltaHeading = 0;
|
||||||
angle = deltaHeading + kf->pXk[3];
|
angle = deltaHeading + ekf->pXk[3];
|
||||||
}
|
}
|
||||||
|
|
||||||
if(step == 0){
|
if(step == 0){
|
||||||
kf->pXk[3] = angle;
|
ekf->pXk[3] = angle;
|
||||||
}
|
}
|
||||||
|
|
||||||
// xk+1 = xk + step * cos(angle) PDR位置更新方程
|
// 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，可能在惯性
|
// phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性
|
||||||
// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
|
// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
|
||||||
// 让预测的值更新，但是效果暂时没有感觉出明显的好
|
// 让预测的值更新，但是效果暂时没有感觉出明显的好
|
||||||
if (fabs(kf->pXk[3]) > 0.000001 &&
|
if (fabs(ekf->pXk[3]) > 0.000001 &&
|
||||||
fabs(angle / kf->pXk[3]) < 3)
|
fabs(angle / ekf->pXk[3]) < 3)
|
||||||
{
|
{
|
||||||
Phi[3][3] = angle / kf->pXk[3];
|
Phi[3][3] = angle / ekf->pXk[3];
|
||||||
}
|
}
|
||||||
|
|
||||||
// 卡尔曼状态更新方程
|
// 卡尔曼状态更新方程
|
||||||
// pXk = phi * pXk
|
// pXk = phi * pXk
|
||||||
VecMatMultiply(kf->pXk, Phi, kf->pXk);
|
VecMatMultiply(ekf->pXk, Phi, ekf->pXk);
|
||||||
|
|
||||||
// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
|
// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
|
||||||
MatrixMultiply(Phi, kf->pPk, pvec1); // F*P
|
MatrixMultiply(Phi, ekf->pPk, pvec1); // F*P
|
||||||
MatrixTrans(Phi, pvec2); // Ft
|
MatrixTrans(Phi, pvec2); // Ft
|
||||||
MatrixMultiply(pvec1, pvec2, pvec1); // F*P*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 : 扩展卡尔曼滤波器的状态更新方程
|
* Description : 扩展卡尔曼滤波器的状态更新方程
|
||||||
* Date : 2020/7/22 logzhan
|
* 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) {
|
int scene_type) {
|
||||||
|
|
||||||
double lambda = 0;
|
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);
|
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
|
||||||
|
|
||||||
// Nx1向量定义
|
// Nx1向量定义
|
||||||
double* z = &(EkfVecBuf[0][0]);
|
|
||||||
double(*pvec5) = &(EkfVecBuf[1][0]);
|
double(*pvec5) = &(EkfVecBuf[1][0]);
|
||||||
double(*pvec6) = &(EkfVecBuf[2][0]);
|
double(*pvec6) = &(EkfVecBuf[2][0]);
|
||||||
double(*pv1) = &(EkfVecBuf[3][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;
|
I[i][i] = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
double* z = &(EkfVecBuf[0][0]);
|
||||||
// 获取观测向量
|
// 获取观测向量
|
||||||
z[0] = pgnss->xNed; // 北向位置
|
z[0] = pgnss->xNed; // 北向位置
|
||||||
z[1] = pgnss->yNed; // 东向位置
|
z[1] = pgnss->yNed; // 东向位置
|
||||||
|
|
@ -323,7 +320,7 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_
|
||||||
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
||||||
* Date : 2022/09/19 logzhan
|
* 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
|
* Function : InsLocationPredict
|
||||||
* Description : PDR惯导位置更新,
|
* Description : PDR惯导位置更新
|
||||||
* Date : 2020/07/08 logzhan
|
* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
|
||||||
* 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
|
|
||||||
* 会造成PDR前后位置不够均匀
|
* 会造成PDR前后位置不够均匀
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data,
|
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
|
* 2、GPS值赋值给result
|
||||||
* Date : 2022/09/19 logzhan
|
* 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 stepLength = 0.7;
|
||||||
double ned[3] = { 0 };
|
double ned[3] = { 0 };
|
||||||
|
|
@ -543,12 +539,12 @@ int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// 输出GPS位置结果
|
// 输出GPS位置结果
|
||||||
OutputOriginGnssPos(pgnss, result);
|
OutputRawGnssPos(pgnss, result);
|
||||||
g_pdr->NoGnssCount = 0;
|
g_pdr->NoGnssCount = 0;
|
||||||
return PDR_TRUE;
|
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
|
double stepLen = 0.7; // 初始运动步长为0.7m
|
||||||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) {
|
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;
|
kf->R[i][l] = 0.0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
OutputOriginGnssPos(pgnss, result);
|
OutputRawGnssPos(pgnss, result);
|
||||||
g_pdr->sysStatus = IS_NORMAL;
|
g_pdr->sysStatus = IS_NORMAL;
|
||||||
|
|
||||||
return PDR_TRUE;
|
return PDR_TRUE;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
|
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_TRUE;
|
||||||
}
|
}
|
||||||
return PDR_FALSE;
|
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
|
* 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) {
|
EKFPara_t *kf) {
|
||||||
|
|
||||||
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD &&
|
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融合定位
|
* Description : PDR的GNSS和INS融合定位
|
||||||
* Date : 2022/09/21 logzhan
|
* Date : 2022/09/21 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *g_pdr, classifer *sys, double yaw_bias,
|
void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *pdr, Classifer_t *sys, double yaw_bias,
|
||||||
EKFPara_t *kf, lct_fs *res) {
|
EKFPara_t *kf, LctFs_t *res) {
|
||||||
double plla[3] = { 0 };
|
double plla[3] = { 0 };
|
||||||
double ned[3] = { 0 };
|
double Ned[3] = { 0 };
|
||||||
double yaw_thr = 6;
|
double yaw_thr = 6;
|
||||||
|
|
||||||
int scene_type = g_pdr->sceneType;
|
int scene_type = pdr->sceneType;
|
||||||
|
|
||||||
plla[0] = pgnss->lat;
|
plla[0] = pgnss->lat;
|
||||||
plla[1] = pgnss->lon;
|
plla[1] = pgnss->lon;
|
||||||
plla[2] = 0;
|
plla[2] = 0;
|
||||||
|
|
||||||
WGS842NED(plla, g_pdr->pllaInit, ned);
|
WGS842NED(plla, pdr->pllaInit, Ned);
|
||||||
|
|
||||||
pgnss->xNed = ned[0];
|
pgnss->xNed = Ned[0];
|
||||||
pgnss->yNed = ned[1];
|
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 i = 0; i < N; i++) {
|
||||||
for (uchar l = 0; l < N; l++) {
|
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];
|
kf->pXk[i] = kf->Xk[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
ned[0] = kf -> pXk[0] - g_pdr->x0;
|
Ned[0] = kf -> pXk[0] - pdr->x0;
|
||||||
ned[1] = kf -> pXk[1] - g_pdr->y0;
|
Ned[1] = kf -> pXk[1] - pdr->y0;
|
||||||
ned[2] = 0;
|
Ned[2] = 0;
|
||||||
|
|
||||||
NED2WGS84(g_pdr->pllaInit, ned, plla);
|
NED2WGS84(pdr->pllaInit, Ned, plla);
|
||||||
|
|
||||||
res->latitude = plla[0];
|
res->latitude = plla[0];
|
||||||
res->latitude_ns = pgnss->lat_ns;
|
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->longitudinal_ew = pgnss->lon_ew;
|
||||||
res->time = pgnss->timestamps;
|
res->time = pgnss->timestamps;
|
||||||
|
|
||||||
g_pdr->NoGnssCount = 0;
|
pdr->NoGnssCount = 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -9,7 +9,7 @@
|
||||||
/* Header File Including -----------------------------------------------------*/
|
/* Header File Including -----------------------------------------------------*/
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include "pdr_linearFit.h"
|
#include "LinearFit.h"
|
||||||
#include "Utils.h"
|
#include "Utils.h"
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
|
|
@ -25,7 +25,7 @@
|
||||||
/* Global Variable Definition ------------------------------------------------*/
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
PDR_t* pdr; // PDR全局信息
|
PDR_t* pdr; // PDR全局信息
|
||||||
StepPredict stepPredict = { 0 };
|
StepPredict stepPredict = { 0 };
|
||||||
classifer sys;
|
Classifer_t sys;
|
||||||
GNSS_t pgnss; // 定位相关信息
|
GNSS_t pgnss; // 定位相关信息
|
||||||
EKFPara_t g_kfPara;
|
EKFPara_t g_kfPara;
|
||||||
double GpsPos[HIST_GPS_NUM][3] = {{0}};
|
double GpsPos[HIST_GPS_NUM][3] = {{0}};
|
||||||
|
|
@ -139,7 +139,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
|
||||||
|
|
||||||
//pdr->heading = CalPredHeading(...);
|
//pdr->heading = CalPredHeading(...);
|
||||||
|
|
||||||
double imuYaw = - g_ahrs.yaw;
|
double imuYaw = - g_ahrs.Yaw;
|
||||||
if (imuYaw < 0) {
|
if (imuYaw < 0) {
|
||||||
imuYaw += 360;
|
imuYaw += 360;
|
||||||
}
|
}
|
||||||
|
|
@ -163,7 +163,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
|
||||||
* Description : PDR GPS融合INS惯性定位
|
* Description : PDR GPS融合INS惯性定位
|
||||||
* Date : 2021/01/29 logzhan
|
* 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数据更新
|
// GPS数据更新
|
||||||
GnssUpdate(&pgnss, nmea_data);
|
GnssUpdate(&pgnss, nmea_data);
|
||||||
|
|
@ -228,7 +228,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result)
|
||||||
// 不满足推算条件，直接输出原始GPS
|
// 不满足推算条件，直接输出原始GPS
|
||||||
if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 &&
|
if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 &&
|
||||||
nmea_data->accuracy.err < 15) {
|
nmea_data->accuracy.err < 15) {
|
||||||
OutputOriginGnssPos(&pgnss, result);
|
OutputRawGnssPos(&pgnss, result);
|
||||||
pdr->NoGnssCount = 0;
|
pdr->NoGnssCount = 0;
|
||||||
return TYPE_FIX_GPS;
|
return TYPE_FIX_GPS;
|
||||||
}
|
}
|
||||||
|
|
@ -248,7 +248,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result)
|
||||||
* Description : 在没有gps信息时预测GPS位置
|
* Description : 在没有gps信息时预测GPS位置
|
||||||
* Date : 2022/09/16
|
* 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};
|
double ned[3] = {0}, lla[3] = {0};
|
||||||
ned[0] = kf->pXk[0] - p_pdr->x0;
|
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信号较好时的偏航角修正磁力计计算方向键的偏移
|
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
||||||
* Date : 2020/07/08 logzhan
|
* 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信号是否正常以及惯导是否平稳）
|
//惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳）
|
||||||
if (refGpsYaw != -1000)return;
|
if (refGpsYaw != -1000)return;
|
||||||
|
|
||||||
if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0)
|
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)
|
&& (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*/
|
/*这里放姿态角与GPS对准part1 start*/
|
||||||
double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]),
|
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，提高
|
* 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 &&
|
if (pgnss->error > 0 && pgnss->error < 15 &&
|
||||||
pgnss->lastError >= pgnss->error) {
|
pgnss->lastError >= pgnss->error) {
|
||||||
OutputOriginGnssPos(pgnss, result);
|
OutputRawGnssPos(pgnss, result);
|
||||||
pgnss->lastError = pgnss->error;
|
pgnss->lastError = pgnss->error;
|
||||||
return TYPE_FIX_PDR;
|
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);
|
ResetSystemStatus(kf);
|
||||||
if (pgnss->error >= 0 || (pgnss->satellites_num > 4
|
if (pgnss->error >= 0 || (pgnss->satellites_num > 4
|
||||||
&& pgnss->HDOP < 2.0)) {
|
&& pgnss->HDOP < 2.0)) {
|
||||||
OutputOriginGnssPos(pgnss, result);
|
OutputRawGnssPos(pgnss, result);
|
||||||
return TYPE_FIX_GPS;
|
return TYPE_FIX_GPS;
|
||||||
}
|
}
|
||||||
// 精度过差，不输出位置
|
// 精度过差，不输出位置
|
||||||
|
|
@ -348,9 +348,9 @@ double GnssCalHeading(GNSS_t* pgnss) {
|
||||||
* Description : nmea数据结构转gnss数据
|
* Description : nmea数据结构转gnss数据
|
||||||
* Date : 2020/07/08 logzhan
|
* 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) {
|
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
|
||||||
rmc = nmea_data->rmc[0];
|
rmc = nmea_data->rmc[0];
|
||||||
}else {
|
}else {
|
||||||
|
|
@ -380,7 +380,7 @@ void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss)
|
||||||
* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
|
* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
|
||||||
* 什么用，还增加运算量
|
* 什么用，还增加运算量
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void GnssUpdate(GNSS_t* gps, lct_nmea* nmea) {
|
void GnssUpdate(GNSS_t* gps, Nmea_t* nmea) {
|
||||||
// nmea数据转gnss数据结构
|
// nmea数据转gnss数据结构
|
||||||
Nmea2Gnss(nmea, &pgnss);
|
Nmea2Gnss(nmea, &pgnss);
|
||||||
// GPS回调更新，暂时不清楚函数功能
|
// GPS回调更新，暂时不清楚函数功能
|
||||||
|
|
|
||||||
|
|
@ -60,7 +60,7 @@
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include "pdr_api.h"
|
#include "pdr_api.h"
|
||||||
#include "pdr_main.h"
|
#include "Main.h"
|
||||||
#include "Utils.h"
|
#include "Utils.h"
|
||||||
#include "time.h"
|
#include "time.h"
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
@ -114,7 +114,7 @@ int main(int argc, char* argv[])
|
||||||
// PDR 仿真流程
|
// PDR 仿真流程
|
||||||
Algorithm_Init();
|
Algorithm_Init();
|
||||||
|
|
||||||
lct_fs locFusion;
|
LctFs_t locFusion;
|
||||||
memset(&locFusion,0, sizeof(locFusion));
|
memset(&locFusion,0, sizeof(locFusion));
|
||||||
memset(&resTracks, 0, sizeof(resTracks));
|
memset(&resTracks, 0, sizeof(resTracks));
|
||||||
|
|
||||||
|
|
@ -190,7 +190,7 @@ double gpsYaw2GoogleYaw(double heading) {
|
||||||
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
||||||
* Date : 2022/09/19 logzhan
|
* 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].lat = lctfs.latitude;
|
||||||
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
|
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
|
||||||
|
|
@ -387,10 +387,10 @@ PosFusion LibPDR_StepUpdateGPS(int useGpsFlg)
|
||||||
PosFusion fusion;
|
PosFusion fusion;
|
||||||
fusion.vaild = 0;
|
fusion.vaild = 0;
|
||||||
|
|
||||||
lct_nmea location_nmea;
|
Nmea_t location_nmea;
|
||||||
lct_nmea* ln = &location_nmea;
|
Nmea_t* ln = &location_nmea;
|
||||||
imu imu_data;
|
imu imu_data;
|
||||||
lct_fs lct_fusion;
|
LctFs_t lct_fusion;
|
||||||
memset(&lct_fusion, 0, sizeof(lct_fusion));
|
memset(&lct_fusion, 0, sizeof(lct_fusion));
|
||||||
//memset(&location_nmea, 0, sizeof(location_nmea));
|
//memset(&location_nmea, 0, sizeof(location_nmea));
|
||||||
memset(&imu_data, 0, sizeof(imu_data));
|
memset(&imu_data, 0, sizeof(imu_data));
|
||||||
|
|
|
||||||
|
|
@ -1,9 +1,9 @@
|
||||||
|
|
||||||
/* Header File Including ------------------------------------------------------------------------ */#include <stdint.h>
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
|
#include <stdint.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include "m_munkres.h"
|
#include "m_munkres.h"
|
||||||
|
|
||||||
//#include "loc_vivo_pdr.h"
|
|
||||||
/* Macro Definition ----------------------------------------------------------------------------- */
|
/* Macro Definition ----------------------------------------------------------------------------- */
|
||||||
#define COST(i, j) cost[(i) + (j) * m]
|
#define COST(i, j) cost[(i) + (j) * m]
|
||||||
#define MARK(i, j) mark[(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;
|
static uint8_t g_col_cover;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* Function Declaration ------------------------------------------------------------------------- */
|
/* Function Declaration ------------------------------------------------------------------------- */
|
||||||
static int step_one(float *cost, int m, int n);
|
static int step_one(float *cost, int m, int n);
|
||||||
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover);
|
static int step_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_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);
|
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 -------------------------------------------------------------------------- */
|
/* Function Definition -------------------------------------------------------------------------- */
|
||||||
int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
|
int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
|
||||||
|
|
@ -40,13 +35,6 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
|
||||||
int j = 0;
|
int j = 0;
|
||||||
int step;
|
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) {
|
if (m > 8 || n > 8) {
|
||||||
return MUNKRES_EXCESSIVE_MATRIX;
|
return MUNKRES_EXCESSIVE_MATRIX;
|
||||||
|
|
@ -17,7 +17,7 @@
|
||||||
#include "buffer.h"
|
#include "buffer.h"
|
||||||
#include "Filter.h"
|
#include "Filter.h"
|
||||||
#include "pdr_base.h"
|
#include "pdr_base.h"
|
||||||
#include "pdr_fft.h"
|
#include "Fft.h"
|
||||||
|
|
||||||
/* Macro Definition ----------------------------------------------------------*/
|
/* Macro Definition ----------------------------------------------------------*/
|
||||||
#define DOWNSAMPLING_RATE 4
|
#define DOWNSAMPLING_RATE 4
|
||||||
|
|
@ -31,7 +31,7 @@
|
||||||
|
|
||||||
/* Global Variable Definition ------------------------------------------------*/
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
PDR_t g_pdr;
|
PDR_t g_pdr;
|
||||||
static DETECTOR_t *g_detector;
|
static Detector_t *g_detector;
|
||||||
static uint64_t g_tick_p;
|
static uint64_t g_tick_p;
|
||||||
static uint32_t g_motion_type;
|
static uint32_t g_motion_type;
|
||||||
static uint32_t g_axis_ceof_counter;
|
static uint32_t g_axis_ceof_counter;
|
||||||
|
|
@ -44,7 +44,7 @@ static int angle_count;
|
||||||
static double p_angle[3][BUF_LEN];
|
static double p_angle[3][BUF_LEN];
|
||||||
double angle_mean[3] = { 0 };
|
double angle_mean[3] = { 0 };
|
||||||
|
|
||||||
static FILTER g_grv_flt[2];
|
static Filter_t g_grv_flt[2];
|
||||||
|
|
||||||
#if defined(__MATLAB_DBG)
|
#if defined(__MATLAB_DBG)
|
||||||
static int g_debug[2];
|
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;
|
const double g_grv_flt_h0 = 1;
|
||||||
|
|
||||||
PDR_t* Base_Init() {
|
PDR_t* Base_Init() {
|
||||||
g_detector = pdr_getDetector();
|
g_detector = GetDetectorObj();
|
||||||
Buffer_initialize((BUFFER *)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||||
Buffer_initialize((BUFFER *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
BufferInit((Buffer_t *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||||
Buffer_initialize((BUFFER *)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN);
|
BufferInit((Buffer_t *)&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);
|
BufferInit((Buffer_t *)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE);
|
||||||
Buffer_initialize((BUFFER *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60);
|
BufferInit((Buffer_t *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60);
|
||||||
Buffer_initialize((BUFFER *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60);
|
BufferInit((Buffer_t *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60);
|
||||||
Buffer_initialize((BUFFER *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60);
|
BufferInit((Buffer_t *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60);
|
||||||
Buffer_initialize((BUFFER *)&g_posi[3], "posi_3", 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);
|
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);
|
||||||
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[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);
|
||||||
|
|
||||||
pdr_resetData();
|
pdr_resetData();
|
||||||
return &g_pdr;
|
return &g_pdr;
|
||||||
|
|
@ -127,24 +127,24 @@ void pdr_resetData(void) {
|
||||||
g_tick_p = 0;
|
g_tick_p = 0;
|
||||||
g_motion_type = g_detector->type;
|
g_motion_type = g_detector->type;
|
||||||
g_axis_ceof_counter = 0;
|
g_axis_ceof_counter = 0;
|
||||||
Buffer_clear((BUFFER *)&g_axis[0]);
|
BufferClear((Buffer_t *)&g_axis[0]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[1]);
|
BufferClear((Buffer_t *)&g_axis[1]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[2]);
|
BufferClear((Buffer_t *)&g_axis[2]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[3]);
|
BufferClear((Buffer_t *)&g_axis[3]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[4]);
|
BufferClear((Buffer_t *)&g_axis[4]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[5]);
|
BufferClear((Buffer_t *)&g_axis[5]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[6]);
|
BufferClear((Buffer_t *)&g_axis[6]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[7]);
|
BufferClear((Buffer_t *)&g_axis[7]);
|
||||||
Buffer_clear((BUFFER *)&g_axis[8]);
|
BufferClear((Buffer_t *)&g_axis[8]);
|
||||||
Buffer_clear((BUFFER *)&g_grav[0]);
|
BufferClear((Buffer_t *)&g_grav[0]);
|
||||||
Buffer_clear((BUFFER *)&g_grav[1]);
|
BufferClear((Buffer_t *)&g_grav[1]);
|
||||||
Buffer_clear((BUFFER *)&g_erro);
|
BufferClear((Buffer_t *)&g_erro);
|
||||||
Buffer_clear((BUFFER *)&g_posi[0]);
|
BufferClear((Buffer_t *)&g_posi[0]);
|
||||||
Buffer_clear((BUFFER *)&g_posi[1]);
|
BufferClear((Buffer_t *)&g_posi[1]);
|
||||||
Buffer_clear((BUFFER *)&g_posi[2]);
|
BufferClear((Buffer_t *)&g_posi[2]);
|
||||||
Buffer_clear((BUFFER *)&g_posi[3]);
|
BufferClear((Buffer_t *)&g_posi[3]);
|
||||||
FILTER_Reset(&g_grv_flt[0]);
|
FilterReset(&g_grv_flt[0]);
|
||||||
FILTER_Reset(&g_grv_flt[1]);
|
FilterReset(&g_grv_flt[1]);
|
||||||
|
|
||||||
angle_count = 0;
|
angle_count = 0;
|
||||||
memset(&p_angle, 0, sizeof(p_angle));
|
memset(&p_angle, 0, sizeof(p_angle));
|
||||||
|
|
@ -376,11 +376,11 @@ void ComputePCA(AHRS_t *ahrs) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/**----------------------------------------------------------------------
|
/**----------------------------------------------------------------------
|
||||||
* Function : OutputOriginGnssPos
|
* Function : OutputRawGnssPos
|
||||||
* Description : PDR输出原始GNSS定位结果
|
* Description : 输出GPS位置
|
||||||
* Date : 2022/09/16 logzhan
|
* 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 = pgnss->lat;
|
||||||
result->latitude_ns = pgnss->lat_ns;
|
result->latitude_ns = pgnss->lat_ns;
|
||||||
result->longitudinal = pgnss->lon;
|
result->longitudinal = pgnss->lon;
|
||||||
|
|
@ -395,7 +395,7 @@ void OutputOriginGnssPos(GNSS_t* pgnss, lct_fs* result) {
|
||||||
* 单运动频率计算量太大，可以考虑用加速度判断用户状态。
|
* 单运动频率计算量太大，可以考虑用加速度判断用户状态。
|
||||||
* Date : 2021/01/28 logzhan
|
* 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 &&
|
if (pgnss->vel != -1 && pgnss->vel < 1 && (delSteps == 0 &&
|
||||||
g_pdr->motionFreq == 0.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，车载模式标志位
|
* Output : int，车载模式标志位
|
||||||
* Date : 2021/01/28 logzhan
|
* 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) ||
|
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)++;
|
(*time)++;
|
||||||
}else {
|
}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推算失误，或者其他情况
|
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
|
||||||
* 重置PDR到GPS的位置。
|
* 重置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))) {
|
if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (g_pdr->motionFreq == 0))) {
|
||||||
(*gpscnt)++;
|
(*gpscnt)++;
|
||||||
|
|
@ -445,11 +445,11 @@ int detPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pdr)
|
||||||
}
|
}
|
||||||
|
|
||||||
/**---------------------------------------------------------------------
|
/**---------------------------------------------------------------------
|
||||||
* Function : detectorUpdate
|
* Function : DetectorUpdate
|
||||||
* Description : 根据新确定的手机携带方式更新g_pdr.status等
|
* Description : 根据新确定的手机携带方式更新状态等
|
||||||
* Date : 2020/7/20 logzhan
|
* Date : 2022/10/15 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void detectorUpdate(DETECTOR_t *detector) {
|
void DetectorUpdate(Detector_t *detector) {
|
||||||
|
|
||||||
// 设置PDR的运动类型
|
// 设置PDR的运动类型
|
||||||
g_pdr.motionType = detector->type;
|
g_pdr.motionType = detector->type;
|
||||||
|
|
@ -511,14 +511,14 @@ void gpsUpdateCb(GNSS_t* gps)
|
||||||
}
|
}
|
||||||
|
|
||||||
/* caculate position */
|
/* caculate position */
|
||||||
Buffer_top((BUFFER *)&g_posi[0], &temp);
|
BufferGetTop((Buffer_t *)&g_posi[0], &temp);
|
||||||
buffer_push((BUFFER *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI));
|
BufferPush((Buffer_t *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI));
|
||||||
Buffer_top((BUFFER *)&g_posi[1], &temp);
|
BufferGetTop((Buffer_t *)&g_posi[1], &temp);
|
||||||
buffer_push((BUFFER *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI));
|
BufferPush((Buffer_t *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI));
|
||||||
Buffer_top((BUFFER *)&g_posi[2], &temp);
|
BufferGetTop((Buffer_t *)&g_posi[2], &temp);
|
||||||
buffer_push((BUFFER *)&g_posi[2], temp + g_pdr.pca_direction[0]);
|
BufferPush((Buffer_t *)&g_posi[2], temp + g_pdr.pca_direction[0]);
|
||||||
Buffer_top((BUFFER *)&g_posi[3], &temp);
|
BufferGetTop((Buffer_t *)&g_posi[3], &temp);
|
||||||
buffer_push((BUFFER *)&g_posi[3], temp + g_pdr.pca_direction[1]);
|
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) {
|
if ((g_posi[0]._top - g_posi[0]._bottom + g_posi[0].length + 1) % (g_posi[0].length + 1) >= 5) {
|
||||||
|
|
||||||
count = 0;
|
count = 0;
|
||||||
|
|
|
||||||
|
|
@ -158,7 +158,7 @@ void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void parseNMEA(char* pt, lct_nmea *ln, double ts)
|
void parseNMEA(char* pt, Nmea_t *ln, double ts)
|
||||||
{
|
{
|
||||||
long i, result;
|
long i, result;
|
||||||
long s = 0 ;
|
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)
|
if (ln->gga.update && (ln->rmc[0].update || ln->rmc[1].update) && (ln->gsa[0].update || ln->gsa[1].update || ln->gsa[2].update)
|
||||||
&& (ln->gsv[0].update || ln->gsv[1].update || ln->gsv[2].update))
|
&& (ln->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))
|
if (!strcmp(pt, GNGGA_STR))
|
||||||
{
|
{
|
||||||
|
|
@ -388,12 +388,12 @@ void ParseGGA(char* pt, lct_nmea *ln, double ts)
|
||||||
ln->gga.time = 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;
|
int i = 0;
|
||||||
double temp = 0;
|
double temp = 0;
|
||||||
double rmc_time = 0;
|
double rmc_time = 0;
|
||||||
lct_nmea_rmc *rmc = NULL;
|
NmeaRMC_t *rmc = NULL;
|
||||||
|
|
||||||
if (!strcmp(pt, GNRMC_STR))
|
if (!strcmp(pt, GNRMC_STR))
|
||||||
{
|
{
|
||||||
|
|
@ -524,13 +524,13 @@ void ParseRMC(char* pt, lct_nmea *ln, double ts)
|
||||||
rmc->time = 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;
|
char *pt_gsv = NULL;
|
||||||
|
|
||||||
//memset(&ln->gsv, ITEM_INVALID, sizeof(ln->gsv));
|
//memset(&ln->gsv, ITEM_INVALID, sizeof(ln->gsv));
|
||||||
int i = 0;
|
int i = 0;
|
||||||
lct_nmea_gsv* gsv = NULL;
|
NmeaGSV_t* gsv = NULL;
|
||||||
if (!strcmp(pt, GPGSV_STR))
|
if (!strcmp(pt, GPGSV_STR))
|
||||||
{
|
{
|
||||||
gsv = &ln->gsv[0];
|
gsv = &ln->gsv[0];
|
||||||
|
|
@ -676,11 +676,11 @@ void parseGSV(char* pt, lct_nmea *ln, double ts)
|
||||||
gsv->time = 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 i = 0;
|
||||||
int ind = -1;
|
int idx = -1;
|
||||||
lct_nmea_gsa gsa = {0};
|
NmeaGSA_t gsa = {0};
|
||||||
if (!strcmp(pt, GNGSA_STR))
|
if (!strcmp(pt, GNGSA_STR))
|
||||||
{
|
{
|
||||||
gsa.type = SENSOR_LOCATION_NMEA_GNGSA;
|
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))
|
else if (!strcmp(pt, GPGSA_STR))
|
||||||
{
|
{
|
||||||
ind = 0;
|
idx = 0;
|
||||||
gsa.type = SENSOR_LOCATION_NMEA_GPGSA;
|
gsa.type = SENSOR_LOCATION_NMEA_GPGSA;
|
||||||
gsa.update = 1;
|
gsa.update = 1;
|
||||||
}
|
}
|
||||||
else if (!strcmp(pt, GLGSA_STR))
|
else if (!strcmp(pt, GLGSA_STR))
|
||||||
{
|
{
|
||||||
ind = 1;
|
idx = 1;
|
||||||
gsa.type = SENSOR_LOCATION_NMEA_GLGSA;
|
gsa.type = SENSOR_LOCATION_NMEA_GLGSA;
|
||||||
gsa.update = 1;
|
gsa.update = 1;
|
||||||
}
|
}
|
||||||
else if (!strcmp(pt, GAGSA_STR))
|
else if (!strcmp(pt, GAGSA_STR))
|
||||||
{
|
{
|
||||||
ind = 2;
|
idx = 2;
|
||||||
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
|
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
|
||||||
gsa.update = 1;
|
gsa.update = 1;
|
||||||
}
|
}
|
||||||
|
|
@ -736,38 +736,38 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts)
|
||||||
{
|
{
|
||||||
gsa.prn[i - 2] = atoi(pt);
|
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;
|
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;
|
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;
|
gsa.prn[i - 2] = (gsa.prn[i - 2] % 36)+1;
|
||||||
}
|
}
|
||||||
if (gsa.prn[i - 2] > 64)
|
if (gsa.prn[i - 2] > 64)
|
||||||
{
|
{
|
||||||
gsa.prnflg |= (1 << (gsa.prn[i - 2] - 64));
|
gsa.prnflg |= (1 << (gsa.prn[i - 2] - 64));
|
||||||
if (ind < 0)
|
if (idx < 0)
|
||||||
{
|
{
|
||||||
ind = 1;
|
idx = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
gsa.prnflg |= (1 << gsa.prn[i - 2]);
|
gsa.prnflg |= (1 << gsa.prn[i - 2]);
|
||||||
if (ind < 0)
|
if (idx < 0)
|
||||||
{
|
{
|
||||||
if (ln->gsa[0].update && ln->gsa[1].update)
|
if (ln->gsa[0].update && ln->gsa[1].update)
|
||||||
{
|
{
|
||||||
ind = 2;
|
idx = 2;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
ind = 0;
|
idx = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -779,19 +779,19 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts)
|
||||||
gsa.vdop = (float)atof(pt);
|
gsa.vdop = (float)atof(pt);
|
||||||
}else if (17 == i)
|
}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;
|
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;
|
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;
|
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -804,9 +804,9 @@ void ParseGSA(char* pt, lct_nmea *ln, double ts)
|
||||||
}
|
}
|
||||||
|
|
||||||
gsa.time = 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精度参数
|
* Description : 解析GPS的Accuracy精度参数
|
||||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
* 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;
|
int i = 0;
|
||||||
while (pt && 8>i){
|
while (pt && 8>i){
|
||||||
|
|
@ -848,7 +848,7 @@ void parseLocAccuracy(char* pt, lct_nmea *ln, double ts)
|
||||||
* Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析
|
* Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析
|
||||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
* 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;
|
char* pt = NULL;
|
||||||
int i = 0, sstp = -1;
|
int i = 0, sstp = -1;
|
||||||
|
|
|
||||||
|
|
@ -36,9 +36,9 @@ void QuaternConj(float _q[], float q[]) {
|
||||||
* Description : 四元数乘法
|
* Description : 四元数乘法
|
||||||
* Date : 2022/09/21 logzhan
|
* Date : 2022/09/21 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
void QuaternProd(float ab[], float a[], float b[]) {
|
void QuaternProd(float qab[], float a[], float b[]) {
|
||||||
ab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
|
qab[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];
|
qab[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];
|
qab[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];
|
qab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
|
||||||
}
|
}
|
||||||
|
|
@ -79,7 +79,7 @@ static int alloc_resources (void);
|
||||||
0--success;
|
0--success;
|
||||||
-1--NULL param inputed;
|
-1--NULL param inputed;
|
||||||
1--lct_nmea _src_nmea is not valiable beacause of the update flag. */
|
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() */
|
/* Function name: static _detect_good_signal() */
|
||||||
/* Usage: judge the quality of positioning result between good and bad. */
|
/* Usage: judge the quality of positioning result between good and bad. */
|
||||||
|
|
@ -182,7 +182,7 @@ SCENE_INIT_STATE scene_recognition_reset(void)
|
||||||
* Author : logzhan
|
* Author : logzhan
|
||||||
* Date : 2020/02/18
|
* 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;
|
SCENE_RECOGNITION_RESULT res = RECOG_UNKNOWN;
|
||||||
int transRes = 0;
|
int transRes = 0;
|
||||||
|
|
@ -222,7 +222,7 @@ SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea *nmea)
|
||||||
* Author : Zhang Jingrui, Geek ID: 11082826
|
* Author : Zhang Jingrui, Geek ID: 11082826
|
||||||
* Date : 2020/02/18 logzhan
|
* Date : 2020/02/18 logzhan
|
||||||
*---------------------------------------------------------------------**/
|
*---------------------------------------------------------------------**/
|
||||||
int isOpenArea(const lct_nmea *nmea)
|
int isOpenArea(const Nmea_t *nmea)
|
||||||
{
|
{
|
||||||
SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea);
|
SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea);
|
||||||
// Èç¹ûÊÇ¿ªÀ«µØÔò·µ»Ø1
|
// Èç¹ûÊÇ¿ªÀ«µØÔò·µ»Ø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;
|
int _parse_res_state = 0;
|
||||||
/* Temporarily deprecated. */
|
/* Temporarily deprecated. */
|
||||||
|
|
@ -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 ------------------------------------------------------------------------ */
|
/* Header File Including ------------------------------------------------------------------------ */
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
@ -9,27 +17,28 @@
|
||||||
#define DEC_PTR(ptr) ((ptr + buffer->length) % (buffer->length + 1))
|
#define DEC_PTR(ptr) ((ptr + buffer->length) % (buffer->length + 1))
|
||||||
|
|
||||||
/* Function Definition -------------------------------------------------------------------------- */
|
/* 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;
|
int i;
|
||||||
if (NULL == buffer) {
|
if (NULL == buf) {
|
||||||
return BUFFER_WRONG_PARAMETER;
|
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) {
|
for (i = 0; i < 20 - 1 && '\0' != name[i]; ++i) {
|
||||||
buffer->name[i] = name[i];
|
buf->name[i] = name[i];
|
||||||
}
|
}
|
||||||
buffer->name[i] = '\0';
|
buf->name[i] = '\0';
|
||||||
buffer->type = type;
|
buf->type = type;
|
||||||
buffer->length = length;
|
buf->length = len;
|
||||||
buffer->_bottom = 0;
|
buf->_bottom = 0;
|
||||||
buffer->_top = 0;
|
buf->_top = 0;
|
||||||
buffer->mean = 0.0;
|
buf->mean = 0.0;
|
||||||
buffer->sum = 0.0;
|
buf->sum = 0.0;
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_clear(BUFFER *buffer) {
|
int BufferClear(Buffer_t *buffer) {
|
||||||
if (NULL == buffer) {
|
if (NULL == buffer) {
|
||||||
return BUFFER_WRONG_PARAMETER;
|
return BUFFER_WRONG_PARAMETER;
|
||||||
}
|
}
|
||||||
|
|
@ -42,17 +51,15 @@ int Buffer_clear(BUFFER *buffer) {
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_count(BUFFER *buffer, int *count) {
|
int BufferCount(Buffer_t *buffer, int *count) {
|
||||||
if (NULL == buffer) {
|
if (NULL == buffer) {
|
||||||
return BUFFER_WRONG_PARAMETER;
|
return BUFFER_WRONG_PARAMETER;
|
||||||
}
|
}
|
||||||
|
|
||||||
*count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
|
*count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
|
||||||
|
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_top(BUFFER *buffer, float *value) {
|
int BufferGetTop(Buffer_t *buffer, float *value) {
|
||||||
*value = 0;
|
*value = 0;
|
||||||
|
|
||||||
if (NULL == buffer) {
|
if (NULL == buffer) {
|
||||||
|
|
@ -68,7 +75,7 @@ int Buffer_top(BUFFER *buffer, float *value) {
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_bottom(BUFFER *buffer, float *value) {
|
int BufferGetBottom(Buffer_t *buffer, float *value) {
|
||||||
if (NULL == buffer) {
|
if (NULL == buffer) {
|
||||||
return BUFFER_WRONG_PARAMETER;
|
return BUFFER_WRONG_PARAMETER;
|
||||||
}
|
}
|
||||||
|
|
@ -82,7 +89,7 @@ int Buffer_bottom(BUFFER *buffer, float *value) {
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_pop(BUFFER *buffer, float *value) {
|
int BufferPop(Buffer_t *buffer, float *value) {
|
||||||
if (NULL == buffer) {
|
if (NULL == buffer) {
|
||||||
return BUFFER_WRONG_PARAMETER;
|
return BUFFER_WRONG_PARAMETER;
|
||||||
}
|
}
|
||||||
|
|
@ -106,7 +113,7 @@ int Buffer_pop(BUFFER *buffer, float *value) {
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int buffer_push(BUFFER *buffer, float value) {
|
int BufferPush(Buffer_t *buffer, float value) {
|
||||||
if (NULL == buffer) {
|
if (NULL == buffer) {
|
||||||
return BUFFER_WRONG_PARAMETER; /*BUFFER_WRONG_PARAMETERֵΪ1*/
|
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) {
|
if (NULL == buffer) {
|
||||||
return BUFFER_WRONG_PARAMETER;
|
return BUFFER_WRONG_PARAMETER;
|
||||||
}
|
}
|
||||||
|
|
@ -137,7 +144,7 @@ int Buffer_get(BUFFER *buffer, float *value, int index) {
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_mean(BUFFER *buffer, float *mean) {
|
int BufferMean(Buffer_t *buffer, float *mean) {
|
||||||
int index;
|
int index;
|
||||||
int count;
|
int count;
|
||||||
|
|
||||||
|
|
@ -154,11 +161,10 @@ int Buffer_mean(BUFFER *buffer, float *mean) {
|
||||||
if (0 != count) {
|
if (0 != count) {
|
||||||
*mean /= count;
|
*mean /= count;
|
||||||
}
|
}
|
||||||
|
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_var(BUFFER *buffer, float *var) {
|
int BufferVar(Buffer_t *buffer, float *var) {
|
||||||
int index;
|
int index;
|
||||||
int count;
|
int count;
|
||||||
float mean;
|
float mean;
|
||||||
|
|
@ -188,11 +194,9 @@ int Buffer_var(BUFFER *buffer, float *var) {
|
||||||
return BUFFER_NO_ERROR;
|
return BUFFER_NO_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Buffer_std(BUFFER *buffer, float *std) {
|
int BufferStd(Buffer_t *buffer, float *std) {
|
||||||
int ret;
|
int ret;
|
||||||
|
ret = BufferVar(buffer, std);
|
||||||
ret = Buffer_var(buffer, std);
|
|
||||||
*std = (float)sqrt(*std);
|
*std = (float)sqrt(*std);
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue