Minor fixes 08 (#2565)

* correct indexes

* best metrics yet 30231f46-08af-477e-9d30-776593913c24/700

* sometimes x decreases

* update model refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

models/supercombo.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:53995037e75ef96e61d14a81d64de402edb5ca035ad0cb32bdc707484e24f348
+oid sha256:642f549aa746dbf08930a5bd008b54f6d967d86ccc3ce8284db09086b2f9ef28
 size 63447275

models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f154a42ad1a6e2e50eac88aefe035d3956be33f3e416acc01b02427c3b82fa19
+oid sha256:a9046e77c6972bbc4445b9f0e715f3e87719e2bf099fb943c75e5ae7091b9d29
 size 63313285

selfdrive/modeld/models/driving.cc

@@ -325,14 +325,6 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
       plan_mhp_max_idx = i;
     }
   }
-  float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
-  valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
-  int valid_len_idx = 0;
-  for (int i=1; i<TRAJECTORY_SIZE; i++) {
-    if (valid_len >= X_IDXS[valid_len_idx]){
-      valid_len_idx = i;
-    }
-  }
 
   float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
   float plan_t_arr[TRAJECTORY_SIZE];
@@ -342,12 +334,12 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
   auto position = framed.initPosition();
   fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
-  auto orientation = framed.initOrientation();
-  fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
   auto velocity = framed.initVelocity();
-  fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 6, plan_t_arr);
+  fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
+  auto orientation = framed.initOrientation();
+  fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
   auto orientation_rate = framed.initOrientationRate();
-  fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
+  fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
 
   // lane lines
   auto lane_lines = framed.initLaneLines(4);
@@ -411,9 +403,17 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
       plan_mhp_max_idx = i;
     }
   }
+
   // x pos at 10s is a good valid_len
-  float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
+  float valid_len = 0;
-  // clamp to 5 and MODEL_PATH_DISTANCE
+  float valid_len_candidate;
+  for (int i=1; i<TRAJECTORY_SIZE; i++) {
+    valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
+    if (valid_len_candidate >= valid_len){
+      valid_len = valid_len_candidate;
+    }
+  }
+  // clamp to 10 and MODEL_PATH_DISTANCE
   valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
   int valid_len_idx = 0;
   for (int i=1; i<TRAJECTORY_SIZE; i++) {

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-12bddb985d32a5efe460224fef55a47f92ecaae3
+0d5ecd64a10debf2f62e27fc23738ccce9d1cdb3