| | |
| | | // |
| | | |
| | | #include "odo_graph.h" |
| | | #include "../test_items2/road_exam.h" |
| | | #include "../common/apptimer.h" |
| | | #include "../utils/xconvert.h" |
| | | #include "../driver_test.h" |
| | | |
| | | static double odoGraph; |
| | | static struct RtkTime odoTimer; |
| | | static double odoPrevSpeed; |
| | | static int odoCnt; |
| | | static int64_t prevTimestamp; |
| | | static double prevSpeed; |
| | | static bool isstop; |
| | | |
| | | void ResetOdo(void) |
| | | { |
| | | odoCnt = 0; |
| | | isstop = true; |
| | | odoGraph = 0; |
| | | } |
| | | |
| | |
| | | return odoGraph; |
| | | } |
| | | |
| | | void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) { |
| | | void UpdataOdo(motion_t &motion) { |
| | | // 行驶距离,含倒车 |
| | | if (odoCnt == 0 && moveDirect != 0) { |
| | | odoPrevSpeed = speed; |
| | | odoCnt = 1; |
| | | odoTimer = *rtkTime; |
| | | } else if (odoCnt == 1) { |
| | | if (moveDirect != 0) { |
| | | uint32_t tm = TimeGetDiff(rtkTime, &odoTimer); |
| | | if (tm >= D_SEC(1)) { |
| | | odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0; |
| | | odoTimer = *rtkTime; |
| | | odoPrevSpeed = speed; |
| | | if (isstop && motion.move != STOP) { |
| | | prevSpeed = motion.speed; |
| | | isstop = false; |
| | | prevTimestamp = motion.timestamp; |
| | | } else if (!isstop) { |
| | | if (motion.move != STOP) { |
| | | uint32_t elapsed = motion.timestamp - prevTimestamp; |
| | | if (elapsed >= D_SEC(5)) { // 中途长时间未定位,重新开始测量 |
| | | prevSpeed = motion.speed; |
| | | prevTimestamp = motion.timestamp; |
| | | return; |
| | | } |
| | | if (elapsed >= D_SEC(1)) { |
| | | odoGraph += ((double) elapsed) * (prevSpeed + motion.speed) / 2.0 / 1000.0; |
| | | prevTimestamp = motion.timestamp; |
| | | prevSpeed = motion.speed; |
| | | } |
| | | } else { |
| | | odoCnt = 0; |
| | | isstop = true; |
| | | } |
| | | } |
| | | } |