From c553f96a99d17a05342b941475c8c1f0dc8f71d6 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 25 Mar 2020 13:47:12 +0100 Subject: [PATCH 01/12] Simplify Dynamic Follow --- selfdrive/controls/lib/long_mpc.py | 158 ++++------------------------- 1 file changed, 20 insertions(+), 138 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 26c410c05db821..e0032f42bfc9fd 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -37,9 +37,6 @@ def __init__(self, mpc_id): self.pm = messaging_arne.PubMaster(['smiskolData']) else: self.pm = None - self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} - self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} - self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() self.sng = False @@ -74,31 +71,13 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a - def get_TR(self, CS): + def get_TR(self, CS, lead): if not self.lead_data['status'] or travis: TR = 1.8 elif CS.vEgo < 5.0: - TRs = [2.0, 1.8, 1.75, 1.6] - vEgos = [2.0, 3.0, 4.0, 5.0] - #TRs = [1.8, 1.6] - #vEgos =[4.0, 5.0] - TR = interp(CS.vEgo, vEgos, TRs) - p_mod_pos = 1.0 - p_mod_neg = 1.0 - TR_mod = [] - x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0] # relative velocity values - y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.055, 0.01] # modification values - TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)) - - x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0] # lead acceleration values - y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0] # modification values - TR_mod.append(interp(self.calculate_lead_accel(), x, y)) - - self.TR_Mod = sum([mod * p_mod_neg if mod < 0 else mod * p_mod_pos for mod in TR_mod]) # calculate TR_Mod so that the cost function works correctly - + TR = 1.8 else: - self.store_df_data() - TR = self.dynamic_follow(CS) + TR = self.dynamic_follow(CS, lead) if not travis: self.change_cost(TR,CS.vEgo) @@ -114,135 +93,40 @@ def send_cur_TR(self, TR): def change_cost(self, TR, vEgo): TRs = [0.9, 1.8, 2.7] - costs = [0.75, 0.2, 0.075] + costs = [1.0, 0.15, 0.05] cost = interp(TR, TRs, costs) - if vEgo < 5.0: - cost = 0.2 - cost = cost * min(max(1.0 , (6.0 - vEgo)),5.0) - if self.TR_Mod > 0: - cost = cost + self.TR_Mod/2.0 if self.last_cost != cost: self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.last_cost = cost - def store_df_data(self): - v_lead_retention = 1.9 # keep only last x seconds - v_ego_retention = 2.5 - - cur_time = time.time() - if self.lead_data['status']: - self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if - cur_time - sample['time'] <= v_lead_retention - and not self.new_lead] # reset when new lead - self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': cur_time}) - - self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if cur_time - sample['time'] <= v_ego_retention] - self.df_data['v_egos'].append({'v_ego': self.car_data['v_ego'], 'time': cur_time}) - - def calculate_lead_accel(self): - min_consider_time = 1.0 # minimum amount of time required to consider calculation - a_lead = self.lead_data['a_lead'] - if len(self.df_data['v_leads']): # if not empty - elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time'] - if elapsed > min_consider_time: # if greater than min time (not 0) - a_calculated = (self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead']) / elapsed # delta speed / delta time - # old version: # if abs(a_calculated) > abs(a_lead) and a_lead < 0.33528: # if a_lead is greater than calculated accel (over last 1.5s, use that) and if lead accel is not above 0.75 mph/s - # a_lead = a_calculated - - # long version of below: if (a_calculated < 0 and a_lead >= 0 and a_lead < -a_calculated * 0.5) or (a_calculated > 0 and a_lead <= 0 and -a_lead > a_calculated * 0.5) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): - if (a_calculated < 0 <= a_lead < -a_calculated * 0.55) or (a_calculated > 0 >= a_lead and -a_lead < a_calculated * 0.45) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): # this is a mess, fix - a_lead = a_calculated - return a_lead # if above doesn't execute, we'll return a_lead from radar - - def dynamic_follow(self, CS): + def dynamic_follow(self, CS, lead): self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() - x_vel = [5.0, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities - p_mod_x = [3, 20, 35] # profile mod speeds + x_vel = [5.0, 55.0] # velocities if self.df_profile == 'roadtrip': - y_dist = [1.6, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74] # TRs - p_mod_pos = [0.99, 0.815, 0.57] - p_mod_neg = [1.0, 1.27, 1.675] + y_dist = [1.8, 2.1] # TRs elif self.df_profile == 'traffic': # for in congested traffic - x_vel = [5.0, 7.4507, 9.3133, 11.5598, 13.645, 17.8816, 22.407, 28.8833, 34.8691, 40.3906] - y_dist = [1.6, 1.437, 1.468, 1.501, 1.506, 1.38, 1.2216, 1.085, 1.0516, 1.016] - p_mod_pos = [1.015, 2.175, 3.65] - p_mod_neg = [0.98, 0.08, 0.0] - # y_dist = [1.384, 1.391, 1.403, 1.415, 1.437, 1.3506, 1.3959, 1.4156, 1.38, 1.1899, 1.026, 0.9859, 0.9432] # from 071-2 (need to fix FCW) - # p_mod_pos = [1.015, 2.2, 3.95] - # p_mod_neg = [0.98, 0.1, 0.0] + x_vel = [5.0, 35.0] + y_dist = [1.8, 1.1] else: # default to relaxed/stock - y_dist = [1.6, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] - p_mod_pos = [1.0, 1.0, 1.0] - p_mod_neg = [1.0, 1.0, 1.0] - - p_mod_pos = interp(self.car_data['v_ego'], p_mod_x, p_mod_pos) - p_mod_neg = interp(self.car_data['v_ego'], p_mod_x, p_mod_neg) - - sng_TR = 1.7 # reacceleration stop and go TR - sng_speed = 15.0 * CV.MPH_TO_MS - - if self.car_data['v_ego'] > sng_speed: # keep sng distance until we're above sng speed again - self.sng = False - - if (self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data['v_ego']) and not self.sng: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease - TR = interp(self.car_data['v_ego'], x_vel, y_dist) - else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating - self.sng = True - x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating - y = [sng_TR, interp(sng_speed, x_vel, y_dist)] - TR = interp(self.car_data['v_ego'], x, y) + y_dist = [1.8, 1.8] TR_mod = [] # Dynamic follow modifications (the secret sauce) - x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0, 0.6847, 1.3772, 1.9007, 2.7452] # relative velocity values - y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.055, 0.0, -0.03, -0.068, -0.142, -0.221] # modification values - TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)) - - x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values - y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0, -0.009, -0.042, -0.053, -0.059] # modification values - TR_mod.append(interp(self.calculate_lead_accel(), x, y)) - - # x = [4.4704, 22.352] # 10 to 50 mph #todo: remove if uneeded/unsafe - # y = [0.94, 1.0] - # TR_mod *= interp(self.car_data['v_ego'], x, y) # modify TR less at lower speeds - - self.TR_Mod = sum([mod * p_mod_neg if mod < 0 else mod * p_mod_pos for mod in TR_mod]) # alter TR modification according to profile - TR += self.TR_Mod - - if CS.leftBlinker or CS.rightBlinker and self.df_profile != 'traffic': - x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph - y = [1.0, .75, .65] # reduce TR when changing lanes - TR *= interp(self.car_data['v_ego'], x, y) + x = [-5.0, 0.0, 5.0] # relative velocity values + y = [0.2, 0.0, -0.2] # modification values + + self.TR_Mod = interp(lead.vRel, x, y) + TR = self.TR_Mod - # TR *= self.get_traffic_level() # modify TR based on last minute of traffic data # todo: look at getting this to work, a model could be used + if CS.leftBlinker or CS.rightBlinker: + x = [9.0, 55.0] # + y = [1.0, 0.65] # reduce TR when changing lanes + TR *= interp(CS.vEgo, x, y) return clip(TR, 0.9, 2.7) - def process_lead(self, v_lead, a_lead, x_lead, status): - self.lead_data['v_lead'] = v_lead - self.lead_data['a_lead'] = a_lead - self.lead_data['x_lead'] = x_lead - self.lead_data['status'] = status - - # def get_traffic_level(self, lead_vels): # generate a value to modify TR by based on fluctuations in lead speed - # if len(lead_vels) < 60: - # return 1.0 # if less than 30 seconds of traffic data do nothing to TR - # lead_vel_diffs = [] - # for idx, vel in enumerate(lead_vels): - # try: - # lead_vel_diffs.append(abs(vel - lead_vels[idx - 1])) - # except: - # pass - # - # x = [0, len(lead_vels)] - # y = [1.15, .9] # min and max values to modify TR by, need to tune - # traffic = interp(sum(lead_vel_diffs), x, y) - # - # return traffic - def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo - self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo} # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -254,7 +138,6 @@ def update(self, pm, CS, lead, v_cruise_setpoint): if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 - self.process_lead(v_lead, a_lead, x_lead, lead.status) self.a_lead_tau = lead.aLeadTau self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: @@ -266,7 +149,6 @@ def update(self, pm, CS, lead, v_cruise_setpoint): self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: - self.process_lead(None, None, None, False) self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 @@ -276,7 +158,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS)) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS, lead)) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: From bd8736520f2fa3f21c14d1863081acfaf546a961 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 25 Mar 2020 14:24:05 +0100 Subject: [PATCH 02/12] Travis fixes --- selfdrive/controls/lib/long_mpc.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index e0032f42bfc9fd..3d327b3e3e2c30 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,6 +1,5 @@ import os import math -import time import cereal.messaging as messaging import cereal.messaging_arne as messaging_arne @@ -12,7 +11,6 @@ from common.op_params import opParams from common.numpy_fast import interp, clip from common.travis_checker import travis -from selfdrive.config import Conversions as CV LOG_MPC = os.environ.get('LOG_MPC', False) @@ -109,14 +107,14 @@ def dynamic_follow(self, CS, lead): y_dist = [1.8, 1.1] else: # default to relaxed/stock y_dist = [1.8, 1.8] - - TR_mod = [] + TR = interp(CS.vEgo, x_vel, y_dist) + # Dynamic follow modifications (the secret sauce) x = [-5.0, 0.0, 5.0] # relative velocity values y = [0.2, 0.0, -0.2] # modification values self.TR_Mod = interp(lead.vRel, x, y) - TR = self.TR_Mod + TR += self.TR_Mod if CS.leftBlinker or CS.rightBlinker: x = [9.0, 55.0] # From 90c96223d962cdec460042a4c5ca16f8d3e3a54e Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 25 Mar 2020 14:41:36 +0100 Subject: [PATCH 03/12] fix last lead_data ref --- selfdrive/controls/lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 3d327b3e3e2c30..37eac66ed06583 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -70,7 +70,7 @@ def set_cur_state(self, v, a): self.cur_state[0].a_ego = a def get_TR(self, CS, lead): - if not self.lead_data['status'] or travis: + if not lead.status or travis: TR = 1.8 elif CS.vEgo < 5.0: TR = 1.8 From c4a0c357851407167482e105779b7dc4ca65547a Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 25 Mar 2020 16:48:48 +0100 Subject: [PATCH 04/12] at 1m before speed limit hand over to pid --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 1daa61ddb9eeac..a56a86381e4093 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -310,7 +310,7 @@ def update(self, sm, pm, CP, VM, PP, arne_sm): time_to_turn = max(1.0, sm['liveMapData'].distToTurn / max((v_ego + v_curvature_map)/2, 1.)) required_decel = min(0, (v_curvature_map - v_ego) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) - if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm['liveMapData'].speedLimitAheadDistance > 0.10 and not following: + if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm['liveMapData'].speedLimitAheadDistance > 1.0 and not following: required_decel = min(0, (v_speedlimit_ahead*v_speedlimit_ahead - v_ego*v_ego)/(sm['liveMapData'].speedLimitAheadDistance*2)) required_decel = max(required_decel, -3.0) decel_for_turn = True From 708c4a8708f125da4c1626956400b726ac3b3d20 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 25 Mar 2020 18:12:24 +0100 Subject: [PATCH 05/12] Clear params on panda connect and not disconnect --- selfdrive/thermald/thermald.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 5c674bc5595bfd..30f1b7222d679e 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -195,8 +195,8 @@ def thermald_thread(): location = location.gpsLocation if location else None msg = read_thermal() - # clear car params when panda gets disconnected - if health is None and health_prev is not None: + # clear car params when panda gets connected + if health is not None and health_prev is None: params.panda_disconnect() health_prev = health From acae7895e3164eee57b40b5cba84d284be9467a8 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 25 Mar 2020 16:03:51 -0700 Subject: [PATCH 06/12] go back to old touch postion for df button. --- selfdrive/ui/ui.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index e9c710856d507c..718fac867973c7 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -95,7 +95,7 @@ static bool handle_df_touch(UIState *s, int touch_x, int touch_y) { //dfButton manager // code below thanks to kumar: https://github.com/arne182/openpilot/commit/71d5aac9f8a3f5942e89634b20cbabf3e19e3e78 if (s->awake && s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) { int padding = 40; - if ((1360 - padding <= touch_x) && (855 - padding <= touch_y)) { + if (touch_x >= 1212 && touch_x <= 1310 && touch_y >= 902 && touch_y <= 1013) { s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping df button s->scene.dfButtonStatus++; if (s->scene.dfButtonStatus > 2) { From 87287f2ae2ebc2c29f44168d9fc07550909dbff0 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 26 Mar 2020 10:33:33 +0100 Subject: [PATCH 07/12] When changing CP values reset the PID to the current speed --- selfdrive/controls/lib/longcontrol.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f68b6575f2cce7..40a65d1a6c9f5f 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -125,16 +125,22 @@ def update(self, active, v_ego, gas_pressed, brake_pressed, standstill, cruise_s self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 0 for x in CP.longitudinalTuning.kiV]) self.pid.i = 0.0 self.pid.k_f=1.0 + self.v_pid = v_ego + self.pid.reset() if self.lastdecelForTurn and not decelForTurn: self.lastdecelForTurn = False self.pid._k_p = (CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV) self.pid._k_i = (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV) self.pid.k_f=1.0 + self.v_pid = v_ego + self.pid.reset() else: self.lastdecelForTurn = False self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV]) self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV]) self.pid.k_f=1.0 + self.v_pid = v_ego + self.pid.reset() output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) From 4f97fc59bd506b60d9e9a51a450b30116c0c1823 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 26 Mar 2020 10:56:13 +0100 Subject: [PATCH 08/12] reset only if kp and ki was 0 --- selfdrive/controls/lib/longcontrol.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 40a65d1a6c9f5f..cac5c2aacb0f77 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -135,12 +135,14 @@ def update(self, active, v_ego, gas_pressed, brake_pressed, standstill, cruise_s self.v_pid = v_ego self.pid.reset() else: + if self.lastdecelForTurn: + self.v_pid = v_ego + self.pid.reset() self.lastdecelForTurn = False self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV]) self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV]) self.pid.k_f=1.0 - self.v_pid = v_ego - self.pid.reset() + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) From 2f3807ef408f20fbacced0ddc1d7d739070a45f7 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 26 Mar 2020 11:26:27 +0100 Subject: [PATCH 09/12] Reduce harsh stopping rate at stop to standstill --- selfdrive/controls/lib/longcontrol.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index cac5c2aacb0f77..5201dedd2f4827 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -13,7 +13,7 @@ STOPPING_BRAKE_RATE = 0.2 # brake_travel/s while trying to stop STARTING_BRAKE_RATE = 0.8 # brake_travel/s while releasing on restart -BRAKE_STOPPING_TARGET = 0.75 # apply at least this amount of brake to maintain the vehicle stationary +BRAKE_STOPPING_TARGET = 0.7 # apply at least this amount of brake to maintain the vehicle stationary _MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints _MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp @@ -154,7 +154,7 @@ def update(self, active, v_ego, gas_pressed, brake_pressed, standstill, cruise_s # Keep applying brakes until the car is stopped factor = 1 if hasLead: - factor = interp(dRel,[2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0], [10.0,5.0,2.0,1.0,0.5,0.1,0.0,-0.1]) + factor = interp(dRel,[2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0], [5.0,2.5,1.0,0.5,0.25,0.05,0.0,-0.1]) if not standstill or output_gb > -BRAKE_STOPPING_TARGET: output_gb -= STOPPING_BRAKE_RATE / RATE * factor output_gb = clip(output_gb, -brake_max, gas_max) From a5a65c18f7009497cbe752d3825e5d2811df1f87 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 26 Mar 2020 21:10:08 +0100 Subject: [PATCH 10/12] New lane planner --- selfdrive/controls/lib/lane_planner.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index a659ec3b2063a1..a57f21ff335679 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -18,12 +18,22 @@ def compute_path_pinv(l=50): def model_polyfit(points, path_pinv): return np.dot(path_pinv, [float(x) for x in points]) +def eval_poly(poly, x): + return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 -def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): + +def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego): # This will improve behaviour when lanes suddenly widen + # these numbers were tested on 2000segments and found to work well lane_width = min(4.0, lane_width) - l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0]) - r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0]) + width_poly = l_poly - r_poly + prob_mods = [] + for t_check in [0.0, 1.5, 3.0]: + width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) + prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) + mod = min(prob_mods) + l_prob = mod * l_prob + r_prob = mod * r_prob path_from_left_lane = l_poly.copy() path_from_left_lane[3] -= lane_width / 2.0 @@ -86,7 +96,7 @@ def update_d_poly(self, v_ego): self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width - self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width) + self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego) def update(self, v_ego, md): self.parse_model(md) From c888636731c8273adc65631ab3dc068eb8c3b083 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 26 Mar 2020 21:11:09 +0100 Subject: [PATCH 11/12] New lane planner --- selfdrive/controls/tests/test_lateral_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 8dfff81ad40b59..cdfad05bc46b15 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -25,7 +25,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., p_p = poly_p.copy() p_p[3] += poly_shift - d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width) + d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width, v_ref) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) From 24a6d5f2e91471eec0512dd34a292811eff508d6 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 26 Mar 2020 21:12:37 +0100 Subject: [PATCH 12/12] New lane planner --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 150649e67152a3..993465c006d56c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b98f4b6858a21fd6602fb41c6a21f69b1ca81921 \ No newline at end of file +dfe0776d21e460a8f8e98321f56f421418132620