logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.04/src/Location.c

389 lines
13 KiB
C
Raw Normal View History

优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_location.c
* Current Version : V1.2
* Author : logzhan
* Date of Issued : 2020.7.3
* Comments : PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <malloc.h>
#include <errno.h>
#include <stdlib.h>
#include <stdbool.h>
#include "Location.h"
#include "Kalman.h"
#include "Pedometer.h"
#include "AHRS.h"
#include "pdr_detector.h"
#include "Utils.h"
#include "scene_recognition.h"
#include "CoordTrans.h"
/* Global Variable Definition ------------------------------------------------*/
PDR_t* pdr; // PDRȫ<52><C8AB><EFBFBD><EFBFBD>Ϣ
StepPredict stepPredict = { 0 };
classifer sys;
GNSS_t pgnss; // <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
EKFPara_t g_kfPara;
double GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status;
extern AHRS_t g_ahrs;
double refGpsYaw = -1000;
double yaw_bias = -1;
float dir[2] = { 0 };
float bias_dir[2] = { 0 };
/**---------------------------------------------------------------------
* Function : PDRNav_Init
* Description : PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵͳ<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void PDRNav_Init(void) {
memset(&g_kfPara, 0, sizeof(g_kfPara));
memset(&pgnss, 0, sizeof(pgnss));
memset(GpsPos, 0, sizeof(GpsPos));
memset(&stepPredict, 0, sizeof(stepPredict));
memset(&sys, 0, sizeof(sys));
memset(dir, 0, sizeof(dir));
memset(bias_dir, 0, sizeof(bias_dir));
// GNSS<53><53>λ<EFBFBD><CEBB>ʼ<EFBFBD><CABC>
InitGnssInfo();
// <20><>ʼ<EFBFBD><CABC>AHRS<52><53>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
AHRS_Init();
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
Detector_Init();
// PDR<44><52>ʼ<EFBFBD><CABC>
pdr = Base_Init();
//base<73>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
EKF_Init();
// <20><><EFBFBD><EFBFBD>ʶ<EFBFBD><CAB6>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
SceneRecog_Init();
yaw_bias = -1;
}
/**---------------------------------------------------------------------
* Function : InitGnssInfo
* Description : PDS GNSS<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
* Date : 2020/02/01 logzhan:
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD>汾
* 2020/02/08: logzhan <EFBFBD>޸<EFBFBD>initGpsΪinitGnssInfo<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD>ӷ<EFBFBD><EFBFBD>Ϻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
void InitGnssInfo(void) {
pgnss.lastError = ACCURACY_ERR_MAX;
pgnss.overVelCount = 0;
}
/**---------------------------------------------------------------------
* Function : ResetSystemStatus
* Description : PDR <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵͳ״̬
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void ResetSystemStatus(EKFPara_t *kf)
{
memset(kf, 0, sizeof(EKFPara_t));
pdr->fusionPdrFlg = PDR_FALSE;
pdr->sysStatus = IS_INITIAL;
pgnss.overVelCount = 0;
}
/**---------------------------------------------------------------------
* Function : InsLocation
* Description : PDR <EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
// AHRS<52><53>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ռ2%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
if (UpdateAHRS(ImuData)) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ƺ<EFBFBD><C6BA><EFBFBD><EFBFBD><EFBFBD>PCA<43><41> ռ26%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
ComputePCA(&g_ahrs);
}
// <20><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>IMU<4D><55>Ϣ<EFBFBD>Լ<EFBFBD><D4BC>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>, ռ21%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
DetectorUpdateIMU(ImuData);
pdr->gyroTime = ImuData->gyr.time;
// Ϊʲô<CAB2>ֳ<EFBFBD><D6B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>Ƶ<EFBFBD>ʣ<EFBFBD>
if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f;
// <20>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>봫<EFBFBD><EBB4AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>ռ10%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
PedometerUpdate(ImuData, &pdr->steps);
// ----- Filter Initialization ------- //
if (pdr->sysStatus == IS_INITIAL) {
pdr->ts = ImuData->gyr.time;
pdr->lastSteps = pdr->steps;
stepPredict.lastTime = 0;
stepPredict.fnum = 0;
stepPredict.fsum = 0;
return 0;
}
pdr->ts = ImuData->gyr.time;
if ((0 == ImuData->acc.s[0] && 0 == ImuData->acc.s[1] && 0 == ImuData->acc.s[2]) ||
(0 == ImuData->mag.s[0] && 0 == ImuData->mag.s[1] && 0 == ImuData->mag.s[2])) {
return 0;
}
// ʶ<><CAB6><EFBFBD><EFBFBD>ǰ״̬<D7B4>Ƿ<EFBFBD>ƽ<EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵͳ10%<25>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD>, <20>Ժ<EFBFBD><D4BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD>
StateRecognition(ImuData->acc.s, &sys);
//pdr->heading = CalPredHeading(...);
double imuYaw = - g_ahrs.yaw;
if (imuYaw < 0) {
imuYaw += 360;
}
pdr->heading = imuYaw / (180 / 3.14159265);
if (fabs(pdr->insHeadingOffset) > 0.000001) {
imuYaw = imuYaw - pdr->insHeadingOffset * (180 / 3.14159265);
pdr->heading = imuYaw / (180 / 3.14159265);
}
// PDR <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>Ԥ<EFBFBD><D4A4>
InsLocationPredict(pdr, &stepPredict, ImuData, &pgnss, Ekf);
pdr->lastSteps = pdr->steps;
pdr->lastHeading = pdr->heading;
return 0;
}
/**----------------------------------------------------------------------
* Function : GnssInsFusionLocation
* Description : PDR GPS<EFBFBD>ں<EFBFBD>INS<EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD>λ
* Date : 2021/01/29 logzhan
*---------------------------------------------------------------------**/
int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result)
{
// GPS<50><53><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
GnssUpdate(&pgnss, nmea_data);
// <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE;
// <20><><EFBFBD><EFBFBD>nmea<65><61><EFBFBD><EFBFBD>ʶ<EFBFBD>𳡾<EFBFBD><F0B3A1BE>Ƿ<EFBFBD><C7B7>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ճ<EFBFBD><D5B3><EFBFBD>
pdr->sceneType = isOpenArea(nmea_data);
// <20><><EFBFBD><EFBFBD>HDOP<4F><50><EFBFBD>ٶȡ<D9B6><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>gps yaw<61>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
pdr->lastGpsHeading = pdr->gpsHeading;
pdr->gpsHeading = GnssCalHeading(&pgnss);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>win32ƽ̨<C6BD>·<EFBFBD><C2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#ifdef _WIN32
优化卡尔曼滤波器的命名
2022-10-10 23:41:17 +08:00
result->pdrHeading = kf->pXk[3];
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
result->gpsHeading = pdr->gpsHeading;
result->hdop = pgnss.HDOP;
result->accuracy = nmea_data->accuracy.err;
result->gpsSpeed = pdr->gpsSpeed;
result->motionType = pdr->motionType;
#endif
CalPdrHeadingOffset(nmea_data, pdr);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPSʱ<53><CAB1>֮<EFBFBD>䣬<EFBFBD><E4A3AC><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD><E4BBAF>
pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps;
pdr->lastGpsSteps = pdr->steps;
int mode = DetectFixMode(&pgnss, kf, pdr, result);
if (mode != TYPE_FIX_PDR)return mode;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʷGPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD>GPS<50>켣<EFBFBD><ECBCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss);
// <20><><EFBFBD><EFBFBD>ϵͳû<CDB3>г<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȳ<EFBFBD>ʼ<EFBFBD><CABC>
if (pdr->sysStatus == IS_INITIAL) {
return InitProc(&pgnss, kf, pdr, result);
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>û<EFBFBD>дﵽ<D0B4><EFB5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD>켣
if (!pdr->fusionPdrFlg)return ResetOutputResult(&pgnss, pdr, kf, result);
// <20><><EFBFBD><EFBFBD>GPS<50><53>Ϣ, <20><><EFBFBD><EFBFBD>kf<6B>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͹۲<CDB9><DBB2><EFBFBD><EFBFBD><EFBFBD>
EKFCalQRMatrix(nmea_data, pdr, &sys, kf);
if (pgnss.satellites_num > 4){
// <20><><EFBFBD><EFBFBD>GPS<50><53><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD><EFBFBD><EFBFBD>Ч״̬<D7B4><CCAC><EFBFBD>ٶ<EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD><EFBFBD>Ч״̬
if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) {
//if(pdr->heading != INVAILD_GPS_YAW){
// pgnss.yaw = (float)(R2D(pdr->heading));
//}else if (pdr->trackHeading != INVAILD_GPS_YAW) {
// pgnss.yaw = (float)(R2D(pdr->trackHeading));
//}
优化卡尔曼滤波器的命名
2022-10-10 23:41:17 +08:00
pgnss.yaw = (float)kf->pXk[3];
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
if (pgnss.yaw != INVAILD_GPS_YAW) {
GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
return TYPE_FIX_PDR;
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼGPS
if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 &&
nmea_data->accuracy.err < 15) {
OutputOriginGnssPos(&pgnss, result);
pdr->NoGnssCount = 0;
return TYPE_FIX_GPS;
}
// <20><><EFBFBD><EFBFBD>û<EFBFBD><C3BB>GPS<50>źţ<C5BA><C5A3><EFBFBD>ôPDRҲ<52><D2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>
if (pdr->NoGnssCount < MAX_NO_GPS_PREDICT) {
// <20><>GPS״̬λ<CCAC><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
NoGnssInfoPredict(kf, result, pdr);
return TYPE_FIX_PDR;
}
pdr->NoGnssCount++;
return 0;
}
/**----------------------------------------------------------------------
* Function : NoGnssInfoPredict
* Description : <EFBFBD><EFBFBD>û<EFBFBD><EFBFBD>gps<EFBFBD><EFBFBD>ϢʱԤ<EFBFBD><EFBFBD>GPSλ<EFBFBD><EFBFBD>
* Date : 2022/09/16
*---------------------------------------------------------------------**/
void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* p_pdr)
{
double ned[3] = {0}, lla[3] = {0};
优化卡尔曼滤波器的命名
2022-10-10 23:41:17 +08:00
ned[0] = kf->pXk[0] - p_pdr->x0;
ned[1] = kf->pXk[1] - p_pdr->y0;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
ned[2] = 0;
NED2WGS84(p_pdr->pllaInit, ned, lla);
result->latitude = lla[0];
result->latitude_ns = pgnss.lat_ns;
result->longitudinal = lla[1];
result->longitudinal_ew = pgnss.lon_ew;
result->time = pgnss.timestamps;
p_pdr->NoGnssCount++;
}
/**----------------------------------------------------------------------
* Function : calPdrHeadingOffset
* Description : <EFBFBD><EFBFBD><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><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD>
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR_t* p_pdr) {
//<2F>ߵ<EFBFBD><DFB5><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>GPS<50>н<EFBFBD><D0BD>ǶȵĶ<C8B5>׼<EFBFBD><D7BC><EFBFBD>ж<EFBFBD>GPS<50>ź<EFBFBD><C5BA>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC>ߵ<EFBFBD><DFB5>Ƿ<EFBFBD>ƽ<EFBFBD>ȣ<EFBFBD>
if (refGpsYaw != -1000)return;
if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0)
&& (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4)
&& (pdr_detStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2))
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>GPS<50><53>׼part1 start*/
double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]),
1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3]));
if (imuHeading < 0) {
imuHeading += TWO_PI;
}
if (fabs(imuHeading - p_pdr->gpsHeading) < PI / 18)
{
//printf("offset <20>궨\n");
//p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading;
}
}
}
/**----------------------------------------------------------------------
* Function : pdr_detectFixMode
* Description : <EFBFBD><EFBFBD><EFBFBD>⵱ǰPDR<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD><EFBFBD>غ;<EFBFBD>ֹģʽ,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* ԭʼGPS<EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2020/07/08 logzhan : <EFBFBD>޸<EFBFBD>-1ΪINVAILD_GPS_YAW<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀɶ<EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, lct_fs* result) {
// <20><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ھ<EFBFBD>ֹ״̬
if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) {
if (pgnss->error > 0 && pgnss->error < 15 &&
pgnss->lastError >= pgnss->error) {
OutputOriginGnssPos(pgnss, result);
pgnss->lastError = pgnss->error;
return TYPE_FIX_PDR;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
return TYPE_FIX_NONE;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC>⵽<EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPSλ<53><CEBB>
if (DetectCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⵽<EFBFBD><E2B5BD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR״̬<D7B4><CCAC><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD>³<EFBFBD>ʼ<EFBFBD><CABC>
ResetSystemStatus(kf);
if (pgnss->error >= 0 || (pgnss->satellites_num > 4
&& pgnss->HDOP < 2.0)) {
OutputOriginGnssPos(pgnss, result);
return TYPE_FIX_GPS;
}
// <20><><EFBFBD>ȹ<EFBFBD><C8B9><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
return TYPE_FIX_NONE;
}
return TYPE_FIX_PDR;
}
/**----------------------------------------------------------------------
* Function : GnssCalHeading
* Description : <EFBFBD><EFBFBD>ȡGPSƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>(0 - 360<EFBFBD><EFBFBD>)
* Date : 2022/09/16 logzhan
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀɶ<EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
double GnssCalHeading(GNSS_t* pgnss) {
double gnssHeading = INVAILD_GPS_YAW;
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0
&& pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) {
gnssHeading = pgnss->yaw * RADIAN_PER_DEG;
}
return gnssHeading;
}
/**----------------------------------------------------------------------
* Function : pdr_nmea2Gnss
* Description : nmea<EFBFBD><EFBFBD><EFBFBD>ݽṹתgnss<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss)
{
lct_nmea_rmc rmc = { 0 };
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
rmc = nmea_data->rmc[0];
}else {
rmc = nmea_data->rmc[1];
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ε<EFBFBD>Accuracy Error
pgnss->lastError = pgnss->error;
pgnss->lat = rmc.latitude * PI / 180;
pgnss->lat_ns = rmc.rmc_latitude_ns;
pgnss->lon = rmc.longitudinal * PI / 180;
pgnss->lon_ew = rmc.longitudinal_ew;
//pgnss->vel = rmc.speed * GPS_SPEED_UNIT;
pgnss->vel = rmc.speed;
pgnss->HDOP = nmea_data->gsa[0].hdop;
pgnss->yaw = rmc.cog;
pgnss->quality = 0;
pgnss->error = nmea_data->accuracy.err;
pgnss->timestamps = rmc.time;
pgnss->satellites_num = nmea_data->gga.satellites_nb;
}
/**----------------------------------------------------------------------
* Function : GnssUpdate
* Description : nmea<EFBFBD><EFBFBD><EFBFBD>ݽṹתgnss<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2020/07/08 logzhan
* 2020/02/09 logzhan : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧԭpdr_gpsUpdate<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>÷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>¿<EFBFBD><EFBFBD><EFBFBD>gnss<EFBFBD>ṹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>nmea<EFBFBD>ṹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵû
* ʲô<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
void GnssUpdate(GNSS_t* gps, lct_nmea* nmea) {
// nmea<65><61><EFBFBD><EFBFBD>תgnss<73><73><EFBFBD>ݽṹ
Nmea2Gnss(nmea, &pgnss);
// GPS<50>ص<EFBFBD><D8B5><EFBFBD><EFBFBD>£<EFBFBD><C2A3><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gpsUpdateCb(gps);
}