重构函数命名
parent
fe1084e9db
commit
2790501743
|
|
@ -27,7 +27,7 @@ extern "C" {
|
|||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
|
||||
void AHRS_Init(void);
|
||||
void pdr_ahrsReset(void);
|
||||
void ResetARHS(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : fv3Norm
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -104,9 +104,6 @@
|
|||
<ClInclude Include="..\include\pdr_fft.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_kalman.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_linearFit.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
|
|
@ -134,5 +131,8 @@
|
|||
<ClInclude Include="..\include\pdr_util.h">
|
||||
<Filter>src\Utils</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_kalman.h">
|
||||
<Filter>src\Location</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
|
|
@ -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]);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 &&
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue