logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

优化若干细节

PDRHistoryBranch
詹力 2023-02-24 23:26:10 +08:00
parent 96c30bb66b
commit 5bd0ca97f2
12 changed files with 54 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
1.Software/PDR 1.05/include/Location.h
View File

@ -26,7 +26,7 @@ void PDRInfoInit(void);
* Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocation(IMU_t *ss_data, EKFPara_t *kf);
int InsLocationUpdate(IMU_t *ss_data, EKFPara_t *kf);
/**----------------------------------------------------------------------

2
1.Software/PDR 1.05/include/ParseData.h
View File

@ -81,7 +81,7 @@ void parseLocAccuracy(char* pt, Nmea_t *ln, double ts);
**************************************************************************/
void preprocessNMEA(Nmea_t *ln);
int ParseLineStr(char* line, IMU_t* imu_p, Nmea_t* ln);
int ParseGnssInsData(char* line, IMU_t* imu_p, Nmea_t* ln);
/**----------------------------------------------------------------------
* Function : HexToDec

2
1.Software/PDR 1.05/include/Pedometer.h
View File

@ -139,7 +139,7 @@ typedef struct {
* Description : 获取计步器版本
* Date : 2020/2/20
*---------------------------------------------------------------------**/
const char* getPedometerLibVersion(void);
const char* GetPedometerVersion(void);
/**---------------------------------------------------------------------
* Function : initPedometer

2
1.Software/PDR 1.05/include/Utils.h
View File

@ -91,7 +91,7 @@ float InvSqrt(float x);
*
*
*---------------------------------------------------------------------**/
void pdr_v3Norm(float* vx, float* vy, float* vz);
void Vec3Norm(float* vx, float* vy, float* vz);
/**----------------------------------------------------------------------
* Function : pdr_writeCsvStr

8
1.Software/PDR 1.05/src/AHRS.c
View File

@ -38,7 +38,7 @@ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, f
float* q = Ahrs.q;
// 归一化磁力计和加速度计
pdr_v3Norm(&ax, &ay, &az);
Vec3Norm(&ax, &ay, &az);
float wx = 0.0f, wy = 0.0f, wz = 0.0f;
@ -87,9 +87,9 @@ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, f
QuaternionNorm(&q[0], &q[1], &q[2], &q[3]);
// 四元数转欧拉角
Ahrs.Pitch = (float)asin(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f);
Ahrs.Yaw = (float)atan2(q[2]*q[1] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * (180.0f / 3.141592f);
Ahrs.Roll = (float)atan2(q[2]*q[3] + q[0]*q[1], 0.5f - q[2]*q[2] - q[1]*q[1]) * (180.0f / 3.141592f);
Ahrs.Pitch = asinf(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f);
Ahrs.Yaw = atan2f(q[2]*q[1] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * (180.0f / 3.141592f);
Ahrs.Roll = atan2f(q[2]*q[3] + q[0]*q[1], 0.5f - q[2]*q[2] - q[1]*q[1]) * (180.0f / 3.141592f);
}
void AHRS_Init(void) {

11
1.Software/PDR 1.05/src/Buffer.c
View File

@ -6,17 +6,17 @@
* Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取
* 和计算
********************************************************************************/
/* Header File Including ------------------------------------------------------------------------ */
/* Header File Including ------------------------------------------------------*/
#include <string.h>
#include <math.h>
#include "buffer.h"
/* Macro Declaration ---------------------------------------------------------------------------- */
/* Macro Declaration ----------------------------------------------------------*/
#define ADD_PTR(ptr, ofs) ((ptr + ofs) % (buffer->length + 1))
#define INC_PTR(ptr) ((ptr + 1) % (buffer->length + 1))
#define DEC_PTR(ptr) ((ptr + buffer->length) % (buffer->length + 1))
/* Function Definition -------------------------------------------------------------------------- */
/* Function Definition --------------------------------------------------------*/
int BufferInit(Buffer_t *buf, const char *name, int type, int len) {
int i;
@ -195,8 +195,7 @@ int BufferVar(Buffer_t *buffer, float *var) {
}
int BufferStd(Buffer_t *buffer, float *std) {
int ret;
ret = BufferVar(buffer, std);
*std = (float)sqrt(*std);
int ret = BufferVar(buffer, std);
*std = sqrtf(*std);
return ret;
}

2
1.Software/PDR 1.05/src/Complex.cpp
View File

@ -15,7 +15,7 @@
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
float ComplexAbs(Complex_t c) {
return (float)sqrt(c.real * c.real + c.image * c.image);
return sqrtf(c.real * c.real + c.image * c.image);
}
/**---------------------------------------------------------------------

12
1.Software/PDR 1.05/src/Interface.c
View File

@ -17,14 +17,14 @@
#include "scene_recognition.h"
/* Global Variable Definition ------------------------------------------------*/
const char* PDR_Version = "2.0";
const char* Pedometer_Version = "2.0";
Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}};
LatLng_t llaLast = { 0 };
Nmea_t Nmea;
IMU_t Imu;
/* Extern Variable Definition ------------------------------------------------*/
extern EKFPara_t g_kfPara;
extern EKFPara_t EkfPara;
extern AHRS_t Ahrs;
extern GNSS_t pgnss;
@ -115,7 +115,7 @@ void Algorithm_Init(void) {
*---------------------------------------------------------------------**/
int ParseDataAndUpdate(char* line, LctFs_t* LocFusion)
{
ParseLineStr(line, &Imu, &Nmea);
ParseGnssInsData(line, &Imu, &Nmea);
// 给定Imu和Nmea结构体，返回融合位置
return PDRLocationMainLoop(&Imu, &Nmea, LocFusion);
}
@ -133,7 +133,7 @@ int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) {
if (imu->gyr.update) {
// 如果陀螺仪更新则采用惯性传感器计算
InsLocation(imu,&g_kfPara);
InsLocationUpdate(imu,&EkfPara);
imu->gyr.update = UN_UPDATE;
}
// 如果GPS不更新，返回
@ -143,7 +143,7 @@ int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) {
SaveGnssInfo(nmea, LocFusion, NULL);
// 有GPS则采用GPS融合定位
int flag = GnssInsFusionLocation(nmea, &g_kfPara, LocFusion);
int flag = GnssInsFusionLocation(nmea, &EkfPara, LocFusion);
if (flag != TYPE_FIX_NONE) {
// 如果是开车这种，直接输出GPS，不进行平滑处理
@ -198,7 +198,7 @@ void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps)
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
const char* GetPDRVersion(void){
return PDR_Version;
return Pedometer_Version;
}
/**----------------------------------------------------------------------

8
1.Software/PDR 1.05/src/Location.c
View File

@ -27,7 +27,7 @@ PDR_t pdr; // PDRȫ
StepPredict stepPredict = { 0 };
Classifer_t sys;
GNSS_t pgnss; // 定位相关信息
EKFPara_t g_kfPara;
EKFPara_t EkfPara;
double GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status;
extern AHRS_t Ahrs;
@ -45,7 +45,7 @@ float bias_dir[2] = { 0 };
*---------------------------------------------------------------------**/
void PDR_Init(void) {
memset(&g_kfPara, 0, sizeof(g_kfPara));
memset(&EkfPara, 0, sizeof(EkfPara));
memset(&pgnss, 0, sizeof(pgnss));
memset(GpsPos, 0, sizeof(GpsPos));
memset(&stepPredict, 0, sizeof(stepPredict));
@ -115,11 +115,11 @@ void ResetSystemStatus(EKFPara_t *kf)
}
/**---------------------------------------------------------------------
* Function : InsLocation
* Function : InsLocationUpdate
* Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) {
// AHRS姿态更新占2%的运行时间
if (UpdateAHRS(ImuData)) {

2
1.Software/PDR 1.05/src/ParseData.c
View File

@ -848,7 +848,7 @@ void parseLocAccuracy(char* pt, Nmea_t *ln, double ts)
* Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
*---------------------------------------------------------------------**/
int ParseLineStr(char* line, IMU_t* imu_p, Nmea_t* ln)
int ParseGnssInsData(char* line, IMU_t* imu_p, Nmea_t* ln)
{
char* pt = NULL;
int i = 0, sstp = -1;

45
1.Software/PDR 1.05/src/Pedometer.c
View File

@ -1,9 +1,9 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : step.c
* Department : Sensor Algorithm Team
* Current Version : V1.2
* Author : logzhan
* Date of Issued : 2020.7.5
/******************** (C) COPYRIGHT 2023 Geek************************************
* File Name : Pedometer.c
* Department : Geek Technology
* Current Version : V1.0.0
* Author : zhanli
* Date of Issued : 2023.02.24
* Comments : PDR计步器相关
********************************************************************************/
#include <stdio.h>
@ -12,7 +12,7 @@
#include <stdlib.h>
#include "Pedometer.h"
const static char* PDR_Version = "1.0.0.20190105_qcom_sdm660_r";
const static char* Pedometer_Version = "Pedometer.1.0.0.20230224";
static void Steps_Acc_Save(double ax, double ay, double az);
static void State_Detect(unsigned long long timeStamp);
@ -21,7 +21,7 @@ static void Steps_Feature(void);
static void Steps_Rcording(unsigned long long timeStamp);
static double stepsFiltAcc[AxisNumber][AccelerationNumber]/* = { 0 }*/;
static double stepsFiltAcc[AxisNumber][AccelerationNumber];
static float AccData[ACC_INSPECT_TIMEWIDTH] = { 0 };
static float lastAccZ = 0;
@ -33,15 +33,15 @@ static double DifFiltCoefficientTwo[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056,
static double DifFiltCoefficientThree[13] = { 0.000416493127863390, 0.00166597251145356, 0.00416493127863390, 0.00832986255726780, 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, 0.0962099125364431 };
static double DifFiltCoefficientFour[17] = { 0.000152415790275873, 0.000609663161103490, 0.00152415790275873, 0.00304831580551745, 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, 0.0745313214449017 };
static double DifFiltOutput[AxisNumberFiltOut][5]/* = { 0 }*/;
static double OutputFilt[AxisNumberFiltOut][2]/* = { 0 }*/;
static double StepLcVar[AxisNumberFiltOut][3]/* = { 0 }*/;
static double DifFiltOutput[AxisNumberFiltOut][5];
static double OutputFilt[AxisNumberFiltOut][2];
static double StepLcVar[AxisNumberFiltOut][3];
static char StepReFeature[AxisNumberFiltOut] = { 0 };
static double StepFetureAmp[AxisNumberFiltOut][8]/* = { 0 }*/;
static unsigned short StepFetureTime[AxisNumberFiltOut][8]/* = { 0 }*/;
static double StepRecordeState[AxisNumberStepSaveState][4]/* = { 0 }*/;
static double StepRecordeStateTime[AxisNumberStepSaveState][9]/* = { 0 }*/;
static unsigned long long timeStampSave[AxisNumberStepSaveState][2]/* = { 0 }*/;
static double StepFetureAmp[AxisNumberFiltOut][8];
static unsigned short StepFetureTime[AxisNumberFiltOut][8];
static double StepRecordeState[AxisNumberStepSaveState][4];
static double StepRecordeStateTime[AxisNumberStepSaveState][9];
static unsigned long long timeStampSave[AxisNumberStepSaveState][2];
static unsigned short StepMaxTime = 100;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ40ms,25HZ*/
@ -64,15 +64,13 @@ stateNowDfine stateNow;
stateDfine Restate;
/**---------------------------------------------------------------------
* Function : getPedometerLibVersion
* Function : GetPedometerVersion
* Description : 获取计步器版本
* Date : 2020/2/20
*
*
* Date : 2023/02/24
*---------------------------------------------------------------------**/
const char* getPedometerLibVersion(void)
const char* GetPedometerVersion(void)
{
return PDR_Version;
return Pedometer_Version;
}
/**---------------------------------------------------------------------
@ -1319,7 +1317,6 @@ static void State_Detect(unsigned long long timeStamp)
StateDetectLcvar[0] = 0;
}
}
void Steps_State_Detect(enumState* state)
{
void Steps_State_Detect(enumState* state){
*state = stateLcVar[0];
}

7
1.Software/PDR 1.05/src/Utils.c
View File

@ -114,12 +114,11 @@ float InvSqrt(float x) {
}
/**---------------------------------------------------------------------
* Function : pdr_v3Norm
* Function : Vec3Norm
* Description : 三维浮点数向量归一化
* Date : 2021/01/26 yuanlin@vivocom &&
*
* Date : 2022/11/1 logzhan
*---------------------------------------------------------------------**/
void pdr_v3Norm(float* vx, float* vy, float* vz)
void Vec3Norm(float* vx, float* vy, float* vz)
{
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
(*vz) * (*vz)));