diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc index 60642f43c17ed1..482ffdc4e35e0c 100644 --- a/opendbc/tesla_can.dbc +++ b/opendbc/tesla_can.dbc @@ -172,22 +172,26 @@ BO_ 532 EPB_epasControl: 3 EPB SG_ EPB_epasEACAllow : 2|3@0+ (1,0) [4|7] "" NEO,EPAS BO_ 792 GTW_carState: 8 GTW - SG_ BOOT_STATE : 47|2@0+ (1,0) [-1|4] "" NEO - SG_ CERRD : 7|1@0+ (1,0) [-1|2] "" NEO - SG_ DAY : 36|5@0+ (1,0) [2|31] "" NEO - SG_ DOOR_STATE_FL : 13|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_FR : 15|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_FrontTrunk : 51|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_RL : 23|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_RR : 30|2@0+ (1,0) [-1|4] "" NEO - SG_ GTW_updateInProgress : 49|2@0+ (1,0) [-1|4] "" NEO - SG_ Hour : 28|5@0+ (1,0) [0|29] "h" NEO - SG_ MCU_factoryMode : 52|1@0+ (1,0) [-1|2] "" NEO - SG_ MCU_transportModeOn : 53|1@0+ (1,0) [1|1] "" NEO - SG_ MINUTE : 45|6@0+ (1,0) [0|61] "min" NEO - SG_ MONTH : 11|4@0+ (1,0) [0|14] "Month" NEO - SG_ SECOND : 21|6@0+ (1,0) [0|61] "s" NEO - SG_ YEAR : 6|7@0+ (1,2000) [2000|2125] "Year" NEO + SG_ YEAR : 0|7@1+ (1,2000) [2000|2127] "Year" NEO + SG_ CERRD : 7|1@1+ (1,0) [0|1] "" NEO + SG_ MONTH : 8|4@1+ (1,0) [1|12] "Month" NEO + SG_ DOOR_STATE_FL : 12|2@1+ (1,0) [0|3] "" NEO + SG_ DOOR_STATE_FR : 14|2@1+ (1,0) [0|3] "" NEO + SG_ SECOND : 16|6@1+ (1,0) [0|59] "s" NEO + SG_ DOOR_STATE_RL : 22|2@1+ (1,0) [0|3] "" NEO + SG_ Hour : 24|5@1+ (1,0) [0|23] "h" NEO + SG_ DOOR_STATE_RR : 29|2@1+ (1,0) [0|3] "" NEO + SG_ DAY : 32|5@1+ (1,0) [0|31] "" NEO + SG_ MINUTE : 40|6@1+ (1,0) [0|59] "min" NEO + SG_ BOOT_STATE : 46|2@1+ (1,0) [0|3] "" NEO + SG_ GTW_updateInProgress : 48|2@1+ (1,0) [0|3] "" NEO + SG_ DOOR_STATE_FrontTrunk : 50|2@1+ (1,0) [0|3] "" NEO + SG_ MCU_factoryMode : 52|1@1+ (1,0) [0|1] "" NEO + SG_ MCU_transportModeOn : 53|1@0+ (1,0) [0|1] "" NEO + SG_ BC_headLightLStatus : 55|2@0+ (1,0) [0|3] "" NEO + SG_ BC_headLightRStatus : 57|2@0+ (1,0) [0|3] "" NEO + SG_ BC_indicatorLStatus : 59|2@0+ (1,0) [0|3] "" NEO + SG_ BC_indicatorRStatus : 61|2@0+ (1,0) [0|3] "" NEO BO_ 872 DI_state: 8 DI SG_ DI_systemState : 0|3@1+ (1,0) [0|0] "" NEO diff --git a/opendbc/tesla_can1916.dbc b/opendbc/tesla_can1916.dbc index 97c42ab9de04ab..70b34dca776c4c 100644 --- a/opendbc/tesla_can1916.dbc +++ b/opendbc/tesla_can1916.dbc @@ -172,22 +172,26 @@ BO_ 532 EPB_epasControl: 3 EPB SG_ EPB_epasEACAllow : 2|3@0+ (1,0) [4|7] "" NEO,EPAS BO_ 792 GTW_carState: 8 GTW - SG_ BOOT_STATE : 47|2@0+ (1,0) [-1|4] "" NEO - SG_ CERRD : 7|1@0+ (1,0) [-1|2] "" NEO - SG_ DAY : 36|5@0+ (1,0) [2|31] "" NEO - SG_ DOOR_STATE_FL : 13|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_FR : 15|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_FrontTrunk : 51|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_RL : 23|2@0+ (1,0) [-1|4] "" NEO - SG_ DOOR_STATE_RR : 30|2@0+ (1,0) [-1|4] "" NEO - SG_ GTW_updateInProgress : 49|2@0+ (1,0) [-1|4] "" NEO - SG_ Hour : 28|5@0+ (1,0) [0|29] "h" NEO - SG_ MCU_factoryMode : 52|1@0+ (1,0) [-1|2] "" NEO - SG_ MCU_transportModeOn : 53|1@0+ (1,0) [1|1] "" NEO - SG_ MINUTE : 45|6@0+ (1,0) [0|61] "min" NEO - SG_ MONTH : 11|4@0+ (1,0) [0|14] "Month" NEO - SG_ SECOND : 21|6@0+ (1,0) [0|61] "s" NEO - SG_ YEAR : 6|7@0+ (1,2000) [2000|2125] "Year" NEO + SG_ YEAR : 0|7@1+ (1,2000) [2000|2127] "Year" NEO + SG_ CERRD : 7|1@1+ (1,0) [0|1] "" NEO + SG_ MONTH : 8|4@1+ (1,0) [1|12] "Month" NEO + SG_ DOOR_STATE_FL : 12|2@1+ (1,0) [0|3] "" NEO + SG_ DOOR_STATE_FR : 14|2@1+ (1,0) [0|3] "" NEO + SG_ SECOND : 16|6@1+ (1,0) [0|59] "s" NEO + SG_ DOOR_STATE_RL : 22|2@1+ (1,0) [0|3] "" NEO + SG_ Hour : 24|5@1+ (1,0) [0|23] "h" NEO + SG_ DOOR_STATE_RR : 29|2@1+ (1,0) [0|3] "" NEO + SG_ DAY : 32|5@1+ (1,0) [0|31] "" NEO + SG_ MINUTE : 40|6@1+ (1,0) [0|59] "min" NEO + SG_ BOOT_STATE : 46|2@1+ (1,0) [0|3] "" NEO + SG_ GTW_updateInProgress : 48|2@1+ (1,0) [0|3] "" NEO + SG_ DOOR_STATE_FrontTrunk : 50|2@1+ (1,0) [0|3] "" NEO + SG_ MCU_factoryMode : 52|1@1+ (1,0) [0|1] "" NEO + SG_ MCU_transportModeOn : 53|1@0+ (1,0) [0|1] "" NEO + SG_ BC_headLightLStatus : 55|2@0+ (1,0) [0|3] "" NEO + SG_ BC_headLightRStatus : 57|2@0+ (1,0) [0|3] "" NEO + SG_ BC_indicatorLStatus : 59|2@0+ (1,0) [0|3] "" NEO + SG_ BC_indicatorRStatus : 61|2@0+ (1,0) [0|3] "" NEO BO_ 872 DI_state: 8 DI SG_ DI_systemState : 0|3@1+ (1,0) [0|0] "" NEO diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index f4403d219fe7cd..fcbbd21378c9ef 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -84,8 +84,6 @@ def __init__(self,carcontroller,alcaEnabled,steerByAngle): self.laneChange_counter = 0 # used to count frames during lane change self.laneChange_wait = 2 # how many seconds to wait before it starts the change self.laneChange_direction = 0 # direction of the lane change - self.prev_right_blinker_on = False # local variable for prev position - self.prev_left_blinker_on = False # local variable for prev position self.laneChange_cancelled = False self.laneChange_cancelled_counter = 0 self.alcaStatusSocket = messaging.pub_sock('alcaStatus') @@ -148,10 +146,9 @@ def update(self,enabled,CS,actuators,alcaStateData,frame): self.laneChange_cancelled = False # Basic highway lane change logic - actuator_delta = 0. turn_signal_needed = 0 # send 1 for left, 2 for right 0 for not needed - if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \ + if CS.turn_signal_stalk_state == 0 and \ (self.laneChange_enabled == 4): self.debug_alca("ALCA reset: resetting 4 -> 1") self.laneChange_enabled =1 @@ -159,15 +156,13 @@ def update(self,enabled,CS,actuators,alcaStateData,frame): self.laneChange_direction =0 CS.UE.custom_alert_message(-1,"",0) - if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \ + if CS.turn_signal_stalk_state == 0 and \ (self.laneChange_enabled > 1): # no blinkers on but we are still changing lane, so we need to send blinker command if self.laneChange_direction == -1: turn_signal_needed = 1 elif self.laneChange_direction == 1: turn_signal_needed = 2 - else: - turn_signal_needed = 0 if (CS.cstm_btns.get_button_status("alca") > 0) and self.alcaEnabled and (self.laneChange_enabled == 1): if ((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or \ @@ -185,14 +180,10 @@ def update(self,enabled,CS,actuators,alcaStateData,frame): self.stop_ALCA(CS, False) return 0, False - if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \ - ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \ + if self.alcaEnabled and enabled and CS.prev_turn_signal_stalk_state == 0 and CS.turn_signal_stalk_state > 0 and \ (CS.v_ego >= cl_min_v) and (abs(actuators.steerAngle) < cl_max_a) and (self.laneChange_enabled == 1): # start blinker, speed and angle is within limits, let's go - laneChange_direction = 1 - # changing lanes - if CS.left_blinker_on: - laneChange_direction = -1 + laneChange_direction = -1 if CS.turn_signal_stalk_state == 1 else 1 # left -1, right 1 self.debug_alca("ALCA blinker on detected") CS.UE.custom_alert_message(2,"Auto Lane Change Engaged!",100) @@ -202,9 +193,6 @@ def update(self,enabled,CS,actuators,alcaStateData,frame): self.laneChange_direction = laneChange_direction CS.cstm_btns.set_button_status("alca",2) - self.prev_right_blinker_on = CS.right_blinker_on - self.prev_left_blinker_on = CS.left_blinker_on - if (not self.alcaEnabled) and self.laneChange_enabled > 1: self.debug_alca("ALCA canceled: not enabled") self.laneChange_enabled = 1 diff --git a/selfdrive/car/tesla/HSO_module.py b/selfdrive/car/tesla/HSO_module.py index daa20935112c59..002b0adb463e89 100644 --- a/selfdrive/car/tesla/HSO_module.py +++ b/selfdrive/car/tesla/HSO_module.py @@ -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 diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index c8bbe3ec2dbfe1..1354c748ad985c 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -620,7 +620,7 @@ def calc_follow_speed_ms(self, CS, alca_enabled): # Enforce limits on speed new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, self.pedal_speed_kph) - if CS.blinker_on or (abs(CS.angle_steers) > ANGLE_STOP_ACCEL) or alca_enabled: + if CS.turn_signal_blinking or (abs(CS.angle_steers) > ANGLE_STOP_ACCEL) or alca_enabled: # Don't accelerate during manual turns, curves or ALCA. new_speed_kph = min(new_speed_kph, self.last_speed_kph) #BB Last safety check. Zero if below MIN_SAFE_DIST_M diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index c9971969c9e681..9a02e88163abc7 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -188,14 +188,17 @@ def __init__(self, dbc_name): self.ldwStatus = 0 self.prev_ldwStatus = 0 + self.blinker_on_frame_start = 0 + self.blinker_extension_frame_end = 0 + self.blinker_extension_blinker_type = 0 + self.radarVin_idx = 0 self.LDW_ENABLE_SPEED = 16 self.should_ldw = False - self.ldw_numb_frame_start = 0 - self.prev_changing_lanes = False - + self.ldw_numb_frame_end = 0 + self.isMetric = (self.params.get("IsMetric") == "1") - + self.ahbLead1 = None def reset_traffic_events(self): @@ -309,32 +312,10 @@ def update(self, enabled, CS, frame, actuators, \ # *** compute control surfaces *** - STEER_MAX = 420 # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) - - # Basic highway lane change logic - changing_lanes = CS.right_blinker_on or CS.left_blinker_on - - if (frame % 5 == 0): - if (changing_lanes and not self.prev_changing_lanes): #we have a transition from blinkers off to blinkers on, save the frame - self.ldw_numb_frame_start = frame - if (CS.v_ego > self.LDW_ENABLE_SPEED): - CS.UE.custom_alert_message(3, "LDW Disabled", 150, 4) - - # update the previous state of the blinkers (chaning_lanes if (self.ALCA.laneChange_enabled > 1): - self.ldw_numb_frame_start = frame - self.prev_changing_lanes = changing_lanes - if self.alca_enabled: - self.ldw_numb_frame_start = frame + 200 #we don't want LDW for 2 seconds after ALCA finishes - #Determine if we should have LDW or not - self.should_ldw = (frame > (self.ldw_numb_frame_start + int( 50 * CS.ldwNumbPeriod)) and CS.v_ego > self.LDW_ENABLE_SPEED) - - if self.should_ldw and self.ldw_numb_frame_start != 0: - self.ldw_numb_frame_start = 0 - CS.UE.custom_alert_message(2, "LDW Enabled", 150, 4) - + #upodate custom UI buttons and alerts CS.UE.update_custom_ui() @@ -380,9 +361,6 @@ def update(self, enabled, CS, frame, actuators, \ # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False - - # Update HSO module info. - human_control = False # update CS.v_cruise_pcm based on module selected. speed_uom_kph = 1. @@ -396,11 +374,13 @@ def update(self, enabled, CS, frame, actuators, \ CS.v_cruise_pcm = max(0.,CS.v_ego * CV.MS_TO_KPH +0.5) * speed_uom_kph # Get the turn signal from ALCA. turn_signal_needed, self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame) - apply_angle = -actuators.steerAngle # Tesla is reversed vs OP. - human_control,turn_signal_needed_hso = self.HSO.update_stat(self,CS, enabled, actuators, frame) if turn_signal_needed == 0: - turn_signal_needed = turn_signal_needed_hso - human_lane_changing = changing_lanes and not self.alca_enabled + turn_signal_needed = self._should_extend_blinker(CS, frame) + self.should_ldw = self._should_ldw(CS, frame) + apply_angle = -actuators.steerAngle # Tesla is reversed vs OP. + # Update HSO module info. + human_control = self.HSO.update_stat(self,CS, enabled, actuators, frame) + human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control @@ -486,7 +466,7 @@ def update(self, enabled, CS, frame, actuators, \ if pathPlanMsg is not None: #to show curvature and lanes on IC if self.alcaStateData is not None: - self.handlePathPlanSocketForCurvatureOnIC(pathPlanMsg = pathPlanMsg, alcaStateData = self.alcaStateData,CS = CS, turn_signal_needed = turn_signal_needed) + self.handlePathPlanSocketForCurvatureOnIC(pathPlanMsg = pathPlanMsg, alcaStateData = self.alcaStateData,CS = CS) if icCarLRMsg is not None: can_messages = self.showLeftAndRightCarsOnICCanMessages(icCarLRMsg = tesla.ICCarsLR.from_bytes(icCarLRMsg)) can_sends.extend(can_messages) @@ -591,7 +571,7 @@ def update(self, enabled, CS, frame, actuators, \ self.DAS_219_lcTempUnavailableSpeed = 1 self.warningCounter = 100 self.warningNeeded = 1 - if enabled and self.ALCA.laneChange_cancelled and (not CS.steer_override) and (not CS.blinker_on) and (self.ALCA.laneChange_cancelled_counter > 0): + if enabled and self.ALCA.laneChange_cancelled and (not CS.steer_override) and (CS.turn_signal_stalk_state == 0) and (self.ALCA.laneChange_cancelled_counter > 0): self.DAS_221_lcAborting = 1 self.warningCounter = 300 self.warningNeeded = 1 @@ -625,8 +605,7 @@ def update(self, enabled, CS, frame, actuators, \ if frame % 100 == 0: # and CS.hasTeslaIcIntegration: #IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake can_sends.append(teslacan.create_fake_IC_msg()) - idx = frame % 16 - cruise_btn = None + # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) @@ -695,7 +674,7 @@ def showLeadCarOnICCanMessage(self, radarStateMsg): self.lead2Id,self.lead2Dx,self.lead2Dy,self.lead2Vx)) return messages - def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData, CS, turn_signal_needed): + def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData, CS): pp = pathPlanMsg.pathPlan if pp.paramsValid: if pp.lProb > 0.75: @@ -731,7 +710,7 @@ def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData, CS, t self.lLine = 3 self.rLine = 3 else: - if self.should_ldw and (CS.enableLdw and (not CS.blinker_on) and (turn_signal_needed == 0)): + if self.should_ldw: if pp.lProb > LDW_LANE_PROBAB: lLaneC0 = -pp.lPoly[3] if abs(lLaneC0) < LDW_WARNING_2: @@ -818,3 +797,31 @@ def handleTrafficEvents(self, trafficEventsMsgs): # Returns speed as it needs to be displayed on the IC def speedUnits(self, fromMetersPerSecond): return fromMetersPerSecond * (CV.MS_TO_KPH if self.isMetric else CV.MS_TO_MPH) + 0.5 + + def _should_extend_blinker(self, CS, frame): + if CS.tapBlinkerExtension <= 0: + return 0 + if CS.turn_signal_stalk_state > 0 and CS.prev_turn_signal_stalk_state == 0 and self.blinker_extension_frame_end == 0: + self.blinker_on_frame_start = frame + elif CS.turn_signal_stalk_state == 0 and CS.prev_turn_signal_stalk_state > 0: # turn signal stalk just turned off + is_blinker_tap = frame - self.blinker_on_frame_start < 55 # stalk signal for less than 550ms means it was tapped + if is_blinker_tap: + blink_duration_frames = 58 # one blink takes ~580ms + self.blinker_extension_frame_end = self.blinker_on_frame_start + blink_duration_frames * (3 + CS.tapBlinkerExtension) + self.blinker_extension_blinker_type = CS.prev_turn_signal_stalk_state + + if 0 < self.blinker_extension_frame_end < frame: + self.blinker_extension_frame_end = 0 + self.blinker_extension_blinker_type = 0 + if self.blinker_on_frame_start > 0 and CS.turn_signal_stalk_state == 0 and not CS.turn_signal_blinking and self.blinker_extension_frame_end == 0: + self.blinker_on_frame_start = 0 + + return self.blinker_extension_blinker_type + + def _should_ldw(self, CS, frame): + if not CS.enableLdw: + return False + if CS.prev_turn_signal_blinking and not CS.turn_signal_blinking: + self.ldw_numb_frame_end = frame + int(100 * CS.ldwNumbPeriod) + + return CS.v_ego >= self.LDW_ENABLE_SPEED and not CS.turn_signal_blinking and frame > self.ldw_numb_frame_end diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 0326a3227c3e6b..437f8622cd58af 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -10,7 +10,7 @@ import os import subprocess from common.params import read_db, write_db - + def parse_gear_shifter(can_gear_shifter, car_fingerprint): # TODO: Use VAL from DBC to parse this field @@ -55,6 +55,8 @@ def get_can_signals(CP): ("DOOR_STATE_FR", "GTW_carState", 1), ("DOOR_STATE_RL", "GTW_carState", 1), ("DOOR_STATE_RR", "GTW_carState", 1), + ("BC_indicatorLStatus", "GTW_carState", 0), + ("BC_indicatorRStatus", "GTW_carState", 0), ("DI_cruiseSet", "DI_state", 0), ("DI_cruiseState", "DI_state", 0), ("LoBm_On_Rq","BODY_R1" , 0), @@ -131,7 +133,7 @@ def get_can_signals(CP): ("UI_bottomQrtlFleetSpeedMPS", "UI_driverAssistRoadSign", 0), ("UI_rampType", "UI_driverAssistRoadSign", 0), - + ] checks = [ @@ -145,7 +147,7 @@ def get_can_signals(CP): #checks = [] return signals, checks - + def get_epas_can_signals(CP): # this function generates lists for signal, messages and initial values signals = [ @@ -175,7 +177,7 @@ def get_pedal_can_signals(CP): checks = [] return signals, checks - + def get_can_parser(CP,mydbc): signals, checks = get_can_signals(CP) return CANParser(mydbc, signals, checks, 0) @@ -200,11 +202,11 @@ def __init__(self, CP): ["", "", [""]], ["msg", "MSG", [""]], ["sound", "SND", [""]]] - + ### START OF MAIN CONFIG OPTIONS ### ### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot self.forcePedalOverCC = True - self.enableHSO = True + self.enableHSO = True self.enableALCA = True self.enableDasEmulation = True self.enableRadarEmulation = True @@ -233,7 +235,7 @@ def __init__(self, CP): self.spinnerText = "" self.hsoNumbPeriod = 1.5 self.ldwNumbPeriod = 1.5 - self.hsoBlinkerExtender = 1.0 + self.tapBlinkerExtension = 0 self.ahbOffDuration = 5 #read config file read_config_file(self) @@ -310,12 +312,16 @@ def __init__(self, CP): self.brake_switch_ts = 0 self.cruise_buttons = 0 - self.blinker_on = 0 - self.left_blinker_on = 0 - self.right_blinker_on = 0 + self.turn_signal_state_left = 0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default + self.turn_signal_state_right = 0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default + self.prev_turn_signal_blinking = False + self.turn_signal_blinking = False + self.prev_turn_signal_stalk_state = 0 + self.turn_signal_stalk_state = 0 # 0 = off, 1 = indicate left (stalk down), 2 = indicate right (stalk up) + self.steer_warning = 0 - + self.stopped = 0 # variables used for the fake DAS creation @@ -371,15 +377,15 @@ def __init__(self, CP): #BB steering_wheel_stalk last position, used by ACC and ALCA self.steering_wheel_stalk = None - + #BB carConfig data used to change IC info self.real_carConfig = None self.real_dasHw = 0 #BB visiond last type self.last_visiond = self.cstm_btns.btns[3].btn_label2 - - + + # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) @@ -426,6 +432,7 @@ def config_ui_buttons(self, pedalAvailable, pedalPresent): btn.btn_label = self.btns_init[1][1] btn.btn_label2 = self.btns_init[1][2][0] btn.btn_status = 1 + if (not pedalAvailable) and pedalPresent: btn.btn_label2 = self.btns_init[1][2][1] self.cstm_btns.update_ui_buttons(1, 1) @@ -463,20 +470,10 @@ def update_ui_buttons(self,btn_id,btn_status): def update(self, cp, epas_cp, pedal_cp): - # car params - v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping - v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero - - # update prevs, update must run once per loop - self.prev_cruise_buttons = self.cruise_buttons - self.prev_blinker_on = self.blinker_on - - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on - self.steering_wheel_stalk = cp.vl["STW_ACTN_RQ"] self.real_carConfig = cp.vl["GTW_carConfig"] self.real_dasHw = cp.vl["GTW_carConfig"]['GTW_dasHw'] + self.prev_cruise_buttons = self.cruise_buttons self.cruise_buttons = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat'] @@ -545,7 +542,7 @@ def update(self, cp, epas_cp, pedal_cp): else: self.userSpeedLimitKph = cp.vl['MCU_gpsVehicleSpeed']["MCU_mppSpeedLimit"] * CV.MPH_TO_KPH - speed_limit_tesla_lookup = [0,5,7,10,15,20,25,30,35,40,45,50,55,60,65,70,75,80,85,90,95,100,105,110,115,120,130,140,150,160,170,0] + speed_limit_tesla_lookup = [0,5,7,10,15,20,25,30,35,40,45,50,55,60,65,70,75,80,85,90,95,100,105,110,115,120,130,140,150,160,170,0] self.speedLimitUnits = cp.vl["UI_driverAssistMapData"]["UI_mapSpeedUnits"] self.speedLimitKph = speed_limit_tesla_lookup[int(cp.vl["UI_driverAssistMapData"]["UI_mapSpeedLimit"])] * (1 + 0.609 * (1 - self.speedLimitUnits)) self.speedLimitToIc = int(cp.vl["UI_driverAssistMapData"]["UI_mapSpeedLimit"]) @@ -566,13 +563,13 @@ def update(self, cp, epas_cp, pedal_cp): self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_medianFleetSpeedMPS"] self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"]["UI_splineLocConfidence"] self.rampType = cp.vl["UI_driverAssistRoadSign"]["UI_rampType"] - + if rdSignMsg == 3: self.topQrtlFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_topQrtlFleetSpeedMPS"] self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"]["UI_splineLocConfidence"] self.baseMapSpeedLimitMPS = cp.vl["UI_driverAssistRoadSign"]["UI_baseMapSpeedLimitMPS"] self.bottomQrtlFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_bottomQrtlFleetSpeedMPS"] - + self.compute_speed() # 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent) @@ -609,22 +606,19 @@ def update(self, cp, epas_cp, pedal_cp): self.pedal_interceptor_value2 = pedal_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2'] can_gear_shifter = cp.vl["DI_torque2"]['DI_gear'] - self.gear = 0 # JCT # self.angle_steers = -(cp.vl["STW_ANGLHP_STAT"]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura self.angle_steers = -(epas_cp.vl["EPAS_sysStatus"]['EPAS_internalSAS']) #BB see if this works better than STW_ANGLHP_STAT for angle - + self.angle_steers_rate = 0 #JCT - self.blinker_on = (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1) or (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2) - self.left_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1 - self.right_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2 + self.turn_signal_state_left = cp.vl["GTW_carState"]['BC_indicatorLStatus'] + self.turn_signal_state_right = cp.vl["GTW_carState"]['BC_indicatorRStatus'] + self.prev_turn_signal_blinking = self.turn_signal_blinking + self.turn_signal_blinking = self.turn_signal_state_left == 1 or self.turn_signal_state_right == 1 + self.prev_turn_signal_stalk_state = self.turn_signal_stalk_state + self.turn_signal_stalk_state = 0 if cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 3 else int(cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat']) - #if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY): - # self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 - # self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] - # self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] - #else: self.park_brake = 0 # TODO self.brake_hold = 0 # TODO @@ -652,9 +646,9 @@ def update(self, cp, epas_cp, pedal_cp): self.standstill = cp.vl["DI_torque2"]['DI_vehicleSpeed'] == 0 self.torqueMotor = cp.vl["DI_torque1"]['DI_torqueMotor'] self.pcm_acc_status = cp.vl["DI_state"]['DI_cruiseState'] - + self.regenLight = cp.vl["DI_state"]['DI_regenLight'] == 1 - + self.prev_pedal_interceptor_available = self.pedal_interceptor_available pedal_has_value = bool(self.pedal_interceptor_value) or bool(self.pedal_interceptor_value2) pedal_interceptor_present = self.pedal_interceptor_state in [0, 5] and pedal_has_value diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 8eaa7ac3f42491..8b6e1d03364bbf 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -283,24 +283,24 @@ def update(self, c, can_strings): # TODO: button presses buttonEvents = [] - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) + ret.leftBlinker = bool(self.CS.turn_signal_state_left == 1) + ret.rightBlinker = bool(self.CS.turn_signal_state_right == 1) ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = 'leftBlinker' - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = 'rightBlinker' - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) + if self.CS.prev_turn_signal_stalk_state != self.CS.turn_signal_stalk_state: + if self.CS.turn_signal_stalk_state == 1 or self.CS.prev_turn_signal_stalk_state == 1: + be = car.CarState.ButtonEvent.new_message() + be.type = 'leftBlinker' + be.pressed = self.CS.turn_signal_stalk_state == 1 + buttonEvents.append(be) + if self.CS.turn_signal_stalk_state == 2 or self.CS.prev_turn_signal_stalk_state == 2: + be = car.CarState.ButtonEvent.new_message() + be.type = 'rightBlinker' + be.pressed = self.CS.turn_signal_stalk_state == 2 + buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() diff --git a/selfdrive/car/tesla/readconfig.py b/selfdrive/car/tesla/readconfig.py index 6107328ada612e..3fa1948077832e 100644 --- a/selfdrive/car/tesla/readconfig.py +++ b/selfdrive/car/tesla/readconfig.py @@ -1,432 +1,432 @@ -import configparser - -default_config_file_path = '/data/bb_openpilot.cfg' - -class ConfigFile(): - config_file_r = 'r' - config_file_w = 'w' - - ### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot - def read(self, into, config_path): - configr = configparser.RawConfigParser() - file_changed = False - - try: - configr.read(config_path) - fd = open(config_path, "r") - prev_file_contents = fd.read() - fd.close() - except IOError: - prev_file_contents = "" - print("no config file, creating with defaults...") - - main_section = 'OP_CONFIG' - pref_section = 'OP_PREFERENCES' - logging_section = 'LOGGING' - config = configparser.RawConfigParser(allow_no_value=True) - config.add_section(main_section) - config.add_section(pref_section) - config.add_section(logging_section) - - #user_handle -> userHandle - into.userHandle, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'user_handle', entry_type = str, - default_value = 'your_tinkla_username', - comment = 'Username at tinkla.com, for dashboard data and support. If you don\'t have a username, ask for one on Discord, or just enter your Discord handle here.' - ) - file_changed |= didUpdate - - #force_fingerprint_tesla -> forceFingerprintTesla - into.forceFingerprintTesla, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'force_fingerprint_tesla', entry_type = bool, - default_value = False, - comment = 'Forces the fingerprint to Tesla Model S if OpenPilot fails to identify car via fingerprint.' - ) - file_changed |= didUpdate - - #force_pedal_over_cc -> forcePedalOverCC - into.forcePedalOverCC, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'force_pedal_over_cc', entry_type = bool, - default_value = False, - comment = 'Forces the use of Tesla Pedal over ACC completely disabling the Tesla CC' - ) - file_changed |= didUpdate - - #enable_hso -> enableHSO - into.enableHSO, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'enable_hso', entry_type = bool, - default_value = True, - comment = 'Enables Human Steering Override (HSO) feature which allows you to take control of the steering wheel and correct the course of the car without disengaging OpenPilot lane keep assis (LKS, lateral control)' - ) - file_changed |= didUpdate - - #enable_alca -> enableALCA - into.enableALCA, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'enable_alca', entry_type = bool, - default_value = True, - comment = 'Enables the Adaptive Lane Change Assist (ALCA) feature which will automatically change lanes when driving above 18 MPH (29 km/h) by just pushing 1/2 way on your turn signal stalk; turn signal will remain on for the duration of lane change' - ) - file_changed |= didUpdate - - #enable_das_emulation -> enableDasEmulation - into.enableDasEmulation, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'enable_das_emulation', entry_type = bool, - default_value = False, - comment = 'The secret sauce of IC/CID integration; this feature makes the Panda generate all the CAN messages needed for IC/CID integration that mimiinto the AP interface' - ) - file_changed |= didUpdate - - #enable_radar_emulation -> enableRadarEmulation - into.enableRadarEmulation, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'enable_radar_emulation', entry_type = bool, - default_value = False, - comment = 'The secret sauce to make the Tesla Radar work; this feature make the Panda generate all the CAN messages needed by the Tesla Bosch Radar to operate' - ) - file_changed |= didUpdate - - #enable_driver_monitor -> enableDriverMonitor - into.enableDriverMonitor, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'enable_driver_monitor', entry_type = bool, - default_value = True, - comment = 'When turned off, the OpenPilot is tricked into thinking you have the hands on the sterring wheel all the time' - ) - file_changed |= didUpdate - - #has_noctua_fan -> hasNoctuaFan - into.hasNoctuaFan, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'has_noctua_fan', entry_type = bool, - default_value = False, - comment = 'Enables control of Noctua fan (at higher RPMS) when you have a Noctua fan installed' - ) - file_changed |= didUpdate - - #limit_battery_minmax -> limitBatteryMinMax - into.limitBatteryMinMax, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'limit_battery_minmax', entry_type = bool, - default_value = True, - comment = 'Enables battery charging limits; the battery will start charging when battery percentage is below limit_battery_min and will stop charging when battery percentage is above limit_battery_max' - ) - file_changed |= didUpdate - - #limit_battery_min -> limitBattery_Min - into.limitBattery_Min, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'limit_battery_min', entry_type = int, - default_value = 60, - comment = 'See limit_battery_minmax' - ) - file_changed |= didUpdate - - #limitBattery_Max -> limitBattery_Max - into.limitBattery_Max, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'limit_battery_max', entry_type = int, - default_value = 80, - comment = 'See limit_battery_minmax' - ) - file_changed |= didUpdate - - #block_upload_while_tethering -> blockUploadWhileTethering - into.blockUploadWhileTethering, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'block_upload_while_tethering', entry_type = bool, - default_value = False, - comment = 'This setting will block uploading OP videos to Comma when you are tethering through the phone. You should set the tether_ip to the first 3 values that your phone provides as IP when you tether. This is phone/carrier specific. For example iPhone give addresses like 172.20.10.x so you would enter 172.20.10.' - ) - file_changed |= didUpdate - - #tether_ip -> tetherIP - into.tetherIP, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'tether_ip', entry_type = str, - default_value = "127.0.0.", - comment = 'See block_upload_while_tethering' - ) - file_changed |= didUpdate - - #use_tesla_gps -> useTeslaGPS - into.useTeslaGPS, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'use_tesla_gps', entry_type = bool, - default_value = False, - comment = 'This setting makes OP to use Tesla GPS data instead of the GPS that comes with the gray panda; both GPS systems use Ublox and both are very close in accuracy; this also allows one to use a White Panda and still have map integration' - ) - file_changed |= didUpdate - - #use_tesla_map_data -> useTeslaMapData - into.useTeslaMapData, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'use_tesla_map_data', entry_type = bool, - default_value = False, - comment = 'This setting (which requires root) allows OP to use Tesla navigation map data (under development)' - ) - file_changed |= didUpdate - - #has_tesla_ic_integration -> hasTeslaIcIntegration - into.hasTeslaIcIntegration, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'has_tesla_ic_integration', entry_type = bool, - default_value = False, - comment = 'This setting (in conjunction with enable_radar_emulation) help create the IC integration' - ) - file_changed |= didUpdate - - #use_tesla_radar -> useTeslaRadar - into.useTeslaRadar, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'use_tesla_radar', entry_type = bool, - default_value = False, - comment = 'Set this setting to True if you have a Tesla Bosch Radar installed (works in conjunction with enable_radar_emulation)' - ) - file_changed |= didUpdate - - #use_without_harness = useWithoutHarness - into.useWithoutHarness, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'use_without_harness', entry_type = bool, - default_value = False, - comment = 'Not used at the moment; should be False' - ) - file_changed |= didUpdate - - #radar_vin -> into.radarVIN - default_radar_vin = '" "' - into.radarVIN, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'radar_vin', entry_type = str, - default_value = default_radar_vin, - comment = 'If you used an aftermarket Tesla Bosch Radar that already has a coded VIN, you will have to enter that VIN value here' - ) - file_changed |= didUpdate - if into.radarVIN == '': - into.radarVIN = default_radar_vin - file_changed = True - - #radar_offset -> radarOffset - into.radarOffset, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'radar_offset', entry_type = float, - default_value = 0, - comment = 'If your Tesla Bosch Radar is not centered on the car, this value will allow to enter a correction offset' - ) - file_changed |= didUpdate - - #radar_epas_type -> radarEpasType - into.radarEpasType, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'radar_epas_type', entry_type = int, - default_value = 0, - comment = 'Depending on the source of your Tesla Bosch Radar (older or newer Model S or Model X), this setting has to match what the radar was programmed to recognize as EPAS; values are between 0 and 4; finding the right one is trial and error' - ) - file_changed |= didUpdate - - #radar_position -> radarPosition - into.radarPosition, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'radar_position', entry_type = int, - default_value = 0, - comment = 'Depending on the source of your Tesla Bosch Radar (older or newer Model S or Model X), this setting has to match what the radar was programmed to have a position (Model S, Model S facelift, Model X); values are between 0 and 3; finding the right one is trial and error' - ) - file_changed |= didUpdate - - #fix_1916 -> fix1916 - into.fix1916, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'fix_1916', entry_type = bool, - default_value = False, - comment = 'Set this value to True if you are running Tesla software v2019.16 and above. This fixes the DI_state can message change for DI_cruiseSet which changed from 9 bits to 8 bits' - ) - file_changed |= didUpdate - - #do_auto_update -> doAutoUpdate - into.doAutoUpdate, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = main_section, - entry = 'do_auto_update', entry_type = bool, - default_value = True, - comment = 'Set this setting to False if you do not want OP to autoupdate every time you reboot and there is a change on the repo' - ) - file_changed |= didUpdate - - #spiner_text -> spinnerText - into.spinnerText, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'spinner_text', entry_type = str, - default_value = '%d', - comment = 'The text that is shown for the spinner when spawning the managed services.' - ) - file_changed |= didUpdate - - #hso_numb_period -> hsoNumbPeriod - into.hsoNumbPeriod, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'hso_numb_period', entry_type = float, - default_value = 1.5, - comment = 'Period to delay (in seconds) the reengagement of LKAS after human turn signal has been used. Time starts when the turn signal is turned on.' - ) - file_changed |= didUpdate - - #enable_ldw = enableLdw - into.enableLdw, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'enable_ldw', entry_type = bool, - default_value = True, - comment = 'Enable the Lane Departure Warning (LDW) feature; this feature warns the driver is the car gets too close to one of the lines when driving above 45 MPH (72 km/h) without touching the steering wheel and when the turn signal is off' - ) - file_changed |= didUpdate - - #ldw_numb_period -> ldwNumbPeriod - into.ldwNumbPeriod, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'ldw_numb_period', entry_type = float, - default_value = 1.5, - comment = 'Period to delay (in seconds) the LDW warnings after human turn signal has been used. Time starts when the turn signal is turned on.' - ) - file_changed |= didUpdate - - #hso_blinker_extender -> hsoBlinkerExtender - into.hsoBlinkerExtender, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'hso_blinker_extender', entry_type = float, - default_value = 0.0, - comment = 'Period to keep the blinker on (in seconds). Time starts when the turn signal is turned off. If LKA is reengaged, the signal is turned off automatically.' - ) - file_changed |= didUpdate - - #enable_show_car -> enableShowCar - into.enableShowCar, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'enable_show_car', entry_type = bool, - default_value = True, - comment = 'Shows a Tesla car in the limitted UI mode instead of the triangle that identifies the lead car; this is only used if you do not have IC/CID integration' - ) - file_changed |= didUpdate - - #enable_show_logo -> enableShowLogo - into.enableShowLogo, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'enable_show_logo', entry_type = bool, - default_value = True, - comment = 'Shows a Tesla red logo on the EON screen when OP is not enabled' - ) - file_changed |= didUpdate - - #ahb_off_duration -> ahbOffDuration - into.ahbOffDuration, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = pref_section, - entry = 'ahb_off_duration', entry_type = int, - default_value = 5, - comment = 'Duration Auto High Beams should be off after last detecting a vehicle' - ) - file_changed |= didUpdate - - into.shouldLogCanErrors, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = logging_section, - entry = 'should_log_can_errors', entry_type = bool, - default_value = False, - comment = 'Enable to log CAN errors to Tinkla' - ) - file_changed |= didUpdate - - into.shouldLogProcessCommErrors, didUpdate = self.read_config_entry( - config, configr, prev_file_contents, section = logging_section, - entry = 'should_log_process_comm_errors', entry_type = bool, - default_value = False, - comment = 'Enable to log Process Comm errors to Tinkla' - ) - file_changed |= didUpdate - - if file_changed: - did_write = True - with open(config_path, self.config_file_w) as configfile: - config.write(configfile) - else: - did_write = False - - # Remove double quotes from VIN (they are required for empty case) - into.radarVIN = into.radarVIN.replace('"', '') - return did_write - - def read_config_entry(self, config, configr, prev_file_contents, section, entry, entry_type, default_value, comment): - updated = self.update_comment(config, prev_file_contents, section, entry, default_value, comment) - result = None - try: - if entry_type == bool: - result = configr.getboolean(section, entry) - elif entry_type == int: - result = configr.getint(section, entry) - elif entry_type == float: - result = configr.getfloat(section, entry) - else: - result = configr.get(section, entry) - except (configparser.NoSectionError, configparser.NoOptionError): - result = default_value - updated = True - config.set(section, entry, result) - return result, updated - - def update_comment(self, config, prev_file_contents, section, entry, default_value, comment): - new_comment = ("# " + entry + ": " + comment + " (Default: " + str(default_value) + ")").lower() - config.set(section, new_comment) - updated = (prev_file_contents.find(new_comment) == -1) - return updated - -class CarSettings(): - - userHandle = None - forceFingerprintTesla = None - forcePedalOverCC = None - enableHSO = None - enableALCA = None - enableDasEmulation = None - enableRadarEmulation = None - enableDriverMonitor = None - enableShowCar = None - enableShowLogo = None - hasNoctuaFan = None - limitBatteryMinMax = None - limitBattery_Min = None - limitBattery_Max = None - blockUploadWhileTethering = None - tetherIP = None - useTeslaGPS = None - useTeslaMapData = None - hasTeslaIcIntegration = None - useTeslaRadar = None - useWithoutHarness = None - radarVIN = None - enableLdw = None - radarOffset = None - radarEpasType = None - radarPosition = None - fix1916 = None - doAutoUpdate = None - spinnerText = None - shouldLogProcessCommErrors = None - shouldLogCanErrors = None - hsoNumbPeriod = None - ldwNumbPeriod = None - hsoBlinkerExtender = None - ahbOffDuration = None - - def __init__(self, optional_config_file_path = default_config_file_path): - config_file = ConfigFile() - self.did_write_file = config_file.read(self, config_path = optional_config_file_path) - - def get_value(self, name_of_variable): - return self.__dict__[name_of_variable] - -# Legacy support -def read_config_file(into, config_path = default_config_file_path): - config_file = ConfigFile() - config_file.read(into, config_path) - +import configparser + +default_config_file_path = '/data/bb_openpilot.cfg' + +class ConfigFile(): + config_file_r = 'r' + config_file_w = 'w' + + ### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot + def read(self, into, config_path): + configr = configparser.RawConfigParser() + file_changed = False + + try: + configr.read(config_path) + fd = open(config_path, "r") + prev_file_contents = fd.read() + fd.close() + except IOError: + prev_file_contents = "" + print("no config file, creating with defaults...") + + main_section = 'OP_CONFIG' + pref_section = 'OP_PREFERENCES' + logging_section = 'LOGGING' + config = configparser.RawConfigParser(allow_no_value=True) + config.add_section(main_section) + config.add_section(pref_section) + config.add_section(logging_section) + + #user_handle -> userHandle + into.userHandle, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'user_handle', entry_type = str, + default_value = 'your_tinkla_username', + comment = 'Username at tinkla.com, for dashboard data and support. If you don\'t have a username, ask for one on Discord, or just enter your Discord handle here.' + ) + file_changed |= didUpdate + + #force_fingerprint_tesla -> forceFingerprintTesla + into.forceFingerprintTesla, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'force_fingerprint_tesla', entry_type = bool, + default_value = False, + comment = 'Forces the fingerprint to Tesla Model S if OpenPilot fails to identify car via fingerprint.' + ) + file_changed |= didUpdate + + #force_pedal_over_cc -> forcePedalOverCC + into.forcePedalOverCC, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'force_pedal_over_cc', entry_type = bool, + default_value = False, + comment = 'Forces the use of Tesla Pedal over ACC completely disabling the Tesla CC' + ) + file_changed |= didUpdate + + #enable_hso -> enableHSO + into.enableHSO, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'enable_hso', entry_type = bool, + default_value = True, + comment = 'Enables Human Steering Override (HSO) feature which allows you to take control of the steering wheel and correct the course of the car without disengaging OpenPilot lane keep assis (LKS, lateral control)' + ) + file_changed |= didUpdate + + #enable_alca -> enableALCA + into.enableALCA, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'enable_alca', entry_type = bool, + default_value = True, + comment = 'Enables the Adaptive Lane Change Assist (ALCA) feature which will automatically change lanes when driving above 18 MPH (29 km/h) by just pushing 1/2 way on your turn signal stalk; turn signal will remain on for the duration of lane change' + ) + file_changed |= didUpdate + + #enable_das_emulation -> enableDasEmulation + into.enableDasEmulation, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'enable_das_emulation', entry_type = bool, + default_value = False, + comment = 'The secret sauce of IC/CID integration; this feature makes the Panda generate all the CAN messages needed for IC/CID integration that mimiinto the AP interface' + ) + file_changed |= didUpdate + + #enable_radar_emulation -> enableRadarEmulation + into.enableRadarEmulation, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'enable_radar_emulation', entry_type = bool, + default_value = False, + comment = 'The secret sauce to make the Tesla Radar work; this feature make the Panda generate all the CAN messages needed by the Tesla Bosch Radar to operate' + ) + file_changed |= didUpdate + + #enable_driver_monitor -> enableDriverMonitor + into.enableDriverMonitor, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'enable_driver_monitor', entry_type = bool, + default_value = True, + comment = 'When turned off, the OpenPilot is tricked into thinking you have the hands on the sterring wheel all the time' + ) + file_changed |= didUpdate + + #has_noctua_fan -> hasNoctuaFan + into.hasNoctuaFan, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'has_noctua_fan', entry_type = bool, + default_value = False, + comment = 'Enables control of Noctua fan (at higher RPMS) when you have a Noctua fan installed' + ) + file_changed |= didUpdate + + #limit_battery_minmax -> limitBatteryMinMax + into.limitBatteryMinMax, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'limit_battery_minmax', entry_type = bool, + default_value = True, + comment = 'Enables battery charging limits; the battery will start charging when battery percentage is below limit_battery_min and will stop charging when battery percentage is above limit_battery_max' + ) + file_changed |= didUpdate + + #limit_battery_min -> limitBattery_Min + into.limitBattery_Min, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'limit_battery_min', entry_type = int, + default_value = 60, + comment = 'See limit_battery_minmax' + ) + file_changed |= didUpdate + + #limitBattery_Max -> limitBattery_Max + into.limitBattery_Max, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'limit_battery_max', entry_type = int, + default_value = 80, + comment = 'See limit_battery_minmax' + ) + file_changed |= didUpdate + + #block_upload_while_tethering -> blockUploadWhileTethering + into.blockUploadWhileTethering, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'block_upload_while_tethering', entry_type = bool, + default_value = False, + comment = 'This setting will block uploading OP videos to Comma when you are tethering through the phone. You should set the tether_ip to the first 3 values that your phone provides as IP when you tether. This is phone/carrier specific. For example iPhone give addresses like 172.20.10.x so you would enter 172.20.10.' + ) + file_changed |= didUpdate + + #tether_ip -> tetherIP + into.tetherIP, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'tether_ip', entry_type = str, + default_value = "127.0.0.", + comment = 'See block_upload_while_tethering' + ) + file_changed |= didUpdate + + #use_tesla_gps -> useTeslaGPS + into.useTeslaGPS, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'use_tesla_gps', entry_type = bool, + default_value = False, + comment = 'This setting makes OP to use Tesla GPS data instead of the GPS that comes with the gray panda; both GPS systems use Ublox and both are very close in accuracy; this also allows one to use a White Panda and still have map integration' + ) + file_changed |= didUpdate + + #use_tesla_map_data -> useTeslaMapData + into.useTeslaMapData, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'use_tesla_map_data', entry_type = bool, + default_value = False, + comment = 'This setting (which requires root) allows OP to use Tesla navigation map data (under development)' + ) + file_changed |= didUpdate + + #has_tesla_ic_integration -> hasTeslaIcIntegration + into.hasTeslaIcIntegration, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'has_tesla_ic_integration', entry_type = bool, + default_value = False, + comment = 'This setting (in conjunction with enable_radar_emulation) help create the IC integration' + ) + file_changed |= didUpdate + + #use_tesla_radar -> useTeslaRadar + into.useTeslaRadar, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'use_tesla_radar', entry_type = bool, + default_value = False, + comment = 'Set this setting to True if you have a Tesla Bosch Radar installed (works in conjunction with enable_radar_emulation)' + ) + file_changed |= didUpdate + + #use_without_harness = useWithoutHarness + into.useWithoutHarness, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'use_without_harness', entry_type = bool, + default_value = False, + comment = 'Not used at the moment; should be False' + ) + file_changed |= didUpdate + + #radar_vin -> into.radarVIN + default_radar_vin = '" "' + into.radarVIN, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'radar_vin', entry_type = str, + default_value = default_radar_vin, + comment = 'If you used an aftermarket Tesla Bosch Radar that already has a coded VIN, you will have to enter that VIN value here' + ) + file_changed |= didUpdate + if into.radarVIN == '': + into.radarVIN = default_radar_vin + file_changed = True + + #radar_offset -> radarOffset + into.radarOffset, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'radar_offset', entry_type = float, + default_value = 0, + comment = 'If your Tesla Bosch Radar is not centered on the car, this value will allow to enter a correction offset' + ) + file_changed |= didUpdate + + #radar_epas_type -> radarEpasType + into.radarEpasType, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'radar_epas_type', entry_type = int, + default_value = 0, + comment = 'Depending on the source of your Tesla Bosch Radar (older or newer Model S or Model X), this setting has to match what the radar was programmed to recognize as EPAS; values are between 0 and 4; finding the right one is trial and error' + ) + file_changed |= didUpdate + + #radar_position -> radarPosition + into.radarPosition, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'radar_position', entry_type = int, + default_value = 0, + comment = 'Depending on the source of your Tesla Bosch Radar (older or newer Model S or Model X), this setting has to match what the radar was programmed to have a position (Model S, Model S facelift, Model X); values are between 0 and 3; finding the right one is trial and error' + ) + file_changed |= didUpdate + + #fix_1916 -> fix1916 + into.fix1916, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'fix_1916', entry_type = bool, + default_value = False, + comment = 'Set this value to True if you are running Tesla software v2019.16 and above. This fixes the DI_state can message change for DI_cruiseSet which changed from 9 bits to 8 bits' + ) + file_changed |= didUpdate + + #do_auto_update -> doAutoUpdate + into.doAutoUpdate, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'do_auto_update', entry_type = bool, + default_value = True, + comment = 'Set this setting to False if you do not want OP to autoupdate every time you reboot and there is a change on the repo' + ) + file_changed |= didUpdate + + #spiner_text -> spinnerText + into.spinnerText, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'spinner_text', entry_type = str, + default_value = '%d', + comment = 'The text that is shown for the spinner when spawning the managed services.' + ) + file_changed |= didUpdate + + #hso_numb_period -> hsoNumbPeriod + into.hsoNumbPeriod, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'hso_numb_period', entry_type = float, + default_value = 1.5, + comment = 'Period to delay (in seconds) the reengagement of LKAS after human turn signal has been used. Time starts when the turn signal is turned on.' + ) + file_changed |= didUpdate + + #enable_ldw = enableLdw + into.enableLdw, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'enable_ldw', entry_type = bool, + default_value = True, + comment = 'Enable the Lane Departure Warning (LDW) feature; this feature warns the driver is the car gets too close to one of the lines when driving above 35 MPH (57 km/h) without touching the steering wheel or when the turn signal is off' + ) + file_changed |= didUpdate + + #ldw_numb_period -> ldwNumbPeriod + into.ldwNumbPeriod, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'ldw_numb_period', entry_type = float, + default_value = 1.5, + comment = 'Period (in seconds) to disable LDW after the blinker stops blinking.' + ) + file_changed |= didUpdate + + #tap_blinker_extension -> tapBlinkerExtension + into.tapBlinkerExtension, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'tap_blinker_extension', entry_type = int, + default_value = 0, + comment = 'Number of additional blinks when tapping the turn signal stalk. A value of 2 means 5 blinks total, because the car normally blinks 3 times.' + ) + file_changed |= didUpdate + + #enable_show_car -> enableShowCar + into.enableShowCar, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'enable_show_car', entry_type = bool, + default_value = True, + comment = 'Shows a Tesla car in the limitted UI mode instead of the triangle that identifies the lead car; this is only used if you do not have IC/CID integration' + ) + file_changed |= didUpdate + + #enable_show_logo -> enableShowLogo + into.enableShowLogo, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'enable_show_logo', entry_type = bool, + default_value = True, + comment = 'Shows a Tesla red logo on the EON screen when OP is not enabled' + ) + file_changed |= didUpdate + + #ahb_off_duration -> ahbOffDuration + into.ahbOffDuration, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = pref_section, + entry = 'ahb_off_duration', entry_type = int, + default_value = 5, + comment = 'Duration Auto High Beams should be off after last detecting a vehicle' + ) + file_changed |= didUpdate + + into.shouldLogCanErrors, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = logging_section, + entry = 'should_log_can_errors', entry_type = bool, + default_value = False, + comment = 'Enable to log CAN errors to Tinkla' + ) + file_changed |= didUpdate + + into.shouldLogProcessCommErrors, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = logging_section, + entry = 'should_log_process_comm_errors', entry_type = bool, + default_value = False, + comment = 'Enable to log Process Comm errors to Tinkla' + ) + file_changed |= didUpdate + + if file_changed: + did_write = True + with open(config_path, self.config_file_w) as configfile: + config.write(configfile) + else: + did_write = False + + # Remove double quotes from VIN (they are required for empty case) + into.radarVIN = into.radarVIN.replace('"', '') + return did_write + + def read_config_entry(self, config, configr, prev_file_contents, section, entry, entry_type, default_value, comment): + updated = self.update_comment(config, prev_file_contents, section, entry, default_value, comment) + result = None + try: + if entry_type == bool: + result = configr.getboolean(section, entry) + elif entry_type == int: + result = configr.getint(section, entry) + elif entry_type == float: + result = configr.getfloat(section, entry) + else: + result = configr.get(section, entry) + except (configparser.NoSectionError, configparser.NoOptionError): + result = default_value + updated = True + config.set(section, entry, result) + return result, updated + + def update_comment(self, config, prev_file_contents, section, entry, default_value, comment): + new_comment = ("# " + entry + ": " + comment + " (Default: " + str(default_value) + ")").lower() + config.set(section, new_comment) + updated = (prev_file_contents.find(new_comment) == -1) + return updated + +class CarSettings(): + + userHandle = None + forceFingerprintTesla = None + forcePedalOverCC = None + enableHSO = None + enableALCA = None + enableDasEmulation = None + enableRadarEmulation = None + enableDriverMonitor = None + enableShowCar = None + enableShowLogo = None + hasNoctuaFan = None + limitBatteryMinMax = None + limitBattery_Min = None + limitBattery_Max = None + blockUploadWhileTethering = None + tetherIP = None + useTeslaGPS = None + useTeslaMapData = None + hasTeslaIcIntegration = None + useTeslaRadar = None + useWithoutHarness = None + radarVIN = None + enableLdw = None + radarOffset = None + radarEpasType = None + radarPosition = None + fix1916 = None + doAutoUpdate = None + spinnerText = None + shouldLogProcessCommErrors = None + shouldLogCanErrors = None + hsoNumbPeriod = None + ldwNumbPeriod = None + tapBlinkerExtension = None + ahbOffDuration = None + + def __init__(self, optional_config_file_path = default_config_file_path): + config_file = ConfigFile() + self.did_write_file = config_file.read(self, config_path = optional_config_file_path) + + def get_value(self, name_of_variable): + return self.__dict__[name_of_variable] + +# Legacy support +def read_config_file(into, config_path = default_config_file_path): + config_file = ConfigFile() + config_file.read(into, config_path) +