From 2790501743a7127832c56c2243938224f1fa7db3 Mon Sep 17 00:00:00 2001 From: logzhan <719901725@qq.com> Date: Mon, 19 Sep 2022 23:05:00 +0800 Subject: [PATCH] =?UTF-8?q?=E9=87=8D=E6=9E=84=E5=87=BD=E6=95=B0=E5=91=BD?= =?UTF-8?q?=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 1.Software/PDR 1.02/include/pdr_ahrs.h | 2 +- 1.Software/PDR 1.02/include/pdr_base.h | 8 +- 1.Software/PDR 1.02/include/pdr_detector.h | 2 +- 1.Software/PDR 1.02/include/pdr_kalman.h | 23 ++- 1.Software/PDR 1.02/include/pdr_location.h | 4 +- 1.Software/PDR 1.02/include/pdr_main.h | 2 +- 1.Software/PDR 1.02/include/pdr_util.h | 4 +- 1.Software/PDR 1.02/include/steps.h | 2 +- .../PDR 1.02/project/PDR.vcxproj.filters | 6 +- 1.Software/PDR 1.02/src/pdr_ahrs.c | 21 ++- 1.Software/PDR 1.02/src/pdr_base.c | 159 +----------------- 1.Software/PDR 1.02/src/pdr_detector.c | 3 +- 1.Software/PDR 1.02/src/pdr_kalman.c | 81 +++------ 1.Software/PDR 1.02/src/pdr_location.c | 59 +++---- 1.Software/PDR 1.02/src/pdr_main.cpp | 11 +- 1.Software/PDR 1.02/src/pdr_util.c | 8 +- 1.Software/PDR 1.02/src/steps.c | 2 +- 17 files changed, 92 insertions(+), 305 deletions(-) diff --git a/1.Software/PDR 1.02/include/pdr_ahrs.h b/1.Software/PDR 1.02/include/pdr_ahrs.h index 0c593b3..041f0ac 100644 --- a/1.Software/PDR 1.02/include/pdr_ahrs.h +++ b/1.Software/PDR 1.02/include/pdr_ahrs.h @@ -27,7 +27,7 @@ extern "C" { /* Function Declaration ------------------------------------------------------------------------- */ void AHRS_Init(void); -void pdr_ahrsReset(void); +void ResetARHS(void); /**--------------------------------------------------------------------- * Function : fv3Norm diff --git a/1.Software/PDR 1.02/include/pdr_base.h b/1.Software/PDR 1.02/include/pdr_base.h index d451270..c655538 100644 --- a/1.Software/PDR 1.02/include/pdr_base.h +++ b/1.Software/PDR 1.02/include/pdr_base.h @@ -234,7 +234,7 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_ * Description : ¸ù¾Ý¼ÓËٶȼ°Åжϵ±Ç°×´Ì¬ÊÇ·ñƽÎÈ 1: ²»Æ½ÎÈ 0: ƽÎÈ * Date : 2020/7/22 logzhan *---------------------------------------------------------------------**/ -void stateRecognition(float *acc, classifer *sys); +void StateRecognition(float *acc, classifer *sys); /************************************************************************** * Description : ¸ù¾ÝGPS½Ç¶È¡¢GPS¹ì¼£½Ç¡¢PDR½Ç¶È£¬¼ÆËãÓû§ÐÐ×ß·½Ïò @@ -260,7 +260,7 @@ double calPredAngle(PDR *g_pdr, classifer *sys, double gps_yaw, double G_yaw, im kf£¬EKF½á¹¹Ìå pdr, PDRÔ¤²âλÖóɹ¦±ê־λ **************************************************************************/ -void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data, +void InsPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data, GNSS_t *pgnss, KfPara_t *kf); /**---------------------------------------------------------------------- @@ -288,7 +288,7 @@ void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result); * kf£¬ EKFÊý¾Ý½á¹¹Ìå * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -int resetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result); +int ResetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result); /**---------------------------------------------------------------------- * Function : detIsCarMode @@ -337,7 +337,7 @@ int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr, lct_fs *result); pgnss£¬GPSÊý¾Ý½á¹¹Ìå * Output : double£¬¹ì¼£½Ç **************************************************************************/ -double calGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss); +double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss); #endif diff --git a/1.Software/PDR 1.02/include/pdr_detector.h b/1.Software/PDR 1.02/include/pdr_detector.h index 62d059f..e909316 100644 --- a/1.Software/PDR 1.02/include/pdr_detector.h +++ b/1.Software/PDR 1.02/include/pdr_detector.h @@ -76,7 +76,7 @@ void mag_min(float a[MAG_BUF_LEN], float *mag_min); void mag_max(float a[MAG_BUF_LEN], float *mag_max); void mag_rand(float r[6]); -void updateDetectorImu(imu* imu); +void UpdateDetectorImu(imu* imu); #ifdef __cplusplus } diff --git a/1.Software/PDR 1.02/include/pdr_kalman.h b/1.Software/PDR 1.02/include/pdr_kalman.h index 24f8c14..8c69fac 100644 --- a/1.Software/PDR 1.02/include/pdr_kalman.h +++ b/1.Software/PDR 1.02/include/pdr_kalman.h @@ -14,31 +14,30 @@ extern "C" { #endif /**---------------------------------------------------------------------- -* Function : pdr_initKalman +* Function : EKF_Init * Description : ³õʼ»¯¿¨¶ûÂüÂ˲¨Æ÷Ïà¹Ø -* Date : 2020/8/27 -* +* Date : 2022/09/19 *---------------------------------------------------------------------**/ -void KalmanFilter_Init(void); +void EKF_Init(void); /**---------------------------------------------------------------------- -* Function : pdr_ekfStatePredict +* Function : EKFStatePredict * Description : pdr¿¨¶ûÂüÂ˲¨Æ÷µÄ״̬Ԥ²â·½³Ì -* Date : 2020/7/22 logzhan +* Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void pdr_ekfStatePredict(KfPara_t* kf, double step_length, PDR* g_pdr, int step); +void EKFStatePredict(KfPara_t* kf, double step_length, PDR* g_pdr, int step); /**---------------------------------------------------------------------- -* Function : getQRValue -* Description : µ÷Õû¿¨¶ûÂüÂ˲¨ÔëÉù¾ØÕó +* Function : EKFCalQRMatrix +* Description : ¼ÆË㿨¶ûÂüÂ˲¨ÔëÉù¾ØÕó * scane_type, Óû§Ëù´¦³¡¾°£¨¿ªÀ«ºÍ·Ç¿ªÀ«£© * nmea_data£¬NMEAÊý¾Ý½á¹¹Ìå * g_pdr£¬PDR½á¹¹Ìå * sys£¬Ô˶¯·ù¶ÈÊý¾Ý½á¹¹Ìå * kf£¬EKFÊý¾Ý½á¹¹Ìå -* Date : 2020/07/09 logzhan +* Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void CalEKFQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara_t* kf); +void EKFCalQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara_t* kf); /**---------------------------------------------------------------------- * Function : pdr_ekfStateUpdate @@ -53,7 +52,7 @@ void EKFStateUpdate(KfPara_t* kf, GNSS_t* pgnss, classifer* sys, PDR* g_pdr, * Description : PDRµÄGNSSºÍINSÈں϶¨Î» * Date : 2020/07/09 logzhan *---------------------------------------------------------------------**/ -void pdr_gnssInsLocFusion(GNSS_t* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias, +void GnssInsLocFusion(GNSS_t* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias, KfPara_t* kf, lct_fs* res); #ifdef __cplusplus diff --git a/1.Software/PDR 1.02/include/pdr_location.h b/1.Software/PDR 1.02/include/pdr_location.h index 07cc362..ace7eb1 100644 --- a/1.Software/PDR 1.02/include/pdr_location.h +++ b/1.Software/PDR 1.02/include/pdr_location.h @@ -50,7 +50,7 @@ void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss); * 2020/02/08 logzhan : ÐÞ¸Ä-1ΪINVAILD_GPS_YAW£¬Ìá¸ß * ´úÂëµÄ¿É¶ÁÐÔ *---------------------------------------------------------------------**/ -int pdr_detectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result); +int DetectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result); /**---------------------------------------------------------------------- * Function : GnssCalHeading @@ -65,7 +65,7 @@ double GnssCalHeading(GNSS_t* pgnss); * Description : ÀûÓÃGPSÐźŽϺÃʱµÄÆ«º½½ÇÐÞÕý´ÅÁ¦¼Æ¼ÆËã·½Ïò¼üµÄÆ«ÒÆ * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr); +void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr); /**--------------------------------------------------------------------- * Function : pdr_resetSysStatus diff --git a/1.Software/PDR 1.02/include/pdr_main.h b/1.Software/PDR 1.02/include/pdr_main.h index 53051f9..d3f2f98 100644 --- a/1.Software/PDR 1.02/include/pdr_main.h +++ b/1.Software/PDR 1.02/include/pdr_main.h @@ -76,7 +76,7 @@ const char* Motion2TypeStr(int type); * Description : ¸üÐÂÐÂÊä³öµÄ½á¹û¹ì¼££¬°üº¬GPS¹ì¼£ºÍPDR¹ì¼£ * Date : 2021/01/25 logzhan *---------------------------------------------------------------------**/ -void updateResTrack(ResultTracks& resTrack, lct_fs& lctfs); +void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs); #endif diff --git a/1.Software/PDR 1.02/include/pdr_util.h b/1.Software/PDR 1.02/include/pdr_util.h index f1e0a30..8976616 100644 --- a/1.Software/PDR 1.02/include/pdr_util.h +++ b/1.Software/PDR 1.02/include/pdr_util.h @@ -83,9 +83,9 @@ void ECEF2Ned(double *ecef, double *plla, double *ned); void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef); -void Wgs842Ned(double *plla, double *ref_lla, double *ned); +void WGS842NED(double *plla, double *ref_lla, double *ned); -void ned2Wgs84(double *ref_plla, double *ned, double* plla); +void NED2WGS84(double *ref_plla, double *ned, double* plla); LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB); diff --git a/1.Software/PDR 1.02/include/steps.h b/1.Software/PDR 1.02/include/steps.h index 54bff7b..39a887c 100644 --- a/1.Software/PDR 1.02/include/steps.h +++ b/1.Software/PDR 1.02/include/steps.h @@ -160,7 +160,7 @@ void reinitPedometer(void); * Description : PDR ¼Æ²½Æ÷µÄ¼ÓËÙ¶ÈÊäÈë, ²¢Í¨¹ýÖ¸Õë·µ»ØÓû§²½Êý * Date : 2020/2/1 logzhan *---------------------------------------------------------------------**/ -void updatePedometer(imu* ss_data, unsigned long* step); +void UpdatePedometer(imu* ss_data, unsigned long* step); void Steps_State_Detect(enumState* state); diff --git a/1.Software/PDR 1.02/project/PDR.vcxproj.filters b/1.Software/PDR 1.02/project/PDR.vcxproj.filters index 3101f4e..8967460 100644 --- a/1.Software/PDR 1.02/project/PDR.vcxproj.filters +++ b/1.Software/PDR 1.02/project/PDR.vcxproj.filters @@ -104,9 +104,6 @@ inc - - inc - inc @@ -134,5 +131,8 @@ src\Utils + + src\Location + \ No newline at end of file diff --git a/1.Software/PDR 1.02/src/pdr_ahrs.c b/1.Software/PDR 1.02/src/pdr_ahrs.c index 96c43c1..ee550e1 100644 --- a/1.Software/PDR 1.02/src/pdr_ahrs.c +++ b/1.Software/PDR 1.02/src/pdr_ahrs.c @@ -56,14 +56,13 @@ static const float g_y_axis[] = {0, 1, 0}; static const float g_z_axis[] = {0, 0, 1}; /**---------------------------------------------------------------------- -* Function : mahonyUpdateAHRS -* Description : mahony×Ë̬¸üÐÂËã·¨ -* Date : 2020/8/3 logzhan -* 2020/02/18 logzhan : +* Function : MahonyUpdateAHRS +* Description : Mahony×Ë̬¸üÐÂËã·¨ +* Date : 2020/02/18 logzhan : * Ôö¼ÓÁËAHRSÎȶ¨ÐÔ±ê־룬Èç¹ûAHRSÎȶ¨ºó²»ÔÙ¼ÆËãerror, ½µµÍÔËËã Á¿¡£ÄÜÌáÉýPDR·ÂÕæ10%µÄÔËÐÐËÙ¶È *---------------------------------------------------------------------**/ -static void mahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az, +static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { // ¹éÒ»»¯´ÅÁ¦¼ÆºÍ¼ÓËÙ¶È¼Æ pdr_v3Norm(&ax, &ay, &az); @@ -157,10 +156,10 @@ void AHRS_Init(void) { Buffer_initialize((BUFFER *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); Buffer_initialize((BUFFER *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); } - pdr_ahrsReset(); + ResetARHS(); } -void pdr_ahrsReset(void) { +void ResetARHS(void) { Buffer_clear((BUFFER *)&g_erro_buf); for (uchar i = 0; i < 3; ++i) { @@ -283,7 +282,7 @@ int UpdateAHRS(imu* imu) { Kp = (float)(2 * 2); /* ÕâÀïÑ­»·ÔËÐÐ1000´Î,ÊÕÁ²£¬Ç÷ÓÚÕýÈ·×Ë̬ */ for (i = 0; i < 1000; ++i) { - mahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]); + MahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]); } AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, grav); @@ -300,7 +299,7 @@ int UpdateAHRS(imu* imu) { estimate_sensor_vector(grav, grav, gyro); // AHRS ¸üР- mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0], + MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]); AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce); @@ -354,7 +353,7 @@ int UpdateAHRS(imu* imu) { } estimate_sensor_vector(grav, grav, gyro); - mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], + MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]); @@ -383,7 +382,7 @@ int UpdateAHRS(imu* imu) { //estimate_sensor_vector(grav, grav, gyro); // logzhan 21/02/06 : ¸Ð¾õÊÇaccÊäÈ뾫¶È»á¸ßһЩ - mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], + MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], acce[0], acce[1], acce[2], magn[0], magn[1], magn[2]); diff --git a/1.Software/PDR 1.02/src/pdr_base.c b/1.Software/PDR 1.02/src/pdr_base.c index 1e653f6..6c49c06 100644 --- a/1.Software/PDR 1.02/src/pdr_base.c +++ b/1.Software/PDR 1.02/src/pdr_base.c @@ -372,164 +372,7 @@ static void correct_axis_ceofficient(float ratio) { * Date : 2020/7/21 logzhan *---------------------------------------------------------------------**/ void ComputePCA(AHRS_t *ahrs) { - float axis_mean[9]; - float freq; - float mold; - int length; - int i; - double attitude[3]; - - double angle_sum[3] = { 0 }; - int angle_len; - - //RESET_1ʱֻ¼Óticker£¬Ö®ºóÖ±½Ó·µ»Ø - /* filter bad data */ - if (AHRS_STATUS_RESET & ahrs->status) { - ++g_tick_p; - return; - } - /* caculate axis ceofficient */ - /*PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40*/ - if (P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && \ - (g_grav[0]._top - g_grav[0]._bottom + g_grav[0].length + 1) % (g_grav[0].length + 1) > (BUF_LEN >> 1)) { - - correct_axis_ceofficient(0.50f); - P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT); - P_SET_BIT(PCA_STABLE); - - g_erro._bottom = 0; - for (g_erro._top = 0; g_erro._top < g_erro.length && g_erro._top < 2 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE; ++g_erro._top) { - g_erro.data[g_erro._top] = (g_erro._top % 2) ? +0.4f : -0.4f; - } - Buffer_clear((BUFFER *)&g_posi[0]); - Buffer_clear((BUFFER *)&g_posi[1]); - Buffer_clear((BUFFER *)&g_posi[2]); - Buffer_clear((BUFFER *)&g_posi[3]); - g_pdr.bias_direction[0] = 1; - g_pdr.bias_direction[1] = 0; - g_pdr.bias_accuracy = -1; - //++flag; - //} - } - - /* correct axis ceofficient */ - if ((g_pdr.status & PDR_STATUS_PCA_STABLE) && 0 == (g_tick_p % (2 * IMU_SAMPLING_FREQUENCY))) { - correct_axis_ceofficient(0.10f); - } - - /* downsample */ - if (0 != (g_tick_p % DOWNSAMPLING_RATE)) { - ++g_tick_p; - return; - } - - /* push data */ - for (i = 0; i < 3; ++i) { - buffer_push((BUFFER *)&g_axis[0 + i], ahrs->x_axis[i]); - buffer_push((BUFFER *)&g_axis[3 + i], ahrs->y_axis[i]); - buffer_push((BUFFER *)&g_axis[6 + i], ahrs->z_axis[i]); - } - for (i = 0; i < 2; ++i) { - //Buffer_push((BUFFER *)&g_grav[i], (float)FILTER_filter(&g_grv_flt[i], ahrs->gravity[i])); - buffer_push((BUFFER *)&g_grav[i], ahrs->gravity[i]); - } - - /* motion frequency */ - freq = get_main_frequency(g_acc_frq_buf, g_acc_amp_buf, 3); - if (3.5 >= freq && freq > 1.5) { - g_pdr.motionFreq = freq; - } - else if (1.5 >= freq && freq > 0.75){ - g_pdr.motionFreq = 2 * freq; - } - - /* axis direction */ - length = get_optimal_length_by_frequency(g_axis, g_pdr.motionFreq); - get_buffer_mean(&axis_mean[0], &g_axis[0], length); - get_buffer_mean(&axis_mean[3], &g_axis[3], length); - get_buffer_mean(&axis_mean[6], &g_axis[6], length); - memset(g_pdr.axis_direction, 0, sizeof(g_pdr.axis_direction)); - for (i = 0; i < 2; ++i) { - g_pdr.axis_direction[i] = g_pdr.axis_ceofficient[0] * axis_mean[0 + i] + g_pdr.axis_ceofficient[1] * axis_mean[3 + i] + g_pdr.axis_ceofficient[2] * axis_mean[6 + i]; - } - mold = (float)GET_MOL(g_pdr.axis_direction); - if (0 != mold) { - g_pdr.axis_direction[0] /= mold; - g_pdr.axis_direction[1] /= mold; - } - - /* pca direction */ - get_pca_direction(g_pdr.pca_direction, g_grav, length); - if (g_pdr.pca_direction[0] * g_pdr.axis_direction[0] + g_pdr.pca_direction[1] * g_pdr.axis_direction[1] < 0) { - g_pdr.pca_direction[0] = -g_pdr.pca_direction[0]; - g_pdr.pca_direction[1] = -g_pdr.pca_direction[1]; - } - - /* direction accuracy */ - /*´úÂëÒÉÎÊ1£ºg_erroΪʲôÓÉÏÂʽȷ¶¨£¬Ê²Ã´º¬Òå*/ - buffer_push((BUFFER *)&g_erro, (float)fabs(g_pdr.axis_direction[0] * g_pdr.pca_direction[1] - g_pdr.axis_direction[1] * g_pdr.pca_direction[0])); - if (!P_TST_BIT(CACULATING_AXIS_CEOFFICIENT)) { - float accuracy; - - Buffer_std((BUFFER *)&g_erro, &accuracy); - if (DETECTOR_TYPE_HANDHELD == g_pdr.motionType) { - if (!P_TST_BIT(STABLE) && !P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && 0.15 >= accuracy) { - P_SET_BIT(CACULATING_AXIS_CEOFFICIENT); - } - else if (g_pdr.pca_accuracy < 0.30 && 0.30 <= accuracy) { - P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT); - P_CLR_BIT(STABLE); - } - } - g_pdr.pca_accuracy = accuracy; - } - else { - g_pdr.pca_accuracy = -1; - } - - /* calibrated direction */ - g_pdr.cal_direction[0] = g_pdr.pca_direction[0] * g_pdr.bias_direction[0] + g_pdr.pca_direction[1] * g_pdr.bias_direction[1]; - g_pdr.cal_direction[1] = g_pdr.pca_direction[1] * g_pdr.bias_direction[0] - g_pdr.pca_direction[0] * g_pdr.bias_direction[1]; - - //PDR_LOGD("%f %f\n", g_pdr.cal_direction[0],g_pdr.cal_direction[1]); - - if (DETECTOR_TYPE_STATIC == g_detector->type || DETECTOR_TYPE_IRREGULAR == g_detector->type) { - g_pdr.motionFreq = 0; - } - - Qnb2Att(ahrs->qnb,attitude); - - if (angle_count < BUF_LEN - 1) - { - p_angle[0][angle_count] = attitude[0]; - p_angle[1][angle_count] = attitude[1]; - p_angle[2][angle_count] = attitude[2]; - angle_count++; - } - else{ - for (int i = 0; i < BUF_LEN - 1; i++) - { - p_angle[0][i] = p_angle[0][i + 1]; - p_angle[1][i] = p_angle[1][i + 1]; - p_angle[2][i] = p_angle[2][i + 1]; - } - p_angle[0][BUF_LEN - 1] = attitude[0]; - p_angle[1][BUF_LEN - 1] = attitude[1]; - p_angle[2][BUF_LEN - 1] = attitude[2]; - - angle_len = (length <= BUF_LEN) ? length : BUF_LEN; - for (int j = BUF_LEN - 1; j >(BUF_LEN - 1 - angle_len); j--) - { - angle_sum[0] = angle_sum[0] + p_angle[0][j]; - angle_sum[1] = angle_sum[1] + p_angle[1][j]; - angle_sum[2] = angle_sum[2] + p_angle[2][j]; - } - angle_mean[0] = angle_sum[0] / angle_len; - angle_mean[1] = angle_sum[1] / angle_len; - angle_mean[2] = angle_sum[2] / angle_len; - } - - ++g_tick_p; + } /**---------------------------------------------------------------------- diff --git a/1.Software/PDR 1.02/src/pdr_detector.c b/1.Software/PDR 1.02/src/pdr_detector.c index 6d9974e..847faf4 100644 --- a/1.Software/PDR 1.02/src/pdr_detector.c +++ b/1.Software/PDR 1.02/src/pdr_detector.c @@ -141,7 +141,7 @@ void pdr_resetDetector(void) { * Date : 2020/09/02 * *---------------------------------------------------------------------**/ -void updateDetectorImu(imu* imu) { +void UpdateDetectorImu(imu* imu) { buffer_push((BUFFER *)&g_mol_buf[ACCE], (float)FILTER_filter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s))); buffer_push((BUFFER *)&g_mol_buf[GYRO], (float)FILTER_filter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s))); @@ -151,7 +151,6 @@ void updateDetectorImu(imu* imu) { // ¼ì²âÓû§µÄÔ˶¯ÀàÐÍ int detectResult = MotionTypeDetect(); - //mag_calibration(imu); if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) { detectorUpdate(&g_detector); g_detector.lastType = g_detector.type; diff --git a/1.Software/PDR 1.02/src/pdr_kalman.c b/1.Software/PDR 1.02/src/pdr_kalman.c index f5c0631..dc26d72 100644 --- a/1.Software/PDR 1.02/src/pdr_kalman.c +++ b/1.Software/PDR 1.02/src/pdr_kalman.c @@ -42,7 +42,7 @@ static double FUR_duration; * Description : ³õʼ»¯¿¨¶ûÂüÂ˲¨Æ÷Ïà¹Ø * Date : 2022/09/16 logzhan *---------------------------------------------------------------------**/ -void KalmanFilter_Init(void) +void EKF_Init(void) { b_timestamp = -1; b_last_time = -1; @@ -76,7 +76,7 @@ int clear_buf(double *buf_p, int dmt) * Description : pdr¿¨¶ûÂüÂ˲¨Æ÷µÄ״̬Ԥ²â·½³Ì * Date : 2020/7/22 logzhan *---------------------------------------------------------------------**/ -void pdr_ekfStatePredict(KfPara_t *kf, double stepLen, PDR* g_pdr, int step) { +void EKFStatePredict(KfPara_t *kf, double stepLen, PDR* g_pdr, int step) { double (*Phi)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[0][0][0]); double (*pvec1)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[1][0][0]); @@ -349,57 +349,16 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_ } /**---------------------------------------------------------------------- -* Function : stateRecognition +* Function : StateRecognition * Description : ¸ù¾Ý¼ÓËٶȼ°Åжϵ±Ç°×´Ì¬ÊÇ·ñƽÎÈ 1: ²»Æ½ÎÈ 0: ƽÎÈ -* Date : 2020/7/22 logzhan +* Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void stateRecognition(float *acc, classifer *sys) { - int i,j,k; - float accNorm = 0; - float sum = 0, max_min = 0, max = -1, min = 999; - float mean = 0, std = 0; +void StateRecognition(float *acc, classifer *sys) { - // ÏȼÆË㵱ǰ¼ÓËٶȵÄÄ£ - accNorm = (float)sqrt(vivopow(acc[0], 2) + vivopow(acc[1], 2) + vivopow(acc[2], 2)); - - // »º´æ¸üР- for (i = 0; i < PATTERN_RECOGNITION_LEN - 1; i++) { - sys->accBuffer[i] = sys->accBuffer[i + 1]; - } - sys->accBuffer[PATTERN_RECOGNITION_LEN - 1] = accNorm; - - // ¼ÆËã×î´ó×îСֵºÍsum - for (j = 0; j < PATTERN_RECOGNITION_LEN - 1; j++) { - sum += sys->accBuffer[j]; - if (sys->accBuffer[j] > max) { - max = sys->accBuffer[j]; - } - if (sys->accBuffer[j] < min) { - min = sys->accBuffer[j]; - } - } - - // ¼ÆËã¾ùÖµ - mean = sum / PATTERN_RECOGNITION_LEN; - // ¼ÆËã·½²î - for (k = 0; k < PATTERN_RECOGNITION_LEN - 1; k++) { - std += (sys->accBuffer[k] - mean) * (sys->accBuffer[k] - mean) / PATTERN_RECOGNITION_LEN; - } - std = (float)sqrt(std); - - // ¼ÆËã×î´ó×îС²îÖµ - max_min = max - min; - - if (std > 5 && max_min > 25) { - sys->type = 1; - } - else { - sys->type = 0; - } } -double calGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) +double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) { int i; double lla[3] = { 0.0 }; @@ -536,7 +495,7 @@ double calGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) * 2021/02/06 logzhan : ¸Ð¾õ¼Æ²½Æ÷Êä³ö²»¹»Æ½¾ù£¬ÓÐʱºò * »áÔì³ÉPDRÇ°ºóλÖò»¹»¾ùÔÈ *---------------------------------------------------------------------**/ -void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data, +void InsPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data, GNSS_t *pgnss, KfPara_t *kf) { int step; long deltaStep; @@ -563,7 +522,7 @@ void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data, if (lastSteps == 0)deltaStep = 0; if (g_pdr->heading > 0) { - pdr_ekfStatePredict(kf, stepLength, g_pdr, deltaStep); + EKFStatePredict(kf, stepLength, g_pdr, deltaStep); g_pdr->fusionPdrFlg = PDR_TRUE; } calStepLastTime(stepPredict, ss_data->gyr.time, lastSteps, steps); @@ -572,19 +531,19 @@ void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data, // ¹ßµ¼¸üÐÂλÖõÄÄ¿µÄ int preStep = predictStep(stepPredict, ss_data->gyr.time, lastSteps, pgnss->vel); if (g_pdr->heading > 0){ - pdr_ekfStatePredict(kf, stepLength, g_pdr, preStep); + EKFStatePredict(kf, stepLength, g_pdr, preStep); if(preStep > 0)g_pdr->fusionPdrFlg = PDR_TRUE; } } } /**---------------------------------------------------------------------- -* Function : resetOutputResult +* Function : ResetOutputResult * Description : 1¡¢³õʼ»¯½âËã³õʼλÖᢿ¨¶ûÂüÂ˲¨³õʼ²ÎÊý * 2¡¢GPSÖµ¸³Öµ¸øresult -* Date : 2020/07/08 logzhan +* Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -int resetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result) { +int ResetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result) { double stepLength = 0.7; double ned[3] = { 0 }; @@ -592,7 +551,7 @@ int resetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result) { g_pdr->pllaInit[0] = pgnss->lat; g_pdr->pllaInit[1] = pgnss->lon; g_pdr->pllaInit[2] = 0; - Wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned); + WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned); g_pdr->x0 = ned[0]; g_pdr->y0 = ned[1]; @@ -632,7 +591,7 @@ int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr,lct_fs *result) // plla ת ned double ned[3] = { 0 }; - Wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned); + WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned); // ÔÚNED×ø±êϵµÄ¼ÆËã, µ¥Î»ÊÇÃ× g_pdr->x0 = ned[0]; @@ -669,13 +628,13 @@ int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr,lct_fs *result) } /**---------------------------------------------------------------------- -* Function : CalEKFQRMatrix +* Function : EKFCalQRMatrix * Description : ¸ù¾ÝGPSÐźÅÌØÕ÷µ÷Õû¿¨¶ûÂüÂ˲¨ÔëÉù¾ØÕó£¬q¾ØÕóÊǹý³ÌÔëÉù¾ØÕó£¬ * ÐèÒª¸ú¹ßµ¼Ô¤²âλÖþ«¶ÈÏà¹Ø¡£r¾ØÕóΪGNSS¹Û²âÔëÉù£¬¸úGPSÊä³öµÄ * ÐÅÏ¢¾«¶ÈÓйء£ -* Date : 2020/07/09 logzhan +* Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void CalEKFQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys, +void EKFCalQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys, KfPara_t *kf) { if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD && @@ -722,7 +681,7 @@ void CalEKFQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys, * Description : PDRµÄGNSSºÍINSÈں϶¨Î» * Date : 2020/07/09 logzhan *---------------------------------------------------------------------**/ -void pdr_gnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_bias, +void GnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_bias, KfPara_t *kf, lct_fs *res) { double plla[3] = { 0 }; double ned[3] = { 0 }; @@ -734,7 +693,7 @@ void pdr_gnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_ plla[1] = pgnss->lon; plla[2] = 0; - Wgs842Ned(plla, g_pdr->pllaInit, ned); + WGS842NED(plla, g_pdr->pllaInit, ned); pgnss->xNed = ned[0]; pgnss->yNed = ned[1]; @@ -753,7 +712,7 @@ void pdr_gnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_ ned[1] = kf -> p_xk[1] - g_pdr->y0; ned[2] = 0; - ned2Wgs84(g_pdr->pllaInit, ned, plla); + NED2WGS84(g_pdr->pllaInit, ned, plla); res->latitude = plla[0]; res->latitude_ns = pgnss->lat_ns; diff --git a/1.Software/PDR 1.02/src/pdr_location.c b/1.Software/PDR 1.02/src/pdr_location.c index 2755b3d..e857231 100644 --- a/1.Software/PDR 1.02/src/pdr_location.c +++ b/1.Software/PDR 1.02/src/pdr_location.c @@ -61,7 +61,7 @@ void NavSys_Init(void) { // PDR³õʼ»¯ pdr = Base_Init(); //baseÎļþÖÐ×Ë̬½Ç¼ÆËã³õʼ»¯ - KalmanFilter_Init(); + EKF_Init(); // ³¡¾°Ê¶±ðÄ£¿é³õʼ»¯ initSceneRecognition(); @@ -99,26 +99,26 @@ void ResetSystemStatus(KfPara_t *kf) * Description : PDR ¹ßÐÔµ¼º½¶¨Î» * Date : 2022/09/16 logzhan *---------------------------------------------------------------------**/ -int InsLocation(imu* ss_data, KfPara_t* kf) { +int InsLocation(imu* imuData, KfPara_t* kf) { // AHRS×Ë̬¸üÐÂÕ¼2%µÄÔËÐÐʱ¼ä - if (UpdateAHRS(ss_data)) { + if (UpdateAHRS(imuData)) { // Íê³É×Ë̬¹À¼Æºó¼ÆËãPCA£¬ Õ¼26%µÄÔËÐÐʱ¼ä - //ComputePCA(&g_ahrs); + ComputePCA(&g_ahrs); } // ¸üÐÂÓû§Ô˶¯ÀàÐͼì²âÆ÷IMUÐÅÏ¢ÒÔ¼°Óû§Ô˶¯ÀàÐͼì²â, Õ¼21%µÄÔËÐÐʱ¼ä - updateDetectorImu(ss_data); + UpdateDetectorImu(imuData); - pdr->gyroTime = ss_data->gyr.time; + pdr->gyroTime = imuData->gyr.time; // ΪʲôÊÖ³ÖÀàÐÍÊÇ0.1µÄÔ˶¯ÆµÂÊ£¿ if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f; // ¼Æ²½Æ÷ÊäÈë´«¸ÐÆ÷Êý¾Ý£¬Õ¼10%µÄÔËÐÐʱ¼ä - updatePedometer(ss_data, &pdr->steps); + UpdatePedometer(imuData, &pdr->steps); // ----- Filter Initialization ------- // if (pdr->sysStatus == IS_INITIAL) { - pdr->ts = ss_data->gyr.time; + pdr->ts = imuData->gyr.time; pdr->lastSteps = pdr->steps; stepPredict.lastTime = 0; stepPredict.fnum = 0; @@ -126,30 +126,17 @@ int InsLocation(imu* ss_data, KfPara_t* kf) { return 0; } - pdr->ts = ss_data->gyr.time; + pdr->ts = imuData->gyr.time; - if ((0 == ss_data->acc.s[0] && 0 == ss_data->acc.s[1] && 0 == ss_data->acc.s[2]) || - (0 == ss_data->mag.s[0] && 0 == ss_data->mag.s[1] && 0 == ss_data->mag.s[2])) { + 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; } // ʶ±ðµ±Ç°×´Ì¬ÊÇ·ñƽÎÈ£¬Õâ¸öº¯Êý¼ÆËãÁ¿µ½´ïÕû¸öϵͳ10%µÄ¼ÆËãÁ¿, ÒÔºó¸ù¾ÝÇé¿ö½øÐÐÓÅ»¯ - // stateRecognition(ss_data->acc.s, &sys); + StateRecognition(imuData->acc.s, &sys); - //pdr->heading = calPredAngle(pdr, &sys, pdr->gpsHeading, pdr->trackHeading, ss_data); - //if (yaw_bias != -1) { - // if (pdr->pca_accuracy > 0 && pdr->pca_accuracy < 0.1 && pdr->motionType == 3 && - // (pdr->bias_accuracy > 1.5 || pdr->bias_accuracy == -1 || pgnss.error > 30) - // && (fabs(yaw_bias - 2 * PI) < YAW_THRES * PI / 180 || fabs(yaw_bias - PI) < YAW_THRES * PI / 180 || - // fabs(yaw_bias) < YAW_THRES * PI / 180)) { - // dir[0] = pdr->pca_direction[0] * bias_dir[0] + pdr->pca_direction[1] * bias_dir[1]; - // dir[1] = pdr->pca_direction[1] * bias_dir[0] - pdr->pca_direction[0] * bias_dir[1]; - // pdr->heading = atan2(dir[1], dir[0]); - // if (pdr->heading < 0) { - // pdr->heading += TWO_PI; - // } - // } - //} + //pdr->heading = CalPredHeading(...); double imuYaw = - g_ahrs.yaw; if (imuYaw < 0) { @@ -163,7 +150,7 @@ int InsLocation(imu* ss_data, KfPara_t* kf) { } // PDR ¹ßÐÔλÖÃÔ¤²â - pdr_insPredict(pdr, &stepPredict, ss_data, &pgnss, kf); + InsPredict(pdr, &stepPredict, imuData, &pgnss, kf); pdr->lastSteps = pdr->steps; pdr->lastHeading = pdr->heading; @@ -200,26 +187,26 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, KfPara_t *kf, lct_fs *result) result->motionType = pdr->motionType; #endif - calPdrHeadingOffset(nmea_data, pdr); + CalPdrHeadingOffset(nmea_data, pdr); // ¼ÆËãÁ½´ÎGPSʱ¼äÖ®¼ä£¬²½Êý±ä»¯Á¿ pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps; pdr->lastGpsSteps = pdr->steps; - int mode = pdr_detectFixMode(&pgnss, kf, pdr, result); + int mode = DetectFixMode(&pgnss, kf, pdr, result); if (mode != TYPE_FIX_PDR)return mode; //¸ù¾ÝÀúÊ·GPSÄâºÏÊä³öÔ¤¹ÀµÄGPS¹ì¼£º½Ïò½Ç - pdr->trackHeading = calGnssTrackHeading(GpsPos, pgnss); + pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss); // Èç¹ûϵͳûÓгõʼ»¯£¬ÔòÏȳõʼ»¯ if (pdr->sysStatus == IS_INITIAL) { return pdr_initProc(&pgnss, kf, pdr, result); } // Èç¹û¹ßµ¼Ã»ÓдﵽÊä³öÌõ¼þ£¬Ôò²»Ô¤²â¹ì¼£ - if (!pdr->fusionPdrFlg)return resetOutputResult(&pgnss, pdr, kf, result); + if (!pdr->fusionPdrFlg)return ResetOutputResult(&pgnss, pdr, kf, result); // ¸ù¾ÝGPSÐÅÏ¢, ¼ÆËãkfµÄ¹ý³ÌÔëÉùºÍ¹Û²âÔëÉù - CalEKFQRMatrix(nmea_data, pdr, &sys, kf); + EKFCalQRMatrix(nmea_data, pdr, &sys, kf); if (pgnss.satellites_num > 4){ // Èç¹ûGPSº½Ïò½Ç´¦ÓÚÎÞЧ״̬ÇÒËÙ¶ÈÒ²ÊÇÎÞЧ״̬ @@ -232,7 +219,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, KfPara_t *kf, lct_fs *result) pgnss.yaw = (float)kf->p_xk[3]; } if (pgnss.yaw != INVAILD_GPS_YAW) { - pdr_gnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result); + GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result); return TYPE_FIX_PDR; } } @@ -267,7 +254,7 @@ void NoGnssInfoPredict(KfPara_t* kf, lct_fs* result, PDR* p_pdr) ned[1] = kf->p_xk[1] - p_pdr->y0; ned[2] = 0; - ned2Wgs84(p_pdr->pllaInit, ned, lla); + NED2WGS84(p_pdr->pllaInit, ned, lla); result->latitude = lla[0]; result->latitude_ns = pgnss.lat_ns; @@ -282,7 +269,7 @@ void NoGnssInfoPredict(KfPara_t* kf, lct_fs* result, PDR* p_pdr) * Description : ÀûÓÃGPSÐźŽϺÃʱµÄÆ«º½½ÇÐÞÕý´ÅÁ¦¼Æ¼ÆËã·½Ïò¼üµÄÆ«ÒÆ * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ -void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) { +void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) { //¹ßµ¼×Ë̬½ÇÓëGPSÐнø½Ç¶ÈµÄ¶Ô×¼£¨ÅжÏGPSÐźÅÊÇ·ñÕý³£ÒÔ¼°¹ßµ¼ÊÇ·ñƽÎÈ£© if (refGpsYaw != -1000)return; @@ -312,7 +299,7 @@ void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) { * Date : 2020/07/08 logzhan : ÐÞ¸Ä-1ΪINVAILD_GPS_YAW£¬Ìá¸ß * ´úÂëµÄ¿É¶ÁÐÔ *---------------------------------------------------------------------**/ -int pdr_detectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result) { +int DetectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result) { // ¼ì²âÓû§ÊÇ·ñ´¦ÓÚ¾²Ö¹×´Ì¬ if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) { if (pgnss->error > 0 && pgnss->error < 15 && diff --git a/1.Software/PDR 1.02/src/pdr_main.cpp b/1.Software/PDR 1.02/src/pdr_main.cpp index 2293844..16e3a16 100644 --- a/1.Software/PDR 1.02/src/pdr_main.cpp +++ b/1.Software/PDR 1.02/src/pdr_main.cpp @@ -79,7 +79,8 @@ extern "C" double refGpsYaw; *---------------------------------------------------------------------**/ int main(int argc, char* argv[]) { - string logPath = "E:\\Software Project\\GnssIns-PDR-Private\\1.Software - 2.0\\Debug\\"; + + string logPath = "E:\\SoftwareProject\\GnssIns\\GnssIns-PDR-Private\\1.Software\\PDR 1.02\\Debug\\"; string logDate = "shenzhen"; string logLabel = "data\\"; string fileHead; @@ -122,7 +123,7 @@ int main(int argc, char* argv[]) // ½âÎöÎı¾Êý¾Ý·ÂÕæ if (ParseLineAndUpdate(line, &locFusion) != TYPE_FIX_NONE){ // ¸üнá¹û¹ì¼££¬ÓÃÓÚ×îºóµÄkmlÉú³É - updateResTrack(resTracks, locFusion); + UpdateResTrack(resTracks, locFusion); } } // ¹Ø±ÕPDRËã·¨£¬ÊÍ·Å×ÊÔ´ @@ -185,11 +186,11 @@ double gpsYaw2GoogleYaw(double heading) { } /**---------------------------------------------------------------------- -* Function : updateResTrack +* Function : UpdateResTrack * Description : ¸üÐÂÐÂÊä³öµÄ½á¹û¹ì¼££¬°üº¬GPS¹ì¼£ºÍPDR¹ì¼£ -* Date : 2021/01/25 logzhan +* Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void updateResTrack(ResultTracks& resTrack, lct_fs& lctfs) +void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs) { resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude; resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal; diff --git a/1.Software/PDR 1.02/src/pdr_util.c b/1.Software/PDR 1.02/src/pdr_util.c index 40f0e70..82c08a2 100644 --- a/1.Software/PDR 1.02/src/pdr_util.c +++ b/1.Software/PDR 1.02/src/pdr_util.c @@ -317,7 +317,7 @@ void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef) { ecef[2] = cos(plla[0])*ned[0] - sin(plla[0])*ned[2] + ecef0[2];; } -void Wgs842Ned(double *plla, double *ref_lla, double *ned) +void WGS842NED(double *plla, double *ref_lla, double *ned) { double ecef[3] = {0}; @@ -325,7 +325,7 @@ void Wgs842Ned(double *plla, double *ref_lla, double *ned) ECEF2Ned(ecef, ref_lla, ned); } -void ned2Wgs84(double *ref_plla, double *ned, double* plla) +void NED2WGS84(double *ref_plla, double *ned, double* plla) { double ecef[3] = { 0 }; double ref_ecef[3] = { 0 }; @@ -477,8 +477,8 @@ double CalDistance(LatLng pointA, LatLng pointB) if (isRealLat(pointA.lat) && isRealLon(pointA.lon) && isRealLat(pointB.lat) && isRealLon(pointB.lon)) { - Wgs842Ned(lla1, lla1, ned1); - Wgs842Ned(lla2, lla1, ned2); + WGS842NED(lla1, lla1, ned1); + WGS842NED(lla2, lla1, ned2); diff_ned[0] = ned1[0] - ned2[0]; diff_ned[1] = ned1[1] - ned2[1]; diff_ned[2] = ned1[2] - ned2[2]; diff --git a/1.Software/PDR 1.02/src/steps.c b/1.Software/PDR 1.02/src/steps.c index 141f39c..4f1731e 100644 --- a/1.Software/PDR 1.02/src/steps.c +++ b/1.Software/PDR 1.02/src/steps.c @@ -128,7 +128,7 @@ void reinitPedometer(void) * Description : PDR 计步器的加速度输入, 并通过指针返回用户步数 * Date : 2020/2/1 logzhan *---------------------------------------------------------------------**/ -void updatePedometer(imu* ss_data, unsigned long* step) +void UpdatePedometer(imu* ss_data, unsigned long* step) { unsigned char k = 0;