diff --git a/panda/board/build.mk b/panda/board/build.mk index f435476e1faee2..fc06448ee57005 100644 --- a/panda/board/build.mk +++ b/panda/board/build.mk @@ -43,7 +43,7 @@ bin: obj/$(PROJ_NAME).bin # this flashes everything recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin -PYTHONPATH=../ python -c "from python import Panda; Panda().reset(enter_bootloader=True)" - sleep 1.0 + sleep 2.0 $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 616a2315a6fdba..c745c7574973b8 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -791,7 +791,7 @@ static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) { if (fake_DAS_counter % 50 == 0) { //send DAS_status - 0x399 MLB = DAS_op_status + 0xF0 + (DAS_speed_limit_kph << 8) + (((DAS_collision_warning << 6) + DAS_speed_limit_kph) << 16); - MHB = ((DAS_cc_state & 0x03) << 3) + (DAS_ldwStatus << 5) + + MHB = ((DAS_fleet_speed_state & 0x03) << 3) + (DAS_ldwStatus << 5) + (((DAS_hands_on_state << 2) + ((DAS_alca_state & 0x03) << 6) + DAS_fleet_speed_state) << 8) + ((( DAS_status_idx << 4) + (DAS_alca_state >> 2)) << 16); int cksm = add_tesla_cksm2(MLB, MHB, 0x399, 7); @@ -819,10 +819,15 @@ static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) { if (DAS_cc_state > 1) { //enabled or hold b4 = 0x84; b5 = 0x25; //BB lssState 0x 0x03 should be LSS_STATE_ELK enhanced LK - //DAS_RobState active 0x02 - //DAS_radarTelemetry normal 0x01 - //DAS_lssState 0x03 - //DAS_acc_report ACC_report_target_CIPV 0x01 + int _DAS_RobState = 0x02; //active + int _DAS_radarTelemetry = 0x01; // normal + int _DAS_lssState = 0x03; + int _DAS_acc_report = 0x01; //ACC_report_target_CIPV + if (DAS_fleet_speed_state == 2) { + _DAS_acc_report = 0x12; //ACC_report_fleet_speed + } + b4 = (_DAS_acc_report << 2) + ((_DAS_lssState & 0x01) << 7); + b5 =((_DAS_lssState >> 1) & 0x01) + (_DAS_radarTelemetry << 2) + (_DAS_RobState <<4); } MLB = MLB + (b4 << 24); MHB = 0x8000 + b5 + (DAS_status2_idx << 20) + (lcw << 16); diff --git a/selfdrive/car/tesla/AHB_module.py b/selfdrive/car/tesla/AHB_module.py index c452147bd8abc0..d7a92cf946e22f 100644 --- a/selfdrive/car/tesla/AHB_module.py +++ b/selfdrive/car/tesla/AHB_module.py @@ -1,5 +1,5 @@ from selfdrive.config import Conversions as CV -from cereal import tesla +from cereal import tesla,log import selfdrive.messaging as messaging import time @@ -40,6 +40,9 @@ def __init__(self,carcontroller): self.ahbInfo = messaging.sub_sock('ahbInfo', conflate=True) self.ahbInfoData = None self.ahbIsEnabled = False + self.frameInfo = messaging.sub_sock('frame', conflate=True) + self.frameInfoData = None + self.frameInfoGain = 0 def set_and_return(self,CS,frame,highLowBeamStatus,highLowBeamReason): self.prev_light_stalk_position = CS.ahbHighBeamStalkPosition @@ -55,9 +58,17 @@ def set_and_return(self,CS,frame,highLowBeamStatus,highLowBeamReason): def update(self, CS, frame,ahbLead1): tms_now = _current_time_millis() ahbInfoMsg = self.ahbInfo.receive(non_blocking=True) + frameInfoMsg = messaging.recv_one_or_none(self.frameInfo) if ahbInfoMsg is not None: self.ahbInfoData = tesla.AHBinfo.from_bytes(ahbInfoMsg) - + if frameInfoMsg is not None: + self.frameInfoData = frameInfoMsg.frame + frameInfoGain = self.frameInfoData.globalGain + exposureTime = self.frameInfoData.androidCaptureResult.exposureTime + frameDuration = self.frameInfoData.androidCaptureResult.frameDuration + if frameInfoGain != self.frameInfoGain: + self.frameInfoGain = frameInfoGain + _debug("AHB Camera has new data [ frame - " + str(self.frameInfoData.frameId) + "] = " + str(frameInfoGain) + ", exposure = " + str(exposureTime) + ", frame duration = " + str(frameDuration)) #if AHB not enabled, then return OFF if CS.ahbEnabled != 1: _debug("AHB not enabled in CID") @@ -84,6 +95,13 @@ def update(self, CS, frame,ahbLead1): highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA return self.set_and_return(CS,frame,highLowBeamStatus,highLowBeamReason) + # if gain less than max, we might see incoming car + if self.frameInfoGain < 510: + _debug("OP camera gain < 510") + self.time_last_car_detected = tms_now + highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF + highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET + 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 False and ahbLead1 is not None: _debug("OP detected car") diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 87b831049ad508..3b78faaec848b5 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -76,6 +76,9 @@ def process_hud_alert(hud_alert): class CarController(): def __init__(self, dbc_name): + self.fleet_speed_state = 0 + self.cc_counter = 0 + self.UI_splineID = -1 self.alcaStateData = None self.icLeadsData = None self.params = Params() @@ -493,6 +496,8 @@ def update(self, enabled, CS, frame, actuators, \ accel_min = -15 accel_max = 5 acc_speed_kph = 0 + send_fake_warning = False + send_fake_msg = False if enabled: #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning alca_state = 0x01 @@ -560,10 +565,6 @@ def update(self, enabled, CS, frame, actuators, \ else: if (CS.pcm_acc_status == 4): cc_state = 3 - - send_fake_msg = False - send_fake_warning = False - if enabled: if frame % 2 == 0: send_fake_msg = True @@ -588,15 +589,27 @@ def update(self, enabled, CS, frame, actuators, \ self.DAS_221_lcAborting = 1 self.warningCounter = 300 self.warningNeeded = 1 - if CS.useTeslaRadar and CS.hasTeslaIcIntegration: + if CS.hasTeslaIcIntegration: highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1) if frame % 5 == 0: - fleet_speed_state = 0x00 #fleet speed unavailable - if CS.medianFleetSpeedMPS > 0: - fleet_speed_state = 0x01 #fleet speed available - if (CS.maxdrivespeed > 0) and CS.useTeslaMapData and (CS.mapAwareSpeed or (CS.baseMapSpeedLimitMPS <2.7)): - fleet_speed_state = 0x02 #fleet speed active - can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,fleet_speed_state)) + self.cc_counter = (self.cc_counter + 1) % 40 #use this to change status once a second + self.fleet_speed_state = 0x00 #fleet speed unavailable + if (CS.medianFleetSpeedMPS > 0) and (CS.mapAwareSpeed) and (CS.splineLocConfidence > 60) and (CS.UI_splineID > 0): + if CS.speed_control_enabled == 1: + self.fleet_speed_state = 0x02 #fleet speed enabled + else: + self.fleet_speed_state = 0x01 #fleet speed available + else: + if CS.speed_control_enabled == 1: + self.fleet_speed_state = 0x00 #fleet speed hold + self.UI_splineID = CS.UI_splineID + #print ("Fleet Speed State = ", self.fleet_speed_state) + can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,self.fleet_speed_state)) + if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02): + CS.v_cruise_pcm = CS.v_cruise_pcm + 1 + send_fake_msg = True + if (self.cc_counter == 3): + send_fake_msg = True if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 39fc68f65f59ac..226feaf4a7a39a 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -129,6 +129,7 @@ def get_can_signals(CP): ("UI_medianFleetSpeedMPS", "UI_driverAssistRoadSign", 0), ("UI_topQrtlFleetSpeedMPS", "UI_driverAssistRoadSign", 0), ("UI_splineLocConfidence", "UI_driverAssistRoadSign", 0), + ("UI_splineID", "UI_driverAssistRoadSign", 0), ("UI_baseMapSpeedLimitMPS", "UI_driverAssistRoadSign", 0), ("UI_bottomQrtlFleetSpeedMPS", "UI_driverAssistRoadSign", 0), ("UI_rampType", "UI_driverAssistRoadSign", 0), @@ -275,6 +276,7 @@ def __init__(self, CP): self.speedLimit = 0 self.meanFleetSplineSpeedMPS = 0. + self.UI_splineID = 0 self.meanFleetSplineAccelMPS2 = 0. self.medianFleetSpeedMPS = 0. self.topQrtlFleetSplineSpeedMPS = 0. @@ -564,6 +566,7 @@ def update(self, cp, epas_cp, pedal_cp): self.meanFleetSplineAccelMPS2 = cp.vl["UI_driverAssistRoadSign"]["UI_meanFleetSplineAccelMPS2"] self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_medianFleetSpeedMPS"] self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"]["UI_splineLocConfidence"] + self.UI_splineID = cp.vl["UI_driverAssistRoadSign"]["UI_splineID"] self.rampType = cp.vl["UI_driverAssistRoadSign"]["UI_rampType"] if rdSignMsg == 3: