From dc40631e97739c8e55f271501b6356eda52d84ff Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Fri, 21 May 2021 15:10:59 -0700
Subject: [PATCH] Fix planner low speed behaviour (#20990)

* first point is 0

* first point is always now

* print replay diffs

* new ref

* changed wrong commit
---
 selfdrive/modeld/models/driving.cc                    | 3 ++-
 selfdrive/test/process_replay/camera_replay.py        | 6 ++++++
 selfdrive/test/process_replay/model_replay_ref_commit | 2 +-
 selfdrive/test/process_replay/ref_commit              | 2 +-
 4 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index def5b7e4cb..aea0d43194 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -272,7 +272,8 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
   // plan
   const float *best_plan = get_plan_data(net_outputs.plan);
   float plan_t_arr[TRAJECTORY_SIZE];
-  int xidx = 0, tidx = 0;
+  plan_t_arr[0] = 0.0;
+  int xidx = 1, tidx = 0;
   for (; xidx<TRAJECTORY_SIZE; xidx++) {
     // increment tidx until we find an element that's further away than the current xidx
     for (; tidx < TRAJECTORY_SIZE - 1 && best_plan[(tidx+1)*PLAN_MHP_COLUMNS] < X_IDXS[xidx]; tidx++) {}
diff --git a/selfdrive/test/process_replay/camera_replay.py b/selfdrive/test/process_replay/camera_replay.py
index 05f79cc9c2..95631956a3 100755
--- a/selfdrive/test/process_replay/camera_replay.py
+++ b/selfdrive/test/process_replay/camera_replay.py
@@ -115,6 +115,12 @@ if __name__ == "__main__":
     results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, ignore_fields=ignore)
     diff1, diff2, failed = format_diff(results, ref_commit)
 
+    print(diff2)
+    print('-------------')
+    print('-------------')
+    print('-------------')
+    print('-------------')
+    print('-------------')
     print(diff1)
     with open("model_diff.txt", "w") as f:
       f.write(diff2)
diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit
index fd8e5f4b0f..8c97a7e554 100644
--- a/selfdrive/test/process_replay/model_replay_ref_commit
+++ b/selfdrive/test/process_replay/model_replay_ref_commit
@@ -1 +1 @@
-3a98c2fc53e9a9628eb63c81c4cbd4d72a756386
+b1447cc1b5492ab28a6fafddf25ade7fc66606bf
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index e1c01596e7..58f4b866ee 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-de4fcc981f5a0ec6c43d9ee0918cdf9c3dbde494
\ No newline at end of file
+de4fcc981f5a0ec6c43d9ee0918cdf9c3dbde494