sunnypilot · sunnyhaibin · Dec 6, 2024 · Sep 18, 2024 · Sep 19, 2024 · Sep 29, 2024
diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml
@@ -128,7 +128,7 @@ jobs:
       - name: Build Docker image
         run: eval "$BUILD"
       - name: Mutation tests
-        timeout-minutes: 5
+        timeout-minutes: 8
         run: ${{ env.RUN }} "GIT_REF=${{ github.event_name == 'push' && (github.ref == 'refs/heads/master' || github.ref == 'refs/heads/master-new') && github.event.before || 'origin/master' }} cd tests/safety && ./mutation.sh"
 
   static_analysis:

diff --git a/board/health.h b/board/health.h
@@ -13,6 +13,7 @@ struct __attribute__((packed)) health_t {
   uint8_t ignition_line_pkt;
   uint8_t ignition_can_pkt;
   uint8_t controls_allowed_pkt;
+  uint8_t controls_allowed_lat_pkt;
   uint8_t car_harness_status_pkt;
   uint8_t safety_mode_pkt;
   uint16_t safety_param_pkt;

diff --git a/board/main_comms.h b/board/main_comms.h
@@ -17,6 +17,7 @@ static int get_health_pkt(void *dat) {
   health->ignition_can_pkt = ignition_can;
 
   health->controls_allowed_pkt = controls_allowed;
+  health->controls_allowed_lat_pkt = mads_is_lateral_control_allowed_by_mads();
   health->safety_tx_blocked_pkt = safety_tx_blocked;
   health->safety_rx_invalid_pkt = safety_rx_invalid;
   health->tx_buffer_overflow_pkt = tx_buffer_overflow;
@@ -247,6 +248,9 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
       // you can only set this if you are in a non car safety mode
       if (!is_car_safety_mode(current_safety_mode)) {
         alternative_experience = req->param1;
+        bool mads_enabled = (alternative_experience & ALT_EXP_ENABLE_MADS) != 0;
+        bool disengage_lateral_on_brake = !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE);
+        mads_set_system_state(mads_enabled, disengage_lateral_on_brake);
       }
       break;
     // **** 0xe0: uart read

diff --git a/board/safety.h b/board/safety.h
@@ -3,6 +3,8 @@
 #include "safety_declarations.h"
 #include "can.h"
 
+#include "sunnypilot/safety_mads.h"
+
 // include the safety policies.
 #include "safety/safety_defaults.h"
 #include "safety/safety_honda.h"
@@ -111,6 +113,10 @@ uint16_t current_safety_param = 0;
 static const safety_hooks *current_hooks = &nooutput_hooks;
 safety_config current_safety_config;
 
+static bool is_lat_active(void) {
+  return controls_allowed || mads_is_lateral_control_allowed_by_mads();
+}
+
 static bool is_msg_valid(RxCheck addr_list[], int index) {
   bool valid = true;
   if (index != -1) {
@@ -301,6 +307,7 @@ void safety_tick(const safety_config *cfg) {
       cfg->rx_checks[i].status.lagging = lagging;
       if (lagging) {
         controls_allowed = false;
+        mads_exit_controls();
       }
 
       if (lagging || !is_msg_valid(cfg->rx_checks, i)) {
@@ -343,6 +350,7 @@ void generic_rx_checks(bool stock_ecu_detected) {
   if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) {
     relay_malfunction_set();
   }
+  mads_state_update(&vehicle_moving, &acc_main_on, brake_pressed || regen_braking, cruise_engaged_prev);
 }
 
 static void relay_malfunction_reset(void) {
@@ -601,7 +609,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   bool violation = false;
   uint32_t ts = microsecond_timer_get();
 
-  if (controls_allowed) {
+  if (is_lat_active()) {
     // *** global torque limit check ***
     violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);
 
@@ -628,7 +636,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   }
 
   // no torque if controls is not allowed
-  if (!controls_allowed && (desired_torque != 0)) {
+  if (!is_lat_active() && (desired_torque != 0)) {
     violation = true;
   }
 
@@ -670,7 +678,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   }
 
   // reset to 0 if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !is_lat_active()) {
     valid_steer_req_count = 0;
     invalid_steer_req_count = 0;
     desired_torque_last = 0;
@@ -686,7 +694,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
 bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) {
   bool violation = false;
 
-  if (controls_allowed && steer_control_enabled) {
+  if (is_lat_active() && 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
@@ -729,7 +737,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
   }
 
   // No angle control allowed when controls are not allowed
-  violation |= !controls_allowed && steer_control_enabled;
+  violation |= !is_lat_active() && steer_control_enabled;
 
   return violation;
 }

diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h
@@ -78,6 +78,8 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) {
   if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) {
     bool cruise_engaged = GET_BIT(to_push, 21U);
     pcm_cruise_check(cruise_engaged);
+
+    acc_main_on = GET_BIT(to_push, 20U);
   }
 
   // TODO: use the same message for both

diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h
@@ -180,6 +180,11 @@ static void ford_rx_hook(const CANPacket_t *to_push) {
       unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
       bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
       pcm_cruise_check(cruise_engaged);
+      acc_main_on = (cruise_state == 3U) || cruise_engaged;
+    }
+
+    if (addr == FORD_Steering_Data_FD1) {
+      lkas_button_press = GET_BIT(to_push, 40U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
     }
 
     // If steering controls messages are received on the destination bus, it's an indication

diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h
@@ -85,6 +85,10 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
       regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
     }
 
+    if (addr == 0xC9) {
+      acc_main_on = GET_BIT(to_push, 29U);
+    }
+
     bool stock_ecu_detected = (addr == 0x180);  // ASCMLKASteeringCmd
 
     // Check ASCMGasRegenCmd only if we're blocking it

diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h
@@ -114,6 +114,13 @@ static void honda_rx_hook(const CANPacket_t *to_push) {
   // 0x1A6 for the ILX, 0x296 for the Civic Touring
   if (((addr == 0x1A6) || (addr == 0x296)) && (bus == pt_bus)) {
     int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5;
+    bool is_main = (button == HONDA_BTN_MAIN);
+    main_button_press = is_main ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
+
+    int cruise_setting = (GET_BYTE(to_push, (addr == 0x296) ? 0U : 5U) & 0x0CU) >> 2U;
+    if (cruise_setting == 1) {
+      lkas_button_press = MADS_BUTTON_PRESSED;
+    }
 
     // enter controls on the falling edge of set or resume
     bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET);

diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h
@@ -126,13 +126,23 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) {
     hyundai_common_cruise_state_check(cruise_engaged);
   }
 
+  if ((addr == 0x420) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) {
+    if (!hyundai_longitudinal) {
+      acc_main_on = GET_BIT(to_push, 0U);
+    }
+  }
+
   if (bus == 0) {
     if (addr == 0x251) {
       int torque_driver_new = (GET_BYTES(to_push, 0, 2) & 0x7ffU) - 1024U;
       // update array of samples
       update_sample(&torque_driver, torque_driver_new);
     }
 
+    if (addr == 0x391) {
+      lkas_button_press = GET_BIT(to_push, 4U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
+    }
+
     // ACC steering wheel buttons
     if (addr == 0x4F1) {
       int cruise_button = GET_BYTE(to_push, 0) & 0x7U;

diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h
@@ -68,9 +68,11 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) {
       if (addr == 0x1cf) {
         cruise_button = GET_BYTE(to_push, 2) & 0x7U;
         main_button = GET_BIT(to_push, 19U);
+        lkas_button_press = GET_BIT(to_push, 23U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
       } else {
         cruise_button = (GET_BYTE(to_push, 4) >> 4) & 0x7U;
         main_button = GET_BIT(to_push, 34U);
+        lkas_button_press = GET_BIT(to_push, 39U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
       }
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
     }

diff --git a/board/safety/safety_hyundai_common.h b/board/safety/safety_hyundai_common.h
@@ -91,6 +91,8 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
     hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
   }
 
+  main_button_press = main_button ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
+
   if (hyundai_longitudinal) {
     // enter controls on falling edge of resume or set
     bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET);

diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h
@@ -36,6 +36,8 @@ static void mazda_rx_hook(const CANPacket_t *to_push) {
     if (addr == MAZDA_CRZ_CTRL) {
       bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
       pcm_cruise_check(cruise_engaged);
+
+      acc_main_on = GET_BIT(to_push, 17U);
     }
 
     if (addr == MAZDA_ENGINE_DATA) {

diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h
@@ -3,6 +3,7 @@
 #include "safety_declarations.h"
 
 static bool nissan_alt_eps = false;
+static bool nissan_leaf = false;
 
 static void nissan_rx_hook(const CANPacket_t *to_push) {
   int bus = GET_BUS(to_push);
@@ -53,6 +54,14 @@ static void nissan_rx_hook(const CANPacket_t *to_push) {
     pcm_cruise_check(cruise_engaged);
   }
 
+  if ((addr == 0x239) && (bus == 0) && nissan_leaf) {
+    acc_main_on = GET_BIT(to_push, 17U);
+  }
+
+  if ((addr == 0x1B6) && (bus == (nissan_alt_eps ? 2 : 1))) {
+    acc_main_on = GET_BIT(to_push, 36U);
+  }
+
   generic_rx_checks((addr == 0x169) && (bus == 0));
 }
 
@@ -150,8 +159,10 @@ static safety_config nissan_init(uint16_t param) {
 
   // EPS Location. false = V-CAN, true = C-CAN
   const int NISSAN_PARAM_ALT_EPS_BUS = 1;
+  const int NISSAN_PARAM_LEAF = 512;
 
   nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
+  nissan_leaf = GET_FLAG(param, NISSAN_PARAM_LEAF);
   return BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS);
 }
 

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
@@ -106,10 +106,19 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
     update_sample(&angle_meas, angle_meas_new);
   }
 
+  if ((addr == MSG_SUBARU_ES_LKAS_State) && (bus == SUBARU_CAM_BUS)) {
+    bool lkas_hud = ((GET_BYTE(to_push, 2) & 0x0CU) >> 2U) > 0U;
+    if (lkas_hud) {
+      lkas_button_press = MADS_BUTTON_PRESSED;
+    }
+  }
+
   // enter controls on rising edge of ACC, exit controls on ACC off
   if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
     bool cruise_engaged = GET_BIT(to_push, 41U);
     pcm_cruise_check(cruise_engaged);
+
+    acc_main_on = GET_BIT(to_push, 40U);
   }
 
   // update vehicle moving with any non-zero wheel speed

diff --git a/board/safety/safety_subaru_preglobal.h b/board/safety/safety_subaru_preglobal.h
@@ -36,6 +36,8 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) {
     if (addr == MSG_SUBARU_PG_CruiseControl) {
       bool cruise_engaged = GET_BIT(to_push, 49U);
       pcm_cruise_check(cruise_engaged);
+
+      acc_main_on = GET_BIT(to_push, 48U);
     }
 
     // update vehicle moving with any non-zero wheel speed

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -64,6 +64,17 @@ static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) {
 static void toyota_rx_hook(const CANPacket_t *to_push) {
   const int TOYOTA_LTA_MAX_ANGLE = 1657;  // EPS only accepts up to 94.9461
 
+  if (GET_BUS(to_push) == 2U) {
+    int addr = GET_ADDR(to_push);
+
+    if (addr == 0x412) {
+      bool lkas_hud_visible = (GET_BYTE(to_push, 0U) & 0xC0U) > 0U;
+      if (lkas_hud_visible) {
+        lkas_button_press = MADS_BUTTON_PRESSED;
+      }
+    }
+  }
+
   if (GET_BUS(to_push) == 0U) {
     int addr = GET_ADDR(to_push);
 
@@ -139,6 +150,10 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
       UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6);
     }
 
+    if (addr == 0x1D3) {
+      acc_main_on = GET_BIT(to_push, 15U);
+    }
+
     bool stock_ecu_detected = addr == 0x2E4;  // STEERING_LKA
     if (!toyota_stock_longitudinal && (addr == 0x343)) {
       stock_ecu_detected = true;  // ACC_CONTROL