Advisoryspeed2.0

.vscode/settings.json

@@ -23,5 +23,9 @@
     "system/**",
     "third_party/**",
     "tools/**",
-  ]
+  ],
+  "files.associations": {
+    "new": "cpp",
+    "algorithm": "cpp"
+  }
 }

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 {
 }
+
+
+
+

cereal/log.capnp

@@ -23,6 +23,7 @@ enum LongitudinalPersonality {
   relaxed @2;
 }
 
+
 struct InitData {
   kernelArgs @0 :List(Text);
   kernelVersion @15 :Text;

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),

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()

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'])
 

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
 

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'])

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<float>(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);

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;

selfdrive/ui/translations/main_ar.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_de.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_es.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_fr.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_ja.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>最高速度</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_ko.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_pt-BR.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>LIMITE</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_th.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>สูงสุด</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_tr.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation type="unfinished">MAX</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>最高定速</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -101,6 +101,10 @@
         <source>MAX</source>
         <translation>最高</translation>
     </message>
+    <message>
+        <source>ADV</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/ui.cc

@@ -186,7 +186,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster>(std::vector<const char*>{
     "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"));

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),