diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 59aff5fdf6..af3f6a8586 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:cad8bd4bf2f9a15126baf58812515822015fb88e8fed69c0f6270d5c92b9ddf8 -size 78605237 +oid sha256:40fbf72761dc0170a5473f4e95a10519bda43f069717c4dd3ee5f796596fc5ce +size 52636596 diff --git a/models/supercombo.keras b/models/supercombo.keras index bb43b04948..6180c11ba7 100644 --- a/models/supercombo.keras +++ b/models/supercombo.keras @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:07df32d582b9d3ae687c7af28717cee3fe036cccf6365eb7d174a4e86c521d28 -size 79200720 +oid sha256:9e810ee9b30e5153b3da0bf15a4f096677788a01ab4170e8d0cd3eca2c247973 +size 53232072 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 193db8bc92..413d71d3a2 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -126,8 +126,8 @@ class PathPlanner(): # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: - # fade out lanelines over .5s - self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) + # fade out over .2s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL/5, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing @@ -154,10 +154,6 @@ class PathPlanner(): if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob - self.libmpc.init_weights(MPC_COST_LAT.PATH / 3.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) - else: - self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) - self.LP.update_d_poly(v_ego) # account for actuation delay diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 3ae9618e79..8065331fe0 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -19,7 +19,7 @@ #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE #ifdef TEMPORAL - #define TEMPORAL_SIZE 1024 + #define TEMPORAL_SIZE 512 #else #define TEMPORAL_SIZE 0 #endif @@ -135,18 +135,18 @@ void model_free(ModelState* s) { delete s->m; } -void poly_fit(float *in_pts, float *in_stds, float *out) { +void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { // References to inputs - Eigen::Map > pts(in_pts, MODEL_PATH_DISTANCE); - Eigen::Map > std(in_stds, MODEL_PATH_DISTANCE); + Eigen::Map > pts(in_pts, valid_len); + Eigen::Map > std(in_stds, valid_len); Eigen::Map > p(out, POLYFIT_DEGREE - 1); float y0 = pts[0]; pts = pts.array() - y0; // Build Least Squares equations - Eigen::Matrix lhs = vander.array().colwise() / std.array(); - Eigen::Matrix rhs = pts.array() / std.array(); + Eigen::Matrix lhs = vander.topRows(valid_len).array().colwise() / std.array(); + Eigen::Matrix rhs = pts.array() / std.array(); // Improve numerical stability Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); @@ -170,15 +170,11 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo float prob; float valid_len; - valid_len = data[MODEL_PATH_DISTANCE*2]; + // clamp to 5 and 192 + valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2])); for (int i=0; i stds(&stds_arr[0], ARRAYSIZE(stds_arr));