Compiled on Feb.24,2025

RELEASES.md

@@ -1,5 +1,6 @@
 Version 0.9.6 (2024-02-27)
 ========================
+* 优化自动跟车功能 (2025-02-24)
 * 外挂自动跟车起步 (2024-12-07)
 * 轻柔变道支持红熊 (2024-05-28)
 * 车机控制更多功能 (2024-05-09)

cereal/log.capnp

@@ -2330,6 +2330,7 @@ struct Event {
     customReservedRawData1 @125 :Data;
     customReservedRawData2 @126 :Data;
 
+
     # *********** Custom: reserved for forks ***********
     frogpilotCarControl @107 :Custom.FrogPilotCarControl;
     frogpilotDeviceState @108 :Custom.FrogPilotDeviceState;

selfdrive/controls/controlsd.py

@@ -2,6 +2,7 @@
 # 12345
 import os
 import math
+import json
 import time
 import threading
 from types import SimpleNamespace

selfdrive/frogpilot/fleetmanager/fleet_manager.py

@@ -447,14 +447,14 @@ def lateral_control_button_route():
   except Exception as e:
     return jsonify({"error": "Failed to update values", "details": str(e)}), 400
 
-@app.route("/udp_broadcast_ip", methods=['POST'])
-def udp_broadcast_ip_route():
-  try:
-    ipaddr = request.args.get('ipaddr')
-    fleet.udp_broadcast_ip(ipaddr)
-    return "UPD Broadcast IP set to " + ipaddr + " successfully", 200
-  except Exception as e:
-    return jsonify({"error": "Failed to update values", "details": str(e)}), 400
+# @app.route("/udp_broadcast_ip", methods=['POST'])
+# def udp_broadcast_ip_route():
+#   try:
+#     ipaddr = request.args.get('ipaddr')
+#     fleet.udp_broadcast_ip(ipaddr)
+#     return "UPD Broadcast IP set to " + ipaddr + " successfully", 200
+#   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():
@@ -466,14 +466,14 @@ 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
+# @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:
@@ -481,7 +481,8 @@ def main():
   except Exception:
     cloudlog.exception("fleet_manager: failed to set core affinity")
   #UDP测试
-  threading.Thread(target=fleet.udp_send_message).start()
+  # stop old auto resume thread
+  #threading.Thread(target=fleet.udp_send_message).start()
   app.secret_key = secrets.token_hex(32)
   app.run(host="0.0.0.0", port=8082)
 

selfdrive/frogpilot/fleetmanager/helpers.py

@@ -196,7 +196,9 @@ def get_broadcast_ip():
   return url
 
 def get_esp32_ipaddr():
-  url = "http://" + esp32.ipaddr + "/admin?CMD=104&Type=2"
+  #url = "http://" + esp32.ipaddr + "/admin?CMD=104&Type=2"
+  ipaddr = params_memory.get("ESP32IPAddress")
+  url = "http://" + ipaddr + "/admin?CMD=104&Type=2"
   return url
 
 # auto_resume
@@ -721,7 +723,8 @@ def lateral_control_button(toggle):
   params_memory.put_bool("FrogPilotTogglesUpdated", False)
 
 def udp_broadcast_ip(ipaddr):
-  params_memory.put_bool("ESP32AutoResume", True)
+  # 测试用途
+  # params_memory.put_bool("ESP32AutoResume", True)
   # 禁用upd消息
   can_msg.ipaddr = ipaddr if ipaddr else ""
 

selfdrive/lqrtx/lqrtxcore.py

@@ -0,0 +1,286 @@
+import socket
+import sys
+import requests
+import subprocess
+import time
+import threading
+import json
+from common.params import Params
+import cereal.messaging as messaging
+from enum import Enum
+
+params = Params()
+params_memory = Params("/dev/shm/params")
+params_storage = Params("/persist/comma/params")
+
+
+# 定义一个枚举类型
+class OPMessageActionType(Enum):
+    """ All the action type defined here for reference """
+    DEVICE_STATUS = 0 #OpenPilot Device status message
+    AUTO_RESUME_CRUISE = 1 #OpenPilot using ESP auto resume cruise
+    C3_AUTO_RESTART = 2 # OpenPilot auto restart event, used by informe user why it is restart, for now just used for tracing restart event when OP is not in ONROAD status
+
+
+class C3UDPSendHelper(threading.Thread):
+    ''' 负责发送C3的消息类 '''
+    __opLocalIP = None # C3 自己的IP Address
+    __c3UDPPort = 6599 # UDP port used by C3
+    __udpSocket = None
+    def __init__(self, port=6599, buffer_size=1024):
+        '''
+        初始化C3UDPSendHelper
+        '''
+        super().__init__(daemon=True)  # 设置为守护线程
+        if port is not None:
+            self.__c3UDPPort=port
+
+    def getOPLocalIP(self):
+        ''' get OpenPolit C3 local IPv4 IP Address '''
+        try:
+            s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            s.connect(("8.8.8.8", 80))
+            ip = s.getsockname()[0]
+            s.close()
+            return ip
+        except Exception :
+            return None
+
+    def sendMsg(self,msg):
+        try:
+            if self.__udpSocket is not None:
+                self.__udpSocket.sendto(json.dumps(msg).encode(), ("255.255.255.255", self.__c3UDPPort))
+        except:
+            pass
+
+    def run(self):
+        while True:
+            try:
+                self.__opLocalIP=self.getOPLocalIP()
+                if self.__opLocalIP is None:
+                    time.sleep(5)
+                    continue
+                if self.__udpSocket is not None:
+                    try:
+                        self.__udpSocket.close()
+                    except:
+                        None
+
+                try:
+                    self.__udpSocket=socket.socket(socket.AF_INET, socket.SOCK_DGRAM,socket.IPPROTO_UDP)
+                    self.__udpSocket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+                except Exception as e:
+                    #49 Can't assign requested address
+                    #print(e.errno,e.strerror)
+                    time.sleep(5)
+                    continue
+             
+                while True:
+                    try:
+                        # 发送广播数据包到 255.255.255.255（全网广播）或 192.168.X.255（子网广播）
+                        msg = {
+                            "ACT":OPMessageActionType.DEVICE_STATUS.value, # ACT is integer type, need to add .value get the Enum is value. 
+                            "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
+                            "a1":"", #C3 IP Address
+                            "a2":""  #ESP32 IP Address
+                        }
+                        if params_memory.get_bool("ESP32HasIP"):
+                            msg["a2"] = params_memory.get("ESP32IPAddress").decode()
+                        if self.__opLocalIP is not None:
+                            msg["a1"] = self.__opLocalIP
+                        self.__udpSocket.sendto(json.dumps(msg).encode(), ("255.255.255.255", self.__c3UDPPort))
+                        time.sleep(1)
+                    except Exception as e:
+                        #print("error",e)
+                        break
+            except Exception as e:
+                None
+                #print(f" catched excepiton: {e}")
+            time.sleep(5)
+  
+
+class ESP32Helper(threading.Thread):
+    ''' 负责接收ESP32的消息类 '''
+    __opLocalIP = None # C3 自己的IP Address
+    __esp32UDPPort = 6499 # UDP port used by ESP32
+    __udpSocket = None
+    __esp32IPAddress=None
+    __lastResPlusClickTime=None
+    __isEnginePowerOn=False
+    __enginePowerOnTime=None
+    __isOnRoadEver=False
+    __udpSender=None
+    def __init__(self, udpSender=None, port=6499, buffer_size=1024):
+        '''
+        初始化ESP32Helper
+        '''
+        super().__init__(daemon=True)  # 设置为守护线程
+        if port is not None:
+            self.__esp32UDPPort=port
+        self.__udpSender = udpSender
+
+    def getOPLocalIP(self):
+        ''' get OpenPolit C3 local IPv4 IP Address '''
+        try:
+            s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            s.connect(("8.8.8.8", 80))
+            ip = s.getsockname()[0]
+            s.close()
+            return ip
+        except Exception :
+            return None
+
+    def unpack(self,data_bytes):
+        data = [byte for byte in data_bytes]  # Convert byte array to list of integers
+        current_gear_num = data[3] >> 4
+        rpm = (data[5] & 0xF) << 8 | data[6]
+        ip_address = f"{data[16]}.{data[17]}.{data[18]}.{data[19]}"
+        return {
+            "gear": current_gear_num,
+            "rpm": rpm,
+            "ESP32IPAddress": ip_address
+        }
+    
+    def __processOnRoadInAdvance(self):
+        try:
+            LqrtxOnRoad = params_memory.get_bool("LqrtxOnRoad")
+            if not self.__isOnRoadEver and LqrtxOnRoad:
+                # save if it on road ever or not, if is on road, it will not reboot in any case.
+                self.__isOnRoadEver = True
+            #if not self.__isOnRoadEver and self.__isEnginePowerOn: 
+            if not LqrtxOnRoad and self.__isEnginePowerOn: 
+                # Engine is power on and not on road will reboot
+                msg = {
+                    "ACT":OPMessageActionType.C3_AUTO_RESTART.value, # ACT is integer type, need to add .value get the Enum is value. 
+                    "a":"Engine is power on but C3 not in ONROAD status after 60 seconds." # Notification message. 
+                }
+                self.notifyMsg(msg)
+                # subprocess.check_output(["sudo", "reboot"]) # need to reboot.
+                params.put_bool("DoReboot", True)
+
+        except:
+            None
+
+    def notifyMsg(self,msg):
+        try:
+            if self.__udpSender is not None:
+                self.__udpSender.sendMsg(msg)
+        except Exception as e:
+            pass
+
+    def processAutResumeInAdvance(self):
+        """ 
+            Check if need to click res+ or not and will reset ESP32AutoResume to false after click res+ 
+            the time gap must be at least 5 second between two clicks.
+        """
+        try:
+            if params_memory.get_bool("ESP32AutoResume"):
+                if self.__esp32IPAddress is not None and ( self.__lastResPlusClickTime is None or time.monotonic() - self.__lastResPlusClickTime >= 5) :
+                    self.__sendResPlusClicked()
+                    self.__lastResPlusClickTime=time.monotonic()
+                    params_memory.put_bool("ESP32AutoResume",False)
+                    msg = {
+                        "ACT":OPMessageActionType.AUTO_RESUME_CRUISE.value, # ACT is integer type, need to add .value get the Enum is value. 
+                        "a":"Vehicle Cruise Auto Resume" # Notification message. 
+                    }
+                    self.notifyMsg(msg)
+                else:
+                    params_memory.put_bool("ESP32AutoResume",False)
+                
+        except:
+            None
+
+    def __sendResPlusClicked(self):
+        """ let esp click on Res+ button via http protocol """
+        try:
+            if self.__esp32IPAddress is not None and self.__esp32IPAddress != "":
+                #"http://192.168.2.15/admin?CMD=104&Type=2"
+                url = "http://" + self.__esp32IPAddress + "/admin?CMD=104&Type=2"
+                requests.get(url)
+        except:
+            None
+
+    def run(self):
+        while True:
+            try:
+                """ Will reset has ESP32 to false before init UDP Receiver or catch any exception when received from ESP32 UDP. """
+                params_memory.put_bool("ESP32HasIP",False)
+                self.__isEnginePowerOn = False
+                self.__enginePowerOnTime = None
+                self.__esp32IPAddress = None
+                self.__opLocalIP=self.getOPLocalIP()
+                if self.__opLocalIP is None:
+                    #print("ESP32Helper cannot get IP Address, waiting for 5 seconds")
+                    time.sleep(5)
+                    continue
+                if self.__udpSocket is not None:
+                    try:
+                        self.__udpSocket.close()
+                    except:
+                        None
+                try:
+                    self.__udpSocket=socket.socket(socket.AF_INET, socket.SOCK_DGRAM,socket.IPPROTO_UDP)
+                    self.__udpSocket.bind(("", self.__esp32UDPPort))
+                    self.__udpSocket.settimeout(5)  # 设置超时时间为 5 秒
+                except Exception as e:
+                    #49 Can't assign requested address
+                    #print(e.errno,e.strerror)
+                    time.sleep(5)
+                    continue
+                while True:
+                    try:
+                        data, addr = self.__udpSocket.recvfrom(1024)
+                        msg = self.unpack(data)
+                        if(msg['ESP32IPAddress'] is not None and msg['ESP32IPAddress'] != self.__esp32IPAddress):
+                            # get and set ESP32 IP Address and add to shared memory. 
+                            self.__esp32IPAddress = msg['ESP32IPAddress']
+                            params_memory.put("ESP32IPAddress",self.__esp32IPAddress)
+                            params_memory.put_bool("ESP32HasIP",True)
+                            #print(f"Received message: {msg} from {addr} {msg['ESP32IPAddress']}")
+
+                        if msg['rpm'] is not None :
+                            if msg["rpm"] > 100:
+                                # get rpm, if rpm is larger than 100, it means the engine power is on and save engine power on time for later use.
+                                if self.__enginePowerOnTime is None:
+                                    # just set it at the first time RPM is larger than 100 
+                                    self.__enginePowerOnTime = time.monotonic()
+                                self.__isEnginePowerOn = True                                
+                                powerOnTimePast = time.monotonic() - self.__enginePowerOnTime
+                                #Only check for Engine Power between 60 and 300 seconds. out of range, will not checking.
+                                if  powerOnTimePast > 30 and powerOnTimePast < 300:
+                                    #check if need to reboot or not.
+                                    self.__processOnRoadInAdvance()
+                            else:
+                                self.__enginePowerOnTime = None
+                                self.__isEnginePowerOn = False
+                        
+                        params_memory.put_bool("ESP32EngineOn",self.__isEnginePowerOn)
+
+                    except socket.timeout:
+                        #print("Timeout! No message received.")
+                        break
+            except Exception as e:
+                #print(f" catched excepiton: {e}")
+                pass
+            time.sleep(5)
+
+def main():
+    c3UDPSendHelper = C3UDPSendHelper()
+    c3UDPSendHelper.start()
+    esp32Helper = ESP32Helper(c3UDPSendHelper)
+    esp32Helper.start()
+    while True:
+        try:
+            # Need to do nessary operator if there has need state changes
+            #print("recevied message from manager ",sm['lqrtxDeviceState'].eventType,sm['lqrtxDeviceState'].eventJson)
+            # Do not care what is the envet type and event json. just do the check.
+            esp32Helper.processAutResumeInAdvance() # check if need to auto resume or not.
+        except:
+            None
+        time.sleep(0.2)
+
+
+if __name__ == "__main__":
+    main()

selfdrive/manager/manager.py

@@ -4,6 +4,7 @@ import os
 import signal
 import subprocess
 import sys
+import json
 import threading
 import traceback
 from pathlib import Path
@@ -288,7 +289,8 @@ def manager_init() -> None:
     ("SetSpeedRatio3", "1.045"),
     ("SpeedDecimal", "0"),
     ("CalibrationCycles", "1"), 
-    ("OnStarGPS", "0"),    
+    ("OnStarGPS", "0"),
+    ("QueitFan", "0"), # 风扇静音
     ("GearNumber", "0"), #GEAR_NUMBER_TEST
     ("FrogPilotPrebuilt", "0"), #FROGPILOT_PREBUILT_TEST
     ("UseRedPanda", "0"), #Red Panda Config BUS 0/1/2/3 -> 4/5/6/7
@@ -397,6 +399,9 @@ def manager_thread() -> None:
   sm = messaging.SubMaster(['deviceState', 'carParams'], poll='deviceState')
   pm = messaging.PubMaster(['managerState'])
 
+  #create new shared memory paramters
+  params_memory.put_bool("LqrtxOnRoad",False)
+  params_memory.put_bool("CruiseAutoResumeActivated",False)
   write_onroad_params(False, params)
   ensure_running(managed_processes.values(), False, params=params, params_memory=params_memory, CP=sm['carParams'], not_run=ignore)
 
@@ -437,6 +442,8 @@ def manager_thread() -> None:
     # update onroad params, which drives boardd's safety setter thread
     if started != started_prev:
       write_onroad_params(started, params)
+      # Update Lqrtx status
+      params_memory.put_bool("LqrtxOnRoad",started)
 
     started_prev = started
 

selfdrive/manager/process_config.py

@@ -103,6 +103,9 @@ procs = [
   PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar),
   PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
 
+  # Lqrtx processes
+  PythonProcess("lqrtxcore", "selfdrive.lqrtx.lqrtxcore", always_run),
+  
   # FrogPilot processes
   PythonProcess("fleet_manager", "selfdrive.frogpilot.fleetmanager.fleet_manager", always_run),
   # PythonProcess("frogpilot_process", "selfdrive.frogpilot.functions.frogpilot_process", always_run),

selfdrive/thermald/fan_controller.py

@@ -6,6 +6,8 @@ from openpilot.common.numpy_fast import interp
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.pid import PIDController
 
+from openpilot.common.params import Params
+
 class BaseFanController(ABC):
   @abstractmethod
   def update(self, cur_temp: float, ignition: bool) -> int:
@@ -20,6 +22,13 @@ class TiciFanController(BaseFanController):
     self.last_ignition = False
     self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML))
 
+    # 风扇静音
+    params = Params()
+    self.queit_fan = False
+    fire_the_baby_sitter = params.get_bool("FireTheBabysitter")
+    if fire_the_baby_sitter:
+      self.queit_fan = params.get_bool("QueitFan")
+
   def update(self, cur_temp: float, ignition: bool) -> int:
     self.controller.neg_limit = -(100 if ignition else 30)
     self.controller.pos_limit = -(30 if ignition else 0)
@@ -27,14 +36,18 @@ class TiciFanController(BaseFanController):
     if ignition != self.last_ignition:
       self.controller.reset()
 
-    error = 70 - cur_temp
-    # fan_pwr_out = -int(self.controller.update(
-    #                   error=error,
-    #                   feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
-    #                 ))
-    fan_pwr_out = int(interp(cur_temp, [60.0, 80.0], [0, 65]))
-    # 确保风扇功率在 0 到 65 之间
-    fan_pwr_out = max(0, min(65, fan_pwr_out))
+    # 风扇静音
+    if self.queit_fan:
+      fan_pwr_out = int(interp(cur_temp, [60.0, 80.0], [0, 65]))
+      # 确保风扇功率在 0 到 65 之间
+      fan_pwr_out = max(0, min(65, fan_pwr_out))
+    else:
+      # 风扇全速工作
+      error = 70 - cur_temp
+      fan_pwr_out = -int(self.controller.update(
+                        error=error,
+                        feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
+                      ))
 
     self.last_ignition = ignition
     return fan_pwr_out