From 728a5aae5077c6a18c2b05f5aac32b2591698d9a Mon Sep 17 00:00:00 2001
From: kegman <8837066+kegman@users.noreply.github.com>
Date: Wed, 18 Dec 2019 12:23:49 -0500
Subject: [PATCH] Enabled toggle for ALC in kegman.json and minSpeed (#250)

* Remove annoying brake chime, disable stock fcw and print thermal msg (#248)

* ALC toggles (#249)

* Add ALC toggles to kegman.json

* Fix retrieval of ALC vars from kegman.json

* Fix ALCnudgeLess logic

* Fix - remove CV import and fix 'true' to True

* Fix kegman.json ALC variable retrieval

* Re-enable LKAS button
---
 selfdrive/car/honda/carcontroller.py  |  2 +-
 selfdrive/controls/lib/pathplanner.py | 17 +++++++++++------
 selfdrive/kegman_conf.py              |  9 ++++++++-
 3 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index b80464fa22d6d0..0deca12637cf0d 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -145,7 +145,7 @@ def update(self, enabled, CS, frame, actuators, \
     apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
     apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
 
-    lkas_active = enabled and not CS.steer_not_allowed # and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on  # add LKAS button to toggle steering
+    lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode #and not CS.left_blinker_on and not CS.right_blinker_on  # add LKAS button to toggle steering
 
     # Send CAN commands.
     can_sends = []
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 91289456fec850..f01d227eca946c 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -7,7 +7,6 @@
 from selfdrive.controls.lib.lane_planner import LanePlanner
 from selfdrive.kegman_conf import kegman_conf
 from common.numpy_fast import interp
-from selfdrive.config import Conversions as CV
 import cereal.messaging as messaging
 from cereal import log
 
@@ -75,7 +74,10 @@ def __init__(self, CP):
 
     self.steerRateCost_prev = self.steerRateCost
     self.setup_mpc()
-   
+
+    self.alc_nudge_less = bool(kegman.conf['ALCnudgeLess'])
+    self.alc_min_speed = float(kegman.conf['ALCminSpeed'])
+
     self.lane_change_state = LaneChangeState.off
     self.lane_change_timer = 0.0
     self.prev_one_blinker = False
@@ -159,10 +161,13 @@ def update(self, sm, pm, CP, VM):
       elif sm['carState'].rightBlinker:
         lane_change_direction = LaneChangeDirection.right
 
-      if lane_change_direction == LaneChangeDirection.left:
-        torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed
+      if self.alc_nudge_less:
+        torque_applied = True
       else:
-        torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed
+        if lane_change_direction == LaneChangeDirection.left:
+          torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed
+        else:
+          torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed
 
       lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
 
@@ -186,7 +191,7 @@ def update(self, sm, pm, CP, VM):
         self.lane_change_state = LaneChangeState.preLaneChange
 
       # Don't allow starting lane change below 45 mph
-      if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange):
+      if (v_ego < self.alc_min_speed) and (self.lane_change_state == LaneChangeState.preLaneChange):
         self.lane_change_state = LaneChangeState.off
 
     if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py
index 6da1f8178ecb75..0bb5a8141849be 100644
--- a/selfdrive/kegman_conf.py
+++ b/selfdrive/kegman_conf.py
@@ -109,6 +109,11 @@ def read_config(self):
         self.config.update({"sR_time":"1"})
         self.element_updated = True
 
+      if "ALCnudgeLess" not in self.config:
+        self.config.update({"ALCnudgeLess":"0"})
+        self.config.update({"ALCminSpeed":"20.1168"})
+        self.element_updated = True
+
       if self.element_updated:
         print("updated")
         self.write_config(self.config)
@@ -122,7 +127,9 @@ def read_config(self):
 		     "3barBP1":"3.0", "1barMax":"2.1", "2barMax":"2.1", "3barMax":"2.1", \
 		     "1barHwy":"0.4", "2barHwy":"0.3", "3barHwy":"0.1", \
 		     "steerRatio":"-1", "steerRateCost":"-1", "slowOnCurves":"0", "Kf":"-1", \
-		     "sR_boost":"0", "sR_BP0":"0", "sR_BP1":"0", "sR_time":"1"} 
+		     "sR_boost":"0", "sR_BP0":"0", "sR_BP1":"0", "sR_time":"1", \
+                     "ALCnudgeLess":"0", "ALCminSpeed":"20.1168"}
+
 
       self.write_config(self.config)
     return self.config