167 lines
5.2 KiB
C
167 lines
5.2 KiB
C
#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
|
||
|