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

314 lines
7.2 KiB
C
Raw Normal View History

优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/******************** (C) COPYRIGHT 2021 Geek************************************
* File Name : pdr_sensor.h
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2021.02.01
* Comments : PDR<EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
#ifndef _SENSOR_H_
#define _SENSOR_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/* Macro Definition ----------------------------------------------------------*/
#define IMU_SENSOR_AXIS 3
#ifdef _WIN32
#define PDR_LOGD printf
#define PDR_LOGE printf
#else
#include <android/log.h>
#define LOG_TAG "Loc_PDR"
#define PDR_LOGD(...) __android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__)
#define PDR_LOGE(...) __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)
#endif
#define ADD_POINT_NUM 10
#define GGA_STR "GGA"
#define RMC_STR "RMC"
#define GSA_STR "GSA"
#define GSV_STR "GSV"
#define GNGGA_STR "$GNGGA"
#define GPGGA_STR "$GPGGA"
#define GNRMC_STR "$GNRMC"
#define GPRMC_STR "$GPRMC"
#define GNGSA_STR "$GNGSA"
#define GPGSA_STR "$GPGSA"
#define GAGSA_STR "$GAGSA"
#define GLGSA_STR "$GLGSA"
#define GPGSV_STR "$GPGSV"
#define GAGSV_STR "$GAGSV"
#define GLGSV_STR "$GLGSV"
#define NORTH "N"
#define SOUTH "S"
#define EAST "E"
#define WEST "W"
#define ITEM_INVALID (-1)
#define INVAILD_GPS_YAW (-1) // <20><>ЧGPS<50><53><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
/* Struct Definition ---------------------------------------------------------*/
typedef enum {
SENSOR_MIN,
SENSOR_ACC = 1,
SENSOR_MAG = 2,
SENSOR_GYRO = 4,
SENSOR_LIGHT = 5,
SENSOR_PROXIMITY = 8,
SENSOR_GAME_ROTATION_VECTOR = 15,
SENSOR_LOCATION_STATELLITE = 70,
SENSOR_LOCATION_NETWORK = 71,
SENSOR_LOCATION_FUSION = 72,
SENSOR_LOCATION_NMEA = 73,
SENSOR_LOCATION_NMEA_GNGGA = 74,
SENSOR_LOCATION_NMEA_GPGGA = 75,
SENSOR_LOCATION_NMEA_GNRMC = 76,
SENSOR_LOCATION_NMEA_GPRMC = 77,
SENSOR_LOCATION_NMEA_GNGSA = 78,
SENSOR_LOCATION_NMEA_GPGSA = 79,
SENSOR_LOCATION_NMEA_GPGSV = 80,
SENSOR_LOCATION_NMEA_GLGSV = 81,
SENSOR_LOCATION_NMEA_GAGSV = 82,
SENSOR_LOCATION_NMEA_GLGSA = 83,
SENSOR_LOCATION_NMEA_GAGSA = 84,
SENSOR_MAX,
} sensor;
typedef enum {
POSITIONING_N = 0,
POSITIONING_Y = 1,
}lct_status;
typedef enum {
POSITIONING_MODE_A = 0,
POSITIONING_MODE_M = 1,
}lct_mode;
typedef enum PDR_Mode {
RAW_GPS = 1,
STOP = 2,
WALK_PDR = 3,
WALK_GPS = 4,
CAR = 5
} PDR_Mode;
typedef enum DIR_Mode {
GPS_DATASHEET = 0,
IMU_PCA = 1,
DELTA_LOCATION = 2
} DIR_Mode;
/*for GSA*/
typedef enum {
PSTN_GSA_N = 1,
PSTN_GSA_Y_2D = 2,
PSTN_GSA_Y_3D = 3,
}lct_type;
typedef enum {
LATITUDE_N = 0,
LATITUDE_S = 1,
LONGITUDINAL_E = 2,
LONGITUDINAL_W = 3,
}LL_NSEW;
typedef enum {
MAG_E = 0,
MAG_W = 1,
}mag_heading;
typedef enum {
MODE_A = 0,
MODE_D = 1,
MODE_E = 2,
MODE_N = 3,
}gnrmc_mode;
typedef enum {
invalid = 0,
fix = 1,
dgps_fix = 2,
invalid_pps = 3,
rtk_fix = 4,
rtk_float = 5,
estimating = 6,
}gngga_quality;
typedef struct Sensor {
unsigned char 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;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
typedef struct NmeaRMC_t {
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
unsigned char update;
char rmc_check_sum[9];
sensor type;
lct_status status;
LL_NSEW rmc_latitude_ns;
LL_NSEW longitudinal_ew;
mag_heading mag_h;
gnrmc_mode mode;
float speed;
float cog;
float utc_ddmmyy;
float magnetic;
double rmc_utc;
double latitude;
double longitudinal;
double time; //ms
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}NmeaRMC_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
typedef struct NmeaGGA{
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
unsigned char update;
char gga_check_sum[9];
sensor type;
LL_NSEW gga_latitude_ns;
LL_NSEW longitudinal_ew;
gngga_quality status;
int satellites_nb;
int unit_a;
int unit_h;
int dgps;
float hdop;
float altitude;
float height;
double gga_utc;
double latitude;
double longitudinal;
double time; //ms
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}NmeaGGA_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
typedef struct NmeaGSA {
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
unsigned char update;
char check_sum[9];
sensor type;
lct_mode mode;
lct_type p_type;
int prnflg;
int prn[12];
float pdop;
float hdop;
float vdop;
double time; //ms
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}NmeaGSA_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
typedef struct satellite_status {
int prn;
int elevation; // elevation >10
int azimuth;
int snr; //snr >25 good
}satellite_status;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
typedef struct NmeaGSV {
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
unsigned char update;
sensor type;
int sentences;
int sentence_number;
int satellites;
int satellite_number;
satellite_status satellites_data[20];
double time; //ms
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}NmeaGSV_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
typedef struct Accuracy{
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
unsigned char update;
int status; //-1,:Status Unknown; 0: Out of Service; 1: Temporarily Unavailable; 2: Available
float err; // m
double time;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}Accuracy_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
typedef struct Nmea_t {
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
unsigned char update;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
double minTime;
double maxTime;
NmeaGGA_t gga;
NmeaRMC_t rmc[2]; // 0 - GNRMC, 1 - GPRMC
NmeaGSA_t gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA
NmeaGSV_t gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV
Accuracy_t accuracy;
}Nmea_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
typedef struct lct_fs {
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;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}LctFs_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
typedef struct gnss {
LL_NSEW lat_ns;
LL_NSEW lon_ew;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
int minElevation;
int satellites_num; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int quality;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
float yaw; // GNSS <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float vel; // GNSS <20>ٶ<EFBFBD>
float HDOP; // GNSS ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float error; // <20><>ǰGPS<50><53>Accuracy
float lastError; // <20><>һ<EFBFBD><D2BB>GPS<50><53>Accuracy
int overVelCount; // <20>ٶȹ<D9B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͳ<EFBFBD><CDB3>, <20><>¼<EFBFBD>ٶȹ<D9B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>ֻ<EFBFBD><D6BB>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ڳ<EFBFBD><DAB3><EFBFBD>ģʽ
double timestamps;
double lat;
double lon;
double xNed;
double yNed;
}GNSS_t;
typedef struct IMU {
double time;
float gyr[3];
float acc[3];
float mag[3];
} IMU;
typedef struct AHRS {
uint8_t status;
uint8_t stable; // <20><>ǰAHRS<52>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>
float error;
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
float qnb[4];
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
float gravity[3];
float x_axis[3];
float y_axis[3];
float z_axis[3];
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
float Kp; // mahony kp<6B><70><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>
float Yaw;
float Pitch;
float Roll;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
float insHeading;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}AHRS_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
#ifdef __cplusplus
}
#endif
#endif