diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2d17ee20f821df..a387a23c01386c 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -5,35 +5,18 @@ create_accel_command, create_acc_cancel_command, \ create_fcw_command, create_lta_steer_command from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ - MIN_ACC_SPEED, PEDAL_HYST_GAP, PEDAL_SCALE, CarControllerParams + MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert -def accel_hysteresis(accel, accel_steady, enabled): - - # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command - if not enabled: - # send 0 when disabled, otherwise acc faults - accel_steady = 0. - elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP: - accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP - elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP: - accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP - accel = accel_steady - - return accel, accel_steady - - class CarController(): def __init__(self, dbc_name, CP, VM): self.last_steer = 0 - self.accel_steady = 0. self.alert_active = False self.last_standstill = False self.standstill_req = False self.steer_rate_limited = False - self.use_interceptor = False self.packer = CANPacker(dbc_name) @@ -41,27 +24,17 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** - - # gas and brake - interceptor_gas_cmd = 0. - pcm_accel_cmd = actuators.accel - - if CS.CP.enableGasInterceptor: - # handle hysteresis when around the minimum acc speed - if CS.out.vEgo < MIN_ACC_SPEED: - self.use_interceptor = True - elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP: - self.use_interceptor = False - - if self.use_interceptor and enabled: - # only send negative accel when using interceptor. gas handles acceleration - # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged - MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5]) - interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS) - pcm_accel_cmd = 0.18 - max(0, -actuators.accel) - - pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled) - pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + if CS.CP.enableGasInterceptor and enabled: + MAX_INTERCEPTOR_GAS = 0.5 + # RAV4 has very sensitive has pedal + if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]: + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.1, 0.3, 0.0]) + else: + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) + interceptor_gas_cmd = clip(PEDAL_SCALE * actuators.accel, 0., MAX_INTERCEPTOR_GAS) + else: + interceptor_gas_cmd = 0. + pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) @@ -88,7 +61,6 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, self.standstill_req = False self.last_steer = apply_steer - self.last_accel = pcm_accel_cmd self.last_standstill = CS.out.standstill can_sends = [] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 643876af118a93..f9ab81d051cf31 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -315,30 +315,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu - if ret.enableGasInterceptor: - ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] - ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] - elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: - # Improved longitudinal tune - ret.longitudinalTuning.deadzoneBP = [0., 8.05] - ret.longitudinalTuning.deadzoneV = [.0, .14] - ret.longitudinalTuning.kpBP = [0., 5., 20.] - ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] - ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.] - ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1] - ret.stoppingDecelRate = 0.3 # reach stopping target smoothly - ret.startingAccelRate = 6.0 # release brakes fast - else: - # Default longitudinal tune - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiV = [0.54, 0.36] - + # Default longitudinal tune + ret.longitudinalTuning.kpBP = [0., 5., 10.] + ret.longitudinalTuning.kpV = [.5, .5, 0.0] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.] + ret.longitudinalActuatorDelayLowerBound = 0.7 + ret.longitudinalActuatorDelayUpperBound = 0.7 + ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + ret.startingAccelRate = 6.0 # release brakes fast return ret # returns a car.CarState diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 543f279fb655f4..d4d6ee46efb363 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -6,9 +6,7 @@ Ecu = car.CarParams.Ecu MIN_ACC_SPEED = 19. * CV.MPH_TO_MS - -PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS -PEDAL_SCALE = 3.0 +PEDAL_TRANSITION = 10. * CV.MPH_TO_MS class CarControllerParams: ACCEL_HYST_GAP = 0.06 # don't change accel command for small oscilalitons within this value diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index d44196e73f3267..5c02be371732ef 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -76,9 +76,9 @@ def update(self, active, CS, CP, long_plan, accel_limits): v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds) a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] - v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) + v_target = long_plan.speeds[0] v_target_future = long_plan.speeds[-1] else: v_target = 0.0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 5c8a98bda08065..f95917d7ff4836 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -19,15 +19,17 @@ X_DIM = 3 U_DIM = 1 -COST_E_DIM = 4 +PARAM_DIM= 4 +COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 X_EGO_OBSTACLE_COST = 3. -V_EGO_COST = 0. X_EGO_COST = 0. +V_EGO_COST = 0. A_EGO_COST = 0. -J_EGO_COST = 10. +J_EGO_COST = 5.0 +A_CHANGE_COST = .5 DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 @@ -80,7 +82,8 @@ def gen_long_model(): x_obstacle = SX.sym('x_obstacle') a_min = SX.sym('a_min') a_max = SX.sym('a_max') - model.p = vertcat(a_min, a_max, x_obstacle) + prev_a = SX.sym('prev_a') + model.p = vertcat(a_min, a_max, x_obstacle, prev_a) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -113,6 +116,7 @@ def gen_long_mpc_solver(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] + prev_a = ocp.model.p[3] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) @@ -127,6 +131,7 @@ def gen_long_mpc_solver(): x_ego, v_ego, a_ego, + 20*(a_ego - prev_a), j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) @@ -143,7 +148,7 @@ def gen_long_mpc_solver(): x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -192,13 +197,14 @@ def reset(self): self.solver = AcadosOcpSolver('long', N, EXPORT_DIR) self.v_solution = [0.0 for i in range(N+1)] self.a_solution = [0.0 for i in range(N+1)] + self.prev_a = self.a_solution self.j_solution = [0.0 for i in range(N)] self.yref = np.zeros((N+1, COST_DIM)) self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N,1)) - self.params = np.zeros((N+1,3)) + self.params = np.zeros((N+1, PARAM_DIM)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 @@ -215,9 +221,10 @@ def set_weights(self): self.set_weights_for_lead_policy() def set_weights_for_lead_policy(self): - W = np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST]) - Ws = np.tile(W[None], reps=(N,1,1)) - self.solver.cost_set_slice(0, N, 'W', Ws, api='old') + W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, A_CHANGE_COST, J_EGO_COST])) + for i in range(N): + W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) + self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) @@ -228,9 +235,9 @@ def set_weights_for_lead_policy(self): self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old') def set_weights_for_xva_policy(self): - W = np.diag([0., 10., 1., 10., 1.]) - Ws = np.tile(W[None], reps=(N,1,1)) - self.solver.cost_set_slice(0, N, 'W', Ws, api='old') + W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.])) + for i in range(N): + self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) @@ -313,6 +320,7 @@ def update(self, carstate, radarstate, v_cruise): x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) + self.params[:,3] = np.copy(self.prev_a) self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and @@ -331,7 +339,8 @@ def update_with_xva(self, x, v, a): self.accel_limit_arr[:,1] = 10. x_obstacle = 1e5*np.ones((N+1)) self.params = np.concatenate([self.accel_limit_arr, - x_obstacle[:,None]], axis=1) + x_obstacle[:,None], + self.prev_a], axis=1) self.run() @@ -348,6 +357,8 @@ def run(self): self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] + self.prev_a = interp(T_IDXS + 0.05, T_IDXS, self.a_solution) + t = sec_since_boot() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 4267f3c9e134d7..35b9f32e68e86a 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -62,7 +62,7 @@ speed_lead_values=[20., 20., 0.], prob_lead_values=[0., 1., 1.], cruise_values=[20., 20., 20.], - breakpoints=[2., 2.01, 8.51], + breakpoints=[2., 2.01, 8.8], ), Maneuver( "approach stopped car at 20m/s", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 69098de7cffa9e..3ccea748430f20 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f1f065451899f9b8e1cec379b32b970c1c16f849 \ No newline at end of file +6540e8c5a765975fd292b1efdef97b2d6391fa9c