diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 292e67f64a..1264119a05 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -5,7 +5,7 @@ from openpilot.selfdrive.controls.lib.pid import PIDController from common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg, \ - create_gas_interceptor_command + create_gas_interceptor_command, rate_limit from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ @@ -48,7 +48,7 @@ class CarController(CarControllerBase): self.last_standstill = False self.standstill_req = False self.steer_rate_counter = 0 - self.pcm_accel_comp = 0 + self.pcm_accel_compensation = 0.0 self.distance_button = 0 self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0) @@ -151,31 +151,20 @@ class CarController(CarControllerBase): # When sp_tss2_long_tune is True and CC.longActive if sp_tss2_long_tune: - # we will throw out PCM's compensations, but that may be a good thing. for example: - # we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc. - # but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds, - # lag to start moving, lag to start braking, etc. - # PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them + # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot + # TODO: validate PCM_CRUISE->ACCEL_NET for braking requests and compensate for imprecise braking as well + if CC.longActive: + pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - actuators.accel) if actuators.accel > 0 else 0.0 - # FIXME? neutral force will only be positive under ~5 mph, which messes up stopping control considerably - # not sure why this isn't captured in the PCM accel net, maybe that just ignores creep force + high speed deceleration - # it also doesn't seem to capture slightly more braking on downhills (VSC1S07->ASLP (pitch, deg.) might have some clues) - offset = min(CS.pcm_neutral_force / self.CP.mass, 0.0) - pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81 # downhill is negative - # TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage? - # self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05) - pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_calc_accel_net) - self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005) - if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping: - self.pcm_accel_comp = 0.0 - self.pid.reset() - pcm_accel_cmd = actuators.accel + self.pcm_accel_comp # + offset - # pcm_accel_cmd = actuators.accel - pitch_offset + # prevent compensation windup + if actuators.accel - pcm_accel_compensation > self.params.ACCEL_MAX: + pcm_accel_compensation = actuators.accel - self.params.ACCEL_MAX - if not CC.longActive: - self.pid.reset() - self.pcm_accel_comp = 0.0 - pcm_accel_cmd = 0.0 + self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01) + pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation + else: + self.pcm_accel_compensation = 0.0 + pcm_accel_cmd = actuators.accel pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX) else: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 0795133213..7642c9092c 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -58,10 +58,10 @@ class CarState(CarStateBase): self.acc_type = 1 self.lkas_hud = {} self.pcm_accel_net = 0.0 - self.pcm_true_accel_net = 0.0 - self.pcm_calc_accel_net = 0.0 - self.pcm_neutral_force = 0.0 - self.vsc_slope_angle = 0.0 + #self.pcm_true_accel_net = 0.0 + #self.pcm_calc_accel_net = 0.0 + #self.pcm_neutral_force = 0.0 + #self.vsc_slope_angle = 0.0 self.lkas_enabled = None self.prev_lkas_enabled = None @@ -133,11 +133,11 @@ class CarState(CarStateBase): # thought to be the gas/brake as issued by the pcm (0=coasting) self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"] # this is only accurate for braking * 43 - self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"] # this is only accurate for acceleration * 78 - self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43 - self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"] - self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] - self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"] + #self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"] # this is only accurate for acceleration * 78 + #self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43 + #self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"] + #self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] + #self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"] ret.standstill = abs(ret.vEgoRaw) < 1e-3 @@ -457,14 +457,14 @@ class CarState(CarStateBase): ("BODY_CONTROL_STATE_2", 2), ("ESP_CONTROL", 3), ("EPS_STATUS", 25), - ("GEAR_PACKET_HYBRID", 60), - ("BRAKE", 80), + #("GEAR_PACKET_HYBRID", 60), + #("BRAKE", 80), ("BRAKE_MODULE", 40), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), ("PCM_CRUISE_SM", 1), - ("VSC1S07", 20), + #("VSC1S07", 20), ("STEER_TORQUE_SENSOR", 50), ("CLUTCH", 16), ]