logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.05/include/PDRBase.h

233 lines
10 KiB
C
Raw Normal View History

优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
/******************** (C) COPYRIGHT 2021 Geek************************************
* File Name : pdr_base.h
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author : yuanlin@vivocom &
*
*
* Date of Issued : 2021.02.01
* Comments : PDR <EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD>ļ<EFBFBD>
********************************************************************************/
#ifndef _PDR_BASE_H_
#define _PDR_BASE_H_
/* Header File Including -----------------------------------------------------*/
#include "stdint.h"
#include "pdr_sensor.h"
/* PDR SYS CFG ---------------------------------------------------------------*/
#define PDR_OUTPUT_SMOOTH 0 // <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>켣ƽ<ECBCA3><C6BD>
/* Macro Definition ----------------------------------------------------------*/
#define PCA_NUM 200
#define ACCURACY_ERR_MAX 1000 // GPS<50><53>accuracy<63><79><EFBFBD><EFBFBD>ֵ,һ<><D2BB><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
#define N 4 // <20><><EFBFBD><EFBFBD>ά<EFBFBD><CEAC>
#define OPEN_SKY 1
#define MAX_NO_GPS_PREDICT 10 // <20><>GPS<50><53>Ϣ״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define UN_UPDATE 0 // <20>Ǹ<EFBFBD><C7B8><EFBFBD>״̬
#define DATA_UPDATE 1 // <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
#define ON 1
#define OFF 0
#define PDR_TRUE 1
#define PDR_FALSE 0
#define TYPE_FIX_NONE 0 // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define TYPE_FIX_GPS 2 // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ԭʼGPS
#define TYPE_FIX_PDR 1 // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪPDR<44>ں<EFBFBD>
// PDRЯ<52><D0AF><EFBFBD><EFBFBD>ʽ
#define UNKNOWN 0
#define FORWARD_UP_RIGHT 1 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F><72><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define BACKWARD_UP_LEFT 2 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define BACKWARD_DOWN_RIGHT 3 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F><72><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define FORWARD_DOWN_LEFT 4 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define LEFT_UP_FORWARD 5 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD>ǰ
#define RIGHT_UP_BACKWARD 6 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define RIGHT_DOWN_FORWARD 7 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD>ǰ
#define LEFT_DOWN_BACKWARD 8 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// PDR<44><52><EFBFBD><EFBFBD>״̬
#define PDR_NO_ERROR 0
#define PDR_OUT_OF_MEMORY 1
#define PDR_MOTION_TYPE_STATIC 0
#define PDR_MOTION_TYPE_IRREGULAR 1
#define PDR_MOTION_TYPE_HANDHELD 2
#define PDR_MOTION_TYPE_SWINGING 3
#define PDR_STATUS_RESET 0x80
#define PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40
#define PDR_STATUS_PCA_STABLE 0x20
#define PDR_STATUS_BIAS_STABLE 0x10
#define PDR_STATUS_STABLE (PDR_STATUS_PCA_STABLE | PDR_STATUS_BIAS_STABLE)
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>س<EFBFBD><D8B3><EFBFBD>
#define WGS84_RE 6378137 // WGS-84<38><34><EFBFBD><EFBFBD><EFBFBD>峤<EFBFBD><E5B3A4><EFBFBD><EFBFBD>
#define WGS84_ECCENTR2 6.69437999014e-3 // <20><>һƫ<D2BB><C6AB><EFBFBD>ʵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>e^2(WGS-84)
#define WGS84_OMEGA_E_DOT 7.2921151467e-5 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ٶ<EFBFBD> rad/sec
#define WGS84_GRAVITY 9.7803267714 // <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> m/s^2
#define DEG_PER_RADIAN 57.295779513082323 // degrees per radian
#define RADIAN_PER_DEG 0.017453292519943 // radians per degree
#define GPS_SPEED_UNIT 0.5144444 // GPSԭʼ<D4AD>ٶ<EFBFBD>תm/s<><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
#define PI 3.1415926
#define DOUBLE_ZERO 1e-10
#define TWO_PI (2.0*PI)
#define R2D(x) (x*57.2957795130823)
#define D2R(x) (x*3.14159265/180.0)
#define USE_BUG_FOR_LOCAL_PARA 1
#define BUF_DMT_1 1
#define BUF_DMT_2 2
#define PRINT_ALGO_RUN_TIME 0
#define PRINT_CLCT 1
#define PATTERN_RECOGNITION_LEN 256 // <20><><EFBFBD><EFBFBD>256 / SAMPLE_RATE<54><45><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define OPEN_AREA 1 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(GPS<50>źŽϺ<C5BD>)
#define UN_OPEN_AREA 0 // <20>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(GPS<50>źŽ<C5BA><C5BD><EFBFBD>)
#define DETECTOR_RUN_FREQ 1280 // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ms
#define DETECTOR_TYPE_STATIC 0 // <20>û<EFBFBD><C3BB><EFBFBD>ֹ
#define DETECTOR_TYPE_IRREGULAR 1 // <20>޹<EFBFBD><DEB9><EFBFBD><EFBFBD>˶<EFBFBD>
#define DETECTOR_TYPE_HANDHELD 2 // <20>ֳ<EFBFBD><D6B3>˶<EFBFBD>
#define DETECTOR_TYPE_SWINGING 3 // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
#define ulong unsigned long
#define uchar unsigned char
typedef enum {
IS_INITIAL = 0,
IS_NORMAL = 1,
}pdrStatus;
typedef struct {
double Xk[N]; // ״̬<D7B4><CCAC><EFBFBD><EFBFBD>Xk xk[0]: <20><><EFBFBD><EFBFBD>x xk[1]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>y xk[2]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> xk[3] <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double pXk[N]; // <20><><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Xk_predict xk[0]: <20><><EFBFBD><EFBFBD>x xk[1]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>y xk[2]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> xk[3] <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double Zk[N];
double pPk[N][N];
double Pk[N][N];
double Phi[N][N];
double hk[N][N];
double Q[N][N]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>Q<EFBFBD><51><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
double R[N][N]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>R<EFBFBD><52><EFBFBD><EFBFBD>(<28>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD>)
double Kk[N][N];
double Lambda;
double pLat;
double pLon;
double initHeading;
}EKFPara_t;
typedef struct{
int fnum;
int deltaStep;
float fsum;
float meanTime;
double lastTime;
}StepPredict;
typedef struct PDR {
uint32_t status; // PDR<44><52>ǰ״̬
uint32_t MotionType; // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
uint32_t NoGnssCount; // û<><C3BB>GNSS<53><53>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>
pdrStatus sysStatus; // PDRϵͳ״̬
int sceneType; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28>źŽ<C5BA><C5BD><EFBFBD>)
int fusionPdrFlg; // <20>ں<EFBFBD>PDRλ<52>ñ<EFBFBD>־λ, <20><>flgΪ0ʱ<30><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS
double PllaInit[3]; // <20><>ʼplla<6C><61><EFBFBD><EFBFBD>
double ts; // ʱ<><CAB1><EFBFBD><EFBFBD>
double x0; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double y0; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double heading; // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double lastHeading; // <20><>һ<EFBFBD><D2BB>PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double deltaHeading; // ƫ<><C6AB><EFBFBD>DZ仯<C7B1><E4BBAF>
double insHeadingOffset; // <20>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
double imuDeltaHeading; // û<><C3BB><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>GPS<50>źţ<C5BA>PDR<44>仯<EFBFBD>ĽǶȣ<C7B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
double GpsHeading; // GPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double LastGpsHeading; // <20><>һ<EFBFBD><D2BB>GPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
double trackHeading; // GPS<50>켣<EFBFBD><ECBCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
double GpsSpeed; // GPS<50>ٶ<EFBFBD>
优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ulong steps; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
ulong lastSteps; // <20><>һ<EFBFBD>εIJ<CEB5><C4B2><EFBFBD>
ulong lastGpsSteps; // <20><>һ<EFBFBD><D2BB>GPS<50><53><EFBFBD><EFBFBD>
ulong deltaStepsPerGps; // <20><><EFBFBD><EFBFBD>GPS<50><53><EFBFBD><EFBFBD>֮<EFBFBD>䣬<EFBFBD><E4A3AC><EFBFBD><EFBFBD><EFBFBD>ı仯<C4B1><E4BBAF>
float motionFreq; // <20>˶<EFBFBD>Ƶ<EFBFBD><C6B5>
double gyroTime; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
float axis_ceofficient[3];
float axis_direction[2];
float pca_direction[2];
float pcaAccuracy;
float bias_direction[2];
float biasAccuracy;
float cal_direction[2];
} PDR_t;
typedef struct {
int type;
float accBuffer[PATTERN_RECOGNITION_LEN];
}Classifer_t;
// <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD>
typedef struct DETECTOR {
uint32_t type; // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD> <20><> 0:<3A><>ֹ<EFBFBD>˶<EFBFBD> 1<><31><EFBFBD>޹<EFBFBD><DEB9><EFBFBD><EFBFBD>˶<EFBFBD> 2<><32><EFBFBD>ֳ<EFBFBD><D6B3>˶<EFBFBD> 3<><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
uint32_t lastType;
uint64_t tick; // <20><><EFBFBD><EFBFBD>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
}Detector_t;
/* Function Declaration ------------------------------------------------------*/
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
* Input : stepPredict<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
timestamp<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
steps_last<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
steps<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps);
/**************************************************************************
* Description : Ԥ<EFBFBD>ⲽ<EFBFBD><EFBFBD>
* Input : stepPredict<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
timestamp<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
steps_last<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
gnssVel<EFBFBD><EFBFBD>gps<EFBFBD>ٶ<EFBFBD>
* Output : int<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
int PredictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel);
优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
/**----------------------------------------------------------------------
* Function : stateRecognition
* Description : <EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><EFBFBD>ٶȼ<EFBFBD><EFBFBD>жϵ<EFBFBD>ǰ״̬<EFBFBD>Ƿ<EFBFBD>ƽ<EFBFBD><EFBFBD> 1: <EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD> 0: ƽ<EFBFBD><EFBFBD>
* Date : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void StateRecognition(float *acc, Classifer_t *sys);
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
/**----------------------------------------------------------------------
* Function : InsLocationPredict
* Description : PDR<EFBFBD>ߵ<EFBFBD>λ<EFBFBD>ø<EFBFBD><EFBFBD><EFBFBD>
* Date : 2021/02/06 logzhan : <EFBFBD>о<EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDRǰ<EFBFBD><EFBFBD>λ<EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu,
GNSS_t* pgnss, EKFPara_t* kf);
优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>켣<EFBFBD><EFBFBD>
* Input : gpsPos<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD>ʷ<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ECEF<EFBFBD>µ<EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
* Output : double<EFBFBD><EFBFBD><EFBFBD>켣<EFBFBD><EFBFBD>
**************************************************************************/
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
#endif
#pragma once