diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index f8abddaf2..c8bf8f779 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/common/params.py b/common/params.py index 19c487bbf..ea7950b2f 100755 --- a/common/params.py +++ b/common/params.py @@ -138,6 +138,7 @@ keys = { "DragonEnableDriverMonitoring": [TxType.PERSISTENT], "DragonCarModel": [TxType.PERSISTENT], "DragonCarVIN": [TxType.PERSISTENT], + "DragonEnableSlowOnCurve": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index db3fe3321..f09843e28 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -88,6 +88,10 @@ class Planner(): self.params = Params() + # dragonpilot + self.dragon_slow_on_curve = True + self.last_ts = 0. + def choose_solution(self, v_cruise_setpoint, enabled): if enabled: solutions = {'model': self.v_model, 'cruise': self.v_cruise} @@ -120,6 +124,12 @@ class Planner(): cur_time = sec_since_boot() v_ego = sm['carState'].vEgo + # dragonpilot + # update variable status every 5 secs + if cur_time - self.last_ts > 5.: + self.dragon_slow_on_curve = False if self.params.get("DragonEnableSlowOnCurve", encoding='utf8') == "0" else True + self.last_ts = cur_time + long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel @@ -130,7 +140,7 @@ class Planner(): enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) - if len(sm['model'].path.poly): + if self.dragon_slow_on_curve and len(sm['model'].path.poly): path = list(sm['model'].path.poly) # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 86cdee49c..4004fbaf4 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -48,6 +48,7 @@ default_conf = { 'DragonEnableDriverMonitoring': '1', 'DragonCarModel': '', 'DragonCarVIN': '', + 'DragonEnableSlowOnCurve': '1', } deprecated_conf = {