diff --git a/1.Software/PDR 1.05/include/Location.h b/1.Software/PDR 1.05/include/Location.h index bb1d35a..9166fc3 100644 --- a/1.Software/PDR 1.05/include/Location.h +++ b/1.Software/PDR 1.05/include/Location.h @@ -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); /**---------------------------------------------------------------------- diff --git a/1.Software/PDR 1.05/include/ParseData.h b/1.Software/PDR 1.05/include/ParseData.h index 6ee6676..9631850 100644 --- a/1.Software/PDR 1.05/include/ParseData.h +++ b/1.Software/PDR 1.05/include/ParseData.h @@ -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 diff --git a/1.Software/PDR 1.05/include/Pedometer.h b/1.Software/PDR 1.05/include/Pedometer.h index ba6e713..c333fa1 100644 --- a/1.Software/PDR 1.05/include/Pedometer.h +++ b/1.Software/PDR 1.05/include/Pedometer.h @@ -139,7 +139,7 @@ typedef struct { * Description : 获取计步器版本 * Date : 2020/2/20 *---------------------------------------------------------------------**/ -const char* getPedometerLibVersion(void); +const char* GetPedometerVersion(void); /**--------------------------------------------------------------------- * Function : initPedometer diff --git a/1.Software/PDR 1.05/include/Utils.h b/1.Software/PDR 1.05/include/Utils.h index 05ceaf7..0702015 100644 --- a/1.Software/PDR 1.05/include/Utils.h +++ b/1.Software/PDR 1.05/include/Utils.h @@ -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 diff --git a/1.Software/PDR 1.05/src/AHRS.c b/1.Software/PDR 1.05/src/AHRS.c index 1147048..8654bc0 100644 --- a/1.Software/PDR 1.05/src/AHRS.c +++ b/1.Software/PDR 1.05/src/AHRS.c @@ -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) { diff --git a/1.Software/PDR 1.05/src/Buffer.c b/1.Software/PDR 1.05/src/Buffer.c index 4b9846c..ea53172 100644 --- a/1.Software/PDR 1.05/src/Buffer.c +++ b/1.Software/PDR 1.05/src/Buffer.c @@ -6,17 +6,17 @@ * Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取 * 和计算 ********************************************************************************/ -/* Header File Including ------------------------------------------------------------------------ */ +/* Header File Including ------------------------------------------------------*/ #include #include #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; @@ -115,18 +115,18 @@ int BufferPop(Buffer_t *buffer, float *value) { int BufferPush(Buffer_t *buffer, float value) { 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->_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) { 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 { - 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 ret; - ret = BufferVar(buffer, std); - *std = (float)sqrt(*std); + int ret = BufferVar(buffer, std); + *std = sqrtf(*std); return ret; } \ No newline at end of file diff --git a/1.Software/PDR 1.05/src/Complex.cpp b/1.Software/PDR 1.05/src/Complex.cpp index e1c0f92..1061770 100644 --- a/1.Software/PDR 1.05/src/Complex.cpp +++ b/1.Software/PDR 1.05/src/Complex.cpp @@ -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); } /**--------------------------------------------------------------------- diff --git a/1.Software/PDR 1.05/src/Interface.c b/1.Software/PDR 1.05/src/Interface.c index e336842..b8a6a43 100644 --- a/1.Software/PDR 1.05/src/Interface.c +++ b/1.Software/PDR 1.05/src/Interface.c @@ -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; } /**---------------------------------------------------------------------- diff --git a/1.Software/PDR 1.05/src/Location.c b/1.Software/PDR 1.05/src/Location.c index 517e2e8..3a29841 100644 --- a/1.Software/PDR 1.05/src/Location.c +++ b/1.Software/PDR 1.05/src/Location.c @@ -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)) { diff --git a/1.Software/PDR 1.05/src/ParseData.c b/1.Software/PDR 1.05/src/ParseData.c index 14170ef..196b8f2 100644 --- a/1.Software/PDR 1.05/src/ParseData.c +++ b/1.Software/PDR 1.05/src/ParseData.c @@ -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; diff --git a/1.Software/PDR 1.05/src/Pedometer.c b/1.Software/PDR 1.05/src/Pedometer.c index 748df46..e25aecf 100644 --- a/1.Software/PDR 1.05/src/Pedometer.c +++ b/1.Software/PDR 1.05/src/Pedometer.c @@ -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 @@ -12,7 +12,7 @@ #include #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,9 +21,9 @@ 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; +static float lastAccZ = 0; static unsigned int ReceivedHowManyAcc = 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 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; /*锟斤拷锟斤拷锟斤拷锟节达拷锟斤拷锟斤拷锟斤拷锟姐法锟侥诧拷锟斤拷锟斤拷锟斤拷为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]; } \ No newline at end of file diff --git a/1.Software/PDR 1.05/src/Utils.c b/1.Software/PDR 1.05/src/Utils.c index c7010db..1aeb5fb 100644 --- a/1.Software/PDR 1.05/src/Utils.c +++ b/1.Software/PDR 1.05/src/Utils.c @@ -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)));