diff --git a/RELEASES.md b/RELEASES.md index 0de95828..7af3e206 100644 --- a/RELEASES.md +++ b/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) diff --git a/cereal/log.capnp b/cereal/log.capnp index 964bb26f..4abaa5c9 100644 --- a/cereal/log.capnp +++ b/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; diff --git a/common/params_pyx.so b/common/params_pyx.so index 52a50e01..6d81975f 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 729b7030..46ba597c 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,6 +2,7 @@ # 12345 import os import math +import json import time import threading from types import SimpleNamespace diff --git a/selfdrive/frogpilot/fleetmanager/fleet_manager.py b/selfdrive/frogpilot/fleetmanager/fleet_manager.py index 1b106ee5..364057d0 100644 --- a/selfdrive/frogpilot/fleetmanager/fleet_manager.py +++ b/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) diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index 841afbc6..26840a25 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/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 "" diff --git a/selfdrive/lqrtx/lqrtxcore.py b/selfdrive/lqrtx/lqrtxcore.py new file mode 100644 index 00000000..1e7bfcd1 --- /dev/null +++ b/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() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index de44b0ca..dac1da0d 100755 --- a/selfdrive/manager/manager.py +++ b/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 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 0d2dfdcf..6423d138 100644 --- a/selfdrive/manager/process_config.py +++ b/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), diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index 6609e100..64c4f4e5 100755 --- a/selfdrive/thermald/fan_controller.py +++ b/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 diff --git a/selfdrive/ui/translations/main_zh-CHS.qm b/selfdrive/ui/translations/main_zh-CHS.qm index e97ca56b..51e9b976 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 538f9278..554fe8d3 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ