Merge pull request #57 from arne182/077-clean

keep 077 INDI up to date

selfdrive/car/chrysler/interface.py

@@ -30,6 +30,7 @@ def get_params(candidate, fingerprint=None, has_relay=False, car_fw=None):
     ret.mass = 2858. + STD_CARGO_KG  # kg curb weight Pacifica Hybrid 2017
     ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kfBP = [[9., 20.], [9., 20.], [0.]]
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.15,0.30], [0.03,0.05], [0.00006]] # full torque for 10 deg at 80mph means 0.00007818594
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.7
     ret.steerLimitTimer = 0.4

selfdrive/car/ford/interface.py

@@ -25,6 +25,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
     ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV, ret.lateralTuning.pid.kfV = [[0.01], [0.005], [1. / MAX_ANGLE]]     # TODO: tune this # MAX Steer angle to normalize FF
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
     ret.steerLimitTimer = 0.8
     ret.steerRateCost = 1.0

selfdrive/car/gm/interface.py

@@ -37,6 +37,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.minSteerSpeed = 7 * CV.MPH_TO_MS
     ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
     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

selfdrive/car/honda/interface.py

@@ -164,6 +164,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     # 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, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
     ret.lateralTuning.pid.kfV = [0.00006]  # conservative feed-forward
 
     eps_modified = False

selfdrive/car/hyundai/interface.py

@@ -35,6 +35,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.lateralTuning.pid.kiV = [0.001, 0.003, 0.005]
     ret.lateralTuning.pid.kfBP = [0., 10., 30.]
     ret.lateralTuning.pid.kfV = [0.00002, 0.00003, 0.00005]
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
 
     if candidate == CAR.SANTA_FE:
       ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG

selfdrive/car/mazda/interface.py

@@ -32,6 +32,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.steerRateCost = 1.0
     ret.steerLimitTimer = 0.8
     tire_stiffness_factor = 0.70   # not optimized yet
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
 
     if candidate in [CAR.CX5]:
       ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG

selfdrive/car/nissan/interface.py

@@ -33,6 +33,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.lateralTuning.pid.kf = 0.00006
     ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.0], [0.0]]
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]]
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
     ret.steerMaxBP = [0.]  # m/s
     ret.steerMaxV = [1.]
 

selfdrive/car/subaru/interface.py

@@ -28,6 +28,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
 
     ret.steerRateCost = 0.7
     ret.steerLimitTimer = 0.4
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
 
     if candidate == CAR.ASCENT:
       ret.mass = 2031. + STD_CARGO_KG

selfdrive/car/toyota/carstate.py

@@ -202,7 +202,6 @@ def update(self, cp, cp_cam, frame):
       else:
         self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
     ret.cruiseState.available = self.main_on
-    v_cruise_pcm_max = ret.cruiseState.speed
     if self.CP.carFingerprint in TSS2_CAR:
       minimum_set_speed = 27.0
     elif self.CP.carFingerprint == CAR.RAV4:
@@ -212,6 +211,11 @@ def update(self, cp, cp_cam, frame):
     maximum_set_speed = 169.0
     if self.CP.carFingerprint == CAR.LEXUS_RXH:
       maximum_set_speed = 177.0
+    v_cruise_pcm_max = ret.cruiseState.speed
+    if v_cruise_pcm_max < minimum_set_speed:
+      minimum_set_speed = v_cruise_pcm_max
+    if v_cruise_pcm_max > maximum_set_speed:
+      maximum_set_speed = v_cruise_pcm_max
     speed_range = maximum_set_speed-minimum_set_speed
     if bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) and not self.pcm_acc_active and self.v_cruise_pcmlast != ret.cruiseState.speed:
       if ret.vEgo < 12.5:

selfdrive/car/toyota/interface.py

@@ -9,6 +9,7 @@
 
 op_params = opParams()
 spairrowtuning = op_params.get('spairrowtuning')
+corolla_tss2_d_tuning = op_params.get('corolla_tss2_d_tuning')
 
 GearShifter = car.CarState.GearShifter
 
@@ -48,6 +49,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     if candidate not in [CAR.PRIUS_2019, CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.COROLLA]: # These cars use LQR/INDI
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
+      ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
 
     if candidate in [CAR.PRIUS, CAR.PRIUS_2019]:
       stop_and_go = True
@@ -258,18 +260,30 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
       ret.lateralTuning.pid.kfV = [0.00007818594]
       if spairrowtuning:
-        ret.steerActuatorDelay = 0.60
+        ret.steerActuatorDelay = 0.40
         ret.steerRatio = 15.33
         ret.steerLimitTimer = 5.0
         tire_stiffness_factor = 0.996  # not optimized yet
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
+        ret.lateralTuning.pid.kdBP = [0.]
+        ret.lateralTuning.pid.kdV = [9.0]
+        ret.lateralTuning.pid.kf = 0.00007818594
         #ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0.0, 15.5, 21.0, 29.0], [0.13, 0.39, 0.39, 0.6]]
         #ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0.0, 15.5, 21.0, 29.0], [0.005, 0.015, 0.015, 0.1]]
         #ret.lateralTuning.pid.kfBP, ret.lateralTuning.pid.kfV = [[0.0, 15.5, 21.0, 29.0], [0.00009, 0.00015, 0.00015, 0.00007818594]]
-        ret.lateralTuning.init('indi')
-        ret.lateralTuning.indi.innerLoopGain = 6
-        ret.lateralTuning.indi.outerLoopGain = 15.0
-        ret.lateralTuning.indi.timeConstant = 5.5
-        ret.lateralTuning.indi.actuatorEffectiveness = 6.0
+
+      if corolla_tss2_d_tuning:
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
+        ret.lateralTuning.pid.kdBP = [0.]
+        ret.lateralTuning.pid.kdV = [9.0]
+        ret.lateralTuning.pid.kf = 0.00007818594
+
+        #ret.lateralTuning.init('indi')
+        #ret.lateralTuning.indi.innerLoopGain = 6
+        #ret.lateralTuning.indi.outerLoopGain = 15.0
+        #ret.lateralTuning.indi.timeConstant = 5.5
+        #ret.lateralTuning.indi.actuatorEffectiveness = 6.0
+
 
     elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:
       stop_and_go = True

selfdrive/car/volkswagen/interface.py

@@ -24,6 +24,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
 
     # VW port is a community feature, since we don't own one to test
     ret.communityFeature = True
+    ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[0.], [0.]]
 
     if candidate == CAR.GOLF:
       # Set common MQB parameters that will apply globally

selfdrive/controls/lib/dynamic_follow/__init__.py

@@ -97,7 +97,7 @@ def _send_cur_state(self):
 
   def _change_cost(self, libmpc):
     TRs = [0.9, 1.8, 2.7]
-    costs = [1.10, 0.21, 0.05]
+    costs = [1.10, 0.12, 0.05]
     cost = interp(self.TR, TRs, costs)
     if self.last_cost != cost:
       libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)

selfdrive/controls/lib/latcontrol_pid.py

@@ -7,8 +7,9 @@
 class LatControlPID():
   def __init__(self, CP):
     self.pid = PIControllerk_f((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
-                            (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
-                            (CP.lateralTuning.pid.kfBP, CP.lateralTuning.pid.kfV), pos_limit=1.0, sat_limit=CP.steerLimitTimer)
+                               (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                               (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
+                               (CP.lateralTuning.pid.kfBP, CP.lateralTuning.pid.kfV), pos_limit=1.0, sat_limit=CP.steerLimitTimer)
     self.angle_steers_des = 0.
 
   def reset(self):

selfdrive/controls/lib/pid.py

@@ -193,9 +193,10 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
     return self.control
 
 class PIControllerk_f:
-  def __init__(self, k_p, k_i, k_f, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+  def __init__(self, k_p, k_i, k_d, k_f, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
     self._k_p = k_p # proportional gain
     self._k_i = k_i # integral gain
+    self._k_d = k_d # integral gain
     self._k_f = k_f  # feedforward gain
 
     self.pos_limit = pos_limit
@@ -216,7 +217,11 @@ def k_p(self):
   @property
   def k_i(self):
     return interp(self.speed, self._k_i[0], self._k_i[1])
-
+
+  @property
+  def k_d(self):
+    return interp(self.speed, self._k_d[0], self._k_d[1])
+
   @property
   def k_f(self):
     return interp(self.speed, self._k_f[0], self._k_f[1])
@@ -240,12 +245,14 @@ def reset(self):
     self.sat_count = 0.0
     self.saturated = False
     self.control = 0
+    self.last_error = 0
 
   def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
     self.speed = speed
 
     error = float(apply_deadzone(setpoint - measurement, deadzone))
     self.p = error * self.k_p
+    d = self.k_d * (error - self.last_error)
     self.f = feedforward * self.k_f
 
     if override:
@@ -264,11 +271,12 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
          not freeze_integrator:
         self.i = i
 
-    control = self.p + self.f + self.i
+    control = self.p + self.f + self.i + d
     if self.convert is not None:
       control = self.convert(control, speed=self.speed)
 
     self.saturated = self._check_saturation(control, check_saturation, error)
+    self.last_error = float(error)
 
     self.control = clip(control, self.neg_limit, self.pos_limit)
     return self.control

selfdrive/data_collection/gps_uploader.py

@@ -1,8 +1,9 @@
-import ftplib
+import os
 import json
+import gzip
+import ftplib
 import string
 import random
-import os
 import datetime
 from common.params import Params
 from common.op_params import opParams
@@ -32,17 +33,22 @@ def upload_data():
         username+="-{}".format(car[0])
 
       filename = "gps-data.{}".format(random.randint(1,99999))
-
+      fp = open(filepath,"rb")
+      data = fp.read()
+      bindata = bytearray(data)
+      with gzip.open("/data/" + filename + ".gz", "wb") as f:
+        f.write(bindata)
       ftp = ftplib.FTP("arneschwarck.dyndns.org")
       ftp.login("openpilot", "communitypilot")
-      with open(filepath, "rb") as f:
+      with open("/data/" + filename + ".gz", "rb") as f:
         try:
           ftp.mkd("/{}".format(username))
         except:
           pass
-        ftp.storbinary("STOR /{}/{}".format(username, filename), f)
+        ftp.storbinary("STOR /{}/{}".format(username, filename + ".gz"), f)
       ftp.quit()
       os.remove(filepath)
+      os.remove("/data/" + filename + ".gz")
       t = datetime.datetime.utcnow().isoformat()
       params.put("LastUpdateTime", t.encode('utf8'))
       return True

selfdrive/manager.py

@@ -419,7 +419,8 @@ def manager_init(should_register=True):
     if reg_res:
       dongle_id = reg_res
     else:
-      raise Exception("server registration failed")
+      cloudlog.info("server registration failed")
+      dongle_id = "0000000000000000"
   else:
     dongle_id = "c"*16
 

selfdrive/ui/dashcam.h

@@ -141,7 +141,7 @@ void start_capture() {
   struct tm tm = get_time_struct();
   snprintf(filename,sizeof(filename),"%04d%02d%02d-%02d%02d%02d.mp4", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec);
   //snprintf(cmd,sizeof(cmd),"screenrecord --size 1280x720 --bit-rate 10000000 %s/%s&",videos_dir,filename);
-  snprintf(cmd,sizeof(cmd),"screenrecord --size 960x540 --bit-rate 5000000 %s/%s&",videos_dir,filename);
+  snprintf(cmd,sizeof(cmd),"screenrecord --size 1920x1080 --bit-rate 5000000 %s/%s&",videos_dir,filename);
   strcpy(filenames[captureNum],filename);
 
   printf("Capturing to file: %s\n",cmd);

selfdrive/ui/uiview.py

@@ -3,52 +3,24 @@
 import time
 import signal
 import subprocess
-import multiprocessing
 import cereal.messaging as messaging
-
 from common.basedir import BASEDIR
 
-KILL_TIMEOUT = 15
-
-
-def send_controls_packet(pm):
-  while True:
-    dat = messaging.new_message('controlsState')
-    dat.controlsState.rearViewCam = False
-    pm.send('controlsState', dat)
-    time.sleep(1 / 100.)
+services = ['controlsState', 'thermal', 'radarState']  # the services needed to be spoofed to start ui offroad
+procs = {'camerad': 'selfdrive/camerad/camerad', 'ui': 'selfdrive/ui/ui',
+         'modeld': 'selfdrive/modeld/modeld', 'calibrationd': 'selfdrive/locationd/calibrationd.py'}
+started_procs = [subprocess.Popen(os.path.join(BASEDIR, procs[p]), cwd=os.path.join(BASEDIR, os.path.dirname(procs[p]))) for p in procs]  # start needed processes
+pm = messaging.PubMaster(services)
 
+dat_cs, dat_thermal, dat_radar = [messaging.new_message(s) for s in services]
+dat_cs.controlsState.rearViewCam = False  # ui checks for these two messages
+dat_thermal.thermal.started = True
 
-def send_thermal_packet(pm):
+try:
   while True:
-    dat = messaging.new_message('thermal')
-    dat.thermal.started = True
-    pm.send('thermal', dat)
-    time.sleep(1 / 2.)  # 2 hz
-
-
-def main():
-  pm = messaging.PubMaster(['controlsState', 'thermal'])
-  controls_sender = multiprocessing.Process(target=send_controls_packet, args=[pm])
-  controls_sender.start()
-  thermal_sender = multiprocessing.Process(target=send_thermal_packet, args=[pm])
-  thermal_sender.start()
-
-  # TODO: refactor with manager start/kill
-  proc_cam = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad"))
-  proc_ui = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/ui/ui"), cwd=os.path.join(BASEDIR, "selfdrive/ui"))
-
-  def terminate(signalNumber, frame):
-    print('got SIGTERM, exiting..')
-    proc_cam.send_signal(signal.SIGINT)
-    proc_ui.send_signal(signal.SIGINT)
-    thermal_sender.terminate()
-    controls_sender.terminate()
-    exit()
-
-  signal.signal(signal.SIGTERM, terminate)
-  signal.signal(signal.SIGINT, terminate)  # catch ctrl-c as well
-
-
-if __name__ == '__main__':
-  main()
+    pm.send('controlsState', dat_cs)
+    pm.send('thermal', dat_thermal)
+    pm.send('radarState', dat_radar)
+    time.sleep(1 / 100)  # continually send, rate doesn't matter
+except KeyboardInterrupt:
+  [p.send_signal(signal.SIGINT) for p in started_procs]