// // Created by YY on 2020/8/11. // #include "odo_graph.h" #include "../common/apptimer.h" #include "../utils/xconvert.h" #include "../driver_test.h" static double odoGraph; static int64_t prevTimestamp; static double prevSpeed; static bool isstop; void ResetOdo(void) { isstop = true; odoGraph = 0; } double ReadOdo(void) { return odoGraph; } void UpdataOdo(motion_t &motion) { // 行驶距离,含倒车 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 { isstop = true; } } }