From 56e6de62e81f19d04e284a3a135b728cb5e57f23 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Tue, 16 Mar 2021 13:01:12 -0700
Subject: [PATCH 01/44] add spairrowtuning to rav4h and prius tss2

---
 selfdrive/car/toyota/interface.py | 68 ++++++++++++++-----------------
 1 file changed, 30 insertions(+), 38 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 2df1c6d010de14..7266f597f50a8e 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -91,26 +91,16 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.pid.kfV = [0.00007818594]
         ret.lateralTuning.pid.newKfTuned = False
       else:
-        ret.steerRateCost = 0.45 #0.45
-        ret.steerLimitTimer = 5.0
         ret.lateralTuning.init('indi')
-        #ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
-        #ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-        #ret.lateralTuning.indi.outerLoopGainBP = [8.3, 25, 27.7, 36.1]
-        #ret.lateralTuning.indi.outerLoopGainV = [4.6, 14.99, 14.99, 19]
-        #ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.1, 33.3, 36.1]
-        #ret.lateralTuning.indi.timeConstantV = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.0, 4.0, 4.0]
-        #ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
-        #ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
-        #ret.lateralTuning.init('indi') #really good tune from cgw.
-        ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25, 36.1]
-        ret.lateralTuning.indi.innerLoopGainV = [9.5, 15, 15]
-        ret.lateralTuning.indi.outerLoopGainBP = [16.7, 25, 36.1]
-        ret.lateralTuning.indi.outerLoopGainV = [9.5, 14.99, 14.99]
-        ret.lateralTuning.indi.timeConstantBP = [16.7, 16.71, 22, 22.01, 26, 26.01, 36, 36.01]
-        ret.lateralTuning.indi.timeConstantV = [0.5, 1, 1, 2, 2, 4, 4, 5]
-        ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25, 36.1]
-        ret.lateralTuning.indi.actuatorEffectivenessV = [9.5, 15, 15]
+        ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
+        ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
+        ret.steerActuatorDelay = 0.42
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -122,25 +112,27 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       if ret.enableGasInterceptor:
         ret.longitudinalTuning.kpV = [0.4, 0.36, 0.325]  # arne's tune.
         ret.longitudinalTuning.kiV = [0.195, 0.10]
-      #ret.lateralTuning.init('lqr')
-      #ret.lateralTuning.lqr.scale = 1500.0
-      #ret.lateralTuning.lqr.ki = 0.05
-      #ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
-      #ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
-      #ret.lateralTuning.lqr.c = [1., 0.]
-      #ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
-      #ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
-      #ret.lateralTuning.lqr.dcGain = 0.002237852961363602
-      ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
-      ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
-      ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
-      ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
-      ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
-      ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
-      ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
-      ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
-      ret.steerActuatorDelay = 0.42
+      if spairrowtuning:
+        ret.lateralTuning.init('indi')
+        ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
+        ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
+        ret.steerActuatorDelay = 0.4
+      else:
+        ret.lateralTuning.init('lqr')
+        ret.lateralTuning.lqr.scale = 1500.0
+        ret.lateralTuning.lqr.ki = 0.05
+        ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+        ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+        ret.lateralTuning.lqr.c = [1., 0.]
+        ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+        ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
+        ret.lateralTuning.lqr.dcGain = 0.002237852961363602
 
     elif candidate == CAR.COROLLA:
       stop_and_go = False

From 66593517b469a5c16da5d4aab91a0de7ce7143da Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Tue, 16 Mar 2021 13:03:56 -0700
Subject: [PATCH 02/44] not needed.

---
 selfdrive/car/toyota/interface.py | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 7266f597f50a8e..e808ac3a78a6d0 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -81,15 +81,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerActuatorDelay = 0.5
-      #ret.steerLimitTimer = 0.70
       if prius_pid:
         ret.lateralTuning.init('pid')
         ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.1]]
         ret.lateralTuning.pid.kdV = [2.0]
         ret.lateralTuning.pid.kfV = [0.00007818594]
-        ret.lateralTuning.pid.newKfTuned = False
       else:
         ret.lateralTuning.init('indi')
         ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]

From cfdc707a974a4ce6df3f7ab8ef7a9eba1cb379c9 Mon Sep 17 00:00:00 2001
From: cgw1968-5779 <58063525+cgw1968-5779@users.noreply.github.com>
Date: Wed, 17 Mar 2021 13:44:48 +0800
Subject: [PATCH 03/44] Rav4_TSS2 ICE tune. (#818)

---
 selfdrive/car/toyota/interface.py | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index e808ac3a78a6d0..0dee753e551e5f 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -265,16 +265,19 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
       ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
       ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
-      ret.stoppingBrakeRate = 0.165 # reach stopping target smoothly
+      ret.stoppingBrakeRate = 0.16 # reach stopping target smoothly
       ret.startingBrakeRate = 0.9 # release brakes fast
       ret.startAccel = 1.50 # Accelerate from 0 faster
       ret.steerActuatorDelay = 0
-      ret.steerLimitTimer = 0.4
+      ret.steerLimitTimer = 0.1
+      ret.startAccel = 1.50 # Accelerate from 0 faster
+      ret.steerActuatorDelay = 0
+      ret.steerLimitTimer = 0.1
       ret.lateralTuning.init('indi')
       ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
       ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2,  25, 30.6, 33.3, 36.1]
-      ret.lateralTuning.indi.outerLoopGainV = [4.65, 6.45, 8.25, 10.05, 11.85, 13.65, 14.99, 14.99, 14.99, 14.99]
+      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
+      ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
       ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
       ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
       ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]

From 3ecb344e3d5fe048c47e012c672c56978aa8f9d0 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Tue, 16 Mar 2021 22:51:06 -0700
Subject: [PATCH 04/44] cgw indi sparrow pid

---
 selfdrive/car/toyota/interface.py | 34 +++++++++++++++----------------
 1 file changed, 17 insertions(+), 17 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 0dee753e551e5f..962a727bfb5bc5 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -83,21 +83,24 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
       if prius_pid:
         ret.lateralTuning.init('pid')
-        ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
-        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.1]]
-        ret.lateralTuning.pid.kdV = [2.0]
-        ret.lateralTuning.pid.kfV = [0.00007818594]
+        ret.lateralTuning.pid.kpBP = [0.0]
+        ret.lateralTuning.pid.kiBP = [0.0]
+        ret.lateralTuning.pid.kpV = [0.028]
+        ret.lateralTuning.pid.kiV = [0.0012]
+        ret.lateralTuning.pid.kf = [0.000153263811757641] # hardcoded in latcontrol_pid, this does nothing for now
+        ret.lateralTuning.pid.newKfTuned = True
       else:
-        ret.lateralTuning.init('indi')
-        ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
-        ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
-        ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
-        ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
-        ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
-        ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
-        ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
-        ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
-        ret.steerActuatorDelay = 0.42
+       ret.steerActuatorDelay = 0
+       ret.steerLimitTimer = 0.1
+       ret.lateralTuning.init('indi')
+       ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
+       ret.lateralTuning.indi.innerLoopGainV = [15, 15]
+       ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
+       ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
+       ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
+       ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
+       ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
+       ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -270,9 +273,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.startAccel = 1.50 # Accelerate from 0 faster
       ret.steerActuatorDelay = 0
       ret.steerLimitTimer = 0.1
-      ret.startAccel = 1.50 # Accelerate from 0 faster
-      ret.steerActuatorDelay = 0
-      ret.steerLimitTimer = 0.1
       ret.lateralTuning.init('indi')
       ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
       ret.lateralTuning.indi.innerLoopGainV = [15, 15]

From 00a74d590b2797e6aa742b7a79573bf4e23c10d2 Mon Sep 17 00:00:00 2001
From: SCshredder17 <57152954+SCshredder17@users.noreply.github.com>
Date: Wed, 17 Mar 2021 00:55:27 -0700
Subject: [PATCH 05/44] Update interface.py

Updated corollatss2 ff tune
---
 selfdrive/car/toyota/interface.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 962a727bfb5bc5..eeff0dd6a62b8e 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -315,7 +315,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 15.33
       tire_stiffness_factor = 0.996  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerActuatorDelay = 0.48
+      ret.steerActuatorDelay = 0.52
       ret.steerLimitTimer = 5.0
       if spairrowtuning:
         ret.lateralTuning.init('indi')
@@ -331,9 +331,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       else:
         ret.lateralTuning.pid.kpBP = [0.0]
         ret.lateralTuning.pid.kiBP = [0.0]
-        ret.lateralTuning.pid.kpV = [0.028]
+        ret.lateralTuning.pid.kpV = [0.036]
         ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = 0.000153263811757641 # hardcoded in latcontrol_pid, this does nothing for now
+        ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
         ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:

From 06a88a4851fa58ed432b999d7d379090b59468b5 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 12:39:11 +0100
Subject: [PATCH 06/44] Let trafficd run all the time

---
 selfdrive/mapd/mapd.py | 40 ++++++++++++++++++++--------------------
 1 file changed, 20 insertions(+), 20 deletions(-)

diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
index 9956a56f085404..e150be77fc22dc 100755
--- a/selfdrive/mapd/mapd.py
+++ b/selfdrive/mapd/mapd.py
@@ -22,7 +22,7 @@
 from selfdrive.version import version, dirty
 from common.transformations.coordinates import geodetic2ecef
 from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points, rate_curvature_points
-from selfdrive.trafficd.traffic_manager import TrafficdThread
+#from selfdrive.trafficd.traffic_manager import TrafficdThread
 from common.op_params import opParams
 
 traffic_lights = opParams().get('traffic_lights')
@@ -72,8 +72,8 @@ def __init__(self, threadID, name, sharedParams={}): # sharedParams is dict of p
             'Accept-Encoding': 'gzip'
         }
         self.prev_ecef = None
-        self.trafficd_thread = TrafficdThread()
-        self.traffic_light_in_range = False
+        #self.trafficd_thread = TrafficdThread()
+        #self.traffic_light_in_range = False
         
     def is_connected_to_local(self, timeout=3.0):
         try:
@@ -136,11 +136,11 @@ def run(self):
             else:
                 start = time.time()
                 
-            if self.trafficd_thread.running and not self.traffic_light_in_range and traffic_lights:
-              self.trafficd_thread.stop()
-            if self.traffic_light_in_range and not self.trafficd_thread.running and traffic_lights:
-              subprocess.call(['pkill','-f','_trafficd'])
-              self.trafficd_thread.start()
+            #if self.trafficd_thread.running and not self.traffic_light_in_range and traffic_lights:
+            #  self.trafficd_thread.stop()
+            #if self.traffic_light_in_range and not self.trafficd_thread.running and traffic_lights:
+            #  subprocess.call(['pkill','-f','_trafficd'])
+            #  self.trafficd_thread.start()
                 
             self.logger.debug("Starting after sleeping for 1 second ...")
             last_gps = self.sharedParams.get('last_gps', None)
@@ -233,16 +233,16 @@ def run(self):
                         query_lock.release()
                     else:
                         self.logger.error("There is not query_lock")
-                    self.traffic_light_in_range = False
-                    for n in real_nodes:
-                        if 'highway' in n.tags:
-                            if n.tags['highway'] == 'traffic_signals':
-                                self.traffic_light_in_range = True
-                                break
-                        if 'railway' in n.tags:
-                            if n.tags['railway'] == 'level_crossing':
-                                self.traffic_light_in_range = True
-                                break
+                    #self.traffic_light_in_range = False
+                    #for n in real_nodes:
+                    #    if 'highway' in n.tags:
+                    #        if n.tags['highway'] == 'traffic_signals':
+                    #            self.traffic_light_in_range = True
+                    #            break
+                    #    if 'railway' in n.tags:
+                    #        if n.tags['railway'] == 'level_crossing':
+                    #            self.traffic_light_in_range = True
+                    #            break
 
                 except Exception as e:
                     self.logger.error("ERROR :" + str(e))
@@ -251,13 +251,13 @@ def run(self):
                     query_lock.acquire()
                     self.sharedParams['last_query_result'] = None
                     query_lock.release()
-                    self.traffic_light_in_range = False
+                    #self.traffic_light_in_range = False
             else:
                 query_lock = self.sharedParams.get('query_lock', None)
                 query_lock.acquire()
                 self.sharedParams['last_query_result'] = None
                 query_lock.release()
-                self.traffic_light_in_range = False
+                #self.traffic_light_in_range = False
 
             self.logger.debug("end of one cycle in endless loop ...")
 

From 083bfeb54db79b3ea0a4ee8f705ce413c1f2079d Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 12:40:37 +0100
Subject: [PATCH 07/44] start trafficd with car

---
 selfdrive/manager.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 5e5cb1aa1fb0f6..3a06ec1b1fa3f7 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -204,6 +204,7 @@ def unblock_stdout():
   "calibrationd": "selfdrive.locationd.calibrationd",
   "paramsd": "selfdrive.locationd.paramsd",
   "camerad": ("selfdrive/camerad", ["./camerad"]),
+  "trafficd": ("selfdrive/trafficd", ["./trafficd"]),
   "sensord": ("selfdrive/sensord", ["./sensord"]),
   "clocksd": ("selfdrive/clocksd", ["./clocksd"]),
   "gpsd": ("selfdrive/sensord", ["./gpsd"]),
@@ -286,6 +287,7 @@ def get_running():
 if traffic_lights:
   car_started_processes += [
     'traffic_manager',
+    'trafficd',
   ]
   
 if WEBCAM:

From e01658c67808cb00e791ea3335af03bec70da68f Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 12:43:23 +0100
Subject: [PATCH 08/44] Add DistanceTraveled variable

---
 common/params_pyx.pyx | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx
index 5c1c97184b852f..7193e3a09eb10b 100755
--- a/common/params_pyx.pyx
+++ b/common/params_pyx.pyx
@@ -27,6 +27,9 @@ keys = {
   b"CompletedTrainingVersion": [TxType.PERSISTENT],
   b"DisablePowerDown": [TxType.PERSISTENT],
   b"DisableUpdates": [TxType.PERSISTENT],
+  b"DistanceTraveled": [TxType.PERSISTENT],
+  b"DistanceTraveledEngaged": [TxType.PERSISTENT],
+  b"DistanceTraveledOverride": [TxType.PERSISTENT],
   b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
   b"DongleId": [TxType.PERSISTENT],
   b"GitBranch": [TxType.PERSISTENT],

From 6c5e2e2cea376234565d2d9cdcc720bbcdc03d39 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 12:44:49 +0100
Subject: [PATCH 09/44] Add DistanceTraveled

---
 selfdrive/manager.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 3a06ec1b1fa3f7..325c4d76d694c2 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -633,6 +633,9 @@ def main():
     ("OpenpilotEnabledToggle", "1"),
     ("LaneChangeEnabled", "1"),
     ("IsDriverViewEnabled", "0"),
+    ("DistanceTraveled", "0"),
+    ("DistanceTraveledEngaged", "0"),
+    ("DistanceTraveledOverride", "0"),
   ]
 
   # set unset params

From 17d8b6c5eb6246cbf024a1fc56494958e578e333 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 12:47:01 +0100
Subject: [PATCH 10/44] add  distance_traveled

---
 selfdrive/crash.py | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index f2beab6f90438d..c8038730a2386e 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -82,11 +82,23 @@ def install():
     dongle_id = params.get("DongleId").decode('utf8')
   except AttributeError:
     dongle_id = "None"
+  try:
+    distance_traveled = params.get("DistanceTraveled").decode('utf8')
+  except AttributeError:
+    distance_traveled = "None"
+  try:
+    distance_traveled_engaged = params.get("DistanceTraveledEngaged").decode('utf8')
+  except AttributeError:
+    distance_traveled_engaged = "None"
+  try:
+    distance_traveled_override = params.get("DistanceTraveledOverride").decode('utf8')
+  except AttributeError:
+    distance_traveled_override = "None"
   try:
     ip = requests.get('https://checkip.amazonaws.com/').text.strip()
   except Exception:
     ip = "255.255.255.255"
-  error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin}
+  error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin, 'distance_traveled': distance_traveled, 'distance_traveled_engaged': distance_traveled_engaged, 'distance_traveled_override': distance_traveled_override}
   #uniqueID = op_params.get('uniqueID')
   username = opParams().get('username')
   if username is None or not isinstance(username, str):

From 322e4b98da85fa3949e8847e13d53318e3711938 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 12:53:47 +0100
Subject: [PATCH 11/44] add DistanceTraveled

---
 selfdrive/controls/controlsd.py | 27 ++++++++++++++++++++++++---
 1 file changed, 24 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 9754f5f898c2e3..e3c114a7202502 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -21,7 +21,7 @@
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.planner import LON_MPC_STEP
 from selfdrive.locationd.calibrationd import Calibration
-#from common.travis_checker import travis
+from common.travis_checker import travis
 #import threading
 from selfdrive.interceptor import Interceptor
 
@@ -135,7 +135,18 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.can_error_counter = 0
     self.last_blinker_frame = 0
     self.saturated_count = 0
-    self.distance_traveled = 0
+    self.distance_traveled_now = 0
+    if not travis:
+      self.distance_traveled = float(params.get("DistanceTraveled", encoding='utf8'))
+      self.distance_traveled_engaged = float(params.get("DistanceTraveledEngaged", encoding='utf8'))
+      self.distance_traveled_override = float(params.get("DistanceTraveledOverride", encoding='utf8'))
+    else:
+      self.distance_traveled = 0
+      self.distance_traveled_engaged = 0
+      self.distance_traveled_override = 0
+
+    self.distance_traveled_frame = 0
+    
     self.last_functional_fan_frame = 0
     self.events_prev = []
     self.current_alert_types = [ET.PERMANENT]
@@ -247,7 +258,7 @@ def update_events(self, CS):
     if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
       if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
         self.events.add(EventName.sensorDataInvalid)
-    if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
+    if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled_now > 1000):
       # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
       if not (SIMULATION or NOSENSOR):  # TODO: send GPS in carla
         self.events.add(EventName.noGps)
@@ -340,7 +351,17 @@ def data_sample(self):
     if not self.sm['dragonConf'].dpAtl and not self.sm['health'].controlsAllowed and self.enabled:
       self.mismatch_counter += 1
 
+    self.distance_traveled_now += CS.vEgo * DT_CTRL
     self.distance_traveled += CS.vEgo * DT_CTRL
+    if self.enabled:
+      self.distance_traveled_engaged += CS.vEgo * DT_CTRL
+      if CS.steeringPressed:
+        self.distance_traveled_override += CS.vEgo * DT_CTRL
+    if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis:
+      put_nonblocking("DistanceTraveled", self.distance_traveled)
+      put_nonblocking("DistanceTraveledEngaged", self.distance_traveled_engaged)
+      put_nonblocking("DistanceTraveledOverride", self.distance_traveled_override)
+      self.distance_traveled_frame = self.sm.frame
 
     return CS
 

From efe6b48b6f793df99899c2687cf2411e51c6f9f6 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 13:08:28 +0100
Subject: [PATCH 12/44] flake8 requirement

---
 selfdrive/crash.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index c8038730a2386e..c21cb68877d375 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -98,7 +98,9 @@ def install():
     ip = requests.get('https://checkip.amazonaws.com/').text.strip()
   except Exception:
     ip = "255.255.255.255"
-  error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin, 'distance_traveled': distance_traveled, 'distance_traveled_engaged': distance_traveled_engaged, 'distance_traveled_override': distance_traveled_override}
+  error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin, 
+                'distance_traveled': distance_traveled, 'distance_traveled_engaged': distance_traveled_engaged, 
+                'distance_traveled_override': distance_traveled_override}
   #uniqueID = op_params.get('uniqueID')
   username = opParams().get('username')
   if username is None or not isinstance(username, str):

From 9b80b2df9cc88827426f703aa1fc2c0f3288acd9 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 16:43:44 +0100
Subject: [PATCH 13/44] Permit braking without lead

Only small amounts of braking is available if this is not set to 1
---
 selfdrive/car/toyota/toyotacan.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index eac7b10008846d..5156cfbce7191a 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -36,7 +36,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, distan
     "DISTANCE": distance,
     "MINI_CAR": lead,
     "SET_ME_X3": 3,
-    "PERMIT_BRAKING": lead,
+    "PERMIT_BRAKING": 1,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
   }

From c53386efa9526fbdffb27a3ca094aff272672c38 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 16:59:25 +0100
Subject: [PATCH 14/44] Require 15% deviation before alert

---
 selfdrive/controls/controlsd.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index e3c114a7202502..2861c7a7ff46b6 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -492,8 +492,8 @@ def state_control(self, CS):
       if (lac_log.saturated and not CS.steeringPressed) or \
               (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
         # Check if we deviated from the path
-        left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
-        right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
+        left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.15
+        right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.15
 
         if left_deviation or right_deviation:
           self.events.add(EventName.steerSaturated)

From c4c44d6c47335c06ad824d9fa8ad27d6b25546d6 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 17 Mar 2021 10:40:12 -0700
Subject: [PATCH 15/44] pidt

---
 selfdrive/car/toyota/interface.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index eeff0dd6a62b8e..7b5340306741fa 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -82,12 +82,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
       if prius_pid:
+        ret.steerLimitTimer = 5.0
+        ret.steerActuatorDelay = 0.52
         ret.lateralTuning.init('pid')
         ret.lateralTuning.pid.kpBP = [0.0]
         ret.lateralTuning.pid.kiBP = [0.0]
-        ret.lateralTuning.pid.kpV = [0.028]
+        ret.lateralTuning.pid.kpV = [0.036]
         ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = [0.000153263811757641] # hardcoded in latcontrol_pid, this does nothing for now
+        ret.lateralTuning.pid.kf = [0.000173263811757641] # hardcoded in latcontrol_pid, this does nothing for now
         ret.lateralTuning.pid.newKfTuned = True
       else:
        ret.steerActuatorDelay = 0

From b148307c98025e507655e5dcb02a4ff53670bc6d Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 17 Mar 2021 10:56:39 -0700
Subject: [PATCH 16/44] kegmans colored path

---
 selfdrive/controls/lib/pathplanner.py |  2 +-
 selfdrive/ui/paint.cc                 | 30 ++++++++++++++++++++++++++-
 selfdrive/ui/ui.cc                    |  5 +++++
 selfdrive/ui/ui.hpp                   |  2 ++
 4 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 9b942df68e6f19..69834f1d412342 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -14,7 +14,7 @@
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
 
-LOG_MPC = os.environ.get('LOG_MPC', False)
+LOG_MPC = os.environ.get('LOG_MPC', True)
 
 LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index 5a45f20521de33..0679aecf4033f6 100644
--- a/selfdrive/ui/paint.cc
+++ b/selfdrive/ui/paint.cc
@@ -181,8 +181,36 @@ static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::R
 }
 
 static void ui_draw_track(UIState *s, track_vertices_data *pvd) {
-  NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
+  if (pvd->cnt == 0) return;
+
+   nvgBeginPath(s->vg);
+   nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y);
+   for (int i=1; i<pvd->cnt; i++) {
+     nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
+   }
+   nvgClosePath(s->vg);
+
+   NVGpaint track_bg;
+   if (s->scene.controls_state.getEnabled()) {
+     // Draw colored MPC track Kegman's
+     if (s->scene.steerOverride) {
+       track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
+         nvgRGBA(0, 191, 255, 255), nvgRGBA(0, 95, 128, 50));
+     } else {
+       int torque_scale = (int)fabs(510*(float)s->scene.output_scale);
+       int red_lvl = fmin(255, torque_scale);
+       int green_lvl = fmin(255, 510-torque_scale);
+       track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
+         nvgRGBA(          red_lvl,            green_lvl,  0, 255),
+         nvgRGBA((int)(0.5*red_lvl), (int)(0.5*green_lvl), 0, 50));
+     }
+   } else {
+     // Draw white vision track
+     track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
                                         COLOR_WHITE, COLOR_WHITE_ALPHA(0));
+  }
+  nvgFillPaint(s->vg, track_bg);
+  nvgFill(s->vg);
   ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg);
 }
 
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index e6ce6b1683ba56..57fdd2b2a5396a 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -123,6 +123,11 @@ void update_sockets(UIState *s) {
     scene.controls_state = event.getControlsState();
 
     // TODO: the alert stuff shouldn't be handled here
+    s->scene.steerOverride= scene.controls_state.getSteerOverride();
+    s->scene.output_scale = scene.controls_state.getLateralControlState().getPidState().getOutput();
+    s->scene.output_scale = scene.controls_state.getLateralControlState().getLqrState().getOutput();
+    s->scene.output_scale = scene.controls_state.getLateralControlState().getIndiState().getOutput();
+
     auto alert_sound = scene.controls_state.getAlertSound();
     if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) {
       if (alert_sound == AudibleAlert::NONE) {
diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp
index 6fc5d0499b2d83..86ff8f07ec24f6 100644
--- a/selfdrive/ui/ui.hpp
+++ b/selfdrive/ui/ui.hpp
@@ -178,6 +178,8 @@ typedef struct UIScene {
   bool rightBlinker;
   bool brakeLights;
   int blinker_blinkingrate;
+  bool steerOverride;
+  float output_scale;
   // for blind spot
   bool leftBlindspot;
   bool rightBlindspot;

From b55283693bdb05b6575add58c79aed891962ecfe Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 19:11:17 +0100
Subject: [PATCH 17/44] Reduce timer based on acceleration profile

---
 selfdrive/controls/lib/pathplanner.py | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 69834f1d412342..c35729ee23fbca 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -19,6 +19,16 @@
 LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 
+# dp
+
+DP_OFF = 0
+
+DP_ECO = 1
+
+DP_NORMAL = 2
+
+DP_SPORT = 3
+
 DESIRES = {
   LaneChangeDirection.none: {
     LaneChangeState.off: log.PathPlan.Desire.none,
@@ -91,7 +101,7 @@ def update(self, sm, pm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
     active = sm['controlsState'].active
-
+    dp_profile = sm['dragonConf'].dpAccelProfile
     angle_offset = sm['liveParameters'].angleOffset
 
     # Run MPC
@@ -149,7 +159,7 @@ def update(self, sm, pm, CP, VM):
           # we only set timer when in preLaneChange state, dragon_auto_lc_delay delay
           if self.lane_change_state == LaneChangeState.preLaneChange:
             self.dragon_auto_lc_timer = cur_time + sm['dragonConf'].dpAutoLcDelay
-        elif cur_time >= self.dragon_auto_lc_timer:
+        elif cur_time >= (self.dragon_auto_lc_timer - dp_profile + 1):
           # if timer is up, we set torque_applied to True to fake user input
           torque_applied = True
           self.dp_did_auto_lc = True

From 46f10756dadff7a01f28dd0a00011df2a63ca12d Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Wed, 17 Mar 2021 21:14:01 +0100
Subject: [PATCH 18/44] RSA only on tss1

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8193c6076c96d6..88ddb8aa90f163 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -342,7 +342,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
         can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
         #print("debug Right blindspot poll")
         
-    if frame > 200:
+    if frame > 200 and CS.CP.carFingerprint not in TSS2_CAR:
       if frame % 100 == 0:
         if speed_signs_in_mph:
           smartspeed =round(CS.smartspeed*2.23694)

From 4a6240859b682ce13fedc0ba4f81fe25cb8e1326 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 17 Mar 2021 13:28:54 -0700
Subject: [PATCH 19/44] kf is no bp

---
 selfdrive/car/toyota/interface.py | 23 +++++++++++------------
 1 file changed, 11 insertions(+), 12 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 7b5340306741fa..b629563283b62b 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -81,28 +81,27 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.steerLimitTimer = 5.0
       if prius_pid:
-        ret.steerLimitTimer = 5.0
         ret.steerActuatorDelay = 0.52
         ret.lateralTuning.init('pid')
         ret.lateralTuning.pid.kpBP = [0.0]
         ret.lateralTuning.pid.kiBP = [0.0]
         ret.lateralTuning.pid.kpV = [0.036]
         ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = [0.000173263811757641] # hardcoded in latcontrol_pid, this does nothing for now
+        ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
         ret.lateralTuning.pid.newKfTuned = True
       else:
-       ret.steerActuatorDelay = 0
-       ret.steerLimitTimer = 0.1
        ret.lateralTuning.init('indi')
-       ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
-       ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-       ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
-       ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
-       ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
-       ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
-       ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
-       ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
+       ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
+       ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
+       ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
+       ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
+       ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
+       ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
+       ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
+       ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
+       ret.steerActuatorDelay = 0.42
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False

From 968dccafbac63d9323232a6468aad39be3393666 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 17 Mar 2021 13:29:32 -0700
Subject: [PATCH 20/44] only 5 min.

---
 selfdrive/updated.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index dbb1791906d659..23179069486e46 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -323,7 +323,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool:
 
 class AutoReboot:
   def __init__(self):
-    self.min_reboot_time = 5. * 60
+    self.min_reboot_time = 5.
     self.need_reboot = False
     self.time_offroad = 0.0
 

From 57bc082f3cc9e90bf0028fc54fea8e6b55a5aa87 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 18 Mar 2021 08:50:56 +0100
Subject: [PATCH 21/44] Round distance and send as string

---
 selfdrive/controls/controlsd.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 2861c7a7ff46b6..57974f046ee60b 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -358,9 +358,9 @@ def data_sample(self):
       if CS.steeringPressed:
         self.distance_traveled_override += CS.vEgo * DT_CTRL
     if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis:
-      put_nonblocking("DistanceTraveled", self.distance_traveled)
-      put_nonblocking("DistanceTraveledEngaged", self.distance_traveled_engaged)
-      put_nonblocking("DistanceTraveledOverride", self.distance_traveled_override)
+      put_nonblocking("DistanceTraveled", str(round(self.distance_traveled,2)))
+      put_nonblocking("DistanceTraveledEngaged", str(round(self.distance_traveled_engaged,2)))
+      put_nonblocking("DistanceTraveledOverride", str(round(self.distance_traveled_override,2)))
       self.distance_traveled_frame = self.sm.frame
 
     return CS

From 609d90952630efe6cf5669beb13fd9f649933eb4 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Thu, 18 Mar 2021 14:41:45 -0700
Subject: [PATCH 22/44] test out the new tune.

---
 selfdrive/car/toyota/interface.py | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index b629563283b62b..69d12b1fff0cfd 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -81,8 +81,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerLimitTimer = 5.0
       if prius_pid:
+        ret.steerLimitTimer = 5.0
         ret.steerActuatorDelay = 0.52
         ret.lateralTuning.init('pid')
         ret.lateralTuning.pid.kpBP = [0.0]
@@ -92,16 +92,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
         ret.lateralTuning.pid.newKfTuned = True
       else:
-       ret.lateralTuning.init('indi')
-       ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
-       ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
-       ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
-       ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
-       ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
-       ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
-       ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
-       ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
-       ret.steerActuatorDelay = 0.42
+        ret.steerActuatorDelay = 0
+        ret.steerLimitTimer = 0.1
+        ret.lateralTuning.init('indi')
+        ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
+        ret.lateralTuning.indi.innerLoopGainV = [15, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
+        ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
+        ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False

From 2a668f7e7cd0cc9097a6320555bb0b183a12ba43 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Fri, 19 Mar 2021 10:52:00 -0700
Subject: [PATCH 23/44] just stright up pid.

---
 common/op_params.py               |  2 +-
 selfdrive/car/toyota/interface.py | 30 +++++++-----------------------
 2 files changed, 8 insertions(+), 24 deletions(-)

diff --git a/common/op_params.py b/common/op_params.py
index 2168e1c6854de9..3d4b08498b134d 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -101,7 +101,7 @@ def __init__(self):
                         #'NoctuaMode': Param(False, bool, 'Noctua Fan are super quite and they run at full speed at all time.'),
                         'offset_limit': Param(0, VT.number, 'Speed at which apk percent offset will work in m/s'),
                         'osm': Param(True, bool, 'Whether to use OSM for drives'),
-                        'prius_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\nFalse: stock INDI, True: TSS2-tuned PID'),
+                        #'prius_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\nFalse: stock INDI, True: TSS2-tuned PID'),
                         'physical_buttons_AP': Param(False, bool, 'This enables the physical buttons to control sport and eco, some cars do not have buttons'),
                         'physical_buttons_DF': Param(False, bool, 'This enables the physical buttons to control following distance, TSS1 works with new SDSU FW'),
                         'physical_buttons_LKAS': Param(False, bool, 'This enables the physical buttons to control LKAS. TSS1 only this may break if used on TSS2 vechicle'),
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 69d12b1fff0cfd..04c78edc10e970 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -39,7 +39,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
 
-    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.PRIUS_TSS2]:  # These cars use LQR/INDI
+    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]:  # 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.]]
@@ -81,28 +81,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      if prius_pid:
-        ret.steerLimitTimer = 5.0
-        ret.steerActuatorDelay = 0.52
-        ret.lateralTuning.init('pid')
-        ret.lateralTuning.pid.kpBP = [0.0]
-        ret.lateralTuning.pid.kiBP = [0.0]
-        ret.lateralTuning.pid.kpV = [0.036]
-        ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
-        ret.lateralTuning.pid.newKfTuned = True
-      else:
-        ret.steerActuatorDelay = 0
-        ret.steerLimitTimer = 0.1
-        ret.lateralTuning.init('indi')
-        ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
-        ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-        ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
-        ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
-        ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
-        ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
-        ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
-        ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
+      ret.steerLimitTimer = 5.0
+      ret.steerActuatorDelay = 0.52
+      ret.lateralTuning.pid.kpV = [0.036]
+      ret.lateralTuning.pid.kiV = [0.0012]
+      ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
+      ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False

From 714fc74219d97b16f77b93455cb135ef7190385c Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 19 Mar 2021 19:03:25 +0100
Subject: [PATCH 24/44] Update op_params.py

---
 common/op_params.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common/op_params.py b/common/op_params.py
index 3d4b08498b134d..2168e1c6854de9 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -101,7 +101,7 @@ def __init__(self):
                         #'NoctuaMode': Param(False, bool, 'Noctua Fan are super quite and they run at full speed at all time.'),
                         'offset_limit': Param(0, VT.number, 'Speed at which apk percent offset will work in m/s'),
                         'osm': Param(True, bool, 'Whether to use OSM for drives'),
-                        #'prius_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\nFalse: stock INDI, True: TSS2-tuned PID'),
+                        'prius_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\nFalse: stock INDI, True: TSS2-tuned PID'),
                         'physical_buttons_AP': Param(False, bool, 'This enables the physical buttons to control sport and eco, some cars do not have buttons'),
                         'physical_buttons_DF': Param(False, bool, 'This enables the physical buttons to control following distance, TSS1 works with new SDSU FW'),
                         'physical_buttons_LKAS': Param(False, bool, 'This enables the physical buttons to control LKAS. TSS1 only this may break if used on TSS2 vechicle'),

From f19da0998dd357d3e8004cca64f0aac8c0b33355 Mon Sep 17 00:00:00 2001
From: SCshredder17 <57152954+SCshredder17@users.noreply.github.com>
Date: Sun, 21 Mar 2021 21:31:24 -0700
Subject: [PATCH 25/44] Tss2 Corolla; Reverted kf edit to prevent oversteer at
 30mph. (#823)

* Update interface.py

Reverted kf edit to prevent oversteer at 30mph.

* Update interface.py
---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 04c78edc10e970..0039bc21f9a807 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -319,7 +319,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.pid.kiBP = [0.0]
         ret.lateralTuning.pid.kpV = [0.036]
         ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
+        ret.lateralTuning.pid.kf = 0.000153263811757641 
         ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:

From 5aa72579f3eed8622aa6a28f79be003aa4e2cfb2 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Mon, 22 Mar 2021 12:44:15 -0700
Subject: [PATCH 26/44] sparrow tune tss2

---
 selfdrive/car/toyota/interface.py | 26 ++++++++++++++++++++------
 1 file changed, 20 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 0039bc21f9a807..0ec3749afc5f80 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -81,12 +81,26 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerLimitTimer = 5.0
       ret.steerActuatorDelay = 0.52
-      ret.lateralTuning.pid.kpV = [0.036]
-      ret.lateralTuning.pid.kiV = [0.0012]
-      ret.lateralTuning.pid.kf = 0.000173263811757641 # hardcoded in latcontrol_pid, this does nothing for now
-      ret.lateralTuning.pid.newKfTuned = True
+      ret.steerLimitTimer = 5.0
+      if spairrowtuning:
+        ret.lateralTuning.init('indi')
+        ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
+        ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
+        ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
+        ret.steerActuatorDelay = 0.42
+      else:
+        ret.lateralTuning.pid.kpBP = [0.0]
+        ret.lateralTuning.pid.kiBP = [0.0]
+        ret.lateralTuning.pid.kpV = [0.036]
+        ret.lateralTuning.pid.kiV = [0.0012]
+        ret.lateralTuning.pid.kf = 0.000153263811757641
+        ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -319,7 +333,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.pid.kiBP = [0.0]
         ret.lateralTuning.pid.kpV = [0.036]
         ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = 0.000153263811757641 
+        ret.lateralTuning.pid.kf = 0.000153263811757641
         ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:

From 2af1cbccd2de82d305a15e863c72bdac4f90a4f7 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Mon, 22 Mar 2021 12:57:38 -0700
Subject: [PATCH 27/44] distance_traveled params.

---
 common/op_params.py             | 1 +
 selfdrive/controls/controlsd.py | 9 +++++++--
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/common/op_params.py b/common/op_params.py
index 2168e1c6854de9..4c6539746781ac 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -80,6 +80,7 @@ def __init__(self):
                         'cloak': Param(True, bool, "make comma believe you are on their fork"),
                         #'corolla_tss2_d_tuning': Param(False, bool, 'lateral tuning using PID w/ true derivative'),
                         'default_brake_distance': Param(250.0, VT.number, 'Distance in m to start braking for mapped speeds.'),
+                        'distance_traveled': Param(False, bool, 'Whether to log distance_traveled or not.')
                         #'enable_long_derivative': Param(False, bool, 'If you have longitudinal overshooting, enable this! This enables derivative-based\n'
                                                                     # 'integral wind-down to help reduce overshooting within the long PID loop'),
                         #'dynamic_follow': Param('normal', str, "Can be: ('close', 'normal', 'far'): Left to right increases in following distance.\n"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 57974f046ee60b..5e9587ea543f28 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -24,6 +24,10 @@
 from common.travis_checker import travis
 #import threading
 from selfdrive.interceptor import Interceptor
+from common.op_params import opParams
+op_params = opParams()
+
+distance_traveled = op_params.get('distance_traveled')
 
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
@@ -136,6 +140,7 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.last_blinker_frame = 0
     self.saturated_count = 0
     self.distance_traveled_now = 0
+    self.distance_traveled_now = op_params.get('distance_traveled')
     if not travis:
       self.distance_traveled = float(params.get("DistanceTraveled", encoding='utf8'))
       self.distance_traveled_engaged = float(params.get("DistanceTraveledEngaged", encoding='utf8'))
@@ -146,7 +151,7 @@ def __init__(self, sm=None, pm=None, can_sock=None):
       self.distance_traveled_override = 0
 
     self.distance_traveled_frame = 0
-    
+
     self.last_functional_fan_frame = 0
     self.events_prev = []
     self.current_alert_types = [ET.PERMANENT]
@@ -379,7 +384,7 @@ def state_transition(self, CS):
       #print("there")
       self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
       #print(" v_cruise_kph = " + str(self.v_cruise_kph))
-    
+
 
     # decrease the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state

From a34db22ac5cae19952ea90414de262f3838511b7 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Mon, 22 Mar 2021 13:01:49 -0700
Subject: [PATCH 28/44] syntax

---
 common/op_params.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common/op_params.py b/common/op_params.py
index 4c6539746781ac..b3ddddffec2d84 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -80,7 +80,7 @@ def __init__(self):
                         'cloak': Param(True, bool, "make comma believe you are on their fork"),
                         #'corolla_tss2_d_tuning': Param(False, bool, 'lateral tuning using PID w/ true derivative'),
                         'default_brake_distance': Param(250.0, VT.number, 'Distance in m to start braking for mapped speeds.'),
-                        'distance_traveled': Param(False, bool, 'Whether to log distance_traveled or not.')
+                        'distance_traveled': Param(False, bool, 'Whether to log distance_traveled or not.'),
                         #'enable_long_derivative': Param(False, bool, 'If you have longitudinal overshooting, enable this! This enables derivative-based\n'
                                                                     # 'integral wind-down to help reduce overshooting within the long PID loop'),
                         #'dynamic_follow': Param('normal', str, "Can be: ('close', 'normal', 'far'): Left to right increases in following distance.\n"

From a220c2bc7494d5568ccbf6fa924b0a5da073f81e Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Mon, 22 Mar 2021 13:05:09 -0700
Subject: [PATCH 29/44] fix auto update.

---
 selfdrive/updated.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index 23179069486e46..8f812db6bf778c 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -323,7 +323,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool:
 
 class AutoReboot:
   def __init__(self):
-    self.min_reboot_time = 5.
+    self.min_reboot_time = 5. * 30
     self.need_reboot = False
     self.time_offroad = 0.0
 

From 29627c8a5ec8338d6c1beea8a9447f518bad9465 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Tue, 23 Mar 2021 23:10:52 -0700
Subject: [PATCH 30/44] FIND TUNE FOR WEEKND TRIP. PRIIUS_TSS2

---
 selfdrive/car/toyota/interface.py | 51 +++++++++++++++++++------------
 1 file changed, 31 insertions(+), 20 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 0ec3749afc5f80..b3fbb1734f896f 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -80,27 +80,38 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.wheelbase = 2.70002
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
-      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerActuatorDelay = 0.52
-      ret.steerLimitTimer = 5.0
-      if spairrowtuning:
-        ret.lateralTuning.init('indi')
-        ret.lateralTuning.indi.innerLoopGainBP = [18, 22, 26]
-        ret.lateralTuning.indi.innerLoopGainV = [9, 12, 15]
-        ret.lateralTuning.indi.outerLoopGainBP = [18, 22, 26]
-        ret.lateralTuning.indi.outerLoopGainV = [8, 11, 14.99]
-        ret.lateralTuning.indi.timeConstantBP = [18, 22, 26]
-        ret.lateralTuning.indi.timeConstantV = [1, 3, 4.5]
-        ret.lateralTuning.indi.actuatorEffectivenessBP = [18, 22, 26]
-        ret.lateralTuning.indi.actuatorEffectivenessV = [9, 12, 15]
-        ret.steerActuatorDelay = 0.42
+      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG\
+      ret.steerActuatorDelay = 0.5
+      if prius_pid:
+        ret.lateralTuning.init('pid')
+        ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.1]]
+        ret.lateralTuning.pid.kdV = [2.0]
+        ret.lateralTuning.pid.kfV = [0.00007818594]
+        ret.lateralTuning.pid.newKfTuned = False
       else:
-        ret.lateralTuning.pid.kpBP = [0.0]
-        ret.lateralTuning.pid.kiBP = [0.0]
-        ret.lateralTuning.pid.kpV = [0.036]
-        ret.lateralTuning.pid.kiV = [0.0012]
-        ret.lateralTuning.pid.kf = 0.000153263811757641
-        ret.lateralTuning.pid.newKfTuned = True
+        ret.steerRateCost = 0.45 #0.45
+        ret.steerLimitTimer = 5.0
+        ret.lateralTuning.init('indi')
+        #ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
+        #ret.lateralTuning.indi.innerLoopGainV = [15, 15]
+        #ret.lateralTuning.indi.outerLoopGainBP = [8.3, 25, 27.7, 36.1]
+        #ret.lateralTuning.indi.outerLoopGainV = [4.6, 14.99, 14.99, 19]
+        #ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.1, 33.3, 36.1]
+        #ret.lateralTuning.indi.timeConstantV = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.0, 4.0, 4.0]
+        #ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
+        #ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
+        #ret.lateralTuning.init('indi') #really good tune from cgw.
+        ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25, 36.1]
+        ret.lateralTuning.indi.innerLoopGainV = [9.5, 15, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [16.7, 25, 36.1]
+        ret.lateralTuning.indi.outerLoopGainV = [9.5, 14.99, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [16.7, 16.71, 22, 22.01, 26, 26.01, 36, 36.01]
+        ret.lateralTuning.indi.timeConstantV = [0.5, 1, 1, 2, 2, 4, 4, 5]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25, 36.1]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [9.5, 15, 15]
+
+
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False

From 59ea339abc9f2f5c19bc451d6dd94b5742f917d1 Mon Sep 17 00:00:00 2001
From: Kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Tue, 23 Mar 2021 23:34:37 -0700
Subject: [PATCH 31/44] Update interface.py

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index b3fbb1734f896f..5e812b8910826e 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -80,7 +80,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.wheelbase = 2.70002
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
-      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG\
+      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
       ret.steerActuatorDelay = 0.5
       if prius_pid:
         ret.lateralTuning.init('pid')

From 6f7897a5bd779fd368ac66cc6642bb4bf10afc2e Mon Sep 17 00:00:00 2001
From: cgw1968-5779 <58063525+cgw1968-5779@users.noreply.github.com>
Date: Wed, 24 Mar 2021 21:56:05 +0800
Subject: [PATCH 32/44] Update interface.py

---
 selfdrive/car/toyota/interface.py | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 5e812b8910826e..4a196527afb586 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -270,7 +270,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.safetyParam = 56
       ret.wheelbase = 2.68986
       ret.steerRatio = 14.3
-      ret.steerRateCost = 0.1
+      ret.steerRateCost = 0.4
       tire_stiffness_factor = 0.7933
       ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG
       ret.longitudinalTuning.deadzoneBP = [0., 8.05]
@@ -279,20 +279,20 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
       ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
       ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
-      ret.stoppingBrakeRate = 0.16 # reach stopping target smoothly
-      ret.startingBrakeRate = 0.9 # release brakes fast
+      ret.stoppingBrakeRate = 0.15 # reach stopping target smoothly
+      ret.startingBrakeRate = 1.21 # release brakes fast
       ret.startAccel = 1.50 # Accelerate from 0 faster
       ret.steerActuatorDelay = 0
-      ret.steerLimitTimer = 0.1
+      ret.steerLimitTimer = 5
       ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
-      ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
-      ret.lateralTuning.indi.outerLoopGainV = [4.55, 6.35, 8.15, 9.95, 11.75, 13.55, 14.99, 14.99, 14.99, 14.99]
-      ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.6, 33.3, 36.1]
-      ret.lateralTuning.indi.timeConstantV = [1.0, 1.3, 1.6, 1.9, 2.2, 2.5, 2.8, 3.4, 3.7, 4.0]
-      ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
-      ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
+      ret.lateralTuning.indi.innerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26]
+      ret.lateralTuning.indi.innerLoopGainV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15]
+      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6]
+      ret.lateralTuning.indi.outerLoopGainV = [3.6, 4.9, 6.15, 7.5, 10, 11.2, 15, 16, 17, 17]
+      ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6]
+      ret.lateralTuning.indi.timeConstantV = [0.46, 0.62, 0.77, 0.93, 1.1, 3.0, 4.0, 5.5, 7.0, 8.0]
+      ret.lateralTuning.indi.actuatorEffectivenessBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26]
+      ret.lateralTuning.indi.actuatorEffectivenessV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15]
 
     elif candidate == CAR.RAV4H_TSS2:
       stop_and_go = True

From 7b512b3aa23379c0743cae735e6dc3b8d535a260 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 24 Mar 2021 10:04:57 -0700
Subject: [PATCH 33/44] tune and ci

---
 Dockerfile.openpilot_base         |  1 +
 selfdrive/car/toyota/interface.py | 40 +++++++++----------------------
 2 files changed, 12 insertions(+), 29 deletions(-)

diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base
index c3c8f8cd15d98f..367d2add73b3f7 100644
--- a/Dockerfile.openpilot_base
+++ b/Dockerfile.openpilot_base
@@ -14,6 +14,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     cppcheck \
     curl \
     ffmpeg \
+    gcc-arm-none-eabi \
     git \
     iputils-ping \
     libarchive-dev \
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 4a196527afb586..2c8f78f7e61fa3 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -81,35 +81,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerActuatorDelay = 0.5
-      if prius_pid:
-        ret.lateralTuning.init('pid')
-        ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
-        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.1]]
-        ret.lateralTuning.pid.kdV = [2.0]
-        ret.lateralTuning.pid.kfV = [0.00007818594]
-        ret.lateralTuning.pid.newKfTuned = False
-      else:
-        ret.steerRateCost = 0.45 #0.45
-        ret.steerLimitTimer = 5.0
-        ret.lateralTuning.init('indi')
-        #ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
-        #ret.lateralTuning.indi.innerLoopGainV = [15, 15]
-        #ret.lateralTuning.indi.outerLoopGainBP = [8.3, 25, 27.7, 36.1]
-        #ret.lateralTuning.indi.outerLoopGainV = [4.6, 14.99, 14.99, 19]
-        #ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.1, 33.3, 36.1]
-        #ret.lateralTuning.indi.timeConstantV = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.0, 4.0, 4.0]
-        #ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
-        #ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
-        #ret.lateralTuning.init('indi') #really good tune from cgw.
-        ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25, 36.1]
-        ret.lateralTuning.indi.innerLoopGainV = [9.5, 15, 15]
-        ret.lateralTuning.indi.outerLoopGainBP = [16.7, 25, 36.1]
-        ret.lateralTuning.indi.outerLoopGainV = [9.5, 14.99, 14.99]
-        ret.lateralTuning.indi.timeConstantBP = [16.7, 16.71, 22, 22.01, 26, 26.01, 36, 36.01]
-        ret.lateralTuning.indi.timeConstantV = [0.5, 1, 1, 2, 2, 4, 4, 5]
-        ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25, 36.1]
-        ret.lateralTuning.indi.actuatorEffectivenessV = [9.5, 15, 15]
+      ret.steerActuatorDelay = 0
+      ret.steerLimitTimer = 5
+      ret.lateralTuning.init('indi')
+      ret.lateralTuning.indi.innerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26]
+      ret.lateralTuning.indi.innerLoopGainV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15]
+      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6]
+      ret.lateralTuning.indi.outerLoopGainV = [3.6, 4.9, 6.15, 7.5, 10, 11.2, 15, 16, 17, 17]
+      ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6]
+      ret.lateralTuning.indi.timeConstantV = [0.46, 0.62, 0.77, 0.93, 1.1, 3.0, 4.0, 5.5, 7.0, 8.0]
+      ret.lateralTuning.indi.actuatorEffectivenessBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26]
+      ret.lateralTuning.indi.actuatorEffectivenessV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15]
 
 
 

From 90b261f6c9467263c7b3a5e3a9505070ac0e1e4f Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 24 Mar 2021 14:59:46 -0700
Subject: [PATCH 34/44] tune

---
 selfdrive/car/toyota/interface.py | 40 ++++++++++++++++++++++---------
 1 file changed, 29 insertions(+), 11 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 2c8f78f7e61fa3..078faa0715506a 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -81,17 +81,35 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
-      ret.steerActuatorDelay = 0
-      ret.steerLimitTimer = 5
-      ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26]
-      ret.lateralTuning.indi.innerLoopGainV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15]
-      ret.lateralTuning.indi.outerLoopGainBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6]
-      ret.lateralTuning.indi.outerLoopGainV = [3.6, 4.9, 6.15, 7.5, 10, 11.2, 15, 16, 17, 17]
-      ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26, 30.3, 33.6]
-      ret.lateralTuning.indi.timeConstantV = [0.46, 0.62, 0.77, 0.93, 1.1, 3.0, 4.0, 5.5, 7.0, 8.0]
-      ret.lateralTuning.indi.actuatorEffectivenessBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 26]
-      ret.lateralTuning.indi.actuatorEffectivenessV = [4.15, 5.55, 7, 8.9, 10.0, 12.15, 14.25, 15]
+      ret.steerActuatorDelay = 0.6
+      if prius_pid:
+        ret.lateralTuning.init('pid')
+        ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kfBP = [[0.], [0.], [0.]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35, 0.18], [0.03, 0.1]]
+        ret.lateralTuning.pid.kdV = [2.2]
+        ret.lateralTuning.pid.kfV = [0.00007818594]
+        ret.lateralTuning.pid.newKfTuned = False
+      else:
+        ret.steerRateCost = 0.45 #0.45
+        ret.steerLimitTimer = 5.0
+        ret.lateralTuning.init('indi')
+        #ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25]
+        #ret.lateralTuning.indi.innerLoopGainV = [15, 15]
+        #ret.lateralTuning.indi.outerLoopGainBP = [8.3, 25, 27.7, 36.1]
+        #ret.lateralTuning.indi.outerLoopGainV = [4.6, 14.99, 14.99, 19]
+        #ret.lateralTuning.indi.timeConstantBP = [8.3, 11.1, 13.9, 16.7, 19.4, 22.2, 25, 30.1, 33.3, 36.1]
+        #ret.lateralTuning.indi.timeConstantV = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.0, 4.0, 4.0]
+        #ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25]
+        #ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15]
+        #ret.lateralTuning.init('indi') #really good tune from cgw.
+        ret.lateralTuning.indi.innerLoopGainBP = [16.7, 25, 36.1]
+        ret.lateralTuning.indi.innerLoopGainV = [9.5, 15, 15]
+        ret.lateralTuning.indi.outerLoopGainBP = [16.7, 25, 36.1]
+        ret.lateralTuning.indi.outerLoopGainV = [9.5, 14.99, 14.99]
+        ret.lateralTuning.indi.timeConstantBP = [16.7, 16.71, 22, 22.01, 26, 26.01, 36, 36.01]
+        ret.lateralTuning.indi.timeConstantV = [0.5, 1, 1, 2, 2, 4, 4, 5]
+        ret.lateralTuning.indi.actuatorEffectivenessBP = [16.7, 25, 36.1]
+        ret.lateralTuning.indi.actuatorEffectivenessV = [9.5, 15, 15]
 
 
 

From b76163c5dc1c80c2c2a5f1c396fb45499345042e Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Mar 2021 12:51:22 +0100
Subject: [PATCH 35/44] 20% less strength for turning back

---
 selfdrive/controls/lib/latcontrol_lqr.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
index 1bd2c5c403983c..58e1eaa05b1415 100644
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -48,9 +48,9 @@ def update(self, active, CS, CP, path_plan):
 
     steers_max = get_steer_max(CP, CS.vEgo)
     if (self.output_steer > 0.0) and (CS.steeringAngle > 0.0):
-      factor = -0.4
+      factor = -0.2
     elif (self.output_steer < 0.0) and (CS.steeringAngle < 0.0):
-      factor = -0.4
+      factor = -0.2
     else:
       factor = 0.0
     torque_scale = (1-factor*max(abs(CS.steeringAngle)/100,1.0))*(0.45 + CS.vEgo / 60.0)**2  # Scale actuator model with speed

From ae3bc74e4524f5f214d157b0ff1341ad820152dd Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Mar 2021 12:52:32 +0100
Subject: [PATCH 36/44] faster lane changes at sport and normal modes

---
 selfdrive/controls/lib/pathplanner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index c35729ee23fbca..90595a7020414c 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -159,7 +159,7 @@ def update(self, sm, pm, CP, VM):
           # we only set timer when in preLaneChange state, dragon_auto_lc_delay delay
           if self.lane_change_state == LaneChangeState.preLaneChange:
             self.dragon_auto_lc_timer = cur_time + sm['dragonConf'].dpAutoLcDelay
-        elif cur_time >= (self.dragon_auto_lc_timer - dp_profile + 1):
+        elif cur_time >= (self.dragon_auto_lc_timer - dp_profile):
           # if timer is up, we set torque_applied to True to fake user input
           torque_applied = True
           self.dp_did_auto_lc = True

From b25923980f263a562096fcd86db6c0e031c75771 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Thu, 25 Mar 2021 15:52:01 +0100
Subject: [PATCH 37/44] fix distance_traveled toggle

---
 selfdrive/controls/controlsd.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 5e9587ea543f28..b49f2c78c7ad44 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -140,7 +140,6 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.last_blinker_frame = 0
     self.saturated_count = 0
     self.distance_traveled_now = 0
-    self.distance_traveled_now = op_params.get('distance_traveled')
     if not travis:
       self.distance_traveled = float(params.get("DistanceTraveled", encoding='utf8'))
       self.distance_traveled_engaged = float(params.get("DistanceTraveledEngaged", encoding='utf8'))
@@ -362,7 +361,7 @@ def data_sample(self):
       self.distance_traveled_engaged += CS.vEgo * DT_CTRL
       if CS.steeringPressed:
         self.distance_traveled_override += CS.vEgo * DT_CTRL
-    if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis:
+    if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis and distance_traveled:
       put_nonblocking("DistanceTraveled", str(round(self.distance_traveled,2)))
       put_nonblocking("DistanceTraveledEngaged", str(round(self.distance_traveled_engaged,2)))
       put_nonblocking("DistanceTraveledOverride", str(round(self.distance_traveled_override,2)))

From d94af57791c774eebecf18300aabf24f941250bc Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 08:24:11 +0100
Subject: [PATCH 38/44] Remove old files as well

---
 selfdrive/data_collection/gps_uploader.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/data_collection/gps_uploader.py b/selfdrive/data_collection/gps_uploader.py
index 665ab557b6b53f..befcd3d342010a 100644
--- a/selfdrive/data_collection/gps_uploader.py
+++ b/selfdrive/data_collection/gps_uploader.py
@@ -48,7 +48,7 @@ def upload_data():
         ftp.storbinary("STOR /{}/{}".format(username, filename + ".gz"), f)
       ftp.quit()
       os.remove(filepath)
-      os.remove("/data/" + filename + ".gz")
+      os.remove("/data/*.gz")
       t = datetime.datetime.utcnow().isoformat()
       params.put("LastUpdateTime", t.encode('utf8'))
       return True

From 0706637a13dedf58af136a54f6a7e17b23d6de0b Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 08:29:01 +0100
Subject: [PATCH 39/44] Create update_now.py

---
 scripts/update_now.py | 15 +++++++++++++++
 1 file changed, 15 insertions(+)
 create mode 100644 scripts/update_now.py

diff --git a/scripts/update_now.py b/scripts/update_now.py
new file mode 100644
index 00000000000000..91b539252f487d
--- /dev/null
+++ b/scripts/update_now.py
@@ -0,0 +1,15 @@
+#!/usr/bin/env python3
+import datetime
+from common.params import Params
+from selfdrive.data_collection import gps_uploader
+
+print("Don't forget to pray!")
+params = Params()
+t = datetime.datetime.utcnow().isoformat()
+params.put("LastUpdateTime", t.encode('utf8'))
+if params.get("IsOffroad") == b"1":
+  print("Please wait for gps to upload to aviod this in future!")
+  gps_uploader.upload_data()
+else:
+  print("Please switch off car and try again!")
+  

From 8f2c21444a8be0de6aa66b06a9d0272c0348a953 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 08:37:45 +0100
Subject: [PATCH 40/44] Enable update warning

---
 selfdrive/thermald/thermald.py | 68 +++++++++++++++++-----------------
 1 file changed, 34 insertions(+), 34 deletions(-)

diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index d9fdeead57425e..b565d204389f32 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-#import datetime
+import datetime
 import os
 import time
 from collections import namedtuple
@@ -345,45 +345,45 @@ def thermald_thread():
     # **** starting logic ****
 
     # Check for last update time and display alerts if needed
-    # now = datetime.datetime.utcnow()
+     now = datetime.datetime.utcnow()
     #
     # # show invalid date/time alert
-    # startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
-    # set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
+     startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
+     set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
     #
     # # Show update prompt
-    # try:
-    #   last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
-    # except (TypeError, ValueError):
-    #   last_update = now
-    # dt = now - last_update
+     try:
+       last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
+     except (TypeError, ValueError):
+       last_update = now
+     dt = now - last_update
     #
-    # update_failed_count = params.get("UpdateFailedCount")
-    # update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
-    # last_update_exception = params.get("LastUpdateException", encoding='utf8')
+     update_failed_count = params.get("UpdateFailedCount")
+     update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
+     last_update_exception = params.get("LastUpdateException", encoding='utf8')
     #
-    # if update_failed_count > 15 and last_update_exception is not None:
-    #   if current_branch in ["release2", "dashcam"]:
-    #     extra_text = "Ensure the software is correctly installed"
-    #   else:
-    #     extra_text = last_update_exception
-    #
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
-    #   set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
-    # elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
-    #   set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
-    # elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
-    #   remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
-    #   set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
-    # else:
-    #   set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
-    #   set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
+     if update_failed_count > 15 and last_update_exception is not None:
+       if current_branch in ["release5", "dashcam"]:
+         extra_text = "Ensure the software is correctly installed"
+       else:
+         extra_text = last_update_exception
+    
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
+       set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
+     elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
+       set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
+     elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
+       remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
+       set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
+     else:
+       set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
+       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
 
     startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1"
     startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version

From 5864610e5531bc63336bba920060242642133d1b Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 09:38:21 +0100
Subject: [PATCH 41/44] Fix indent

---
 selfdrive/thermald/thermald.py | 62 +++++++++++++++++-----------------
 1 file changed, 31 insertions(+), 31 deletions(-)

diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index b565d204389f32..83d5211617164e 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -345,45 +345,45 @@ def thermald_thread():
     # **** starting logic ****
 
     # Check for last update time and display alerts if needed
-     now = datetime.datetime.utcnow()
+      now = datetime.datetime.utcnow()
     #
     # # show invalid date/time alert
-     startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
-     set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
+      startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
+      set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
     #
     # # Show update prompt
-     try:
-       last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
-     except (TypeError, ValueError):
+      try:
+        last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
+      except (TypeError, ValueError):
        last_update = now
-     dt = now - last_update
+      dt = now - last_update
     #
-     update_failed_count = params.get("UpdateFailedCount")
-     update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
-     last_update_exception = params.get("LastUpdateException", encoding='utf8')
+      update_failed_count = params.get("UpdateFailedCount")
+      update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
+      last_update_exception = params.get("LastUpdateException", encoding='utf8')
     #
-     if update_failed_count > 15 and last_update_exception is not None:
-       if current_branch in ["release5", "dashcam"]:
-         extra_text = "Ensure the software is correctly installed"
-       else:
-         extra_text = last_update_exception
+      if update_failed_count > 15 and last_update_exception is not None:
+        if current_branch in ["release5", "dashcam"]:
+          extra_text = "Ensure the software is correctly installed"
+        else:
+          extra_text = last_update_exception
     
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
-       set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
-     elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
-       set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
-     elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
-       remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
-       set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
-     else:
-       set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
-       set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
+        set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
+      elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
+        set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
+      elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
+        remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
+        set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
+      else:
+        set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
+        set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
 
     startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1"
     startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version

From b376ff34146ff97724471029295b2a95aa5685d0 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 09:41:04 +0100
Subject: [PATCH 42/44] Update updated.py

---
 selfdrive/updated.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index 8f812db6bf778c..3572601ecdc6d6 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -109,7 +109,7 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) -
   params.put("UpdateFailedCount", str(failed_count))
   if failed_count == 0:
     t = datetime.datetime.utcnow().isoformat()
-    params.put("LastUpdateTime", t.encode('utf8'))
+    #params.put("LastUpdateTime", t.encode('utf8'))
 
   if exception is None:
     params.delete("LastUpdateException")
@@ -120,7 +120,7 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) -
     branch_name = run(["git", "rev-parse", "--abbrev-ref", "HEAD"], FINALIZED).rstrip()
     if branch_name == "testing":
       postfix = ''
-    elif branch_name == "devel-i18n":
+    elif branch_name == "DP08-clean":
       postfix = '-DEV'
     else:
       postfix = '-REL'

From 9cbaf9dc13940a22eda2303c06f6ad69c1e468a0 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 10:20:15 +0100
Subject: [PATCH 43/44] Update updated.py

---
 selfdrive/updated.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index 3572601ecdc6d6..17048cdda7ed6d 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -107,8 +107,8 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) -
   params = Params()
 
   params.put("UpdateFailedCount", str(failed_count))
-  if failed_count == 0:
-    t = datetime.datetime.utcnow().isoformat()
+  #if failed_count == 0:
+    #t = datetime.datetime.utcnow().isoformat()
     #params.put("LastUpdateTime", t.encode('utf8'))
 
   if exception is None:

From e16d336bbbac7667cc17797a563c118521f5f7a6 Mon Sep 17 00:00:00 2001
From: Arne Schwarck <arneschwarck@gmail.com>
Date: Fri, 26 Mar 2021 10:56:53 +0100
Subject: [PATCH 44/44] Fix for non indi colour path

---
 selfdrive/ui/ui.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 57fdd2b2a5396a..3cd1541085b73f 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -124,9 +124,9 @@ void update_sockets(UIState *s) {
 
     // TODO: the alert stuff shouldn't be handled here
     s->scene.steerOverride= scene.controls_state.getSteerOverride();
-    s->scene.output_scale = scene.controls_state.getLateralControlState().getPidState().getOutput();
-    s->scene.output_scale = scene.controls_state.getLateralControlState().getLqrState().getOutput();
-    s->scene.output_scale = scene.controls_state.getLateralControlState().getIndiState().getOutput();
+    s->scene.output_scale = scene.controls_state.getLateralControlState().getPidState().getOutput() + 
+      scene.controls_state.getLateralControlState().getLqrState().getOutput() + 
+      scene.controls_state.getLateralControlState().getIndiState().getOutput();
 
     auto alert_sound = scene.controls_state.getAlertSound();
     if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) {