WIP: Honda Steering Improvements (#983)
* move honda steer params to CarParams * use breakpoints/values from firmware * remove comment * civic fw mod max included deadzone * civic bosch needs to be separate * script to measure steering accuracy * civic tuning * update cr-v values * cleanup * averaging over 10 samples was silly * update stock civic and cr-v tuning * remove comment * update replay expected resultspull/1134/head
Greg Hogan
GitHub
parent
1328a41a35
commit
57c44e3705
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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.]
|
||||
|
|
|
|||
|
|
@ -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}')
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
d0e2657d3a57ba231e3775d3bc901221d8794281
|
||||
e4fabe28e2223e40376320812ff803e833f72a7f
|
||||
Loading…
Reference in New Issue