From 637297f11fb937a5a5a0098daa17e21ded80abc4 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Sat, 23 Nov 2019 16:20:21 -0500 Subject: [PATCH] CC as option for ACC; fix ALCA IC integration --- panda/board/safety/safety_tesla.h | 11 +++++-- selfdrive/car/tesla/ACC_module.py | 46 +++++++++++++++++++--------- selfdrive/car/tesla/AHB_module.py | 2 +- selfdrive/car/tesla/carcontroller.py | 9 +++++- selfdrive/car/tesla/teslacan.py | 4 +-- 5 files changed, 51 insertions(+), 21 deletions(-) diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 9b8b58a01fb2a4..6fa6142c482474 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -218,6 +218,9 @@ int DAS_high_low_beam_request = 0x00; int DAS_high_low_beam_reason = 0x00; int DAS_ahb_is_enabled = 0; +//fake DAS - plain CC condition +int DAS_plain_cc_enabled = 0x00; + static int add_tesla_crc(uint32_t MLB, uint32_t MHB , int msg_len) { //"""Calculate CRC8 using 1D poly, FF start, FF end""" @@ -381,6 +384,7 @@ static void reset_DAS_data(void) { DAS_LEFT_OBJECT_MHB = 0x03FFFF83; DAS_RIGHT_OBJECT_MLB = 0xFFFFFF02; DAS_RIGHT_OBJECT_MHB = 0x03FFFF83; + DAS_plain_cc_enabled = 0x00; } @@ -1033,7 +1037,9 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) if ((EON_is_connected == 1) && (DAS_usingPedal == 0) && (DAS_cc_state != 2) && ( acc_state >= 2) && ( acc_state <= 4)) { //disable if more than two seconds since last pull, or there was never a stalk pull if (((current_car_time >= time_at_last_stalk_pull + 2) && (current_car_time != -1) && (time_at_last_stalk_pull != -1)) || (time_at_last_stalk_pull == -1)) { - do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + if (DAS_plain_cc_enabled == 0) { + do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + } } } } @@ -1542,7 +1548,8 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) int b2 = ((to_send->RDLR >> 16) & 0xFF); DAS_high_low_beam_request = b0; DAS_high_low_beam_reason = b1; - DAS_ahb_is_enabled = b2; + DAS_ahb_is_enabled = b2 & 0x01; + DAS_plain_cc_enabled = (b2 >> 1) & 0x01; //intercept and do not forward return false; } diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 7811e8456c48ec..f9bd8b3b9a1f9f 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -16,24 +16,26 @@ class ACCState(): NOT_READY = 9 # Not ready to be engaged due to the state of the car. class _Mode(): - def __init__(self, label, autoresume, state): + def __init__(self, label, autoresume, state,adaptive): self.label = label self.autoresume = autoresume self.state = state + self.adaptive = adaptive self.next = None class ACCMode(): # Possible ACC modes, controlling how ACC behaves. # This is separate from ACC state. For example, you could # have ACC in "Autoresume" mode in "Standby" state. - FOLLOW = _Mode(label="follow", autoresume=False, state=ACCState.STANDBY) - AUTO = _Mode(label="auto", autoresume=True, state=ACCState.STANDBY) + FOLLOW = _Mode(label="follow", autoresume=False, state=ACCState.STANDBY, adaptive=True) + AUTO = _Mode(label="auto", autoresume=True, state=ACCState.STANDBY, adaptive=True) + PLAINCC = _Mode(label="cc", autoresume=False, state=ACCState.STANDBY, adaptive=False) BUTTON_NAME = 'acc' BUTTON_ABREVIATION = 'ACC' # Toggle order: OFF -> ON -> AUTO -> OFF - _all_modes = [FOLLOW, AUTO] + _all_modes = [FOLLOW, AUTO,PLAINCC] for index, mode in enumerate(_all_modes): mode.next = _all_modes[(index + 1) % len(_all_modes)] @@ -67,6 +69,7 @@ def __init__(self,carcontroller): # Whether to re-engage automatically after being paused due to low speed or # user-initated deceleration. self.autoresume = False + self.adaptive = False self.last_brake_press_time = 0 self.last_cruise_stalk_pull_time = 0 self.prev_cruise_buttons = CruiseButtons.IDLE @@ -95,33 +98,37 @@ def update_stat(self, CS, enabled): acc_mode = ACCMode.from_label(acc_string) CS.cstm_btns.get_button(ACCMode.BUTTON_NAME).btn_label2 = acc_mode.label self.autoresume = acc_mode.autoresume + self.adaptive = acc_mode.adaptive curr_time_ms = _current_time_millis() # Handle pressing the enable button. - if (CS.cruise_buttons == CruiseButtons.MAIN and - self.prev_cruise_buttons != CruiseButtons.MAIN): + if (CS.cruise_buttons == CruiseButtons.MAIN or CS.cruise_buttons == CruiseButtons.DECEL_SET) and self.prev_cruise_buttons != CS.cruise_buttons: double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 self.last_cruise_stalk_pull_time = curr_time_ms ready = (CS.cstm_btns.get_button_status(ACCMode.BUTTON_NAME) > ACCState.OFF - and enabled + and (enabled or (CS.cruise_buttons == CruiseButtons.DECEL_SET and not self.adaptive)) and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) and CS.v_ego > self.MIN_CRUISE_SPEED_MS) - if ready and double_pull: + if ready and double_pull and ((self.adaptive and CS.cruise_buttons == CruiseButtons.MAIN) or ((not self.adaptive) and CS.cruise_buttons == CruiseButtons.DECEL_SET)): # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_adaptive_cruise = True # Increase ACC speed to match current, if applicable. - self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.speed_limit_kph) + if self.adaptive: + self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.speed_limit_kph) + else: + self.acc_speed_kph = CS.v_ego_raw * CV.MS_TO_KPH self.user_has_braked = False self.has_gone_below_min_speed = False else: # A single pull disables ACC (falling back to just steering). - self.enable_adaptive_cruise = False + if CS.cruise_buttons == CruiseButtons.MAIN: + self.enable_adaptive_cruise = False # Handle pressing the cancel button. - elif CS.cruise_buttons == CruiseButtons.CANCEL: + if CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_adaptive_cruise = False self.acc_speed_kph = 0. self.last_cruise_stalk_pull_time = 0 # Handle pressing up and down buttons. - elif (self.enable_adaptive_cruise and + elif (CS.cruise_buttons != CruiseButtons.MAIN and self.enable_adaptive_cruise and CS.cruise_buttons != self.prev_cruise_buttons): self._update_max_acc_speed(CS) @@ -134,9 +141,9 @@ def update_stat(self, CS, enabled): if CS.v_ego < self.MIN_CRUISE_SPEED_MS: self.has_gone_below_min_speed = True - # If autoresume is not enabled, manually steering or slowing disables ACC. + # If autoresume is not enabled and not in standard CC, disable if we hit the brakes or gone below 18mph if not self.autoresume: - if not enabled or self.user_has_braked or self.has_gone_below_min_speed: + if (self.adaptive and not enabled) or self.user_has_braked or self.has_gone_below_min_speed: self.enable_adaptive_cruise = False # Notify if ACC was toggled @@ -150,7 +157,7 @@ def update_stat(self, CS, enabled): # Update the UI to show whether the current car state allows ACC. if CS.cstm_btns.get_button_status(ACCMode.BUTTON_NAME) in [ACCState.STANDBY, ACCState.NOT_READY]: - if (enabled + if ((enabled or not self.adaptive) and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) and CS.v_ego > self.MIN_CRUISE_SPEED_MS): CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.STANDBY) @@ -211,6 +218,15 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, and (CS.pcm_acc_status == CruiseState.ENABLED)): button_to_press = CruiseButtons.CANCEL + #if non addaptive and we just engaged ACC but pcm is not engaged, then engage + if (not self.adaptive) and self.enable_adaptive_cruise and (CS.pcm_acc_status != CruiseState.ENABLED): + button_to_press = CruiseButtons.MAIN + + #if plain cc, not adaptive, then just return None or Cancel + if not self.adaptive and self.enable_adaptive_cruise: + self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH + return button_to_press + #disengage if cruise is canceled if (not self.enable_adaptive_cruise) and (CS.pcm_acc_status >= 2) and (CS.pcm_acc_status <= 4): button_to_press = CruiseButtons.CANCEL diff --git a/selfdrive/car/tesla/AHB_module.py b/selfdrive/car/tesla/AHB_module.py index 1b7d8f1849844a..c452147bd8abc0 100644 --- a/selfdrive/car/tesla/AHB_module.py +++ b/selfdrive/car/tesla/AHB_module.py @@ -85,7 +85,7 @@ def update(self, CS, frame,ahbLead1): highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA return self.set_and_return(CS,frame,highLowBeamStatus,highLowBeamReason) #if lead car detected by radarD, i.e. OP has Lead, then reset timer and return OFF - if ahbLead1 is not None: + if False and ahbLead1 is not None: _debug("OP detected car") self.time_last_car_detected = tms_now highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index c5e093fb0f8eba..f929db4472dca9 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -556,12 +556,17 @@ def update(self, enabled, CS, frame, actuators, \ if (CS.pedal_interceptor_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise): speed_control_enabled = 1 cc_state = 2 + if not self.ACC.adaptive: + cc_state = 3 CS.speed_control_enabled = 1 else: CS.speed_control_enabled = 0 if (CS.pcm_acc_status == 4): #car CC enabled but not OP, display the HOLD message cc_state = 3 + else: + if (CS.pcm_acc_status == 4): + cc_state = 3 send_fake_msg = False send_fake_warning = False @@ -593,7 +598,7 @@ def update(self, enabled, CS, frame, actuators, \ if CS.useTeslaRadar and CS.hasTeslaIcIntegration: highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1) if frame % 5 == 0: - can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled)) + can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,(not CS.pedal_interceptor_available) and (not self.ACC.adaptive))) if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 @@ -722,6 +727,8 @@ def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData, CS, t self.curv1 = 0.0 #straighten the turn for ALCA self.curv0 = -self.ALCA.laneChange_direction * alcaStateData.alcaLaneWidth * alcaStateData.alcaStep / alcaStateData.alcaTotalSteps #animas late change on IC self.curv0 = clip(self.curv0, -3.5, 3.5) + 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 pp.lProb > LDW_LANE_PROBAB: diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 9729fa55c4a410..0cd44fe33ad0dc 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -130,11 +130,11 @@ def create_DAS_LR_object_msg(lane,v1Class,v1Id,v1Dx,v1Dy,v1V,v2Class,v2Id,v2Dx,v int((v2y >> 5) & 0x03) + ((v2Id << 2) & 0xFC)) return [msg_id, 0, msg.raw, 0] -def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled): +def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled,plainccenabled): msg_id = 0x65A msg_len = 3 msg = create_string_buffer(msg_len) - struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,1 if ahbIsEnabled else 0) + struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,(1 if ahbIsEnabled else 0) + ((1 if plainccenabled else 0)<<1)) return [msg_id, 0, msg.raw, 0]