diff --git a/.gitmodules b/.gitmodules index ad6530de9ac..403f883d348 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,7 +3,7 @@ url = ../../commaai/panda.git [submodule "opendbc"] path = opendbc_repo - url = ../../commaai/opendbc.git + url = git@github.com:JamesL787/opendbc.git [submodule "msgq"] path = msgq_repo url = ../../commaai/msgq.git diff --git a/opendbc_repo b/opendbc_repo index b4ef5e1cf40..5e47e8fe317 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit b4ef5e1cf406ff143fa67bdbfb154739d43279c9 +Subproject commit 5e47e8fe317da24fc39a5c4f3c07f75dac6dc5b8 diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 2a49690b7ef..abe4cc443ce 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -55,6 +55,7 @@ inline static std::unordered_map keys = { {"GsmRoaming", {PERSISTENT, BOOL}}, {"HardwareSerial", {PERSISTENT, STRING}}, {"HasAcceptedTerms", {PERSISTENT, STRING, "0"}}, + {"HondaBoschARadar", {PERSISTENT, BOOL, "1"}}, {"InstallDate", {PERSISTENT, TIME}}, {"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}}, {"IsEngaged", {PERSISTENT, BOOL}}, diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index aef92fb7abd..a97119ae719 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -76,7 +76,7 @@ def state_control(self): # Update VehicleModel lp = self.sm['vehicleParameters'] x = max(lp.stiffnessFactor, 0.1) - sr = max(lp.steerRatio, 0.1) + sr = max(self.CP.steerRatio if getattr(self.LaC, "tuned", False) else lp.steerRatio, 0.1) self.VM.update_params(x, sr) steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) @@ -91,6 +91,8 @@ def state_control(self): long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] + if isinstance(self.LaC, LatControlPID): + self.LaC.lane_changing = model_v2.meta.laneChangeState != LaneChangeState.off CC = car.CarControl.new_message() CC.enabled = self.sm['selfdriveState'].enabled diff --git a/openpilot/selfdrive/controls/lib/latcontrol_pid.py b/openpilot/selfdrive/controls/lib/latcontrol_pid.py index 48074c823de..3217434ebd6 100644 --- a/openpilot/selfdrive/controls/lib/latcontrol_pid.py +++ b/openpilot/selfdrive/controls/lib/latcontrol_pid.py @@ -1,10 +1,195 @@ import math +import numpy as np + from openpilot.cereal import log +from opendbc.car.honda.values import CAR as HONDA from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController +_CIVIC_TAIL_X = (105.022, 111.968, 118.982, 126.058, 133.203, 140.419, 147.705, 155.055, 162.478, 169.966, 177.528, + 185.157, 192.86, 200.64, 208.495, 216.42, 224.423, 232.498, 240.652, 248.879, 257.186, 265.575, + 274.046, 282.592, 291.221, 299.928, 308.719, 317.588, 326.545, 335.589, 344.722, 353.935, 363.239, 1135.663) +_CIVIC_TAIL_Y = (90, 95.053, 100.06, 105.018, 109.93, 114.799, 119.623, 124.399, 129.132, 133.818, 138.461, 143.058, + 147.614, 152.128, 156.602, 161.031, 165.42, 169.766, 174.073, 178.337, 182.563, 186.75, 190.899, + 195.007, 199.078, 203.109, 207.103, 211.057, 214.975, 218.858, 222.706, 226.515, 230.29, 720) +_CIVIC_C120 = ( + (0, 3.161, 4.712, 6.252, 9.375, 12.501, 15.624, 18.752, 31.248, 39.042, 46.866, 52.893, 58.973, 65.014, 71.15, + 72.196, 73.261, 74.349, 75.463, 76.604, 77.778, 78.99, 80.247, 82.521, 85.047, 87.555, 94.123, 96.059, + 97.8, 99.767, 101.605, 103.351) + _CIVIC_TAIL_X, + (0, 3.162, 4.769, 6.402, 9.524, 12.699, 15.747, 19.05, 31.744, 39.506, 47.236, 52.989, 58.723, 64.352, 70, + 70.931, 71.831, 72.701, 73.543, 74.36, 75.154, 75.931, 76.693, 77.977, 79.289, 80.512, 83.615, 84.576, + 85.485, 86.58, 87.685, 88.821) + _CIVIC_TAIL_Y, +) +_CIVIC_C020 = ( + (0, 4.128, 8.173, 12.142, 16.111, 20.158, 28.328, 32.296, 40.559, 45.697, 50.844, 56.007, 61.179, 66.567, + 71.98, 73.014, 74.068, 75.144, 76.247, 77.376, 78.539, 79.738, 80.983, 83.217, 85.698, 88.153, 94.526, + 96.394, 98.074, 99.964, 101.733, 103.414) + _CIVIC_TAIL_X, + (0, 4.128, 8.193, 12.099, 16.119, 20.039, 28.126, 32.083, 40.164, 45.142, 50.106, 55.06, 60, 65.009, 70, + 70.925, 71.82, 72.689, 73.532, 74.353, 75.153, 75.937, 76.709, 78.007, 79.341, 80.586, 83.728, 84.69, + 85.596, 86.675, 87.757, 88.862) + _CIVIC_TAIL_Y, +) +_CURVES = { + HONDA.HONDA_CLARITY: ( + (0, 2.016, 8, 11.902, 15.799, 19.8, 23.7, 31.7, 39.462, 44.588, 49.862, 55.122, 60.396, 65.769, 71.173, + 73.026, 74.958, 76.989, 79.152, 81.168, 83.403, 85.638, 91.532, 94.853, 96.638, 98.306, 99.89, 101.406, + 108.627, 115.904, 123.239, 130.631, 138.089, 145.605, 153.18, 160.814, 168.509, 176.263, 184.079, + 191.956, 199.902, 207.911, 215.983, 224.118, 232.325, 240.598, 248.935, 257.338, 265.814, 274.358, + 282.968, 291.646, 300.392, 309.206, 318.09, 327.043, 336.076, 345.179, 354.354, 363.601, 1047.17), + (0, 2.029, 8.104, 12.002, 15.999, 20, 23.999, 31.999, 39.665, 44.681, 49.807, 54.887, 59.95, 64.983, 70, + 71.641, 73.212, 74.729, 76.217, 77.508, 78.857, 80.146, 83.473, 85.45, 86.585, 87.709, 88.843, 90, + 95.643, 101.24, 106.792, 112.298, 117.766, 123.189, 128.568, 133.903, 139.195, 144.445, 149.652, + 154.816, 159.944, 165.031, 170.076, 175.08, 180.049, 184.978, 189.867, 194.716, 199.531, 204.306, + 209.043, 213.741, 218.402, 223.024, 227.609, 232.157, 236.672, 241.151, 245.594, 250, 720), + ), + HONDA.HONDA_CIVIC: _CIVIC_C120, + HONDA.HONDA_CRV_5G: ( + (0, 4.1, 8.2, 12.203, 14.247, 16.303, 20.403, 24.503, 28.506, 32.704, 40.904, 46.179, 51.464, 56.771, + 62.088, 67.408, 72.75, 74.599, 76.528, 78.557, 80.721, 82.702, 84.9, 92.755, 95.917, 97.601, 99.177, + 100.675, 102.109, 108.892, 115.729, 122.621, 129.569, 136.572, 143.632, 150.748, 157.921, 165.138, + 172.413, 179.746, 187.138, 194.59, 202.101, 209.672, 217.305, 225, 232.756, 240.575, 248.457, + 256.402, 264.411, 272.484, 280.622, 288.81, 297.062, 305.382, 313.768, 322.221, 330.743, 339.332, + 347.991, 1050.146), + (0, 4.1, 8.197, 12.194, 14.159, 16.115, 20.167, 24.049, 28, 32.098, 40.122, 45.172, 50.203, 55.227, + 60.233, 65.126, 70, 71.619, 73.186, 74.718, 76.241, 77.555, 78.944, 83.674, 85.647, 86.753, 87.835, + 88.914, 90, 95.228, 100.416, 105.565, 110.674, 115.743, 120.774, 125.766, 130.72, 135.626, 140.495, + 145.326, 150.12, 154.878, 159.599, 164.284, 168.933, 173.547, 178.126, 182.669, 187.178, 191.652, + 196.092, 200.497, 204.868, 209.198, 213.494, 217.758, 221.989, 226.187, 230.353, 234.487, 238.589, 720), + ), + HONDA.HONDA_INSIGHT: ( + (0, 4, 6.08, 8.186, 12.093, 16.279, 20.372, 24.465, 28.465, 32.651, 41.023, 46.35, 51.688, 57.048, + 62.418, 67.848, 73.3, 74.895, 76.558, 78.308, 80.174, 83.766, 90.489, 93.189, 95.975, 98.48, 105.666, + 112.895, 120.168, 127.484, 134.859, 142.279, 149.744, 157.253, 164.807, 172.407, 180.053, 187.745, + 195.498, 203.299, 211.146, 219.04, 226.983, 234.973, 243.011, 251.098, 259.25, 267.45, 275.7, 284, + 292.35, 300.75, 309.2, 317.702, 326.272, 334.893, 343.566, 352.291, 962.671), + (0, 4, 6.011, 8, 11.908, 15.955, 19.955, 23.955, 27.909, 31.91, 39.955, 45.028, 50.086, 55.14, 60.177, + 65.102, 70, 71.403, 72.82, 74.266, 75.767, 78.566, 83.674, 85.75, 87.95, 90, 95.965, 101.89, + 107.777, 113.626, 119.448, 125.232, 130.98, 136.689, 142.361, 147.996, 153.595, 159.157, 164.694, + 170.196, 175.661, 181.091, 186.486, 191.846, 197.17, 202.461, 207.728, 212.959, 218.157, 223.322, + 228.453, 233.55, 238.614, 243.646, 248.655, 253.63, 258.574, 263.485, 720), + ), + HONDA.HONDA_ACCORD: ( + (0, 4.638, 9.303, 14.006, 18.738, 23.507, 28.305, 33.142, 38.008, 42.903, 47.827, 52.791, 57.785, + 62.819, 67.884, 72.989, 78.124, 85.865, 93.684, 101.581, 109.557, 117.614, 125.752, 133.971, + 142.273, 150.676, 159.163, 167.736, 176.394, 185.14, 193.973, 202.896, 211.908, 221.031, 230.247, + 239.555, 248.956, 258.452, 268.044, 277.732, 287.517, 297.421, 307.424, 317.528, 327.733, 338.041, + 348.453, 358.969, 369.591, 1115.33), + (0, 4.606, 9.176, 13.721, 18.231, 22.714, 27.163, 31.587, 35.977, 40.332, 44.654, 48.952, 53.217, + 57.457, 61.665, 65.849, 70, 76.151, 82.238, 88.261, 94.222, 100.12, 105.957, 111.733, 117.449, + 123.117, 128.725, 134.276, 139.767, 145.203, 150.58, 155.903, 161.17, 166.393, 171.563, 176.678, + 181.739, 186.748, 191.705, 196.61, 201.464, 206.277, 211.04, 215.752, 220.416, 225.031, 229.598, + 234.117, 238.589, 720), + ), + HONDA.HONDA_CIVIC_BOSCH_DIESEL: ( + (0, 2.324, 4.657, 6.999, 9.351, 11.707, 14.073, 16.448, 18.833, 21.227, 23.631, 26.045, 28.468, 33.334, + 38.239, 43.174, 48.148, 53.153, 58.197, 63.282, 68.408, 73.565, 78.763, 86.278, 93.869, 101.552) + _CIVIC_TAIL_X, + (0, 2.315, 4.623, 6.922, 9.214, 11.493, 13.765, 16.029, 18.286, 20.534, 22.776, 25.01, 27.236, 31.657, + 36.048, 40.402, 44.725, 49.013, 53.27, 57.499, 61.7, 65.864, 70, 75.874, 81.683, 87.439) + _CIVIC_TAIL_Y, + ), +} +_RATIOS = { + HONDA.HONDA_CLARITY: (18.5, 12.72), + HONDA.HONDA_CIVIC: (17.24, 10.93), + HONDA.HONDA_CIVIC_BOSCH: (17.24, 10.93), + HONDA.HONDA_CIVIC_BOSCH_DIESEL: (17.24, 10.93), + HONDA.HONDA_ACCORD: (18.31, 11.82), + HONDA.HONDA_CRV_5G: (17.94, 12.3), + HONDA.HONDA_INSIGHT: (16.82, 12.58), +} + + +class _LatStiction: + TRACK, CAPTURE, HOLD, REACQUIRE = range(4) + + def __init__(self, dt): + self.dt = dt + self.reset() + + def reset(self): + self.state = self.TRACK + self.hold_torque = self.relative_motion = self._dwell_s = self._drift = self._reacquire_from = 0.0 + self._state_s = 0.12 + + def _enter(self, state, output): + self.state, self._state_s, self._dwell_s, self._drift = state, 0.0, 0.0, 0.0 + if state == self.HOLD: + self.hold_torque = float(np.clip(output, -1.0, 1.0)) + elif state == self.REACQUIRE: + self._reacquire_from = output + + def _passthrough(self, live_torque): + self._enter(self.TRACK, live_torque) + return live_torque + + def _reacquire(self, live_torque): + if self._reacquire_from * live_torque <= 0.0: + return self._passthrough(live_torque) + weight = float(np.clip(self._state_s / 0.08, 0.0, 1.0)) + output = self._reacquire_from + weight * (live_torque - self._reacquire_from) + if weight >= 1.0: + self._enter(self.TRACK, live_torque) + output = live_torque + return output + + def update(self, active, v_ego, error, desired_rate, wheel_rate, live_torque, steering_pressed, lane_changing, saturated): + self._state_s += self.dt + raw_motion = wheel_rate - desired_rate + if not active or steering_pressed or lane_changing or saturated: + self.relative_motion = raw_motion + return self._passthrough(live_torque) + + self.relative_motion += float(np.clip(self.dt / 0.05, 0.0, 1.0)) * (raw_motion - self.relative_motion) + e_hi = float(np.interp(v_ego, (8.0, 30.0), (0.9, 0.4))) + e_lo = float(np.interp(v_ego, (8.0, 30.0), (0.35, 0.2))) + closing = float(np.sign(error)) * self.relative_motion + + if self.state == self.TRACK: + settled = abs(error) <= e_lo and abs(self.relative_motion) <= 3.0 + self._dwell_s = self._dwell_s + self.dt if settled else 0.0 + capture_error = e_lo + min(0.12 * max(closing, 0.0), 5.0) + if self._state_s >= 0.12 and abs(desired_rate) < 100.0 and closing > 1.0 and abs(error) <= capture_error: + self._enter(self.CAPTURE, live_torque) + elif settled and self._dwell_s >= 0.15: + self._enter(self.HOLD, live_torque) + return self.hold_torque + else: + return live_torque + + if self.state == self.CAPTURE: + output = live_torque + if live_torque * self.relative_motion > 0.0: + removal = min(abs(live_torque), 0.008 * abs(self.relative_motion), 0.25) + output -= float(np.sign(live_torque)) * removal * float(np.clip(self._state_s / 0.08, 0.0, 1.0)) + settled = abs(error) <= e_lo and abs(self.relative_motion) <= 3.0 + self._dwell_s = self._dwell_s + self.dt if settled else 0.0 + moving_away = closing <= 0.0 and abs(error) >= e_hi + if settled and self._dwell_s >= 0.15: + self._enter(self.HOLD, output) + return self.hold_torque + if abs(desired_rate) >= 100.0: + return self._passthrough(live_torque) + if moving_away or self._state_s >= 0.75: + self._enter(self.REACQUIRE, output) + return self._reacquire(live_torque) + return output + + if self.state == self.HOLD: + if abs(desired_rate) >= 100.0 or (self.hold_torque * live_torque < 0.0 and abs(live_torque) >= 0.03): + return self._passthrough(live_torque) + self._drift += error * self.dt + breakaway = abs(error) >= e_hi or abs(self.relative_motion) >= 6.0 or abs(self._drift) >= 0.35 + urgent = abs(error) >= 2.0 * e_hi or abs(self.relative_motion) >= 12.0 + if urgent or (breakaway and self._state_s >= 0.2): + if urgent: + return self._passthrough(live_torque) + self._enter(self.REACQUIRE, self.hold_torque) + return self._reacquire(live_torque) + return self.hold_torque + + return self._reacquire(live_torque) + + class LatControlPID(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -13,13 +198,37 @@ def __init__(self, CP, CI, dt): pos_limit=self.steer_max, neg_limit=-self.steer_max) self.ff_factor = CP.lateralTuning.pid.kf self.get_steer_feedforward = CI.get_steer_feedforward_function() + self.car_fingerprint = CP.carFingerprint + self.tuned = self.car_fingerprint in _RATIOS + self.eps_modified = any(fw.ecu == "eps" and b"," in fw.fwVersion for fw in CP.carFw) + self.center_ratio, self.outer_ratio = _RATIOS.get(self.car_fingerprint, (CP.steerRatio, CP.steerRatio)) + if self.car_fingerprint == HONDA.HONDA_CIVIC_BOSCH: + eps_fw = {fw.fwVersion.split(b"\x00", 1)[0].replace(b",", b"-") for fw in CP.carFw if fw.ecu == "eps"} + self.angle_curve = _CIVIC_C020 if eps_fw & {b"39990-TBA-C020", b"39990-TGG-A120"} else _CIVIC_C120 + else: + self.angle_curve = _CURVES.get(self.car_fingerprint) + self.lane_changing = False + self.stiction = _LatStiction(dt) + self.previous_desired_angle = self.previous_output = 0.0 + self.previous_saturated = False + self.center_fade = 1.0 + self.phase_direction = self.steering_pressed_duration = 0.0 + self.previous_steering_pressed = False def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) - angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + base_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + if self.tuned: + if self.lane_changing: + angle_steers_des_no_offset = base_angle * self.outer_ratio / VM.sR + else: + linear_angle = base_angle * self.center_ratio / VM.sR + angle_steers_des_no_offset = math.copysign(float(np.interp(abs(linear_angle), *self.angle_curve)), linear_angle) + else: + angle_steers_des_no_offset = base_angle angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg error = angle_steers_des - CS.steeringAngleDeg @@ -28,22 +237,70 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat if not active: output_torque = 0.0 pid_log.active = False - + self.stiction.reset() + self.previous_desired_angle = angle_steers_des_no_offset + self.previous_output = 0.0 + self.previous_saturated = False + self.center_fade = 1.0 + self.phase_direction = self.steering_pressed_duration = 0.0 + self.previous_steering_pressed = False else: - # offset does not contribute to resistive torque - ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) - freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 + angle_delta = angle_steers_des_no_offset - self.previous_desired_angle + phase = angle_steers_des_no_offset * angle_delta + if self.tuned and phase != 0.0 and (CS.vEgo > 0.22352 or self.phase_direction == 0.0): + self.phase_direction = float(np.sign(phase)) + phase = abs(phase) * self.phase_direction + + steering_pressed = bool(CS.steeringPressed) + if self.tuned and self.eps_modified: + if not steering_pressed: + self.steering_pressed_duration = 0.0 + self.previous_steering_pressed = False + elif self.previous_steering_pressed or abs(self.previous_output) < 0.1 or CS.steeringTorque * self.previous_output < 0.0: + self.steering_pressed_duration, self.previous_steering_pressed = 1.0, True + else: + self.steering_pressed_duration = min(1.0, self.steering_pressed_duration + self.dt) + steering_pressed = self.steering_pressed_duration >= 0.28 + self.previous_steering_pressed = steering_pressed + + if self.tuned: + kf = float(np.interp(CS.vEgo, (0.0, 11.176, 22.352), (6e-5, 3e-5, 3e-5))) if self.car_fingerprint == HONDA.HONDA_ACCORD else \ + float(np.interp(CS.vEgo, (0.0, 11.175, 11.176, 22.352), (2.4e-6, 1.8e-6, 3.6e-6, 6e-6))) + else: + kf = self.ff_factor + ff = kf * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + freeze_speed = 2.0 if self.tuned and self.eps_modified else 5.0 + freeze_integrator = (steer_limited_by_safety or steering_pressed or CS.vEgo < freeze_speed + or (self.tuned and self.stiction.state != self.stiction.TRACK) + or (self.tuned and phase < 0.0 and self.pid.i * error > 0.0)) + self.pid.update(error, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator) + + if self.tuned and self.eps_modified: + if CS.leftBlinker or CS.rightBlinker: + self.center_fade = 0.0 + else: + self.center_fade += self.dt / (0.25 + self.dt) * (1.0 - self.center_fade) + boost = 1.0 + self.center_fade * float(np.clip(4.0 - abs(angle_steers_des_no_offset), 0.0, 1.0)) * \ + float(np.clip((CS.vEgo - 22.352) / 2.2352, 0.0, 1.0)) + output_torque = float(np.clip(self.pid.p * boost + self.pid.i + self.pid.d + self.pid.f, -self.steer_max, self.steer_max)) + else: + output_torque = float(self.pid.control) - output_torque = self.pid.update(error, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + if self.tuned: + stiction_limited = (curvature_limited or self.previous_saturated or abs(output_torque) >= self.steer_max - 1e-3 + or CS.steerFaultTemporary or CS.steerFaultPermanent) + output_torque = float(self.stiction.update(active, CS.vEgo, error, angle_delta / self.dt, float(CS.steeringRateDeg), + output_torque, steering_pressed, self.lane_changing, stiction_limited)) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_torque) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, + steer_limited_by_safety, curvature_limited)) + self.previous_desired_angle = angle_steers_des_no_offset + self.previous_output = output_torque + self.previous_saturated = bool(pid_log.saturated) return output_torque, angle_steers_des, pid_log diff --git a/openpilot/selfdrive/controls/tests/test_latcontrol.py b/openpilot/selfdrive/controls/tests/test_latcontrol.py index 2b7a0cf695a..2b33e230f25 100644 --- a/openpilot/selfdrive/controls/tests/test_latcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_latcontrol.py @@ -1,3 +1,8 @@ +import math +from types import SimpleNamespace + +import numpy as np + from openpilot.common.test import OpenpilotTestCase from openpilot.common.parameterized import parameterized @@ -10,13 +15,31 @@ from opendbc.car.gm.values import CAR as GM from opendbc.car.vehicle_model import VehicleModel from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID, _CIVIC_C020, _CIVIC_C120, _CURVES, _LatStiction from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle class TestLatControl(OpenpilotTestCase): + @staticmethod + def pid_controller(car_name=HONDA.HONDA_CLARITY, modified=True, fw=b"39990-TRW-A020"): + tune = SimpleNamespace(pid=SimpleNamespace(kpBP=[0.], kpV=[0.1], kiBP=[0.], kiV=[0.], kf=0.01)) + car_fw = [SimpleNamespace(ecu="eps", fwVersion=fw.replace(b"-", b",", 1) if modified else fw)] + CP = SimpleNamespace(steerLimitTimer=0.8, lateralTuning=tune, steerRatio=16.5, + carFingerprint=car_name, carFw=car_fw) + CI = SimpleNamespace(get_steer_feedforward_function=lambda: lambda angle, speed: 1.0) + return LatControlPID(CP, CI, DT_CTRL) + + @staticmethod + def pid_inputs(v_ego=30.0, steering_angle=-1.0): + CS = SimpleNamespace(vEgo=v_ego, steeringAngleDeg=steering_angle, steeringRateDeg=0.0, steeringPressed=False, + steeringTorque=0.0, leftBlinker=False, rightBlinker=False, + steerFaultTemporary=False, steerFaultPermanent=False) + VM = SimpleNamespace(sR=18.5, get_steer_from_curvature=lambda curvature, speed, roll: curvature) + params = SimpleNamespace(roll=0.0, angleOffsetDeg=0.0) + return CS, VM, params + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)]) def test_saturation(self, car_name, controller): @@ -45,3 +68,76 @@ def test_saturation(self, car_name, controller): for _ in range(1000): _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False, 0.2) assert lac_log.saturated + + def test_honda_fixed_steering_curves(self): + curves = [ + (_CURVES[HONDA.HONDA_CLARITY], (59.570751, 76.770346, 82.599905, 104.350769)), + (_CIVIC_C120, (59.684930, 76.546436, 81.663266, 100.779317)), + (_CIVIC_C020, (58.875419, 76.102489, 81.495405, 100.779317)), + (_CURVES[HONDA.HONDA_CRV_5G], (58.270013, 75.745112, 82.018583, 103.616196)), + (_CURVES[HONDA.HONDA_INSIGHT], (57.911805, 75.628947, 83.301413, 107.642051)), + (_CURVES[HONDA.HONDA_ACCORD], (55.089501, 71.502052, 79.385722, 101.843927)), + (_CURVES[HONDA.HONDA_CIVIC_BOSCH_DIESEL], (54.776499, 70.975658, 78.737376, 100.779317)), + ] + for (x, y), expected in curves: + assert len(x) == len(y) + assert np.all(np.diff(x) > 0.0) and np.all(np.diff(y) > 0.0) + np.testing.assert_allclose(np.interp((60, 80, 90, 120), x, y), expected, atol=0.02) + np.testing.assert_allclose(-np.interp((60, 80, 90, 120), x, y), -np.asarray(expected), atol=0.02) + + def test_honda_curve_selection_and_lane_change(self): + c120 = self.pid_controller(HONDA.HONDA_CIVIC_BOSCH, fw=b"39990-TBA-C120") + c020 = self.pid_controller(HONDA.HONDA_CIVIC_BOSCH, fw=b"39990-TBA-C020") + assert c120.angle_curve is _CIVIC_C120 + assert c020.angle_curve is _CIVIC_C020 + + controller = self.pid_controller() + CS, VM, params = self.pid_inputs(steering_angle=400.0) + _, mapped, _ = controller.update(False, CS, VM, params, False, -math.radians(120.0), False, 0.1) + CS.steeringAngleDeg = -400.0 + _, mapped_again, _ = controller.update(False, CS, VM, params, False, -math.radians(120.0), False, 0.1) + assert mapped == mapped_again + assert np.isclose(mapped, np.interp(120.0, *_CURVES[HONDA.HONDA_CLARITY])) + controller.lane_changing = True + _, lane_change, _ = controller.update(False, CS, VM, params, False, -math.radians(120.0), False, 0.1) + assert np.isclose(lane_change, 120.0 * 12.72 / 18.5) + + def test_center_boost_and_feedforward(self): + controller = self.pid_controller() + CS, VM, params = self.pid_inputs() + output, _, log = controller.update(True, CS, VM, params, False, 0.0, False, 0.1) + assert np.isclose(output, 2.0 * log.p + log.f) + assert np.isclose(log.f, np.interp(CS.vEgo, (0.0, 11.175, 11.176, 22.352), (2.4e-6, 1.8e-6, 3.6e-6, 6e-6))) + + controller = self.pid_controller() + CS.leftBlinker = True + output, _, log = controller.update(True, CS, VM, params, False, 0.0, False, 0.1) + assert np.isclose(output, log.p + log.f) + + def test_predictive_stiction_capture_hold_and_release(self): + stiction = _LatStiction(DT_CTRL) + first = stiction.update(True, 0.0, 1.0, 0.0, 30.0, 0.5, False, False, False) + assert stiction.state == stiction.CAPTURE and first == 0.5 + for _ in range(8): + captured = stiction.update(True, 0.0, 1.0, 0.0, 30.0, 0.5, False, False, False) + assert 0.25 <= captured < 0.5 + + for _ in range(80): + held = stiction.update(True, 0.0, 0.1, 0.0, 0.0, 0.3, False, False, False) + if stiction.state == stiction.HOLD: + break + assert stiction.state == stiction.HOLD + assert stiction.update(True, 0.0, 0.1, 0.0, 0.0, 0.7, False, False, False) == held + assert stiction.update(True, 0.0, -0.1, 0.0, 0.0, -0.1, False, False, False) == -0.1 + assert stiction.state == stiction.TRACK + + def test_predictive_stiction_gates_and_zero_speed(self): + for gate in ((False, False, True), (False, True, False), (True, False, False)): + stiction = _LatStiction(DT_CTRL) + output = stiction.update(True, 0.0, 1.0, 0.0, 30.0, 0.5, *gate) + assert output == 0.5 and stiction.state == stiction.TRACK + + stiction = _LatStiction(DT_CTRL) + stiction._enter(stiction.REACQUIRE, 0.2) + assert stiction.update(True, 0.0, 0.1, 0.0, 0.0, -0.3, False, False, False) == -0.3 + assert stiction.state == stiction.TRACK