diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index bb8ac4b37983eb..d61462222990b6 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -347,7 +347,7 @@ def get_params(candidate, fingerprint): ret.longitudinalKiV = [0.18, 0.12] else: - raise ValueError("unsupported car %s" % candidate) + raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.torque diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 4ed3118534ccc5..7b7118ccf77a36 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -4,6 +4,7 @@ from cereal import car import math import numpy as np +from selfdrive.kegman_conf import kegman_conf _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz @@ -25,7 +26,30 @@ def apply_deadzone(angle, deadzone): class LatControl(object): def __init__(self, CP): - + + kegman = kegman_conf() + self.write_conf = False + + if kegman.conf['react'] == "-1": + kegman.conf['react'] = str(CP.steerReactance) + self.write_conf = True + if kegman.conf['damp'] == "-1": + kegman.conf['damp'] = str(CP.steerInductance) + self.write_conf = True + if kegman.conf['resist'] == "-1": + kegman.conf['resist'] = str(CP.steerResistance) + self.write_conf = True + if kegman.conf['Kp'] == "-1": + kegman.conf['Kp'] = str(round(CP.steerKpV[0],2)) + self.write_conf = True + if kegman.conf['Ki'] == "-1": + kegman.conf['Ki'] = str(round(CP.steerKiV[0],2)) + self.write_conf = True + + if self.write_conf: + kegman.write_config(kegman.conf) + + self.mpc_frame = 0 if CP.steerResistance > 0 and CP.steerReactance >= 0 and CP.steerInductance > 0: self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) self.projection_factor = CP.steerReactance * CP.steerActuatorDelay / 2.0 # Mutiplier for reactive component (PI) @@ -62,8 +86,36 @@ def __init__(self, CP): def reset(self): self.pid.reset() + + def live_tune(self, CP): + self.mpc_frame += 1 + if self.mpc_frame % 300 == 0: + # live tuning through /data/openpilot/tune.py overrides interface.py settings + kegman = kegman_conf() + if kegman.conf['tuneGernby'] == "1": + self.reactance = float(kegman.conf['react']) + self.inductance = float(kegman.conf['damp']) + self.resistance = float(kegman.conf['resist']) + self.steerKpV = np.array([float(kegman.conf['Kp'])]) + self.steerKiV = np.array([float(kegman.conf['Ki'])]) + + self.accel_limit = 2.0 / self.resistance + self.projection_factor = self.reactance * CP.steerActuatorDelay / 2.0 + self.smooth_factor = self.inductance * 2.0 * CP.steerActuatorDelay / _DT + + # Eliminate break-points, since they aren't needed (and would cause problems for resonance) + #KpV = [np.interp(25.0, CP.steerKpBP, self.steerKpV)] + KpV = [np.interp(25.0, CP.steerKpBP, self.steerKpV)] + #KiV = [np.interp(25.0, CP.steerKiBP, self.steerKiV)] + KiV = [np.interp(25.0, CP.steerKiBP, self.steerKiV)] + self.pid = PIController(([0.], KpV), + ([0.], KiV), + k_f=CP.steerKf, pos_limit=1.0) + + def update(self, active, v_ego, angle_steers, angle_rate, angle_offset, steer_override, CP, VM, path_plan): + self.live_tune(CP) if angle_rate == 0.0 and self.calculate_rate: if angle_steers != self.prev_angle_steers: self.steer_counter_prev = self.steer_counter diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py index e269c77e45ddde..1a10d4127324a8 100644 --- a/selfdrive/kegman_conf.py +++ b/selfdrive/kegman_conf.py @@ -11,15 +11,21 @@ def read_config(self): if os.path.isfile('/data/kegman.json'): with open('/data/kegman.json', 'r') as f: self.config = json.load(f) + if "battPercOff" not in self.config: self.config.update({"battPercOff":"25"}) - self.element_updated = True - if "carVoltageMinEonShutdown" not in self.config: self.config.update({"carVoltageMinEonShutdown":"11800"}) - self.element_updated = True - if "brakeStoppingTarget" not in self.config: self.config.update({"brakeStoppingTarget":"0.25"}) self.element_updated = True + + if "tuneGernby" not in self.config: + self.config.update({"tuneGernby":"0"}) + self.config.update({"react":"-1"}) + self.config.update({"damp":"-1"}) + self.config.update({"resist":"-1"}) + self.config.update({"Kp":"-1"}) + self.config.update({"Ki":"-1"}) + self.element_updated = True # Force update battery charge limits to higher values for Big Model #if self.config['battChargeMin'] != "75": @@ -31,7 +37,10 @@ def read_config(self): self.write_config(self.config) else: - self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"85", "battChargeMax":"90", "wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", "brakeStoppingTarget":"0.25" } + self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \ + "wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", \ + "brakeStoppingTarget":"0.25", "tuneGernby":"0", "react":"-1", "damp":"-1", \ + "resist":"-1", "Kp":"-1", "Ki":"-1"} self.write_config(self.config) return self.config diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index ff58f7f6a6c556..1f630853ad8870 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ diff --git a/tune.py b/tune.py new file mode 100644 index 00000000000000..7c5979be8b3a6d --- /dev/null +++ b/tune.py @@ -0,0 +1,113 @@ +from selfdrive.kegman_conf import kegman_conf + +letters = { "a":[ "###", "# #", "###", "# #", "# #"], "b":[ "###", "# #", "###", "# #", "###"], "c":[ "###", "#", "#", "#", "###"], "d":[ "##", "# #", "# #", "# #", "##"], "e":[ "###", "#", "###", "#", "###"], "f":[ "###", "#", "###", "#", "#"], "g":[ "###", "# #", "###", " #", "###"], "h":[ "# #", "# #", "###", "# #", "# #"], "i":[ "###", " #", " #", " #", "###"], "j":[ "###", " #", " #", " #", "##"], "k":[ "# #", "##", "#", "##", "# #"], "l":[ "#", "#", "#", "#", "###"], "m":[ "# #", "###", "###", "# #", "# #"], "n":[ "###", "# #", "# #", "# #", "# #"], "o":[ "###", "# #", "# #", "# #", "###"], "p":[ "###", "# #", "###", "#", "#"], "q":[ "###", "# #", "###", " #", " #"], "r":[ "###", "# #", "##", "# #", "# #"], "s":[ "###", "#", "###", " #", "###"], "t":[ "###", " #", " #", " #", " #"], "u":[ "# #", "# #", "# #", "# #", "###"], "v":[ "# #", "# #", "# #", "# #", " #"], "w":[ "# #", "# #", "# #", "###", "###"], "x":[ "# #", " #", " #", " #", "# #"], "y":[ "# #", "# #", "###", " #", "###"], "z":[ "###", " #", " #", "#", "###"], " ":[ " "], "1":[ " #", "##", " #", " #", "###"], "2":[ "###", " #", "###", "#", "###"], "3":[ "###", " #", "###", " #", "###"], "4":[ "#", "#", "# #", "###", " #"], "5":[ "###", "#", "###", " #", "###"], "6":[ "###", "#", "###", "# #", "###"], "7":[ "###", " # ", " #", " #", "#"], "8":[ "###", "# #", "###", "# #", "###"], "9":[ "###", "# #", "###", " #", "###"], "0":[ "###", "# #", "# #", "# #", "###"], "!":[ " # ", " # ", " # ", " ", " # "], "?":[ "###", " #", " ##", " ", " # "], ".":[ " ", " ", " ", " ", " # "], "]":[ " ", " ", " ", " #", " # "], "/":[ " #", " #", " # ", "# ", "# "], ":":[ " ", " # ", " ", " # ", " "], "@":[ "###", "# #", "## ", "# ", "###"], "'":[ " # ", " # ", " ", " ", " "], "#":[ " # ", "###", " # ", "###", " # "], "-":[ " ", " ","###"," "," "] } +# letters stolen from here: http://www.stuffaboutcode.com/2013/08/raspberry-pi-minecraft-twitter.html + +def print_letters(text): + bigletters = [] + for i in text: + bigletters.append(letters.get(i.lower(),letters[' '])) + output = ['']*5 + for i in range(5): + for j in bigletters: + temp = ' ' + try: + temp = j[i] + except: + pass + temp += ' '*(5-len(temp)) + temp = temp.replace(' ',' ') + temp = temp.replace('#','@') + output[i] += temp + return '\n'.join(output) +import sys, termios, tty, os, time + +def getch(): + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +button_delay = 0.2 + +kegman = kegman_conf() +#kegman.conf['tuneGernby'] = "1" +#kegman.write_config(kegman.conf) +param = ["tuneGernby", "react", "damp", "resist", "Kp", "Ki"] + +j = 0 +while True: + print "" + print "" + print print_letters(param[j]) + print "" + print print_letters(kegman.conf[param[j]]) + print "" + print ("Press 7, 5, 3 to increase by 0.1, 0.05, 0.01") + print ("press d, g, j to decrease by 0.1, 0.05, 0.01") + print ("press 0 to make the value 0") + print ("press 1 to make the value 1") + print ("press SPACE for next parameter") + print ("press m for previous parameter") + print ("press q to quit") + + char = getch() + if (char == "7"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01) + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "5"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05) + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "3"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1) + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "j"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01) + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "g"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05) + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "d"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1) + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "0"): + kegman.conf[param[j]] = "0" + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == "1"): + kegman.conf[param[j]] = "1" + kegman.write_config(kegman.conf) + time.sleep(button_delay) + + elif (char == " "): + if j < len(param) - 1: + j = j + 1 + else: + j = 0 + + elif (char == "m"): + if j > 0: + j = j - 1 + else: + j = len(param) - 1 + + elif (char == "q"): + break diff --git a/tune.sh b/tune.sh new file mode 100755 index 00000000000000..d13626c182b26c --- /dev/null +++ b/tune.sh @@ -0,0 +1 @@ +python2.7 tune.py