diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 45f4fe2cf09f32..5787dbe6cceedb 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -144,8 +144,8 @@ def get_cam_can_parser(CP): class CarState(object): def __init__(self, CP): - self.k = kegman_conf() - self.trMode = int(self.k.conf['lastTrMode']) # default to last distance interval on startup + self.kegman = kegman_conf() + self.trMode = int(self.kegman.conf['lastTrMode']) # default to last distance interval on startup self.lkMode = True self.read_distance_lines_prev = 4 self.CP = CP @@ -317,8 +317,9 @@ 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.k.conf['lastTrMode'] = str(self.trMode) # write last distance bar setting to file - self.k.write_config(self.k.conf) + self.kegman = kegman_conf() + self.kegman.conf['lastTrMode'] = str(self.trMode) # write last distance bar setting to file + self.kegman.write_config(self.kegman.conf) # when user presses LKAS button on steering wheel if self.cruise_setting == 1: diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 57d5af8db4f4ea..d5f0c9cfc4a321 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -2,13 +2,13 @@ from common.realtime import sec_since_boot from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from common.filter_simple import FirstOrderFilter -from kegman_conf import kegman_conf +from selfdrive.kegman_conf import kegman_conf -k = kegman_conf() +kegman = kegman_conf() _DT = 0.01 # update runs at 100Hz _DTM = 0.1 # DM runs at 10Hz -_AWARENESS_TIME = min(int(k.conf['wheelTouchSeconds']), 600) # x minutes limit without user touching steering wheels make the car enter a terminal status +_AWARENESS_TIME = min(int(kegman.conf['wheelTouchSeconds']), 600) # x minutes limit without user touching steering wheels make the car enter a terminal status _AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration _AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car _DISTRACTED_TIME = 7. diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 45fcb29ac2a0d6..d18e36b5192721 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -24,6 +24,7 @@ def get_steer_max(CP, v_ego): class LatControl(object): def __init__(self, CP): + kegman_conf(CP) self.pid = PIController((CP.steerKpBP, CP.steerKpV), (CP.steerKiBP, CP.steerKiV), k_f=CP.steerKf, pos_limit=1.0) @@ -50,8 +51,24 @@ def setup_mpc(self, steer_rate_cost): 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.steerKpV = [float(kegman.conf['Kp'])] + self.steerKiV = [float(kegman.conf['Ki'])] + self.pid = PIController((CP.steerKpBP, self.steerKpV), + (CP.steerKiBP, self.steerKiV), + k_f=CP.steerKf, pos_limit=1.0) + self.mpc_frame = 0 def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL): + + self.live_tune(CP) + cur_time = sec_since_boot() self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 692f8574e5595c..1932b2588789e7 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -2,8 +2,8 @@ from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv from selfdrive.kegman_conf import kegman_conf -k = kegman_conf() -CAMERA_OFFSET = float(k.conf['cameraOffset']) # m from center car to camera +kegman = kegman_conf() +CAMERA_OFFSET = float(kegman.conf['cameraOffset']) # m from center car to camera class PathPlanner(object): def __init__(self): diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py index 6e3a43c8cd5191..f7ea586770d9db 100644 --- a/selfdrive/kegman_conf.py +++ b/selfdrive/kegman_conf.py @@ -2,20 +2,76 @@ import os class kegman_conf(): - def __init__(self): + 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 + if self.conf['Kp'] == "-1": + self.conf['Kp'] = str(round(CP.lateralTuning.pid.kpV[0],3)) + write_conf = True + if self.conf['Ki'] == "-1": + self.conf['Ki'] = str(round(CP.lateralTuning.pid.kiV[0],3)) + write_conf = True + + if write_conf: + self.write_config(self.config) def read_config(self): + self.element_updated = False + 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 + + + # Force update battery charge limits to higher values for Big Model + #if self.config['battChargeMin'] != "75": + # self.config.update({"battChargeMin":"75"}) + # self.config.update({"battChargeMax":"80"}) + # 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" } + 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"} + self.write_config(self.config) return self.config def write_config(self, config): - with open('/data/kegman.json', 'w') as f: - json.dump(self.config, f, indent=2, sort_keys=True) - os.chmod("/data/kegman.json", 0o764) - + 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: + 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) diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 3bb6da9331b15a..9fb1ee8091faa2 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -13,9 +13,12 @@ from common.numpy_fast import clip from common.filter_simple import FirstOrderFilter from selfdrive.kegman_conf import kegman_conf +import subprocess +import signal +kegman = kegman_conf() ThermalStatus = log.ThermalData.ThermalStatus -CURRENT_TAU = 2. # 2s time constant +CURRENT_TAU = 15. # 15s time constant def read_tz(x): with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: @@ -47,7 +50,7 @@ def setup_eon_fan(): bus.write_byte_data(0x21, 0x02, 0x2) # needed? bus.write_byte_data(0x21, 0x04, 0x4) # manual override source except IOError: - print "LEON detected" + print("LEON detected") #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") LEON = True bus.close() @@ -113,16 +116,15 @@ def check_car_battery_voltage(should_start, health, charging_disabled, msg): # - 12V battery voltage is too low, and; # - onroad isn't started # - keep battery within 67-70% State of Charge to preserve longevity - k = kegman_conf() - print k + print health # print car battery voltage status - if charging_disabled and (health is None or health.health.voltage > 11800) and msg.thermal.batteryPercent < int(k.conf['battChargeMin']): + if charging_disabled and (health is None or health.health.voltage > (int(kegman.conf['carVoltageMinEonShutdown'])+500)) and msg.thermal.batteryPercent < int(kegman.conf['battChargeMin']): charging_disabled = False os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') - elif not charging_disabled and (msg.thermal.batteryPercent > int(k.conf['battChargeMax']) or (health is not None and health.health.voltage < 11500 and not should_start)): + elif not charging_disabled and (msg.thermal.batteryPercent > int(kegman.conf['battChargeMax']) or (health is not None and health.health.voltage < int(kegman.conf['carVoltageMinEonShutdown']) and not should_start)): charging_disabled = True os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') - elif msg.thermal.batteryCurrent < 0 and msg.thermal.batteryPercent > int(k.conf['battChargeMax']): + elif msg.thermal.batteryCurrent < 0 and msg.thermal.batteryPercent > int(kegman.conf['battChargeMax']): charging_disabled = True os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') @@ -164,7 +166,7 @@ def thermald_thread(): setup_eon_fan() # prevent LEECO from undervoltage - BATT_PERC_OFF = 10 if LEON else 3 + BATT_PERC_OFF = int(kegman.conf['battPercOff']) # now loop context = zmq.Context() @@ -177,18 +179,19 @@ def thermald_thread(): off_ts = None started_ts = None ignition_seen = False - started_seen = False + #started_seen = False passive_starter = LocationStarter() thermal_status = ThermalStatus.green health_sock.RCVTIMEO = 1500 current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.) + services_killed = False # Make sure charging is enabled charging_disabled = False os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') params = Params() - + while 1: health = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) @@ -211,7 +214,7 @@ def thermald_thread(): msg.thermal.batteryVoltage = int(f.read()) with open("/sys/class/power_supply/usb/present") as f: msg.thermal.usbOnline = bool(int(f.read())) - + current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check @@ -286,19 +289,37 @@ def thermald_thread(): if started_ts is None: params.car_start() started_ts = sec_since_boot() - started_seen = True + #started_seen = True + os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor') else: started_ts = None if off_ts is None: off_ts = sec_since_boot() + os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor') # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running - if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \ - started_seen and (sec_since_boot() - off_ts) > 60: - os.system('LD_LIBRARY_PATH="" svc power shutdown') + if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryCurrent > 0 and \ + sec_since_boot() > 180: + #started_seen and (sec_since_boot() - off_ts) > 60: + if msg.thermal.usbOnline: + # if there is power through the USB then shutting down just results in an immediate restart so kill services instead (E.g. Nidec) + kill_list = ["updated", "gpsd", "logcatd", "pandad", "ui", "uploader", "tombstoned", "logmessaged", "athena", "ai.comma", "boardd"] + # Kill processes to save battery cannot shutdown if plugged in because it will just restart after shutdown + for process_name in kill_list: + proc = subprocess.Popen(["pgrep", process_name], stdout=subprocess.PIPE) + for pid in proc.stdout: + os.kill(int(pid), signal.SIGTERM) + else: + # if not just shut it down completely (E.g. Bosch or disconnected) + os.system('LD_LIBRARY_PATH="" svc power shutdown') + + services_killed = True - charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled, msg) + if services_killed: + charging_disabled = True + else: + charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled, msg) # need to force batteryStatus because after NEOS update for 0.5.7 this doesn't work properly if msg.thermal.batteryCurrent > 0: @@ -307,13 +328,13 @@ def thermald_thread(): msg.thermal.batteryStatus = "Charging" msg.thermal.chargingDisabled = charging_disabled - msg.thermal.chargingError = current_filter.x > 1.0 # if current is > 1A out, then charger might be off + msg.thermal.chargingError = current_filter.x > 0. # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9*(started_ts or 0)) msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) - print msg + print(msg) # report to server once per minute if (count%60) == 0: diff --git a/tune.py b/tune.py new file mode 100644 index 00000000000000..ed1417628bba43 --- /dev/null +++ b/tune.py @@ -0,0 +1,143 @@ +from selfdrive.kegman_conf import kegman_conf +import subprocess +import os +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) + +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", "liveParams", "Kp", "Ki"] + +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 to incr 0.1, 0.05, 0.01, 0.001") + print ("a, d, g, j to decr 0.1, 0.05, 0.01, 0.001") + 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 == "7"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.001) + write_json = True + + if (char == "5"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01) + write_json = True + + elif (char == "3"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05) + write_json = True + + elif (char == "1"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1) + write_json = True + + elif (char == "j"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.001) + write_json = True + + elif (char == "g"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01) + write_json = True + + elif (char == "d"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05) + write_json = True + + elif (char == "a"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1) + write_json = True + + elif (char == "0"): + kegman.conf[param[j]] = "0" + write_json = True + + elif (char == "l"): + kegman.conf[param[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['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: + kegman.conf['tuneGernby'] = "1" + + if float(kegman.conf['Ki']) < 0 and float(kegman.conf['Ki']) != -1: + kegman.conf['Ki'] = "0" + + 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['Kp']) > 3: + kegman.conf['Kp'] = "3" + + if kegman.conf['liveParams'] != "1" and kegman.conf['liveParams'] != "0": + kegman.conf['liveParams'] = "1" + + + if write_json: + kegman.write_config(kegman.conf) + + time.sleep(button_delay) + +else: + process.kill() 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