From da5dc7693edc12dd281ce365a655dab56fb62494 Mon Sep 17 00:00:00 2001
From: Andre Volmensky <andre@volmens.ky>
Date: Tue, 21 Jul 2020 08:30:40 +0900
Subject: [PATCH] Nissan: Tweaking steeringPressed/LKAS_MAX_TORQUE (#1865)

* Tweaking steeringPressed/LKAS_MAX_TORQUE

* Update ref commit
---
 selfdrive/car/nissan/carcontroller.py    | 8 ++++++--
 selfdrive/car/nissan/carstate.py         | 7 ++++++-
 selfdrive/car/nissan/values.py           | 2 +-
 selfdrive/test/process_replay/ref_commit | 2 +-
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py
index 5d988cb935..5838d67ae3 100644
--- a/selfdrive/car/nissan/carcontroller.py
+++ b/selfdrive/car/nissan/carcontroller.py
@@ -2,7 +2,7 @@ from cereal import car
 from common.numpy_fast import clip, interp
 from selfdrive.car.nissan import nissancan
 from opendbc.can.packer import CANPacker
-from selfdrive.car.nissan.values import CAR
+from selfdrive.car.nissan.values import CAR, STEER_THRESHOLD
 
 # Steer angle limits
 ANGLE_DELTA_BP = [0., 5., 15.]
@@ -53,7 +53,11 @@ class CarController():
       else:
         # Scale max torque based on how much torque the driver is applying to the wheel
         self.lkas_max_torque = max(
-            0, LKAS_MAX_TORQUE - 0.4 * abs(CS.out.steeringTorque))
+          # Scale max torque down to half LKAX_MAX_TORQUE as a minimum
+          LKAS_MAX_TORQUE * 0.5,
+          # Start scaling torque at STEER_THRESHOLD
+          LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - STEER_THRESHOLD)
+        )
 
     else:
       apply_angle = CS.out.steeringAngle
diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py
index d43db51b6b..f2076183d9 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -1,4 +1,5 @@
 import copy
+from collections import deque
 from cereal import car
 from opendbc.can.can_define import CANDefine
 from selfdrive.car.interfaces import CarStateBase
@@ -6,12 +7,14 @@ from selfdrive.config import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.nissan.values import CAR, DBC, STEER_THRESHOLD
 
+TORQUE_SAMPLES = 12
 
 class CarState(CarStateBase):
   def __init__(self, CP):
     super().__init__(CP)
     can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
 
+    self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES)
     self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
 
   def update(self, cp, cp_adas, cp_cam):
@@ -60,7 +63,9 @@ class CarState(CarStateBase):
       ret.cruiseState.speed = speed * conversion
 
     ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
-    ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
+    self.steeringTorqueSamples.append(ret.steeringTorque)
+    # Filtering driver torque to prevent steeringPressed false positives
+    ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > STEER_THRESHOLD)
 
     ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
 
diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py
index 4271b7f0e5..0630a8cb94 100644
--- a/selfdrive/car/nissan/values.py
+++ b/selfdrive/car/nissan/values.py
@@ -2,7 +2,7 @@
 
 from selfdrive.car import dbc_dict
 
-STEER_THRESHOLD = 1.75
+STEER_THRESHOLD = 1.0
 
 class CAR:
   XTRAIL = "NISSAN X-TRAIL 2017"
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 5ba0101a8d..39ccd58aae 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-d0b7760731f1e310c7770dafee215ed2dee0d92d
\ No newline at end of file
+e3ccebfa372cc2e85b742e86567899beb1233ced