diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 58628a193b..c87ac0d2e0 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -148,6 +148,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { speedLimit @3 :SpeedLimit; vTarget @4 :Float32; aTarget @5 :Float32; + events @6 :List(OnroadEventSP.Event); struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -201,6 +202,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { struct SpeedLimit { resolver @0 :Resolver; + assist @1 :Assist; struct Resolver { speedLimit @0 :Float32; @@ -214,17 +216,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { speedLimitLastValid @8 :Bool; } + struct Assist { + state @0 :AssistState; + enabled @1 :Bool; + active @2 :Bool; + vTarget @3 :Float32; + aTarget @4 :Float32; + } + enum Source { none @0; car @1; map @2; } + + enum AssistState { + disabled @0; + inactive @1; # No speed limit set or not enabled by parameter. + preActive @2; + pending @3; # Awaiting new speed limit. + adapting @4; # Reducing speed to match new speed limit. + active @5; # Cruising at speed limit. + } } enum LongitudinalPlanSource { cruise @0; sccVision @1; sccMap @2; + speedLimitAssist @3; } } @@ -267,6 +287,10 @@ struct OnroadEventSP @0xda96579883444c35 { pedalPressedAlertOnly @16; laneTurnLeft @17; laneTurnRight @18; + speedLimitPreActive @19; + speedLimitActive @20; + speedLimitChanged @21; + speedLimitPending @22; } } diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2e52c00827..34f0283ce4 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -71,7 +71,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP']) self.can_rcv_cum_timeout_counter = 0 @@ -216,6 +216,7 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP']) self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index c82287d2b1..9973862b85 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -53,6 +53,7 @@ class VCruiseHelper(VCruiseHelperSP): if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, _enabled, is_metric) + self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -104,6 +105,12 @@ class VCruiseHelper(VCruiseHelperSP): if not self.button_change_states[button_type]["enabled"]: return + # Speed Limit Assist for Non PCM long cars. + # True: Disallow set speed changes when user confirmed the target set speed during preActive state + # False: Allow set speed changes as SLA is not requesting user confirmation + if self.update_speed_limit_assist_pre_active_confirmed(button_type): + return + long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta) if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 139cdc06e7..53eca38356 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -146,7 +146,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) - # Get new v_cruise and a_desired from Smart Cruise Control + # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise) if force_slow_decel: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 8994fb7c08..4565a9c047 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -24,10 +24,11 @@ def main(): pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'liveMapDataSP', 'carStateSP', gps_location_service], - poll='modelV2') + poll='carState') while True: sm.update() + longitudinal_planner.sla.update_car_state(sm['carState']) if sm.updated['modelV2']: longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 170e184629..a13c873fbe 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -205,6 +205,7 @@ class SelfdriveD(CruiseHelper): if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) + self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events) # Add car events, ignore if CAN isn't valid if CS.canValid: diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index 4cfa3d0e3c..d95fe5fd80 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -47,10 +47,11 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) { bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0)); p.fillRect(0, 0, surface_rect.width(), UI_HEADER_HEIGHT, bg); - +#ifndef SUNNYPILOT if (is_cruise_available) { drawSetSpeed(p, surface_rect); } +#endif drawCurrentSpeed(p, surface_rect); p.restore(); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h index 22cdd8c631..295072d1ef 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h @@ -38,10 +38,12 @@ enum class SpeedLimitMode { OFF, INFORMATION, WARNING, + ASSIST, }; inline const QString SpeedLimitModeTexts[]{ QObject::tr("Off"), QObject::tr("Information"), QObject::tr("Warning"), + QObject::tr("Assist"), }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc index eed0306ca0..ff5d6221ce 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -28,6 +28,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) SpeedLimitModeTexts[static_cast(SpeedLimitMode::OFF)], SpeedLimitModeTexts[static_cast(SpeedLimitMode::INFORMATION)], SpeedLimitModeTexts[static_cast(SpeedLimitMode::WARNING)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::ASSIST)], }; speed_limit_mode_settings = new ButtonParamControlSP( "SpeedLimitMode", @@ -35,7 +36,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) "", "", speed_limit_mode_texts, - 385); + 380); list->addItem(speed_limit_mode_settings); list->addItem(horizontal_line()); @@ -107,6 +108,25 @@ void SpeedLimitSettings::refresh() { SpeedLimitOffsetType offset_type_param = static_cast(std::atoi(params.get("SpeedLimitOffsetType").c_str())); QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); + bool has_longitudinal_control; + bool intelligent_cruise_button_management_available; + auto cp_bytes = params.get("CarParamsPersistent"); + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { + AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); + + has_longitudinal_control = hasLongitudinalControl(CP); + intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); + } else { + has_longitudinal_control = false; + intelligent_cruise_button_management_available = false; + } + speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param)); speed_limit_offset->setDescription(offsetDescription(offset_type_param)); @@ -124,6 +144,13 @@ void SpeedLimitSettings::refresh() { speed_limit_offset->showDescription(); } + if (has_longitudinal_control || intelligent_cruise_button_management_available) { + speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues())); + } else { + speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues( + {SpeedLimitMode::OFF,SpeedLimitMode::INFORMATION, SpeedLimitMode::WARNING})); + } + speed_limit_mode_settings->showDescription(); speed_limit_offset->showDescription(); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h index 61c86f9208..f76f9bea2a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h @@ -58,8 +58,11 @@ private: QString off_str = tr("⦿ Off: Disables the Speed Limit functions."); QString info_str = tr("⦿ Information: Displays the current road's speed limit."); QString warning_str = tr("⦿ Warning: Provides a warning when exceeding the current road's speed limit."); + QString assist_str = tr("⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons."); - if (mode == SpeedLimitMode::WARNING) { + if (mode == SpeedLimitMode::ASSIST) { + assist_str = "" + assist_str + ""; + } else if (mode == SpeedLimitMode::WARNING) { warning_str = "" + warning_str + ""; } else if (mode == SpeedLimitMode::INFORMATION) { info_str = "" + info_str + ""; @@ -67,9 +70,27 @@ private: off_str = "" + off_str + ""; } - return QString("%1
%2
%3") + return QString("%1
%2
%3
%4") .arg(off_str) .arg(info_str) - .arg(warning_str); + .arg(warning_str) + .arg(assist_str); + } + + static std::vector getSpeedLimitModeValues() { + std::vector values; + for (int i = static_cast(SpeedLimitMode::OFF); + i <= static_cast(SpeedLimitMode::ASSIST); ++i) { + values.push_back(static_cast(i)); + } + return values; + } + + static std::vector convertSpeedLimitModeValues(const std::vector &modes) { + std::vector values; + for (const auto& mode : modes) { + values.push_back(static_cast(mode)); + } + return values; } }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index 8987f38514..3e3977ff64 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -87,7 +87,6 @@ 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()) { @@ -100,12 +99,11 @@ void LongitudinalPanel::refresh(bool _offroad) { has_longitudinal_control = hasLongitudinalControl(CP); is_pcm_cruise = CP.getPcmCruise(); - icbm_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); - has_intelligent_cruise_button_management = hasIntelligentCruiseButtonManagement(CP_SP); + intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); } else { has_longitudinal_control = false; is_pcm_cruise = false; - has_intelligent_cruise_button_management = false; + intelligent_cruise_button_management_available = false; } QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease."); @@ -117,7 +115,7 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->setDescription(onroadOnlyDescription); customAccIncrement->showDescription(); } else { - if (has_longitudinal_control || icbm_available) { + if (has_longitudinal_control || intelligent_cruise_button_management_available) { if (is_pcm_cruise) { customAccIncrement->setDescription(accPcmCruiseDisabledDescription); customAccIncrement->showDescription(); @@ -134,7 +132,7 @@ void LongitudinalPanel::refresh(bool _offroad) { } } - bool icbm_allowed = has_intelligent_cruise_button_management && !has_longitudinal_control; + bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control; intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad); // enable toggle when long is available and is not PCM cruise diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index 2fd15e955c..0b7f39c645 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -7,7 +7,6 @@ #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" @@ -25,7 +24,7 @@ private: Params params; bool has_longitudinal_control = false; bool is_pcm_cruise = false; - bool has_intelligent_cruise_button_management = false;; + bool intelligent_cruise_button_management_available = false;; bool offroad = false; QStackedLayout *main_layout = nullptr; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index a3e31455a5..df1bc3c002 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -11,7 +11,10 @@ #include "selfdrive/ui/qt/util.h" -HudRendererSP::HudRendererSP() {} +HudRendererSP::HudRendererSP() { + plus_arrow_up_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_plus_arrow_up", {105, 105}); + minus_arrow_down_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_minus_arrow_down", {105, 105}); +} void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); @@ -34,8 +37,10 @@ void HudRendererSP::updateState(const UIState &s) { speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid(); speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid(); - speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast(); + speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv; speedLimitMode = static_cast(s.scene.speed_limit_mode); + speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState(); + speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive(); roadName = s.scene.road_name; if (sm.updated("liveMapDataSP")) { roadNameStr = QString::fromStdString(lmd.getRoadName()); @@ -104,6 +109,13 @@ void HudRendererSP::updateState(const UIState &s) { void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { HudRenderer::draw(p, surface_rect); + + p.save(); + + if (is_cruise_available) { + drawSetSpeedSP(p, surface_rect); + } + if (!reversing) { // Smart Cruise Control int x_offset = -260; @@ -152,14 +164,39 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { } // Speed Limit - if (speedLimitMode != SpeedLimitMode::OFF) { - drawSpeedLimitSigns(p); - drawUpcomingSpeedLimit(p); + bool showSpeedLimit; + bool speed_limit_assist_pre_active_pulse = pulseElement(speedLimitAssistFrame); + + // Position speed limit sign next to set speed box + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + QRect sign_rect(sign_x, sign_y, sign_width, sign_height); + + if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) { + speedLimitAssistFrame++; + showSpeedLimit = speed_limit_assist_pre_active_pulse; + drawSpeedLimitPreActiveArrow(p, sign_rect); + } else { + speedLimitAssistFrame = 0; + showSpeedLimit = speedLimitMode != SpeedLimitMode::OFF; + } + + if (showSpeedLimit) { + drawSpeedLimitSigns(p, sign_rect); + + // do not show during SLA's preActive state + if (speedLimitAssistState != cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) { + drawUpcomingSpeedLimit(p); + } } // Road Name drawRoadName(p, surface_rect); } + + p.restore(); } void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) { @@ -348,10 +385,10 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) { } } -void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { - bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING; // TODO-SP: update to include SpeedLimitMode::ASSIST +void HudRendererSP::drawSpeedLimitSigns(QPainter &p, QRect &sign_rect) { + bool speedLimitWarningEnabled = speedLimitMode >= SpeedLimitMode::WARNING; // TODO-SP: update to include SpeedLimitMode::ASSIST bool hasSpeedLimit = speedLimitValid || speedLimitLastValid; - bool overspeed = hasSpeedLimit && speedLimitFinalLast < std::nearbyint(speed); + bool overspeed = hasSpeedLimit && std::nearbyint(speedLimitFinalLast) < std::nearbyint(speed); QString speedLimitStr = hasSpeedLimit ? QString::number(std::nearbyint(speedLimitLast)) : "---"; // Offset display text @@ -365,13 +402,6 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { speedLimitSubTextFactor = 0.475; } - // Position next to MAX speed box - const int sign_width = is_metric ? 200 : 172; - const int sign_x = is_metric ? 280 : 272; - const int sign_y = 45; - const int sign_height = 204; - QRect sign_rect(sign_x, sign_y, sign_width, sign_height); - int alpha = 255; QColor red_color = QColor(255, 0, 0, alpha); QColor speed_color = (speedLimitWarningEnabled && overspeed) ? red_color : @@ -573,3 +603,66 @@ void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) { QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20); p.drawText(road_rect, Qt::AlignCenter, truncated); } + +void HudRendererSP::drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect) { + const int sign_margin = 12; + const int arrow_spacing = sign_margin * 3; + int arrow_x = sign_rect.right() + arrow_spacing; + + int _set_speed = std::nearbyint(set_speed); + int _speed_limit_final_last = std::nearbyint(speedLimitFinalLast); + + // Calculate the vertical offset using a sinusoidal function for smooth bouncing + double bounce_frequency = 2.0 * M_PI / UI_FREQ; // 20 frames for one full oscillation + int bounce_offset = 20 * sin(speedLimitAssistFrame * bounce_frequency); // Adjust the amplitude (20 pixels) as needed + + if (_set_speed < _speed_limit_final_last) { + QPoint iconPosition(arrow_x, sign_rect.center().y() - plus_arrow_up_img.height() / 2 + bounce_offset); + p.drawPixmap(iconPosition, plus_arrow_up_img); + } else if (_set_speed > _speed_limit_final_last) { + QPoint iconPosition(arrow_x, sign_rect.center().y() - minus_arrow_down_img.height() / 2 - bounce_offset); + p.drawPixmap(iconPosition, minus_arrow_down_img); + } +} + +void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) { + // Draw outer box + border to contain set speed + const QSize default_size = {172, 204}; + QSize set_speed_size = is_metric ? QSize(200, 204) : default_size; + QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); + + // Draw set speed box + p.setPen(QPen(QColor(255, 255, 255, 75), 6)); + p.setBrush(QColor(0, 0, 0, 166)); + p.drawRoundedRect(set_speed_rect, 32, 32); + + // Colors based on status + QColor max_color = QColor(0xa6, 0xa6, 0xa6, 0xff); + QColor set_speed_color = QColor(0x72, 0x72, 0x72, 0xff); + if (is_cruise_set) { + set_speed_color = QColor(255, 255, 255); + if (speedLimitAssistActive) { + set_speed_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 0xff, 0, 0xff); + max_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0x80, 0xd8, 0xa6, 0xff); + } else { + if (status == STATUS_DISENGAGED) { + max_color = QColor(255, 255, 255); + } else if (status == STATUS_OVERRIDE) { + max_color = QColor(0x91, 0x9b, 0x95, 0xff); + } else { + max_color = QColor(0x80, 0xd8, 0xa6, 0xff); + } + } + } + + // Draw "MAX" text + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(max_color); + p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("MAX")); + + // Draw set speed + QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(set_speed)) : "–"; + p.setFont(InterFont(90, QFont::Bold)); + p.setPen(set_speed_color); + p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index d77892fa2d..043536bf4c 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -31,9 +31,11 @@ private: void drawStandstillTimer(QPainter &p, int x, int y); bool pulseElement(int frame); void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name); - void drawSpeedLimitSigns(QPainter &p); + void drawSpeedLimitSigns(QPainter &p, QRect &sign_rect); void drawUpcomingSpeedLimit(QPainter &p); void drawRoadName(QPainter &p, const QRect &surface_rect); + void drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect); + void drawSetSpeedSP(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -86,4 +88,9 @@ private: SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF; bool roadName; QString roadNameStr; + cereal::LongitudinalPlanSP::SpeedLimit::AssistState speedLimitAssistState; + bool speedLimitAssistActive; + int speedLimitAssistFrame; + QPixmap plus_arrow_up_img; + QPixmap minus_arrow_down_img; }; diff --git a/selfdrive/ui/sunnypilot/qt/util.cc b/selfdrive/ui/sunnypilot/qt/util.cc index eaf05942a9..2e066e88b5 100644 --- a/selfdrive/ui/sunnypilot/qt/util.cc +++ b/selfdrive/ui/sunnypilot/qt/util.cc @@ -123,7 +123,3 @@ std::optional 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"); -} diff --git a/selfdrive/ui/sunnypilot/qt/util.h b/selfdrive/ui/sunnypilot/qt/util.h index 60a73615ba..4b9d615ce5 100644 --- a/selfdrive/ui/sunnypilot/qt/util.h +++ b/selfdrive/ui/sunnypilot/qt/util.h @@ -23,4 +23,3 @@ std::optional getParamIgnoringDefault(const std::string ¶m_name, co QMap loadPlatformList(); QStringList searchFromList(const QString &query, const QStringList &list); std::optional loadCerealEvent(Params& params, const std::string& _param); -bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp); diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index f16b688358..b75bd8ccda 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -102,12 +102,13 @@ class ModularAssistiveDrivingSystem: def update_events(self, CS: structs.CarState): if not self.selfdrive.enabled and self.enabled: - if self.events.has(EventName.doorOpen): - self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen) - self.transition_paused_state() - if self.events.has(EventName.seatbeltNotLatched): - self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched) - self.transition_paused_state() + if CS.standstill: + if self.events.has(EventName.doorOpen): + self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen) + self.transition_paused_state() + if self.events.has(EventName.seatbeltNotLatched): + self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched) + self.transition_paused_state() if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse): self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear) self.transition_paused_state() diff --git a/sunnypilot/selfdrive/assets/img_minus_arrow_down.png b/sunnypilot/selfdrive/assets/img_minus_arrow_down.png new file mode 100644 index 0000000000..250f640585 --- /dev/null +++ b/sunnypilot/selfdrive/assets/img_minus_arrow_down.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0f4a0bf26cfd2c64939759d43bee839aadfd017a6f74065095a27b03615e55a4 +size 26627 diff --git a/sunnypilot/selfdrive/assets/img_plus_arrow_up.png b/sunnypilot/selfdrive/assets/img_plus_arrow_up.png new file mode 100644 index 0000000000..14c1529da3 --- /dev/null +++ b/sunnypilot/selfdrive/assets/img_plus_arrow_up.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e451ff31e9b3f144822ab282b8e47cd5a603ca59ff94bd1849271181b86c6b1 +size 29220 diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index 716e3e1c93..3691c35972 100644 --- a/sunnypilot/selfdrive/car/cruise_ext.py +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -6,18 +6,24 @@ See the LICENSE.md file in the root directory for more details. """ import numpy as np -from cereal import car +from cereal import car, custom from opendbc.car import structs +from openpilot.common.constants import CV from openpilot.common.params import Params from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target ButtonType = car.CarState.ButtonEvent.Type +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.setCruise: 0, ButtonType.resumeCruise: 0, ButtonType.cancel: 0, ButtonType.mainCruise: 0} V_CRUISE_MIN = 8 +V_CRUISE_MAX = 145 +V_CRUISE_UNSET = 255 def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None: @@ -32,11 +38,12 @@ def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarSta button_timers[b.type.raw] = 1 if b.pressed else 0 - class VCruiseHelperSP: def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None: self.CP = CP self.CP_SP = CP_SP + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET self.params = Params() self.v_cruise_min = 0 self.enabled_prev = False @@ -47,6 +54,16 @@ class VCruiseHelperSP: self.enable_button_timers = CRUISE_BUTTON_TIMER + # Speed Limit Assist + self.sla_state = SpeedLimitAssistState.disabled + self.prev_sla_state = SpeedLimitAssistState.disabled + self.has_speed_limit = False + self.speed_limit_final_last = 0. + self.speed_limit_final_last_kph = 0. + self.prev_speed_limit_final_last_kph = 0. + self.req_plus = False + self.req_minus = False + def read_custom_set_speed_params(self) -> None: self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) @@ -89,3 +106,33 @@ class VCruiseHelperSP: return enabled and self.enabled_prev return enabled + + def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None: + resolver = LP_SP.speedLimit.resolver + self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid + self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast + self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH + self.sla_state = LP_SP.speedLimit.assist.state + self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS, + self.speed_limit_final_last, is_metric) + + @property + def update_speed_limit_final_last_changed(self) -> bool: + return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph) + + def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool: + if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive: + if button_type == ButtonType.decelCruise and self.req_minus: + return True + if button_type == ButtonType.accelCruise and self.req_plus: + return True + + return False + + def update_speed_limit_assist_v_cruise_non_pcm(self) -> None: + if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or + self.update_speed_limit_final_last_changed): + self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX) + + self.prev_sla_state = self.sla_state + self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 40d30100c5..ece2ff2419 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,22 +7,29 @@ See the LICENSE.md file in the root directory for more details. from cereal import messaging, custom from opendbc.car import structs +from openpilot.common.constants import CV +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState -Source = custom.LongitudinalPlanSP.LongitudinalPlanSource +LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): + self.events_sp = EventsSP() + self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() + self.sla = SpeedLimitAssist(CP) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None - self.source = Source.cruise + self.source = LongitudinalPlanSource.cruise self.output_v_target = 0. self.output_a_target = 0. @@ -39,15 +46,31 @@ class LongitudinalPlannerSP: return self.dec.mode() def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: - self.scc.update(sm, v_ego, a_ego, v_cruise) + CS = sm['carState'] + v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) + v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS + + long_enabled = sm['carControl'].enabled + long_override = sm['carControl'].cruiseControl.override + + self.events_sp.clear() + + # Smart Cruise Control + self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) # Speed Limit Resolver self.resolver.update(v_ego, sm) + # Speed Limit Assist + has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid + self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit, + self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp) + targets = { - Source.cruise: (v_cruise, a_ego), - Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target), - Source.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target), + LongitudinalPlanSource.cruise: (v_cruise, a_ego), + LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target), + LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target), + LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target), } self.source = min(targets, key=lambda k: targets[k][0]) @@ -66,6 +89,7 @@ class LongitudinalPlannerSP: longitudinalPlanSP.longitudinalPlanSource = self.source longitudinalPlanSP.vTarget = float(self.output_v_target) longitudinalPlanSP.aTarget = float(self.output_a_target) + longitudinalPlanSP.events = self.events_sp.to_msg() # Dynamic Experimental Control dec = longitudinalPlanSP.dec @@ -104,5 +128,11 @@ class LongitudinalPlannerSP: resolver.speedLimitOffset = float(self.resolver.speed_limit_offset) resolver.distToSpeedLimit = float(self.resolver.distance) resolver.source = self.resolver.source + assist = speedLimit.assist + assist.state = self.sla.state + assist.enabled = self.sla.is_enabled + assist.active = self.sla.is_active + assist.vTarget = float(self.sla.output_v_target) + assist.aTarget = float(self.sla.output_a_target) pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py index 0f60621658..4ca45202fc 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py @@ -14,9 +14,6 @@ class SmartCruiseControl: self.vision = SmartCruiseControlVision() self.map = SmartCruiseControlMap() - def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> None: - long_enabled = sm['carControl'].enabled - long_override = sm['carControl'].cruiseControl.override - + def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None: self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise) self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py index af14a87f0c..22ea75fe19 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py @@ -6,3 +6,14 @@ See the LICENSE.md file in the root directory for more details. """ LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. + +# Speed Limit Assist constants +PCM_LONG_REQUIRED_MAX_SET_SPEED = { + True: (33.3333, 36.1111), # km/h, (120, 130) + False: (31.2928, 35.7632), # mph, (70, 80) +} + +CONFIRM_SPEED_THRESHOLD = { + True: 80, # km/h + False: 50, # mph +} diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py index 7d219ac3bb..baf9328032 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py @@ -19,3 +19,10 @@ class OffsetType(IntEnum): off = 0 fixed = 1 percentage = 2 + + +class Mode(IntEnum): + off = 0 + information = 1 + warning = 2 + assist = 3 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py new file mode 100644 index 0000000000..40e8f653ff --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py @@ -0,0 +1,19 @@ +""" +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. +""" + +from openpilot.common.constants import CV + + +def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]: + speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH + v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv) + target_set_speed_conv = round(target_set_speed * speed_conv) + + req_plus = v_cruise_cluster_conv < target_set_speed_conv + req_minus = v_cruise_cluster_conv > target_set_speed_conv + + return req_plus, req_minus diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py new file mode 100644 index 0000000000..bb03a9eb1c --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -0,0 +1,401 @@ +""" +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 cereal import custom, car +from openpilot.common.params import Params +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target +from openpilot.selfdrive.modeld.constants import ModelConstants + +ButtonType = car.CarState.ButtonEvent.Type +EventNameSP = custom.OnroadEventSP.EventName +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source + +ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting) +ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES) + +DISABLED_GUARD_PERIOD = 0.5 # secs. +PRE_ACTIVE_GUARD_PERIOD = 15 # secs. Time to wait after activation before considering temp deactivation signal. +SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive. + +LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. +LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. +LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. +LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. +V_CRUISE_UNSET = 255 + +CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise) +CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise) +CRUISE_BUTTON_CONFIRM_HOLD = 0.5 # secs. + + +class SpeedLimitAssist: + _speed_limit_final_last: float + _distance: float + v_ego: float + a_ego: float + v_offset: float + + def __init__(self, CP): + self.params = Params() + self.CP = CP + self.frame = -1 + self.long_engaged_timer = 0 + self.pre_active_timer = 0 + self.speed_limit_changed_timer = 0 + self.is_metric = self.params.get_bool("IsMetric") + self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist + self.long_enabled = False + self.long_enabled_prev = False + self.is_enabled = False + self.is_active = False + self.output_v_target = V_CRUISE_UNSET + self.output_a_target = 0. + self.v_ego = 0. + self.a_ego = 0. + self.v_offset = 0. + self.target_set_speed_conv = 0 + self.prev_target_set_speed_conv = 0 + self.v_cruise_cluster = 0. + self.v_cruise_cluster_prev = 0. + self.v_cruise_cluster_conv = 0 + self.prev_v_cruise_cluster_conv = 0 + self._has_speed_limit = False + self._speed_limit = 0. + self._speed_limit_final_last = 0. + self.speed_limit_prev = 0. + self.speed_limit_final_last_conv = 0 + self.prev_speed_limit_final_last_conv = 0 + self._distance = 0. + self.state = SpeedLimitAssistState.disabled + self._state_prev = SpeedLimitAssistState.disabled + self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise + + self._plus_hold = 0. + self._minus_hold = 0. + self._last_carstate_ts = 0. + + # TODO-SP: SLA's own output_a_target for planner + # Solution functions mapped to respective states + self.acceleration_solutions = { + SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target, + SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target, + SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target, + SpeedLimitAssistState.pending: self.get_current_acceleration_as_target, + SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration, + SpeedLimitAssistState.active: self.get_active_state_target_acceleration, + } + + @property + def speed_limit_changed(self) -> bool: + return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev) + + @property + def v_cruise_cluster_changed(self) -> bool: + return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv) + + @property + def target_set_speed_confirmed(self) -> bool: + return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv) + + def get_v_target_from_control(self) -> float: + if self._has_speed_limit: + if self.pcm_op_long and self.is_enabled: + return self._speed_limit_final_last + if not self.pcm_op_long and self.is_active: + return self._speed_limit_final_last + + # Fallback + return V_CRUISE_UNSET + + # TODO-SP: SLA's own output_a_target for planner + def get_a_target_from_control(self) -> float: + return self.a_ego + + def update_params(self) -> None: + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.is_metric = self.params.get_bool("IsMetric") + self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist + + def update_car_state(self, CS: car.CarState) -> None: + now = time.monotonic() + self._last_carstate_ts = now + + for b in CS.buttonEvents: + if not b.pressed: + if b.type in CRUISE_BUTTONS_PLUS: + self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD) + elif b.type in CRUISE_BUTTONS_MINUS: + self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD) + + def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool: + now = time.monotonic() + if req_plus and now <= self._plus_hold: + self._plus_hold = 0. + return True + elif req_minus and now <= self._minus_hold: + self._minus_hold = 0. + return True + + # expired + if now > self._plus_hold: + self._plus_hold = 0. + if now > self._minus_hold: + self._minus_hold = 0. + return False + + def update_calculations(self, v_cruise_cluster: float) -> None: + speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + self.v_cruise_cluster = v_cruise_cluster + + # Update current velocity offset (error) + self.v_offset = self._speed_limit_final_last - self.v_ego + + self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv) + self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv) + + cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] + pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \ + cst_high + pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) + + self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv + + @property + def apply_confirm_speed_threshold(self) -> bool: + # below CST: always require user confirmation + if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]: + return True + + # at/above CST: + # - new speed limit >= CST: auto change + # - new speed limit < CST: user confirmation required + return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]) + + def get_current_acceleration_as_target(self) -> float: + return self.a_ego + + def get_adapting_state_target_acceleration(self) -> float: + if self._distance > 0: + return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance) + + return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) + + def get_active_state_target_acceleration(self) -> float: + return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) + + def _update_confirmed_state(self): + if self._has_speed_limit: + if self.v_offset < LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.adapting + else: + self.state = SpeedLimitAssistState.active + else: + self.state = SpeedLimitAssistState.pending + + def _update_non_pcm_long_confirmed_state(self) -> bool: + if self.target_set_speed_confirmed: + return True + + if self.state != SpeedLimitAssistState.preActive: + return False + + req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric) + + return self._get_button_release(req_plus, req_minus) + + def update_state_machine_pcm_op_long(self): + self.long_engaged_timer = max(0, self.long_engaged_timer - 1) + self.pre_active_timer = max(0, self.pre_active_timer - 1) + + # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE + if self.state != SpeedLimitAssistState.disabled: + if not self.long_enabled or not self.enabled: + self.state = SpeedLimitAssistState.disabled + + else: + # ACTIVE + if self.state == SpeedLimitAssistState.active: + if self.v_cruise_cluster_changed: + self.state = SpeedLimitAssistState.inactive + elif self.speed_limit_changed and self.apply_confirm_speed_threshold: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.adapting + + # ADAPTING + elif self.state == SpeedLimitAssistState.adapting: + if self.v_cruise_cluster_changed: + self.state = SpeedLimitAssistState.inactive + elif self.speed_limit_changed and self.apply_confirm_speed_threshold: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + elif self.v_offset >= LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.active + + # PENDING + elif self.state == SpeedLimitAssistState.pending: + if self._has_speed_limit: + if self.v_offset < LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.adapting + else: + self.state = SpeedLimitAssistState.active + + # PRE_ACTIVE + elif self.state == SpeedLimitAssistState.preActive: + if self.target_set_speed_confirmed: + self._update_confirmed_state() + elif self.pre_active_timer <= 0: + # Timeout - session ended + self.state = SpeedLimitAssistState.inactive + + # INACTIVE + elif self.state == SpeedLimitAssistState.inactive: + pass + + # DISABLED + elif self.state == SpeedLimitAssistState.disabled: + if self.long_enabled and self.enabled: + # start or reset preActive timer if initially enabled or manual set speed change detected + if not self.long_enabled_prev or self.v_cruise_cluster_changed: + self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) + + elif self.long_engaged_timer <= 0: + if self.target_set_speed_confirmed: + self._update_confirmed_state() + else: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + + enabled = self.state in ENABLED_STATES + active = self.state in ACTIVE_STATES + + return enabled, active + + def update_state_machine_non_pcm_long(self): + self.long_engaged_timer = max(0, self.long_engaged_timer - 1) + self.pre_active_timer = max(0, self.pre_active_timer - 1) + self.speed_limit_changed_timer = max(0, self.speed_limit_changed_timer - 1) + + # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE + if self.state != SpeedLimitAssistState.disabled: + if not self.long_enabled or not self.enabled: + self.state = SpeedLimitAssistState.disabled + + else: + # ACTIVE + if self.state == SpeedLimitAssistState.active: + if self.v_cruise_cluster_changed: + self.state = SpeedLimitAssistState.inactive + + elif self.speed_limit_changed and self.apply_confirm_speed_threshold: + self.speed_limit_changed_timer = int(SPEED_LIMIT_CHANGED_HOLD_PERIOD / DT_MDL) + elif self.speed_limit_changed_timer <= 0: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + + # PRE_ACTIVE + elif self.state == SpeedLimitAssistState.preActive: + if self._update_non_pcm_long_confirmed_state(): + self.state = SpeedLimitAssistState.active + elif self.pre_active_timer <= 0: + # Timeout - session ended + self.state = SpeedLimitAssistState.inactive + + # INACTIVE + elif self.state == SpeedLimitAssistState.inactive: + if self.speed_limit_changed: + self.speed_limit_changed_timer = int(SPEED_LIMIT_CHANGED_HOLD_PERIOD / DT_MDL) + elif self.speed_limit_changed_timer <= 0: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + elif self._update_non_pcm_long_confirmed_state(): + self.state = SpeedLimitAssistState.active + + # DISABLED + elif self.state == SpeedLimitAssistState.disabled: + if self.long_enabled and self.enabled: + # start or reset preActive timer if initially enabled or manual set speed change detected + if not self.long_enabled_prev or self.v_cruise_cluster_changed: + self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) + + elif self.long_engaged_timer <= 0: + if self._update_non_pcm_long_confirmed_state(): + self.state = SpeedLimitAssistState.active + elif self._has_speed_limit: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + else: + self.state = SpeedLimitAssistState.inactive + + enabled = self.state in ENABLED_STATES + active = self.state in ACTIVE_STATES + + return enabled, active + + def update_events(self, events_sp: EventsSP) -> None: + if self.state == SpeedLimitAssistState.preActive: + events_sp.add(EventNameSP.speedLimitPreActive) + + if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending: + events_sp.add(EventNameSP.speedLimitPending) + + if self.is_active: + if self._state_prev not in ACTIVE_STATES: + events_sp.add(EventNameSP.speedLimitActive) + + # only notify if we acquire a valid speed limit + # do not check has_speed_limit here + elif self._speed_limit != self.speed_limit_prev: + if self.speed_limit_prev <= 0: + events_sp.add(EventNameSP.speedLimitActive) + elif self.speed_limit_prev > 0 and self._speed_limit > 0: + events_sp.add(EventNameSP.speedLimitChanged) + + def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float, + speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None: + self.long_enabled = long_enabled + self.v_ego = v_ego + self.a_ego = a_ego + + self._has_speed_limit = has_speed_limit + self._speed_limit = speed_limit + self._speed_limit_final_last = speed_limit_final_last + self._distance = distance + + self.update_params() + self.update_calculations(v_cruise_cluster) + + self._state_prev = self.state + if self.pcm_op_long: + self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long() + else: + self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long() + + self.update_events(events_sp) + + # Update change tracking variables + self.speed_limit_prev = self._speed_limit + self.v_cruise_cluster_prev = self.v_cruise_cluster + self.long_enabled_prev = self.long_enabled + self.prev_target_set_speed_conv = self.target_set_speed_conv + self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv + self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv + + self.output_v_target = self.get_v_target_from_control() + self.output_a_target = self.get_a_target_from_control() + + self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py index f145b16c10..51fb8f6d64 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py @@ -57,6 +57,12 @@ class SpeedLimitResolver: self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) + self.speed_limit = 0. + self.speed_limit_last = 0. + self.speed_limit_final = 0. + self.speed_limit_final_last = 0. + self.speed_limit_offset = 0. + def update_speed_limit_states(self) -> None: self.speed_limit_final = self.speed_limit + self.speed_limit_offset diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py new file mode 100644 index 0000000000..3b2716265d --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -0,0 +1,227 @@ +""" +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. +""" +from cereal import custom + +from opendbc.car.car_helpers import interfaces +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.common.constants import CV +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \ + PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP + +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState + +ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values()) + +SPEED_LIMITS = { + 'residential': 25 * CV.MPH_TO_MS, # 25 mph + 'city': 35 * CV.MPH_TO_MS, # 35 mph + 'highway': 65 * CV.MPH_TO_MS, # 65 mph + 'freeway': 80 * CV.MPH_TO_MS, # 80 mph +} + + +class TestSpeedLimitAssist: + + def setup_method(self): + self.params = Params() + self.reset_custom_params() + self.events_sp = EventsSP() + CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2) + self.sla = SpeedLimitAssist(CI.CP) + self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now + self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH + + def teardown_method(self, method): + self.reset_state() + + def _setup_platform(self, car_name): + CarInterface = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) + CI = CarInterface(CP, CP_SP) + sunnypilot_interfaces.setup_interfaces(CI, self.params) + return CI + + def reset_custom_params(self): + self.params.put("SpeedLimitMode", int(Mode.assist)) + self.params.put_bool("IsMetric", False) + self.params.put("SpeedLimitOffsetType", 0) + self.params.put("SpeedLimitValueOffset", 0) + + def reset_state(self): + self.sla.state = SpeedLimitAssistState.disabled + self.sla.frame = -1 + self.sla.last_op_engaged_frame = 0 + self.sla.op_engaged = False + self.sla.op_engaged_prev = False + self.sla._speed_limit = 0. + self.sla.speed_limit_prev = 0. + self.sla.last_valid_speed_limit_offsetted = 0. + self.sla._distance = 0. + self.events_sp.clear() + + def initialize_active_state(self, initialize_v_cruise): + self.sla.state = SpeedLimitAssistState.active + self.sla.v_cruise_cluster = initialize_v_cruise + self.sla.v_cruise_cluster_prev = initialize_v_cruise + self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv) + + def test_initial_state(self): + assert self.sla.state == SpeedLimitAssistState.disabled + assert not self.sla.is_enabled + assert not self.sla.is_active + assert V_CRUISE_UNSET == self.sla.get_v_target_from_control() + + def test_disabled(self): + self.params.put("SpeedLimitMode", int(Mode.off)) + for _ in range(int(10. / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.disabled + + def test_transition_disabled_to_preactive(self): + for _ in range(int(3. / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.preActive + assert self.sla.is_enabled and not self.sla.is_active + + def test_preactive_to_active_with_max_speed_confirmation(self): + self.sla.state = SpeedLimitAssistState.preActive + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], + SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + assert self.sla.is_enabled and self.sla.is_active + assert self.sla.output_v_target == SPEED_LIMITS['highway'] + + def test_preactive_timeout_to_inactive(self): + self.sla.state = SpeedLimitAssistState.preActive + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + + for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.inactive + + def test_preactive_to_pending_no_speed_limit(self): + self.sla.state = SpeedLimitAssistState.preActive + self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.pending + assert self.sla.is_enabled and not self.sla.is_active + + def test_pending_to_active_when_speed_limit_available(self): + self.sla.state = SpeedLimitAssistState.pending + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + + def test_pending_to_adapting_when_below_speed_limit(self): + self.sla.state = SpeedLimitAssistState.pending + self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + assert self.sla.is_enabled and self.sla.is_active + + def test_active_to_adapting_transition(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + + self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], + SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + + def test_adapting_to_active_transition(self): + self.sla.state = SpeedLimitAssistState.adapting + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], + SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + + def test_manual_cruise_change_detection(self): + self.sla.state = SpeedLimitAssistState.active + expected_cruise = SPEED_LIMITS['highway'] + self.sla.v_cruise_cluster_prev = expected_cruise + + different_cruise = SPEED_LIMITS['highway'] + 5 + self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.inactive + + # TODO-SP: test lower CST cases + def test_rapid_speed_limit_changes(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']] + + for _, speed_limit in enumerate(speed_limits): + self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp) + assert self.sla.state in ACTIVE_STATES + + def test_invalid_speed_limits_handling(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + + invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] + + for invalid_limit in invalid_limits: + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp) + assert isinstance(self.sla.output_v_target, (int, float)) + assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0 + + def test_stale_data_handling(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + old_speed_limit = SPEED_LIMITS['city'] + + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp) + assert self.sla.state in ACTIVE_STATES + assert self.sla.output_v_target == old_speed_limit + + def test_distance_based_adapting(self): + self.sla.state = SpeedLimitAssistState.adapting + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + distance = 100.0 + current_speed = SPEED_LIMITS['freeway'] + target_speed = SPEED_LIMITS['highway'] + + self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions + + def test_long_disengaged_to_disabled(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + + self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], + SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.disabled + assert self.sla.output_v_target == V_CRUISE_UNSET + + def test_maintain_states_with_no_changes(self): + """Test that states are maintained when no significant changes occur""" + test_states = [ + SpeedLimitAssistState.preActive, + SpeedLimitAssistState.pending, + SpeedLimitAssistState.active, + SpeedLimitAssistState.adapting + ] + + for state in test_states: + self.sla.state = state + self.sla.op_engaged = True + + initial_state = state + + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + + assert self.sla.state in ALL_STATES # Sanity check + + if initial_state == SpeedLimitAssistState.preActive: + assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active] + elif initial_state in ACTIVE_STATES: + assert self.sla.state in ACTIVE_STATES diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 7c723397fb..7700550c2d 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -1,6 +1,10 @@ +import cereal.messaging as messaging from cereal import log, car, custom +from openpilot.common.constants import CV from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target AlertSize = log.SelfdriveState.AlertSize @@ -14,6 +18,49 @@ EventNameSP = custom.OnroadEventSP.EventName EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()} +def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit + speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) + message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit' + return Alert( + message, + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.) + + +def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH + speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast + speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv) + + if CP.openpilotLongitudinalControl and CP.pcmCruise: + # PCM long + cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric] + pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high + pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) + speed_unit = "km/h" if metric else "mph" + alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate" + else: + # Non PCM long + v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS + + req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric) + arrow_str = "" + if req_plus: + arrow_str = "RES/+" + elif req_minus: + arrow_str = "SET/-" + + alert_2_str = f"Operate the {arrow_str} cruise control button to activate" + + return Alert( + "Speed Limit Assist: Activation Required", + alert_2_str, + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) + + class EventsSP(EventsBase): def __init__(self): super().__init__() @@ -148,5 +195,33 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), - } + }, + + EventNameSP.speedLimitActive: { + ET.WARNING: Alert( + "Automatically adjusting to the posted speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + }, + + EventNameSP.speedLimitChanged: { + ET.WARNING: Alert( + "Set speed changed", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + }, + + EventNameSP.speedLimitPreActive: { + ET.WARNING: speed_limit_pre_active_alert, + }, + + EventNameSP.speedLimitPending: { + ET.WARNING: Alert( + "Automatically adjusting to the last speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + }, }