diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 58628a193..44b8bd69f 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -148,6 +148,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { speedLimit @3 :SpeedLimit; vTarget @4 :Float32; aTarget @5 :Float32; + events @6 :List(OnroadEventSP.Event); + e2eAlerts @7 :E2eAlerts; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -201,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { struct SpeedLimit { resolver @0 :Resolver; + assist @1 :Assist; struct Resolver { speedLimit @0 :Float32; @@ -214,17 +217,40 @@ 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; + } + + struct E2eAlerts { + greenLightAlert @0 :Bool; + leadDepartAlert @1 :Bool; } } @@ -267,6 +293,11 @@ struct OnroadEventSP @0xda96579883444c35 { pedalPressedAlertOnly @16; laneTurnLeft @17; laneTurnRight @18; + speedLimitPreActive @19; + speedLimitActive @20; + speedLimitChanged @21; + speedLimitPending @22; + e2eChime @23; } } @@ -275,6 +306,7 @@ struct CarParamsSP @0x80ae746ee2596b11 { safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags pcmCruiseSpeed @3 :Bool; intelligentCruiseButtonManagementAvailable @4 :Bool; + enableGasInterceptor @5 :Bool; neuralNetworkLateralControl @2 :NeuralNetworkLateralControl; diff --git a/common/params_keys.h b/common/params_keys.h index 8d32689f2..227dfdd17 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -160,15 +160,20 @@ inline static std::unordered_map keys = { {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}}, {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}}, {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}}, + {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}}, {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}}, {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}}, {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}}, {"LastGPSPositionLLK", {PERSISTENT, STRING}}, + {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}}, {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}}, {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}}, {"Offroad_TiciSupport", {CLEAR_ON_MANAGER_START, JSON}}, + {"OnroadScreenOffBrightness", {PERSISTENT | BACKUP, INT, "100"}}, + {"OnroadScreenOffControl", {PERSISTENT | BACKUP, BOOL}}, + {"OnroadScreenOffTimer", {PERSISTENT | BACKUP, INT, "0"}}, {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}}, {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/opendbc_repo b/opendbc_repo index 637cb3e68..8b89eb421 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 637cb3e683b3b63448ae55f406ff29283135e430 +Subproject commit 8b89eb421502c69636c7f442bf7f850ebbb4e7fb diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py old mode 100755 new mode 100644 index 06d5c5245..4d8d85b5f --- 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 @@ -124,7 +124,7 @@ class Car: self.CP.alternativeExperience = 0 # mads - set_alternative_experience(self.CP, self.params) + set_alternative_experience(self.CP, self.CP_SP, self.params) set_car_specific_params(self.CP, self.CP_SP, self.params) # Dynamic Experimental Control @@ -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, self.sm['carControl'].cruiseControl.speedLimit, self.sm['carControl'].cruiseControl.speedLimitPredicative) 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 512db7259..63a0718f0 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -55,6 +55,7 @@ class VCruiseHelper(VCruiseHelperSP): # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_speed_limit(CS, _enabled, speed_limit_control, speed_limit_predicative) 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: @@ -121,6 +122,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/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 7c7a56300..821767ee4 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -60,7 +60,7 @@ class TestCarInterfaces: # Test controller initialization # TODO: wait until card refactor is merged to run controller a few times, # hypothesis also slows down significantly with just one more message draw - LongControl(car_params) + LongControl(car_params, car_params_sp) if car_params.steerControlType == CarParams.SteerControlType.angle: LatControlAngle(car_params, car_params_sp, car_interface) elif car_params.lateralTuning.which() == 'pid': diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 297b36401..01483f24a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -70,7 +70,7 @@ class Controls(ControlsExt, ModelStateBase): self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None - self.LoC = LongControl(self.CP) + self.LoC = LongControl(self.CP, self.CP_SP) self.VM = VehicleModel(self.CP) self.LaC: LatControl if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6d4f92246..b8b00e2fb 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -10,8 +10,11 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] LongCtrlState = car.CarControl.Actuators.LongControlState -def long_control_state_trans(CP, active, long_control_state, v_ego, +def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego, should_stop, brake_pressed, cruise_standstill): + # Gas Interceptor + cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor + stopping_condition = should_stop starting_condition = (not should_stop and not cruise_standstill and @@ -45,8 +48,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, return long_control_state class LongControl: - def __init__(self, CP): + def __init__(self, CP, CP_SP): self.CP = CP + self.CP_SP = CP_SP self.long_control_state = LongCtrlState.off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), @@ -61,7 +65,7 @@ class LongControl: self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] - self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, + self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, CS.cruiseState.standstill) if self.long_control_state == LongCtrlState.off: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index fb215ad81..f46d4b8f7 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -155,7 +155,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 8994fb7c0..4565a9c04 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/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index ab50810d8..cf0ab24e0 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, custom from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans @@ -8,49 +8,52 @@ class TestLongControlStateTransition: def test_stay_stopped(self): CP = car.CarParams.new_message() + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=True, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=True, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=True) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid active = False - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.off def test_engage(): CP = car.CarParams.new_message() + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.off - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=True, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=True, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=True) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid def test_starting(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.starting - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.starting - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 170e18462..4bc75021f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -159,7 +159,7 @@ class SelfdriveD(CruiseHelper): self.mads = ModularAssistiveDrivingSystem(self) self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP) - self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP) CruiseHelper.__init__(self, self.CP) @@ -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 b191a7a54..b2fefe486 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -57,10 +57,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); auto params = Params(); diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 5c2680b29..3f0e013d3 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -24,6 +24,7 @@ qt_src = [ "sunnypilot/qt/offroad/offroad_home.cc", "sunnypilot/qt/offroad/settings/developer_panel.cc", "sunnypilot/qt/offroad/settings/device_panel.cc", + "sunnypilot/qt/offroad/settings/display_panel.cc", "sunnypilot/qt/offroad/settings/lateral_panel.cc", "sunnypilot/qt/offroad/settings/longitudinal_panel.cc", "sunnypilot/qt/offroad/settings/max_time_offroad.cc", @@ -90,9 +91,13 @@ brand_settings_qt_src = [ "sunnypilot/qt/offroad/settings/vehicle/volkswagen_settings.cc", ] +display_panel_qt_src = [ + "sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc", +] sp_widgets_src = widgets_src + network_src -sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + longitudinal_panel_qt_src + osm_panel_qt_src +sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + \ + longitudinal_panel_qt_src + osm_panel_qt_src + display_panel_qt_src sp_qt_util = qt_util Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util") diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc new file mode 100644 index 000000000..3e9dc427b --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc @@ -0,0 +1,63 @@ +/** + * 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. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h" + +OnroadScreenBrightnessControl::OnroadScreenBrightnessControl(const QString ¶m, const QString &title, + const QString &description, const QString &icon, + QWidget *parent) + : ExpandableToggleRow(param, title, description, icon, parent) { + auto *mainFrame = new QFrame(this); + auto *mainFrameLayout = new QGridLayout(); + mainFrame->setLayout(mainFrameLayout); + mainFrameLayout->setSpacing(0); + + onroadScreenOffTimer = new OptionControlSP( + "OnroadScreenOffTimer", + "", + "", + "", + {0, 11}, 1, true, &onroadScreenOffTimerOptions); + + onroadScreenBrightness = new OptionControlSP( + "OnroadScreenOffBrightness", + "", + "", + "", + {0, 100}, 10, true, nullptr, false); + + connect(onroadScreenOffTimer, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh); + connect(onroadScreenBrightness, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh); + onroadScreenOffTimer->setFixedWidth(280); + onroadScreenBrightness->setFixedWidth(280); + mainFrameLayout->addWidget(onroadScreenOffTimer, 0, 0, Qt::AlignLeft); + mainFrameLayout->addWidget(onroadScreenBrightness, 0, 1, Qt::AlignRight); + + addItem(mainFrame); + + refresh(); +} + +void OnroadScreenBrightnessControl::refresh() { + // Driving Screen Off Timer + int valTimer = std::atoi(params.get("OnroadScreenOffTimer").c_str()); + std::string labelTimer = ""; + labelTimer += "Delay"; + labelTimer += "
"; + labelTimer += (valTimer < 60 ? std::to_string(valTimer) + "s" : std::to_string(valTimer / 60) + "m"); + labelTimer += "
"; + onroadScreenOffTimer->setLabel(QString::fromStdString(labelTimer)); + + // Driving Screen Off Brightness + std::string valBrightness = params.get("OnroadScreenOffBrightness"); + std::string labelBrightness = ""; + labelBrightness += "Brightness"; + labelBrightness += "
"; + labelBrightness += (valBrightness == "0" ? " Screen Off" : valBrightness + "%"); + labelBrightness += "
"; + onroadScreenBrightness->setLabel(QString::fromStdString(labelBrightness)); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h new file mode 100644 index 000000000..02335d1ce --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h @@ -0,0 +1,42 @@ +/** + * 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. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h" + +static const QMap onroadScreenOffTimerOptions = { + {"0", "15"}, + {"1", "30"}, + {"2", "60"}, + {"3", "120"}, + {"4", "180"}, + {"5", "240"}, + {"6", "300"}, + {"7", "360"}, + {"8", "420"}, + {"9", "480"}, + {"10", "540"}, + {"11", "600"} +}; + +class OnroadScreenBrightnessControl : public ExpandableToggleRow { + Q_OBJECT + +public: + OnroadScreenBrightnessControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + QWidget *parent = nullptr); + void refresh(); + +private: + Params params; + OptionControlSP *onroadScreenOffTimer; + OptionControlSP *onroadScreenBrightness; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc new file mode 100644 index 000000000..b441021ff --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc @@ -0,0 +1,40 @@ +/** + * 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. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h" + +DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) { + main_layout = new QStackedLayout(this); + ListWidgetSP *list = new ListWidgetSP(this, false); + + sunnypilotScreen = new QWidget(this); + QVBoxLayout* vlayout = new QVBoxLayout(sunnypilotScreen); + vlayout->setContentsMargins(50, 20, 50, 20); + + // Onroad Screen Off/Brightness + onroadScreenBrightnessControl = new OnroadScreenBrightnessControl( + "OnroadScreenOffControl", + tr("Driving Screen Off: Non-Critical Events"), + tr("Turn off device screen or reduce brightness after driving starts. " + "It automatically brightens again when screen is touched or a critical event occurs."), + "", + this); + list->addItem(onroadScreenBrightnessControl); + + sunnypilotScroller = new ScrollViewSP(list, this); + vlayout->addWidget(sunnypilotScroller); + main_layout->addWidget(sunnypilotScreen); +} + +void DisplayPanel::showEvent(QShowEvent *event) { + QWidget::showEvent(event); + refresh(); +} + +void DisplayPanel::refresh() { + onroadScreenBrightnessControl->refresh(); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h new file mode 100644 index 000000000..1714c8a43 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h @@ -0,0 +1,28 @@ +/** + * 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. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" + +class DisplayPanel : public QWidget { + Q_OBJECT + +public: + explicit DisplayPanel(QWidget *parent = nullptr); + void showEvent(QShowEvent *event) override; + void refresh(); + +private: + QStackedLayout* main_layout = nullptr; + QWidget* sunnypilotScreen = nullptr; + ScrollViewSP *sunnypilotScroller = nullptr; + Params params; + OnroadScreenBrightnessControl *onroadScreenBrightnessControl = nullptr; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc index 72e7a5c8c..749bc9e3f 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -52,12 +52,16 @@ void MadsSettings::updateToggles(bool _offroad) { ); auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + 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(); - if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) { + if (madsLimitedSettings(CP, CP_SP)) { params.remove("MadsMainCruiseAllowed"); params.putBool("MadsUnifiedEngagementMode", true); params.put("MadsSteeringMode", std::to_string(static_cast(MadsSteeringMode::DISENGAGE))); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h index af624865c..9cf62a0ff 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h @@ -12,7 +12,16 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" -const std::vector mads_limited_settings_brands = {"rivian", "tesla"}; +inline bool madsLimitedSettings(const cereal::CarParams::Reader &CP, const cereal::CarParamsSP::Reader &CP_SP) { + if (CP.getBrand() == "rivian") { + return true; + } + if (CP.getBrand() == "tesla") { + return !(CP_SP.getFlags() & 1); // 1 == TeslaFlagsSP.HAS_VEHICLE_BUS + } + + return false; +} enum class MadsSteeringMode { REMAIN_ACTIVE = 0, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index ae954d753..59636ca5a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -134,12 +134,16 @@ void LateralPanel::updateToggles(bool _offroad) { } auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + 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(); - if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) { + if (madsLimitedSettings(CP, CP_SP)) { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY, MADS_BASE_DESC)); } else { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC)); 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 22cdd8c63..295072d1e 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 eed0306ca..ff5d6221c 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 61c86f920..f76f9bea2 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/settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc index 0f8488712..33cb99acf 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc @@ -13,6 +13,7 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/developer_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h" @@ -88,6 +89,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) { PanelInfo(" " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"), PanelInfo(" " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"), PanelInfo(" " + tr("Visuals"), new VisualsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"), + PanelInfo(" " + tr("Display"), new DisplayPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_display.png"), PanelInfo(" " + tr("OSM"), new OsmPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_map.png"), PanelInfo(" " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"), PanelInfo(" " + tr("Vehicle"), new VehiclePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"), diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index 10ead5377..7c1107d65 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -49,6 +49,26 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { "", false, }, + { + "GreenLightAlert", + tr("Green Traffic Light Alert (Beta)"), + QString("%1
" + "

%2


") + .arg(tr("A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you.")) + .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")), + "", + false, + }, + { + "LeadDepartAlert", + tr("Lead Departure Alert (Beta)"), + QString("%1
" + "

%2


") + .arg(tr("A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving.")) + .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")), + "", + false, + }, }; // Add regular toggles first diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index c07825c97..21d66ffa9 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -11,7 +11,17 @@ #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}); + + int small_max = e2e_alert_small * 2 - 40; + int large_max = e2e_alert_large * 2 - 40; + green_light_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {small_max, small_max}); + green_light_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {large_max, large_max}); + lead_depart_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {small_max, small_max}); + lead_depart_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {large_max, large_max}); +} void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); @@ -36,6 +46,8 @@ void HudRendererSP::updateState(const UIState &s) { speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid(); 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()); @@ -100,10 +112,22 @@ void HudRendererSP::updateState(const UIState &s) { smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive(); smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled(); smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive(); + + greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert(); + leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert(); } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { HudRenderer::draw(p, surface_rect); + + e2eAlertDisplayTimer = std::max(0, e2eAlertDisplayTimer - 1); + + p.save(); + + if (is_cruise_available) { + drawSetSpeedSP(p, surface_rect); + } + if (!reversing) { // Smart Cruise Control int x_offset = -260; @@ -152,14 +176,61 @@ 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); + + // Green Light & Lead Depart Alerts + if (greenLightAlert or leadDepartAlert) { + e2eAlertDisplayTimer = 3 * UI_FREQ; + // reset onroad sleep timer for e2e alerts + uiStateSP()->reset_onroad_sleep_timer(); + } + + if (e2eAlertDisplayTimer > 0) { + e2eAlertFrame++; + if (greenLightAlert) { + alert_text = tr("GREEN\nLIGHT"); + alert_img = devUiInfo > 0 ? green_light_alert_small_img : green_light_alert_large_img; + } + else if (leadDepartAlert) { + alert_text = tr("LEAD VEHICLE\nDEPARTING"); + alert_img = devUiInfo > 0 ? lead_depart_alert_small_img : lead_depart_alert_large_img; + } + drawE2eAlert(p, surface_rect); + } else { + e2eAlertFrame = 0; + } } + + p.restore(); } void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) { @@ -348,10 +419,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 +436,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 +637,97 @@ 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); +} + +void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) { + int size = devUiInfo > 0 ? e2e_alert_small : e2e_alert_large; + int x = surface_rect.center().x() + surface_rect.width() / 4; + int y = surface_rect.center().y() + 40; + x += devUiInfo > 0 ? 0 : 50; + y += devUiInfo > 0 ? 0 : 80; + QRect alertRect(x - size, y - size, size * 2, size * 2); + + // Alert Circle + QPoint center = alertRect.center(); + QColor frameColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 75) : QColor(0, 255, 0, 75); + p.setPen(QPen(frameColor, 15)); + p.setBrush(QColor(0, 0, 0, 190)); + p.drawEllipse(center, size, size); + + // Alert Text + QColor txtColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 255) : QColor(0, 255, 0, 255); + p.setFont(InterFont(48, QFont::Bold)); + p.setPen(txtColor); + QFontMetrics fm(p.font()); + QRect textRect = fm.boundingRect(alertRect, Qt::TextWordWrap, alert_text); + textRect.moveCenter({alertRect.center().x(), alertRect.center().y()}); + textRect.moveBottom(alertRect.bottom() - alertRect.height() / 7); + p.drawText(textRect, Qt::AlignCenter, alert_text); + + // Alert Image + QPointF pixmapCenterOffset = QPointF(alert_img.width() / 2.0, alert_img.height() / 2.0); + QPointF drawPoint = center - pixmapCenterOffset; + p.drawPixmap(drawPoint, alert_img); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index d77892fa2..cf840ee3d 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -31,9 +31,12 @@ 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); + void drawE2eAlert(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -86,4 +89,21 @@ 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; + int e2e_alert_small = 250; + int e2e_alert_large = 300; + QPixmap green_light_alert_small_img; + QPixmap green_light_alert_large_img; + bool greenLightAlert; + int e2eAlertFrame; + int e2eAlertDisplayTimer = 0; + bool leadDepartAlert; + QPixmap lead_depart_alert_small_img; + QPixmap lead_depart_alert_large_img; + QString alert_text; + QPixmap alert_img; }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc b/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc index b26d1b828..3f853e336 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc @@ -25,8 +25,10 @@ void OnroadWindowSP::updateState(const UIStateSP &s) { void OnroadWindowSP::mousePressEvent(QMouseEvent *e) { OnroadWindow::mousePressEvent(e); + uiStateSP()->reset_onroad_sleep_timer(); } void OnroadWindowSP::offroadTransition(bool offroad) { OnroadWindow::offroadTransition(offroad); + uiStateSP()->reset_onroad_sleep_timer(); } diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index fd7365821..7f4fc8823 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -11,6 +11,15 @@ void UIStateSP::updateStatus() { UIState::updateStatus(); + + if (scene.started && scene.onroadScreenOffControl) { + auto selfdriveState = (*sm)["selfdriveState"].getSelfdriveState(); + if (selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE) { + reset_onroad_sleep_timer(); + } else if (scene.onroadScreenOffTimer > 0) { + scene.onroadScreenOffTimer--; + } + } } UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { @@ -55,6 +64,20 @@ void ui_update_params_sp(UIStateSP *s) { s->scene.standstill_timer = params.getBool("StandstillTimer"); s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str()); s->scene.road_name = params.getBool("RoadNameToggle"); + + // Onroad Screen Brightness + s->scene.onroadScreenOffBrightness = std::atoi(params.get("OnroadScreenOffBrightness").c_str()); + s->scene.onroadScreenOffControl = params.getBool("OnroadScreenOffControl"); + s->scene.onroadScreenOffTimerParam = std::atoi(params.get("OnroadScreenOffTimer").c_str()); + s->reset_onroad_sleep_timer(); +} + +void UIStateSP::reset_onroad_sleep_timer() { + if (scene.onroadScreenOffTimerParam >= 0 and scene.onroadScreenOffControl) { + scene.onroadScreenOffTimer = scene.onroadScreenOffTimerParam * UI_FREQ; + } else { + scene.onroadScreenOffTimer = -1; + } } DeviceSP::DeviceSP(QObject *parent) : Device(parent) { diff --git a/selfdrive/ui/sunnypilot/ui.h b/selfdrive/ui/sunnypilot/ui.h index 393f997cb..55dd43ad5 100644 --- a/selfdrive/ui/sunnypilot/ui.h +++ b/selfdrive/ui/sunnypilot/ui.h @@ -61,6 +61,7 @@ public: return user.user_id.toLower() != "unregisteredsponsor" && user.user_id.toLower() != "temporarysponsor"; }); } + void reset_onroad_sleep_timer(); signals: void sunnylinkRoleChanged(bool subscriber); diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index 768cc5d7a..7cfabb15c 100644 --- a/selfdrive/ui/sunnypilot/ui_scene.h +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -12,4 +12,7 @@ typedef struct UISceneSP : UIScene { bool standstill_timer = false; int speed_limit_mode = 0; bool road_name = false; + int onroadScreenOffBrightness, onroadScreenOffTimer = 0; + bool onroadScreenOffControl; + int onroadScreenOffTimerParam; } UISceneSP; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2d40650f8..50ce3e95d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -210,6 +210,13 @@ void Device::updateBrightness(const UIState &s) { brightness = 0; } + // Onroad Brightness Control +#ifdef SUNNYPILOT + if (awake && s.scene.started && s.scene.onroadScreenOffControl && s.scene.onroadScreenOffTimer == 0) { + brightness = s.scene.onroadScreenOffBrightness * 0.01 * brightness; + } +#endif + if (brightness != last_brightness) { if (!brightness_future.isRunning()) { brightness_future = QtConcurrent::run(Hardware::set_brightness, brightness); diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 70b590a8b..f9c4057ba 100644 --- a/sunnypilot/mads/helpers.py +++ b/sunnypilot/mads/helpers.py @@ -9,6 +9,10 @@ from openpilot.common.params import Params from opendbc.car import structs from opendbc.safety import ALTERNATIVE_EXPERIENCE from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP +from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP + + +MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla") class MadsSteeringModeOnBrake: @@ -17,20 +21,25 @@ class MadsSteeringModeOnBrake: DISENGAGE = 2 -def get_mads_limited_brands(CP: structs.CarParams) -> bool: - return CP.brand in ("rivian", "tesla") +def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool: + if CP.brand == 'rivian': + return True + if CP.brand == 'tesla': + return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS + + return False -def read_steering_mode_param(CP: structs.CarParams, params: Params): - if get_mads_limited_brands(CP): +def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params): + if get_mads_limited_brands(CP, CP_SP): return MadsSteeringModeOnBrake.DISENGAGE return params.get("MadsSteeringMode", return_default=True) -def set_alternative_experience(CP: structs.CarParams, params: Params): +def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params): enabled = params.get_bool("Mads") - steering_mode = read_steering_mode_param(CP, params) + steering_mode = read_steering_mode_param(CP, CP_SP, params) if enabled: CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS @@ -53,9 +62,12 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p # MADS Partial Support # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms - # TODO-SP: To enable MADS full support for Rivian/Tesla, identify consistent signals for MADS toggling - mads_partial_support = get_mads_limited_brands(CP) + # TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling + mads_partial_support = get_mads_limited_brands(CP, CP_SP) if mads_partial_support: params.put("MadsSteeringMode", 2) params.put_bool("MadsUnifiedEngagementMode", True) + + # no ACC MAIN button for these brands + if CP.brand in MADS_NO_ACC_MAIN_BUTTON: params.remove("MadsMainCruiseAllowed") diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index f16b68835..21e7f7d3e 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -10,7 +10,7 @@ from cereal import log, custom from opendbc.car import structs from opendbc.car.hyundai.values import HyundaiFlags from openpilot.common.params import Params -from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, get_mads_limited_brands +from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState @@ -27,6 +27,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): self.CP = selfdrive.CP + self.CP_SP = selfdrive.CP_SP self.params = selfdrive.params self.enabled = False @@ -43,14 +44,16 @@ class ModularAssistiveDrivingSystem: if self.CP.brand == "hyundai": if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True + if self.CP.brand == "tesla": + self.allow_always = True - if get_mads_limited_brands(self.CP): + if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON: self.no_main_cruise = True # read params on init self.enabled_toggle = self.params.get_bool("Mads") self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") - self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params) + self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params) self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") def read_params(self): @@ -102,12 +105,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/images/green_light.png b/sunnypilot/selfdrive/assets/images/green_light.png new file mode 100644 index 000000000..2da2c13a8 --- /dev/null +++ b/sunnypilot/selfdrive/assets/images/green_light.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3aa5ec9ac1daee6a549e62647d90bcaa66d2485f7df7f386ff902fcfb04c1716 +size 6583 diff --git a/sunnypilot/selfdrive/assets/images/lead_depart.png b/sunnypilot/selfdrive/assets/images/lead_depart.png new file mode 100644 index 000000000..6030ee67c --- /dev/null +++ b/sunnypilot/selfdrive/assets/images/lead_depart.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:087db35bd469e85aefe3b45636f11ab3e8b55ceb7bc94ea059cfd9a69c2f338f +size 8914 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 000000000..250f64058 --- /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 000000000..14c1529da --- /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/assets/offroad/icon_display.png b/sunnypilot/selfdrive/assets/offroad/icon_display.png new file mode 100644 index 000000000..547977f52 --- /dev/null +++ b/sunnypilot/selfdrive/assets/offroad/icon_display.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:614767308d435d165a9ab78e3eac22ee15697d94066df8200ef32afb33ef8d60 +size 5698 diff --git a/sunnypilot/selfdrive/car/car_specific.py b/sunnypilot/selfdrive/car/car_specific.py index 7e6f300d7..97ce26429 100644 --- a/sunnypilot/selfdrive/car/car_specific.py +++ b/sunnypilot/selfdrive/car/car_specific.py @@ -18,9 +18,9 @@ GearShifter = structs.CarState.GearShifter class CarSpecificEventsSP: - def __init__(self, CP: structs.CarParams, params): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP): self.CP = CP - self.params = params + self.CP_SP = CP_SP self.low_speed_alert = False @@ -42,4 +42,10 @@ class CarSpecificEventsSP: if self.low_speed_alert: events.add(EventName.belowSteerSpeed) + elif self.CP.brand == 'toyota': + if self.CP.openpilotLongitudinalControl: + if CS.cruiseState.standstill and not CS.brakePressed and self.CP_SP.enableGasInterceptor: + if events.has(EventName.resumeRequired): + events.remove(EventName.resumeRequired) + return events_sp diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index 716e3e1c9..3691c3597 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/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py new file mode 100644 index 000000000..eedf8bccb --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -0,0 +1,58 @@ +""" +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 messaging, custom + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP + +TRIGGER_THRESHOLD = 30 + + +class E2EAlertsHelper: + def __init__(self): + self._params = Params() + self._frame = -1 + + self.green_light_alert = False + self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + self.lead_depart_alert = False + self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") + + def _read_params(self) -> None: + if self._frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") + + self._frame += 1 + + def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None: + self._read_params() + + if not (self.green_light_alert_enabled or self.lead_depart_alert_enabled): + return + + CS = sm['carState'] + CC = sm['carControl'] + + model_x = sm['modelV2'].position.x + max_idx = len(model_x) - 1 + has_lead = sm['radarState'].leadOne.status + lead_vRel: float = sm['radarState'].leadOne.vRel + + # Green light alert + self.green_light_alert = (self.green_light_alert_enabled and model_x[max_idx] > TRIGGER_THRESHOLD + and not has_lead and CS.standstill and not CS.gasPressed and not CC.enabled) + + # Lead Departure Alert + self.lead_depart_alert = (self.lead_depart_alert_enabled and CS.standstill and model_x[max_idx] > 30 + and has_lead and lead_vRel > 1 and not CS.gasPressed) + + if self.green_light_alert or self.green_light_alert: + events_sp.add(custom.OnroadEventSP.EventName.e2eChime) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 40d30100c..4c2443f25 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,22 +7,31 @@ 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.e2e_alerts_helper import E2EAlertsHelper 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.e2e_alerts_helper = E2EAlertsHelper() self.output_v_target = 0. self.output_a_target = 0. @@ -39,15 +48,29 @@ 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 + + # 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]) @@ -55,7 +78,9 @@ class LongitudinalPlannerSP: return self.output_v_target, self.output_a_target def update(self, sm: messaging.SubMaster) -> None: + self.events_sp.clear() self.dec.update(sm) + self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') @@ -66,6 +91,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 +130,16 @@ 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) + + # E2E Alerts + e2eAlerts = longitudinalPlanSP.e2eAlerts + e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert + e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert 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 0f6062165..4ca45202f 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 af14a87f0..22ea75fe1 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 7d219ac3b..baf932803 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 000000000..40e8f653f --- /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 000000000..5d798949a --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -0,0 +1,395 @@ +""" +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.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) + + # 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) + + # 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.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/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py new file mode 100644 index 000000000..3b2716265 --- /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 7c723397f..e9f4b29ba 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,41 @@ 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.), + }, + + EventNameSP.e2eChime: { + ET.PERMANENT: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 0.1), + }, }