From 487b6e5f21c388ca749f01352d3a8b2768381d67 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 31 Mar 2025 11:59:01 +0800 Subject: [PATCH] alka - 2025-06-17 --- cereal/custom.capnp | 3 ++- cereal/log.capnp | 2 +- cereal/services.py | 1 + common/params_keys.h | 1 + .../opendbc/car/nissan/carcontroller.py | 2 +- opendbc_repo/opendbc/car/structs.py | 1 + .../opendbc/car/toyota/carcontroller.py | 2 +- opendbc_repo/opendbc/car/toyota/values.py | 2 ++ opendbc_repo/opendbc/safety/__init__.py | 2 ++ opendbc_repo/opendbc/safety/lateral.h | 24 ++++++++++++------- opendbc_repo/opendbc/safety/modes/honda.h | 3 ++- .../opendbc/safety/safety_declarations.h | 2 ++ selfdrive/car/card.py | 7 ++++++ selfdrive/controls/controlsd.py | 16 +++++++++++-- selfdrive/selfdrived/selfdrived.py | 6 +++-- selfdrive/selfdrived/state.py | 5 ++-- selfdrive/ui/qt/offroad/dp_panel.cc | 5 ++++ selfdrive/ui/qt/onroad/onroad_home.cc | 2 +- selfdrive/ui/ui.cc | 2 ++ selfdrive/ui/ui.h | 3 +++ system/manager/manager.py | 1 + 21 files changed, 71 insertions(+), 21 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3348e859e..988938bff 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -10,7 +10,8 @@ $Cxx.namespace("cereal"); # DO rename the structs # DON'T change the identifier (e.g. @0x81c2f05a394cf4af) -struct CustomReserved0 @0x81c2f05a394cf4af { +struct DpControlsState @0x81c2f05a394cf4af { + alkaActive @0 :Bool; } struct CustomReserved1 @0xaedffd8f31e7b55d { diff --git a/cereal/log.capnp b/cereal/log.capnp index 9ab51e0b7..85f6fe89c 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2602,7 +2602,7 @@ struct Event { # DO change the name of the field and struct # DON'T change the ID (e.g. @107) # DON'T change which struct it points to - customReserved0 @107 :Custom.CustomReserved0; + dpControlsState @107 :Custom.DpControlsState; customReserved1 @108 :Custom.CustomReserved1; customReserved2 @109 :Custom.CustomReserved2; customReserved3 @110 :Custom.CustomReserved3; diff --git a/cereal/services.py b/cereal/services.py index 82fc04bd0..6aa625905 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -92,6 +92,7 @@ _services: dict[str, tuple] = { "customReservedRawData0": (True, 0.), "customReservedRawData1": (True, 0.), "customReservedRawData2": (True, 0.), + "dpControlsState": (False, 100., 10), } SERVICE_LIST = {name: Service(*vals) for idx, (name, vals) in enumerate(_services.items())} diff --git a/common/params_keys.h b/common/params_keys.h index 92853f92f..2e6151b2b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -122,4 +122,5 @@ inline static std::unordered_map keys = { {"Version", PERSISTENT}, {"dp_device_last_log", CLEAR_ON_ONROAD_TRANSITION}, {"dp_device_reset_conf", CLEAR_ON_MANAGER_START}, + {"dp_lat_alka", PERSISTENT}, }; diff --git a/opendbc_repo/opendbc/car/nissan/carcontroller.py b/opendbc_repo/opendbc/car/nissan/carcontroller.py index 74c6041b4..17d6f427a 100644 --- a/opendbc_repo/opendbc/car/nissan/carcontroller.py +++ b/opendbc_repo/opendbc/car/nissan/carcontroller.py @@ -60,7 +60,7 @@ class CarController(CarControllerBase): # Below are the HUD messages. We copy the stock message and modify if self.CP.carFingerprint != CAR.NISSAN_ALTIMA: if self.frame % 2 == 0: - can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, + can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.latActive, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) if self.frame % 50 == 0: diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index 0959d5c75..80156fdf7 100644 --- a/opendbc_repo/opendbc/car/structs.py +++ b/opendbc_repo/opendbc/car/structs.py @@ -20,4 +20,5 @@ CarControlT = capnp.lib.capnp._StructModule CarParamsT = capnp.lib.capnp._StructModule class DPFlags: + LateralALKA = 1 pass diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index cb6ef47d2..f7e81adab 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -266,7 +266,7 @@ class CarController(CarControllerBase): if self.frame % 20 == 0 or send_ui: can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, - hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) + hud_control.rightLaneDepart, lat_active, CS.lkas_hud)) if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value): can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert)) diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 59dee497a..f10f7d0ce 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -76,6 +76,8 @@ class ToyotaFlags(IntFlag): RAISED_ACCEL_LIMIT = 1024 SECOC = 2048 + ALKA = 2 ** 12 + class Footnote(Enum): CAMRY = CarFootnote( "openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.", diff --git a/opendbc_repo/opendbc/safety/__init__.py b/opendbc_repo/opendbc/safety/__init__.py index 4ce0cd5f0..2e2933ac2 100644 --- a/opendbc_repo/opendbc/safety/__init__.py +++ b/opendbc_repo/opendbc/safety/__init__.py @@ -8,3 +8,5 @@ class ALTERNATIVE_EXPERIENCE: DISABLE_STOCK_AEB = 2 RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 ALLOW_AEB = 16 + + ALKA = 2 ** 10 diff --git a/opendbc_repo/opendbc/safety/lateral.h b/opendbc_repo/opendbc/safety/lateral.h index f842eaa47..86122cf55 100644 --- a/opendbc_repo/opendbc/safety/lateral.h +++ b/opendbc_repo/opendbc/safety/lateral.h @@ -60,8 +60,9 @@ static bool rt_torque_rate_limit_check(int val, int val_last, const int MAX_RT_D bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueSteeringLimits limits) { bool violation = false; uint32_t ts = microsecond_timer_get(); + bool alka = (alternative_experience & ALT_EXP_ALKA) != 0; - if (controls_allowed) { + if (controls_allowed || alka) { // Some safety models support variable torque limit based on vehicle speed int max_torque = limits.max_torque; if (limits.dynamic_max_torque) { @@ -96,7 +97,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee } // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { + if (!(controls_allowed || alka) && (desired_torque != 0)) { violation = true; } @@ -138,7 +139,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee } // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(controls_allowed || alka)) { valid_steer_req_count = 0; invalid_steer_req_count = 0; desired_torque_last = 0; @@ -176,7 +177,9 @@ static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) { bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits) { bool violation = false; - if (controls_allowed && steer_control_enabled) { + bool alka = (alternative_experience & ALT_EXP_ALKA) != 0; + + if ((controls_allowed || alka) && steer_control_enabled) { // convert floating point angle rate limits to integers in the scale of the desired angle on CAN, // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are // always slightly above openpilot's in case we read an updated speed in between angle commands @@ -262,12 +265,12 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const } // No angle control allowed when controls are not allowed - if (!controls_allowed) { + if (!(controls_allowed || alka)) { violation |= steer_control_enabled; } // reset to current angle if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(controls_allowed || alka)) { if (limits.inactive_angle_is_zero) { desired_angle_last = 0; } else { @@ -304,7 +307,10 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co bool violation = false; - if (controls_allowed && steer_control_enabled) { + bool alka = (alternative_experience & ALT_EXP_ALKA) != 0; + + + if ((controls_allowed || alka) && steer_control_enabled) { // *** ISO lateral jerk limit *** // calculate maximum angle rate per second const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed); @@ -340,12 +346,12 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co } // No angle control allowed when controls are not allowed - if (!controls_allowed) { + if (!(controls_allowed || alka)) { violation |= steer_control_enabled; } // reset to current angle if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(controls_allowed || alka)) { desired_angle_last = CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle); } diff --git a/opendbc_repo/opendbc/safety/modes/honda.h b/opendbc_repo/opendbc/safety/modes/honda.h index a702b2008..ee80c1370 100644 --- a/opendbc_repo/opendbc/safety/modes/honda.h +++ b/opendbc_repo/opendbc/safety/modes/honda.h @@ -242,8 +242,9 @@ static bool honda_tx_hook(const CANPacket_t *to_send) { } // STEER: safety check + bool alka = (alternative_experience & ALT_EXP_ALKA) != 0; if ((addr == 0xE4) || (addr == 0x194)) { - if (!controls_allowed) { + if (!(controls_allowed || alka)) { bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1); if (steer_applied) { tx = false; diff --git a/opendbc_repo/opendbc/safety/safety_declarations.h b/opendbc_repo/opendbc/safety/safety_declarations.h index b703b7196..b6db2c399 100644 --- a/opendbc_repo/opendbc/safety/safety_declarations.h +++ b/opendbc_repo/opendbc/safety/safety_declarations.h @@ -303,6 +303,8 @@ extern struct sample_t angle_meas; // last 6 steer angles/curvatures // This flag allows AEB to be commanded from openpilot. #define ALT_EXP_ALLOW_AEB 16 +#define ALT_EXP_ALKA 1024 + extern int alternative_experience; // time since safety mode has been changed diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 732043c59..519e94a81 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -20,6 +20,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState +from opendbc.safety import ALTERNATIVE_EXPERIENCE REPLAY = "REPLAY" in os.environ @@ -101,6 +102,9 @@ class Car: with car.CarParams.from_bytes(cached_params_raw) as _cached_params: cached_params = _cached_params + if self.params.get_bool("dp_lat_alka"): + dp_params |= structs.DPFlags.LateralALKA + self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params) self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP) self.CP = self.CI.CP @@ -112,6 +116,9 @@ class Car: self.RI = RI self.CP.alternativeExperience = 0 + if dp_params & structs.DPFlags.LateralALKA: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALKA + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly self.CP.passive = not controller_available or self.CP.dashcamOnly diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5ecff1ccd..f00d03647 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,7 +38,7 @@ class Controls: self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') - self.pm = messaging.PubMaster(['carControl', 'controlsState']) + self.pm = messaging.PubMaster(['carControl', 'controlsState', 'dpControlsState']) self.steer_limited_by_controls = False self.curvature = 0.0 @@ -57,6 +57,9 @@ class Controls: elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI) + self.alka_enabled = self.params.get_bool("dp_lat_alka") + self.alka_active = False + def update(self): self.sm.update(15) if self.sm.updated["liveCalibration"]: @@ -92,7 +95,9 @@ class Controls: # Check which actuators can be enabled standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill - CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + self.alka_active = self.alka_enabled and CS.cruiseState.available and not standstill and CS.gearShifter != car.CarState.GearShifter.reverse + lat_active = self.sm['selfdriveState'].active or self.alka_active + CC.latActive = lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.CP.steerAtStandstill) CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl @@ -178,6 +183,13 @@ class Controls: # TODO: both controlsState and carControl valids should be set by # sm.all_checks(), but this creates a circular dependency + # dpControlsState + dat = messaging.new_message('dpControlsState') + dat.valid = True + ncs = dat.dpControlsState + ncs.alkaActive = self.alka_active + self.pm.send('dpControlsState', dat) + # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 3065f6b80..1282821fe 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -21,6 +21,7 @@ from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroa from openpilot.system.hardware import HARDWARE from openpilot.system.version import get_build_metadata +from opendbc.safety import ALTERNATIVE_EXPERIENCE REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -38,7 +39,6 @@ SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) - class SelfdriveD: def __init__(self, CP=None): self.params = Params() @@ -55,6 +55,8 @@ class SelfdriveD: self.car_events = CarSpecificEvents(self.CP) + self.alka = bool(self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALKA) + # Setup sockets self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) @@ -111,7 +113,7 @@ class SelfdriveD: self.experimental_mode = False self.personality = self.read_personality_param() self.recalibrating_seen = False - self.state_machine = StateMachine() + self.state_machine = StateMachine(self.alka) self.rk = Ratekeeper(100, print_delay_threshold=None) # some comma three with NVMe experience NVMe dropouts mid-drive that diff --git a/selfdrive/selfdrived/state.py b/selfdrive/selfdrived/state.py index 073ddb56e..940fcb029 100644 --- a/selfdrive/selfdrived/state.py +++ b/selfdrive/selfdrived/state.py @@ -9,10 +9,11 @@ ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) class StateMachine: - def __init__(self): + def __init__(self, alka = False): self.current_alert_types = [ET.PERMANENT] self.state = State.disabled self.soft_disable_timer = 0 + self.alka = alka def update(self, events: Events): # decrement the soft disable timer at every step, as it's reset on @@ -92,7 +93,7 @@ class StateMachine: # Check if openpilot is engaged and actuators are enabled enabled = self.state in ENABLED_STATES active = self.state in ACTIVE_STATES - if active: + if active or self.alka: self.current_alert_types.append(ET.WARNING) return enabled, active diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index 548b02be5..e4fbb7b59 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/selfdrive/ui/qt/offroad/dp_panel.cc @@ -106,6 +106,11 @@ void DPPanel::add_lateral_toggles() { QString::fromUtf8("🐉 ") + tr("Lateral Ctrl"), "", }, + { + "dp_lat_alka", + tr("Always-on Lane Keeping Assist (ALKA)"), + "", + }, }; QWidget *label = nullptr; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 080f9bd50..88fb5ce8d 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -47,7 +47,7 @@ void OnroadWindow::updateState(const UIState &s) { alerts->updateState(s); nvg->updateState(s); - QColor bgColor = bg_colors[s.status]; + QColor bgColor = bg_colors[s.scene.alka_active && s.status == STATUS_DISENGAGED? STATUS_ALKA : s.status]; if (bg != bgColor) { // repaint border bg = bgColor; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 79a245a0e..182cf869b 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -60,6 +60,7 @@ static void update_state(UIState *s) { scene.light_sensor = -1; } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; + scene.alka_active = sm["dpControlsState"].getDpControlsState().getAlkaActive(); } void ui_update_params(UIState *s) { @@ -99,6 +100,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", + "dpControlsState", }); prime_state = new PrimeState(this); language = QString::fromStdString(Params().get("LanguageSetting")); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index fd2aee771..702af710c 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -42,12 +42,14 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + STATUS_ALKA, } UIStatus; const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + [STATUS_ALKA] = QColor(0x22, 0xa0, 0xdc, 0xf1), }; typedef struct UIScene { @@ -60,6 +62,7 @@ typedef struct UIScene { float light_sensor = -1; bool started, ignition, is_metric; uint64_t started_frame; + bool alka_active = false; } UIScene; class UIState : public QObject { diff --git a/system/manager/manager.py b/system/manager/manager.py index 5dc08c6d3..f3c3bf7b4 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -40,6 +40,7 @@ def manager_init() -> None: ("OpenpilotEnabledToggle", "1"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), ("DisableLogging", "0"), + ("dp_lat_alka", "0"), ] if params.get_bool("RecordFrontLock"):