From d2a2ccfee45fdf94425354efbfd17fabce4cb5e7 Mon Sep 17 00:00:00 2001 From: Joost Wooning Date: Fri, 23 Apr 2021 02:55:41 +0200 Subject: [PATCH] compare carspeed float to epsilon instead of zero (#20714) * compare carspeed float to epsilon instead of zero * update ref * add ref commit again Co-authored-by: Harald Schafer --- selfdrive/locationd/locationd.cc | 4 ++-- selfdrive/locationd/locationd.h | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 427c29c41b..00996b3198 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -239,9 +239,9 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re this->speed_counter++; if (this->speed_counter % SENSOR_DECIMATION == 0) { - this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgo()).finished() }); + this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() }); this->car_speed = std::abs(log.getVEgo()); - if (log.getVEgo() == 0.0) { // TODO probably never really 0.0 + if (this->car_speed < 1e-3) { this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); } } diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index a11cdeb60c..6c00a43a10 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -53,7 +53,7 @@ private: MatrixXdr calib_from_device; bool calibrated = false; - int car_speed = 0; + double car_speed = 0.0; std::deque posenet_stds; std::unique_ptr converter; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7ddcabce67..b1cc095f59 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -bb1af9fc73c15967a4160d8899f2688b059627bf \ No newline at end of file +381c4976c45f232f413b37945fbcd43391174089 \ No newline at end of file