3
3
from opendbc .car import Bus , DT_CTRL , apply_driver_steer_torque_limits , common_fault_avoidance , make_tester_present_msg , structs
4
4
from opendbc .car .common .conversions import Conversions as CV
5
5
from opendbc .car .hyundai import hyundaicanfd , hyundaican
6
- from opendbc .car .hyundai .carstate import CarState
7
6
from opendbc .car .hyundai .hyundaicanfd import CanBus
8
7
from opendbc .car .hyundai .values import HyundaiFlags , Buttons , CarControllerParams , CAR
9
8
from opendbc .car .interfaces import CarControllerBase
@@ -72,6 +71,7 @@ def update(self, CC, CS, now_nanos):
72
71
apply_torque = 0
73
72
74
73
# Hold torque with induced temporary fault when cutting the actuation bit
74
+ # FIXME: we don't use this with CAN FD?
75
75
torque_fault = CC .latActive and not apply_steer_req
76
76
77
77
self .apply_torque_last = apply_torque
@@ -81,10 +81,6 @@ def update(self, CC, CS, now_nanos):
81
81
stopping = actuators .longControlState == LongCtrlState .stopping
82
82
set_speed_in_units = hud_control .setSpeed * (CV .MS_TO_KPH if CS .is_metric else CV .MS_TO_MPH )
83
83
84
- # HUD messages
85
- sys_warning , sys_state , left_lane_warning , right_lane_warning = process_hud_alert (CC .enabled , self .car_fingerprint ,
86
- hud_control )
87
-
88
84
can_sends = []
89
85
90
86
# *** common hyundai stuff ***
@@ -101,70 +97,13 @@ def update(self, CC, CS, now_nanos):
101
97
if self .CP .flags & HyundaiFlags .ENABLE_BLINKERS :
102
98
can_sends .append (make_tester_present_msg (0x7b1 , self .CAN .ECAN , suppress_response = True ))
103
99
104
- # CAN- FD platforms
100
+ # *** CAN/CAN FD specific ***
105
101
if self .CP .flags & HyundaiFlags .CANFD :
106
- lka_steering = self .CP .flags & HyundaiFlags .CANFD_LKA_STEERING
107
- lka_steering_long = lka_steering and self .CP .openpilotLongitudinalControl
108
-
109
- # steering control
110
- can_sends .extend (hyundaicanfd .create_steering_messages (self .packer , self .CP , self .CAN , CC .enabled , apply_steer_req , apply_torque ))
111
-
112
- # prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU
113
- if self .frame % 5 == 0 and lka_steering :
114
- can_sends .append (hyundaicanfd .create_suppress_lfa (self .packer , self .CAN , CS .lfa_block_msg ,
115
- self .CP .flags & HyundaiFlags .CANFD_LKA_STEERING_ALT ))
116
-
117
- # LFA and HDA icons
118
- if self .frame % 5 == 0 and (not lka_steering or lka_steering_long ):
119
- if self .CP .flags & HyundaiFlags .CCNC :
120
- can_sends .extend (hyundaicanfd .create_ccnc (self .packer , self .CAN , self .CP , CC , CS ))
121
- else :
122
- can_sends .append (hyundaicanfd .create_lfahda_cluster (self .packer , self .CAN , CC .enabled ))
123
-
124
- # blinkers
125
- if lka_steering and self .CP .flags & HyundaiFlags .ENABLE_BLINKERS :
126
- can_sends .extend (hyundaicanfd .create_spas_messages (self .packer , self .CAN , self .frame , CC .leftBlinker , CC .rightBlinker ))
127
-
128
- if self .CP .openpilotLongitudinalControl :
129
- if lka_steering :
130
- can_sends .extend (hyundaicanfd .create_adrv_messages (self .packer , self .CAN , self .frame ))
131
- elif not self .CP .flags & HyundaiFlags .CCNC :
132
- can_sends .extend (hyundaicanfd .create_fca_warning_light (self .packer , self .CAN , self .frame ))
133
- if self .frame % 2 == 0 :
134
- can_sends .append (hyundaicanfd .create_acc_control (self .packer , self .CAN , CC .enabled , self .accel_last , accel , stopping , CC .cruiseControl .override ,
135
- set_speed_in_units , hud_control , CS .cruise_info if self .CP .flags & HyundaiFlags .CCNC else None ))
136
- self .accel_last = accel
137
- else :
138
- # button presses
139
- can_sends .extend (self .create_button_messages (CC , CS , use_clu11 = False ))
102
+ can_sends .extend (self .create_canfd_msgs (apply_steer_req , apply_torque , set_speed_in_units , accel ,
103
+ stopping , hud_control , CS , CC ))
140
104
else :
141
- can_sends .append (hyundaican .create_lkas11 (self .packer , self .frame , self .CP , apply_torque , apply_steer_req ,
142
- torque_fault , CS .lkas11 , sys_warning , sys_state , CC .enabled ,
143
- hud_control .leftLaneVisible , hud_control .rightLaneVisible ,
144
- left_lane_warning , right_lane_warning ))
145
-
146
- if not self .CP .openpilotLongitudinalControl :
147
- can_sends .extend (self .create_button_messages (CC , CS , use_clu11 = True ))
148
-
149
- if self .frame % 2 == 0 and self .CP .openpilotLongitudinalControl :
150
- # TODO: unclear if this is needed
151
- jerk = 3.0 if actuators .longControlState == LongCtrlState .pid else 1.0
152
- use_fca = self .CP .flags & HyundaiFlags .USE_FCA .value
153
- can_sends .extend (hyundaican .create_acc_commands (self .packer , CC .enabled , accel , jerk , int (self .frame / 2 ),
154
- hud_control , set_speed_in_units , stopping ,
155
- CC .cruiseControl .override , use_fca , self .CP ))
156
-
157
- # 20 Hz LFA MFA message
158
- if self .frame % 5 == 0 and self .CP .flags & HyundaiFlags .SEND_LFA .value :
159
- can_sends .append (hyundaican .create_lfahda_mfc (self .packer , CC .enabled ))
160
-
161
- # 5 Hz ACC options
162
- if self .frame % 20 == 0 and self .CP .openpilotLongitudinalControl :
163
- can_sends .extend (hyundaican .create_acc_opt (self .packer , self .CP ))
164
-
165
- # 2 Hz front radar options
166
- if self .frame % 50 == 0 and self .CP .openpilotLongitudinalControl :
167
- can_sends .append (hyundaican .create_frt_radar_opt (self .packer ))
105
+ can_sends .extend (self .create_can_msgs (apply_steer_req , apply_torque , torque_fault , set_speed_in_units , accel ,
106
+ stopping , hud_control , actuators , CS , CC ))
168
107
169
108
new_actuators = actuators .as_builder ()
170
109
new_actuators .torque = apply_torque / self .params .STEER_MAX
@@ -174,9 +113,20 @@ def update(self, CC, CS, now_nanos):
174
113
self .frame += 1
175
114
return new_actuators , can_sends
176
115
177
- def create_button_messages (self , CC : structs . CarControl , CS : CarState , use_clu11 : bool ):
116
+ def create_can_msgs (self , apply_steer_req , apply_torque , torque_fault , set_speed_in_units , accel , stopping , hud_control , actuators , CS , CC ):
178
117
can_sends = []
179
- if use_clu11 :
118
+
119
+ # HUD messages
120
+ sys_warning , sys_state , left_lane_warning , right_lane_warning = process_hud_alert (CC .enabled , self .car_fingerprint ,
121
+ hud_control )
122
+
123
+ can_sends .append (hyundaican .create_lkas11 (self .packer , self .frame , self .CP , apply_torque , apply_steer_req ,
124
+ torque_fault , CS .lkas11 , sys_warning , sys_state , CC .enabled ,
125
+ hud_control .leftLaneVisible , hud_control .rightLaneVisible ,
126
+ left_lane_warning , right_lane_warning ))
127
+
128
+ # Button messages
129
+ if not self .CP .openpilotLongitudinalControl :
180
130
if CC .cruiseControl .cancel :
181
131
can_sends .append (hyundaican .create_clu11 (self .packer , self .frame , CS .clu11 , Buttons .CANCEL , self .CP ))
182
132
elif CC .cruiseControl .resume :
@@ -186,7 +136,65 @@ def create_button_messages(self, CC: structs.CarControl, CS: CarState, use_clu11
186
136
can_sends .extend ([hyundaican .create_clu11 (self .packer , self .frame , CS .clu11 , Buttons .RES_ACCEL , self .CP )] * 25 )
187
137
if (self .frame - self .last_button_frame ) * DT_CTRL >= 0.15 :
188
138
self .last_button_frame = self .frame
139
+
140
+ if self .frame % 2 == 0 and self .CP .openpilotLongitudinalControl :
141
+ # TODO: unclear if this is needed
142
+ jerk = 3.0 if actuators .longControlState == LongCtrlState .pid else 1.0
143
+ use_fca = self .CP .flags & HyundaiFlags .USE_FCA .value
144
+ can_sends .extend (hyundaican .create_acc_commands (self .packer , CC .enabled , accel , jerk , int (self .frame / 2 ),
145
+ hud_control , set_speed_in_units , stopping ,
146
+ CC .cruiseControl .override , use_fca , self .CP ))
147
+
148
+ # 20 Hz LFA MFA message
149
+ if self .frame % 5 == 0 and self .CP .flags & HyundaiFlags .SEND_LFA .value :
150
+ can_sends .append (hyundaican .create_lfahda_mfc (self .packer , CC .enabled ))
151
+
152
+ # 5 Hz ACC options
153
+ if self .frame % 20 == 0 and self .CP .openpilotLongitudinalControl :
154
+ can_sends .extend (hyundaican .create_acc_opt (self .packer , self .CP ))
155
+
156
+ # 2 Hz front radar options
157
+ if self .frame % 50 == 0 and self .CP .openpilotLongitudinalControl :
158
+ can_sends .append (hyundaican .create_frt_radar_opt (self .packer ))
159
+
160
+ return can_sends
161
+
162
+ def create_canfd_msgs (self , apply_steer_req , apply_torque , set_speed_in_units , accel , stopping , hud_control , CS , CC ):
163
+ can_sends = []
164
+
165
+ lka_steering = self .CP .flags & HyundaiFlags .CANFD_LKA_STEERING
166
+ lka_steering_long = lka_steering and self .CP .openpilotLongitudinalControl
167
+
168
+ # steering control
169
+ can_sends .extend (hyundaicanfd .create_steering_messages (self .packer , self .CP , self .CAN , CC .enabled , apply_steer_req , apply_torque ))
170
+
171
+ # prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU
172
+ if self .frame % 5 == 0 and lka_steering :
173
+ can_sends .append (hyundaicanfd .create_suppress_lfa (self .packer , self .CAN , CS .lfa_block_msg ,
174
+ self .CP .flags & HyundaiFlags .CANFD_LKA_STEERING_ALT ))
175
+
176
+ # LFA and HDA icons
177
+ if self .frame % 5 == 0 and (not lka_steering or lka_steering_long ):
178
+ if self .CP .flags & HyundaiFlags .CCNC :
179
+ can_sends .extend (hyundaicanfd .create_ccnc (self .packer , self .CAN , self .CP , CC , CS ))
180
+ else :
181
+ can_sends .append (hyundaicanfd .create_lfahda_cluster (self .packer , self .CAN , CC .enabled ))
182
+
183
+ # blinkers
184
+ if lka_steering and self .CP .flags & HyundaiFlags .ENABLE_BLINKERS :
185
+ can_sends .extend (hyundaicanfd .create_spas_messages (self .packer , self .CAN , CC .leftBlinker , CC .rightBlinker ))
186
+
187
+ if self .CP .openpilotLongitudinalControl :
188
+ if lka_steering :
189
+ can_sends .extend (hyundaicanfd .create_adrv_messages (self .packer , self .CAN , self .frame ))
190
+ elif not self .CP .flags & HyundaiFlags .CCNC :
191
+ can_sends .extend (hyundaicanfd .create_fca_warning_light (self .packer , self .CAN , self .frame ))
192
+ if self .frame % 2 == 0 :
193
+ can_sends .append (hyundaicanfd .create_acc_control (self .packer , self .CAN , CC .enabled , self .accel_last , accel , stopping , CC .cruiseControl .override ,
194
+ set_speed_in_units , hud_control , CS .cruise_info if self .CP .flags & HyundaiFlags .CCNC else None ))
195
+ self .accel_last = accel
189
196
else :
197
+ # button presses
190
198
if (self .frame - self .last_button_frame ) * DT_CTRL > 0.25 :
191
199
# cruise cancel
192
200
if CC .cruiseControl .cancel :
0 commit comments