Bring kegman-0.5.12 features into kegman-0.5.8 (commaai#173)

* Upgrade kegman_conf, tune.py, tune.sh, thermald.py from 0.5.12

* Live tuner Kp and Ki

* Refresh kegman.json parameters before saving lastTR

* selfdrive.kegman_conf

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:

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.

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

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):

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)

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: