1
1
import numpy as np
2
2
from opendbc .can .packer import CANPacker
3
- from opendbc .car import Bus , DT_CTRL , apply_driver_steer_torque_limits , common_fault_avoidance , make_tester_present_msg , structs
3
+ from opendbc .car import Bus , DT_CTRL , apply_driver_steer_torque_limits , apply_std_steer_angle_limits , common_fault_avoidance , \
4
+ make_tester_present_msg , structs
4
5
from opendbc .car .common .conversions import Conversions as CV
5
6
from opendbc .car .hyundai import hyundaicanfd , hyundaican
6
7
from opendbc .car .hyundai .carstate import CarState
@@ -59,6 +60,9 @@ def __init__(self, dbc_names, CP, CP_SP):
59
60
self .apply_steer_last = 0
60
61
self .car_fingerprint = CP .carFingerprint
61
62
self .last_button_frame = 0
63
+ self .apply_angle_last = 0
64
+ self .lkas_max_torque = 0
65
+ self .driver_applied_torque_reducer = 0
62
66
63
67
def update (self , CC , CC_SP , CS , now_nanos ):
64
68
EsccCarController .update (self , CS )
@@ -75,8 +79,56 @@ def update(self, CC, CC_SP, CS, now_nanos):
75
79
self .angle_limit_counter , MAX_ANGLE_FRAMES ,
76
80
MAX_ANGLE_CONSECUTIVE_FRAMES )
77
81
82
+ apply_angle = apply_std_steer_angle_limits (actuators .steeringAngleDeg , self .apply_angle_last , CS .out .vEgoRaw , self .params )
83
+
84
+ # Figure out torque value. On Stock when LKAS is active, this is variable,
85
+ # but 0 when LKAS is not actively steering, so because we're "tricking" ADAS
86
+ # into thinking LKAS is always active, we need to make sure we're applying
87
+ # torque when the driver is not actively steering. The default value chosen
88
+ # here is based on observations of the stock LKAS system when it's engaged
89
+ # CS.out.steeringPressed and steeringTorque are based on the
90
+ # STEERING_COL_TORQUE value
91
+ MAX_TORQUE = 240
92
+ # Interpolate a percent to apply to max torque based on vEgo value, which is
93
+ # the "best estimate of speed". This means that under 20 (units?) we will
94
+ # apply less torque, and over 20 we will apply the full calculated torque.
95
+ ego_weight = np .interp (CS .out .vEgo , [0 , 5 , 10 , 20 ], [0.2 , 0.3 , 0.5 , 1.0 ])
96
+
97
+ # Track if and how long the driver has been applying torque and create a
98
+ # value to reduce the max torque applied. This block will cause the
99
+ # `driver_applied_torque_reducer` to settle to value between 30 and 150.
100
+ # While the driver applies torque the value will decrease to 30, and while
101
+ # the driver is not applying torque the value will increase to 150.
102
+ if abs (CS .out .steeringTorque ) > 200 :
103
+ # If the driver is applying some torque manually, reduce the value down to 30 (the min)
104
+ self .driver_applied_torque_reducer -= 1
105
+ if self .driver_applied_torque_reducer < 30 :
106
+ self .driver_applied_torque_reducer = 30
107
+ else :
108
+ # While the driver is not applying torque, increase the value up to 150 (the max)
109
+ self .driver_applied_torque_reducer += 1
110
+ if self .driver_applied_torque_reducer > 150 :
111
+ self .driver_applied_torque_reducer = 150
112
+
113
+ if self .driver_applied_torque_reducer < 150 :
114
+ # If the driver has just started applying torque, the reducer value will
115
+ # be around 150 so we won't reduce the max torque much. As the driver
116
+ # continues to apply torque, the reducer value will decrease to 30, so we
117
+ # will reduce the max torque more to fight them less (at this level we'll
118
+ # be doing 1/5 of the torque)
119
+ self .lkas_max_torque = int (round (MAX_TORQUE * ego_weight * (self .driver_applied_torque_reducer / 150 )))
120
+ else :
121
+ # A torque reducer value of 150 means the driver has not been applying
122
+ # torque for a while, so we will apply the full max torque value, adjusted
123
+ # by the ego weight (based on driving speed)
124
+ self .lkas_max_torque = MAX_TORQUE * ego_weight
125
+
78
126
if not CC .latActive :
127
+ apply_angle = CS .out .steeringAngleDeg
79
128
apply_steer = 0
129
+ self .lkas_max_torque = 0
130
+
131
+ self .apply_angle_last = apply_angle
80
132
81
133
# Hold torque with induced temporary fault when cutting the actuation bit
82
134
torque_fault = CC .latActive and not apply_steer_req
@@ -115,15 +167,18 @@ def update(self, CC, CC_SP, CS, now_nanos):
115
167
hda2_long = hda2 and self .CP .openpilotLongitudinalControl
116
168
117
169
# steering control
118
- can_sends .extend (hyundaicanfd .create_steering_messages (self .packer , self .CP , self .CAN , CC .enabled , apply_steer_req , apply_steer , self .lkas_icon ))
170
+ can_sends .extend (hyundaicanfd .create_steering_messages (self .packer , self .CP , self .CAN , CC .enabled ,
171
+ apply_steer_req , CS .out .steeringPressed ,
172
+ apply_steer , self .lkas_icon , apply_angle , self .lkas_max_torque ))
119
173
120
174
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
121
175
if self .frame % 5 == 0 and hda2 :
122
176
can_sends .append (hyundaicanfd .create_suppress_lfa (self .packer , self .CAN , CS .hda2_lfa_block_msg ,
123
177
self .CP .flags & HyundaiFlags .CANFD_HDA2_ALT_STEERING ))
124
178
125
179
# LFA and HDA icons
126
- if self .frame % 5 == 0 and (not hda2 or hda2_long ):
180
+ update_lfahda_icons = (not hda2 or hda2_long ) or self .CP .flags & HyundaiFlags .ANGLE_CONTROL
181
+ if self .frame % 5 == 0 and update_lfahda_icons :
127
182
can_sends .append (hyundaicanfd .create_lfahda_cluster (self .packer , self .CAN , CC .enabled , self .lfa_icon ))
128
183
129
184
# blinkers
@@ -175,6 +230,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
175
230
new_actuators = actuators .as_builder ()
176
231
new_actuators .steer = apply_steer / self .params .STEER_MAX
177
232
new_actuators .steerOutputCan = apply_steer
233
+ new_actuators .steeringAngleDeg = apply_angle
178
234
new_actuators .accel = accel
179
235
180
236
self .frame += 1
0 commit comments