PDR/1.Software/PDR 1.04/include/Utils.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_t;

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

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

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

float fmean(float *data, int len);

float stdf(float* data, int len);

double meanAngle(double* angle, int len);

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

double pow_i(double num, long long n);

double pow_f(double num, double m);

double pow(double x, double y);

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

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

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


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

double CalDistance(LatLng_t pointA, LatLng_t pointB);

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

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

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

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

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

double CalRadianDifferent(double s_dir, double d_dir);

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


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

int pdr_min(int param_a, int param_b);

#ifdef __cplusplus
}
#endif

#endif