Compiled on Dec.7,2024

2024-12-07 21:14:32 +08:00
parent 2123a12507
commit 9e99d41df7
17 changed files with 195 additions and 37 deletions
@@ -1,5 +1,6 @@
 Version 0.9.6 (2024-02-27)
 ========================
+* 支持外挂自动跟车 (2024-12-07)
 * 轻柔变道支持红熊 (2024-05-28)
 * 车机控制更多功能 (2024-05-09)
 * 车机控制录屏侧栏 (2024-04-29)
@@ -126,6 +126,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
    holidayActive @124;
    laneChangeBlockedLoud @125;
    leadDeparting @126;
+    autoResumeEvent @138; # auto_resume
    noLaneAvailable @127;
    openpilotCrashed @128;
    openpilotCrashedRandomEvents @129;
@@ -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
@@ -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
@@ -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)
@@ -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)
@@ -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:
@@ -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,
@@ -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)

@@ -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 ""
+  params_memory.put_bool("ESP32AutoResume", True)
+  # 禁用upd消息
+  can_msg.ipaddr = ipaddr if ipaddr else ""
+
@@ -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)
@@ -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')))