From 8f6453898a033acb86e6ce15ac7de0cebd0123a8 Mon Sep 17 00:00:00 2001
From: Rick Lan <ricklan@gmail.com>
Date: Mon, 30 Nov 2020 16:48:03 +1000
Subject: [PATCH] smart mdps steering speed override

---
 selfdrive/car/hyundai/carcontroller.py |  4 ++-
 selfdrive/controls/controlsd.py        | 35 +++++++++++++++++++++++++-
 2 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 73f1717ab..61fc6e53e 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -5,6 +5,7 @@ from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create
 from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR
 from opendbc.can.packer import CANPacker
 from common.dp_common import common_controller_ctrl
+from common.params import Params
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -49,6 +50,7 @@ class CarController():
     # dp
     self.last_blinker_on = False
     self.blinker_end_frame = 0.
+    self.dp_hkg_smart_mdps = Params().get('dp_hkg_smart_mdps') == b'1'
 
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
              left_lane, right_lane, left_lane_depart, right_lane_depart, dragonconf):
@@ -61,7 +63,7 @@ class CarController():
     lkas_active = enabled and abs(CS.out.steeringAngle) < 90.
 
     # fix for Genesis hard fault at low speed
-    if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
+    if not self.dp_hkg_smart_mdps and CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
       lkas_active = False
 
     if not lkas_active:
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 52b0b9831..9721cf39c 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -2,7 +2,7 @@
 import os
 from cereal import car, log
 from common.hardware import HARDWARE
-from common.numpy_fast import clip
+from common.numpy_fast import clip, interp
 from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL
 from common.profiler import Profiler
 from common.params import Params, put_nonblocking
@@ -39,6 +39,10 @@ LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
 EventName = car.CarEvent.EventName
 
+LEAD_AWAY_STATE_OFF = 0
+LEAD_AWAY_STATE_ON = 1
+LEAD_AWAY_STATE_ALERTED = 2
+
 
 class Controls:
   def __init__(self, sm=None, pm=None, can_sock=None):
@@ -160,6 +164,13 @@ class Controls:
     self.sm['dragonConf'].dpAtl = False
     self.sm['dragonConf'].dpCameraOffset = 6
 
+    self.dp_lead_away_alert = params.get('dp_driver_monitor') == b'0' and params.get('dp_steering_monitor') == b'0'
+    self.dp_lead_away_min_speed = 40 # kph
+    self.dp_lead_away_alert_lead_count = 0
+    self.dp_lead_away_alert_nolead_count = 0
+
+    self.dp_lead_away_state = LEAD_AWAY_STATE_OFF
+
   def update_events(self, CS):
     """Compute carEvents from carState"""
 
@@ -258,6 +269,28 @@ class Controls:
       and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
       self.events.add(EventName.noTarget)
 
+    if self.dp_lead_away_alert:
+      current_speed = CS.vEgo * 3.6
+
+      if CS.brakePressed or current_speed < self.dp_lead_away_min_speed or self.dp_lead_away_state == LEAD_AWAY_STATE_ALERTED:
+        self.dp_lead_away_alert_lead_count = 0
+        self.dp_lead_away_alert_nolead_count = 0
+        self.dp_lead_away_state = LEAD_AWAY_STATE_OFF
+
+      if current_speed >= self.dp_lead_away_min_speed:
+        nolead_count = interp(current_speed, [self.dp_lead_away_min_speed, 100], [500, 250])
+        # when car had lead for 5 more secs and lead move away for 3 secs
+        if self.dp_lead_away_state == LEAD_AWAY_STATE_OFF and self.sm['plan'].hasLead:
+          self.dp_lead_away_alert_lead_count += 1
+        elif self.dp_lead_away_state == LEAD_AWAY_STATE_ON and not self.sm['plan'].hasLead:
+          self.dp_lead_away_alert_nolead_count += 1
+
+        if self.dp_lead_away_state == LEAD_AWAY_STATE_OFF and self.dp_lead_away_alert_lead_count >= 300:
+          self.dp_lead_away_state = LEAD_AWAY_STATE_ON
+        elif self.dp_lead_away_state == LEAD_AWAY_STATE_ON and self.dp_lead_away_alert_nolead_count >= nolead_count:
+          self.events.add(EventName.leadCarMoving)
+          self.dp_lead_away_state = LEAD_AWAY_STATE_ALERTED
+
     # dp lead car moving alert
     if self.sm['dragonConf'].dpLeadCarAlert:
       if not self.CP.radarOffCan and self.sm['plan'].hasLead and CS.vEgo <= 0.01 and 0.3 >= abs(self.sm['plan'].vTarget) >= 0: