try MADS

opendbc/safety/__init__.py

@@ -8,3 +8,4 @@ class ALTERNATIVE_EXPERIENCE:
   DISABLE_STOCK_AEB = 2
   RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
   ALLOW_AEB = 16
+  ENABLE_MADS = 1024

opendbc/safety/declarations.h

@@ -67,6 +67,9 @@ extern const int MAX_WRONG_COUNTERS;
 #define VEHICLE_SPEED_FACTOR 1000.0
 #define MAX_RT_INTERVAL 250000U
 
+// MADS keeps lateral authorization separate from longitudinal controls_allowed.
+#define ALT_EXP_ENABLE_MADS 1024
+
 // Conversions
 #define KPH_TO_MS (1.0 / 3.6)
 
@@ -322,6 +325,7 @@ extern CurvatureSteeringState curvature_state;
 #define ALT_EXP_ALLOW_AEB 16
 
 extern int alternative_experience;
+extern bool controls_allowed_lateral;
 
 // time since safety mode has been changed
 extern uint32_t safety_mode_cnt;

opendbc/safety/lateral.h

@@ -61,7 +61,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
   bool violation = false;
   uint32_t ts = microsecond_timer_get();
 
-  if (controls_allowed) {
+  if (controls_allowed || controls_allowed_lateral) {
     // Some safety models support variable torque limit based on vehicle speed
     int max_torque = limits.max_torque;
     if (limits.dynamic_max_torque) {
@@ -96,7 +96,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 || controls_allowed_lateral) && (desired_torque != 0)) {
     violation = true;
   }
 
@@ -138,7 +138,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 || controls_allowed_lateral)) {
     valid_steer_req_count = 0;
     invalid_steer_req_count = 0;
     desired_torque_last = 0;
@@ -198,7 +198,7 @@ static bool rt_curvature_rate_limit_check(CurvatureSteeringLimits 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) {
+  if ((controls_allowed || controls_allowed_lateral) && 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
@@ -225,12 +225,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 || controls_allowed_lateral)) {
     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 || controls_allowed_lateral)) {
     desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
   }
 
@@ -249,7 +249,7 @@ bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool ste
 
   speed_mismatch_check((float)vehicle_speed_2.values[0] / VEHICLE_SPEED_FACTOR);
 
-  if (controls_allowed && steer_control_enabled) {
+  if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
     // *** absolute curvature cap ***
     violation |= safety_max_limit_check(desired_curvature, limits.max_curvature, -limits.max_curvature);
 
@@ -289,15 +289,15 @@ bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool ste
   if (limits.max_steer_power != 0) {
     violation |= safety_max_limit_check(steer_power, limits.max_steer_power, 0);
     violation |= (steer_power != 0) && !steer_control_enabled;
-    violation |= !controls_allowed && (steer_power != 0) && (steer_power >= curvature_state.steer_power_last);
+    violation |= !(controls_allowed || controls_allowed_lateral) && (steer_power != 0) && (steer_power >= curvature_state.steer_power_last);
     curvature_state.steer_power_last = steer_power;
   } else {
     // No curvature control allowed when controls are not allowed
-    violation |= !controls_allowed && steer_control_enabled;
+    violation |= !(controls_allowed || controls_allowed_lateral) && steer_control_enabled;
   }
 
   // reset to zero or measured curvature depending on EPS expectation
-  if (violation || !controls_allowed) {
+  if (violation || !(controls_allowed || controls_allowed_lateral)) {
     curvature_state.desired_last = limits.inactive_curvature_is_zero ? 0 : curvature_state.meas.values[0];
   }
 
@@ -330,7 +330,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
 
   bool violation = false;
 
-  if (controls_allowed && steer_control_enabled) {
+  if ((controls_allowed || controls_allowed_lateral) && 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);
@@ -366,12 +366,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 || controls_allowed_lateral)) {
     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 || controls_allowed_lateral)) {
     desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
   }
 

opendbc/safety/modes/hyundai.h

@@ -49,6 +49,9 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
 #define HYUNDAI_SCC12_ADDR_CHECK(scc_bus)                                                                            \
   {.msg = {{0x421, (scc_bus), 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
 
+#define HYUNDAI_SCC11_ADDR_CHECK(scc_bus) \
+  {.msg = {{0x420, (scc_bus), 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
+
 #define HYUNDAI_FCEV_GAS_ADDR_CHECK \
   {.msg = {{0x91,  0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
 
@@ -57,6 +60,9 @@ static const CanMsg HYUNDAI_TX_MSGS[] = {
 };
 
 static bool hyundai_legacy = false;
+static uint32_t hyundai_mads_main_ts = 0U;
+static bool hyundai_mads_main_button_prev = false;
+static const uint32_t HYUNDAI_MADS_MAIN_TIMEOUT = 100000U;
 
 static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
 
@@ -69,6 +75,8 @@ static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
     cnt = (msg->data[1] >> 5) & 0x7U;
   } else if (msg->addr == 0x421U) {
     cnt = msg->data[7] & 0xFU;
+  } else if (msg->addr == 0x420U) {
+    cnt = (msg->data[0] >> 4) & 0xFU;
   } else if (msg->addr == 0x4F1U) {
     cnt = (msg->data[3] >> 4) & 0xFU;
   } else {
@@ -128,6 +136,15 @@ static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
 
 static void hyundai_rx_hook(const CANPacket_t *msg) {
 
+  // MainMode_ACC is the physical ACC main switch. With MADS it is the
+  // independent authorization source for lateral control.
+  if (msg->addr == 0x420U) {
+    if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
+      acc_main_on = GET_BIT(msg, 0U);
+      hyundai_mads_main_ts = microsecond_timer_get();
+    }
+  }
+
   // SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
   if (msg->addr == 0x421U) {
     if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
@@ -148,6 +165,15 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
     if (msg->addr == 0x4F1U) {
       int cruise_button = msg->data[0] & 0x7U;
       bool main_button = GET_BIT(msg, 3U);
+      if (hyundai_longitudinal && ((alternative_experience & ALT_EXP_ENABLE_MADS) != 0)) {
+        if (main_button && !hyundai_mads_main_button_prev) {
+          acc_main_on = !acc_main_on;
+          if (!acc_main_on) {
+            controls_allowed = false;
+          }
+        }
+        hyundai_mads_main_button_prev = main_button;
+      }
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
     }
 
@@ -174,6 +200,11 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
       brake_pressed = ((msg->data[5] >> 5U) & 0x3U) == 0x2U;
     }
   }
+
+  const bool mads_requested = ((alternative_experience & ALT_EXP_ENABLE_MADS) != 0) && acc_main_on;
+  const bool mads_main_fresh = hyundai_longitudinal ||
+                               (safety_get_ts_elapsed(microsecond_timer_get(), hyundai_mads_main_ts) <= HYUNDAI_MADS_MAIN_TIMEOUT);
+  controls_allowed_lateral = mads_requested && mads_main_fresh && heartbeat_engaged && !safety_rx_checks_invalid;
 }
 
 static bool hyundai_tx_hook(const CANPacket_t *msg) {
@@ -183,6 +214,11 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
 
   bool tx = true;
 
+  if (!hyundai_longitudinal && ((alternative_experience & ALT_EXP_ENABLE_MADS) != 0) && controls_allowed_lateral &&
+      (safety_get_ts_elapsed(microsecond_timer_get(), hyundai_mads_main_ts) > HYUNDAI_MADS_MAIN_TIMEOUT)) {
+    controls_allowed_lateral = false;
+  }
+
   // FCA11: Block any potential actuation
   if (msg->addr == 0x38DU) {
     int CR_VSM_DecCmd = msg->data[1];
@@ -266,6 +302,8 @@ static safety_config hyundai_init(uint16_t param) {
 
   hyundai_common_init(param);
   hyundai_legacy = false;
+  hyundai_mads_main_ts = 0U;
+  hyundai_mads_main_button_prev = false;
 
   safety_config ret;
   if (hyundai_longitudinal) {
@@ -303,6 +341,12 @@ static safety_config hyundai_init(uint16_t param) {
        HYUNDAI_SCC12_ADDR_CHECK(0)
     };
 
+    static RxCheck hyundai_mads_rx_checks[] = {
+       HYUNDAI_COMMON_RX_CHECKS(false)
+       HYUNDAI_SCC12_ADDR_CHECK(0)
+       HYUNDAI_SCC11_ADDR_CHECK(0)
+    };
+
     static RxCheck hyundai_fcev_rx_checks[] = {
       HYUNDAI_COMMON_RX_CHECKS(false)
       HYUNDAI_SCC12_ADDR_CHECK(0)
@@ -312,6 +356,8 @@ static safety_config hyundai_init(uint16_t param) {
     SET_TX_MSGS(HYUNDAI_TX_MSGS, ret);
     if (hyundai_fcev_gas_signal) {
       SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
+    } else if ((alternative_experience & ALT_EXP_ENABLE_MADS) != 0) {
+      SET_RX_CHECKS(hyundai_mads_rx_checks, ret);
     } else {
       SET_RX_CHECKS(hyundai_rx_checks, ret);
     }

opendbc/safety/modes/hyundai_common.h

@@ -99,7 +99,8 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
     // enter controls on falling edge of resume or set
     bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET);
     bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME);
-    if (set || res) {
+    const bool mads_longitudinal_allowed = ((alternative_experience & ALT_EXP_ENABLE_MADS) == 0) || acc_main_on;
+    if ((set || res) && mads_longitudinal_allowed) {
       controls_allowed = true;
     }
 

opendbc/safety/safety.h

@@ -43,6 +43,7 @@ const int MAX_WRONG_COUNTERS = 5;
 
 // This can be set by the safety hooks
 bool controls_allowed = false;
+bool controls_allowed_lateral = false;
 bool relay_malfunction = false;
 bool gas_pressed = false;
 bool gas_pressed_prev = false;
@@ -103,6 +104,8 @@ static bool is_msg_valid(RxCheck addr_list[], int index) {
     if (!addr_list[index].status.valid_checksum || !addr_list[index].status.valid_quality_flag || (addr_list[index].status.wrong_counters >= MAX_WRONG_COUNTERS)) {
       valid = false;
       controls_allowed = false;
+      controls_allowed_lateral = false;
+      safety_rx_checks_invalid = true;
     }
   }
   return valid;
@@ -330,13 +333,15 @@ void safety_tick(const safety_config *cfg) {
       cfg->rx_checks[i].status.lagging = lagging;
       if (lagging) {
         controls_allowed = false;
+        controls_allowed_lateral = false;
       }
 
       // enforce minimum frequency for safety-relevant messages
       bool frequency_invalid = frequency < 10U;
       if (lagging || frequency_invalid || !is_msg_valid(cfg->rx_checks, i)) {
         rx_checks_invalid = true;
         controls_allowed = false;
+        controls_allowed_lateral = false;
       }
     }
   }
@@ -463,8 +468,11 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   reset_sample(&curvature_state.meas);
 
   controls_allowed = false;
+  controls_allowed_lateral = false;
   relay_malfunction_reset();
-  safety_rx_checks_invalid = false;
+  // Lateral-only authorization must remain fail-closed until safety_tick has
+  // verified that the complete configured RX set is present and valid.
+  safety_rx_checks_invalid = true;
 
   current_safety_config.rx_checks = NULL;
   current_safety_config.rx_checks_len = 0;

opendbc/safety/tests/libsafety/libsafety_py.py

@@ -63,6 +63,9 @@ class CANPacket:
 
 void set_controls_allowed(bool c);
 bool get_controls_allowed(void);
+void set_controls_allowed_lateral(bool c);
+bool get_controls_allowed_lateral(void);
+void set_heartbeat_engaged(bool engaged);
 bool get_longitudinal_allowed(void);
 void set_alternative_experience(int mode);
 int get_alternative_experience(void);

opendbc/safety/tests/libsafety/safety.c

@@ -38,6 +38,10 @@ void set_controls_allowed(bool c){
   controls_allowed = c;
 }
 
+void set_controls_allowed_lateral(bool c){
+  controls_allowed_lateral = c;
+}
+
 void set_alternative_experience(int mode){
   alternative_experience = mode;
 }
@@ -54,6 +58,14 @@ bool get_controls_allowed(void){
   return controls_allowed;
 }
 
+bool get_controls_allowed_lateral(void){
+  return controls_allowed_lateral;
+}
+
+void set_heartbeat_engaged(bool engaged){
+  heartbeat_engaged = engaged;
+}
+
 bool get_ignition_can(void){
   return ignition_can;
 }