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/custom.capnp b/cereal/custom.capnp index 369222add8c622..6245adb82f7856 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -12,8 +12,10 @@ struct CustomReserved0 @0x81c2f05a394cf4af { } struct CustomReserved1 @0xaedffd8f31e7b55d { + advisorySpeed @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; 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/assets/images/button_home.png b/selfdrive/assets/images/button_home.png index dd3f97f44bbeea..40f29d8bbc2870 100644 --- a/selfdrive/assets/images/button_home.png +++ b/selfdrive/assets/images/button_home.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3fca2a1f675cad4f454f3ba56d6e7f090a352321b61f241fc2ffcbede7a7593a -size 1501 +oid sha256:008cd45193bbaaabe87e531d8ea72c3a62e9ced2f5b6e0ee34bd9b75c9227743 +size 42786 diff --git a/selfdrive/controls/advisory_speed_receiver.py b/selfdrive/controls/advisory_speed_receiver.py new file mode 100755 index 00000000000000..9cf29e2080f3e9 --- /dev/null +++ b/selfdrive/controls/advisory_speed_receiver.py @@ -0,0 +1,32 @@ +import cereal.messaging as messaging +import socket +import zmq + +def main(): + pm = messaging.PubMaster(['customReserved1']) + + context = zmq.Context() + socket = context.socket(zmq.REP) + socket.bind("tcp://*:5090") + print('Listen on 5090 for ZMQ') + + while True: + try: + # 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(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('customReserved1', dat) + + except Exception as e: + print(f"Error: {e}") + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 80331c6ee765b4..a1ebe2ae57b17e 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', '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 103d3f70470752..90201d531a8bc0 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['customReserved1'].advisorySpeed + 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) + long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ae8301871d2575..38ada43e23b61a 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', 'customReserved1'], 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']) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index c528501cbe289f..769f1f0e6d5edf 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(); + 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; @@ -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(QColor(207,196,147,0xff), 6)); + p.setBrush(whiteColor(100)); + p.drawRoundedRect(adv_speed_rect, 32, 32); + + + // Draw advisory speed + 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")); + 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")); 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),