From c883a633a8490814bf9c165ddb39b212d927e8c1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 25 Oct 2024 01:20:15 -0700 Subject: [PATCH 1/3] Ford: small cc clean up --- opendbc/car/ford/carcontroller.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py index 85f42b1cfd..9c573ad420 100644 --- a/opendbc/car/ford/carcontroller.py +++ b/opendbc/car/ford/carcontroller.py @@ -102,23 +102,22 @@ def update(self, CC, CS, now_nanos): # Compensate for engine creep at low speed. # Either the ABS does not account for engine creep, or the correction is very slow # TODO: verify this applies to EV/hybrid - self.accel = actuators.accel + accel = actuators.accel if CC.longActive: - self.accel = apply_creep_compensation(self.accel, CS.out.vEgo) - self.accel = clip(self.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + accel = apply_creep_compensation(accel, CS.out.vEgo) + accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # Both gas and accel are in m/s^2, accel is used solely for braking - self.gas = self.accel - if not CC.longActive or self.gas < CarControllerParams.MIN_GAS: - self.gas = CarControllerParams.INACTIVE_GAS + gas = accel + if not CC.longActive or gas < CarControllerParams.MIN_GAS: + gas = CarControllerParams.INACTIVE_GAS # PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits + accel_due_to_pitch = 0.0 if len(CC.orientationNED) == 3: accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY - else: - accel_due_to_pitch = 0.0 - accel_pitch_compensated = self.accel + accel_due_to_pitch + accel_pitch_compensated = accel + accel_due_to_pitch if accel_pitch_compensated > 0.3 or not CC.longActive: self.brake_request = False elif accel_pitch_compensated < 0.0: @@ -126,7 +125,10 @@ def update(self, CC, CS, now_nanos): stopping = CC.actuators.longControlState == LongCtrlState.stopping # TODO: look into using the actuators packet to send the desired speed - can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, self.gas, self.accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX)) + can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX)) + + self.accel = accel + self.gas = gas ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) From 04bdb33df888f22f28b7c55ecb0e1caa92047e30 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 25 Oct 2024 03:39:56 -0500 Subject: [PATCH 2/3] Ford: reduce brake step response overshooting (#1419) clean up --- opendbc/car/ford/carcontroller.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py index 9c573ad420..748027073f 100644 --- a/opendbc/car/ford/carcontroller.py +++ b/opendbc/car/ford/carcontroller.py @@ -1,6 +1,6 @@ import math from opendbc.can.packer import CANPacker -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, apply_std_steer_angle_limits, structs +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL, apply_std_steer_angle_limits, structs from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags from opendbc.car.common.numpy_fast import clip, interp @@ -99,16 +99,22 @@ def update(self, CC, CS, now_nanos): ### longitudinal control ### # send acc msg at 50Hz if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: - # Compensate for engine creep at low speed. - # Either the ABS does not account for engine creep, or the correction is very slow - # TODO: verify this applies to EV/hybrid accel = actuators.accel + gas = accel + if CC.longActive: + # Compensate for engine creep at low speed. + # Either the ABS does not account for engine creep, or the correction is very slow + # TODO: verify this applies to EV/hybrid accel = apply_creep_compensation(accel, CS.out.vEgo) + + # The stock system has been seen rate limiting the brake accel to 5 m/s^3, + # however even 3.5 m/s^3 causes some overshoot with a step response. + accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL)) + accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # Both gas and accel are in m/s^2, accel is used solely for braking - gas = accel if not CC.longActive or gas < CarControllerParams.MIN_GAS: gas = CarControllerParams.INACTIVE_GAS From 92da02f476d0b17ade35fcb33b5b483c8f8bfac2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 25 Oct 2024 04:16:30 -0500 Subject: [PATCH 3/3] Ford: add long tuning back (#1413) * add long tuning back * try with radar * Revert "try with radar" This reverts commit 64c3e789e188e4f64aa8ef233c4ebfd03fe66075. --- opendbc/car/ford/interface.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py index dfc64747cb..53a5eb99c9 100644 --- a/opendbc/car/ford/interface.py +++ b/opendbc/car/ford/interface.py @@ -19,6 +19,9 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.5] + if DBC[candidate]['radar'] == RADAR.DELPHI_MRR: # average of 33.3 Hz radar timestep / 4 scan modes = 60 ms # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms