-
Notifications
You must be signed in to change notification settings - Fork 9.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* hkg * Cleanup * Update readme * more fixes and cleanup * Old genesis * Typoe * Test car models * Update comment * Fix brake pressed * Update release notes * Fix vEgo * Add sonata * Add sonata values * Temporarily remove doors check. It doesn't work * Sonata uses crc8 * Fix tests * Changes for LFA * Add comment * Does this improve the hud? * Proper signal name * Force the right value * some ui stuff * more comments * Show lane lines on sonata * cleanup dash * fix last ui issues * Fix doors * update CI Co-authored-by: Comma Device <[email protected]>
- Loading branch information
Showing
13 changed files
with
515 additions
and
179 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Submodule panda
updated
35 files
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,47 +1,103 @@ | ||
| from cereal import car | ||
| from selfdrive.car import apply_std_steer_torque_limits | ||
| from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11 | ||
| from selfdrive.car.hyundai.values import Buttons, SteerLimitParams | ||
| from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa | ||
| from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR | ||
| from opendbc.can.packer import CANPacker | ||
|
|
||
| VisualAlert = car.CarControl.HUDControl.VisualAlert | ||
|
|
||
|
|
||
| def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, | ||
| right_lane, left_lane_depart, right_lane_depart): | ||
| sys_warning = (visual_alert == VisualAlert.steerRequired) | ||
|
|
||
| # initialize to no line visible | ||
| sys_state = 1 | ||
| if left_lane and right_lane or sys_warning: #HUD alert only display when LKAS status is active | ||
| if enabled or sys_warning: | ||
| sys_state = 3 | ||
| else: | ||
| sys_state = 4 | ||
| elif left_lane: | ||
| sys_state = 5 | ||
| elif right_lane: | ||
| sys_state = 6 | ||
|
|
||
| # initialize to no warnings | ||
| left_lane_warning = 0 | ||
| right_lane_warning = 0 | ||
| if left_lane_depart: | ||
| left_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 | ||
| if right_lane_depart: | ||
| right_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 | ||
|
|
||
| return sys_warning, sys_state, left_lane_warning, right_lane_warning | ||
|
|
||
|
|
||
| class CarController(): | ||
| def __init__(self, dbc_name, CP, VM): | ||
| self.apply_steer_last = 0 | ||
| self.car_fingerprint = CP.carFingerprint | ||
| self.lkas11_cnt = 0 | ||
| self.cnt = 0 | ||
| self.last_resume_cnt = 0 | ||
| self.packer = CANPacker(dbc_name) | ||
| self.steer_rate_limited = False | ||
| self.resume_cnt = 0 | ||
| self.last_resume_frame = 0 | ||
| self.last_lead_distance = 0 | ||
|
|
||
| def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): | ||
|
|
||
| ### Steering Torque | ||
| def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, | ||
| left_lane, right_lane, left_lane_depart, right_lane_depart): | ||
| # Steering Torque | ||
| new_steer = actuators.steer * SteerLimitParams.STEER_MAX | ||
| apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) | ||
| self.steer_rate_limited = new_steer != apply_steer | ||
|
|
||
| if not enabled: | ||
| apply_steer = 0 | ||
| # disable if steer angle reach 90 deg, otherwise mdps fault in some models | ||
| 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: | ||
| lkas_active = 0 | ||
|
|
||
| steer_req = 1 if enabled else 0 | ||
| if not lkas_active: | ||
| apply_steer = 0 | ||
|
|
||
| self.apply_steer_last = apply_steer | ||
|
|
||
| sys_warning, sys_state, left_lane_warning, right_lane_warning =\ | ||
| process_hud_alert(enabled, self.car_fingerprint, visual_alert, | ||
| left_lane, right_lane, left_lane_depart, right_lane_depart) | ||
|
|
||
| can_sends = [] | ||
| can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, | ||
| CS.lkas11, sys_warning, sys_state, enabled, | ||
| left_lane, right_lane, | ||
| left_lane_warning, right_lane_warning)) | ||
|
|
||
| self.lkas11_cnt = self.cnt % 0x10 | ||
| self.clu11_cnt = self.cnt % 0x10 | ||
| if pcm_cancel_cmd: | ||
| can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) | ||
|
|
||
| can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, | ||
| enabled, CS.lkas11, hud_alert, keep_stock=True)) | ||
| elif CS.out.cruiseState.standstill: | ||
| # run only first time when the car stopped | ||
| if self.last_lead_distance == 0: | ||
| # get the lead distance from the Radar | ||
| self.last_lead_distance = CS.lead_distance | ||
| self.resume_cnt = 0 | ||
| # when lead car starts moving, create 6 RES msgs | ||
| elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5: | ||
| can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) | ||
| self.resume_cnt += 1 | ||
| # interval after 6 msgs | ||
| if self.resume_cnt > 5: | ||
| self.last_resume_frame = frame | ||
| self.clu11_cnt = 0 | ||
| # reset lead distnce after the car starts moving | ||
| elif self.last_lead_distance != 0: | ||
| self.last_lead_distance = 0 | ||
|
|
||
| if pcm_cancel_cmd: | ||
| can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) | ||
| elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5: | ||
| self.last_resume_cnt = self.cnt | ||
| can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) | ||
|
|
||
| self.cnt += 1 | ||
| # 20 Hz LFA MFA message | ||
| if frame % 5 == 0 and self.car_fingerprint == CAR.SONATA: | ||
| can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) | ||
|
|
||
|
|
||
| return can_sends |
Oops, something went wrong.