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 惯性导航定位 * Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan * 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); 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 * Function : HexToDec

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

@ -139,7 +139,7 @@ typedef struct {
* Description : 获取计步器版本 * Description : 获取计步器版本
* Date : 2020/2/20 * Date : 2020/2/20
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
const char* getPedometerLibVersion(void); const char* GetPedometerVersion(void);
/**--------------------------------------------------------------------- /**---------------------------------------------------------------------
* Function : initPedometer * 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 * 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; float* q = Ahrs.q;
// 归一化磁力计和加速度计 // 归一化磁力计和加速度计
pdr_v3Norm(&ax, &ay, &az); Vec3Norm(&ax, &ay, &az);
float wx = 0.0f, wy = 0.0f, wz = 0.0f; 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]); 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.Pitch = asinf(-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.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 = (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.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) { void AHRS_Init(void) {

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

@ -6,17 +6,17 @@
* Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取 * Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取
* 和计算 * 和计算
********************************************************************************/ ********************************************************************************/
/* Header File Including ------------------------------------------------------------------------ */ /* Header File Including ------------------------------------------------------*/
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include "buffer.h" #include "buffer.h"
/* Macro Declaration ---------------------------------------------------------------------------- */ /* Macro Declaration ----------------------------------------------------------*/
#define ADD_PTR(ptr, ofs) ((ptr + ofs) % (buffer->length + 1)) #define ADD_PTR(ptr, ofs) ((ptr + ofs) % (buffer->length + 1))
#define INC_PTR(ptr) ((ptr + 1) % (buffer->length + 1)) #define INC_PTR(ptr) ((ptr + 1) % (buffer->length + 1))
#define DEC_PTR(ptr) ((ptr + buffer->length) % (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 BufferInit(Buffer_t *buf, const char *name, int type, int len) {
int i; int i;
@ -115,18 +115,18 @@ int BufferPop(Buffer_t *buffer, float *value) {
int BufferPush(Buffer_t *buffer, float value) { int BufferPush(Buffer_t *buffer, float value) {
if (NULL == buffer) { if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER; /*BUFFER_WRONG_PARAMETER值为1*/ return BUFFER_WRONG_PARAMETER; /* BUFFER_WRONG_PARAMETER值为1 */
} }
buffer->data[buffer->_top] = value; buffer->data[buffer->_top] = value;
buffer->_top = INC_PTR(buffer->_top); /*INC_PTR为加1（超过buffer长度就循环加1）*/ buffer->_top = INC_PTR(buffer->_top); /* INC_PTR为加1（超过buffer长度就循环加1）*/
if (buffer->_top == buffer->_bottom) { if (buffer->_top == buffer->_bottom) {
buffer->_bottom = INC_PTR(buffer->_bottom); buffer->_bottom = INC_PTR(buffer->_bottom);
return BUFFER_OUT_OF_MEMORY; /*BUFFER_OUT_OF_MEMORY值为2*/ return BUFFER_OUT_OF_MEMORY; /* BUFFER_OUT_OF_MEMORY值为2 */
} }
else { else {
return BUFFER_NO_ERROR; /*BUFFER_NO_ERROR值为0*/ return BUFFER_NO_ERROR; /* BUFFER_NO_ERROR值为0 */
} }
} }
@ -195,8 +195,7 @@ int BufferVar(Buffer_t *buffer, float *var) {
} }
int BufferStd(Buffer_t *buffer, float *std) { int BufferStd(Buffer_t *buffer, float *std) {
int ret; int ret = BufferVar(buffer, std);
ret = BufferVar(buffer, std); *std = sqrtf(*std);
*std = (float)sqrt(*std);
return ret; return ret;
} }

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

@ -15,7 +15,7 @@
* Date : 2022/10/15 logzhan * Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
float ComplexAbs(Complex_t c) { 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" #include "scene_recognition.h"
/* Global Variable Definition ------------------------------------------------*/ /* Global Variable Definition ------------------------------------------------*/
const char* PDR_Version = "2.0"; const char* Pedometer_Version = "2.0";
Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}}; Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}};
LatLng_t llaLast = { 0 }; LatLng_t llaLast = { 0 };
Nmea_t Nmea; Nmea_t Nmea;
IMU_t Imu; IMU_t Imu;
/* Extern Variable Definition ------------------------------------------------*/ /* Extern Variable Definition ------------------------------------------------*/
extern EKFPara_t g_kfPara; extern EKFPara_t EkfPara;
extern AHRS_t Ahrs; extern AHRS_t Ahrs;
extern GNSS_t pgnss; extern GNSS_t pgnss;
@ -115,7 +115,7 @@ void Algorithm_Init(void) {
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
int ParseDataAndUpdate(char* line, LctFs_t* LocFusion) int ParseDataAndUpdate(char* line, LctFs_t* LocFusion)
{ {
ParseLineStr(line, &Imu, &Nmea); ParseGnssInsData(line, &Imu, &Nmea);
// 给定Imu和Nmea结构体，返回融合位置 // 给定Imu和Nmea结构体，返回融合位置
return PDRLocationMainLoop(&Imu, &Nmea, LocFusion); return PDRLocationMainLoop(&Imu, &Nmea, LocFusion);
} }
@ -133,7 +133,7 @@ int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) {
if (imu->gyr.update) { if (imu->gyr.update) {
// 如果陀螺仪更新则采用惯性传感器计算 // 如果陀螺仪更新则采用惯性传感器计算
InsLocation(imu,&g_kfPara); InsLocationUpdate(imu,&EkfPara);
imu->gyr.update = UN_UPDATE; imu->gyr.update = UN_UPDATE;
} }
// 如果GPS不更新，返回 // 如果GPS不更新，返回
@ -143,7 +143,7 @@ int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) {
SaveGnssInfo(nmea, LocFusion, NULL); SaveGnssInfo(nmea, LocFusion, NULL);
// 有GPS则采用GPS融合定位 // 有GPS则采用GPS融合定位
int flag = GnssInsFusionLocation(nmea, &g_kfPara, LocFusion); int flag = GnssInsFusionLocation(nmea, &EkfPara, LocFusion);
if (flag != TYPE_FIX_NONE) { if (flag != TYPE_FIX_NONE) {
// 如果是开车这种，直接输出GPS，不进行平滑处理 // 如果是开车这种，直接输出GPS，不进行平滑处理
@ -198,7 +198,7 @@ void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps)
* Date : 2022/10/15 logzhan * Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
const char* GetPDRVersion(void){ 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 }; StepPredict stepPredict = { 0 };
Classifer_t sys; Classifer_t sys;
GNSS_t pgnss; // 定位相关信息 GNSS_t pgnss; // 定位相关信息
EKFPara_t g_kfPara; EKFPara_t EkfPara;
double GpsPos[HIST_GPS_NUM][3] = {{0}}; double GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status; static uint32_t g_status;
extern AHRS_t Ahrs; extern AHRS_t Ahrs;
@ -45,7 +45,7 @@ float bias_dir[2] = { 0 };
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
void PDR_Init(void) { void PDR_Init(void) {
memset(&g_kfPara, 0, sizeof(g_kfPara)); memset(&EkfPara, 0, sizeof(EkfPara));
memset(&pgnss, 0, sizeof(pgnss)); memset(&pgnss, 0, sizeof(pgnss));
memset(GpsPos, 0, sizeof(GpsPos)); memset(GpsPos, 0, sizeof(GpsPos));
memset(&stepPredict, 0, sizeof(stepPredict)); memset(&stepPredict, 0, sizeof(stepPredict));
@ -115,11 +115,11 @@ void ResetSystemStatus(EKFPara_t *kf)
} }
/**--------------------------------------------------------------------- /**---------------------------------------------------------------------
* Function : InsLocation * Function : InsLocationUpdate
* Description : PDR 惯性导航定位 * Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan * Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) {
// AHRS姿态更新占2%的运行时间 // AHRS姿态更新占2%的运行时间
if (UpdateAHRS(ImuData)) { 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 : 解析字符串信息，根据传感器数据类型，选择不同的函数解析 * Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析
* Date : 2020/7/9 yuanlin@vivo.com &logzhan * 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; char* pt = NULL;
int i = 0, sstp = -1; int i = 0, sstp = -1;

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

@ -1,9 +1,9 @@
/******************** (C) COPYRIGHT 2020 Geek************************************ /******************** (C) COPYRIGHT 2023 Geek************************************
* File Name : step.c * File Name : Pedometer.c
* Department : Sensor Algorithm Team * Department : Geek Technology
* Current Version : V1.2 * Current Version : V1.0.0
* Author : logzhan * Author : zhanli
* Date of Issued : 2020.7.5 * Date of Issued : 2023.02.24
* Comments : PDR计步器相关 * Comments : PDR计步器相关
********************************************************************************/ ********************************************************************************/
#include <stdio.h> #include <stdio.h>
@ -12,7 +12,7 @@
#include <stdlib.h> #include <stdlib.h>
#include "Pedometer.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 Steps_Acc_Save(double ax, double ay, double az);
static void State_Detect(unsigned long long timeStamp); static void State_Detect(unsigned long long timeStamp);
@ -21,9 +21,9 @@ static void Steps_Feature(void);
static void Steps_Rcording(unsigned long long timeStamp); 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 AccData[ACC_INSPECT_TIMEWIDTH] = { 0 };
static float lastAccZ = 0; static float lastAccZ = 0;
static unsigned int ReceivedHowManyAcc = 0; static unsigned int ReceivedHowManyAcc = 0;
static double SumAcc = 0; static double SumAcc = 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 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 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 DifFiltOutput[AxisNumberFiltOut][5];
static double OutputFilt[AxisNumberFiltOut][2]/* = { 0 }*/; static double OutputFilt[AxisNumberFiltOut][2];
static double StepLcVar[AxisNumberFiltOut][3]/* = { 0 }*/; static double StepLcVar[AxisNumberFiltOut][3];
static char StepReFeature[AxisNumberFiltOut] = { 0 }; static char StepReFeature[AxisNumberFiltOut] = { 0 };
static double StepFetureAmp[AxisNumberFiltOut][8]/* = { 0 }*/; static double StepFetureAmp[AxisNumberFiltOut][8];
static unsigned short StepFetureTime[AxisNumberFiltOut][8]/* = { 0 }*/; static unsigned short StepFetureTime[AxisNumberFiltOut][8];
static double StepRecordeState[AxisNumberStepSaveState][4]/* = { 0 }*/; static double StepRecordeState[AxisNumberStepSaveState][4];
static double StepRecordeStateTime[AxisNumberStepSaveState][9]/* = { 0 }*/; static double StepRecordeStateTime[AxisNumberStepSaveState][9];
static unsigned long long timeStampSave[AxisNumberStepSaveState][2]/* = { 0 }*/; static unsigned long long timeStampSave[AxisNumberStepSaveState][2];
static unsigned short StepMaxTime = 100; 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*/ /*<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; stateDfine Restate;
/**--------------------------------------------------------------------- /**---------------------------------------------------------------------
* Function : getPedometerLibVersion * Function : GetPedometerVersion
* Description : 获取计步器版本 * 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; StateDetectLcvar[0] = 0;
} }
} }
void Steps_State_Detect(enumState* state) void Steps_State_Detect(enumState* state){
{
*state = stateLcVar[0]; *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 : 三维浮点数向量归一化 * 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) + float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
(*vz) * (*vz))); (*vz) * (*vz)));