From f8e70e78f58ed4ebd46b7ca9dece239b0b8d7706 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 14:34:32 -0400 Subject: [PATCH 01/14] add advisory speed to the custom capnp as openpilot suggests, remain back compatability --- cereal/custom.capnp | 6 ++++++ cereal/log.capnp | 1 + 2 files changed, 7 insertions(+) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 369222add8c622..954af12d465892 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -12,8 +12,10 @@ struct CustomReserved0 @0x81c2f05a394cf4af { } struct CustomReserved1 @0xaedffd8f31e7b55d { + speed @0 :Float32; # The advisory speed data (e.g., in km/h) } + struct CustomReserved2 @0xf35cc4560bbf6ec2 { } @@ -37,3 +39,7 @@ struct CustomReserved8 @0xf416ec09499d9d19 { struct CustomReserved9 @0xa1680744031fdb2d { } + + + + diff --git a/cereal/log.capnp b/cereal/log.capnp index ddde5e4d16227b..cfbd19badd87d1 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -23,6 +23,7 @@ enum LongitudinalPersonality { relaxed @2; } + struct InitData { kernelArgs @0 :List(Text); kernelVersion @15 :Text; From 6ba1c57b363f30864a6d8b7a951c964993db28d6 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 14:45:18 -0400 Subject: [PATCH 02/14] Create advisory_speed_receiver.py Create the advisory_speed_receiver.py Module This module will be responsible for reading advisory speed data from the second USB port and publishing it to the OpenPilot messaging system. --- selfdrive/controls/advisory_speed_receiver.py | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100755 selfdrive/controls/advisory_speed_receiver.py diff --git a/selfdrive/controls/advisory_speed_receiver.py b/selfdrive/controls/advisory_speed_receiver.py new file mode 100755 index 00000000000000..bf8a074f45252f --- /dev/null +++ b/selfdrive/controls/advisory_speed_receiver.py @@ -0,0 +1,30 @@ +import cereal.messaging as messaging +import serial +import time +from cereal import custom # This will import the message from custom.capnp + +def main(): + pm = messaging.PubMaster(['advisorySpeed']) + + # Open the USB serial port where the tablet is sending advisory speed + ser = serial.Serial('/dev/ttyGS0', baudrate=9600, timeout=1) + + while True: + try: + # Read data from the serial port + line = ser.readline().decode('utf-8').strip() + if line: + advisory_speed = float(line) # Convert string to float + + # Create and send the message using custom.AdvisorySpeed + dat = messaging.new_message('advisorySpeed') + dat.advisorySpeed.speed = advisory_speed + pm.send('advisorySpeed', dat) + + time.sleep(0.1) # Adjust delay as needed + except Exception as e: + print(f"Error: {e}") + time.sleep(1) + +if __name__ == "__main__": + main() \ No newline at end of file From 23aaffcf9c77ea6712b01a9f786109f6ff1948e4 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 14:57:46 -0400 Subject: [PATCH 03/14] Update process_config.py add the new python process for receiving the advisory speed --- system/manager/process_config.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 538c55813bea5f..1c34b9884cb301 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -96,6 +96,9 @@ def only_offroad(started, params, CP: car.CarParams) -> bool: PythonProcess("uploader", "system.loggerd.uploader", always_run), PythonProcess("statsd", "system.statsd", always_run), + ##66 add the new process for receiving advisory speed + PythonProcess("advisory_speed_receiver", "selfdrive.controls.advisory_speed_receiver", always_run), + # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), From 885aa1850046a9b43deeeefb6be78940c99484b1 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 15:07:07 -0400 Subject: [PATCH 04/14] add 'advisorySpeed' to the subscribe message in controlsd --- selfdrive/controls/controlsd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 80331c6ee765b4..09b46aee386d70 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -34,9 +34,9 @@ def __init__(self) -> None: cloudlog.info("controlsd got CarParams") self.CI = get_car_interface(self.CP) - + ## add 'advisorySpeed' to the subscribed messages in the control thread self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', + 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'advisorySpeed', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) From c05c969ad6f8d26fa32876baa40bf890992f225c Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 15:09:31 -0400 Subject: [PATCH 05/14] Update custom.capnp set the added new message name advisorySpeed, ensuring consistency --- cereal/custom.capnp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 954af12d465892..6245adb82f7856 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -12,7 +12,7 @@ struct CustomReserved0 @0x81c2f05a394cf4af { } struct CustomReserved1 @0xaedffd8f31e7b55d { - speed @0 :Float32; # The advisory speed data (e.g., in km/h) + advisorySpeed @0 :Float32; # The advisory speed data (e.g., in km/h) } From 6879c32d40d399c0d81d37f17d59a10e83b1bbc6 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 15:39:54 -0400 Subject: [PATCH 06/14] add 'advisorySpeed' to sub message in plannerd note that this will allow longitudinal_planner to access 'advisorySpeed' and we can modify the long control there --- selfdrive/controls/plannerd.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ae8301871d2575..e9729469bc68ee 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -19,13 +19,14 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'], + ### add advisorySpeed to sm in the planner + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState', 'advisorySpeed'], poll='modelV2', ignore_avg_freq=['radarState']) while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm) + longitudinal_planner.update(sm) ### long planner can access 'advisorySpeed' through the sm longitudinal_planner.publish(sm, pm) ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) From eeeee5a21486812fdb0d3f738c488244fd544928 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Sun, 15 Sep 2024 15:42:32 -0400 Subject: [PATCH 07/14] demo of how to use advisorySpeed here the demo usage is to use advisorySpeed as another cruise speed limit --- selfdrive/controls/lib/longitudinal_planner.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 103d3f70470752..6a7a98f794186f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -102,6 +102,13 @@ def update(self, sm): v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS + #66 Get the advisory speed from the SubMaster + advisory_speed = sm['advisorySpeed'].advisorySpeed + advisory_speed_ms = advisory_speed * CV.KPH_TO_MS # Convert advisory speed from km/h to m/s + + #66 Limit the cruise speed by the advisory speed + v_cruise = min(v_cruise, advisory_speed_ms) + long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel From c66d2e7dde28e4252974fa0841f823e90235edd8 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Mon, 16 Sep 2024 00:48:47 -0400 Subject: [PATCH 08/14] Update advisory_speed_receiver.py Using UDP (ethernet) connection --- selfdrive/controls/advisory_speed_receiver.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/advisory_speed_receiver.py b/selfdrive/controls/advisory_speed_receiver.py index bf8a074f45252f..349ff079cded48 100755 --- a/selfdrive/controls/advisory_speed_receiver.py +++ b/selfdrive/controls/advisory_speed_receiver.py @@ -1,30 +1,30 @@ import cereal.messaging as messaging -import serial -import time -from cereal import custom # This will import the message from custom.capnp +import socket def main(): pm = messaging.PubMaster(['advisorySpeed']) - # Open the USB serial port where the tablet is sending advisory speed - ser = serial.Serial('/dev/ttyGS0', baudrate=9600, timeout=1) - + # Set up a UDP socket to receive speed data from the tablet + udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + udp_socket.bind(('192.168.2.1', 9090)) # Comma 3X IP and the port number + + print("Listening for UDP packets on port 9090...") + while True: try: - # Read data from the serial port - line = ser.readline().decode('utf-8').strip() - if line: - advisory_speed = float(line) # Convert string to float + # Receive data from the socket + data, addr = udp_socket.recvfrom(1024) + data = data.decode('utf-8').strip() + if data: + advisory_speed = float(data) # Convert the string to float # Create and send the message using custom.AdvisorySpeed dat = messaging.new_message('advisorySpeed') dat.advisorySpeed.speed = advisory_speed pm.send('advisorySpeed', dat) - time.sleep(0.1) # Adjust delay as needed except Exception as e: print(f"Error: {e}") - time.sleep(1) if __name__ == "__main__": - main() \ No newline at end of file + main() From b64642881422978be0c6c9f97fb0b525cb63c824 Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Tue, 17 Sep 2024 13:41:47 -0400 Subject: [PATCH 09/14] Delete selfdrive/assets/images/button_home.png --- selfdrive/assets/images/button_home.png | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 selfdrive/assets/images/button_home.png diff --git a/selfdrive/assets/images/button_home.png b/selfdrive/assets/images/button_home.png deleted file mode 100644 index dd3f97f44bbeea..00000000000000 --- a/selfdrive/assets/images/button_home.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3fca2a1f675cad4f454f3ba56d6e7f090a352321b61f241fc2ffcbede7a7593a -size 1501 From 60c0cb863458622d9542bec82dc7144f008c3c0e Mon Sep 17 00:00:00 2001 From: Comma Device Date: Tue, 17 Sep 2024 16:29:34 -0400 Subject: [PATCH 10/14] change inform to zmq --- selfdrive/assets/images/button_home.png | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 selfdrive/assets/images/button_home.png diff --git a/selfdrive/assets/images/button_home.png b/selfdrive/assets/images/button_home.png new file mode 100644 index 00000000000000..40f29d8bbc2870 --- /dev/null +++ b/selfdrive/assets/images/button_home.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:008cd45193bbaaabe87e531d8ea72c3a62e9ced2f5b6e0ee34bd9b75c9227743 +size 42786 From e0d87cbc5f7cc5e5d08687f0a535f21e3b0b8c7b Mon Sep 17 00:00:00 2001 From: Comma Device Date: Tue, 17 Sep 2024 16:30:45 -0400 Subject: [PATCH 11/14] change to zmq --- selfdrive/controls/advisory_speed_receiver.py | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/advisory_speed_receiver.py b/selfdrive/controls/advisory_speed_receiver.py index 349ff079cded48..2e9be22a28eab3 100755 --- a/selfdrive/controls/advisory_speed_receiver.py +++ b/selfdrive/controls/advisory_speed_receiver.py @@ -1,28 +1,30 @@ import cereal.messaging as messaging import socket +import zmq def main(): pm = messaging.PubMaster(['advisorySpeed']) - - # Set up a UDP socket to receive speed data from the tablet - udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - udp_socket.bind(('192.168.2.1', 9090)) # Comma 3X IP and the port number - print("Listening for UDP packets on port 9090...") + context = zmq.Context() + socket = context.socket(zmq.REP) + socket.bind("tcp://*:5090") + print('Listen on 5090 for ZMQ') while True: try: - # Receive data from the socket - data, addr = udp_socket.recvfrom(1024) - data = data.decode('utf-8').strip() - if data: - advisory_speed = float(data) # Convert the string to float - + # Wait for next request from client + message = socket.recv() + print("Received request: %s" % message) + # Send reply back to client + socket.send(b"ok") + if message: + advisory_speed = float(message) # Convert the string to float + print("send speed to " + advisory_speed) # Create and send the message using custom.AdvisorySpeed - dat = messaging.new_message('advisorySpeed') - dat.advisorySpeed.speed = advisory_speed + dat = messaging.new_message('customReserved1') + dat.customReserved1.advisorySpeed = advisory_speed pm.send('advisorySpeed', dat) - + except Exception as e: print(f"Error: {e}") From 11ee0dacba38d2866f50e5aa857fd9609da903fa Mon Sep 17 00:00:00 2001 From: Shengming Yuan Date: Thu, 19 Sep 2024 03:33:47 -0400 Subject: [PATCH 12/14] add adv ui --- .vscode/settings.json | 6 +++- cereal/services.py | 4 +++ selfdrive/controls/advisory_speed_receiver.py | 2 +- selfdrive/controls/controlsd.py | 2 +- .../controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/ui/qt/onroad/annotated_camera.cc | 34 +++++++++++++++++++ selfdrive/ui/qt/onroad/annotated_camera.h | 1 + selfdrive/ui/translations/main_ar.ts | 4 +++ selfdrive/ui/translations/main_de.ts | 4 +++ selfdrive/ui/translations/main_es.ts | 4 +++ selfdrive/ui/translations/main_fr.ts | 4 +++ selfdrive/ui/translations/main_ja.ts | 4 +++ selfdrive/ui/translations/main_ko.ts | 4 +++ selfdrive/ui/translations/main_pt-BR.ts | 4 +++ selfdrive/ui/translations/main_th.ts | 4 +++ selfdrive/ui/translations/main_tr.ts | 4 +++ selfdrive/ui/translations/main_zh-CHS.ts | 4 +++ selfdrive/ui/translations/main_zh-CHT.ts | 4 +++ selfdrive/ui/ui.cc | 2 +- 20 files changed, 93 insertions(+), 6 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index f0731c362d9a5c..6c94b97874c3f8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -23,5 +23,9 @@ "system/**", "third_party/**", "tools/**", - ] + ], + "files.associations": { + "new": "cpp", + "algorithm": "cpp" + } } diff --git a/cereal/services.py b/cereal/services.py index e56f8f0c1b76bf..f187f957f1d12e 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -73,6 +73,10 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "userFlag": (True, 0., 1), "microphone": (True, 10., 10), + # MOTIF Pilot Custom + "customReserved0": (True, 0.), + "customReserved1": (True, 0.), + # debug "uiDebug": (True, 0., 1), "alertDebug": (True, 20., 5), diff --git a/selfdrive/controls/advisory_speed_receiver.py b/selfdrive/controls/advisory_speed_receiver.py index 2e9be22a28eab3..c5f8ce792ecf91 100755 --- a/selfdrive/controls/advisory_speed_receiver.py +++ b/selfdrive/controls/advisory_speed_receiver.py @@ -3,7 +3,7 @@ import zmq def main(): - pm = messaging.PubMaster(['advisorySpeed']) + pm = messaging.PubMaster(['customReserved1']) context = zmq.Context() socket = context.socket(zmq.REP) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 09b46aee386d70..a1ebe2ae57b17e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -36,7 +36,7 @@ def __init__(self) -> None: self.CI = get_car_interface(self.CP) ## add 'advisorySpeed' to the subscribed messages in the control thread self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'advisorySpeed', + 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'customReserved1', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6a7a98f794186f..80fa6b80a3d4ee 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -103,7 +103,7 @@ def update(self, sm): v_cruise = v_cruise_kph * CV.KPH_TO_MS #66 Get the advisory speed from the SubMaster - advisory_speed = sm['advisorySpeed'].advisorySpeed + advisory_speed = sm['customReserved1'].advisorySpeed advisory_speed_ms = advisory_speed * CV.KPH_TO_MS # Convert advisory speed from km/h to m/s #66 Limit the cruise speed by the advisory speed diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index e9729469bc68ee..38ada43e23b61a 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -20,7 +20,7 @@ def main(): longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) ### add advisorySpeed to sm in the planner - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState', 'advisorySpeed'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState', 'customReserved1'], poll='modelV2', ignore_avg_freq=['radarState']) while True: diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index c528501cbe289f..ba2f573038cccc 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -29,6 +29,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { const bool cs_alive = sm.alive("carState"); const auto cs = sm["controlsState"].getControlsState(); const auto car_state = sm["carState"].getCarState(); + const auto custom_reserved_1_msg = sm["customReserved1"].getCustomReserved1(); is_metric = s.scene.is_metric; @@ -46,6 +47,16 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { speed = cs_alive ? std::max(0.0, v_ego) : 0.0; speed *= is_metric ? MS_TO_KPH : MS_TO_MPH; + // set advisory speed + float v_adv = custom_reserved_1_msg.getAdvisorySpeed(); + printf("Advisory speed: %f\n", v_adv); + // if (advisorySpeed > 0 && is_metric) { + // advisorySpeed *= MILE_TO_KM; + // } + // if(advisorySpeed < 0) { + // advisorySpeed = 0; + // } + speedUnit = is_metric ? tr("km/h") : tr("mph"); status = s.status; @@ -67,6 +78,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { QString speedStr = QString::number(std::nearbyint(speed)); QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–"; + QString advSpeedStr = QString::number(std::nearbyint(advisorySpeed)); // Draw outer box + border to contain set speed const QSize default_size = {172, 204}; @@ -98,6 +110,28 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(set_speed_color); p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr); + + // Draw outer box + border to contain advisory speed + QSize adv_speed_size = default_size; + if (is_metric) adv_speed_size.rwidth() = 200; + + QRect adv_speed_rect(QPoint(60 + (default_size.width() - adv_speed_size.width()) / 2, (45*2) + set_speed_size.height()), adv_speed_size); + p.setPen(QPen(whiteColor(75), 6)); + p.setBrush(blackColor(166)); + p.drawRoundedRect(adv_speed_rect, 32, 32); + + + // Draw advisory speed + QColor adv_color = QColor(0xa6, 0xa6, 0xa6, 0xff); + QColor adv_speed_color = QColor(0xa6, 0xa6, 0xa6, 0xff); + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(adv_color); + p.drawText(adv_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("ADV")); + p.setFont(InterFont(90, QFont::Bold)); + p.setPen(adv_speed_color); + p.drawText(adv_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, advSpeedStr); + + // current speed p.setFont(InterFont(176, QFont::Bold)); drawText(p, rect().center().x(), 210, speedStr); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 3820de5d86653c..bea1041855e4ae 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -23,6 +23,7 @@ class AnnotatedCameraWidget : public CameraWidget { float speed; QString speedUnit; float setSpeed; + float advisorySpeed; bool is_cruise_set = false; bool is_metric = false; bool v_ego_cluster_seen = false; diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index d73a5771fd3fe2..d2f4a7fc965194 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -101,6 +101,10 @@ MAX MAX + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 8ad55be66673f5..af8efcccc6a6f2 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -101,6 +101,10 @@ MAX MAX + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 510a2d98e04008..b1c0b4684b8a7d 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -101,6 +101,10 @@ MAX MAX + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index 80cca809e0a600..38763b2626b5a6 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -101,6 +101,10 @@ MAX MAX + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 2b8f7d242961c0..cc1f6162a1c582 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -101,6 +101,10 @@ MAX 最高速度 + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 935342d546d9b4..ae16ea3fe27cdd 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -101,6 +101,10 @@ MAX MAX + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 1c1ebff15bd604..671e844b3a0cd7 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -101,6 +101,10 @@ MAX LIMITE + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 06841222ccd696..acd350b6b5ef3e 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -101,6 +101,10 @@ MAX สูงสุด + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index f934b9d9cc3bca..9e30685b166748 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -101,6 +101,10 @@ MAX MAX + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index d90020251526a0..7b011b901fe3b0 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -101,6 +101,10 @@ MAX 最高定速 + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 91b1cf3866509e..285472d43b4bed 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -101,6 +101,10 @@ MAX 最高 + + ADV + + ConfirmationDialog diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 852980d54e8290..04e73b2b31c3a0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -186,7 +186,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique(std::vector{ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", - "wideRoadCameraState", "managerState", "selfdriveState", + "wideRoadCameraState", "managerState", "selfdriveState", "customReserved1" }); prime_state = new PrimeState(this); language = QString::fromStdString(Params().get("LanguageSetting")); From 77b7ffa30172a1398a35e8c922d702be7c00cdce Mon Sep 17 00:00:00 2001 From: Comma Device Date: Thu, 19 Sep 2024 12:56:19 -0400 Subject: [PATCH 13/14] change ui --- selfdrive/controls/advisory_speed_receiver.py | 4 ++-- selfdrive/ui/qt/onroad/annotated_camera.cc | 22 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/advisory_speed_receiver.py b/selfdrive/controls/advisory_speed_receiver.py index c5f8ce792ecf91..9cf29e2080f3e9 100755 --- a/selfdrive/controls/advisory_speed_receiver.py +++ b/selfdrive/controls/advisory_speed_receiver.py @@ -19,11 +19,11 @@ def main(): socket.send(b"ok") if message: advisory_speed = float(message) # Convert the string to float - print("send speed to " + advisory_speed) + print(f"send speed to f{advisory_speed}") # Create and send the message using custom.AdvisorySpeed dat = messaging.new_message('customReserved1') dat.customReserved1.advisorySpeed = advisory_speed - pm.send('advisorySpeed', dat) + pm.send('customReserved1', dat) except Exception as e: print(f"Error: {e}") diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index ba2f573038cccc..769f1f0e6d5edf 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -49,13 +49,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { // set advisory speed float v_adv = custom_reserved_1_msg.getAdvisorySpeed(); - printf("Advisory speed: %f\n", v_adv); - // if (advisorySpeed > 0 && is_metric) { - // advisorySpeed *= MILE_TO_KM; - // } - // if(advisorySpeed < 0) { - // advisorySpeed = 0; - // } + advisorySpeed = v_adv; + if (advisorySpeed > 0 && is_metric) { + advisorySpeed *= MILE_TO_KM; + } + if(advisorySpeed < 0) { + advisorySpeed = 0; + } speedUnit = is_metric ? tr("km/h") : tr("mph"); status = s.status; @@ -116,14 +116,14 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { if (is_metric) adv_speed_size.rwidth() = 200; QRect adv_speed_rect(QPoint(60 + (default_size.width() - adv_speed_size.width()) / 2, (45*2) + set_speed_size.height()), adv_speed_size); - p.setPen(QPen(whiteColor(75), 6)); - p.setBrush(blackColor(166)); + p.setPen(QPen(QColor(207,196,147,0xff), 6)); + p.setBrush(whiteColor(100)); p.drawRoundedRect(adv_speed_rect, 32, 32); // Draw advisory speed - QColor adv_color = QColor(0xa6, 0xa6, 0xa6, 0xff); - QColor adv_speed_color = QColor(0xa6, 0xa6, 0xa6, 0xff); + QColor adv_color = QColor(0, 103, 71, 0xff); + QColor adv_speed_color = QColor(0, 103, 71, 0xff); p.setFont(InterFont(40, QFont::DemiBold)); p.setPen(adv_color); p.drawText(adv_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("ADV")); From 1083d849db25d76a958af7c93cf5a1e80a2ed2fb Mon Sep 17 00:00:00 2001 From: Hao <46762282+HaoZhouGT@users.noreply.github.com> Date: Thu, 19 Sep 2024 19:36:08 -0400 Subject: [PATCH 14/14] mph to ms --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 80fa6b80a3d4ee..90201d531a8bc0 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -104,7 +104,7 @@ def update(self, sm): #66 Get the advisory speed from the SubMaster advisory_speed = sm['customReserved1'].advisorySpeed - advisory_speed_ms = advisory_speed * CV.KPH_TO_MS # Convert advisory speed from km/h to m/s + advisory_speed_ms = advisory_speed * CV.MPH_TO_MS # Convert advisory speed from mph to m/s #66 Limit the cruise speed by the advisory speed v_cruise = min(v_cruise, advisory_speed_ms)