40 lines
1.4 KiB
C
40 lines
1.4 KiB
C
|
/******************** (C) COPYRIGHT 2020 Geek************************************
|
|||
|
|
* File Name : PDRBase.c
|
|||
|
|
* Current Version : V2.0
|
|||
|
|
* Author : logzhan
|
|||
|
|
* Date of Issued : 2022.10.15
|
|||
|
|
* Comments : PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܺ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
********************************************************************************/
|
|||
|
|
#include "PDRBase.h"
|
|||
|
|
|
|||
|
|
//extern double GpsPos[HIST_GPS_NUM][3];
|
|||
|
|
|
|||
|
|
//int predictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
|
|||
|
|
//{
|
|||
|
|
// int step = 0;
|
|||
|
|
// float mean_time = 400;
|
|||
|
|
// double dis = 0;
|
|||
|
|
//
|
|||
|
|
// if (stepPredict->meanTime > 0)
|
|||
|
|
// mean_time = stepPredict->meanTime;
|
|||
|
|
//
|
|||
|
|
// if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
|
|||
|
|
// {
|
|||
|
|
// stepPredict->fsum = stepPredict->fsum / stepPredict->fnum;
|
|||
|
|
// dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0] - GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2)
|
|||
|
|
// + pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2));
|
|||
|
|
//
|
|||
|
|
// if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
|
|||
|
|
// {
|
|||
|
|
// step = (int)((timestamp - stepPredict->lastTime) / mean_time);
|
|||
|
|
// stepPredict->deltaStep += step;
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
// stepPredict->lastTime = timestamp;
|
|||
|
|
// stepPredict->fnum = 0;
|
|||
|
|
// stepPredict->fsum = 0;
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
// return step;
|
|||
|
|
//}
|