diff --git a/RELEASES.md b/RELEASES.md index 368483d5..1006ae21 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,5 +1,6 @@ Version 0.9.6 (2024-02-27) ======================== +* 支持外挂自动跟车 (2024-12-07) * 轻柔变道支持红熊 (2024-05-28) * 车机控制更多功能 (2024-05-09) * 车机控制录屏侧栏 (2024-04-29) diff --git a/cereal/car.capnp b/cereal/car.capnp index 49008a16..a4414c94 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -126,6 +126,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { holidayActive @124; laneChangeBlockedLoud @125; leadDeparting @126; + autoResumeEvent @138; # auto_resume noLaneAvailable @127; openpilotCrashed @128; openpilotCrashedRandomEvents @129; diff --git a/common/params_pyx.so b/common/params_pyx.so index eaf9ed0f..52a50e01 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index 13626d10..e652765d 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -23,16 +23,38 @@ def is_registered_device() -> bool: dongle = Params().get("DongleId", encoding='utf-8') return dongle not in (None, UNREGISTERED_DONGLE_ID) +def local_register(): + serial = HARDWARE.get_serial() + imei1: Optional[str] = None + imei2: Optional[str] = None + while imei1 is None and imei2 is None: + try: + imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) + except Exception: + time.sleep(1) + + params = Params() + params.put("IMEI", imei1) + params.put("HardwareSerial", serial) + + return serial, imei1 def register(show_spinner=False) -> Optional[str]: params = Params() IMEI = params.get("IMEI", encoding='utf8') HardwareSerial = params.get("HardwareSerial", encoding='utf8') + if None in (IMEI, HardwareSerial): + HardwareSerial, IMEI = local_register() + dongle_id: Optional[str] = params.get("DongleId", encoding='utf8') + if dongle_id in (None, ""): + dongle_id = IMEI[0:8] + params.put("DongleId", dongle_id) + needs_registration = None in (IMEI, HardwareSerial, dongle_id) # 不检查dongle_id - return UNREGISTERED_DONGLE_ID + return dongle_id #UNREGISTERED_DONGLE_ID pubkey = Path(Paths.persist_root()+"/comma/id_rsa.pub") if not pubkey.is_file(): dongle_id = UNREGISTERED_DONGLE_ID diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 43162c09..4f2ca6bf 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS from openpilot.common.params import Params #Fixed XT5 OnStar CAN Errors params = Params() +params_memory = Params("/dev/shm/params") # UDP Broadcast Params TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation @@ -72,6 +73,9 @@ class CarState(CarStateBase): ret.currentGearNumber = pt_cp.vl["ECMPRDNL2"]["CurrentGearNumber"] # ONSTAR_GPS_TEST ret.nextGearNumber = pt_cp.vl["ECMPRDNL2"]["NextGearNumber"] # ONSTAR_GPS_TEST + # UDP Broadcast Params + params_memory.put_int("UDP_CurrentGearNumber", ret.currentGearNumber) + self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) # This is to avoid a fault where you engage while still moving backwards after shifting to D. # An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2) @@ -143,6 +147,7 @@ class CarState(CarStateBase): ret.steerFaultTemporary = self.lkas_status == 2 ret.steerFaultPermanent = self.lkas_status == 3 + hazardLights = 0 if self.CP.carFingerprint not in SDGM_CAR: # 1 - open, 0 - closed ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or @@ -167,8 +172,21 @@ class CarState(CarStateBase): ret.seatbeltUnlatched = cam_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0 ret.leftBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 ret.rightBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 + # HazardLights + hazardLights = cam_cp.vl["BCMTurnSignals"]["HazardLights"] ret.parkingBrake = cam_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1 + + # UDP Broadcast Params + if ret.leftBlinker: + params_memory.put_int("UDP_TurnSignals", 1) + elif ret.rightBlinker: + params_memory.put_int("UDP_TurnSignals", 2) + else: + params_memory.put_int("UDP_TurnSignals", 0) + # HazardLights + params_memory.put_int("UDP_HazardLights", hazardLights) + ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or @@ -198,6 +216,9 @@ class CarState(CarStateBase): else: ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 + # Blindspot + params_memory.put_int("UDP_LeftBlindspot", 1 if ret.leftBlindspot else 0) + params_memory.put_int("UDP_RightBlindspot", 1 if ret.rightBlindspot else 0) # FrogPilot functions has_camera = self.CP.networkLocation == NetworkLocation.fwdCamera diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index e491e2c2..e0a79352 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +import time from cereal import car from math import fabs, exp from panda import Panda @@ -484,13 +485,18 @@ class CarInterface(CarInterfaceBase): if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and (self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)): events.add(EventName.belowEngageSpeed) + # only show 5 seconds if ret.cruiseState.standstill and not self.CP.autoResumeSng and not self.disable_resumeRequired: events.add(EventName.resumeRequired) self.resumeRequired_shown = True + if not self.resumeRequired_time: + self.resumeRequired_time = int(time.time()) # Disable the "resumeRequired" event after it's been shown once to not annoy the driver - if self.resumeRequired_shown and not ret.cruiseState.standstill: - self.disable_resumeRequired = True + #if self.resumeRequired_shown and not ret.cruiseState.standstill: + if self.resumeRequired_shown and not self.disable_resumeRequired: + if self.resumeRequired_time and (int(time.time()) - self.resumeRequired_time >= 5): + self.disable_resumeRequired = True if ret.vEgo < self.CP.minSteerSpeed and not self.disable_belowSteerSpeed: events.add(EventName.belowSteerSpeed) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 3ae1de7a..c4bc5d02 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -219,6 +219,7 @@ class CarInterfaceBase(ABC): self.resumeRequired_shown = False self.disable_resumeRequired = False + self.resumeRequired_time = 0 # only show 5 seconds def get_ff_nn(self, x): return self.lat_torque_nn_model.evaluate(x) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5bd2192b..729b7030 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# 12345 import os import math import time @@ -6,6 +7,8 @@ import threading from types import SimpleNamespace from typing import SupportsFloat +import requests #auto_resume + import cereal.messaging as messaging import openpilot.selfdrive.sentry as sentry @@ -177,6 +180,8 @@ class Controls: # FrogPilot variables self.params = Params() self.params_memory = Params("/dev/shm/params") + # auto_resume + self.standstill_time = 0 self.ignore_controls_mismatch = False @@ -470,9 +475,9 @@ class Controls: if self.nudgeless_smooth and self.frogpilot_variables.smoother_lane_change <= 0: long_personality = self.params.get_int("LongitudinalPersonality") if long_personality == 2: #relaxed - self.frogpilot_variables.smoother_lane_change = 5 #95% + self.frogpilot_variables.smoother_lane_change = 5 if CS.vEgo < 70 else 8 #old_value: 5 95% elif long_personality == 1: #standard - self.frogpilot_variables.smoother_lane_change = 2.5 #97.5% + self.frogpilot_variables.smoother_lane_change = 2.5 if CS.vEgo < 70 else 4 #old_value: 2.5 #97.5% elif self.sm['modelV2'].meta.laneChangeState == LaneChangeState.laneChangeFinishing: self.frogpilot_variables.smoother_lane_change = 0 # clear pre-value self.events.add(EventName.laneChange) @@ -649,6 +654,12 @@ class Controls: if green_light: self.events.add(EventName.greenLight) + # auto_resume + if CS.standstill and self.standstill_time == 0: + self.standstill_time = int(time.time()) + elif not CS.standstill and self.standstill_time != 0: + self.standstill_time = 0 + # Lead departing alert if self.lead_departing_alert and self.sm.frame % 50 == 0: lead = self.sm['radarState'].leadOne @@ -659,9 +670,21 @@ class Controls: lead_departing &= not CS.gasPressed lead_departing &= lead.vLead > 1 lead_departing &= self.driving_gear - + + # auto_resume if lead_departing: - self.events.add(EventName.leadDeparting) + # wait time 3 seconds + if (int(time.time()) - self.standstill_time) >= 3: + # read param only when lead_departing = true + cruise_auto_resume = self.params.get_bool("CruiseAutoResume") and self.params_memory.get_bool("ESP32HasIP") #auto_resume + long_personality = self.params.get_int("LongitudinalPersonality") == 0 + if long_personality and cruise_auto_resume and self.state == State.enabled and not CS.brakePressed and self.v_cruise_helper.v_cruise_cluster_kph < 24.0: + self.params_memory.put_bool("ESP32AutoResume", True) + self.events.add(EventName.autoResumeEvent) + else: + self.events.add(EventName.leadDeparting) + else: + self.events.add(EventName.leadDeparting) # Speed limit changed alert if self.speed_limit_alert or self.speed_limit_confirmation: diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 5552f992..c62b38be 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1089,6 +1089,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { AlertStatus.frogpilot, AlertSize.small, Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), }, + # auto_resume + EventName.autoResumeEvent: { + ET.PERMANENT: Alert( + "Pay Attention", + "Vehicle Cruise Auto Resume", + AlertStatus.frogpilot, AlertSize.mid, + Priority.MID, VisualAlert.none, AudibleAlert.engage, 3.), + }, EventName.noLaneAvailable : { ET.PERMANENT: no_lane_available_alert, diff --git a/selfdrive/frogpilot/assets/other_images/aggressive_new2.png b/selfdrive/frogpilot/assets/other_images/aggressive_new2.png new file mode 100644 index 00000000..940f7e87 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/aggressive_new2.png differ diff --git a/selfdrive/frogpilot/assets/other_images/aggressive_new3.png b/selfdrive/frogpilot/assets/other_images/aggressive_new3.png new file mode 100644 index 00000000..1705b55a Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/aggressive_new3.png differ diff --git a/selfdrive/frogpilot/fleetmanager/fleet_manager.py b/selfdrive/frogpilot/fleetmanager/fleet_manager.py index b4668f36..1b106ee5 100644 --- a/selfdrive/frogpilot/fleetmanager/fleet_manager.py +++ b/selfdrive/frogpilot/fleetmanager/fleet_manager.py @@ -456,13 +456,32 @@ def udp_broadcast_ip_route(): except Exception as e: return jsonify({"error": "Failed to update values", "details": str(e)}), 400 +@app.route("/get_broadcast_ip", methods=['GET']) +def get_broadcast_ip_route(): + ipaddr = fleet.get_broadcast_ip() + return ipaddr + +@app.route("/get_esp32_ipaddr", methods=['GET']) +def get_esp32_ipaddr_route(): + ipaddr = fleet.get_esp32_ipaddr() + return ipaddr + +@app.route("/esp32_ipaddr", methods=['POST']) +def esp32_ipaddr_route(): + try: + ipaddr = request.args.get('ipaddr') + fleet.esp32_ipaddr(ipaddr) + return "ESP32 IP Address set to " + ipaddr + " successfully", 200 + except Exception as e: + return jsonify({"error": "Failed to update values", "details": str(e)}), 400 + def main(): try: set_core_affinity([0, 1, 2, 3]) except Exception: cloudlog.exception("fleet_manager: failed to set core affinity") #UDP测试 - #threading.Thread(target=fleet.udp_send_message).start() + threading.Thread(target=fleet.udp_send_message).start() app.secret_key = secrets.token_hex(32) app.run(host="0.0.0.0", port=8082) diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index 02ba24e8..841afbc6 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -45,6 +45,10 @@ from openpilot.system.loggerd.xattr_cache import getxattr # otisserv conversion from urllib.parse import parse_qs, quote +params = Params() +params_memory = Params("/dev/shm/params") +params_storage = Params("/persist/comma/params") + class CanMsg: ipaddr = "" GEARS = [1, 2, 3, 4, 5, 6, 7, 8, 9, 13, 14, 15] @@ -52,8 +56,8 @@ class CanMsg: def __init__(self): self.length = 31 self.version = 1 - self.steeringWheelAngle = random.randint(-540, 540) - self.currentGearNum = random.choice(self.GEARS) + self.steeringWheelAngle = 0 + self.currentGearNum = 0 self.driverMode = 0 self.brakePressure = 0 self.turnSignals = 0 @@ -79,6 +83,15 @@ class CanMsg: self.rrSpeed = 0 self.nextGearNum = 0 + def readparams(self): + self.turnSignals = params_memory.get_int("UDP_TurnSignals") + self.currentGearNum = params_memory.get_int("UDP_CurrentGearNumber") + # Blindspot + self.leftBSM = params_memory.get_int("UDP_LeftBlindspot") + self.rightBSM = params_memory.get_int("UDP_RightBlindspot") + # HazardLights + self.hazardLights = params_memory.get_int("UDP_HazardLights") + def pack(self): data = bytearray(32) data[0] = ((self.length - 1) << 3) | (self.version >> 5) @@ -101,22 +114,15 @@ class CanMsg: data[14] = (tmpDistance >> 8) & 0xFF data[15] = tmpDistance & 0xFF - data[16] = (self.speed >> 4) & 0xFF - data[17] = ((self.speed & 0xF) << 4) & 0xFF - - data[17] += ((self.flSpeed >> 8) & 0xF) - data[18] = self.flSpeed & 0xFF - - data[19] = (self.frSpeed >> 4) & 0xFF - data[20] = ((self.frSpeed & 0xF) << 4) & 0xFF - - data[20] += ((self.rlSpeed >> 8) & 0xF) - data[21] = self.rlSpeed & 0xFF - - data[22] = (self.rrSpeed >> 4) & 0xFF - data[23] = ((self.rrSpeed & 0xF) << 4) & 0xFF - - data[23] += (self.nextGearNum & 0xF) + # IP Address + # data[16]=ipAddress & 0xff; + # data[17]=(ipAddress >> 8) & 0xff; + # data[18]=(ipAddress >> 16) & 0xff; + # data[19]=(ipAddress >> 24) & 0xff; + data[16] = 0 + data[17] = 0 + data[18] = 0 + data[19] = 0 return bytes(data) @@ -148,18 +154,18 @@ class CanMsg: self.rlSpeed = self.speed + random.randint(0, 30) self.nextGearNum = random.choice(self.GEARS) - packed_data = self.pack() + # packed_data = self.pack() # print(list(packed_data)) +#ESP32用类实现 +class ESP32: + ipaddr = "" + pi = 3.1415926535897932384626 x_pi = 3.14159265358979324 * 3000.0 / 180.0 a = 6378245.0 ee = 0.00669342162296594323 -params = Params() -params_memory = Params("/dev/shm/params") -params_storage = Params("/persist/comma/params") - PRESERVE_ATTR_NAME = 'user.preserve' PRESERVE_ATTR_VALUE = b'1' PRESERVE_COUNT = 5 @@ -175,16 +181,57 @@ else: #UDP测试 UDP_PORT = 6499 can_msg = CanMsg() +esp32 = ESP32() #ESP32 ip地址 + +def esp32_ipaddr(ipaddr): + if ipaddr: + esp32.ipaddr = ipaddr + params_memory.put_bool("ESP32HasIP", True) + else: + esp32.ipaddr = "" + params_memory.put_bool("ESP32HasIP", False) + +def get_broadcast_ip(): + url = "UPD Broadcast IP is: " + can_msg.ipaddr + return url + +def get_esp32_ipaddr(): + url = "http://" + esp32.ipaddr + "/admin?CMD=104&Type=2" + return url + +# auto_resume +def on_auto_resume(): + try: + if esp32.ipaddr != "": + #"http://192.168.2.15/admin?CMD=104&Type=2" + url = "http://" + esp32.ipaddr + "/admin?CMD=104&Type=2" + res = requests.get(url) + return True + else: + return False + except Exception as e: + return False def udp_send_message(): UDP_SOCKET = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - + idx = 1 + single = False while True: try: - if can_msg.ipaddr: - can_msg.randomize() + # disable udp + if can_msg.ipaddr and idx==0: + # can_msg.randomize() + can_msg.readparams() UDP_SOCKET.sendto(can_msg.pack(), (can_msg.ipaddr, UDP_PORT)) - time.sleep(1) + #time.sleep(1) + if not single and params_memory.get_bool("ESP32AutoResume"): + single = True + on_auto_resume() + time.sleep(5) # trigger once in 5 seconds + params_memory.put_bool("ESP32AutoResume", False) + single = False + time.sleep(0.25) + idx = (idx+1) % 4 except Exception: pass continue @@ -595,7 +642,8 @@ def get_all_toggle_values(): "MinSteerSpeedStandard", "MinSteerSpeedEngage", "DashSpeedRatio1", "DashSpeedRatio2", "DashSpeedRatio3", "SetSpeedRatio1", "SetSpeedRatio2", "SetSpeedRatio3", "SpeedDecimal", "FrogPilotDrives", "FrogPilotKilometers", "FrogPilotMinutes", "CarMake", "CarModel", - "DriverPrivacyProtectionFake", "CSLCEnabled", "CalibrationCycles", + "DriverPrivacyProtectionFake", "CSLCEnabled", "CalibrationCycles", + "UseRedPanda", "CruiseAutoResume" ] toggle_values = {} @@ -673,4 +721,7 @@ def lateral_control_button(toggle): params_memory.put_bool("FrogPilotTogglesUpdated", False) def udp_broadcast_ip(ipaddr): - can_msg.ipaddr = ipaddr if ipaddr else "" \ No newline at end of file + params_memory.put_bool("ESP32AutoResume", True) + # 禁用upd消息 + can_msg.ipaddr = ipaddr if ipaddr else "" + diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index f2758cf3..4a23959d 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -19,6 +19,8 @@ TOTAL_SCONS_NODES = 2560 MAX_BUILD_PROGRESS = 100 def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: + # test scons_cache + # os.system("sudo rm -rf /data/scons_cache/*") env = os.environ.copy() env['SCONS_PROGRESS'] = "1" nproc = os.cpu_count() @@ -69,6 +71,8 @@ def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: # Show TextWindow spinner.close() if not os.getenv("CI"): + # fix scons_cache error + os.system("sudo rm -rf /data/scons_cache/*") with TextWindow("openpilot failed to build\n \n" + error_s) as t: t.wait_for_exit() exit(1) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 3f59b505..de44b0ca 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -292,6 +292,7 @@ def manager_init() -> None: ("GearNumber", "0"), #GEAR_NUMBER_TEST ("FrogPilotPrebuilt", "0"), #FROGPILOT_PREBUILT_TEST ("UseRedPanda", "0"), #Red Panda Config BUS 0/1/2/3 -> 4/5/6/7 + ("CruiseAutoResume", "0"), #auto_resume ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8'))) diff --git a/selfdrive/ui/translations/main_zh-CHS.qm b/selfdrive/ui/translations/main_zh-CHS.qm index 051836bc..e97ca56b 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 e4e6bd1e..538f9278 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ