diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 676a2ace37..8a62ed3f04 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:dc46a24d4b4afa9730785264834e7f7c04c84b6a28a689acb503d6663818c256 -size 58797567 +oid sha256:d5d753debf1796223896637cd62f2bee881eeb3290d9e739f5d3c7910ab542f2 +size 56720671 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 6b3ec952c1..ffb2ccff47 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:77a31e5a3a70c39a3fcb07e939a668baf80b1eb778fe792fbe256de983fab5cd -size 58823636 +oid sha256:adcc169da199333b315548cc210a4860f24e9b3144f776315632488e194316c6 +size 56742706 diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 4eb3068054..49e200d543 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,7 +1,7 @@ #pragma once const int TRAJECTORY_SIZE = 33; -const int LON_MPC_N = 32; const int LAT_MPC_N = 16; +const int LON_MPC_N = 32; const float MIN_DRAW_DISTANCE = 10.0; const float MAX_DRAW_DISTANCE = 100.0; diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 0c258d8dba..1c3ca89898 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -55,6 +55,7 @@ class LateralPlanner(): self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 + self.keep_pulse_timer = 0.0 self.prev_one_blinker = False self.desire = log.LateralPlan.Desire.none @@ -157,6 +158,16 @@ class LateralPlanner(): self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + # Send keep pulse once per second during LaneChangeStart.preLaneChange + if self.lane_change_state in [LaneChangeState.off, LaneChangeState.laneChangeStarting]: + self.keep_pulse_timer = 0.0 + elif self.lane_change_state == LaneChangeState.preLaneChange: + self.keep_pulse_timer += DT_MDL + if self.keep_pulse_timer > 1.0: + self.keep_pulse_timer = 0.0 + elif self.desire in [log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight]: + self.desire = log.LateralPlan.Desire.none + # Turn off lanes during lane change if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 132bba8bd3..01498fa138 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -132,11 +132,11 @@ class Cluster(): def get_RadarState_from_vision(self, lead_msg, v_ego): return { - "dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA), - "yRel": float(-lead_msg.xyva[1]), - "vRel": float(lead_msg.xyva[2]), - "vLead": float(v_ego + lead_msg.xyva[2]), - "vLeadK": float(v_ego + lead_msg.xyva[2]), + "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), + "yRel": float(-lead_msg.y[0]), + "vRel": float(lead_msg.v[0] - v_ego), + "vLead": float(lead_msg.v[0]), + "vLeadK": float(lead_msg.v[0]), "aLeadK": float(0), "aLeadTau": _LEAD_ACCEL_TAU, "fcw": False, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 244d02d23f..5d94804d75 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -38,12 +38,12 @@ def laplacian_cdf(x, mu, b): def match_vision_to_cluster(v_ego, lead, clusters): # match vision point to best statistical cluster match - offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): - prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0]) - prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1]) - prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2]) + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v @@ -53,7 +53,7 @@ def match_vision_to_cluster(v_ego, lead, clusters): # if no 'sane' match is found return -1 # stationary radar points can be false positives dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3) + vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) if dist_sane and vel_sane: return cluster else: @@ -166,9 +166,9 @@ class RadarD(): radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: - if len(sm['modelV2'].leads) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False) + if len(sm['modelV2'].leadsV3) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[1], low_speed_override=False) return dat diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index f7a72b34e1..3fb6f56255 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -24,7 +24,9 @@ constexpr int PLAN_MHP_SELECTION = 1; constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION); constexpr int LEAD_MHP_N = 5; -constexpr int LEAD_MHP_VALS = 4; +constexpr int LEAD_TRAJ_LEN = 6; +constexpr int LEAD_PRED_DIM = 4; +constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN; constexpr int LEAD_MHP_SELECTION = 3; constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION); @@ -147,18 +149,38 @@ void fill_sigmoid(const float *input, float *output, int len, int stride) { } } -void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) { +void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *lead_data, const float *prob, int t_offset, float prob_t) { + float t[LEAD_TRAJ_LEN] = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; const float *data = get_lead_data(lead_data, t_offset); lead.setProb(sigmoid(prob[t_offset])); - lead.setT(t); - float xyva_arr[LEAD_MHP_VALS]; - float xyva_stds_arr[LEAD_MHP_VALS]; - for (int i=0; i