2022-09-15 16:35:25 +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 p_xk[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 p_pk[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;
|
2022-09-17 01:12:48 +08:00
|
|
|
|
}KfPara_t;
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
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>
|
2022-09-17 01:12:48 +08:00
|
|
|
|
uint32_t NoGnssCount; // û<><C3BB>GNSS<53><53>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>
|
2022-09-15 16:35:25 +08:00
|
|
|
|
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>
|
|
|
|
|
|
double gpsHeading; // GPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double lastGpsHeading; // <20><>һ<EFBFBD><D2BB>GPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double trackHeading; // GPS<50>켣<EFBFBD><ECBCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// <20>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double gpsSpeed; // GPS<50>ٶ<EFBFBD>
|
|
|
|
|
|
// <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 pca_accuracy;
|
|
|
|
|
|
float bias_direction[2];
|
|
|
|
|
|
float bias_accuracy;
|
|
|
|
|
|
float cal_direction[2];
|
|
|
|
|
|
} PDR;
|
|
|
|
|
|
|
|
|
|
|
|
typedef struct {
|
|
|
|
|
|
int type;
|
|
|
|
|
|
float accBuffer[PATTERN_RECOGNITION_LEN];
|
|
|
|
|
|
}classifer;
|
|
|
|
|
|
|
|
|
|
|
|
// <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;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
DETECTOR *pdr_getDetector(void);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* Function Declaration ------------------------------------------------------*/
|
|
|
|
|
|
|
2022-09-17 01:12:48 +08:00
|
|
|
|
PDR* Base_Init(void);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : pdr_resetData
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Date : 2020/7/21
|
|
|
|
|
|
*
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
|
|
|
|
|
void pdr_resetData(void);
|
|
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : pdr_computePCA
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>RESET_PHASE_1\STABLE<EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PCA<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Date : 2020/7/21 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
2022-09-17 01:12:48 +08:00
|
|
|
|
void ComputePCA(AHRS_t* ahrs);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**---------------------------------------------------------------------
|
|
|
|
|
|
* Function : detectorUpdate
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD>Я<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>g_pdr.status<EFBFBD><EFBFBD>
|
|
|
|
|
|
* Date : 2020/7/20 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
|
|
|
|
|
void detectorUpdate(DETECTOR* detector);
|
|
|
|
|
|
|
|
|
|
|
|
|
2022-09-17 01:12:48 +08:00
|
|
|
|
void gpsUpdateCb(GNSS_t* gps);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**************************************************************************
|
|
|
|
|
|
* 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>
|
|
|
|
|
|
**************************************************************************/
|
|
|
|
|
|
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel);
|
|
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* 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
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
2022-09-19 23:05:00 +08:00
|
|
|
|
void StateRecognition(float *acc, classifer *sys);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**************************************************************************
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>Ƕȡ<EFBFBD>GPS<EFBFBD>켣<EFBFBD>ǡ<EFBFBD>PDR<EFBFBD>Ƕȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Input : g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
sys<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
gps_yaw<EFBFBD><EFBFBD>GPS<EFBFBD>켣<EFBFBD><EFBFBD>
|
|
|
|
|
|
G_yaw<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
ss_data<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
* Output : double<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
**************************************************************************/
|
|
|
|
|
|
double calPredAngle(PDR *g_pdr, classifer *sys, double gps_yaw, double G_yaw, imu *ss_data);
|
|
|
|
|
|
|
|
|
|
|
|
/**************************************************************************
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>Ƕȡ<EFBFBD>GPS<EFBFBD>켣<EFBFBD>ǡ<EFBFBD>PDR<EFBFBD>Ƕȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Input : g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
steps<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
steps_last<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
stepPredict<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
pdr_angle<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
scene_type<EFBFBD><EFBFBD>GPS<EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
ss_data<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
kf<EFBFBD><EFBFBD>EKF<EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
pdr, PDRԤ<EFBFBD><EFBFBD>λ<EFBFBD>óɹ<EFBFBD><EFBFBD><EFBFBD>־λ
|
|
|
|
|
|
**************************************************************************/
|
2022-09-19 23:05:00 +08:00
|
|
|
|
void InsPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data,
|
2022-09-17 01:12:48 +08:00
|
|
|
|
GNSS_t *pgnss, KfPara_t *kf);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : pdr_detStatic
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ھ<EFBFBD>ֹ״̬
|
|
|
|
|
|
* Date : 2021/01/28 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
2022-09-17 01:12:48 +08:00
|
|
|
|
int pdr_detStatic(PDR *g_pdr, GNSS_t *pgnss, unsigned long delSteps);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : pdr_outputGpsPos
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPSλ<EFBFBD><EFBFBD>
|
|
|
|
|
|
* Date : 2020/07/08 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
2022-09-17 01:12:48 +08:00
|
|
|
|
void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : resetOutputResult
|
|
|
|
|
|
* Description : 1<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>
|
|
|
|
|
|
* 2<EFBFBD><EFBFBD>GPSֵ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>result
|
|
|
|
|
|
* pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
* plla_init<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD>㾭γ<EFBFBD><EFBFBD>
|
|
|
|
|
|
* x_init<EFBFBD><EFBFBD>y_init<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>n<EFBFBD><EFBFBD>e<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* kf<EFBFBD><EFBFBD> EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
* Date : 2020/07/08 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
2022-09-19 23:05:00 +08:00
|
|
|
|
int ResetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : detIsCarMode
|
|
|
|
|
|
* Description : ʶ<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD>dz<EFBFBD><EFBFBD><EFBFBD>ģʽ
|
|
|
|
|
|
* Input : pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
delSteps<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı仯<EFBFBD><EFBFBD>
|
|
|
|
|
|
time<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Output : int<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD>־λ
|
|
|
|
|
|
* Date : 2021/01/28 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
2022-09-17 01:12:48 +08:00
|
|
|
|
int DetectCarMode(GNSS_t *pgnss, PDR *g_pdr, unsigned long delSteps, int *time);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------
|
|
|
|
|
|
* Function : detPdrToReset
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDRϵͳ<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><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD>
|
|
|
|
|
|
* Date : 2021/01/28 logzhan
|
|
|
|
|
|
*---------------------------------------------------------------------**/
|
|
|
|
|
|
int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR *g_pdr);
|
|
|
|
|
|
|
|
|
|
|
|
/**************************************************************************
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>EKF<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Input : pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
plla_init<EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD>㾭γ<EFBFBD><EFBFBD>
|
|
|
|
|
|
kf<EFBFBD><EFBFBD> EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
x_init<EFBFBD><EFBFBD>y_init<EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>n<EFBFBD><EFBFBD>e<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
sys_status<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λϵͳ״̬<EFBFBD><EFBFBD>δ<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѳ<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* Output : int<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ
|
|
|
|
|
|
result<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
**************************************************************************/
|
2022-09-17 01:12:48 +08:00
|
|
|
|
int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr, lct_fs *result);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
/**************************************************************************
|
|
|
|
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>EKF<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Q<EFBFBD><EFBFBD>R
|
|
|
|
|
|
* Input : scane_type, <EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
nmea_data<EFBFBD><EFBFBD>NMEA<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
sys<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
kf<EFBFBD><EFBFBD>EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|
|
|
|
|
**************************************************************************/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**************************************************************************
|
|
|
|
|
|
* 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>
|
|
|
|
|
|
**************************************************************************/
|
2022-09-19 23:05:00 +08:00
|
|
|
|
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
|
2022-09-15 16:35:25 +08:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#pragma once
|
