Merge branch 'min-feat/lat/alka' into full

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 {

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;

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

common/params_keys.h

@@ -125,4 +125,5 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"dp_device_is_rhd", PERSISTENT},
     {"dp_device_monitoring_disabled", PERSISTENT},
     {"dp_device_beep", PERSISTENT},
+    {"dp_lat_alka", PERSISTENT},
 };

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:

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

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

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.",

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

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);
   }
 

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;

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

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

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

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'])
 
@@ -113,7 +115,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

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
 

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;

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;

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) {
@@ -100,6 +101,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"));

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 {
@@ -61,6 +63,7 @@ typedef struct UIScene {
   bool started, ignition, is_metric;
   uint64_t started_frame;
   bool disable_driver = false;
+  bool alka_active = false;
 } UIScene;
 
 class UIState : public QObject {

system/manager/manager.py

@@ -43,6 +43,7 @@ def manager_init() -> None:
     ("dp_device_is_rhd", "0"),
     ("dp_device_monitoring_disabled", "0"),
     ("dp_device_beep", "0"),
+    ("dp_lat_alka", "0"),
   ]
 
   if params.get_bool("RecordFrontLock"):