From c39a1e3e40415b5d5bed1e9adefc034b607db297 Mon Sep 17 00:00:00 2001
From: dragonpilot <dgnpilot@gmail.com>
Date: Mon, 16 Mar 2020 16:57:37 +1000
Subject: [PATCH] add 'OFF' option to dynamic follow profile (stock mode)

---
 selfdrive/controls/lib/long_mpc.py           | 46 +++++++++++---------
 selfdrive/dragonpilot/dragonconf/__init__.py |  2 +-
 2 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 278550ed1..35cfde668 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -17,9 +17,10 @@ from selfdrive.dragonpilot.dragonconf import dp_get_last_modified
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
 # df profile
-DF_PROFILE_ROADTRIP = -1
-DF_PROFILE_STANDARD = 0
-DF_PROFILE_TRAFFIC = 1
+DF_PROFILE_OFF = -2
+DF_PROFILE_LONG = -1
+DF_PROFILE_NORMAL = 0
+DF_PROFILE_SHORT = 1
 
 class LongitudinalMpc():
   def __init__(self, mpc_id):
@@ -40,7 +41,7 @@ class LongitudinalMpc():
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
     self.last_cost = 0.0
-    self.df_profile = DF_PROFILE_STANDARD
+    self.df_profile = DF_PROFILE_OFF
     self.sng = False
 
     # dragonpilot
@@ -79,13 +80,28 @@ class LongitudinalMpc():
     self.cur_state[0].a_ego = a
 
   def get_TR(self, CS):
-    if not self.lead_data['status']:
+    # load profile
+    ts = sec_since_boot()
+    if ts - self.last_ts >= 10.:
+      modified = dp_get_last_modified()
+      if self.last_modified != modified:
+        try:
+          self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8'))
+          self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_OFF, DF_PROFILE_LONG, DF_PROFILE_NORMAL, DF_PROFILE_SHORT] else DF_PROFILE_OFF
+        except TypeError:
+          self.df_profile = DF_PROFILE_OFF
+      self.last_modified = modified
+      self.last_ts = ts
+
+    if not self.lead_data['status'] or self.df_profile == DF_PROFILE_OFF:
       TR = 1.8
     else:
       self.store_df_data()
       TR = self.dynamic_follow(CS)
 
-    self.change_cost(TR)
+    # only change cost when profile is not off
+    if not self.df_profile == DF_PROFILE_OFF:
+      self.change_cost(TR)
     return TR
 
   def change_cost(self, TR):
@@ -127,25 +143,13 @@ class LongitudinalMpc():
     return a_lead  # if above doesn't execute, we'll return a_lead from radar
 
   def dynamic_follow(self, CS):
-    ts = sec_since_boot()
-    if ts - self.last_ts >= 10.:
-      modified = dp_get_last_modified()
-      if self.last_modified != modified:
-        try:
-          self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8'))
-          self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_STANDARD, DF_PROFILE_ROADTRIP, DF_PROFILE_TRAFFIC] else DF_PROFILE_STANDARD
-        except TypeError:
-          self.df_profile = DF_PROFILE_STANDARD
-      self.last_modified = modified
-      self.last_ts = ts
-
     x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
     profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632]  # profile mod speeds, mph: [5., 30., 55., 80.]
-    if self.df_profile == DF_PROFILE_ROADTRIP:
+    if self.df_profile == DF_PROFILE_LONG:
       y_dist = [1.3847, 1.3946, 1.4078, 1.4243, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74]  # TRs
       profile_mod_pos = [0.99, 0.9025, 0.815, 0.55]
       profile_mod_neg = [1.0, 1.18, 1.382, 1.787]
-    elif self.df_profile == DF_PROFILE_TRAFFIC:  # for in congested traffic
+    elif self.df_profile == DF_PROFILE_SHORT:  # for in congested traffic
       x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35]
       y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562]
       profile_mod_pos = [1.05, 1.375, 2.99, 3.8]
@@ -186,7 +190,7 @@ class LongitudinalMpc():
     TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mod])  # alter TR modification according to profile
     TR += TR_mod
 
-    if CS.leftBlinker or CS.rightBlinker and self.df_profile != DF_PROFILE_TRAFFIC:
+    if CS.leftBlinker or CS.rightBlinker and self.df_profile != DF_PROFILE_SHORT:
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .75, .65]  # reduce TR when changing lanes
       TR *= interp(self.car_data['v_ego'], x, y)
diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py
index a789e5e30..9bd27b33d 100644
--- a/selfdrive/dragonpilot/dragonconf/__init__.py
+++ b/selfdrive/dragonpilot/dragonconf/__init__.py
@@ -71,7 +71,7 @@ default_conf = {
   'DragonAccelProfile': '0',
   'DragonLastModified': str(floor(time.time())),
   'DragonEnableRegistration': '1',
-  'DragonDynamicFollow': '0', # traffic = -1, stock = 0, roadtrip = 1
+  'DragonDynamicFollow': '-2', # OFF = -2, LONG = -1, NORMAL = 0, SHORT = 1
 }
 
 deprecated_conf = {