dragonpilot beta3

date: 2023-10-17T17:15:50
commit: 40ccd4c168d7fb1576d8fb8874823c2efd297c5b

CHANGELOGS.md

@@ -1,4 +1,11 @@
-dragonpilot beta3 2023.10.09
+dragonpilot beta3 2023.10.17
+=======================
+* openpilot master branch on 2023.10.20.
+* Otisserv improvement/bug fixes.
+* TSS2 updated long tune.
+* @krkeegan SNG boost tune.
+
+dragonpilot beta3 [2023-10-09]
 =======================
 * openpilot master branch on 2023.07.21.
 * Comma 0.9.6 release

body/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[8] = "b6dc9e48";
+const uint8_t gitversion[8] = "0ca26c43";

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "2023.10.09"
+#define COMMA_VERSION "2023.10.17"

panda/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[] = "DEV-b6dc9e48-DEBUG";
+const uint8_t gitversion[] = "DEV-0ca26c43-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-b6dc9e48-DEBUG
+DEV-0ca26c43-DEBUG

selfdrive/car/toyota/interface.py

@@ -267,15 +267,15 @@ class CarInterface(CarInterfaceBase):
     if candidate in TSS2_CAR or ret.enableGasInterceptor:
       tune.kpBP = [0., 5., 20.]
       tune.kpV = [1.3, 1.0, 0.7] if dp_toyota_enhanced_long_tune else [1.8, 1.0, 0.7]
-      tune.kiBP = [0.,   1.,    2.,    3.,   4.,   5.,    12.,  20.,  27., 40.] if dp_toyota_enhanced_long_tune else [0., 5., 12., 20., 27.]
-      tune.kiV = [.34,  .325,   .31, .29, .27, .226, .198, .17,  .10, .01] if dp_toyota_enhanced_long_tune else [.35, .23, .20, .17, .1]
+      tune.kiBP = [0.,   1.,    2.,    3.,   4.,   5.,   8.,   12.,  20.,  27.,  40.] if dp_toyota_enhanced_long_tune else [0., 5., 12., 20., 27.]
+      tune.kiV = [.35,  .331,   .308,   .285,  .26,  .227, .21, .19,  .17,  .10,  .001] if dp_toyota_enhanced_long_tune else [.35, .23, .20, .17, .1]
       #tune.kiBP = [0.,  5.,  12., 20.,  25.,  30.,  40.] if dp_toyota_enhanced_long_tune else [0., 5., 12., 20., 27.]
       #tune.kiV = [.35, .23, .20, .17, .10, .01,  .001]  if dp_toyota_enhanced_long_tune else [.35, .23, .20, .17, .1]
       if candidate in TSS2_CAR:
-        ret.vEgoStopping = 0.2 if dp_toyota_enhanced_long_tune else 0.25 # car is near 0.1 to 0.2 when car starts requesting stopping accel
-        ret.vEgoStarting = 0.2 if dp_toyota_enhanced_long_tune else 0.25 # needs to be > or == vEgoStopping
-        ret.stopAccel = -0.42  if dp_toyota_enhanced_long_tune else -2.0 # Toyota requests -0.4 when stopped
-        ret.stoppingDecelRate = 0.057 if dp_toyota_enhanced_long_tune else 0.3 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
+        ret.vEgoStopping = 0.1 if dp_toyota_enhanced_long_tune else 0.25 # car is near 0.1 to 0.2 when car starts requesting stopping accel
+        ret.vEgoStarting = 0.1 if dp_toyota_enhanced_long_tune else 0.25 # needs to be > or == vEgoStopping
+        ret.stopAccel = -0.40  if dp_toyota_enhanced_long_tune else -2.0 # Toyota requests -0.4 when stopped
+        ret.stoppingDecelRate = 0.05 if dp_toyota_enhanced_long_tune else 0.3 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
         #ret.longitudinalActuatorDelayLowerBound = 1.5 if dp_toyota_enhanced_long_tune else 1.5
         #ret.longitudinalActuatorDelayUpperBound = 1.5 if dp_toyota_enhanced_long_tune else 1.5
     else:

selfdrive/controls/lib/accel_controller.py

@@ -29,15 +29,15 @@ DP_ACCEL_NORMAL = 2
 DP_ACCEL_SPORT = 3
 
 # accel profile by @arne182 modified by cgw
-_DP_CRUISE_MIN_V =       [-0.750, -0.750, -0.72, -0.72, -0.52, -0.40, -0.36, -0.32, -0.32,  -0.25, -0.25]
-_DP_CRUISE_MIN_V_ECO =   [-0.730, -0.730, -0.68, -0.68, -0.50, -0.36, -0.32, -0.28, -0.28,  -0.25, -0.25]
-_DP_CRUISE_MIN_V_SPORT = [-0.750, -0.750, -0.75, -0.75, -0.54, -0.42, -0.40, -0.35, -0.35,  -0.30, -0.30]
+_DP_CRUISE_MIN_V =       [-0.490, -0.490, -0.41, -0.41, -0.41, -0.40, -0.36, -0.32, -0.32,  -0.25, -0.25]
+_DP_CRUISE_MIN_V_ECO =   [-0.480, -0.480, -0.40, -0.40, -0.40, -0.36, -0.32, -0.28, -0.28,  -0.25, -0.25]
+_DP_CRUISE_MIN_V_SPORT = [-0.500, -0.500, -0.42, -0.42, -0.42, -0.42, -0.40, -0.35, -0.35,  -0.30, -0.30]
 _DP_CRUISE_MIN_BP =      [0.,     2.0,    2.01,  11.,   11.01, 18.,   18.01, 28.,   28.01,  33.,   55.]
 
-_DP_CRUISE_MAX_V =       [3.5, 3.2, 2.6, 2.0, 1.3,  0.9, .71,  .48, .36, .14]
-_DP_CRUISE_MAX_V_ECO =   [3.5, 3.0, 1.6, 1.2, .76,  .62, .48,  .36, .28, .09]
-_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 3.5, 3.5, 2.8,  2.0,  1.5, 1.0, .7,  .5]
-_DP_CRUISE_MAX_BP =      [0.,  3,   6.,  8.,  11.,  15.,  20., 25., 30., 55.]
+_DP_CRUISE_MAX_V =       [3.5, 3.4, 2.2, 1.5, .92,  0.76, .54,  .42, .32, .14]
+_DP_CRUISE_MAX_V_ECO =   [3.5, 3.3, 1.7, 1.1, .76,  .62,  .47,  .36, .28, .09]
+_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 3.0, 2.6, 1.4,  1.0,  0.7,  0.6, .38, .2]
+_DP_CRUISE_MAX_BP =      [0.,  3,   6.,  8.,  11.,  15.,  20.,  25., 30., 55.]
 
 class AccelController:
 

selfdrive/controls/lib/dynamic_endtoend_controller.py

@@ -48,8 +48,6 @@ STOP_AND_GO_FRAME = 500
 
 MODE_SWITCH_DELAY_FRAME = 500
 
-MAX_YREL_THRESHOLD = 0.25
-
 class SNG_State:
   off = 0
   stopped = 1
@@ -134,7 +132,6 @@ class DynamicEndtoEndController:
     # lead detection
     self._lead_gmac.add_data(lead_one.status)
     self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB
-    self._has_lead_yrel = lead_one.yRel > MAX_YREL_THRESHOLD
 
   # slow down detection
     self._slow_down_gmac.add_data(len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST))
@@ -220,8 +217,7 @@ class DynamicEndtoEndController:
 
   def _acc_priority_mode(self):
     # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
-    # e,g. stopped at the red light and car crossing the intersection.
-    if self._has_lead_filtered and not self._has_standstill and self._has_lead_yrel:
+    if self._has_lead_filtered and not self._has_standstill:
       self._mode = 'acc'
       return
 

selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json

@@ -1,7 +1,7 @@
 {
-    "acados_include_path": "/data/openpilot/third_party/acados/include",
-    "acados_lib_path": "/data/openpilot/third_party/acados/lib",
-    "code_export_directory": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
+    "acados_include_path": "/data/dp-private/third_party/acados/include",
+    "acados_lib_path": "/data/dp-private/third_party/acados/lib",
+    "code_export_directory": "/data/dp-private/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
     "constraints": {
         "C": [],
         "C_e": [],
@@ -343,7 +343,7 @@
         "ny_e": 5,
         "nz": 0
     },
-    "json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
+    "json_file": "/data/dp-private/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
     "model": {
         "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
         "con_h_expr_e": null,

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -86,7 +86,7 @@ def get_dynamic_follow(v_ego, personality=log.LongitudinalPersonality.standard):
     y_dist = [1.0,  1.35,  1.35,  1.5,  1.5,  1.5]
   elif personality==log.LongitudinalPersonality.aggressive:
     x_vel =  [0.0,  2.0,   5.55,  19.99, 20,    25,   40]
-    y_dist = [0.9,  1.0,   1.07,  1.09,  1.11,  1.11, 1.2]
+    y_dist = [0.9,  1.0,   1.08,  1.105,  1.11,  1.11, 1.2]
   else:
     raise NotImplementedError("Dynamic Follow personality not supported")
   return np.interp(v_ego, x_vel, y_dist)
@@ -102,6 +102,16 @@ def desired_follow_distance(v_ego, v_lead, t_follow=None):
     t_follow = get_T_FOLLOW()
   return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
 
+def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
+  # KRKeegan this offset rapidly decreases the following distance when the lead pulls
+  # away, resulting in an early demand for acceleration.
+  v_diff_offset = 0
+  if np.all(v_lead - v_ego > 0):
+    v_diff_offset = ((v_lead - v_ego) * 1.)
+    v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
+    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
+  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
+  return distance
 
 def gen_long_model():
   model = AcadosModel()
@@ -345,7 +355,7 @@ class LongitudinalMpc:
     self.max_a = max_a
 
   # def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, use_df_tune=False):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, use_df_tune=False, use_krkeegen_tune=False):
     # t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     t_follow = get_T_FOLLOW(personality) if not use_df_tune else get_dynamic_follow(v_ego, personality)
@@ -357,8 +367,12 @@ class LongitudinalMpc:
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    if use_krkeegen_tune:
+      lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego)
+      lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego)
+    else:
+      lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+      lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = self.max_a

selfdrive/controls/lib/longitudinal_planner.py

@@ -79,6 +79,7 @@ class LongitudinalPlanner:
     self.personality = log.LongitudinalPersonality.standard
     self.dp_long_use_df_tune = False
     self.dp_long_taco = self.params.get_bool('dp_long_taco')
+    self.dp_long_use_krkeegen_tune = False
 
   def read_param(self):
     try:
@@ -87,6 +88,7 @@ class LongitudinalPlanner:
       self.personality = log.LongitudinalPersonality.standard
 
     self.dp_long_use_df_tune = self.params.get_bool('dp_long_use_df_tune')
+    self.dp_long_use_krkeegen_tune = self.params.get_bool('dp_long_use_krkeegen_tune')
 
   @staticmethod
   def parse_model(model_msg, model_error, v_ego, taco=False):
@@ -185,7 +187,7 @@ class LongitudinalPlanner:
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, taco=self.dp_long_taco)
-    self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality, use_df_tune=self.dp_long_use_df_tune)
+    self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality, use_df_tune=self.dp_long_use_df_tune, use_krkeegen_tune=self.dp_long_use_krkeegen_tune)
 
     self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/manager/manager.py

@@ -80,6 +80,7 @@ def manager_init() -> None:
     ("dp_lateral_road_edge_detection", "0"),
     ("dp_nav_voice_guidance", "0"),
     ("dp_hkg_tacobell_lat", "0"),
+    ("dp_long_use_krkeegen_tune", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))

selfdrive/ui/translations/main_ar.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_de.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_fr.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_ja.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_ko.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_pt-BR.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_th.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_tr.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -435,6 +435,15 @@ Reboot Required.</source>
 Reboot Required.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable krkeegan Tune (SNG Boost)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When enabled, it will apply an offset to get_stopped_equivalence_factor in longitudinal MPC resulting in an early demand for acceleration.
+Credit to krkeegen: https://github.com/krkeegan/</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>On-Screen Personality Button</source>
         <translation type="unfinished"></translation>