From 4726c05dfa3cf44ec4dccd8fe4996c14511c4ae2 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Mon, 16 Sep 2024 07:36:22 -0700 Subject: [PATCH] shanes changes --- selfdrive/car/toyota/carcontroller.py | 7 +++---- selfdrive/car/toyota/carstate.py | 7 ++++++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f192970fab..292e67f64a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -51,7 +51,7 @@ class CarController(CarControllerBase): self.pcm_accel_comp = 0 self.distance_button = 0 - self.pid = PIDController(k_p=0.5, k_i=0.25, k_f=0) + self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0) self.packer = CANPacker(dbc_name) self.gas = 0 @@ -164,13 +164,12 @@ class CarController(CarControllerBase): 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_true_accel_net) + 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() - # TODO: just set kp to 1 and remove *2 here - pcm_accel_cmd = actuators.accel + self.pcm_accel_comp * 2 # + offset + pcm_accel_cmd = actuators.accel + self.pcm_accel_comp # + offset # pcm_accel_cmd = actuators.accel - pitch_offset if not CC.longActive: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 482c766c43..ec724ae176 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -59,6 +59,7 @@ class CarState(CarStateBase): 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 @@ -131,7 +132,9 @@ class CarState(CarStateBase): ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars # thought to be the gas/brake as issued by the pcm (0=coasting) - self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"] + 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"] @@ -452,6 +455,8 @@ class CarState(CarStateBase): ("BODY_CONTROL_STATE_2", 2), ("ESP_CONTROL", 3), ("EPS_STATUS", 25), + ("GEAR_PACKET_HYBRID", 60), + ("BRAKE", 80), ("BRAKE_MODULE", 40), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80),