140 lines
5.4 KiB
C
140 lines
5.4 KiB
C
#ifndef __PDR_LOCATION_H__
|
||
#define __PDR_LOCATION_H__
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include "PDRBase.h"
|
||
|
||
#define HIST_GPS_NUM 8
|
||
#define ACCURACY_THRES 0.6f // 精度阈值
|
||
#define YAW_THRES 10.0f // Yaw角范围阈值
|
||
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : NavSys_Init
|
||
* Description : PDR导航系统初始化
|
||
* Date : 2022/9/16
|
||
*---------------------------------------------------------------------**/
|
||
void PDR_Init(void);
|
||
|
||
void PDRInfoInit(void);
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : InsLocation
|
||
* Description : PDR 惯性导航定位
|
||
* Date : 2022/09/16 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
int InsLocationUpdate(IMU_t *ss_data, EKFPara_t *kf);
|
||
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_noGpsPredict
|
||
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
|
||
* Date : 2020/07/08 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* g_pdr);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : Nmea2Gnss
|
||
* Description : nmea数据结构转gnss数据
|
||
* Date : 2020/07/08 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss);
|
||
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_detectFixMode
|
||
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
|
||
* 原始GPS或者不输出
|
||
* Date : 2020/07/08 logzhan
|
||
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
|
||
* 代码的可读性
|
||
*---------------------------------------------------------------------**/
|
||
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : GnssCalHeading
|
||
* Description : 获取GPS偏航角(0 - 360°)
|
||
* Date : 2022/09/16 logzhan
|
||
* 代码的可读性
|
||
*---------------------------------------------------------------------**/
|
||
double GnssCalHeading(GNSS_t* pgnss);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : calPdrHeadingOffset
|
||
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
||
* Date : 2020/07/08 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr);
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : pdr_resetSysStatus
|
||
* Description : PDS 重设系统状态
|
||
* Date : 2020/2/1 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void ResetSystemStatus(EKFPara_t* kf);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_gnssInsLocation
|
||
* Description : PDR GPS融合INS惯性定位
|
||
* Date : 2021/01/29 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
int GnssInsFusionLocation(Nmea_t* nmea_data, EKFPara_t* kf, LctFs_t* result);
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : pdr_initGnssInfo
|
||
* Description : PDS GNSS定位信息初始化
|
||
* Date : 2020/2/1 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void InitGnssInfo(void);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : GnssUpdate
|
||
* Description : nmea数据结构转gnss数据
|
||
* Date : 2020/07/08 logzhan
|
||
* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数，
|
||
* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
|
||
* 什么用，还增加运算量
|
||
*---------------------------------------------------------------------**/
|
||
void GnssUpdate(GNSS_t* gps, Nmea_t* nmea);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : OutputRawGnssPos
|
||
* Description : 输出GPS位置
|
||
* Date : 2022/10/15 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : ResetOutputLoction
|
||
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
|
||
* 2、GPS值赋值给result
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result);
|
||
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : EkfGnssInsLocFusion
|
||
* Description : 采用EKF对GNSS和INS融合定位
|
||
* Date : 2022/09/21 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias,
|
||
EKFPara_t* kf, LctFs_t* res);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : PDRInitialAlignment
|
||
* Description : PDR初始对准
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#endif
|
||
|