From de34cbe97ba8ad50c8a12ded45f98758b4f34117 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Sun, 24 Nov 2019 17:54:30 -0500 Subject: [PATCH] fix CC for ACC --- panda/board/safety/safety_tesla.h | 6 +++++- selfdrive/car/tesla/ACC_module.py | 2 +- selfdrive/car/tesla/carcontroller.py | 5 +++-- selfdrive/car/tesla/teslacan.py | 8 ++++---- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 6fa6142c482474..70338c6998c28f 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -1549,7 +1549,6 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) DAS_high_low_beam_request = b0; DAS_high_low_beam_reason = b1; DAS_ahb_is_enabled = b2 & 0x01; - DAS_plain_cc_enabled = (b2 >> 1) & 0x01; //intercept and do not forward return false; } @@ -1575,6 +1574,11 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) DAS_turn_signal_request = ((b2 & 0xC0) >> 6); DAS_forward_collision_warning = ((b2 & 0x10) >> 4); DAS_units_included = ((b2 & 0x20) >> 5); + if (((b2 >> 3) & 0x01) == 0) { + DAS_plain_cc_enabled = 1; + } else { + DAS_plain_cc_enabled = 0; + } DAS_hands_on_state = (b2 & 0x07); DAS_cc_state = ((b3 & 0xC0)>>6); DAS_usingPedal = ((b3 & 0x20) >> 5); diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index f9bd8b3b9a1f9f..f97004e6593a5c 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -223,7 +223,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, 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: + 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 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index f929db4472dca9..c9971969c9e681 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -598,13 +598,14 @@ 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,(not CS.pedal_interceptor_available) and (not self.ACC.adaptive))) + can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled)) if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 + adaptive_cruise = 1 if (not CS.pedal_interceptor_available and self.ACC.adaptive) or CS.pedal_interceptor_available else 0 can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ - turn_signal_needed,forward_collision_warning, hands_on_state, \ + turn_signal_needed,forward_collision_warning, adaptive_cruise, hands_on_state, \ cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \ CS.v_cruise_pcm, #acc_speed_limit_mph, CS.speedLimitToIc, #speed_limit_to_car, diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 0cd44fe33ad0dc..f246443986ff7b 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -130,17 +130,17 @@ 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,plainccenabled): +def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled): 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) + ((1 if plainccenabled else 0)<<1)) + struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,(1 if ahbIsEnabled else 0)) return [msg_id, 0, msg.raw, 0] def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ - turn_signal_needed,forward_collission_warning,hands_on_state, \ + turn_signal_needed,forward_collission_warning,adaptive_cruise, hands_on_state, \ cc_state, pedal_state, alca_state, \ acc_speed_limit_mph, legal_speed_limit, @@ -153,7 +153,7 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll c_apply_steer = int(((int( apply_angle * 10 + 0x4000 )) & 0x7FFF) + (enable_steer_control << 15)) struct.pack_into('BBBBBBBB', msg, 0,int((speed_control_enabled << 7) + (speed_override << 6) + (apUnavailable << 5) + (collision_warning << 4) + op_status), \ int(acc_speed_kph), \ - int((turn_signal_needed << 6) + (units_included << 5) + (forward_collission_warning << 4) + hands_on_state), \ + int((turn_signal_needed << 6) + (units_included << 5) + (forward_collission_warning << 4) + (adaptive_cruise << 3) + hands_on_state), \ int((cc_state << 6) + (pedal_state << 5) + alca_state), \ int(acc_speed_limit_mph), int(legal_speed_limit),