From 46902cd7ec2a86b25fc13a18aa5dc9018db5412c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 20 Nov 2024 23:48:46 -0800 Subject: [PATCH 01/22] try derivative instead of complicated stuff --- opendbc/car/toyota/carcontroller.py | 49 +++++++++++++++++++++++++---- opendbc/car/toyota/interface.py | 2 +- 2 files changed, 44 insertions(+), 7 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index e37c93fbcc..275a4d2af5 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -2,6 +2,7 @@ from opendbc.car import Bus, carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL from opendbc.car.can_definitions import CanData +from opendbc.car.common.pid import PIDController from opendbc.car.common.filter_simple import FirstOrderFilter from opendbc.car.common.numpy_fast import clip from opendbc.car.secoc import add_mac, build_sync_mac @@ -19,7 +20,7 @@ # The up limit allows the brakes/gas to unwind quickly leaving a stop, # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup -ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame +ACCEL_WINDUP_LIMIT = 2.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits @@ -49,6 +50,10 @@ def __init__(self, dbc_names, CP): self.steer_rate_counter = 0 self.distance_button = 0 + self.pid = PIDController(0.0, 0.5, k_f=1.0, k_d=0.1, rate=1 / DT_CTRL / 3) + + self.error = FirstOrderFilter(0.0, 0.15, DT_CTRL * 3) + self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3) @@ -198,12 +203,39 @@ def update(self, CC, CS, now_nanos): self.prev_accel = pcm_accel_cmd # calculate amount of acceleration PCM should apply to reach target, given pitch - accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY - net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch - self.net_acceleration_request.update(net_acceleration_request) + # accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY + # net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch + # self.net_acceleration_request.update(net_acceleration_request) + + if CC.longActive and not CS.out.cruiseState.standstill: + # filter ACCEL_NET so it more closely matches aEgo delay for error correction + # self.pcm_accel_net.update(CS.pcm_accel_net) + + prev_error = self.error.x + self.error.update(pcm_accel_cmd - CS.out.aEgo) + error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) + + # let PCM handle stopping for now + pcm_accel_compensation = 0.0 + if not stopping and not CS.out.standstill: + self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX + self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN + + # TODO: freeze_integrator when stopping or at standstill? + pcm_accel_cmd = self.pid.update(pcm_accel_cmd - CS.out.aEgo, + error_rate=error_rate, feedforward=pcm_accel_cmd) + else: + self.pid.reset() + self.error.x = 0.0 + + # pcm_accel_cmd = pcm_accel_cmd + self.pcm_accel_compensation.update(pcm_accel_compensation) + + else: + self.pid.reset() + self.error.x = 0.0 # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking - if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill: + if False: # self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill: # filter ACCEL_NET so it more closely matches aEgo delay for error correction self.pcm_accel_net.update(CS.pcm_accel_net) @@ -218,7 +250,12 @@ def update(self, CC, CS, now_nanos): # let PCM handle stopping for now, error correct on a delayed acceleration request pcm_accel_compensation = 0.0 if not stopping: - pcm_accel_compensation = 2.0 * (new_pcm_accel_net - self.net_acceleration_request.x) + self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX + self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN + + pcm_accel_compensation = self.pid.update(new_pcm_accel_net - self.net_acceleration_request.x) + + # pcm_accel_compensation = 2.0 * (new_pcm_accel_net - self.net_acceleration_request.x) # prevent compensation windup pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX, diff --git a/opendbc/car/toyota/interface.py b/opendbc/car/toyota/interface.py index 66d250233c..1b29321d96 100644 --- a/opendbc/car/toyota/interface.py +++ b/opendbc/car/toyota/interface.py @@ -47,7 +47,7 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime found_ecus = [fw.ecu for fw in car_fw] ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) - if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: + if True: # candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value if candidate == CAR.TOYOTA_PRIUS: From 96330f3279d145182589dabe6c5ca56e1759d5f5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 20 Nov 2024 23:51:05 -0800 Subject: [PATCH 02/22] fix that --- opendbc/car/toyota/carcontroller.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 275a4d2af5..7e7a7ffc57 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -50,7 +50,9 @@ def __init__(self, dbc_names, CP): self.steer_rate_counter = 0 self.distance_button = 0 - self.pid = PIDController(0.0, 0.5, k_f=1.0, k_d=0.1, rate=1 / DT_CTRL / 3) + self.pid = PIDController(0.0, 0.5, k_f=1.0, k_d=0.1, + pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, + rate=1 / DT_CTRL / 3) self.error = FirstOrderFilter(0.0, 0.15, DT_CTRL * 3) @@ -218,8 +220,8 @@ def update(self, CC, CS, now_nanos): # let PCM handle stopping for now pcm_accel_compensation = 0.0 if not stopping and not CS.out.standstill: - self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX - self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN + # self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX + # self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN # TODO: freeze_integrator when stopping or at standstill? pcm_accel_cmd = self.pid.update(pcm_accel_cmd - CS.out.aEgo, From fd7c09d1fca5b43ac095e9af80dd5a98bbf264e9 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Thu, 21 Nov 2024 13:29:51 -0800 Subject: [PATCH 03/22] local changes --- opendbc/car/car.capnp | 4 +++ opendbc/car/toyota/carcontroller.py | 44 +++++++++++++++++++++-------- 2 files changed, 37 insertions(+), 11 deletions(-) diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index 42f193629d..9b1d18094b 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -367,6 +367,10 @@ struct CarControl { steerOutputCan @8: Float32; # value sent over can to the car speed @6: Float32; # m/s + debug @9: Float32; + debug2 @10: Float32; + debug3 @11: Float32; + enum LongControlState @0xe40f3a917d908282{ off @0; pid @1; diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 7e7a7ffc57..aad9ea846a 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -1,6 +1,7 @@ import math from opendbc.car import Bus, carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL +from collections import deque from opendbc.car.can_definitions import CanData from opendbc.car.common.pid import PIDController from opendbc.car.common.filter_simple import FirstOrderFilter @@ -50,14 +51,17 @@ def __init__(self, dbc_names, CP): self.steer_rate_counter = 0 self.distance_button = 0 - self.pid = PIDController(0.0, 0.5, k_f=1.0, k_d=0.1, + self.deque = deque([0] * 300, maxlen=300) + + self.pid = PIDController(0.0, 0.5, k_f=0.0, k_d=0.5, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) - self.error = FirstOrderFilter(0.0, 0.15, DT_CTRL * 3) + self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3) + self.pcm_accel_cmd = FirstOrderFilter(0, 0.15 + 0.2, DT_CTRL * 3) self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3) @@ -75,6 +79,10 @@ def __init__(self, dbc_names, CP): self.accel = 0 self.prev_accel = 0 + self.debug = 0 + self.debug2 = 0 + self.debug3 = 0 + self.secoc_lka_message_counter = 0 self.secoc_lta_message_counter = 0 self.secoc_prev_reset_counter = 0 @@ -203,10 +211,12 @@ def update(self, CC, CS, now_nanos): if CC.longActive: pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT) self.prev_accel = pcm_accel_cmd + self.deque.append(pcm_accel_cmd) + self.pcm_accel_cmd.update(pcm_accel_cmd) # calculate amount of acceleration PCM should apply to reach target, given pitch - # accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY - # net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch + accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY + net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch # self.net_acceleration_request.update(net_acceleration_request) if CC.longActive and not CS.out.cruiseState.standstill: @@ -214,27 +224,34 @@ def update(self, CC, CS, now_nanos): # self.pcm_accel_net.update(CS.pcm_accel_net) prev_error = self.error.x - self.error.update(pcm_accel_cmd - CS.out.aEgo) + self.error.update(self.pcm_accel_cmd.x - CS.out.aEgo) error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) + self.debug = error_rate # let PCM handle stopping for now pcm_accel_compensation = 0.0 if not stopping and not CS.out.standstill: - # self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX - # self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN + self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX + self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN # TODO: freeze_integrator when stopping or at standstill? - pcm_accel_cmd = self.pid.update(pcm_accel_cmd - CS.out.aEgo, - error_rate=error_rate, feedforward=pcm_accel_cmd) + #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, + pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - CS.out.aEgo, + error_rate=error_rate) #, feedforward=pcm_accel_cmd) + #pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) + pcm_accel_cmd += pcm_accel_compensation else: self.pid.reset() self.error.x = 0.0 + self.pcm_accel_compensation.x = 0.0 # pcm_accel_cmd = pcm_accel_cmd + self.pcm_accel_compensation.update(pcm_accel_compensation) else: self.pid.reset() self.error.x = 0.0 + self.pcm_accel_cmd.x = 0.0 + self.pcm_accel_compensation.x = 0.0 # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking if False: # self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill: @@ -266,7 +283,7 @@ def update(self, CC, CS, now_nanos): pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation) else: - self.pcm_accel_compensation.x = 0.0 + #self.pcm_accel_compensation.x = 0.0 self.pcm_accel_net_offset.x = 0.0 self.net_acceleration_request.x = 0.0 self.pcm_accel_net.x = CS.pcm_accel_net @@ -274,7 +291,8 @@ def update(self, CC, CS, now_nanos): # Along with rate limiting positive jerk above, this greatly improves gas response time # Consider the net acceleration request that the PCM should be applying (pitch included) - net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request) + #net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request) + net_acceleration_request_min = actuators.accel + accel_due_to_pitch if net_acceleration_request_min < 0.1 or stopping or not CC.longActive: self.permit_braking = True elif net_acceleration_request_min > 0.2: @@ -331,5 +349,9 @@ def update(self, CC, CS, now_nanos): new_actuators.steeringAngleDeg = self.last_angle new_actuators.accel = self.accel + new_actuators.debug = self.debug + new_actuators.debug2 = self.debug2 + new_actuators.debug3 = self.debug3 + self.frame += 1 return new_actuators, can_sends From e3c3a2752a4e3916164c6da3f457e506f3e0cfea Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 21 Nov 2024 14:35:36 -0800 Subject: [PATCH 04/22] fix the error rate calculation --- opendbc/car/toyota/carcontroller.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index aad9ea846a..facab80837 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,11 +53,13 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.0, 0.5, k_f=0.0, k_d=0.5, + self.pid = PIDController(0, 0.25, k_f=0.0, k_d=0.25, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) + self.error_rate = FirstOrderFilter(0.0, 0.15, DT_CTRL * 3) + self.prev_error = 0.0 self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3) @@ -224,10 +226,18 @@ def update(self, CC, CS, now_nanos): # self.pcm_accel_net.update(CS.pcm_accel_net) prev_error = self.error.x - self.error.update(self.pcm_accel_cmd.x - CS.out.aEgo) + error = pcm_accel_cmd - CS.out.aEgo + self.error.update(error) error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) self.debug = error_rate + self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) + + self.debug2 = self.error_rate.x + self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) + + self.prev_error = error + # let PCM handle stopping for now pcm_accel_compensation = 0.0 if not stopping and not CS.out.standstill: @@ -237,7 +247,7 @@ def update(self, CC, CS, now_nanos): # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - CS.out.aEgo, - error_rate=error_rate) #, feedforward=pcm_accel_cmd) + error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) #pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) pcm_accel_cmd += pcm_accel_compensation else: From cd1f89c5c981be021498768ba1fe8ea42ddbfc56 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 21 Nov 2024 14:41:09 -0800 Subject: [PATCH 05/22] tune --- opendbc/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index facab80837..7ab5bba432 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,12 +53,12 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0, 0.25, k_f=0.0, k_d=0.25, + self.pid = PIDController(0, 0.5, k_f=0.0, k_d=0.25, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) - self.error_rate = FirstOrderFilter(0.0, 0.15, DT_CTRL * 3) + self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) self.prev_error = 0.0 self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) From ea1ad0b322c2a39691eb1f884edd0522285605ec Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 15:31:05 -0800 Subject: [PATCH 06/22] fix that --- opendbc/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 7ab5bba432..d60c247b02 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -241,8 +241,8 @@ def update(self, CC, CS, now_nanos): # let PCM handle stopping for now pcm_accel_compensation = 0.0 if not stopping and not CS.out.standstill: - self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX - self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN + self.pid.neg_limit = self.params.ACCEL_MIN - pcm_accel_cmd + self.pid.pos_limit = self.params.ACCEL_MAX - pcm_accel_cmd # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, From 0546114fed5b77bec3fd56130f2c17bee8e42146 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 16:03:11 -0800 Subject: [PATCH 07/22] trash --- opendbc/car/car.capnp | 1 + opendbc/car/common/simple_kalman.py | 72 +++++++++++++++++------ opendbc/car/interfaces.py | 44 +++++++++++--- opendbc/car/toyota/carcontroller.py | 90 +++++++++++++++++++++++++++-- opendbc/car/toyota/carstate.py | 2 +- 5 files changed, 178 insertions(+), 31 deletions(-) diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index 9b1d18094b..ca53012a80 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -164,6 +164,7 @@ struct CarState { # car speed vEgo @1 :Float32; # best estimate of speed aEgo @16 :Float32; # best estimate of aCAN cceleration + jEgo @57 :Float32; # best estimate of aCAN cceleration vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI diff --git a/opendbc/car/common/simple_kalman.py b/opendbc/car/common/simple_kalman.py index 194b27204b..c2427bf058 100644 --- a/opendbc/car/common/simple_kalman.py +++ b/opendbc/car/common/simple_kalman.py @@ -12,43 +12,79 @@ def get_kalman_gain(dt, A, C, Q, R, iterations=100): class KF1D: - # this EKF assumes constant covariance matrix, so calculations are much simpler - # the Kalman gain also needs to be precomputed using the control module + # This EKF assumes a constant covariance matrix, so calculations are simpler. + # The Kalman gain also needs to be precomputed using the control module. def __init__(self, x0, A, C, K): - self.x0_0 = x0[0][0] - self.x1_0 = x0[1][0] + # State variables + self.x0_0 = x0[0][0] # vEgo (velocity) + self.x1_0 = x0[1][0] # aEgo (acceleration) + self.x2_0 = x0[2][0] # jEgo (jerk) - Added + + # State transition matrix elements self.A0_0 = A[0][0] self.A0_1 = A[0][1] + self.A0_2 = A[0][2] # Added self.A1_0 = A[1][0] self.A1_1 = A[1][1] - self.C0_0 = C[0] - self.C0_1 = C[1] + self.A1_2 = A[1][2] # Added + self.A2_0 = A[2][0] # Added + self.A2_1 = A[2][1] # Added + self.A2_2 = A[2][2] # Added + + # Observation matrix elements + self.C0 = C[0] + self.C1 = C[1] + self.C2 = C[2] # Added + + # Kalman gain elements self.K0_0 = K[0][0] self.K1_0 = K[1][0] + self.K2_0 = K[2][0] # Added - self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 - self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 - self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 - self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 + # Precompute (A - K * C) matrix elements + self.A_K_0 = self.A0_0 - self.K0_0 * self.C0 + self.A_K_1 = self.A0_1 - self.K0_0 * self.C1 + self.A_K_2 = self.A0_2 - self.K0_0 * self.C2 # Added + self.A_K_3 = self.A1_0 - self.K1_0 * self.C0 + self.A_K_4 = self.A1_1 - self.K1_0 * self.C1 + self.A_K_5 = self.A1_2 - self.K1_0 * self.C2 # Added + self.A_K_6 = self.A2_0 - self.K2_0 * self.C0 # Added + self.A_K_7 = self.A2_1 - self.K2_0 * self.C1 # Added + self.A_K_8 = self.A2_2 - self.K2_0 * self.C2 # Added - # K matrix needs to be pre-computed as follow: + # K matrix needs to be precomputed as follows: # import control - # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) - # self.K = np.transpose(K) + # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) + # K = np.transpose(K) def update(self, meas): - #self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) - x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas - x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas + # State update equations with jEgo included + x0_0 = (self.A_K_0 * self.x0_0 + + self.A_K_1 * self.x1_0 + + self.A_K_2 * self.x2_0 + # Added + self.K0_0 * meas) + x1_0 = (self.A_K_3 * self.x0_0 + + self.A_K_4 * self.x1_0 + + self.A_K_5 * self.x2_0 + # Added + self.K1_0 * meas) + x2_0 = (self.A_K_6 * self.x0_0 + + self.A_K_7 * self.x1_0 + + self.A_K_8 * self.x2_0 + # Added + self.K2_0 * meas) # Added + + # Update state variables self.x0_0 = x0_0 self.x1_0 = x1_0 - return [self.x0_0, self.x1_0] + self.x2_0 = x2_0 # Added + + return [self.x0_0, self.x1_0, self.x2_0] # Modified to include jEgo @property def x(self): - return [[self.x0_0], [self.x1_0]] + return [[self.x0_0], [self.x1_0], [self.x2_0]] # Modified to include jEgo def set_x(self, x): self.x0_0 = x[0][0] self.x1_0 = x[1][0] + self.x2_0 = x[2][0] # Added diff --git a/opendbc/car/interfaces.py b/opendbc/car/interfaces.py index 4b7a0956e8..10a5d17c9f 100644 --- a/opendbc/car/interfaces.py +++ b/opendbc/car/interfaces.py @@ -280,12 +280,42 @@ def __init__(self, CP: structs.CarParams): self.cluster_min_speed = 0.0 # min speed before dropping to 0 self.secoc_key: bytes = b"00" * 16 - Q = [[0.0, 0.0], [0.0, 100.0]] + Q = [ + [0.0, 0.0, 0.0], + [0.0, 100.0, 0.0], + [0.0, 0.0, 1000.0] # Adjust this value based on expected jerk variability + ] + + # Measurement noise covariance R (scalar) R = 0.3 - A = [[1.0, DT_CTRL], [0.0, 1.0]] - C = [[1.0, 0.0]] - x0=[[0.0], [0.0]] - K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) + + # State transition matrix A (3x3) + A = [ + [1.0, DT_CTRL, 0.5 * DT_CTRL ** 2], + [0.0, 1.0, DT_CTRL], + [0.0, 0.0, 1.0] + ] + + # Observation matrix C (1x3) + C = [[1.0, 0.0, 0.0]] # Assuming we only measure vEgoRaw + + # Initial state vector x0 (3x1) + x0 = [ + [0.0], # Initial vEgo + [0.0], # Initial aEgo + [0.0] # Initial jEgo + ] + + # Convert lists to NumPy arrays + A = np.array(A) + C = np.array(C) + Q = np.array(Q) + x0 = np.array(x0) + + # Compute Kalman gain K using the updated matrices + K = get_kalman_gain(DT_CTRL, A, C, Q, R) + + # Initialize the Kalman filter with the updated matrices self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) @abstractmethod @@ -294,10 +324,10 @@ def update(self, can_parsers) -> structs.CarState: def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) + self.v_ego_kf.set_x([[v_ego_raw], [0.0], [0.0]]) v_ego_x = self.v_ego_kf.update(v_ego_raw) - return float(v_ego_x[0]), float(v_ego_x[1]) + return float(v_ego_x[0]), float(v_ego_x[1]), float(v_ego_x[2]) def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS): factor = unit * self.CP.wheelSpeedFactor diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index d60c247b02..747e1fe89f 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -38,6 +38,69 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes +class SimpleKalmanFilter: + def __init__(self, initial_estimate, initial_error_covariance, process_noise_covariance, measurement_noise_covariance): + # Initial estimates + self.aEgo_est = initial_estimate # Estimated acceleration + self.P = initial_error_covariance # Estimated error covariance + + # Kalman filter parameters + self.Q = process_noise_covariance # Process noise covariance (Q) + self.R = measurement_noise_covariance # Measurement noise covariance (R) + + # For extrapolation + self.aEgo_prev_est = initial_estimate # Previous estimated acceleration + self.dt = None # Time step (to be set later) + self.delay = 0.0 # Delay to predict ahead (seconds) + + # For filtering jerk estimate + self.jerk_est_filtered = 0.0 + self.alpha_jerk = None # Filter coefficient, to be set later + + def set_time_step_and_delay(self, dt, delay, jerk_filter_rc=None): + self.dt = dt + self.delay = delay + + if jerk_filter_rc is not None: + self.alpha_jerk = dt / (jerk_filter_rc + dt) + else: + self.alpha_jerk = 1.0 # No filtering + + def update(self, measurement): + # Predict step (since we have no control input, prediction is the same) + self.P = self.P + self.Q + + # Store previous estimate before updating + aEgo_est_prev = self.aEgo_est + + # Update step + K = self.P / (self.P + self.R) + self.aEgo_est = self.aEgo_est + K * (measurement - self.aEgo_est) + self.P = (1 - K) * self.P + + # Update previous estimate after the estimate is used in jerk calculation + self.aEgo_prev_est = aEgo_est_prev # Now aEgo_prev_est holds the previous estimate + + # Return the updated estimate + return self.aEgo_est + + def predict_future_aEgo(self): + # Check that time step and delay have been set + if self.dt is None: + raise ValueError("Time step 'dt' and delay must be set using 'set_time_step_and_delay' method.") + + # Estimate jerk (rate of change of acceleration) + jerk_est = (self.aEgo_est - self.aEgo_prev_est) / self.dt + + # Apply low-pass filter to jerk_est + self.jerk_est_filtered = self.alpha_jerk * jerk_est + (1 - self.alpha_jerk) * self.jerk_est_filtered + + # Predict future acceleration + aEgo_future_est = self.aEgo_est + self.jerk_est_filtered * self.delay + + return aEgo_future_est + + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -53,10 +116,19 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0, 0.5, k_f=0.0, k_d=0.25, + self.pid = PIDController(1, 0.0, k_f=0.0, k_d=0.0, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) + self.aego_kf = SimpleKalmanFilter( + initial_estimate=0.0, + initial_error_covariance=1.0, + process_noise_covariance=0.1, + measurement_noise_covariance=0.2 + ) + + self.aego_kf.set_time_step_and_delay(dt=DT_CTRL, delay=0.15) + self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) self.prev_error = 0.0 @@ -184,6 +256,10 @@ def update(self, CC, CS, now_nanos): # *** gas and brake *** + aEgo_est = self.aego_kf.update(CS.out.aEgo) + self.debug = aEgo_est + self.debug2 = self.aego_kf.predict_future_aEgo() + # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR): self.standstill_req = True @@ -229,15 +305,19 @@ def update(self, CC, CS, now_nanos): error = pcm_accel_cmd - CS.out.aEgo self.error.update(error) error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) - self.debug = error_rate + # self.debug = error_rate self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) - self.debug2 = self.error_rate.x - self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) + # self.debug2 = self.error_rate.x + # self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) self.prev_error = error + # aEgo_est = self.aego_kf.update(CS.out.aEgo) + # self.debug = aEgo_est + # self.debug2 = self.aego_kf.predict_future_aEgo() + # let PCM handle stopping for now pcm_accel_compensation = 0.0 if not stopping and not CS.out.standstill: @@ -246,7 +326,7 @@ def update(self, CC, CS, now_nanos): # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, - pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - CS.out.aEgo, + pcm_accel_compensation = self.pid.update(pcm_accel_cmd - CS.out.aEgo, error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) #pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) pcm_accel_cmd += pcm_accel_compensation diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 960e7c3b21..418d771625 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -102,7 +102,7 @@ def update(self, can_parsers) -> structs.CarState: cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"], ) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.vEgo, ret.aEgo, ret.jEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars ret.standstill = abs(ret.vEgoRaw) < 1e-3 From 49e584c927d9e88808e560d1ac951a0e07e1b32e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 16:03:15 -0800 Subject: [PATCH 08/22] Revert "trash" This reverts commit 0546114fed5b77bec3fd56130f2c17bee8e42146. --- opendbc/car/car.capnp | 1 - opendbc/car/common/simple_kalman.py | 72 ++++++----------------- opendbc/car/interfaces.py | 44 +++----------- opendbc/car/toyota/carcontroller.py | 90 ++--------------------------- opendbc/car/toyota/carstate.py | 2 +- 5 files changed, 31 insertions(+), 178 deletions(-) diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index ca53012a80..9b1d18094b 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -164,7 +164,6 @@ struct CarState { # car speed vEgo @1 :Float32; # best estimate of speed aEgo @16 :Float32; # best estimate of aCAN cceleration - jEgo @57 :Float32; # best estimate of aCAN cceleration vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI diff --git a/opendbc/car/common/simple_kalman.py b/opendbc/car/common/simple_kalman.py index c2427bf058..194b27204b 100644 --- a/opendbc/car/common/simple_kalman.py +++ b/opendbc/car/common/simple_kalman.py @@ -12,79 +12,43 @@ def get_kalman_gain(dt, A, C, Q, R, iterations=100): class KF1D: - # This EKF assumes a constant covariance matrix, so calculations are simpler. - # The Kalman gain also needs to be precomputed using the control module. + # this EKF assumes constant covariance matrix, so calculations are much simpler + # the Kalman gain also needs to be precomputed using the control module def __init__(self, x0, A, C, K): - # State variables - self.x0_0 = x0[0][0] # vEgo (velocity) - self.x1_0 = x0[1][0] # aEgo (acceleration) - self.x2_0 = x0[2][0] # jEgo (jerk) - Added - - # State transition matrix elements + self.x0_0 = x0[0][0] + self.x1_0 = x0[1][0] self.A0_0 = A[0][0] self.A0_1 = A[0][1] - self.A0_2 = A[0][2] # Added self.A1_0 = A[1][0] self.A1_1 = A[1][1] - self.A1_2 = A[1][2] # Added - self.A2_0 = A[2][0] # Added - self.A2_1 = A[2][1] # Added - self.A2_2 = A[2][2] # Added - - # Observation matrix elements - self.C0 = C[0] - self.C1 = C[1] - self.C2 = C[2] # Added - - # Kalman gain elements + self.C0_0 = C[0] + self.C0_1 = C[1] self.K0_0 = K[0][0] self.K1_0 = K[1][0] - self.K2_0 = K[2][0] # Added - # Precompute (A - K * C) matrix elements - self.A_K_0 = self.A0_0 - self.K0_0 * self.C0 - self.A_K_1 = self.A0_1 - self.K0_0 * self.C1 - self.A_K_2 = self.A0_2 - self.K0_0 * self.C2 # Added - self.A_K_3 = self.A1_0 - self.K1_0 * self.C0 - self.A_K_4 = self.A1_1 - self.K1_0 * self.C1 - self.A_K_5 = self.A1_2 - self.K1_0 * self.C2 # Added - self.A_K_6 = self.A2_0 - self.K2_0 * self.C0 # Added - self.A_K_7 = self.A2_1 - self.K2_0 * self.C1 # Added - self.A_K_8 = self.A2_2 - self.K2_0 * self.C2 # Added + self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 + self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 + self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 + self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 - # K matrix needs to be precomputed as follows: + # K matrix needs to be pre-computed as follow: # import control - # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) - # K = np.transpose(K) + # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) + # self.K = np.transpose(K) def update(self, meas): - # State update equations with jEgo included - x0_0 = (self.A_K_0 * self.x0_0 + - self.A_K_1 * self.x1_0 + - self.A_K_2 * self.x2_0 + # Added - self.K0_0 * meas) - x1_0 = (self.A_K_3 * self.x0_0 + - self.A_K_4 * self.x1_0 + - self.A_K_5 * self.x2_0 + # Added - self.K1_0 * meas) - x2_0 = (self.A_K_6 * self.x0_0 + - self.A_K_7 * self.x1_0 + - self.A_K_8 * self.x2_0 + # Added - self.K2_0 * meas) # Added - - # Update state variables + #self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) + x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas + x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas self.x0_0 = x0_0 self.x1_0 = x1_0 - self.x2_0 = x2_0 # Added - - return [self.x0_0, self.x1_0, self.x2_0] # Modified to include jEgo + return [self.x0_0, self.x1_0] @property def x(self): - return [[self.x0_0], [self.x1_0], [self.x2_0]] # Modified to include jEgo + return [[self.x0_0], [self.x1_0]] def set_x(self, x): self.x0_0 = x[0][0] self.x1_0 = x[1][0] - self.x2_0 = x[2][0] # Added diff --git a/opendbc/car/interfaces.py b/opendbc/car/interfaces.py index 10a5d17c9f..4b7a0956e8 100644 --- a/opendbc/car/interfaces.py +++ b/opendbc/car/interfaces.py @@ -280,42 +280,12 @@ def __init__(self, CP: structs.CarParams): self.cluster_min_speed = 0.0 # min speed before dropping to 0 self.secoc_key: bytes = b"00" * 16 - Q = [ - [0.0, 0.0, 0.0], - [0.0, 100.0, 0.0], - [0.0, 0.0, 1000.0] # Adjust this value based on expected jerk variability - ] - - # Measurement noise covariance R (scalar) + Q = [[0.0, 0.0], [0.0, 100.0]] R = 0.3 - - # State transition matrix A (3x3) - A = [ - [1.0, DT_CTRL, 0.5 * DT_CTRL ** 2], - [0.0, 1.0, DT_CTRL], - [0.0, 0.0, 1.0] - ] - - # Observation matrix C (1x3) - C = [[1.0, 0.0, 0.0]] # Assuming we only measure vEgoRaw - - # Initial state vector x0 (3x1) - x0 = [ - [0.0], # Initial vEgo - [0.0], # Initial aEgo - [0.0] # Initial jEgo - ] - - # Convert lists to NumPy arrays - A = np.array(A) - C = np.array(C) - Q = np.array(Q) - x0 = np.array(x0) - - # Compute Kalman gain K using the updated matrices - K = get_kalman_gain(DT_CTRL, A, C, Q, R) - - # Initialize the Kalman filter with the updated matrices + A = [[1.0, DT_CTRL], [0.0, 1.0]] + C = [[1.0, 0.0]] + x0=[[0.0], [0.0]] + K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) @abstractmethod @@ -324,10 +294,10 @@ def update(self, can_parsers) -> structs.CarState: def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_kf.set_x([[v_ego_raw], [0.0], [0.0]]) + self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) v_ego_x = self.v_ego_kf.update(v_ego_raw) - return float(v_ego_x[0]), float(v_ego_x[1]), float(v_ego_x[2]) + return float(v_ego_x[0]), float(v_ego_x[1]) def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS): factor = unit * self.CP.wheelSpeedFactor diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 747e1fe89f..d60c247b02 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -38,69 +38,6 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes -class SimpleKalmanFilter: - def __init__(self, initial_estimate, initial_error_covariance, process_noise_covariance, measurement_noise_covariance): - # Initial estimates - self.aEgo_est = initial_estimate # Estimated acceleration - self.P = initial_error_covariance # Estimated error covariance - - # Kalman filter parameters - self.Q = process_noise_covariance # Process noise covariance (Q) - self.R = measurement_noise_covariance # Measurement noise covariance (R) - - # For extrapolation - self.aEgo_prev_est = initial_estimate # Previous estimated acceleration - self.dt = None # Time step (to be set later) - self.delay = 0.0 # Delay to predict ahead (seconds) - - # For filtering jerk estimate - self.jerk_est_filtered = 0.0 - self.alpha_jerk = None # Filter coefficient, to be set later - - def set_time_step_and_delay(self, dt, delay, jerk_filter_rc=None): - self.dt = dt - self.delay = delay - - if jerk_filter_rc is not None: - self.alpha_jerk = dt / (jerk_filter_rc + dt) - else: - self.alpha_jerk = 1.0 # No filtering - - def update(self, measurement): - # Predict step (since we have no control input, prediction is the same) - self.P = self.P + self.Q - - # Store previous estimate before updating - aEgo_est_prev = self.aEgo_est - - # Update step - K = self.P / (self.P + self.R) - self.aEgo_est = self.aEgo_est + K * (measurement - self.aEgo_est) - self.P = (1 - K) * self.P - - # Update previous estimate after the estimate is used in jerk calculation - self.aEgo_prev_est = aEgo_est_prev # Now aEgo_prev_est holds the previous estimate - - # Return the updated estimate - return self.aEgo_est - - def predict_future_aEgo(self): - # Check that time step and delay have been set - if self.dt is None: - raise ValueError("Time step 'dt' and delay must be set using 'set_time_step_and_delay' method.") - - # Estimate jerk (rate of change of acceleration) - jerk_est = (self.aEgo_est - self.aEgo_prev_est) / self.dt - - # Apply low-pass filter to jerk_est - self.jerk_est_filtered = self.alpha_jerk * jerk_est + (1 - self.alpha_jerk) * self.jerk_est_filtered - - # Predict future acceleration - aEgo_future_est = self.aEgo_est + self.jerk_est_filtered * self.delay - - return aEgo_future_est - - class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -116,19 +53,10 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(1, 0.0, k_f=0.0, k_d=0.0, + self.pid = PIDController(0, 0.5, k_f=0.0, k_d=0.25, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) - self.aego_kf = SimpleKalmanFilter( - initial_estimate=0.0, - initial_error_covariance=1.0, - process_noise_covariance=0.1, - measurement_noise_covariance=0.2 - ) - - self.aego_kf.set_time_step_and_delay(dt=DT_CTRL, delay=0.15) - self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) self.prev_error = 0.0 @@ -256,10 +184,6 @@ def update(self, CC, CS, now_nanos): # *** gas and brake *** - aEgo_est = self.aego_kf.update(CS.out.aEgo) - self.debug = aEgo_est - self.debug2 = self.aego_kf.predict_future_aEgo() - # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR): self.standstill_req = True @@ -305,19 +229,15 @@ def update(self, CC, CS, now_nanos): error = pcm_accel_cmd - CS.out.aEgo self.error.update(error) error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) - # self.debug = error_rate + self.debug = error_rate self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) - # self.debug2 = self.error_rate.x - # self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) + self.debug2 = self.error_rate.x + self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) self.prev_error = error - # aEgo_est = self.aego_kf.update(CS.out.aEgo) - # self.debug = aEgo_est - # self.debug2 = self.aego_kf.predict_future_aEgo() - # let PCM handle stopping for now pcm_accel_compensation = 0.0 if not stopping and not CS.out.standstill: @@ -326,7 +246,7 @@ def update(self, CC, CS, now_nanos): # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, - pcm_accel_compensation = self.pid.update(pcm_accel_cmd - CS.out.aEgo, + pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - CS.out.aEgo, error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) #pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) pcm_accel_cmd += pcm_accel_compensation diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 418d771625..960e7c3b21 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -102,7 +102,7 @@ def update(self, can_parsers) -> structs.CarState: cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"], ) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) - ret.vEgo, ret.aEgo, ret.jEgo = self.update_speed_kf(ret.vEgoRaw) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars ret.standstill = abs(ret.vEgoRaw) < 1e-3 From 7abc6b888cb604f4583c600de50521aa8c01e7d1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 16:19:50 -0800 Subject: [PATCH 09/22] future --- opendbc/car/toyota/carcontroller.py | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index d60c247b02..25e889e26e 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -57,6 +57,8 @@ def __init__(self, dbc_names, CP): pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) + self.aego = FirstOrderFilter(0.0, 0.5, DT_CTRL) + self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) self.prev_error = 0.0 @@ -184,6 +186,14 @@ def update(self, CC, CS, now_nanos): # *** gas and brake *** + prev_aego = self.aego.x + self.aego.update(CS.out.aEgo) + jEgo = (self.aego.x - prev_aego) / DT_CTRL + future_aego = CS.out.aEgo + jEgo * 0.5 + + self.debug = jEgo + self.debug2 = future_aego + # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR): self.standstill_req = True @@ -226,15 +236,15 @@ def update(self, CC, CS, now_nanos): # self.pcm_accel_net.update(CS.pcm_accel_net) prev_error = self.error.x - error = pcm_accel_cmd - CS.out.aEgo + error = pcm_accel_cmd - future_aego # CS.out.aEgo self.error.update(error) error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) - self.debug = error_rate + # self.debug = error_rate self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) - self.debug2 = self.error_rate.x - self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) + # self.debug2 = self.error_rate.x + # self.debug3 = (error - self.prev_error) / (DT_CTRL * 3) self.prev_error = error @@ -246,7 +256,7 @@ def update(self, CC, CS, now_nanos): # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, - pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - CS.out.aEgo, + pcm_accel_compensation = self.pid.update(pcm_accel_cmd - future_aego, error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) #pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) pcm_accel_cmd += pcm_accel_compensation From e5fd0c118d0a931ce037ce046a2ea34af7180320 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Fri, 22 Nov 2024 17:25:14 -0800 Subject: [PATCH 10/22] local changes --- opendbc/car/toyota/carcontroller.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 25e889e26e..492fabda28 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0, 0.5, k_f=0.0, k_d=0.25, + self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0., pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) @@ -189,7 +189,7 @@ def update(self, CC, CS, now_nanos): prev_aego = self.aego.x self.aego.update(CS.out.aEgo) jEgo = (self.aego.x - prev_aego) / DT_CTRL - future_aego = CS.out.aEgo + jEgo * 0.5 + future_aego = CS.out.aEgo + jEgo * 0.35 self.debug = jEgo self.debug2 = future_aego @@ -256,10 +256,10 @@ def update(self, CC, CS, now_nanos): # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, - pcm_accel_compensation = self.pid.update(pcm_accel_cmd - future_aego, + pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - future_aego, error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) - #pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) - pcm_accel_cmd += pcm_accel_compensation + pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) + #pcm_accel_cmd += pcm_accel_compensation else: self.pid.reset() self.error.x = 0.0 From e39f4983d16b903e605fd7dfbe10977c9ee24136 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Sat, 23 Nov 2024 05:48:28 +0000 Subject: [PATCH 11/22] local hybrid changes --- opendbc/car/toyota/carcontroller.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 492fabda28..1e6d07c860 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -22,7 +22,7 @@ # The up limit allows the brakes/gas to unwind quickly leaving a stop, # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup ACCEL_WINDUP_LIMIT = 2.0 * DT_CTRL * 3 # m/s^2 / frame -ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame +ACCEL_WINDDOWN_LIMIT = -8.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0., + self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.1, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) @@ -189,7 +189,7 @@ def update(self, CC, CS, now_nanos): prev_aego = self.aego.x self.aego.update(CS.out.aEgo) jEgo = (self.aego.x - prev_aego) / DT_CTRL - future_aego = CS.out.aEgo + jEgo * 0.35 + future_aego = CS.out.aEgo + jEgo * 0.15 self.debug = jEgo self.debug2 = future_aego @@ -236,7 +236,8 @@ def update(self, CC, CS, now_nanos): # self.pcm_accel_net.update(CS.pcm_accel_net) prev_error = self.error.x - error = pcm_accel_cmd - future_aego # CS.out.aEgo + #error = pcm_accel_cmd - future_aego # CS.out.aEgo + error = pcm_accel_cmd - CS.out.aEgo self.error.update(error) error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) # self.debug = error_rate From 88d486040abd302c9f14372f96a5a14b3b075f69 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 22:31:24 -0800 Subject: [PATCH 12/22] 5 --- opendbc/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 1e6d07c860..f4e45f07df 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -21,8 +21,8 @@ # The up limit allows the brakes/gas to unwind quickly leaving a stop, # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup -ACCEL_WINDUP_LIMIT = 2.0 * DT_CTRL * 3 # m/s^2 / frame -ACCEL_WINDDOWN_LIMIT = -8.0 * DT_CTRL * 3 # m/s^2 / frame +ACCEL_WINDUP_LIMIT = 5.0 * DT_CTRL * 3 # m/s^2 / frame +ACCEL_WINDDOWN_LIMIT = -5.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long From 60b0f9f2b13bc3bed172a5cf3bcfeaf2de6086b1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 22:35:35 -0800 Subject: [PATCH 13/22] try --- opendbc/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index f4e45f07df..6265c7f868 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -21,7 +21,7 @@ # The up limit allows the brakes/gas to unwind quickly leaving a stop, # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup -ACCEL_WINDUP_LIMIT = 5.0 * DT_CTRL * 3 # m/s^2 / frame +ACCEL_WINDUP_LIMIT = 3.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_WINDDOWN_LIMIT = -5.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits @@ -189,7 +189,7 @@ def update(self, CC, CS, now_nanos): prev_aego = self.aego.x self.aego.update(CS.out.aEgo) jEgo = (self.aego.x - prev_aego) / DT_CTRL - future_aego = CS.out.aEgo + jEgo * 0.15 + future_aego = CS.out.aEgo + jEgo * 0.15 # TODO: only for hybrid self.debug = jEgo self.debug2 = future_aego From a0decd4b2faa92d542fc85ebc58c8e2a87d84aec Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 22 Nov 2024 22:39:39 -0800 Subject: [PATCH 14/22] remove old code --- opendbc/car/toyota/carcontroller.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 6265c7f868..9dca65d539 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,13 +53,12 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.1, + self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.125, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) self.aego = FirstOrderFilter(0.0, 0.5, DT_CTRL) - self.error = FirstOrderFilter(0.0, 2.0, DT_CTRL * 3) self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) self.prev_error = 0.0 @@ -235,13 +234,7 @@ def update(self, CC, CS, now_nanos): # filter ACCEL_NET so it more closely matches aEgo delay for error correction # self.pcm_accel_net.update(CS.pcm_accel_net) - prev_error = self.error.x - #error = pcm_accel_cmd - future_aego # CS.out.aEgo error = pcm_accel_cmd - CS.out.aEgo - self.error.update(error) - error_rate = (self.error.x - prev_error) / (DT_CTRL * 3) - # self.debug = error_rate - self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) # self.debug2 = self.error_rate.x From 77a0f511df7759c60de6b3b278624754f05c9a33 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 23 Nov 2024 02:38:32 -0800 Subject: [PATCH 15/22] Update carcontroller.py --- opendbc/car/toyota/carcontroller.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 9dca65d539..40abe3fcfa 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -256,14 +256,12 @@ def update(self, CC, CS, now_nanos): #pcm_accel_cmd += pcm_accel_compensation else: self.pid.reset() - self.error.x = 0.0 self.pcm_accel_compensation.x = 0.0 # pcm_accel_cmd = pcm_accel_cmd + self.pcm_accel_compensation.update(pcm_accel_compensation) else: self.pid.reset() - self.error.x = 0.0 self.pcm_accel_cmd.x = 0.0 self.pcm_accel_compensation.x = 0.0 From 9c963f617233f5221d3e8a7c39da838fd1fc82eb Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 23 Nov 2024 20:58:14 -0800 Subject: [PATCH 16/22] use aEgoBlended fix --- opendbc/car/car.capnp | 1 + opendbc/car/toyota/carcontroller.py | 10 +++++----- opendbc/car/toyota/carstate.py | 7 ++++++- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index 9b1d18094b..5a35981728 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -164,6 +164,7 @@ struct CarState { # car speed vEgo @1 :Float32; # best estimate of speed aEgo @16 :Float32; # best estimate of aCAN cceleration + aEgoBlended @57 :Float32; # best estimate of aCAN cceleration vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 40abe3fcfa..404e2a5bfe 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -186,9 +186,9 @@ def update(self, CC, CS, now_nanos): # *** gas and brake *** prev_aego = self.aego.x - self.aego.update(CS.out.aEgo) + self.aego.update(CS.out.aEgoBlended) jEgo = (self.aego.x - prev_aego) / DT_CTRL - future_aego = CS.out.aEgo + jEgo * 0.15 # TODO: only for hybrid + future_aego = CS.out.aEgoBlended + jEgo * 0.15 # TODO: only for hybrid self.debug = jEgo self.debug2 = future_aego @@ -234,7 +234,7 @@ def update(self, CC, CS, now_nanos): # filter ACCEL_NET so it more closely matches aEgo delay for error correction # self.pcm_accel_net.update(CS.pcm_accel_net) - error = pcm_accel_cmd - CS.out.aEgo + error = pcm_accel_cmd - CS.out.aEgoBlended self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) # self.debug2 = self.error_rate.x @@ -249,7 +249,7 @@ def update(self, CC, CS, now_nanos): self.pid.pos_limit = self.params.ACCEL_MAX - pcm_accel_cmd # TODO: freeze_integrator when stopping or at standstill? - #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgo, + #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgoBlended, pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - future_aego, error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) @@ -276,7 +276,7 @@ def update(self, CC, CS, now_nanos): # TODO: check if maintaining the offset from before stopping is beneficial self.pcm_accel_net_offset.x = 0.0 else: - new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgo) + new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgoBlended) # let PCM handle stopping for now, error correct on a delayed acceleration request pcm_accel_compensation = 0.0 diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 960e7c3b21..685eecc967 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -5,7 +5,7 @@ from opendbc.car import Bus, DT_CTRL, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.common.filter_simple import FirstOrderFilter -from opendbc.car.common.numpy_fast import mean +from opendbc.car.common.numpy_fast import interp, mean from opendbc.car.interfaces import CarStateBase from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, SECOC_CAR @@ -105,6 +105,10 @@ def update(self, can_parsers) -> structs.CarState: ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars + # TODO: check that this isn't crazy on inclines, one ego acceleration signal was + # GVC can capture ego acceleration 0.5s earlier than aEgo, this is useful for start from stop control + ret.aEgoBlended = interp(ret.vEgo, [1, 2], [cp.vl["VSC1S07"]["GVC"], ret.aEgo]) + ret.standstill = abs(ret.vEgoRaw) < 1e-3 ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] @@ -218,6 +222,7 @@ def get_can_parsers(CP): ("PCM_CRUISE", 33), ("PCM_CRUISE_SM", 1), ("STEER_TORQUE_SENSOR", 50), + ("VSC1S07", 20), ] if CP.flags & ToyotaFlags.SECOC.value: From 4fa1857a761421f3d6495f7fdd9f099f8f9bdac3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 23 Nov 2024 21:01:49 -0800 Subject: [PATCH 17/22] try that --- opendbc/car/toyota/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 404e2a5bfe..1821717e70 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.125, + self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.0, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) From 924245573d806317cb0fe25be89637fa1583b2da Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 25 Nov 2024 16:09:38 -0800 Subject: [PATCH 18/22] more immediate jerk estimate --- opendbc/car/toyota/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 1821717e70..ad7a231395 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -57,7 +57,7 @@ def __init__(self, dbc_names, CP): pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) - self.aego = FirstOrderFilter(0.0, 0.5, DT_CTRL) + self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL) self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) self.prev_error = 0.0 From f5bae440f7b6cb2d20540c850b121af7d9995bfa Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 25 Nov 2024 16:35:34 -0800 Subject: [PATCH 19/22] try this too --- opendbc/car/toyota/carcontroller.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index ad7a231395..0d16c1e13a 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.0, + self.pid = PIDController(0.5, 0.0, k_f=0.0, k_d=0.0, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) @@ -188,10 +188,12 @@ def update(self, CC, CS, now_nanos): prev_aego = self.aego.x self.aego.update(CS.out.aEgoBlended) jEgo = (self.aego.x - prev_aego) / DT_CTRL - future_aego = CS.out.aEgoBlended + jEgo * 0.15 # TODO: only for hybrid + # TODO: adjust for hybrid + future_aego = CS.out.aEgoBlended + jEgo * 0.5 self.debug = jEgo self.debug2 = future_aego + self.debug3 = self.aego.x # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR): @@ -251,9 +253,10 @@ def update(self, CC, CS, now_nanos): # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgoBlended, pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - future_aego, + # pcm_accel_compensation = self.pid.update(pcm_accel_cmd - future_aego, error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) - #pcm_accel_cmd += pcm_accel_compensation + # pcm_accel_cmd += pcm_accel_compensation else: self.pid.reset() self.pcm_accel_compensation.x = 0.0 From e73d60e989bf41cdf97b3c66d2fe68a170ea1285 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 25 Nov 2024 16:47:50 -0800 Subject: [PATCH 20/22] want to also try this --- opendbc/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 0d16c1e13a..0353096f2a 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.5, 0.0, k_f=0.0, k_d=0.0, + self.pid = PIDController(1, 0.0, k_f=0.0, k_d=0.0, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) @@ -189,7 +189,7 @@ def update(self, CC, CS, now_nanos): self.aego.update(CS.out.aEgoBlended) jEgo = (self.aego.x - prev_aego) / DT_CTRL # TODO: adjust for hybrid - future_aego = CS.out.aEgoBlended + jEgo * 0.5 + future_aego = CS.out.aEgoBlended + jEgo * 0.35 self.debug = jEgo self.debug2 = future_aego From 23c34e67cac26e5d0710302c95b374f55061e1bc Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 26 Nov 2024 13:25:49 -0800 Subject: [PATCH 21/22] Revert "want to also try this" This reverts commit e73d60e989bf41cdf97b3c66d2fe68a170ea1285. --- opendbc/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 0353096f2a..0d16c1e13a 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(1, 0.0, k_f=0.0, k_d=0.0, + self.pid = PIDController(0.5, 0.0, k_f=0.0, k_d=0.0, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) @@ -189,7 +189,7 @@ def update(self, CC, CS, now_nanos): self.aego.update(CS.out.aEgoBlended) jEgo = (self.aego.x - prev_aego) / DT_CTRL # TODO: adjust for hybrid - future_aego = CS.out.aEgoBlended + jEgo * 0.35 + future_aego = CS.out.aEgoBlended + jEgo * 0.5 self.debug = jEgo self.debug2 = future_aego From 2473351dc6f3a2ead31f84d5a714feed01e9fdbe Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 26 Nov 2024 13:50:21 -0800 Subject: [PATCH 22/22] apply the pcm accel net offset from standstill to i term and wind down --- opendbc/car/toyota/carcontroller.py | 17 ++++++++++++++--- opendbc/car/toyota/carstate.py | 4 +++- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 0d16c1e13a..30c06847b9 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -53,7 +53,7 @@ def __init__(self, dbc_names, CP): self.deque = deque([0] * 300, maxlen=300) - self.pid = PIDController(0.5, 0.0, k_f=0.0, k_d=0.0, + self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.125, pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN, rate=1 / DT_CTRL / 3) @@ -78,6 +78,8 @@ def __init__(self, dbc_names, CP): if not any(fw.ecu == Ecu.hybrid for fw in self.CP.carFw): self.pcm_accel_net.update_alpha(self.CP.longitudinalActuatorDelay + 0.2) + self.last_brake_frame = 0 + self.packer = CANPacker(dbc_names[Bus.pt]) self.accel = 0 self.prev_accel = 0 @@ -246,15 +248,24 @@ def update(self, CC, CS, now_nanos): # let PCM handle stopping for now pcm_accel_compensation = 0.0 - if not stopping and not CS.out.standstill: + if not stopping: self.pid.neg_limit = self.params.ACCEL_MIN - pcm_accel_cmd self.pid.pos_limit = self.params.ACCEL_MAX - pcm_accel_cmd + if self.permit_braking or CS.acc_braking: + self.last_brake_frame = self.frame + + if (self.frame - self.last_brake_frame) == 2 and pcm_accel_cmd < 0: + # unwind from diff of CS.pcm_accel_net and pcm_accel_cmd + self.i = (CS.pcm_accel_net - pcm_accel_cmd) # * self.pid.k_i # TODO: try different combos with with override + # TODO: freeze_integrator when stopping or at standstill? #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgoBlended, pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - future_aego, # pcm_accel_compensation = self.pid.update(pcm_accel_cmd - future_aego, - error_rate=self.error_rate.x) #, feedforward=pcm_accel_cmd) + error_rate=self.error_rate.x, #, feedforward=pcm_accel_cmd) + freeze_integrator=CS.out.standstill, # TODO: useless with override + override=CS.out.standstill) pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation) # pcm_accel_cmd += pcm_accel_compensation else: diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 685eecc967..a34626a19c 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -48,6 +48,7 @@ def __init__(self, CP): self.pcm_follow_distance = 0 + self.acc_braking = False self.acc_type = 1 self.lkas_hud = {} self.pcm_accel_net = 0.0 @@ -67,7 +68,8 @@ def update(self, can_parsers) -> structs.CarState: self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0) # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already - if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]: + self.acc_braking = cp.vl["PCM_CRUISE"]["ACC_BRAKING"] + if self.acc_braking: self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0) # add creeping force at low speeds only for braking, CLUTCH->ACCEL_NET already shows this