118 lines
3.2 KiB
C
118 lines
3.2 KiB
C
|
#ifndef _PDR_BASE_H_
|
|||
|
|
#define _PDR_BASE_H_
|
|||
|
|
|
|||
|
|
#include <stdint.h>
|
|||
|
|
|
|||
|
|
#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 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 IMU_SENSOR_AXIS 3 // IMU<4D><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>3
|
|||
|
|
// <20>û<EFBFBD><C3BB>˶<EFBFBD>ʶ<EFBFBD><CAB6>
|
|||
|
|
#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 DETECTOR_NO_ERROR 0
|
|||
|
|
|
|||
|
|
#define TYPE_FIX_NONE 0
|
|||
|
|
|
|||
|
|
#define PDR_TRUE 1
|
|||
|
|
#define PDR_FALSE 0
|
|||
|
|
#define NO_UPDATE 0
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
double Xk[N]; // ϵͳ״̬<D7B4><CCAC><EFBFBD><EFBFBD> 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[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; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}EKFPara_t;
|
|||
|
|
|
|||
|
|
typedef struct Sensor {
|
|||
|
|
uint8_t update;
|
|||
|
|
int type;
|
|||
|
|
double time;
|
|||
|
|
float s[IMU_SENSOR_AXIS];
|
|||
|
|
}Sensor_t;
|
|||
|
|
|
|||
|
|
typedef struct IMU {
|
|||
|
|
Sensor_t acc;
|
|||
|
|
Sensor_t gyr;
|
|||
|
|
Sensor_t mag;
|
|||
|
|
}IMU_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;
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef struct PDR {
|
|||
|
|
uint32_t Status; // PDR<44><52>ǰ״̬
|
|||
|
|
uint32_t MotionType; // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
// <20>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double GnssSpeed; // GNSS<53>ٶ<EFBFBD>
|
|||
|
|
double Heading; // <20><><EFBFBD><EFBFBD>
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
uint64_t Steps; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
uint64_t LastSteps; // <20><>һ<EFBFBD>εIJ<CEB5><C4B2><EFBFBD>
|
|||
|
|
} PDR_t;
|
|||
|
|
|
|||
|
|
typedef struct AHRS {
|
|||
|
|
uint8_t status;
|
|||
|
|
uint8_t stable; // <20><>ǰAHRS<52>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>
|
|||
|
|
float error;
|
|||
|
|
float q[4];
|
|||
|
|
float gravity[3];
|
|||
|
|
float x_axis[3];
|
|||
|
|
float y_axis[3];
|
|||
|
|
float z_axis[3];
|
|||
|
|
float Dt;
|
|||
|
|
float Kp; // mahony kp<6B><70><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>
|
|||
|
|
float Yaw;
|
|||
|
|
float Pitch;
|
|||
|
|
float Roll;
|
|||
|
|
float insHeading;
|
|||
|
|
}AHRS_t;
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
uint8_t Update;
|
|||
|
|
double MinTime;
|
|||
|
|
double MaxTime;
|
|||
|
|
}Nmea_t;
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
//LL_NSEW latitude_ns;
|
|||
|
|
//LL_NSEW longitudinal_ew;
|
|||
|
|
double latitude;
|
|||
|
|
double longitudinal;
|
|||
|
|
double gpsLat;
|
|||
|
|
double gpsLon;
|
|||
|
|
double gpsHeading;
|
|||
|
|
double pdrHeading;
|
|||
|
|
double hdop;
|
|||
|
|
double gpsSpeed;
|
|||
|
|
double accuracy;
|
|||
|
|
double time;
|
|||
|
|
double yaw;
|
|||
|
|
double lambda;
|
|||
|
|
double last_lat;
|
|||
|
|
double last_lon;
|
|||
|
|
unsigned long step;
|
|||
|
|
uint8_t motionType;
|
|||
|
|
}LctFs_t;
|
|||
|
|
|
|||
|
|
#endif // ! _PDR_BASE_H
|