diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 4b5354c47c..26ad2e655e 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -2,7 +2,7 @@ from collections import namedtuple from cereal import car from common.realtime import DT_CTRL from selfdrive.controls.lib.drive_helpers import rate_limit -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD @@ -74,6 +74,15 @@ HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "car", "lanes", "fcw", "acc_alert", "steer_required"]) +class CarControllerParams(): + def __init__(self, CP): + self.BRAKE_MAX = 1024//4 + self.STEER_MAX = CP.lateralParams.torqueBP[-1] + # mirror of list (assuming first item is zero) for interp of signed request values + assert(CP.lateralParams.torqueBP[0] == 0) + assert(CP.lateralParams.torqueBP[0] == 0) + self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) + self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) class CarController(): def __init__(self, dbc_name, CP): @@ -84,16 +93,15 @@ class CarController(): self.last_pump_ts = 0. self.packer = CANPacker(dbc_name) self.new_radar_config = False - self.eps_modified = False - for fw in CP.carFw: - if fw.ecu == "eps" and b"," in fw.fwVersion: - print("EPS FW MODIFIED!") - self.eps_modified = True + + self.params = CarControllerParams(CP) def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): + P = self.params + # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) @@ -126,29 +134,10 @@ class CarController(): # **** process the car messages **** - # *** compute control surfaces *** - BRAKE_MAX = 1024//4 - if CS.CP.carFingerprint in (CAR.ACURA_ILX): - STEER_MAX = 0xF00 - elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): - STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value - elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): - STEER_MAX = 0x7FFF - elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: - STEER_MAX = 0x1400 - else: - STEER_MAX = 0x1000 - # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) - apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) - apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) - - if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: - if apply_steer > 0xA00: - apply_steer = (apply_steer - 0xA00) / 2 + 0xA00 - elif apply_steer < -0xA00: - apply_steer = (apply_steer + 0xA00) / 2 - 0xA00 + apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) + apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 92224ff287..9665b6c702 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -161,6 +161,7 @@ class CarInterface(CarInterfaceBase): # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward @@ -169,16 +170,40 @@ class CarInterface(CarInterfaceBase): if fw.ecu == "eps" and b"," in fw.fwVersion: eps_modified = True - if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: + if candidate == CAR.CIVIC: stop_and_go = True ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec + if eps_modified: + # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE + # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 + # modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180 + # stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108 + # modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480 + # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if eps_modified else [[0.8], [0.24]] - ret.lateralTuning.pid.kf = 0.00006 + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] + + elif candidate == CAR.CIVIC_BOSCH: + stop_and_go = True + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT + ret.steerRatio = 15.38 # 10.93 is end-to-end spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 1. + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -192,6 +217,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 16.33 # 11.82 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.8467 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -205,6 +231,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.72 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -218,6 +245,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.89 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -232,8 +260,16 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end + if eps_modified: + # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F + # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 + # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] tire_stiffness_factor = 0.677 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -246,6 +282,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -259,6 +296,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.53 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 13.06 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -272,6 +310,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -285,6 +324,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -298,6 +338,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY ret.steerRatio = 14.35 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -311,6 +352,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.82 ret.centerToFront = ret.wheelbase * 0.428 ret.steerRatio = 17.25 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -324,6 +366,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.59 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] diff --git a/selfdrive/debug/internal/measure_steering_accuracy.py b/selfdrive/debug/internal/measure_steering_accuracy.py new file mode 100755 index 0000000000..83f297bcb7 --- /dev/null +++ b/selfdrive/debug/internal/measure_steering_accuracy.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +import os +import argparse +import signal +from collections import deque, defaultdict +from statistics import mean + +import cereal.messaging as messaging + +def sigint_handler(signal, frame): + print("handler!") + exit(0) +signal.signal(signal.SIGINT, sigint_handler) + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description='Sniff a communcation socket') + parser.add_argument('--addr', default='127.0.0.1') + args = parser.parse_args() + + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) + sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'], addr=args.addr) + + msg_cnt = 0 + stats = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0}) + cnt = 0 + total_error = 0 + + while messaging.recv_one(carControl): + sm.update() + msg_cnt += 1 + + actual_speed = sm['carState'].vEgo + enabled = sm['controlsState'].enabled + steer_override = sm['controlsState'].steerOverride + + # must be above 10 m/s, engaged and not overriding steering + if actual_speed > 10.0 and enabled and not steer_override: + cnt += 1 + + # wait 5 seconds after engage/override + if cnt >= 500: + # calculate error before rounding + actual_angle = sm['controlsState'].angleSteers + desired_angle = sm['carControl'].actuators.steerAngle + angle_error = abs(desired_angle - actual_angle) + + # round numbers + actual_angle = round(actual_angle, 1) + desired_angle = round(desired_angle, 1) + angle_error = round(angle_error, 2) + angle_abs = int(abs(round(desired_angle, 0))) + + # collect stats + stats[angle_abs]["err"] += angle_error + stats[angle_abs]["cnt"] += 1 + if actual_angle == desired_angle: + stats[angle_abs]["="] += 1 + else: + if desired_angle == 0.: + overshoot = True + else: + overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle + stats[angle_abs]["+" if overshoot else "-"] += 1 + else: + cnt = 0 + + if msg_cnt % 100 == 0: + print(chr(27) + "[2J") + if cnt != 0: + print("COLLECTING ...") + else: + print("DISABLED (speed too low, not engaged, or steer override)") + for k in sorted(stats.keys()): + v = stats[k] + print(f'angle: {k:#2} | error: {round(v["err"] / v["cnt"], 2):2.2f} | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#4}% | -:{int(v["-"] / v["cnt"] * 100):#3}% | count: {v["cnt"]:#4}') diff --git a/selfdrive/debug/internal/measure_torque_time_to_max.py b/selfdrive/debug/internal/measure_torque_time_to_max.py index 65f13afc84..a7d450ede9 100755 --- a/selfdrive/debug/internal/measure_torque_time_to_max.py +++ b/selfdrive/debug/internal/measure_torque_time_to_max.py @@ -3,6 +3,8 @@ import os import sys import argparse import struct +from collections import deque +from statistics import mean from cereal import log import cereal.messaging as messaging @@ -26,7 +28,7 @@ if __name__ == "__main__": start_v = 0 max_v = 0 max_t = 0 - window = [0] * 10 + window = deque(maxlen=10) avg = 0 while 1: polld = poller.poll(1000) @@ -49,8 +51,7 @@ if __name__ == "__main__": if item.address == 0x1ab and item.src == 0: motor_torque = ((item.dat[0] & 0x3) << 8) + item.dat[1] window.append(motor_torque) - window.pop(0) - avg = sum(window) / len(window) + avg = mean(window) #print(f'{evt.logMonoTime}: {avg}') if active and avg > max_v + 0.5: max_v = avg diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0bd156c6e0..23d4a28f43 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d0e2657d3a57ba231e3775d3bc901221d8794281 \ No newline at end of file +e4fabe28e2223e40376320812ff803e833f72a7f \ No newline at end of file