logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.04 - bak/src/Main.cpp

460 lines
16 KiB
C++
Raw Normal View History

优化PDR框架结构
2022-09-22 23:41:37 +08:00
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_main.cpp
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2021.1.28
* Comments : PDR PC<EFBFBD>˷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
* Modify : 2021/02/04 logzhan : <EFBFBD>Ż<EFBFBD>kml<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>kml<EFBFBD><EFBFBD><EFBFBD>Ӻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>hdop<EFBFBD><EFBFBD>heading<EFBFBD><EFBFBD>accuracy<EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD>Google Map<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD>Դ<EFBFBD><EFBFBD>롣
* 2021/02/18 logzhan : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>kml<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><EFBFBD><EFBFBD><EFBFBD>
*
* һ<EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͹<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 1) GPS<EFBFBD>ٶ<EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0.51444<EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>dz<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>GPS<EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD>
* 2) <EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>²<EFBFBD><EFBFBD><EFBFBD>buffer meanʹ<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>buffer mean<EFBFBD>ķ<EFBFBD><EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
* 2021/02/10 logzhan:
* 1) <EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD>Ʒ<EFBFBD><EFBFBD≯<EFBFBD><EFBFBD>ͨ<EFBFBD>ԱȰ汾<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD><EFBFBD>͸<EFBFBD>ͨSAP5.0<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) ĿǰGPS<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>0.51444<EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ط<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD>ͳһ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Ҫͳһ<EFBFBD><EFBFBD>
* 3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>²<EFBFBD><EFBFBD><EFBFBD>buffer meanʹ<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>buffer mean<EFBFBD>ķ<EFBFBD><EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
* 4)*Ŀǰ<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><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><EFBFBD><EFBFBD><EFBFBD>ֶΡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><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><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><EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD>ⷽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>׼ȷ<EFBFBD>̶ȡ<EFBFBD>
* 5)*<EFBFBD><EFBFBD>̬ƫ<EFBFBD>ƶ<EFBFBD>׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ὺʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PCA<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD><EFBFBD><EFBFBD>ôPCA<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><EFBFBD>̬<EFBFBD><EFBFBD>׼ƫ<EFBFBD>ơ<EFBFBD>
* 6<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><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>ƹ켣<EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><EFBFBD>ǼƲ<EFBFBD>ƫ<EFBFBD>٣<EFBFBD>
* 7<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><EFBFBD><EFBFBD>fft<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><EFBFBD><EFBFBD>Ժ󿪷<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD>Բο<EFBFBD>ԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><EFBFBD><EFBFBD>
* 8) <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><EFBFBD>Ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD>ij<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><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>Ư<EFBFBD><EFBFBD>)<EFBFBD><EFBFBD>
* 9) PDR Ŀǰһ<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><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>ǣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 10) <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><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><EFBFBD>ǰ<EFBFBD><EFBFBD>Ƶ<EFBFBD>ֻ<EFBFBD><EFBFBD>IJ<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>
* 11) ĿǰPDR<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>򲻹<EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>򣬿<EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԡ<EFBFBD>
* 12) kml֧<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><EFBFBD> ---- logzhan finish
*
* <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><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>
<EFBFBD>ڶ<EFBFBD><EFBFBD>ڣ<EFBFBD><EFBFBD>ֳ<EFBFBD><EFBFBD>ȶ<EFBFBD><EFBFBD>Ż<EFBFBD> + <EFBFBD><EFBFBD><EFBFBD><EFBFBD>(PCA + ƽ<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>ڣ<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>ȫ<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><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><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Է<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
#include <stdio.h>
#include <string>
#include <errno.h>
#include <stdlib.h>
#include "pdr_api.h"
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
#include "Main.h"
优化PDR框架结构
2022-09-22 23:41:37 +08:00
#include "Utils.h"
#include "time.h"
using namespace std;
/* Global Variable Definition ------------------------------------------------*/
ResultTracks resTracks;
FILE* SimFile = NULL;
/* Extern Variable Definition ------------------------------------------------*/
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
extern "C" PDR Pdr;
优化PDR框架结构
2022-09-22 23:41:37 +08:00
extern "C" double refGpsYaw;
/**----------------------------------------------------------------------
* Function : main
* Description : pdr<EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>main<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
int main(int argc, char* argv[])
{
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
string logPath = "E:\\SoftwareProject\\GnssIns\\PDR_Data\\Datasets\\";
string logDate = "shenzhen";
优化PDR框架结构
2022-09-22 23:41:37 +08:00
string logLabel = "data\\";
string fileHead;
char line[256] = { 0 };
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (int i = 0; i < argc; i++) {
string s = argv[i];
if(i == 1)logPath = s;
if(i == 2)logDate = s;
}
clock_t t = clock();
// <20><>ȡkml<6D><6C><EFBFBD><EFBFBD>·<EFBFBD><C2B7>
string kmlPath = logPath + logDate + "\\output\\";
// <20><><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>·<EFBFBD><C2B7><EFBFBD><EFBFBD>Catalog.txt·<74><C2B7>
string filePath = logPath + logDate + "\\" + logLabel + "\\";
string catalogPath = filePath + "catalog.txt";
FILE* catalogFp = fopen(catalogPath.c_str(), "r");
if (catalogFp == NULL){
printf("open catalog.txt file %s error %s\n", catalogPath.c_str(),
strerror(errno));
system("pause");
return -1;
}
while (!feof(catalogFp))
{
FILE* fileFp = getSimulateFile(catalogFp, filePath, fileHead);
// PDR <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Algorithm_Init();
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
LctFs_t locFusion;
优化PDR框架结构
2022-09-22 23:41:37 +08:00
memset(&locFusion,0, sizeof(locFusion));
memset(&resTracks, 0, sizeof(resTracks));
// <20><><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD><C4B1><EFBFBD><EFBFBD><EFBFBD>
while (fgets(line, 256, fileFp)){
// <20><><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD><C4B1><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD>
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
优化PDR框架结构
2022-09-22 23:41:37 +08:00
// <20><><EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD>켣<EFBFBD><ECBCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>kml<6D><6C><EFBFBD><EFBFBD>
UpdateResTrack(resTracks, locFusion);
}
}
// <20>ر<EFBFBD>PDR<44>㷨<EFBFBD><E3B7A8><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>Դ
Algorithm_DeInit();
// д<><D0B4><EFBFBD><EFBFBD>kml
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1c"));
优化PDR框架结构
2022-09-22 23:41:37 +08:00
// <20>ر<EFBFBD><D8B1>ļ<EFBFBD>
fclose(fileFp);
}
// <20><>ӡ<EFBFBD><D3A1><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ϣ
printf("PDR simulate spends %f s\n", ((double)(clock() - t) / CLOCKS_PER_SEC));
system("pause");
return 0;
}
/**----------------------------------------------------------------------
* Function : getSimulateFile
* Description : <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><EFBFBD>ļ<EFBFBD>ָ<EFBFBD><EFBFBD>
* catalogFp : Ŀ¼<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>ָ<EFBFBD><EFBFBD>
* path_file <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>Ŀ¼
* fileHead : ͨ<EFBFBD><EFBFBD>C++<EFBFBD><EFBFBD><EFBFBD>÷<EFBFBD><EFBFBD>ص<EFBFBD>ȥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead) {
char file_name[256] = { 0 };
FILE* fp = NULL;
if (fscanf(catalogFp, "%s\n", file_name) == -1){
printf("get data file name failed\n");
system("pause");
exit(-1);
}
string fileNamePath = path_file + string(file_name);
fp = fopen(fileNamePath.c_str(), "rb");
if (fp == NULL){
printf("open file %s failed error %s\n", fileNamePath.c_str(), strerror(errno));
system("pause");
exit(-1);
}
// ȥ<><C8A5><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>չ<EFBFBD><D5B9>.csv
fileHead = file_name;
fileHead = fileHead.substr(0, fileHead.length() - 4);
printf("%s\n", fileNamePath.c_str());
return fp;
}
/**----------------------------------------------------------------------
* Function : gpsYaw2GoogleYaw
* Description : Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>kml<EFBFBD><EFBFBD>ʾ<EFBFBD>ĽǶȷ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD>0-360˳ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Yawת<EFBFBD><EFBFBD>
* Ϊ<EFBFBD>ȸ<EFBFBD>֧<EFBFBD>ֵ<EFBFBD>Yaw<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
double gpsYaw2GoogleYaw(double heading) {
double gh = R2D(heading) - 180;
while (gh > 360)gh -= 360;
while (gh < 0)gh += 360;
return gh;
}
/**----------------------------------------------------------------------
* Function : UpdateResTrack
* Description : <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>PDR<EFBFBD>켣
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs)
优化PDR框架结构
2022-09-22 23:41:37 +08:00
{
resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude;
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
resTrack.pdrTrack[resTrack.pdrLen].heading =
gpsYaw2GoogleYaw(lctfs.pdrHeading);
resTrack.pdrTrack[resTrack.pdrLen].hdop = lctfs.hdop;
resTrack.pdrTrack[resTrack.pdrLen].accuracy = lctfs.accuracy;
resTrack.pdrTrack[resTrack.pdrLen].time = lctfs.time;
resTrack.pdrTrack[resTrack.pdrLen].motionType = lctfs.motionType;
resTrack.pdrLen++;
if (lctfs.gpsLat > 0 && lctfs.gpsLon > 0) {
resTrack.gpsTrack[resTrack.gpsLen].lat = lctfs.gpsLat;
resTrack.gpsTrack[resTrack.gpsLen].lon = lctfs.gpsLon;
resTrack.gpsTrack[resTrack.gpsLen].heading =
gpsYaw2GoogleYaw(lctfs.gpsHeading);
resTrack.gpsTrack[resTrack.gpsLen].vel = lctfs.gpsSpeed;
resTrack.gpsTrack[resTrack.gpsLen].hdop = lctfs.hdop;
resTrack.gpsTrack[resTrack.gpsLen].accuracy = lctfs.accuracy;
resTrack.gpsTrack[resTrack.gpsLen].time = lctfs.time;
resTrack.gpsLen++;
}
}
string time2str(double t) {
long time = (long)t;
if (t < 10) {
string str = "0" + std::to_string(time);
return str;
}else {
string str = std::to_string(time);
return str;
}
}
/**----------------------------------------------------------------------
* Function : KmlWrite
* Description : <EFBFBD><EFBFBD>pdr<EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>gps<EFBFBD><EFBFBD>pdr<EFBFBD>켣дΪkml<EFBFBD><EFBFBD>ʽ
* path : kml<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>·<EFBFBD><EFBFBD>
* name : kml<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* postfix <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><EFBFBD><EFBFBD><EFBFBD>ͻ<EFBFBD><EFBFBD>߰汾
* Date : 2020/11/1 logzhan
*---------------------------------------------------------------------**/
void KmlWrite(string path, string name, string postfix)
{
string pdrColor = "ff0000ff";
string gpsColor = "ff00ffff";
if (path == "") {
return;
}
string kmlPath = path + name + postfix + ".kml";
string kmlName = name + postfix;
FILE* fid = fopen(kmlPath.c_str(), "w");
fprintf(fid, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
fprintf(fid, "\t<Document>\n");
fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());
fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
fprintf(fid, "\t\t<name>GPS Fixs</name>\n");
// дgps<70><73><EFBFBD><EFBFBD>
for (int i = 0; i < resTracks.gpsLen; i++) {
fprintf(fid, "\t\t<Placemark>\n");
fprintf(fid, "\t\t\t<Style>\n");
fprintf(fid, "\t\t\t\t<IconStyle>\n");
fprintf(fid, "\t\t\t\t\t<Icon>\n");
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
fprintf(fid, "\t\t\t\t\t</Icon>\n");
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
// <20><><EFBFBD><EFBFBD><EFBFBD>켣<EFBFBD><ECBCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", resTracks.gpsTrack[i].heading);
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
fprintf(fid, "\t\t\t\t</IconStyle>\n");
fprintf(fid, "\t\t\t</Style>\n");
// д<><D0B4>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ϣ
double h, m, s;
pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s);
fprintf(fid, "\t\t\t<TimeStamp>\n");
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
time2str(m).c_str(), time2str(s).c_str());
fprintf(fid, "\t\t\t</TimeStamp>\n");
// д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
fprintf(fid, "\t\t\t<description>\n");
fprintf(fid, "\t\t\t\t<![CDATA[\n");
fprintf(fid, "\t\t\t\t<dl>\n");
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
resTracks.gpsTrack[i].hdop);
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
resTracks.gpsTrack[i].accuracy);
fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
resTracks.gpsTrack[i].vel);
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
resTracks.gpsTrack[i].heading);
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
fprintf(fid, "\t\t\t</description>\n");
// д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
fprintf(fid, "\t\t\t<Point>\n");
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
resTracks.gpsTrack[i].lon,
resTracks.gpsTrack[i].lat);
fprintf(fid, "\t\t\t</Point>\n");
fprintf(fid, "\t\t</Placemark>\n");
}
fprintf(fid, "\t\t</Folder>\n");
fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
// дpdr<64><72><EFBFBD><EFBFBD>
for (int i = 0; i < resTracks.pdrLen; i++) {
fprintf(fid, "\t\t<Placemark>\n");
fprintf(fid, "\t\t\t<Style>\n");
fprintf(fid, "\t\t\t\t<IconStyle>\n");
fprintf(fid, "\t\t\t\t\t<Icon>\n");
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
fprintf(fid, "\t\t\t\t\t</Icon>\n");
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n",
resTracks.pdrTrack[i].heading);
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
fprintf(fid, "\t\t\t\t</IconStyle>\n");
fprintf(fid, "\t\t\t</Style>\n");
// д<><D0B4>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ϣ
double h, m, s;
pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
fprintf(fid, "\t\t\t<TimeStamp>\n");
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
time2str(m).c_str(), time2str(s).c_str());
fprintf(fid, "\t\t\t</TimeStamp>\n");
// description
fprintf(fid, "\t\t\t<description>\n");
fprintf(fid, "\t\t\t\t<![CDATA[\n");
fprintf(fid, "\t\t\t\t<dl>\n");
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
resTracks.pdrTrack[i].hdop);
fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
Motion2TypeStr(resTracks.pdrTrack[i].motionType));
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
resTracks.pdrTrack[i].accuracy);
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
resTracks.pdrTrack[i].heading);
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
fprintf(fid, "\t\t\t</description>\n");
fprintf(fid, "\t\t\t<Point>\n");
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
resTracks.pdrTrack[i].lon,
resTracks.pdrTrack[i].lat);
fprintf(fid, "\t\t\t</Point>\n");
fprintf(fid, "\t\t</Placemark>\n");
}
fprintf(fid, "\t\t</Folder>\n");
fprintf(fid, "\t</Document>\n");
fprintf(fid, "</kml>\n");
fclose(fid);
}
#ifdef _WIN32
/**----------------------------------------------------------------------
* Function : PDR_Init
* Description : <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>PDR<EFBFBD>㷨(DLL<EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
* Date : 2022/09/15 logzhan
*---------------------------------------------------------------------**/
void LibPDR_Init() {
Algorithm_Init();
}
/**----------------------------------------------------------------------
* Function : PDR_StepUpdateGPS
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><EFBFBD>(DLL<EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
* Date : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
PosFusion LibPDR_StepUpdateGPS(int useGpsFlg)
{
PosFusion fusion;
fusion.vaild = 0;
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
Nmea_t location_nmea;
Nmea_t* ln = &location_nmea;
优化PDR框架结构
2022-09-22 23:41:37 +08:00
imu imu_data;
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
LctFs_t lct_fusion;
优化PDR框架结构
2022-09-22 23:41:37 +08:00
memset(&lct_fusion, 0, sizeof(lct_fusion));
//memset(&location_nmea, 0, sizeof(location_nmea));
memset(&imu_data, 0, sizeof(imu_data));
/*memset(&kf, 0, sizeof(kf));*/
char line[256] = { 0 };
while (fgets(line, 256, SimFile)) {
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
int flag = ParseDataAndUpdate(line, &lct_fusion);
优化PDR框架结构
2022-09-22 23:41:37 +08:00
if (flag) {
fusion.gpsLat = lct_fusion.gpsLat;
fusion.gpsLon = lct_fusion.gpsLon;
}
// дPDR<44>ںϽ<DABA><CFBD><EFBFBD>
if (flag) {
fusion.lat = lct_fusion.latitude;
fusion.lon = lct_fusion.longitudinal;
fusion.t = lct_fusion.time;
fusion.vaild = 1;
break;
}
}
return fusion;
}
/**----------------------------------------------------------------------
* Function : setSimulatorFileCsvPath
* Description : <EFBFBD><EFBFBD><EFBFBD>÷<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD>(DLL<EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setSimulatorFileCsvPath(char* path) {
if (SimFile == NULL) {
SimFile = fopen(path, "rb");
}
else {
fclose(SimFile);
SimFile = NULL;
}
}
/**----------------------------------------------------------------------
* Function : setRefGpsYaw
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>ο<EFBFBD><EFBFBD>Ƕ<EFBFBD>
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setRefGpsYaw() {
重构代码，删除冗余版本
2024-12-06 21:30:26 +08:00
refGpsYaw = Pdr.gpsHeading;
优化PDR框架结构
2022-09-22 23:41:37 +08:00
printf("ref Gps Yaw = %f\n", refGpsYaw);
//g_pdr.sysStatus = IS_INITIAL;
//g_pdr.fusionPdrFlg = ON;
}
/**----------------------------------------------------------------------
* Function : closePdrAlgo
* Description : <EFBFBD>ر<EFBFBD>pdr<EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ棬<EFBFBD><EFBFBD><EFBFBD><EFBFBD>dll<EFBFBD><EFBFBD><EFBFBD><EFBFBD>(DLL<EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void LibPDR_DeInit() {
Algorithm_DeInit();
if (SimFile != NULL) {
fclose(SimFile);
SimFile = NULL;
}
}
#endif