From 7c7367f6c1344863a64c5e5ac56cf007b64e39cf Mon Sep 17 00:00:00 2001 From: Raf Date: Mon, 9 Nov 2020 08:07:58 -0500 Subject: [PATCH] Revert "Map based speed limit improvements (#146)" This reverts commit 9a663809734a8cafa755ad7e4415673db14d8e8b. --- opendbc/tesla_can.dbc | 24 +++--- opendbc/tesla_can1916.dbc | 24 +++--- selfdrive/car/tesla/ACC_module.py | 17 ++--- selfdrive/car/tesla/PCC_module.py | 24 +++--- selfdrive/car/tesla/carcontroller.py | 45 ++++++++--- selfdrive/car/tesla/carstate.py | 75 +++++++++---------- .../car/tesla/speed_utils/fleet_speed.py | 8 +- 7 files changed, 120 insertions(+), 97 deletions(-) diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc index b48d1319cbffd5..84c07d4c6245fd 100644 --- a/opendbc/tesla_can.dbc +++ b/opendbc/tesla_can.dbc @@ -284,15 +284,16 @@ BO_ 643 BODY_R1: 8 GTW SG_ VTA_Alm_Actv : 13|1@0+ (1,0) [0|1] "" NEO SG_ WprOutsdPkPosn : 29|1@0+ (1,0) [0|1] "" NEO -BO_ 760 UI_gpsVehicleSpeed: 8 GTW - SG_ UI_gpsHDOP : 0|8@1+ (0.1,0) [0|25.5] "1" DAS - SG_ UI_gpsVehicleHeading : 8|16@1+ (0.0078125,0) [0|511.9921875] "deg" DAS - SG_ UI_gpsVehicleSpeed : 24|16@1+ (0.00390625,0) [0|250.996] "km/hr" Vector__XXX - SG_ UI_userSpeedOffset : 40|6@1+ (1,-30) [-30|33] "kph/mph" DAS - SG_ UI_mapSpeedLimitUnits : 46|1@1+ (1,0) [0|1] "" DAS - SG_ UI_userSpeedOffsetUnits : 47|1@1+ (1,0) [0|1] "" DAS - SG_ UI_mppSpeedLimit : 48|5@1+ (5,0) [0|155] "kph/mph" DAS - SG_ UI_gpsNmeaMIA : 53|1@1+ (1,0) [0|0] "" DAS +BO_ 760 MCU_gpsVehicleSpeed: 8 GTW + SG_ MCU_gpsHDOP : 0|8@1+ (0.1,0) [0|25.5] "1" DAS + SG_ MCU_gpsVehicleHeading : 8|16@1+ (0.0078125,0) [0|511.9921875] "deg" DAS + SG_ MCU_gpsVehicleSpeed : 24|16@1+ (0.00390625,0) [0|250.996] "km/hr" DAS + SG_ MCU_userSpeedOffset : 40|6@1+ (1,-30) [-30|33] "kph/mph" DAS + SG_ MCU_mapSpeedLimitUnits : 46|1@1+ (1,0) [0|1] "" DAS + SG_ MCU_userSpeedOffsetUnits : 47|1@1+ (1,0) [0|1] "" DAS + SG_ MCU_mppSpeedLimit : 48|5@1+ (5,0) [0|155] "kph/mph" DAS + SG_ MCU_gpsNmeaMIA : 53|1@1+ (1,0) [0|0] "" DAS + SG_ MCU_gpsAntennaDisconnected : 54|1@1+ (1,0) [0|0] "" DAS BO_ 536 MCU_chassisControl: 8 GTW SG_ MCU_dasDebugEnable : 0|1@1+ (1,0) [0|0] "" NEO @@ -664,9 +665,8 @@ VAL_ 309 ESP_absFaultLamp 0 "OFF" 1 "ON" ; VAL_ 309 ESP_espOffLamp 0 "OFF" 1 "ON" ; VAL_ 309 ESP_stabilityControlSts 2 "ENGAGED" 3 "FAULTED" 5 "INIT" 4 "NOT_CONFIGURED" 0 "OFF" 1 "ON" ; VAL_ 309 ESP_tcLampFlash 1 "FLASH" 0 "OFF" ; -VAL_ 568 UI_mapSpeedLimit 31 "SNA" 30 "UNLIMITED" 29 "LESS_OR_EQ_160" 28 "LESS_OR_EQ_150" 27 "LESS_OR_EQ_140" 26 "LESS_OR_EQ_130" 25 "LESS_OR_EQ_120" 24 "LESS_OR_EQ_115" 23 "LESS_OR_EQ_110" 22 "LESS_OR_EQ_105" 21 "LESS_OR_EQ_100" 20 "LESS_OR_EQ_95" 19 "LESS_OR_EQ_90" 18 "LESS_OR_EQ_85" 17 "LESS_OR_EQ_80" 16 "LESS_OR_EQ_75" 15 "LESS_OR_EQ_70" 14 "LESS_OR_EQ_65" 13 "LESS_OR_EQ_60" 12 "LESS_OR_EQ_55" 11 "LESS_OR_EQ_50" 10 "LESS_OR_EQ_45" 9 "LESS_OR_EQ_40" 8 "LESS_OR_EQ_35" 7 "LESS_OR_EQ_30" 6 "LESS_OR_EQ_25" 5 "LESS_OR_EQ_20" 4 "LESS_OR_EQ_15" 3 "LESS_OR_EQ_10" 2 "LESS_OR_EQ_7" 1 "LESS_OR_EQ_5" 0 "UNKNOWN" ; -VAL_ 760 UI_mapSpeedLimitUnits 1 "KPH" 0 "MPH" ; -VAL_ 760 UI_userSpeedOffsetUnits 1 "KPH" 0 "MPH" ; +VAL_ 760 MCU_speedLimitUnits 1 "KPH" 0 "MPH" ; +VAL_ 760 MCU_userSpeedOffsetUnits 1 "KPH" 0 "MPH" ; VAL_ 643 AirTemp_Insd 255 "SNA" ; VAL_ 643 AirTemp_Outsd 254 "INIT" 255 "SNA" ; VAL_ 643 Bckl_Sw_RL_Stat_SAM_R 2 "FLT" 1 "NOT" 0 "OK" 3 "SNA" ; diff --git a/opendbc/tesla_can1916.dbc b/opendbc/tesla_can1916.dbc index a02da2bb4b4ec8..2d3527b685327c 100644 --- a/opendbc/tesla_can1916.dbc +++ b/opendbc/tesla_can1916.dbc @@ -284,15 +284,16 @@ BO_ 643 BODY_R1: 8 GTW SG_ VTA_Alm_Actv : 13|1@0+ (1,0) [0|1] "" NEO SG_ WprOutsdPkPosn : 29|1@0+ (1,0) [0|1] "" NEO -BO_ 760 UI_gpsVehicleSpeed: 8 GTW - SG_ UI_gpsHDOP : 0|8@1+ (0.1,0) [0|25.5] "1" DAS - SG_ UI_gpsVehicleHeading : 8|16@1+ (0.0078125,0) [0|511.9921875] "deg" DAS - SG_ UI_gpsVehicleSpeed : 24|16@1+ (0.00390625,0) [0|250.996] "km/hr" Vector__XXX - SG_ UI_userSpeedOffset : 40|6@1+ (1,-30) [-30|33] "kph/mph" DAS - SG_ UI_mapSpeedLimitUnits : 46|1@1+ (1,0) [0|1] "" DAS - SG_ UI_userSpeedOffsetUnits : 47|1@1+ (1,0) [0|1] "" DAS - SG_ UI_mppSpeedLimit : 48|5@1+ (5,0) [0|155] "kph/mph" DAS - SG_ UI_gpsNmeaMIA : 53|1@1+ (1,0) [0|0] "" DAS +BO_ 760 MCU_gpsVehicleSpeed: 8 GTW + SG_ MCU_gpsHDOP : 0|8@1+ (0.1,0) [0|25.5] "1" DAS + SG_ MCU_gpsVehicleHeading : 8|16@1+ (0.0078125,0) [0|511.9921875] "deg" DAS + SG_ MCU_gpsVehicleSpeed : 24|16@1+ (0.00390625,0) [0|250.996] "km/hr" DAS + SG_ MCU_userSpeedOffset : 40|6@1+ (1,-30) [-30|33] "kph/mph" DAS + SG_ MCU_mapSpeedLimitUnits : 46|1@1+ (1,0) [0|1] "" DAS + SG_ MCU_userSpeedOffsetUnits : 47|1@1+ (1,0) [0|1] "" DAS + SG_ MCU_mppSpeedLimit : 48|5@1+ (5,0) [0|155] "kph/mph" DAS + SG_ MCU_gpsNmeaMIA : 53|1@1+ (1,0) [0|0] "" DAS + SG_ MCU_gpsAntennaDisconnected : 54|1@1+ (1,0) [0|0] "" DAS BO_ 536 MCU_chassisControl: 8 GTW SG_ MCU_dasDebugEnable : 0|1@1+ (1,0) [0|0] "" NEO @@ -664,9 +665,8 @@ VAL_ 309 ESP_absFaultLamp 0 "OFF" 1 "ON" ; VAL_ 309 ESP_espOffLamp 0 "OFF" 1 "ON" ; VAL_ 309 ESP_stabilityControlSts 2 "ENGAGED" 3 "FAULTED" 5 "INIT" 4 "NOT_CONFIGURED" 0 "OFF" 1 "ON" ; VAL_ 309 ESP_tcLampFlash 1 "FLASH" 0 "OFF" ; -VAL_ 568 UI_mapSpeedLimit 31 "SNA" 30 "UNLIMITED" 29 "LESS_OR_EQ_160" 28 "LESS_OR_EQ_150" 27 "LESS_OR_EQ_140" 26 "LESS_OR_EQ_130" 25 "LESS_OR_EQ_120" 24 "LESS_OR_EQ_115" 23 "LESS_OR_EQ_110" 22 "LESS_OR_EQ_105" 21 "LESS_OR_EQ_100" 20 "LESS_OR_EQ_95" 19 "LESS_OR_EQ_90" 18 "LESS_OR_EQ_85" 17 "LESS_OR_EQ_80" 16 "LESS_OR_EQ_75" 15 "LESS_OR_EQ_70" 14 "LESS_OR_EQ_65" 13 "LESS_OR_EQ_60" 12 "LESS_OR_EQ_55" 11 "LESS_OR_EQ_50" 10 "LESS_OR_EQ_45" 9 "LESS_OR_EQ_40" 8 "LESS_OR_EQ_35" 7 "LESS_OR_EQ_30" 6 "LESS_OR_EQ_25" 5 "LESS_OR_EQ_20" 4 "LESS_OR_EQ_15" 3 "LESS_OR_EQ_10" 2 "LESS_OR_EQ_7" 1 "LESS_OR_EQ_5" 0 "UNKNOWN" ; -VAL_ 760 UI_mapSpeedLimitUnits 1 "KPH" 0 "MPH" ; -VAL_ 760 UI_userSpeedOffsetUnits 1 "KPH" 0 "MPH" ; +VAL_ 760 MCU_speedLimitUnits 1 "KPH" 0 "MPH" ; +VAL_ 760 MCU_userSpeedOffsetUnits 1 "KPH" 0 "MPH" ; VAL_ 643 AirTemp_Insd 255 "SNA" ; VAL_ 643 AirTemp_Outsd 254 "INIT" 255 "SNA" ; VAL_ 643 Bckl_Sw_RL_Stat_SAM_R 2 "FLT" 1 "NOT" 0 "OK" 3 "SNA" ; diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 68ee860196f4c9..e2cb1bb375387c 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -183,17 +183,16 @@ def _update_max_acc_speed(self, CS): self.acc_speed_kph = min(self.acc_speed_kph, 170) self.acc_speed_kph = max(self.acc_speed_kph, 0) - # Decide which cruise control buttons to simluate to get the car to the desired speed. - def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, set_speed_limit_active, speed_limit_offset): + # Decide which cruise control buttons to simluate to get the car to the + # desired speed. + def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, speed_limit_valid, set_speed_limit_active, speed_limit_offset): # Adaptive cruise control self.prev_speed_limit_kph = self.speed_limit_kph - if set_speed_limit_active and speed_limit_kph > 0: - self.speed_limit_kph = speed_limit_kph + speed_limit_offset - if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph): + if speed_limit_valid and set_speed_limit_active and (speed_limit_kph >= 10): + self.speed_limit_kph = speed_limit_kph + speed_limit_offset + if not (int(self.prev_speed_limit_kph) == int(self.speed_limit_kph)): self.acc_speed_kph = self.speed_limit_kph self.fleet_speed.reset_averager() - else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway) - self.speed_limit_kph = 0. current_time_ms = _current_time_millis() if CruiseButtons.should_be_throttled(CS.cruise_buttons): self.human_cruise_action_time = current_time_ms @@ -232,7 +231,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, # and speed more directly. # Bring in the lead car distance from the radarState feed - button_to_press = self._calc_follow_button(CS, lead_1, speed_limit_kph, set_speed_limit_active, speed_limit_offset, frame) + button_to_press = self._calc_follow_button(CS, lead_1, speed_limit_kph, speed_limit_valid, set_speed_limit_active, speed_limit_offset, frame) if button_to_press: self.automated_cruise_action_time = current_time_ms # If trying to slow below the min cruise speed, just cancel cruise. @@ -248,7 +247,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, return button_to_press # function to calculate the cruise button based on a safe follow distance - def _calc_follow_button(self, CS, lead_car, speed_limit_kph, set_speed_limit_active, speed_limit_offset, frame): + def _calc_follow_button(self, CS, lead_car, speed_limit_kph, speed_limit_valid, set_speed_limit_active, speed_limit_offset, frame): if lead_car is None: return None # Desired gap (in seconds) between cars. diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index fb2e7c6c42794e..dd86783a3add5b 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -261,16 +261,18 @@ def update_stat(self, CS, frame): # Handle pressing up and down buttons. elif (self.enable_pedal_cruise and CS.cruise_buttons != self.prev_cruise_buttons): - # Real stalk command while PCC is already enabled. Adjust the max PCC speed if necessary. - # We round the target speed in the user's units of measurement to avoid jumpy speed readings - actual_speed_kph_uom_rounded = int(CS.v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph + # Real stalk command while PCC is already enabled. Adjust the max PCC + # speed if necessary. + actual_speed_kph = CS.v_ego * CV.MS_TO_KPH if CS.cruise_buttons == CruiseButtons.RES_ACCEL: - self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph_uom_rounded) + speed_uom_kph + self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + speed_uom_kph elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: - self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph_uom_rounded) + 5 * speed_uom_kph + self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + 5 * speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_SET: - self.pedal_speed_kph = self.pedal_speed_kph - speed_uom_kph + #self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) - speed_uom_kph + self.pedal_speed_kph =self.pedal_speed_kph - speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: + #self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) - 5 * speed_uom_kph self.pedal_speed_kph = self.pedal_speed_kph - 5 * speed_uom_kph # Clip PCC speed between 0 and 170 KPH. self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) @@ -308,7 +310,7 @@ def update_stat(self, CS, frame): return can_sends - def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, set_speed_limit_active, speed_limit_offset,alca_enabled): + def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, speed_limit_valid, set_speed_limit_active, speed_limit_offset,alca_enabled): idx = self.pedal_idx self.prev_speed_limit_kph = self.speed_limit_kph @@ -329,14 +331,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s #print ("Torque level at detection %s" % (CS.torqueLevel)) #print ("Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)) - if set_speed_limit_active and speed_limit_ms > 0: - self.speed_limit_kph = (speed_limit_ms + speed_limit_offset) * CV.MS_TO_KPH - if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph): + if speed_limit_valid and set_speed_limit_active and (speed_limit_ms > 2.7): + self.speed_limit_kph = (speed_limit_ms + speed_limit_offset) * CV.MS_TO_KPH + if not (int(self.prev_speed_limit_kph) == int(self.speed_limit_kph)): self.pedal_speed_kph = self.speed_limit_kph # reset MovingAverage for fleet speed when speed limit changes self.fleet_speed.reset_averager() - else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway) - self.speed_limit_kph = 0. self.pedal_idx = (self.pedal_idx + 1) % 16 if not self.pcc_available or not enabled: diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 51743a30960ff1..f5541ccf73fffb 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -105,6 +105,9 @@ def __init__(self, dbc_name): self.icCarLR = messaging.sub_sock('uiIcCarLR', conflate=True) self.alcaState = messaging.sub_sock('alcaState', conflate=True) self.gpsLocationExternal = None + self.speedlimit_ms = 0. + self.speedlimit_valid = False + self.speedlimit_units = 0 self.opState = 0 # 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning self.accPitch = 0. self.accRoll = 0. @@ -117,7 +120,7 @@ def __init__(self, dbc_name): self.gyroYaw = 0. self.set_speed_limit_active = False self.speed_limit_offset = 0. - self.speed_limit_ms = 0. + self.speed_limit_for_cc = 0. # for warnings self.warningCounter = 0 @@ -322,14 +325,16 @@ def update(self, enabled, CS, frame, actuators, \ if CS.hasTeslaIcIntegration: self.set_speed_limit_active = True self.speed_limit_offset = CS.userSpeedLimitOffsetKph + self.speed_limit_for_cc = CS.userSpeedLimitKph + #print self.speed_limit_for_cc else: self.set_speed_limit_active = (self.params.get("SpeedLimitOffset") is not None) and (self.params.get("LimitSetSpeed") == "1") if self.set_speed_limit_active: self.speed_limit_offset = float(self.params.get("SpeedLimitOffset")) - if not self.isMetric: - self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS else: self.speed_limit_offset = 0. + if not self.isMetric: + self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS if CS.useTeslaGPS and (frame % 10 == 0): if self.gpsLocationExternal is None: self.gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') @@ -412,8 +417,21 @@ def update(self, enabled, CS, frame, actuators, \ # DAS_acc_speed_limit (8), # DAS_speed_limit_units(8) #send fake_das data as 0x553 - # TODO: forward collision warning + # TODO: forward collission warning + if CS.hasTeslaIcIntegration: + self.set_speed_limit_active = True + self.speed_limit_offset = CS.userSpeedLimitOffsetKph + # only change the speed limit when we have a valid vaue + if CS.userSpeedLimitKph >= 10: + self.speed_limit_for_cc = CS.userSpeedLimitKph + + if CS.useTeslaMapData: + self.speedlimit_ms = CS.speedLimitKph * CV.KPH_TO_MS + self.speedlimit_valid = True + if self.speedlimit_ms == 0: + self.speedlimit_valid = False + self.speedlimit_units = self.speedUnits(fromMetersPerSecond = self.speedlimit_ms) if frame % 10 == 0: speedlimitMsg = None if self.speedlimit is not None: @@ -426,11 +444,13 @@ def update(self, enabled, CS, frame, actuators, \ trafficeventsMsgs = None if self.trafficevents is not None: trafficeventsMsgs = messaging.recv_sock(self.trafficevents) - if CS.hasTeslaIcIntegration: - self.speed_limit_ms = CS.speed_limit_ms if (speedlimitMsg is not None) and not CS.useTeslaMapData: + #get speed limit lmd = speedlimitMsg.liveMapData - self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0 + self.speedlimit_ms = lmd.speedLimit + self.speedlimit_valid = lmd.speedLimitValid + self.speedlimit_units = self.speedUnits(fromMetersPerSecond = self.speedlimit_ms) + self.speed_limit_for_cc = self.speedlimit_ms * CV.MS_TO_KPH if icLeadsMsg is not None: self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg) if radarStateMsg is not None: @@ -460,6 +480,7 @@ def update(self, enabled, CS, frame, actuators, \ forward_collision_warning = 1 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold cc_state = 1 + speed_limit_to_car = int(self.speedlimit_units) alca_state = 0x00 speed_override = 0 @@ -587,7 +608,7 @@ def update(self, enabled, CS, frame, actuators, \ self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \ cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \ CS.v_cruise_pcm, - CS.DAS_fusedSpeedLimit, + CS.speedLimitToIc, #speed_limit_to_car, apply_angle, 1 if enable_steer_control else 0, park_brake_request)) @@ -611,7 +632,7 @@ def update(self, enabled, CS, frame, actuators, \ can_sends.append(teslacan.create_enabled_eth_msg(1)) if (not self.PCC.pcc_available) and frame % 5 == 0: # acc processed at 20Hz cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ - self.speed_limit_ms * CV.MS_TO_KPH, + self.speed_limit_for_cc, self.speedlimit_valid, \ self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( @@ -623,7 +644,7 @@ def update(self, enabled, CS, frame, actuators, \ apply_accel = 0. if self.PCC.pcc_available and frame % 5 == 0: # pedal processed at 20Hz apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ - self.speed_limit_ms, + self.speed_limit_for_cc * CV.KPH_TO_MS, self.speedlimit_valid, \ self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled) can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) self.last_angle = apply_angle @@ -794,6 +815,10 @@ def handleTrafficEvents(self, trafficEventsMsgs): messages.append(teslacan.create_fake_DAS_sign_msg(self.roadSignType,self.roadSignStopDist,self.roadSignColor,self.roadSignControlActive)) return messages + # 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_ldw(self, CS, frame): if not CS.enableLdw: return False diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index e901b02165865d..a3296f04beaaaf 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -32,12 +32,12 @@ def parse_gear_shifter(can_gear_shifter, car_fingerprint): def get_can_signals(CP): # this function generates lists for signal, messages and initial values signals = [ - ("UI_gpsVehicleHeading", "UI_gpsVehicleSpeed", 0), - ("UI_gpsVehicleSpeed", "UI_gpsVehicleSpeed", 0), - ("UI_userSpeedOffset", "UI_gpsVehicleSpeed", 0), - ("UI_userSpeedOffsetUnits", "UI_gpsVehicleSpeed", 0), - ("UI_mppSpeedLimit", "UI_gpsVehicleSpeed", 0), - ("UI_mapSpeedLimitUnits", "UI_gpsVehicleSpeed", 0), + ("MCU_gpsVehicleHeading", "MCU_gpsVehicleSpeed", 0), + ("MCU_gpsVehicleSpeed", "MCU_gpsVehicleSpeed", 0), + ("MCU_userSpeedOffset", "MCU_gpsVehicleSpeed", 0), + ("MCU_userSpeedOffsetUnits", "MCU_gpsVehicleSpeed", 0), + ("MCU_mppSpeedLimit", "MCU_gpsVehicleSpeed", 0), + ("MCU_mapSpeedLimitUnits", "MCU_gpsVehicleSpeed", 0), ("MCU_gpsAccuracy", "MCU_locationStatus", 0), ("MCU_latitude", "MCU_locationStatus", 0), ("MCU_longitude", "MCU_locationStatus", 0), @@ -275,6 +275,9 @@ def __init__(self, CP): self.roadCurvC2 = 0. self.roadCurvC3 = 0. + self.speedLimitUnits = 0 + self.speedLimit = 0 + self.meanFleetSplineSpeedMPS = 0. self.UI_splineID = 0 self.meanFleetSplineAccelMPS2 = 0. @@ -296,9 +299,8 @@ def __init__(self, CP): self.gpsHeading = 0. self.gpsVehicleSpeed = 0. - self.speed_limit_ms = 0. + self.userSpeedLimitKph = 0. self.userSpeedLimitOffsetKph = 0. - self.DAS_fusedSpeedLimit = 0 self.brake_only = CP.enableCruise self.last_cruise_stalk_pull_time = 0 @@ -413,6 +415,7 @@ def __init__(self, CP): self.angle_offset = 0. self.init_angle_offset = False + self.speedLimitToIc = 0 #AHB params self.ahbHighBeamStalkPosition = 0 @@ -436,16 +439,6 @@ def config_ui_buttons(self, pcc_available, pcc_blocked_by_acc_mode): btn.btn_label2 = self.btns_init[1][2][1] self.cstm_btns.update_ui_buttons(1, 1) - def _convert_to_DAS_fusedSpeedLimit(self, speed_limit_uom, speed_limit_type): - if speed_limit_uom > 0: - if speed_limit_type == 0x1E: # Autobahn with no speed limit - return 0x1F # no speed limit sign - return int(speed_limit_uom / 5 + 0.5) # sign ID in 5 kph/mph increments (7 shows as 5) - else: - if speed_limit_type == 0x1F: # SNA (parking lot, no public road, etc.) - return 0 # no sign - return 1 # show 5 kph/mph for unknown limit where we should have one - def compute_speed(self): # if one of them is zero, select max of the two if self.meanFleetSplineSpeedMPS == 0 or self.medianFleetSpeedMPS == 0: @@ -457,7 +450,7 @@ def compute_speed(self): if self.splineLocConfidence > 60: self.mapBasedSuggestedSpeed = (self.splineLocConfidence * self.meanFleetSplineSpeedMPS + (100-self.splineLocConfidence) * self.bottomQrtlFleetSpeedMPS ) / 100. else: - self.mapBasedSuggestedSpeed = self.speed_limit_ms + self.mapBasedSuggestedSpeed = self.baseMapSpeedLimitMPS if self.rampType > 0: #we are on a ramp, use the spline info if available if self.splineBasedSuggestedSpeed > 0: @@ -524,8 +517,8 @@ def update(self, cp, epas_cp, pedal_cp): self.gpsLatitude = cp.vl['MCU_locationStatus']["MCU_latitude"] self.gpsAccuracy = cp.vl['MCU_locationStatus']["MCU_gpsAccuracy"] self.gpsElevation = cp.vl['MCU_locationStatus2']["MCU_elevation"] - self.gpsHeading = cp.vl['UI_gpsVehicleSpeed']["UI_gpsVehicleHeading"] - self.gpsVehicleSpeed = cp.vl['UI_gpsVehicleSpeed']["UI_gpsVehicleSpeed"] * CV.KPH_TO_MS + self.gpsHeading = cp.vl['MCU_gpsVehicleSpeed']["MCU_gpsVehicleHeading"] + self.gpsVehicleSpeed = cp.vl['MCU_gpsVehicleSpeed']["MCU_gpsVehicleSpeed"] * CV.KPH_TO_MS if (self.hasTeslaIcIntegration): #BB: AutoSteer enabled does not work unless we do old style port mapping on MCU @@ -542,18 +535,32 @@ def update(self, cp, epas_cp, pedal_cp): self.ahbHiBeamOn = cp.vl["BODY_R1"]["HiBm_On"] self.ahbNightMode = cp.vl["BODY_R1"]["LgtSens_Night"] - usu = cp.vl['UI_gpsVehicleSpeed']["UI_userSpeedOffsetUnits"] + usu = cp.vl['MCU_gpsVehicleSpeed']["MCU_userSpeedOffsetUnits"] if usu == 1: - self.userSpeedLimitOffsetKph = cp.vl['UI_gpsVehicleSpeed']["UI_userSpeedOffset"] + self.userSpeedLimitOffsetKph = cp.vl['MCU_gpsVehicleSpeed']["MCU_userSpeedOffset"] else: - self.userSpeedLimitOffsetKph = cp.vl['UI_gpsVehicleSpeed']["UI_userSpeedOffset"] * CV.MPH_TO_KPH - msu = cp.vl['UI_gpsVehicleSpeed']["UI_mapSpeedLimitUnits"] - map_speed_uom_to_ms = CV.KPH_TO_MS if msu == 1 else CV.MPH_TO_MS - map_speed_ms_to_uom = CV.MS_TO_KPH if msu == 1 else CV.MS_TO_MPH - speed_limit_type = int(cp.vl["UI_driverAssistMapData"]["UI_mapSpeedLimit"]) + self.userSpeedLimitOffsetKph = cp.vl['MCU_gpsVehicleSpeed']["MCU_userSpeedOffset"] * CV.MPH_TO_KPH + msu = cp.vl['MCU_gpsVehicleSpeed']["MCU_mapSpeedLimitUnits"] + if msu == 1: + self.userSpeedLimitKph = cp.vl['MCU_gpsVehicleSpeed']["MCU_mppSpeedLimit"] + 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] + 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"]) + #BB unsure yet but DBC tells us that the map data has 0x00 as unknown, 0x1E as UNLIMITED and 0x1F as SNA while + #the DAS_status has 0x00 as UNKNOWN/SNA and 0x1F as UNLIMITED. Needs testing on Autobahn + if self.speedLimitToIc == 0x1F: # SNA + self.speedLimitToIc = 0 + elif self.speedLimitToIc == 0x1E: # no speed limit + self.speedLimitToIc = 0x1F + elif self.speedLimitToIc >= 2: + self.speedLimitToIc -= 1 #map data has 7 in second position rdSignMsg = cp.vl["UI_driverAssistRoadSign"]["UI_roadSign"] - if rdSignMsg == 4: # ROAD_SIGN_SPEED_SPLINE + if rdSignMsg == 4: self.meanFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_meanFleetSplineSpeedMPS"] self.meanFleetSplineAccelMPS2 = cp.vl["UI_driverAssistRoadSign"]["UI_meanFleetSplineAccelMPS2"] self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_medianFleetSpeedMPS"] @@ -561,20 +568,12 @@ def update(self, cp, epas_cp, pedal_cp): self.UI_splineID = cp.vl["UI_driverAssistRoadSign"]["UI_splineID"] self.rampType = cp.vl["UI_driverAssistRoadSign"]["UI_rampType"] - elif rdSignMsg == 3: # ROAD_SIGN_SPEED_LIMIT + 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"] - # we round the speed limit in the map's units of measurement to fix noisy data (there are no signs with a limit of 79.2 kph) - self.baseMapSpeedLimitMPS = int(self.baseMapSpeedLimitMPS * map_speed_ms_to_uom + 0.99) / map_speed_ms_to_uom self.bottomQrtlFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_bottomQrtlFleetSpeedMPS"] - if self.baseMapSpeedLimitMPS > 0 and (speed_limit_type != 0x1F or self.baseMapSpeedLimitMPS <= 5.56): - self.speed_limit_ms = self.baseMapSpeedLimitMPS # this one is earlier than the actual sign but can also be unreliable, so we ignore it on SNA at higher speeds - else: - self.speed_limit_ms = cp.vl['UI_gpsVehicleSpeed']["UI_mppSpeedLimit"] * map_speed_uom_to_ms - self.DAS_fusedSpeedLimit = self._convert_to_DAS_fusedSpeedLimit(self.speed_limit_ms * map_speed_ms_to_uom, speed_limit_type) - self.compute_speed() # 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent) diff --git a/selfdrive/car/tesla/speed_utils/fleet_speed.py b/selfdrive/car/tesla/speed_utils/fleet_speed.py index fe5a6db4a27adf..fc1e1f7fd3af69 100644 --- a/selfdrive/car/tesla/speed_utils/fleet_speed.py +++ b/selfdrive/car/tesla/speed_utils/fleet_speed.py @@ -11,8 +11,8 @@ def adjust(self, CS, max_speed_ms, frame): if CS.mapAwareSpeed and self.is_valid(CS, max_speed_ms): self.frame_last_adjustment = frame # if max speed is greater than the speed limit, apply a relative offset to map speed - if CS.rampType == 0 and max_speed_ms > CS.speed_limit_ms > CS.map_suggested_speed: - return self.speed_avg.add(max_speed_ms * CS.map_suggested_speed / CS.speed_limit_ms) + if CS.rampType == 0 and max_speed_ms > CS.baseMapSpeedLimitMPS > CS.map_suggested_speed: + return self.speed_avg.add(max_speed_ms * CS.map_suggested_speed / CS.baseMapSpeedLimitMPS) else: return self.speed_avg.add(CS.map_suggested_speed) return max_speed_ms @@ -31,9 +31,9 @@ def is_available(cls, CS): def is_valid(cls, CS, max_speed_ms): if CS.map_suggested_speed <= 0 or CS.map_suggested_speed > max_speed_ms: return False - if CS.speed_limit_ms == 0: # no or unknown speed limit + if CS.baseMapSpeedLimitMPS == 0: # no or unknown speed limit if CS.rampType == 0 and CS.map_suggested_speed >= 17: # more than 61 kph / 38 mph, means we may be on a road without speed limit return False - elif CS.speed_limit_ms < CS.map_suggested_speed: # if map speed exceeds the speed limit, we'll ignore it + elif CS.baseMapSpeedLimitMPS < CS.map_suggested_speed: # if map speed exceeds the speed limit, we'll ignore it return False return True