Modular Assistive Driving System (MADS)

.github/workflows/test.yaml

@@ -117,7 +117,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:

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;

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

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) {
@@ -343,6 +349,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 +608,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 +635,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 +677,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 +693,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 +736,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;
 }

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

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) ? 1 : 0;
     }
 
     // If steering controls messages are received on the destination bus, it's an indication

board/safety/safety_honda.h

@@ -114,6 +114,7 @@ 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;
+    lkas_button_press = ((GET_BYTE(to_push, (addr == 0x296) ? 0 : 5) & 0x0CU) >> 2) == 1;
 
     // enter controls on the falling edge of set or resume
     bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET);

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) ? 1 : 0;
+    }
+
     // ACC steering wheel buttons
     if (addr == 0x4F1) {
       int cruise_button = GET_BYTE(to_push, 0) & 0x7U;

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) ? 1 : 0;
       } 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) ? 1 : 0;
       }
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
     }

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 ? 1 : 0;
+
   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);

board/safety/safety_toyota.h

@@ -64,6 +64,15 @@ 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) {
+      uint8_t lkas_hud = GET_BYTE(to_push, 0U) & 0xC0U;
+      lkas_button_press = lkas_hud;
+    }
+  }
+
   if (GET_BUS(to_push) == 0U) {
     int addr = GET_ADDR(to_push);
 
@@ -139,6 +148,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