forked from jeankalud/openpilot
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Blinker + LDW improvements (commaai#131)
blinker extension: - now only triggers when tapping - now also works when OP is not engaged - extension configuration changed to blink count instead of seconds other: - implemented real blinker states (thanks BogGyver!) - refactored blinker and blinker stalk related stuff - ldw won't trigger while blinking by tapping anymore and numb time starts after blinking ends - removed eon on-screen ldw messages
- Loading branch information
Showing
9 changed files
with
597 additions
and
653 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
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,85 +1,32 @@ | ||
| #human steer override module | ||
| import time | ||
|
|
||
| TIME_REMEMBER_LAST_BLINKER = 50 # in 0.01 seconds | ||
|
|
||
| def _current_time_millis(): | ||
| return int(round(time.time() * 1000)) | ||
|
|
||
| class HSOController(): | ||
| def __init__(self,carcontroller): | ||
| self.human_control = False | ||
| self.frame_humanSteered = 0 | ||
| self.turn_signal_needed = 0 # send 1 for left, 2 for right 0 for not needed | ||
| self.last_blinker_on = 0 | ||
| self.blinker_on = 0 | ||
| self.frame_blinker_on = 0 | ||
| self.last_human_blinker_on = 0 | ||
| self.frame_human_blinker_on = 0 | ||
| self.HSO_frame_blinker_on = 0 | ||
|
|
||
| def __init__(self, carcontroller): | ||
| self.human_control = False | ||
| self.frame_humanSteered = 0 | ||
|
|
||
| def update_stat(self,CC,CS,enabled,actuators,frame): | ||
| human_control = False | ||
| self.last_blinker_on = self.blinker_on | ||
| if CS.right_blinker_on: | ||
| self.blinker_on = 2 | ||
| self.frame_human_blinker_on = frame | ||
| elif CS.left_blinker_on: | ||
| self.blinker_on = 1 | ||
| self.frame_human_blinker_on = frame | ||
| if (self.last_blinker_on == 0) and (self.blinker_on > 0) and (CC.ALCA.laneChange_enabled <= 1): | ||
| self.HSO_frame_blinker_on = frame | ||
| if (self.last_blinker_on == 0) and (self.blinker_on == 0): | ||
| self.HSO_frame_blinker_on = 0 | ||
| if self.blinker_on > 0: | ||
| self.frame_blinker_on = frame | ||
| self.last_human_blinker_on = self.blinker_on | ||
| else: | ||
| if frame - self.frame_blinker_on < TIME_REMEMBER_LAST_BLINKER: | ||
| self.blinker_on = self.last_human_blinker_on | ||
| else: | ||
| self.last_human_blinker_on = 0 | ||
| if frame - self.frame_human_blinker_on > 100 * CS.hsoBlinkerExtender: | ||
| self.blinker_on = 0 | ||
| self.turn_signal_needed = 0 | ||
| self.last_blinker_on = 0 | ||
| def update_stat(self, CC, CS, enabled, actuators, frame): | ||
| human_control = False | ||
|
|
||
| if CS.enableHSO and enabled: | ||
| #if steering but not by ALCA | ||
| if (CS.right_blinker_on or CS.left_blinker_on) and (CC.ALCA.laneChange_enabled <= 1):# and (self.last_blinker_on != self.blinker_on): | ||
| if CS.enableHSO and enabled: | ||
| # if steering but not by ALCA | ||
| if CS.steer_override > 0: | ||
| self.frame_humanSteered = frame | ||
| elif CC.ALCA.laneChange_enabled <= 1: | ||
| if CS.turn_signal_stalk_state > 0 or frame <= (CC.blinker_on_frame_start + int(100 * CS.hsoNumbPeriod)): # stalk locked or blinker within numbPeriod | ||
| self.frame_humanSteered = frame | ||
| elif frame - self.frame_humanSteered < 50: # Need more human testing of handoff timing | ||
| # Find steering difference between visiond model and human (no need to do every frame if we run out of CPU): | ||
| apply_steer = int(-actuators.steerAngle) | ||
| angle_diff = abs(apply_steer - CS.angle_steers) | ||
| if angle_diff > 15.: | ||
| self.frame_humanSteered = frame | ||
| if (CS.steer_override > 0): # and (frame - self.frame_humanSteered > 50): #let's try with human touch only | ||
| self.frame_humanSteered = frame | ||
| else: | ||
| if (CC.ALCA.laneChange_enabled <= 1) and (frame - self.frame_humanSteered < 50): # Need more human testing of handoff timing | ||
| # Find steering difference between visiond model and human (no need to do every frame if we run out of CPU): | ||
| steer_current=(CS.angle_steers) # Formula to convert current steering angle to match apply_steer calculated number | ||
| apply_steer = int(-actuators.steerAngle) | ||
| angle = abs(apply_steer-steer_current) | ||
| if frame < (self.HSO_frame_blinker_on + int(100 * CS.hsoNumbPeriod)) or angle > 15.: | ||
| self.frame_humanSteered = frame | ||
| if enabled: | ||
| if CS.enableHSO: | ||
| if (frame - self.frame_humanSteered < 50): | ||
| human_control = True | ||
| CS.UE.custom_alert_message(3,"Manual Steering Enabled",51,4) | ||
| #if human control and turn signal on, and previous turn signal was not on or was different, adjust | ||
| #same direction again cancels signal | ||
| if human_control: | ||
| self.turn_signal_needed = self.blinker_on | ||
| #if no human control, no turn signal needed | ||
| if not human_control: | ||
| self.turn_signal_needed = 0 | ||
| self.last_blinker_on = 0 | ||
| self.blinker_on = 0 | ||
| if (not human_control) and (CC.DAS_219_lcTempUnavailableSpeed == 1): | ||
| CC.DAS_219_lcTempUnavailableSpeed = 0 | ||
| CC.warningNeeded = 1 | ||
| self.turn_signal_needed = 0 | ||
| if (CC.ALCA.laneChange_enabled > 1): | ||
| self.turn_signal_needed = 0 | ||
| self.blinker_on = 0 | ||
| self.last_blinker_on = 0 | ||
| self.human_control = human_control | ||
| return human_control and enabled, self.turn_signal_needed | ||
| if frame - self.frame_humanSteered < 50: | ||
| human_control = True | ||
| CS.UE.custom_alert_message(3, "Manual Steering Enabled", 51, 4) | ||
|
|
||
| if (not human_control) and (CC.DAS_219_lcTempUnavailableSpeed == 1): | ||
| CC.DAS_219_lcTempUnavailableSpeed = 0 | ||
| CC.warningNeeded = 1 | ||
| self.human_control = human_control | ||
| return human_control and enabled |
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
Oops, something went wrong.