diff --git a/RELEASES.md b/RELEASES.md index 39136f94..cc9b7428 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,5 +1,6 @@ Version 0.9.6 (2024-02-27) ======================== +* 发送前车跟车距离 (2025-03-20) * 跟车起步距离控制 (2025-03-05) * 优化自动跟车功能 (2025-02-24) * 外挂自动跟车起步 (2024-12-07) diff --git a/common/params_pyx.so b/common/params_pyx.so index 1925ecc8..d85ad81d 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index c6cf5a75..a6765af4 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -232,7 +232,7 @@ def crash_log(candidate): "AdjacentPath", "AdjacentPathMetrics", "BlindSpotPath", "FPSCounter", "LeadInfo", "UseSI", "PedalsOnUI", "RoadNameUI", "UseVienna", "DriverCamera", "ModelUI", "DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength", "QOLVisuals", "DriveStats", "FullMap", "HideSpeed", "HideSpeedUI", "ShowSLCOffset", "SpeedLimitChangedAlert", "WheelSpeed", "RandomEvents", "ScreenBrightness", "WheelIcon", - "DashSpeedRatio1", "DashSpeedRatio2", "DashSpeedRatio3", "SetSpeedRatio1", "SetSpeedRatio2", "SetSpeedRatio3", + "DashSpeedRatio1", "DashSpeedRatio2", "DashSpeedRatio3", "RotatingWheel", "NumericalTemp", "Fahrenheit", "ShowCPU", "ShowGPU", "ShowIP", "ShowMemoryUsage", "ShowStorageLeft", "ShowStorageUsed", "Sidebar" ], [ "FrogPilotDrives", "FrogPilotKilometers", "FrogPilotMinutes" diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 4f2ca6bf..910ea247 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -283,6 +283,13 @@ class CarState(CarStateBase): self.distance_previously_pressed = distance_pressed + # Override FrogPilot Personality Profile Bug for Enclave + if has_camera: + pp = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCGapLevel"] - 1 + if self.personality_profile != pp: + self.personality_profile = pp + self.fpf.distance_button_function(self.personality_profile) + # Toggle Experimental Mode from steering wheel function if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available: if self.CP.carFingerprint in SDGM_CAR: diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index d06deaa3..a999ba5c 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -4,7 +4,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import make_can_msg from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CanBus - +from openpilot.selfdrive.lqrtx.speed import SpeedMap def create_buttons(packer, bus, idx, button): values = { @@ -289,30 +289,49 @@ def create_gm_acc_spam_command(packer, controller, CS, slcSet, bus, Vego, frogpi # using ms def create_gm_acc_spam_command_ms(packer, controller, CS, slcSet_ms, bus, Vego, frogpilot_variables, accel): + #SpeedMap + speed_map = SpeedMap() + speed_map.enable_acc_speed_maps(frogpilot_variables.use_acc_speed_maps) + is_metric = controller.is_metric MS_CONVERT = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH - slcSet = slcSet_ms * MS_CONVERT + + #slcSet = slcSet_ms * MS_CONVERT + slcSet = speed_map.get_acc_speed_display(slcSet_ms * MS_CONVERT) if frogpilot_variables.use_acc_speed_maps else slcSet_ms * MS_CONVERT cruiseBtn = CruiseButtons.INIT byfive = 0 - speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT)) + #speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT)) + speedSetPoint = speed_map.get_acc_speed_display(CS.out.cruiseState.speed * MS_CONVERT) if frogpilot_variables.use_acc_speed_maps else int(round(CS.out.cruiseState.speed * MS_CONVERT)) FRAMES_ON = 6 FRAMES_OFF = 30 - FRAMES_ON - if not frogpilot_variables.experimentalMode: - if slcSet + 5 < Vego * MS_CONVERT: - slcSet = slcSet - 10 + #SpeedMap + if frogpilot_variables.use_acc_speed_maps: + if not frogpilot_variables.experimentalMode: + if slcSet + 5 < speed_map.get_acc_speed_display(Vego * MS_CONVERT): + slcSet = slcSet - 10 + else: + slcSet = speed_map.get_acc_speed_display((Vego * 1.01 + 4.6 * accel + 0.7 * accel ** 3 - 1 / 35 * accel ** 5) * MS_CONVERT) + v_max = 30 + v_min = 25 else: - slcSet = int(round((Vego * 1.01 + 4.6 * accel + 0.7 * accel ** 3 - 1 / 35 * accel ** 5) * MS_CONVERT)) # 1.01 factor to match cluster speed better + if not frogpilot_variables.experimentalMode: + if slcSet + 5 < Vego * MS_CONVERT: + slcSet = slcSet - 10 + else: + slcSet = int(round((Vego * 1.01 + 4.6 * accel + 0.7 * accel ** 3 - 1 / 35 * accel ** 5) * MS_CONVERT)) # 1.01 factor to match cluster speed better + v_max = 28 + v_min = 24 - if slcSet <= int(math.floor((speedSetPoint - 1)/5.0)*5.0) and speedSetPoint > (28 if is_metric else 20): + if slcSet <= int(math.floor((speedSetPoint - 1)/5.0)*5.0) and speedSetPoint > (v_max if is_metric else 20): cruiseBtn = CruiseButtons.DECEL_SET byfive = 1 elif slcSet >= int(math.ceil((speedSetPoint + 1)/5.0)*5.0): cruiseBtn = CruiseButtons.RES_ACCEL byfive = 1 - elif slcSet <= (speedSetPoint - 1) and speedSetPoint > (24 if is_metric else 16): + elif slcSet <= (speedSetPoint - 1) and speedSetPoint > (v_min if is_metric else 16): cruiseBtn = CruiseButtons.DECEL_SET byfive = 0 elif slcSet >= (speedSetPoint + 1): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 93197d4e..489d37e3 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -204,6 +204,7 @@ class Controls: self.max_acceleration = 0 self.previous_drive_distance = 0 self.previous_lead_distance = 0 + self.standstill_lead_distance = 0 self.previous_speed_limit = SpeedLimitController.desired_speed_limit self.random_event_timer = 0 @@ -666,7 +667,16 @@ class Controls: if self.lead_departing_alert and self.sm.frame % 50 == 0: lead = self.sm['radarState'].leadOne lead_distance = lead.dRel - lead_departing = lead_distance - self.previous_lead_distance > 0.5 and CS.standstill #and self.previous_lead_distance != 0 + + #记录停止时的第一个跟车距离，防止红绿灯头车错误提示 + if CS.standstill: + if self.standstill_lead_distance == 0: + self.standstill_lead_distance = lead_distance + else: + self.standstill_lead_distance = 0 + lead_departing = self.standstill_lead_distance > 0 and lead_distance > self.standstill_lead_distance + + lead_departing &= lead_distance - self.previous_lead_distance > 0.5 and CS.standstill #and self.previous_lead_distance != 0 # below 15 meters lead_departing &= self.previous_lead_distance > 0 and self.previous_lead_distance <= 15 previous_lead = self.previous_lead_distance @@ -678,7 +688,7 @@ class Controls: # auto_resume if lead_departing: - self.params_memory.put_int("LeadDepartDistance", previous_lead * 10) + self.params_memory.put_int("LeadDepartDistance", previous_lead * 10) #dm分米 # wait time 3 seconds if (int(time.time()) - self.standstill_time) >= 3: # read param only when lead_departing = true @@ -1274,6 +1284,7 @@ class Controls: self.frogpilot_variables.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental") self.frogpilot_variables.CSLC = self.params.get_bool("CSLCEnabled") + self.frogpilot_variables.use_acc_speed_maps = self.params.get_bool("QOLVisuals") and self.params.get_bool("UseAccSpeedMaps") custom_alerts = self.params.get_bool("CustomAlerts") self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert") diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index e4c9b47b..cb5a094d 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -62,7 +62,7 @@ class DesireHelper: self.update_frogpilot_params() def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): - v_ego = carstate.vEgo + v_ego = carstate.vEgo #ms one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -106,7 +106,7 @@ class DesireHelper: # Conduct a nudgeless lane change if all the conditions are met self.lane_change_wait_timer += DT_MDL - if self.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay: + if self.nudgeless and v_ego >= self.nudgeless_speed * CV.KPH_TO_MS and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay: self.lane_change_wait_timer = 0 torque_applied = True @@ -174,6 +174,8 @@ class DesireHelper: is_metric = self.params.get_bool("IsMetric") self.nudgeless = self.params.get_bool("NudgelessLaneChange") + self.nudgeless_speed = self.params.get_int("NudgelessSpeed") + self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0 self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection") self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 158fe7f8..aab34b2c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -5,14 +5,15 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.lqrtx.speed import SpeedMap # WARNING: this value was determined based on the model's training distribution, # model predictions above this speed can be unpredictable # V_CRUISE's are in kph -V_CRUISE_MIN = 23 #For Baby Enclave 8 -V_CRUISE_MAX = 146 #For Baby Enclave 145 +V_CRUISE_MIN = 23.87 #For Baby Enclave 25-23.87 +V_CRUISE_MAX = 153.56 #For Baby Enclave 160-153.56 V_CRUISE_UNSET = 255 -V_CRUISE_INITIAL = 23 #For Baby Enclave 40 +V_CRUISE_INITIAL = 23.87 #For Baby Enclave 25-23.87 V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors @@ -48,13 +49,17 @@ class VCruiseHelper: # FrogPilot variables self.params_memory = Params("/dev/shm/params") + self.speed_map = SpeedMap() @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables): - self.v_cruise_kph_last = self.v_cruise_kph + #SpeedMap + self.speed_map.enable_acc_speed_maps(frogpilot_variables.use_acc_speed_maps) + + self.v_cruise_kph_last = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(self.v_cruise_kph)) if CS.cruiseState.available: if not self.CP.pcmCruise or frogpilot_variables.CSLC: @@ -69,6 +74,11 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET + if self.v_cruise_kph > 0 and self.v_cruise_kph != V_CRUISE_UNSET: + self.params_memory.put_float("LqrtxVCruiseKPH", self.v_cruise_kph) + else: + self.params_memory.put_float("LqrtxVCruiseKPH", 0) + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition @@ -122,6 +132,8 @@ class VCruiseHelper: if not self.button_change_states[button_type]["enabled"]: return + last_standstill_speed = self.v_cruise_kph + v_cruise_delta_interval = frogpilot_variables.custom_cruise_increase_long if long_press else frogpilot_variables.custom_cruise_increase # v_cruise_delta = v_cruise_delta * (5 if long_press else 1) v_cruise_delta = v_cruise_delta * v_cruise_delta_interval @@ -131,9 +143,12 @@ class VCruiseHelper: # self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta # else: # self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] - self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] - # Apply offset + #self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + #先获取整数倍的kph，再转换成实际kph + self.v_cruise_kph = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(self.v_cruise_kph) + v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]) + + # Apply offset 不要用set_speed_offset v_cruise_offset = (frogpilot_variables.set_speed_offset * CRUISE_INTERVAL_SIGN[button_type]) if long_press else 0 if v_cruise_offset < 0: v_cruise_offset = frogpilot_variables.set_speed_offset - v_cruise_delta @@ -141,9 +156,14 @@ class VCruiseHelper: # If set is pressed while overriding, clip cruise speed to minimum of vEgo if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): - self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) + #self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) + self.v_cruise_kph = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH))) - self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + #保留2位小数 + self.v_cruise_kph = clip(round(self.v_cruise_kph, 2), V_CRUISE_MIN, V_CRUISE_MAX) + #防止ACC静止时，速度被修改 + if self.speed_map.get_acc_speed_display(last_standstill_speed) <= 25 and self.speed_map.get_acc_speed_display(self.v_cruise_kph) > last_standstill_speed and CS.vEgo < 0.1: + self.v_cruise_kph = last_standstill_speed def update_button_timers(self, CS, enabled): # increment timer for buttons still pressed @@ -167,6 +187,8 @@ class VCruiseHelper: else: initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + self.speed_map.enable_acc_speed_maps(frogpilot_variables.use_acc_speed_maps) + # CSLC resume/set logic if frogpilot_variables.CSLC: if frogpilot_variables.prev_button == ButtonType.resumeCruise and self.v_cruise_kph_last < 250: @@ -175,10 +197,13 @@ class VCruiseHelper: # Initial set speed if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit: # If there's a known speed limit and the corresponding FP toggle is set, push it to the car - self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH)) + #self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH)) + self.v_cruise_kph = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(desired_speed_limit * CV.MS_TO_KPH)) else: # Use fixed initial set speed from mode etc. - self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + #self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + self.v_cruise_kph = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + self.v_cruise_cluster_kph = self.v_cruise_kph return @@ -189,11 +214,13 @@ class VCruiseHelper: # Initial set speed if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit: # If there's a known speed limit and the corresponding FP toggle is set, push it to the car - self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH)) + #self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH)) + self.v_cruise_kph = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(desired_speed_limit * CV.MS_TO_KPH)) else: # Use fixed initial set speed from mode etc. - self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) - + #self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + self.v_cruise_kph = self.speed_map.get_acc_speed_actual(self.speed_map.get_acc_speed_display(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + self.v_cruise_cluster_kph = self.v_cruise_kph def apply_deadzone(error, deadzone): diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d797b14e..80fde33d 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -249,7 +249,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S def lead_departing_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: params_memory = Params("/dev/shm/params") - dis = params_memory.get_int("LeadDepartDistance") / 10 + dis = params_memory.get_int("LeadDepartDistance") / 10 #dm分米 return Alert( f"Lead departed {dis} meters", "", diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index 26840a25..0c020711 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -623,7 +623,7 @@ def get_all_toggle_values(): "ExperimentalModeActivation", "ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaScreen", "FireTheBabysitter", "NoLogging", "NoDashCam", "NoUploads", "MuteOverheated", "OfflineMode", "LateralTune", "ForceAutoTune", "NNFF", "SteerRatio", "UseLateralJerk", "LongitudinalTune", "AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance", "SmoothBraking", "Model", "MTSCEnabled", - "DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit", "NudgelessLaneChange", "LaneChangeTime", "LaneDetection", + "DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit", "NudgelessLaneChange", "LaneChangeTime", "LaneDetection", "NudgelessSpeed", "NudgelessSmooth", "LaneDetectionWidth", "OneLaneChange", "QOLControls", "DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "ReverseCruiseUI", "SetSpeedLimit", "SetSpeedOffset", "SpeedLimitController", "Offset1", "Offset2", "Offset3", "Offset4", "SLCConfirmation", "SLCConfirmationLower", "SLCConfirmationHigher", "SLCFallback", "SLCPriority1", "SLCPriority2", "SLCPriority3", "SLCOverride", @@ -642,10 +642,10 @@ def get_all_toggle_values(): "ShowStorageLeft", "ShowStorageUsed", "Sidebar", "MinSteerSpeedStandard", "MinSteerSpeedEngage", - "DashSpeedRatio1", "DashSpeedRatio2", "DashSpeedRatio3", "SetSpeedRatio1", "SetSpeedRatio2", "SetSpeedRatio3", "SpeedDecimal", + "DashSpeedRatio1", "DashSpeedRatio2", "DashSpeedRatio3", "SpeedDecimal", "UseAccSpeedMaps", "FrogPilotDrives", "FrogPilotKilometers", "FrogPilotMinutes", "CarMake", "CarModel", "DriverPrivacyProtectionFake", "CSLCEnabled", "CalibrationCycles", - "UseRedPanda", "CruiseAutoResume" + "UseRedPanda", "CruiseAutoResume", "AutoResumeDistance" ] toggle_values = {} diff --git a/selfdrive/lqrtx/lqrtxcore.py b/selfdrive/lqrtx/lqrtxcore.py index 1e7bfcd1..b2bcf53d 100644 --- a/selfdrive/lqrtx/lqrtxcore.py +++ b/selfdrive/lqrtx/lqrtxcore.py @@ -83,6 +83,10 @@ class C3UDPSendHelper(threading.Thread): "a":params_memory.get_bool("CruiseAutoResumeActivated"), #CruiseAutoResumeActivated "b":params_memory.get_bool("ESP32HasIP"), # has ESP32 or not. "c":params_memory.get_bool("LqrtxOnRoad"), # OnRoad status + "d":params_memory.get_float("LqrtxVCruiseKPH"), #ACC Set Speed send to ESP32 + "l1":params_memory.get_float("LqrtxDistanceRelated"), #meter, Lead Car Distince Related send out + "l2":params_memory.get_float("LqrtxVelocityRelated"), #kph, Lead Car Velocity Related send out + #"k":params_memory.get("LqrtxDebugText", encoding='utf-8'), #测试用途 "a1":"", #C3 IP Address "a2":"" #ESP32 IP Address } diff --git a/selfdrive/lqrtx/speed.py b/selfdrive/lqrtx/speed.py new file mode 100644 index 00000000..917a8d0a --- /dev/null +++ b/selfdrive/lqrtx/speed.py @@ -0,0 +1,54 @@ +class SpeedMap: + def __init__(self): + self.use_acc_speed_maps = False + #25 - 160 KPH + #self.acc_speed_maps = {"25":23.87,"26":24.81,"27":25.68,"28":26.62,"29":27.5,"30":28.37,"31":29.31,"32":30.18,"33":31.06,"34":31.93,"35":32.87,"36":33.75,"37":34.62,"38":35.5,"39":36.31,"40":37.18,"41":38.18,"42":39.12,"43":40.06,"44":41,"45":42,"46":42.93,"47":43.87,"48":44.81,"49":45.81,"50":46.75,"51":47.75,"52":48.68,"53":49.62,"54":50.62,"55":51.56,"56":52.56,"57":53.5,"58":54.5,"59":55.43,"60":56.43,"61":57.37,"62":58.37,"63":59.31,"64":60.31,"65":61.25,"66":62.25,"67":63.25,"68":64.18,"69":65.18,"70":66.18,"71":67.18,"72":68.12,"73":69.12,"74":70.12,"75":71.12,"76":72.06,"77":73.06,"78":74.06,"79":75.06,"80":76.06,"81":77,"82":78,"83":78.93,"84":79.87,"85":80.87,"86":81.81,"87":82.81,"88":83.75,"89":84.75,"90":85.68,"91":86.68,"92":87.62,"93":88.56,"94":89.56,"95":90.5,"96":91.5,"97":92.43,"98":93.43,"99":94.37,"100":95.37,"101":96.31,"102":97.31,"103":98.31,"104":99.25,"105":100.25,"106":101.18,"107":102.18,"108":103.12,"109":104.12,"110":105.06,"111":106.06,"112":107.06,"113":108,"114":109,"115":109.93,"116":110.93,"117":111.87,"118":112.87,"119":113.87,"120":114.81,"121":115.81,"122":116.75,"123":117.75,"124":118.68,"125":119.62,"126":120.62,"127":121.56,"128":122.56,"129":123.5,"130":124.5,"131":125.43,"132":126.43,"133":127.37,"134":128.37,"135":129.31,"136":130.31,"137":131.25,"138":132.25,"139":133.18,"140":134.18,"141":135.12,"142":136.06,"143":137.06,"144":138.06,"145":139,"146":139.93,"147":140.93,"148":141.93,"149":142.87,"150":143.81,"151":144.81,"152":145.81,"153":146.75,"154":147.75,"155":148.68,"156":149.68,"157":150.62,"158":151.62,"159":152.56,"160":153.56} + self.acc_speed_maps = {"1":0.95,"2":1.91,"3":2.86,"4":3.82,"5":4.77,"6":5.73,"7":6.68,"8":7.64,"9":8.59,"10":9.55,"11":10.5,"12":11.46,"13":12.41,"14":13.37,"15":14.32,"16":15.28,"17":16.23,"18":17.19,"19":18.14,"20":19.1,"21":20.05,"22":21.01,"23":21.96,"24":22.92,"25":23.87,"26":24.81,"27":25.68,"28":26.62,"29":27.5,"30":28.37,"31":29.31,"32":30.18,"33":31.06,"34":31.93,"35":32.87,"36":33.75,"37":34.62,"38":35.5,"39":36.31,"40":37.18,"41":38.18,"42":39.12,"43":40.06,"44":41,"45":42,"46":42.93,"47":43.87,"48":44.81,"49":45.81,"50":46.75,"51":47.75,"52":48.68,"53":49.62,"54":50.62,"55":51.56,"56":52.56,"57":53.5,"58":54.5,"59":55.43,"60":56.43,"61":57.37,"62":58.37,"63":59.31,"64":60.31,"65":61.25,"66":62.25,"67":63.25,"68":64.18,"69":65.18,"70":66.18,"71":67.18,"72":68.12,"73":69.12,"74":70.12,"75":71.12,"76":72.06,"77":73.06,"78":74.06,"79":75.06,"80":76.06,"81":77,"82":78,"83":78.93,"84":79.87,"85":80.87,"86":81.81,"87":82.81,"88":83.75,"89":84.75,"90":85.68,"91":86.68,"92":87.62,"93":88.56,"94":89.56,"95":90.5,"96":91.5,"97":92.43,"98":93.43,"99":94.37,"100":95.37,"101":96.31,"102":97.31,"103":98.31,"104":99.25,"105":100.25,"106":101.18,"107":102.18,"108":103.12,"109":104.12,"110":105.06,"111":106.06,"112":107.06,"113":108,"114":109,"115":109.93,"116":110.93,"117":111.87,"118":112.87,"119":113.87,"120":114.81,"121":115.81,"122":116.75,"123":117.75,"124":118.68,"125":119.62,"126":120.62,"127":121.56,"128":122.56,"129":123.5,"130":124.5,"131":125.43,"132":126.43,"133":127.37,"134":128.37,"135":129.31,"136":130.31,"137":131.25,"138":132.25,"139":133.18,"140":134.18,"141":135.12,"142":136.06,"143":137.06,"144":138.06,"145":139,"146":139.93,"147":140.93,"148":141.93,"149":142.87,"150":143.81,"151":144.81,"152":145.81,"153":146.75,"154":147.75,"155":148.68,"156":149.68,"157":150.62,"158":151.62,"159":152.56,"160":153.56,"161":154.53,"162":155.49,"163":156.45,"164":157.41,"165":158.37,"166":159.33,"167":160.29,"168":161.25,"169":162.21,"170":163.17,"171":164.13,"172":165.09,"173":166.05,"174":167.01,"175":167.97,"176":168.92,"177":169.88,"178":170.84,"179":171.8,"180":172.76,"181":173.72,"182":174.68,"183":175.64,"184":176.6,"185":177.56,"186":178.52,"187":179.48,"188":180.44,"189":181.4,"190":182.36,"191":183.32,"192":184.28,"193":185.24,"194":186.2,"195":187.16,"196":188.12,"197":189.08,"198":190.04,"199":191,"200":191.96,"201":192.92,"202":193.88,"203":194.84,"204":195.8,"205":196.76,"206":197.72,"207":198.68,"208":199.64,"209":200.6,"210":201.56,"211":202.52,"212":203.48,"213":204.44,"214":205.4,"215":206.36,"216":207.32,"217":208.28,"218":209.24,"219":210.2,"220":211.16,"221":212.12,"222":213.08,"223":214.04,"224":215,"225":215.96,"226":216.91,"227":217.87,"228":218.83,"229":219.79,"230":220.75,"231":221.71,"232":222.67,"233":223.63,"234":224.59,"235":225.55,"236":226.51,"237":227.47,"238":228.43,"239":229.39,"240":230.35,"241":231.31,"242":232.27,"243":233.23,"244":234.19,"245":235.15,"246":236.11,"247":237.07,"248":238.03,"249":238.99,"250":239.95} + + def enable_acc_speed_maps(self, enable_map: bool) -> None: + self.use_acc_speed_maps = enable_map + return + + #acc实际速度 -> 显示速度 + def get_acc_speed_display(self, acc_speed: float) -> int: + r = int(acc_speed) + if not self.use_acc_speed_maps: + return r + + min_key = 0 + max_key = 0 + min_value = 0 + max_value = 0 + for key, value in self.acc_speed_maps.items(): + if value == acc_speed: + return int(key) + elif value < acc_speed and value > min_value: + min_key = int(key) + min_value = value + elif value > acc_speed and (value < max_value or max_value==0): + max_key = int(key) + max_value = value + + mid = 0.5 + if min_value > 0 and max_value > 0: + mid = (max_value - min_value) / 2 + mid = 0.5 if mid > 0.5 else mid + + if min_value > 0 and (acc_speed - min_value) < mid: + return int(min_key) + if max_value > 0 and (max_value - acc_speed) < mid: + return int(max_key) + + return r + + #acc显示速度 -> 实际速度 + def get_acc_speed_actual(self, dis_speed: int) -> float: + r = dis_speed + if not self.use_acc_speed_maps: + return r + + for key, value in self.acc_speed_maps.items(): + if int(key) == dis_speed: + return value + + return r \ No newline at end of file diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index d1f3e77c..8df5c069 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -200,6 +200,7 @@ def manager_init() -> None: ("NoDashCam", "0"), ("NoUploads", "0"), ("NudgelessLaneChange", "1"), + ("NudgelessSpeed", "0"), # Nudgeless Lane Change Enable Speed ("NudgelessSmooth", "0"), # Smoother Nudgeless Lane Change ("NumericalTemp", "0"), ("OfflineMode", "0"), @@ -284,9 +285,7 @@ def manager_init() -> None: ("DashSpeedRatio1", "1.065"), ("DashSpeedRatio2", "1.055"), ("DashSpeedRatio3", "1.045"), - ("SetSpeedRatio1", "1.065"), - ("SetSpeedRatio2", "1.055"), - ("SetSpeedRatio3", "1.045"), + ("UseAccSpeedMaps", "0"), #SpeedMap ("SpeedDecimal", "0"), ("CalibrationCycles", "1"), ("OnStarGPS", "0"), diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 796c9cb3..80b19c02 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -164,28 +164,33 @@ class Soundd: cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}") while True: - sm.update(0) + try: + sm.update(0) - if sm.updated['microphone'] and self.current_alert == AudibleAlert.none and not self.alert_volume_control: # only update volume filter when not playing alert - self.spl_filter_weighted.update(sm["microphone"].soundPressureWeightedDb) - self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) + if sm.updated['microphone'] and self.current_alert == AudibleAlert.none and not self.alert_volume_control: # only update volume filter when not playing alert + self.spl_filter_weighted.update(sm["microphone"].soundPressureWeightedDb) + self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) - elif self.alert_volume_control and self.current_alert in self.volume_map: - self.current_volume = self.volume_map[self.current_alert] / 100.0 + elif self.alert_volume_control and self.current_alert in self.volume_map: + self.current_volume = self.volume_map[self.current_alert] / 100.0 - # Increase the volume for Random Events - elif self.current_alert in self.random_events_map: - self.current_volume = self.random_events_map[self.current_alert] + # Increase the volume for Random Events + elif self.current_alert in self.random_events_map: + self.current_volume = self.random_events_map[self.current_alert] - self.get_audible_alert(sm) + self.get_audible_alert(sm) - rk.keep_time() + rk.keep_time() - assert stream.active + assert stream.active - # Update FrogPilot parameters - if self.params_memory.get_bool("FrogPilotTogglesUpdated"): - self.update_frogpilot_params() + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + except Exception as e: + time.sleep(5) + continue def update_frogpilot_params(self): self.random_events_map = { diff --git a/selfdrive/ui/translations/main_zh-CHS.qm b/selfdrive/ui/translations/main_zh-CHS.qm index 056fcce4..e49c3453 100644 Binary files a/selfdrive/ui/translations/main_zh-CHS.qm and b/selfdrive/ui/translations/main_zh-CHS.qm differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index f948196e..57b3fc65 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ