diff --git a/models/supercombo.dlc b/models/supercombo.dlc index fc01cf0357..b6df1c3b1f 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:53995037e75ef96e61d14a81d64de402edb5ca035ad0cb32bdc707484e24f348 +oid sha256:642f549aa746dbf08930a5bd008b54f6d967d86ccc3ce8284db09086b2f9ef28 size 63447275 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 2937ce0005..ddacbe01c1 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f154a42ad1a6e2e50eac88aefe035d3956be33f3e416acc01b02427c3b82fa19 +oid sha256:a9046e77c6972bbc4445b9f0e715f3e87719e2bf099fb943c75e5ae7091b9d29 size 63313285 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9af34fb8ea..301e7fafb3 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/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= 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]; - // clamp to 5 and MODEL_PATH_DISTANCE + float valid_len = 0; + float valid_len_candidate; + for (int i=1; i= 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