diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 67872053431a13..ecace8f68d39fc 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -6,7 +6,7 @@ from selfdrive.car.tesla.values import CruiseState, CruiseButtons from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother -from selfdrive.controls.lib.planner import calc_cruise_accel_limits +from selfdrive.controls.lib.planner import calc_cruise_accel_limits, limit_accel_in_turns import cereal.messaging as messaging import time import math @@ -25,8 +25,8 @@ MAX_RADAR_DISTANCE = 120. #max distance to take in consideration radar reading MAX_PEDAL_VALUE = 112. PEDAL_HYST_GAP = 1.0 # don't change pedal command for small oscilalitons within this value -# Cap the pedal to go from 0 to max in 4 seconds -PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 4 +# Cap the pedal to go from 0 to max in 2 seconds +PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 2 # Cap the pedal to go from max to 0 in 0.4 seconds PEDAL_MAX_DOWN = MAX_PEDAL_VALUE * _DT / 0.4 @@ -173,26 +173,32 @@ def __init__(self,carcontroller): self.params = Params() average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second) self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions) - - def load_pid(self): - try: - v_pid_json = open(V_PID_FILE) - data = json.load(v_pid_json) - if (self.LoC): - if self.LoC.pid: - self.LoC.pid.p = data['p'] - self.LoC.pid.i = data['i'] - self.LoC.pid.f = data['f'] - else: - print("self.LoC not initialized!") - except : - print("file not present, creating at next reset") + + def load_pid(self): + try: + v_pid_json = open(V_PID_FILE) + data = json.load(v_pid_json) + if (self.LoC): + if self.LoC.pid: + self.LoC.pid.p = data['p'] + self.LoC.pid.i = data['i'] + if 'd' not in data: + self.Loc.pid.d = 0.01 + else: + self.LoC.pid.d = data['d'] + self.LoC.pid.f = data['f'] + else: + print("self.LoC not initialized!") + except : + print("file not present, creating at next reset") + #Helper function for saving the PCC pid constants across drives def save_pid(self, pid): data = {} data['p'] = pid.p data['i'] = pid.i + data['d'] = pid.d data['f'] = pid.f try: with open(V_PID_FILE , 'w') as outfile : @@ -359,14 +365,17 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s v_ego = CS.v_ego + following = False if self.lead_1: following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego,following)] + + accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1) accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph) jerk_limits = [min(-0.1, accel_limits[0]/2.), max(0.1, accel_limits[1]/2.)] # TODO: make a separate lookup for jerk tuning - #accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP) + accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP) output_gb = 0 #################################################################### @@ -379,7 +388,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping] # determine if pedal is pressed by human self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed - self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10 + self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 5 #reset PID if we just lifted foot of accelerator if (not self.accelerator_pedal_pressed) and self.prev_accelerator_pedal_pressed: self.reset(CS.v_ego) @@ -396,13 +405,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s # cruise speed can't be negative even if user is distracted self.v_pid = max(self.v_pid, 0.) - jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, self.v_pid, accel_limits[1], accel_limits[0], - jerk_limits[1], jerk_limits[0], #jerk_max, jerk_min, + jerk_limits[1], jerk_limits[0], _DT_MPC) - + # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) @@ -459,7 +467,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s self.last_output_gb = output_gb # accel and brake - apply_accel = clip(output_gb, 0., _accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS)) + apply_accel = clip(output_gb, 0., 1) #_accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS)) MPC_BRAKE_MULTIPLIER = 6. apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, _brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph), 0.) @@ -674,10 +682,10 @@ def _accel_limit_multiplier(CS, lead): if CS.teslaModel in ["SP","SPD"]: accel_by_speed = OrderedDict([ # (speed m/s, decel) - (0., 0.95), # 0 kmh - (10., 0.95), # 35 kmh - (20., 0.925), # 72 kmh - (30., 0.875)]) # 107 kmh + (0., 0.985), # 0 kmh + (10., 0.975), # 35 kmh + (20., 0.95), # 72 kmh + (30., 0.9)]) # 107 kmh accel_mult = _interp_map(CS.v_ego, accel_by_speed) if _is_present(lead): safe_dist_m = _safe_distance_m(CS.v_ego,CS) @@ -736,35 +744,6 @@ def _decel_limit(accel_min,v_ego, lead, CS, max_speed_kph): #BB: if we don't have a lead, don't do full regen to slow down smoother return accel_min * 0.5 * max_speed_mult -def _accel_pedal_max(v_ego, v_target, lead, prev_tesla_accel,CS): - pedal_max = prev_tesla_accel - if _is_present(lead): - #we have lead, base on speed and distance - safe_dist_m = _safe_distance_m(CS.v_ego,CS) - v_rel = lead.vLeadK - v_ego - accel_speed_map = OrderedDict([ - # (speed m/s, decel) change in accel (0..1) per second - (0., 0.01), # 0 MPH - (1., 0.1), # 4 MPH - (5., 0.15), # 11 MPH - (30., 0.20)]) # 67 MPH - accel_distance_map = OrderedDict([ - # (distance in m, acceleration fraction) - (0.6 * safe_dist_m, 0.3), - (1.0 * safe_dist_m, 1.0), - (3.0 * safe_dist_m, 2.0)]) - pedal_max = prev_tesla_accel + _interp_map(safe_dist_m, accel_distance_map) * _interp_map(v_rel, accel_speed_map) * _DT - else: - #no lead, do just based on speed - accel_speed_map = OrderedDict([ - # (speed m/s, decel) change in accel (0..1) per second - (0., 0.25), # 0 MPH - (10., 0.15), # 22 MPH - (20., 0.12), # 45 MPH - (30., 0.10)]) # 67 MPH - pedal_max = prev_tesla_accel + _interp_map(v_ego, accel_speed_map) * _DT - return 1. #pedal_max - def _brake_pedal_min(v_ego, v_target, lead, CS, max_speed_kph): #if less than 7 MPH we don't have much left till 5MPH to brake, so full regen if v_ego <= 7 * CV.MPH_TO_MS: @@ -792,51 +771,4 @@ def _brake_pedal_min(v_ego, v_target, lead, CS, max_speed_kph): brake_mult2 = _interp_map(lead.dRel, brake_distance_map) brake_mult = max(brake_mult1, brake_mult2) return -brake_mult - -def _jerk_limits(v_ego, lead, max_speed_kph, lead_last_seen_time_ms, CS): - # Allow higher accel jerk at low speed, to get started - accel_jerk_by_speed = OrderedDict([ - # (Speed in m/s, accel jerk) - (0, 0.18), - (9, 0.10)]) - accel_jerk = _interp_map(v_ego, accel_jerk_by_speed) - - # prevent high accel jerk near max speed - near_max_speed_multipliers = OrderedDict([ - # (kph under max speed, accel jerk multiplier) - (0, 0.01), - (4, 1.0)]) - near_max_speed_multiplier = _interp_map(max_speed_kph - v_ego * CV.MS_TO_KPH, near_max_speed_multipliers) - accel_jerk *= near_max_speed_multiplier - if _is_present(lead): - # pick decel jerk based on how much time we have til collision - decel_jerk_map = OrderedDict([ - # (sec to collision, decel jerk) - (0, -1.00), - (2, -0.25), - (4, -0.01), - (80, -0.001)]) - decel_jerk = _interp_map(_sec_til_collision(lead, CS), decel_jerk_map) - safe_dist_m = _safe_distance_m(v_ego,CS) - distance_multipliers = OrderedDict([ - # (distance in m, accel jerk) - (0.8 * safe_dist_m, 0.01), - (2.8 * safe_dist_m, 1.00)]) - distance_multiplier = _interp_map(lead.dRel, distance_multipliers) - accel_jerk *= distance_multiplier - return decel_jerk, accel_jerk - else: - # In the absence of a lead car - decel_jerk = -0.15 - # Limit accel jerk if the lead was only recently lost, to prevent - # bucking as a lead is intermittently detected. - time_since_lead_seen_ms = _current_time_millis() - lead_last_seen_time_ms - time_since_lead_seen_multipliers = OrderedDict([ - # (ms since last lead sighting, accel jerk multiplier) - (0, 0.1), - (2000, 1.0)]) - time_since_lead_seen_multiplier = _interp_map(time_since_lead_seen_ms, time_since_lead_seen_multipliers) - accel_jerk *= time_since_lead_seen_multiplier - - return decel_jerk, accel_jerk diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 3dbe733dffb6ed..ed610db9356a90 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -9,8 +9,6 @@ from common.params import read_db from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.tesla.readconfig import CarSettings -import cereal.messaging as messaging -from cereal.services import service_list from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V from selfdrive.car.interfaces import CarInterfaceBase @@ -149,31 +147,31 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # Kp and Ki for the longitudinal control if teslaModel == "S": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.01,0.01,0.01] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4, 0.4] + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01] elif teslaModel == "SP": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] # 0km/h, 18 km/h, 80, 128km/h + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.3, 0.3, 0.35, 0.37] + ret.longitudinalTuning.kiV = [0.07, 0.07, 0.093, 0.092] elif teslaModel == "SD": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.01,0.01,0.01] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4,0.4] + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01] elif teslaModel == "SPD": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325, 0.325] + ret.longitudinalTuning.kiBP = [0., 5., 22.,35.] + ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725, 0.00725] else: #use S numbers if we can't match anything - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.08,0.08,0.08] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3, 0.3] + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kiV = [0.08,0.08,0.08, 0.08] else: @@ -207,13 +205,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerMaxBP = [0.,15.] # m/s ret.steerMaxV = [420.,420.] # max steer allowed - ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed - ret.brakeMaxBP = [0., 20.] # m/s - ret.brakeMaxV = [1., 1.] # max brake allowed - BB: since we are using regen, make this even + ret.gasMaxBP = [0., 20.] # m/s + ret.gasMaxV = [0.225, 0.525] #if ret.enableGasInterceptor else [0.] # max gas allowed + ret.brakeMaxBP = [0.] # m/s + ret.brakeMaxV = [1.] # max brake allowed - BB: since we are using regen, make this even - ret.longitudinalTuning.deadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune - ret.longitudinalTuning.deadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now + ret.longitudinalTuning.deadzoneBP = [0.] #BB: added from Toyota to start pedal work; need to tune + ret.longitudinalTuning.deadzoneV = [0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now ret.stoppingControl = True ret.openpilotLongitudinalControl = True diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 3fc48aa264962d..721684efe822aa 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,6 +1,6 @@ from cereal import log from common.numpy_fast import clip, interp -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid_real import PIController LongCtrlState = log.ControlsState.LongControlState @@ -57,9 +57,14 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, class LongControl(): def __init__(self, CP, compute_gb): + + kdBp = [0, 5., 22.,35.] + kdV = [0.02, 0.02, 0.022, 0.025] + self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), + (kdBp,kdV), rate=RATE, sat_limit=0.8, convert=compute_gb) @@ -127,4 +132,4 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) - return final_gas, final_brake \ No newline at end of file + return final_gas, final_brake diff --git a/selfdrive/controls/lib/pid_real.py b/selfdrive/controls/lib/pid_real.py new file mode 100644 index 00000000000000..4cdcc733ee3701 --- /dev/null +++ b/selfdrive/controls/lib/pid_real.py @@ -0,0 +1,113 @@ +import numpy as np +from common.numpy_fast import clip, interp +from selfdrive.car.tesla.speed_utils.movingaverage import MovingAverage + + +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + +class PIController(): + def __init__(self, k_p, k_i, k_d, k_f=0.85, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p # proportional gain + self._k_i = k_i # integral gain + self._k_d = k_d # derivative gain + self.k_f = k_f # feedforward gain + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.rate = 1.0 / rate + self.d_rate = 7.0 / rate + self.sat_limit = sat_limit + self.convert = convert + self.past_errors_avg = 0 + self.past_errors = MovingAverage(3) + + self.reset() + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + @property + def k_i(self): + return interp(self.speed, self._k_i[0], self._k_i[1]) + + @property + def k_d(self): + return interp(self.speed, self._k_d[0], self._k_d[1]) + + + def _check_saturation(self, control, override, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and not override and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.f = 0.0 + self.d = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + self.past_errors_avg = 0 + self.past_errors.reset() + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + self.speed = speed + + error = float(apply_deadzone(setpoint - measurement, deadzone)) + self.p = error * self.k_p + + #clip the feedforward during the last 5 kmh for smooth transition + clipped_error = clip(error,0,5) + self.k_f = clipped_error * 0.2 + + self.f = feedforward * self.k_f + self.d = 0.0 + if self.past_errors.no_items == self.past_errors.length: + self.d = self.k_d * ((error - self.past_errors_avg) / self.d_rate) + self.past_errors_avg = self.past_errors.add(error) + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + self.f + i + self.d + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + else: + control = self.p + self.f + self.i + self.d + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + if check_saturation: + self.saturated = self._check_saturation(control, override, error) + else: + self.saturated = False + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control