Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

cereal/custom.capnp

@@ -146,6 +146,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
   speedLimit @3 :SpeedLimit;
+  vTarget @4 :Float32;
+  aTarget @5 :Float32;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;

common/params_keys.h

@@ -234,7 +234,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OsmStateName", {PERSISTENT, STRING, "All"}},
     {"OsmStateTitle", {PERSISTENT, STRING}},
     {"OsmWayTest", {PERSISTENT, STRING}},
-    {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
+    {"RoadName", {PERSISTENT, STRING}},
 
     // Speed Limit
     {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}},

selfdrive/selfdrived/selfdrived.py

@@ -96,7 +96,7 @@ class SelfdriveD(CruiseHelper):
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP'] + \
+                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -444,7 +444,7 @@ class SelfdriveD(CruiseHelper):
           self.events.add(EventName.personalityChanged)
         self.experimental_mode_switched = False
 
-    self.icbm.run(CS, self.sm['carControl'], self.is_metric)
+    self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
 
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc

@@ -4,6 +4,7 @@
  * This file is part of sunnypilot and is licensed under the MIT License.
  * See the LICENSE.md file in the root directory for more details.
  */
+#include <QScrollBar>
 
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h"
 
@@ -38,8 +39,9 @@ SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) {
   list->addItem(speed_limit_policy);
   connect(speed_limit_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitPolicy::refresh);
 
+  speedLimitPolicyScroller = new ScrollViewSP(list, this);
+  main_layout->addWidget(speedLimitPolicyScroller);
   refresh();
-  main_layout->addWidget(list);
 };
 
 void SpeedLimitPolicy::refresh() {
@@ -48,6 +50,7 @@ void SpeedLimitPolicy::refresh() {
 }
 
 void SpeedLimitPolicy::showEvent(QShowEvent *event) {
+  speedLimitPolicyScroller->verticalScrollBar()->setValue(0);
   refresh();
   speed_limit_policy->showDescription();
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h

@@ -10,6 +10,7 @@
 #include "selfdrive/ui/sunnypilot/ui.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 
 class SpeedLimitPolicy : public QWidget {
   Q_OBJECT
@@ -25,6 +26,7 @@ signals:
 private:
   Params params;
   ButtonParamControlSP *speed_limit_policy;
+  ScrollViewSP *speedLimitPolicyScroller;
 
   static QString sourceDescription(SpeedLimitSourcePolicy type = SpeedLimitSourcePolicy::CAR_ONLY) {
     QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car");

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc

@@ -31,7 +31,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
   };
   speed_limit_mode_settings = new ButtonParamControlSP(
     "SpeedLimitMode",
-    tr("Speed Limit Mode"),
+    tr("Speed Limit"),
     "",
     "",
     speed_limit_mode_texts,
@@ -43,10 +43,12 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
 
   speedLimitSource = new PushButtonSP(tr("Customize Source"));
   connect(speedLimitSource, &QPushButton::clicked, [&]() {
+    speedLimitScroller->setLastScrollPosition();
     setCurrentWidget(speedLimitPolicyScreen);
     speedLimitPolicyScreen->refresh();
   });
   connect(speedLimitPolicyScreen, &SpeedLimitPolicy::backPress, [&]() {
+    speedLimitScroller->restoreScrollPosition();
     setCurrentWidget(subPanelFrame);
     showEvent(new QShowEvent());
   });
@@ -92,7 +94,8 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
   connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
 
   refresh();
-  subPanelLayout->addWidget(list);
+  speedLimitScroller = new ScrollViewSP(list, this);
+  subPanelLayout->addWidget(speedLimitScroller);
   addWidget(subPanelFrame);
   addWidget(speedLimitPolicyScreen);
   setCurrentWidget(subPanelFrame);

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h

@@ -12,6 +12,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 
 class SpeedLimitSettings : public QStackedWidget {
   Q_OBJECT
@@ -26,6 +27,7 @@ signals:
 
 private:
   Params params;
+  ScrollViewSP *speedLimitScroller;
   QFrame *subPanelFrame;
   ButtonParamControlSP *speed_limit_mode_settings;
   PushButtonSP *speedLimitSource;

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc

@@ -80,6 +80,7 @@ void LongitudinalPanel::showEvent(QShowEvent *event) {
 }
 
 void LongitudinalPanel::refresh(bool _offroad) {
+  auto icbm_available = false;
   auto cp_bytes = params.get("CarParamsPersistent");
   auto cp_sp_bytes = params.get("CarParamsSPPersistent");
   if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
@@ -92,11 +93,12 @@ void LongitudinalPanel::refresh(bool _offroad) {
 
     has_longitudinal_control = hasLongitudinalControl(CP);
     is_pcm_cruise = CP.getPcmCruise();
-    intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
+    icbm_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
+    has_intelligent_cruise_button_management = hasIntelligentCruiseButtonManagement(CP_SP);
   } else {
     has_longitudinal_control = false;
     is_pcm_cruise = false;
-    intelligent_cruise_button_management_available = false;
+    has_intelligent_cruise_button_management = false;
   }
 
   QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease.");
@@ -108,7 +110,7 @@ void LongitudinalPanel::refresh(bool _offroad) {
     customAccIncrement->setDescription(onroadOnlyDescription);
     customAccIncrement->showDescription();
   } else {
-    if (has_longitudinal_control || intelligent_cruise_button_management_available) {
+    if (has_longitudinal_control || icbm_available) {
       if (is_pcm_cruise) {
         customAccIncrement->setDescription(accPcmCruiseDisabledDescription);
         customAccIncrement->showDescription();
@@ -125,7 +127,7 @@ void LongitudinalPanel::refresh(bool _offroad) {
     }
   }
 
-  bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control;
+  bool icbm_allowed = has_intelligent_cruise_button_management && !has_longitudinal_control;
   intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad);
 
   // enable toggle when long is available and is not PCM cruise

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include "selfdrive/ui/sunnypilot/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
@@ -24,7 +25,7 @@ private:
   Params params;
   bool has_longitudinal_control = false;
   bool is_pcm_cruise = false;
-  bool intelligent_cruise_button_management_available = false;;
+  bool has_intelligent_cruise_button_management = false;;
   bool offroad = false;
 
   QStackedLayout *main_layout = nullptr;

selfdrive/ui/sunnypilot/qt/onroad/hud.cc

@@ -333,15 +333,16 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
 }
 
 void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
+  bool speedLimitValid = speedLimit > 0;
   int speedLimitRounded = std::nearbyint(speedLimit);
   bool overspeed = speedLimitRounded < std::nearbyint(speed) && speedLimitRounded > 0;
   bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING;
-  QString speedLimitStr = speedLimit > 0 ? QString::number(speedLimitRounded) : "---";
+  QString speedLimitStr = speedLimitValid ? QString::number(speedLimitRounded) : "---";
 
   // Offset display text
   QString speedLimitSubText = "";
   if (speedLimitOffset != 0) {
-    speedLimitSubText = (speedLimitOffset > 0 ? "+" : "") + QString::number(std::nearbyint(speedLimitOffset));
+    speedLimitSubText = (speedLimitOffset > 0 ? "+" : "-") + QString::number(std::nearbyint(speedLimitOffset));
   }
 
   // Position next to MAX speed box
@@ -392,7 +393,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
     p.drawText(center_circle, Qt::AlignCenter, speedLimitStr);
 
     // Offset value in small circular box
-    if (!speedLimitSubText.isEmpty()) {
+    if (!speedLimitSubText.isEmpty() && speedLimitValid) {
       int offset_circle_size = circle_size * 0.4;
       int overlap = offset_circle_size * 0.25;
       QRect offset_circle_rect(
@@ -437,7 +438,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
     p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
 
     // Offset value in small box
-    if (!speedLimitSubText.isEmpty()) {
+    if (!speedLimitSubText.isEmpty() && speedLimitValid) {
       int offset_box_size = sign_rect.width() * 0.4;
       int overlap = offset_box_size * 0.25;
       QRect offset_box_rect(

selfdrive/ui/sunnypilot/qt/util.cc

@@ -123,3 +123,7 @@ std::optional<cereal::Event::Reader> loadCerealEvent(Params& params, const std::
     return std::nullopt;
   }
 }
+
+bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp) {
+  return car_params_sp.getIntelligentCruiseButtonManagementAvailable() && Params().getBool("IntelligentCruiseButtonManagement");
+}

selfdrive/ui/sunnypilot/qt/util.h

@@ -23,3 +23,4 @@ std::optional<QString> getParamIgnoringDefault(const std::string &param_name, co
 QMap<QString, QVariantMap> loadPlatformList();
 QStringList searchFromList(const QString &query, const QStringList &list);
 std::optional<cereal::Event::Reader> loadCerealEvent(Params& params, const std::string& _param);
+bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp);

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -4,12 +4,12 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
-import time
 from abc import abstractmethod, ABC
 
 from cereal import messaging
 from openpilot.common.gps import get_gps_location_service
 from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param
 
 
@@ -18,7 +18,9 @@ class BaseMapData(ABC):
     self.params = Params()
 
     self.gps_location_service = get_gps_location_service(self.params)
-    self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service])
+    gps_packets = [self.gps_location_service]
+    self.sm = messaging.SubMaster(['livePose'] + gps_packets, ignore_alive=gps_packets, ignore_avg_freq=gps_packets,
+                                  ignore_valid=gps_packets, poll='livePose')
     self.pm = messaging.PubMaster(['liveMapDataSP'])
 
     self.last_position = coordinate_from_param("LastGPSPosition", self.params)
@@ -44,20 +46,22 @@ class BaseMapData(ABC):
     gps = self.sm[self.gps_location_service]
 
     # ignore the message if the fix is invalid
-    gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0
+    gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0
     if not gps_ok and self.sm['livePose'].inputsOK:
-      return None
+      return
 
     # livePose has these data, but aren't on cereal
     self.last_position = Coordinate(gps.latitude, gps.longitude)
     self.last_altitude = gps.altitude
 
+    return
+
   def publish(self) -> None:
     speed_limit = self.get_current_speed_limit()
     next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
 
     mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose'])
+    mapd_sp_send.valid = self.sm.all_checks(['livePose'])
     live_map_data = mapd_sp_send.liveMapDataSP
 
     live_map_data.speedLimitValid = bool(speed_limit > 0)

sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -49,19 +49,11 @@ class IntelligentCruiseButtonManagement:
   def v_cruise_equal(self) -> bool:
     return self.v_target == self.v_cruise_cluster
 
-  def update_calculations(self, CS: car.CarState) -> None:
+  def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None:
     speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
     ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
-    v_cruise_ms = CS.vCruise * CV.KPH_TO_MS
 
-    # all targets in m/s
-    v_targets = {
-      LongitudinalPlanSource.cruise: v_cruise_ms
-    }
-    source = min(v_targets, key=lambda k: v_targets[k])
-    v_target_ms = v_targets[source]
-
-    self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv)
+    self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv)
 
     self.v_target = round(self.v_target_ms_last * speed_conv)
     self.v_cruise_min = get_minimum_set_speed(self.is_metric)
@@ -116,20 +108,18 @@ class IntelligentCruiseButtonManagement:
   def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
     update_manual_button_timers(CS, self.cruise_button_timers)
 
-    allowed_speed = CS.vEgo > ALLOWED_SPEED_THRESHOLD
-    ready = CS.cruiseState.enabled and allowed_speed and not CC.cruiseControl.override and not CC.cruiseControl.cancel and \
-            not CC.cruiseControl.resume
+    ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume
     button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers)
 
     self.is_ready = ready and not button_pressed
 
-  def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None:
+  def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
     if self.CP_SP.pcmCruiseSpeed:
       return
 
     self.is_metric = is_metric
 
-    self.update_calculations(CS)
+    self.update_calculations(CS, LP_SP)
     self.update_readiness(CS, CC)
 
     self.cruise_button = self.update_state_machine()

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -24,6 +24,9 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = Source.cruise
 
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+
   @property
   def mlsim(self) -> bool:
     # If we don't have a generation set, we assume it's default model. Which as of today are mlsim.
@@ -47,9 +50,8 @@ class LongitudinalPlannerSP:
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
-    v_target, a_target = targets[self.source]
-
-    return v_target, a_target
+    self.output_v_target, self.output_a_target = targets[self.source]
+    return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
@@ -61,6 +63,8 @@ class LongitudinalPlannerSP:
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
     longitudinalPlanSP.longitudinalPlanSource = self.source
+    longitudinalPlanSP.vTarget = float(self.output_v_target)
+    longitudinalPlanSP.aTarget = float(self.output_a_target)
 
     # Dynamic Experimental Control
     dec = longitudinalPlanSP.dec