From a02bc737cf30c6f38d6971f10592a1f0583b9630 Mon Sep 17 00:00:00 2001 From: Kishan Karunaratne Date: Fri, 10 Jan 2020 06:39:19 -0800 Subject: [PATCH] Clean up and update Live Tuner (#3) --- selfdrive/car/gm/interface.py | 3 +- selfdrive/car/honda/carcontroller.py | 6 +- selfdrive/car/honda/carstate.py | 6 +- selfdrive/controls/lib/drive_helpers.py | 4 +- selfdrive/controls/lib/driver_monitor.py | 6 +- selfdrive/controls/lib/lane_planner.py | 5 +- selfdrive/controls/lib/latcontrol_pid.py | 6 +- selfdrive/controls/lib/long_mpc.py | 8 +- selfdrive/controls/lib/longcontrol.py | 4 +- selfdrive/controls/lib/pathplanner.py | 19 +- selfdrive/controls/lib/planner.py | 6 +- selfdrive/kegman_conf.py | 273 ++++++++++--------- selfdrive/thermald.py | 4 +- tune.py | 323 ++++++++++++----------- 14 files changed, 358 insertions(+), 315 deletions(-) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 37264624885b65..9f064de304a153 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -116,6 +116,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1.0 + ret.steerActuatorDelay = 0.1 # Default delay, not measured yet if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) @@ -126,8 +127,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay ret.steerRatio = 15.7 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 # wild guess - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.12], [0.05]] - ret.steerRateCost = 0.7 elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 0deca12637cf0d..bb0eb1f739ebfd 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -5,12 +5,12 @@ from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import AH, CruiseButtons, CAR -from selfdrive.kegman_conf import kegman_conf - -kegman = kegman_conf() +from selfdrive.kegman_conf import KegmanConf from opendbc.can.packer import CANPacker +kegman = KegmanConf() + def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params brake_hyst_on = 0.02 # to activate brakes exceed this value diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 04da68964e5f2a..1b4a628ee319cb 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -6,7 +6,7 @@ from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf GearShifter = car.CarState.GearShifter @@ -202,7 +202,7 @@ def get_cam_can_parser(CP): class CarState(): def __init__(self, CP): - self.kegman = kegman_conf() + self.kegman = KegmanConf() self.trMode = int(self.kegman.conf['lastTrMode']) # default to last distance interval on startup #self.trMode = 1 self.lkMode = True @@ -392,7 +392,7 @@ def update(self, cp, cp_cam): if self.cruise_setting == 3: if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0: self.trMode = (self.trMode + 1 ) % 4 - self.kegman = kegman_conf() + self.kegman = KegmanConf() self.kegman.conf['lastTrMode'] = str(self.trMode) # write last distance bar setting to file self.kegman.write_config(self.kegman.conf) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f8d432e186435f..e97b9c75b11bbb 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,10 +1,10 @@ from cereal import car from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf # kph -kegman = kegman_conf() +kegman = KegmanConf() V_CRUISE_MAX = 144 V_CRUISE_MIN = 8 V_CRUISE_DELTA = int(kegman.conf['CruiseDelta']) diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 98eeae71f4bbcc..43207224dea1b1 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -3,8 +3,10 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter -from selfdrive.kegman_conf import kegman_conf -kegman = kegman_conf() +from selfdrive.kegman_conf import KegmanConf + + +kegman = KegmanConf() _AWARENESS_TIME = min(int(kegman.conf['wheelTouchSeconds']), 600) # x minutes limit without user touching steering wheels make the car enter a terminal status diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 846c74661c0344..73ba864fa722de 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,9 +1,10 @@ from common.numpy_fast import interp import numpy as np -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf from cereal import log -kegman = kegman_conf() + +kegman = KegmanConf() CAMERA_OFFSET = float(kegman.conf['cameraOffset']) # m from center car to camera #zorrobyte diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 4742abf509df3e..c63f3333f3bf18 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -2,12 +2,12 @@ from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import car from cereal import log -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf class LatControlPID(): def __init__(self, CP): - self.kegman = kegman_conf(CP) + self.kegman = KegmanConf(CP) self.deadzone = float(self.kegman.conf['deadzone']) self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), @@ -22,7 +22,7 @@ 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 - self.kegman = kegman_conf() + self.kegman = KegmanConf(CP) if self.kegman.conf['tuneGernby'] == "1": self.steerKpV = [float(self.kegman.conf['Kp'])] self.steerKiV = [float(self.kegman.conf['Ki'])] diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index e528589c2285a6..3d7631cd6f2c8c 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -8,10 +8,10 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf # One, two and three bar distances (in s) -kegman = kegman_conf() +kegman = KegmanConf() if "ONE_BAR_DISTANCE" in kegman.conf: ONE_BAR_DISTANCE = float(kegman.conf['ONE_BAR_DISTANCE']) else: @@ -88,7 +88,7 @@ def __init__(self, mpc_id): self.bp_counter = 0 - kegman = kegman_conf() + kegman = KegmanConf() self.oneBarBP = [float(kegman.conf['1barBP0']), float(kegman.conf['1barBP1'])] self.twoBarBP = [float(kegman.conf['2barBP0']), float(kegman.conf['2barBP1'])] self.threeBarBP = [float(kegman.conf['3barBP0']), float(kegman.conf['3barBP1'])] @@ -178,7 +178,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Live Tuning of breakpoints for braking profile change self.bp_counter += 1 if self.bp_counter % 500 == 0: - kegman = kegman_conf() + kegman = KegmanConf() self.oneBarBP = [float(kegman.conf['1barBP0']), float(kegman.conf['1barBP1'])] self.twoBarBP = [float(kegman.conf['2barBP0']), float(kegman.conf['2barBP1'])] self.threeBarBP = [float(kegman.conf['3barBP0']), float(kegman.conf['3barBP1'])] diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f6babe783fd21a..acf854448c9338 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,9 +1,9 @@ from cereal import log from common.numpy_fast import clip, interp from selfdrive.controls.lib.pid import PIController -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf -kegman = kegman_conf() +kegman = KegmanConf() LongCtrlState = log.ControlsState.LongControlState STOPPING_EGO_SPEED = 0.5 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index f096febbd88c47..88a39c7a52d30a 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -5,7 +5,7 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT from selfdrive.controls.lib.lane_planner import LanePlanner -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf from common.numpy_fast import interp import cereal.messaging as messaging from cereal import log @@ -58,7 +58,7 @@ def __init__(self, CP): self.steerRatio_new = 0.0 self.sR_time = 1 - kegman = kegman_conf(CP) + kegman = KegmanConf(CP) if kegman.conf['steerRatio'] == "-1": self.steerRatio = CP.steerRatio else: @@ -68,6 +68,11 @@ def __init__(self, CP): self.steerRateCost = CP.steerRateCost else: self.steerRateCost = float(kegman.conf['steerRateCost']) + + if kegman.conf['steerActuatorDelay'] == "-1": + self.steerActuatorDelay = CP.steerActuatorDelay + else: + self.steerActuatorDelay = float(kegman.conf['steerActuatorDelay']) self.sR = [float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost']))] self.sRBP = [float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1'])] @@ -116,16 +121,18 @@ def update(self, sm, pm, CP, VM): VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) curvature_factor = VM.curvature_factor(v_ego) - # Get steerRatio and steerRateCost from kegman.json every x seconds + # Get steerRatio, steerRateCost, steerActuatorDelay from kegman.json every x seconds self.mpc_frame += 1 if self.mpc_frame % 500 == 0: # live tuning through /data/openpilot/tune.py overrides interface.py settings - kegman = kegman_conf() + kegman = KegmanConf() if kegman.conf['tuneGernby'] == "1": self.steerRateCost = float(kegman.conf['steerRateCost']) if self.steerRateCost != self.steerRateCost_prev: self.setup_mpc() self.steerRateCost_prev = self.steerRateCost + + self.steerActuatorDelay = float(kegman.conf['steerActuatorDelay']) self.sR = [float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost']))] self.sRBP = [float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1'])] @@ -146,8 +153,6 @@ def update(self, sm, pm, CP, VM): self.sR_delay_counter = 0 else: self.steerRatio = self.sR[0] - - print("steerRatio = ", self.steerRatio) self.LP.parse_model(sm['model']) @@ -229,7 +234,7 @@ def update(self, sm, pm, CP, VM): # self.path_offset_i = 0.0 # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, self.steerRatio, CP.steerActuatorDelay) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, self.steerRatio, self.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index a272ae35770655..fdec30b0cdd72a 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -13,7 +13,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf MAX_SPEED = 255.0 @@ -82,7 +82,7 @@ def __init__(self, CP): self.path_x = np.arange(192) self.params = Params() - self.kegman = kegman_conf() + self.kegman = KegmanConf() self.mpc_frame = 0 def choose_solution(self, v_cruise_setpoint, enabled): @@ -128,7 +128,7 @@ def update(self, sm, pm, CP, VM, PP): enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) if self.mpc_frame % 1000 == 0: - self.kegman = kegman_conf() + self.kegman = KegmanConf() self.mpc_frame = 0 self.mpc_frame += 1 diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py index 4c59a96e734bdf..ed4116c00d5b47 100644 --- a/selfdrive/kegman_conf.py +++ b/selfdrive/kegman_conf.py @@ -1,158 +1,171 @@ import json import os -class kegman_conf(): + +class KegmanConf: def __init__(self, CP=None): self.conf = self.read_config() if CP is not None: self.init_config(CP) def init_config(self, CP): - write_conf = False if self.conf['tuneGernby'] != "1": self.conf['tuneGernby'] = str(1) - write_conf = True - + # only fetch Kp, Ki, Kf sR and sRC from interface.py if it's a PID controlled car if CP.lateralTuning.which() == 'pid': if self.conf['Kp'] == "-1": - self.conf['Kp'] = str(round(CP.lateralTuning.pid.kpV[0],3)) - write_conf = True + self.conf['Kp'] = str(round(CP.lateralTuning.pid.kpV[0], 3)) if self.conf['Ki'] == "-1": - self.conf['Ki'] = str(round(CP.lateralTuning.pid.kiV[0],3)) - write_conf = True + self.conf['Ki'] = str(round(CP.lateralTuning.pid.kiV[0], 3)) if self.conf['Kf'] == "-1": self.conf['Kf'] = str('{:f}'.format(CP.lateralTuning.pid.kf)) - write_conf = True - + if self.conf['steerRatio'] == "-1": - self.conf['steerRatio'] = str(round(CP.steerRatio,3)) - write_conf = True - + self.conf['steerRatio'] = str(round(CP.steerRatio, 3)) + if self.conf['steerRateCost'] == "-1": - self.conf['steerRateCost'] = str(round(CP.steerRateCost,3)) - write_conf = True + self.conf['steerRateCost'] = str(round(CP.steerRateCost, 3)) - if write_conf: - self.write_config(self.config) + if self.conf['steerActuatorDelay'] == "-1": + self.conf['steerActuatorDelay'] = str(round(CP.steerActuatorDelay, 3)) + + self.write_config(self.conf) def read_config(self): - self.element_updated = False + config = { + "Kp": "-1", + "Ki": "-1", + "Kf": "-1", + "steerRatio": "-1", + "steerRateCost": "-1", + "steerActuatorDelay": "-1", + "deadzone": "0.0", + "cameraOffset": "0.06", + "wheelTouchSeconds": "180", + "tuneGernby": "1", + "slowOnCurves": "0", + "lastTrMode": "1", + "battChargeMin": "60", + "battChargeMax": "70", + "battPercOff": "25", + "carVoltageMinEonShutdown": "11800", + "brakeStoppingTarget": "0.25", + "liveParams": "1", + "leadDistance": "5", + "1barBP0": "-0.1", + "1barBP1": "2.25", + "2barBP0": "-0.1", + "2barBP1": "2.5", + "3barBP0": "0.0", + "3barBP1": "3.0", + "1barMax": "2.1", + "2barMax": "2.1", + "3barMax": "2.1", + "1barHwy": "0.4", + "2barHwy": "0.3", + "3barHwy": "0.1", + "sR_boost": "0", + "sR_BP0": "0", + "sR_BP1": "0", + "sR_time": "1", + "ALCnudgeLess": "0", + "ALCminSpeed": "20.1168", + "ALCtimer": "2.0", + "CruiseDelta": "8", + "CruiseEnableMin": "40" + } 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.config.update({"carVoltageMinEonShutdown":"11800"}) - self.config.update({"brakeStoppingTarget":"0.25"}) - self.element_updated = True - - if "tuneGernby" not in self.config: - self.config.update({"tuneGernby":"1"}) - self.config.update({"Kp":"-1"}) - self.config.update({"Ki":"-1"}) - self.element_updated = True - - if "liveParams" not in self.config: - self.config.update({"liveParams":"1"}) - self.element_updated = True - - if "steerRatio" not in self.config: - self.config.update({"steerRatio":"-1"}) - self.config.update({"steerRateCost":"-1"}) - self.element_updated = True - - if "leadDistance" not in self.config: - self.config.update({"leadDistance":"5"}) - self.element_updated = True - - if "deadzone" not in self.config: - self.config.update({"deadzone":"0.0"}) - self.element_updated = True - - if "1barBP0" not in self.config: - self.config.update({"1barBP0":"-0.1"}) - self.config.update({"1barBP1":"2.25"}) - self.config.update({"2barBP0":"-0.1"}) - self.config.update({"2barBP1":"2.5"}) - self.config.update({"3barBP0":"0.0"}) - self.config.update({"3barBP1":"3.0"}) - self.element_updated = True - - - if "1barMax" not in self.config: - self.config.update({"1barMax":"2.1"}) - self.config.update({"2barMax":"2.1"}) - self.config.update({"3barMax":"2.1"}) - self.element_updated = True - - if "1barHwy" not in self.config: - self.config.update({"1barHwy":"0.4"}) - self.config.update({"2barHwy":"0.3"}) - self.config.update({"3barHwy":"0.1"}) - self.element_updated = True - - if "slowOnCurves" not in self.config: - self.config.update({"slowOnCurves":"0"}) - self.element_updated = True - - if "Kf" not in self.config: - self.config.update({"Kf":"-1"}) - self.element_updated = True - - if "sR_boost" not in self.config: - self.config.update({"sR_boost":"0"}) - self.config.update({"sR_BP0":"0"}) - self.config.update({"sR_BP1":"0"}) - self.config.update({"sR_time":"1"}) - self.element_updated = True - - if "ALCnudgeLess" not in self.config: - self.config.update({"ALCnudgeLess":"0"}) - self.config.update({"ALCminSpeed":"20.1168"}) - self.element_updated = True - - if "ALCtimer" not in self.config: - self.config.update({"ALCtimer":"2.0"}) - self.element_updated = True - - if "CruiseDelta" not in self.config: - self.config.update({"CruiseDelta":"8"}) - self.element_updated = True - - if "CruiseEnableMin" not in self.config: - self.config.update({"CruiseEnableMin":"40"}) - self.element_updated = True - - if self.element_updated: - print("updated") - self.write_config(self.config) - - else: - self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \ - "wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", \ - "brakeStoppingTarget":"0.25", "tuneGernby":"1", \ - "Kp":"-1", "Ki":"-1", "liveParams":"1", "leadDistance":"5", "deadzone":"0.0", \ - "1barBP0":"-0.1", "1barBP1":"2.25", "2barBP0":"-0.1", "2barBP1":"2.5", "3barBP0":"0.0", \ - "3barBP1":"3.0", "1barMax":"2.1", "2barMax":"2.1", "3barMax":"2.1", \ - "1barHwy":"0.4", "2barHwy":"0.3", "3barHwy":"0.1", \ - "steerRatio":"-1", "steerRateCost":"-1", "slowOnCurves":"0", "Kf":"-1", \ - "sR_boost":"0", "sR_BP0":"0", "sR_BP1":"0", "sR_time":"1", \ - "ALCnudgeLess":"0", "ALCminSpeed":"20.1168", "ALCtimer":"2.0", "CruiseDelta":"8", "CruiseEnableMin":"40"} - - - self.write_config(self.config) - return self.config + read_config = json.load(f) + + if "tuneGernby" in read_config: + config["tuneGernby"] = read_config["tuneGernby"] + config["Kp"] = read_config["Kp"] + config["Ki"] = read_config["Ki"] + + if "Kf" in read_config: + config["Kf"] = read_config["Kf"] + + if "steerRatio" in read_config: + config["steerRatio"] = read_config["steerRatio"] + + if "steerRateCost" in read_config: + config["steerRateCost"] = read_config["steerRateCost"] + + if "steerActuatorDelay" in read_config: + config["steerActuatorDelay"] = read_config["steerActuatorDelay"] + + if "deadzone" in read_config: + config["deadzone"] = read_config["deadzone"] + + if "cameraOffset" in read_config: + config["cameraOffset"] = read_config["cameraOffset"] + + if "wheelTouchSeconds" in read_config: + config["wheelTouchSeconds"] = read_config["wheelTouchSeconds"] + + if "slowOnCurves" in read_config: + config["slowOnCurves"] = read_config["slowOnCurves"] + + if "battPercOff" in read_config: + config["battPercOff"] = read_config["battPercOff"] + config["carVoltageMinEonShutdown"] = read_config["carVoltageMinEonShutdown"] + config["brakeStoppingTarget"] = read_config["brakeStoppingTarget"] + + if "liveParams" in read_config: + config["liveParams"] = read_config["liveParams"] + + if "leadDistance" in read_config: + config["leadDistance"] = read_config["leadDistance"] + + if "1barBP0" in read_config: + config["1barBP0"] = read_config["1barBP0"] + config["1barBP1"] = read_config["1barBP1"] + config["2barBP0"] = read_config["2barBP0"] + config["2barBP1"] = read_config["2barBP1"] + config["3barBP0"] = read_config["3barBP0"] + config["3barBP1"] = read_config["3barBP1"] + + if "1barMax" in read_config: + config["1barMax"] = read_config["1barMax"] + config["2barMax"] = read_config["2barMax"] + config["3barMax"] = read_config["3barMax"] + + if "1barHwy" in read_config: + config["1barHwy"] = read_config["1barHwy"] + config["2barHwy"] = read_config["2barHwy"] + config["3barHwy"] = read_config["3barHwy"] + + if "sR_boost" in read_config: + config["sR_boost"] = read_config["sR_boost"] + config["sR_BP0"] = read_config["sR_BP0"] + config["sR_BP1"] = read_config["sR_BP1"] + config["sR_time"] = read_config["sR_time"] + + if "ALCnudgeLess" in read_config: + config["ALCnudgeLess"] = read_config["ALCnudgeLess"] + config["ALCminSpeed"] = read_config["ALCnudgeLess"] + + if "ALCtimer" in read_config: + config["ALCtimer"] = read_config["ALCtimer"] + + if "CruiseDelta" in read_config: + config["CruiseDelta"] = read_config["CruiseDelta"] + + if "CruiseEnableMin" in read_config: + config["CruiseEnableMin"] = read_config["CruiseEnableMin"] + + self.write_config(config) + return config def write_config(self, config): - try: - with open('/data/kegman.json', 'w') as f: - json.dump(self.config, f, indent=2, sort_keys=True) - os.chmod("/data/kegman.json", 0o764) - except IOError: + if not os.path.exists('/data'): os.mkdir('/data') - with open('/data/kegman.json', 'w') as f: - json.dump(self.config, f, indent=2, sort_keys=True) - os.chmod("/data/kegman.json", 0o764) + + with open('/data/kegman.json', 'w') as f: + json.dump(config, f, indent=2, sort_keys=True) + os.chmod("/data/kegman.json", 0o764) + + print("updated kegman.json") diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index f03e63fe0ce525..bad702b690e39a 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -15,9 +15,9 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.loggerd.config import get_available_percent -from selfdrive.kegman_conf import kegman_conf +from selfdrive.kegman_conf import KegmanConf -kegman = kegman_conf() +kegman = KegmanConf() from selfdrive.pandad import get_expected_version FW_VERSION = get_expected_version() diff --git a/tune.py b/tune.py index 131f5507399c0b..c57e5b7f351ad2 100644 --- a/tune.py +++ b/tune.py @@ -1,134 +1,161 @@ -from selfdrive.kegman_conf import kegman_conf -import subprocess +import sys +import termios +import tty import os +import time +from selfdrive.kegman_conf import KegmanConf + BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) +button_delay = 0.2 -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 +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": ["###", "# #", "# #", "# #", "###"], + "!": [" # ", " # ", " # ", " ", " # "], "?": ["###", " #", " ##", " ", " # "], + ".": [" ", " ", " ", " ", " # "], "]": [" ", " ", " ", " #", " # "], + "/": [" #", " #", " # ", "# ", "# "], ":": [" ", " # ", " ", " # ", " "], + "@": ["###", "# #", "## ", "# ", "###"], "'": [" # ", " # ", " ", " ", " "], + "#": [" # ", "###", " # ", "###", " # "], "-": [" ", " ", "###", " ", " "]} -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) +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) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch -button_delay = 0.2 +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 + +params = ["Kp", "Ki", "Kf", "steerRatio", "steerRateCost", "steerActuatorDelay", + "deadzone", "sR_boost", "sR_BP0", "sR_BP1", "sR_time", "slowOnCurves", + "1barBP0", "1barBP1", "1barMax", "2barBP0", "2barBP1", + "2barMax", "3barBP0", "3barBP1", "3barMax", + "1barHwy", "2barHwy", "3barHwy"] -kegman = kegman_conf() +kegman = KegmanConf() kegman.conf['tuneGernby'] = "1" -#kegman.write_config(kegman.conf) -param = ["Kp", "Ki", "Kf", "steerRatio", "sR_boost", "sR_BP0", \ - "sR_BP1", "sR_time", "steerRateCost", "deadzone", "slowOnCurves", \ - "1barBP0", "1barBP1", "1barMax", "2barBP0", "2barBP1", \ - "2barMax", "3barBP0", "3barBP1", "3barMax", \ - "1barHwy", "2barHwy", "3barHwy"] +# kegman.write_config(kegman.conf) j = 0 while True: - print ("") - print (print_letters(param[j][0:9])) - print ("") - print (print_letters(kegman.conf[param[j]])) - print ("") - print ("1,3,5,7,r to incr 0.1,0.05,0.01,0.001,0.00001") - print ("a,d,g,j,v to decr 0.1,0.05,0.01,0.001,0.00001") - print ("0 / L to make the value 0 / 1") - print ("press SPACE / m for next /prev parameter") - print ("press z to quit") - - char = getch() + print("") + print(print_letters(params[j][0:9])) + print("") + print(print_letters(kegman.conf[params[j]])) + print("") + print("1,3,5,7,r to incr 0.1,0.05,0.01,0.001,0.00001") + print("a,d,g,j,v to decr 0.1,0.05,0.01,0.001,0.00001") + print("0 / L to make the value 0 / 1") + print("press SPACE / m for next /prev parameter") + print("press z to quit") + + char = getch() write_json = False - if (char == "v"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) - 0.00001),5)) + + if (char == "z"): + sys.exit(0) + + elif (char == " "): + j += 1 + if j == len(params): + j = 0 + + elif (char == "m"): + j -= 1 + if j < 0: + j = len(params) - 1 + + elif (char == "v"): + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) - 0.00001), 5)) write_json = True - if (char == "r"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) + 0.00001),5)) + elif (char == "r"): + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) + 0.00001), 5)) write_json = True - - if (char == "7"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) + 0.001),5)) + + elif (char == "7"): + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) + 0.001), 5)) write_json = True - if (char == "5"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) + 0.01),5)) + elif (char == "5"): + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) + 0.01), 5)) write_json = True elif (char == "3"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) + 0.05),5)) + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) + 0.05), 5)) write_json = True elif (char == "1"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) + 0.1),5)) + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) + 0.1), 5)) write_json = True elif (char == "j"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) - 0.001),5)) + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) - 0.001), 5)) write_json = True elif (char == "g"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) - 0.01),5)) + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) - 0.01), 5)) write_json = True elif (char == "d"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) - 0.05),5)) + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) - 0.05), 5)) write_json = True elif (char == "a"): - kegman.conf[param[j]] = str(round((float(kegman.conf[param[j]]) - 0.1),5)) + kegman.conf[params[j]] = str(round((float(kegman.conf[params[j]]) - 0.1), 5)) write_json = True elif (char == "0"): - kegman.conf[param[j]] = "0" + kegman.conf[params[j]] = "0" write_json = True elif (char == "l"): - kegman.conf[param[j]] = "1" + kegman.conf[params[j]] = "1" write_json = True - 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 == "z"): - process.kill() - break - + if float(kegman.conf['Kp']) < 0 and float(kegman.conf['Kp']) != -1: + kegman.conf['Kp'] = "0" - if float(kegman.conf['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: - kegman.conf['tuneGernby'] = "1" + if float(kegman.conf['Kp']) > 3: + kegman.conf['Kp'] = "3" if float(kegman.conf['Ki']) < 0 and float(kegman.conf['Ki']) != -1: kegman.conf['Ki'] = "0" @@ -136,128 +163,124 @@ def getch(): if float(kegman.conf['Ki']) > 2: kegman.conf['Ki'] = "2" - if float(kegman.conf['Kp']) < 0 and float(kegman.conf['Kp']) != -1: - kegman.conf['Kp'] = "0" + if float(kegman.conf['Kf']) < 0 and float(kegman.conf['Kp']) != -1: + kegman.conf['Kf'] = "0" + + if float(kegman.conf['Kf']) > 0.01: + kegman.conf['Kf'] = "0.01" + + # if float(kegman.conf['Kf']) < 0.00001: + kegman.conf['Kf'] = str("{:.5f}".format(float(kegman.conf['Kf']))) - if float(kegman.conf['Kp']) > 3: - kegman.conf['Kp'] = "3" - - if kegman.conf['liveParams'] != "1" and kegman.conf['liveParams'] != "0": - kegman.conf['liveParams'] = "1" - if float(kegman.conf['steerRatio']) < 1 and float(kegman.conf['steerRatio']) != -1: kegman.conf['steerRatio'] = "1" - + if float(kegman.conf['steerRateCost']) < 0.01 and float(kegman.conf['steerRateCost']) != -1: kegman.conf['steerRateCost'] = "0.01" - + + if float(kegman.conf['steerActuatorDelay']) < 0.1 and float(kegman.conf['steerActuatorDelay']) != -1: + kegman.conf['steerRateCost'] = "0.1" + if float(kegman.conf['deadzone']) < 0: kegman.conf['deadzone'] = "0" - + + if float(kegman.conf['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: + kegman.conf['tuneGernby'] = "1" + + if kegman.conf['liveParams'] != "1" and kegman.conf['liveParams'] != "0": + kegman.conf['liveParams'] = "1" + if float(kegman.conf['1barBP0']) < -0.5: - kegman.conf['1barBP0'] = "-0.5" - + kegman.conf['1barBP0'] = "-0.5" + if float(kegman.conf['1barBP0']) > 0.5: - kegman.conf['1barBP0'] = "0.5" - + kegman.conf['1barBP0'] = "0.5" + if float(kegman.conf['1barBP1']) < 0.5: kegman.conf['1barBP1'] = "0.5" - + if float(kegman.conf['1barBP1']) > 8: kegman.conf['1barBP1'] = "8" - + if float(kegman.conf['1barMax']) < 0.9: - kegman.conf['1barMax'] = "0.9" - + kegman.conf['1barMax'] = "0.9" + if float(kegman.conf['1barMax']) > 2.5: kegman.conf['1barMax'] = "2.5" - + if float(kegman.conf['2barBP0']) < -0.5: - kegman.conf['2barBP0'] = "-0.5" - + kegman.conf['2barBP0'] = "-0.5" + if float(kegman.conf['2barBP0']) > 0.5: - kegman.conf['2barBP0'] = "0.5" - + kegman.conf['2barBP0'] = "0.5" + if float(kegman.conf['2barBP1']) < 0.5: kegman.conf['2barBP1'] = "0.5" - + if float(kegman.conf['2barBP1']) > 8: kegman.conf['2barBP1'] = "8" - + if float(kegman.conf['2barMax']) < 1.3: - kegman.conf['2barMax'] = "1.3" - + kegman.conf['2barMax'] = "1.3" + if float(kegman.conf['2barMax']) > 2.5: kegman.conf['2barMax'] = "2.5" - + if float(kegman.conf['3barBP0']) < -0.5: - kegman.conf['3barBP0'] = "-0.5" - + kegman.conf['3barBP0'] = "-0.5" + if float(kegman.conf['3barBP0']) > 0.5: - kegman.conf['3barBP0'] = "0.5" - + kegman.conf['3barBP0'] = "0.5" + if float(kegman.conf['3barBP1']) < 0.5: kegman.conf['3barBP1'] = "0.5" - + if float(kegman.conf['3barBP1']) > 8: kegman.conf['3barBP1'] = "8" - + if float(kegman.conf['3barMax']) < 1.8: - kegman.conf['3barMax'] = "1.8" - + kegman.conf['3barMax'] = "1.8" + if float(kegman.conf['3barMax']) > 2.5: - kegman.conf['3barMax'] = "2.5" - + kegman.conf['3barMax'] = "2.5" + if float(kegman.conf['1barHwy']) < 0: kegman.conf['1barHwy'] = "0" - + if float(kegman.conf['2barHwy']) < 0: kegman.conf['2barHwy'] = "0" - + if float(kegman.conf['3barHwy']) < 0: - kegman.conf['3barHwy'] = "0" - + kegman.conf['3barHwy'] = "0" + if float(kegman.conf['1barHwy']) > 2: kegman.conf['1barHwy'] = "2" - + if float(kegman.conf['2barHwy']) > 2: kegman.conf['2barHwy'] = "2" - + if float(kegman.conf['3barHwy']) > 2: - kegman.conf['3barHwy'] = "2" - - if float(kegman.conf['Kf']) > 0.01: - kegman.conf['Kf'] = "0.01" - - if float(kegman.conf['Kf']) < 0: - kegman.conf['Kf'] = "0" - + kegman.conf['3barHwy'] = "2" + if float(kegman.conf['sR_boost']) < 0: kegman.conf['sR_boost'] = "0" - + if float(kegman.conf['sR_BP0']) < 0: kegman.conf['sR_BP0'] = "0" - + if float(kegman.conf['sR_BP1']) < 0: kegman.conf['sR_BP1'] = "0" - + if float(kegman.conf['sR_time']) < 1: kegman.conf['sR_time'] = "1" - #if float(kegman.conf['Kf']) < 0.00001: - kegman.conf['Kf'] = str("{:.5f}".format(float(kegman.conf['Kf']))) - if float(kegman.conf['slowOnCurves']) > 0.00001: kegman.conf['slowOnCurves'] = "1" - + if float(kegman.conf['slowOnCurves']) <= 0.99999: - kegman.conf['slowOnCurves'] = "0" - + kegman.conf['slowOnCurves'] = "0" if write_json: kegman.write_config(kegman.conf) time.sleep(button_delay) - -else: - process.kill()