From 9a663809734a8cafa755ad7e4415673db14d8e8b Mon Sep 17 00:00:00 2001 From: neon-dev <1169307+neon-dev@users.noreply.github.com> Date: Tue, 21 Jan 2020 17:06:14 +0100 Subject: [PATCH] Map based speed limit improvements (#146) * fixed double pull didn't set acc/pcc speed to current speed when you were driving on a road with unknown speed limit (used the last known limit) * fixed fleet speed, pcc, acc and IC did not work with the same speed limit data, causing potential differences between displayed and driven speed * improved speed limit detection for sometimes earlier info, so we can start to accelerate or slow down before the sign * better detection for low speed limit vs unknown speed limit (enables speed limit based adjustments below 10kph) * removed redundant code * fix CC stalk up could cause alternating target speed --- 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, 97 insertions(+), 120 deletions(-) diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc index 84c07d4c6245fd..b48d1319cbffd5 100644 --- a/opendbc/tesla_can.dbc +++ b/opendbc/tesla_can.dbc @@ -284,16 +284,15 @@ 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 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_ 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_ 536 MCU_chassisControl: 8 GTW SG_ MCU_dasDebugEnable : 0|1@1+ (1,0) [0|0] "" NEO @@ -665,8 +664,9 @@ 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_ 760 MCU_speedLimitUnits 1 "KPH" 0 "MPH" ; -VAL_ 760 MCU_userSpeedOffsetUnits 1 "KPH" 0 "MPH" ; +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_ 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 2d3527b685327c..a02da2bb4b4ec8 100644 --- a/opendbc/tesla_can1916.dbc +++ b/opendbc/tesla_can1916.dbc @@ -284,16 +284,15 @@ 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 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_ 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_ 536 MCU_chassisControl: 8 GTW SG_ MCU_dasDebugEnable : 0|1@1+ (1,0) [0|0] "" NEO @@ -665,8 +664,9 @@ 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_ 760 MCU_speedLimitUnits 1 "KPH" 0 "MPH" ; -VAL_ 760 MCU_userSpeedOffsetUnits 1 "KPH" 0 "MPH" ; +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_ 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 e2cb1bb375387c..68ee860196f4c9 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -183,16 +183,17 @@ 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, speed_limit_valid, 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, set_speed_limit_active, speed_limit_offset): # Adaptive cruise control self.prev_speed_limit_kph = 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)): + 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): 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 @@ -231,7 +232,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, speed_limit_valid, set_speed_limit_active, speed_limit_offset, frame) + button_to_press = self._calc_follow_button(CS, lead_1, speed_limit_kph, 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. @@ -247,7 +248,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, speed_limit_valid, set_speed_limit_active, speed_limit_offset, frame): + def _calc_follow_button(self, CS, lead_car, speed_limit_kph, 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 dd86783a3add5b..fb2e7c6c42794e 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -261,18 +261,16 @@ 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. - actual_speed_kph = CS.v_ego * CV.MS_TO_KPH + # 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 if CS.cruise_buttons == CruiseButtons.RES_ACCEL: - self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + speed_uom_kph + self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph_uom_rounded) + speed_uom_kph elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: - self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + 5 * speed_uom_kph + self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph_uom_rounded) + 5 * speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_SET: - #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 + 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) @@ -310,7 +308,7 @@ def update_stat(self, CS, frame): return can_sends - 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): + def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, set_speed_limit_active, speed_limit_offset,alca_enabled): idx = self.pedal_idx self.prev_speed_limit_kph = self.speed_limit_kph @@ -331,12 +329,14 @@ 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 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)): + 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): 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 f5541ccf73fffb..51743a30960ff1 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -105,9 +105,6 @@ 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. @@ -120,7 +117,7 @@ def __init__(self, dbc_name): self.gyroYaw = 0. self.set_speed_limit_active = False self.speed_limit_offset = 0. - self.speed_limit_for_cc = 0. + self.speed_limit_ms = 0. # for warnings self.warningCounter = 0 @@ -325,16 +322,14 @@ 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') @@ -417,21 +412,8 @@ 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 collission warning + # TODO: forward collision 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: @@ -444,13 +426,11 @@ 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.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 + self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0 if icLeadsMsg is not None: self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg) if radarStateMsg is not None: @@ -480,7 +460,6 @@ 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 @@ -608,7 +587,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.speedLimitToIc, #speed_limit_to_car, + CS.DAS_fusedSpeedLimit, apply_angle, 1 if enable_steer_control else 0, park_brake_request)) @@ -632,7 +611,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_for_cc, self.speedlimit_valid, \ + self.speed_limit_ms * CV.MS_TO_KPH, self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( @@ -644,7 +623,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_for_cc * CV.KPH_TO_MS, self.speedlimit_valid, \ + self.speed_limit_ms, 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 @@ -815,10 +794,6 @@ 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 a4f062d104f749..4f5e7c79d3f39e 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 = [ - ("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), + ("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_gpsAccuracy", "MCU_locationStatus", 0), ("MCU_latitude", "MCU_locationStatus", 0), ("MCU_longitude", "MCU_locationStatus", 0), @@ -275,9 +275,6 @@ 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. @@ -299,8 +296,9 @@ def __init__(self, CP): self.gpsHeading = 0. self.gpsVehicleSpeed = 0. - self.userSpeedLimitKph = 0. + self.speed_limit_ms = 0. self.userSpeedLimitOffsetKph = 0. + self.DAS_fusedSpeedLimit = 0 self.brake_only = CP.enableCruise self.last_cruise_stalk_pull_time = 0 @@ -415,7 +413,6 @@ def __init__(self, CP): self.angle_offset = 0. self.init_angle_offset = False - self.speedLimitToIc = 0 #AHB params self.ahbHighBeamStalkPosition = 0 @@ -439,6 +436,16 @@ 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: @@ -450,7 +457,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.baseMapSpeedLimitMPS + self.mapBasedSuggestedSpeed = self.speed_limit_ms if self.rampType > 0: #we are on a ramp, use the spline info if available if self.splineBasedSuggestedSpeed > 0: @@ -517,8 +524,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['MCU_gpsVehicleSpeed']["MCU_gpsVehicleHeading"] - self.gpsVehicleSpeed = cp.vl['MCU_gpsVehicleSpeed']["MCU_gpsVehicleSpeed"] * CV.KPH_TO_MS + self.gpsHeading = cp.vl['UI_gpsVehicleSpeed']["UI_gpsVehicleHeading"] + self.gpsVehicleSpeed = cp.vl['UI_gpsVehicleSpeed']["UI_gpsVehicleSpeed"] * CV.KPH_TO_MS if (self.hasTeslaIcIntegration): self.apEnabled = (cp.vl["MCU_chassisControl"]["MCU_latControlEnable"] == 1) @@ -534,32 +541,18 @@ 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['MCU_gpsVehicleSpeed']["MCU_userSpeedOffsetUnits"] + usu = cp.vl['UI_gpsVehicleSpeed']["UI_userSpeedOffsetUnits"] if usu == 1: - self.userSpeedLimitOffsetKph = cp.vl['MCU_gpsVehicleSpeed']["MCU_userSpeedOffset"] + self.userSpeedLimitOffsetKph = cp.vl['UI_gpsVehicleSpeed']["UI_userSpeedOffset"] else: - 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 + 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"]) rdSignMsg = cp.vl["UI_driverAssistRoadSign"]["UI_roadSign"] - if rdSignMsg == 4: + if rdSignMsg == 4: # ROAD_SIGN_SPEED_SPLINE self.meanFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_meanFleetSplineSpeedMPS"] self.meanFleetSplineAccelMPS2 = cp.vl["UI_driverAssistRoadSign"]["UI_meanFleetSplineAccelMPS2"] self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_medianFleetSpeedMPS"] @@ -567,12 +560,20 @@ 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"] - if rdSignMsg == 3: + elif rdSignMsg == 3: # ROAD_SIGN_SPEED_LIMIT 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 fc1e1f7fd3af69..fe5a6db4a27adf 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.baseMapSpeedLimitMPS > CS.map_suggested_speed: - return self.speed_avg.add(max_speed_ms * CS.map_suggested_speed / CS.baseMapSpeedLimitMPS) + 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) 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.baseMapSpeedLimitMPS == 0: # no or unknown speed limit + if CS.speed_limit_ms == 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.baseMapSpeedLimitMPS < CS.map_suggested_speed: # if map speed exceeds the speed limit, we'll ignore it + elif CS.speed_limit_ms < CS.map_suggested_speed: # if map speed exceeds the speed limit, we'll ignore it return False return True