PDR/1.Software/PDR 1.02/include/pdr_util.h

#ifndef _PDR_UTIL_H_
#define _PDR_UTIL_H_

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>


#define TRAJ_POINT_COUNT  1000
#define	MIN_POINT_COUNT   15
#define OPT_TRAJ_TIMEOUT  1000

#define FALIED_LINE 0
#define IN_LINE  1
#define CLOSE_START  2
#define CLOSE_END  3

typedef struct LatLng{
	double lat;
	double lon;
} LatLng;

typedef struct TrajSign
{
	int count;
	double lastTime;
	double weight;
	double yaw[TRAJ_POINT_COUNT];
	double error[TRAJ_POINT_COUNT];
	double points[TRAJ_POINT_COUNT][2];
}TrajSign;

/**地球相关参数*******************************************************************/
typedef struct
{
	double rmh;                         /* 子午圈曲率半径				 		*/
	double rnh;						    /* 卯酉圈曲率半径					 	*/
	double grav;						/* 当地重力加速度					 	*/
	double lat;						    /* 当地纬度（rad）*/
	double wnie[3];					    /* 地理系相对惯性系的角速度在导航系分量   */
} EarthData_t;

/**----------------------------------------------------------------------
* Function    : mean
* Description : 对输入double数组求均值
* Review      : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
 *              不同函数
* Date        : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
double mean(double *x, int n);

float fmean(float *data, int len);

float stdf(float* data, int len);

double meanAngle(double* angle, int len);

void modAngle(double * angle, double min, double max);

double pow_i(double num, long long n);

double pow_f(double num, double m);

double vivopow(double x, double y);

void centralization(double *x, double *x_out, int n);

void getCovMatrix(double *x, double *y, double cov[2][2], int n);

int Jacobi(double a[][2], double p[][2], int n, double eps, int T);

void QuaternConj(float _q[], float q[]);

void quaternProd(float ab[], float a[], float b[]);

void WGS842ECEF(double *plla, double *ecef);

void ECEF2WGS84(double *ecef, double *plla);

void ECEF2Ned(double *ecef, double *plla, double *ned);

void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef);

void WGS842NED(double *plla, double *ref_lla, double *ned);

void NED2WGS84(double *ref_plla, double *ned, double* plla);


LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);

double CalDistance(LatLng pointA, LatLng pointB);

void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]);
/**----------------------------------------------------------------------
* Function    : pdr_invSqrt
* Description : 计算1/sqrt(x)的快速的算法
* Date        : 2020/6/30 logzhan
*---------------------------------------------------------------------**/
float InvSqrt(float x);

/**---------------------------------------------------------------------
* Function    : pdr_v3Norm
* Description : 三维浮点数向量归一化
* Date        : 2021/01/26 yuanlin@vivocom && 
* 
* 
*---------------------------------------------------------------------**/
void pdr_v3Norm(float* vx, float* vy, float* vz);

/**---------------------------------------------------------------------
* Function    : quatNorm
* Description : 四元数归一化
* Date        : 2021/01/26 yuanlin@vivocom && logzhan
*               2021/01/27 logzhan : 优化了代码逻辑，防止当四元数
*               全为0时产生的异常情况。
*---------------------------------------------------------------------**/
void QuatNorm(float* q0, float* q1, float* q2, float* q3);

/**----------------------------------------------------------------------
* Function    : pdr_writeCsvStr
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void WriteCsvStr(FILE* fp_write, char* str);

/**----------------------------------------------------------------------
* Function    : pdr_utc2hms
* Description : 将UTC时间转换为时分秒
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void pdr_utc2hms(double utc, double* h, double* m, double* s);

/**----------------------------------------------------------------------
* Function    : qnb2att
* Description : 四元数转欧拉角
* Date        : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
void Qnb2Att(float* q, double* attitude);

double CalRadianDifferent(double s_dir, double d_dir);

/**----------------------------------------------------------------------
* Function    : pdr_earthParameter
* Description : 根据输入纬度返回该纬度地球相关参数
* Date        : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
EarthData_t CalEarthParameter(double oriLat);


/**----------------------------------------------------------------------
* Function    : Motion2TypeStr
* Description : 把用户运动类型转换为字符串输出
* Date        : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
const char* Motion2TypeStr(int type);

int pdr_min(int param_a, int param_b);

#ifdef __cplusplus
}
#endif

#endif