MADS: state check cleanup (#15)

* MADS: cruise main button check cleanup * exit and resume controls cleanup * shorten and simplier * Update var names * simplify LKAS button press * more cleanup * Hyundai longitudinal: revert back for now * Add todo for future PR
sunnypilot · Aug 20, 2023 · 446ad94 · 446ad94
1 parent 59f157d
commit 446ad94
Show file tree

Hide file tree

Showing 16 changed files with 75 additions and 188 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -1,6 +1,8 @@
 #include "safety_declarations.h"
 #include "can_definitions.h"
 
+#include "safety_sunnypilot_common.h"
+
 // include the safety policies.
 #include "safety/safety_defaults.h"
 #include "safety/safety_honda.h"
@@ -269,47 +271,17 @@ void generic_rx_checks(bool stock_ecu_detected) {
 
   // exit controls on rising edge of brake press
   if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
-    if (alternative_experience & ALT_EXP_MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE) {
-      disengageFromBrakes = true;
-      controls_allowed_long = 0;
-    } else if (alternative_experience & ALT_EXP_ENABLE_MADS) {
-      if (controls_allowed == 1) {
-        disengageFromBrakes = true;
-      }
-      controls_allowed = 0;
-      controls_allowed_long = 0;
-    } else {
-      controls_allowed = 0;
-      controls_allowed_long = 0;
-    }
+    mads_exit_controls_check();
   } else if (!brake_pressed && disengageFromBrakes) {
-    disengageFromBrakes = false;
-    if (alternative_experience & ALT_EXP_ENABLE_MADS) {
-      controls_allowed = 1;
-    }
+    mads_resume_controls_check();
   }
   brake_pressed_prev = brake_pressed;
 
   // exit controls on rising edge of regen paddle
   if (regen_braking && (!regen_braking_prev || vehicle_moving)) {
-    if (alternative_experience & ALT_EXP_MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE) {
-      disengageFromBrakes = true;
-      controls_allowed_long = 0;
-    } else if (alternative_experience & ALT_EXP_ENABLE_MADS) {
-      if (controls_allowed == 1) {
-        disengageFromBrakes = true;
-      }
-      controls_allowed = 0;
-      controls_allowed_long = 0;
-    } else {
-      controls_allowed = 0;
-      controls_allowed_long = 0;
-    }
+    mads_exit_controls_check();
   } else if (!regen_braking && disengageFromBrakes) {
-    disengageFromBrakes = false;
-    if (alternative_experience & ALT_EXP_ENABLE_MADS) {
-      controls_allowed = 1;
-    }
+    mads_resume_controls_check();
   }
   regen_braking_prev = regen_braking;
 

diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h
@@ -211,15 +211,7 @@ static int chrysler_rx_hook(CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       acc_main_on = GET_BIT(to_push, 20U) != 0U;
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     // TODO: use the same message for both

diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h
@@ -268,15 +268,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       acc_main_on = (cruise_state == 3U) || (cruise_state == 4U) || (cruise_state == 5U);
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     // 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
@@ -133,15 +133,7 @@ static int gm_rx_hook(CANPacket_t *to_push) {
 
     if (addr == 0xC9) {
       acc_main_on = GET_BIT(to_push, 29U) != 0U;
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     bool stock_ecu_detected = (addr == 384);  // ASCMLKASteeringCmd

diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h
@@ -141,15 +141,7 @@ static int honda_rx_hook(CANPacket_t *to_push) {
     // 0x326 for all Bosch and some Nidec, 0x1A6 for some Nidec
     if ((addr == 0x326) || (addr == 0x1A6)) {
       acc_main_on = GET_BIT(to_push, ((addr == 0x326) ? 28U : 47U));
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     // state machine to enter and exit controls for button enabling

diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h
@@ -207,16 +207,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
     // ACC main state
     if (!hyundai_longitudinal && (addr == 1056)) {
       acc_main_on = GET_BIT(to_push, 0U);
-      // ACC main state
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
     if (addr == 1057) {
       // 2 bits: 13-14
@@ -232,12 +223,9 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
       update_sample(&torque_driver, torque_driver_new);
     }
 
-    if (addr == 913) {
+    if ((addr == 913) && hyundai_lfa_button && mads_enabled) {
       bool lfa_pressed = GET_BIT(to_push, 4U); // LFA_PRESSED signal
-      if (lfa_pressed && !lfa_pressed_prev && hyundai_lfa_button && mads_enabled) {
-        controls_allowed = 1;
-      }
-      lfa_pressed_prev = lfa_pressed;
+      mads_lkas_button_check(lfa_pressed);
     }
 
     // ACC steering wheel buttons
@@ -249,6 +237,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
       bool main_enabled_prev = false;
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
 
+      // TODO: Refactor this method at a later time
       if (hyundai_longitudinal) {
         // enter controls on rising edge of main
         if (main_button && !main_button_prev) {
@@ -270,28 +259,12 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
     if (hyundai_non_scc) {
       if (addr == 871) {
         bool cruise_engaged = GET_BYTE(to_push, 0) != 0U; // CF_Lvr_CruiseSet signal
-        if (cruise_engaged && !cruise_engaged_prev) {
-          controls_allowed = 1;
-          controls_allowed_long = 1;
-        }
-
-        if (!cruise_engaged) {
-          controls_allowed_long = 0;
-        }
-        cruise_engaged_prev = cruise_engaged;
+        hyundai_common_cruise_state_check(cruise_engaged);
       }
 
       if (addr == 608) {
         acc_main_on = GET_BIT(to_push, 25U); // CRUISE_LAMP_M signal
-        if (acc_main_on && mads_enabled) {
-          controls_allowed = 1;
-        }
-        if (!acc_main_on && acc_main_on_prev) {
-          disengageFromBrakes = false;
-          controls_allowed = 0;
-          controls_allowed_long = 0;
-        }
-        acc_main_on_prev = acc_main_on;
+        mads_acc_main_check(acc_main_on);
       }
     }
 

diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h
@@ -200,10 +200,9 @@ static int hyundai_canfd_rx_hook(CANPacket_t *to_push) {
       }
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
 
-      if (lfa_pressed && !lfa_pressed_prev && mads_enabled) {
-        controls_allowed = 1;
+      if (mads_enabled) {
+        mads_lkas_button_check(lfa_pressed);
       }
-      lfa_pressed_prev = lfa_pressed;
     }
 
     // gas press, different for EV, hybrid, and ICE models

diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h
@@ -55,15 +55,7 @@ static int mazda_rx_hook(CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       acc_main_on = GET_BIT(to_push, 17U) != 0U;
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     if (addr == MAZDA_ENGINE_DATA) {

diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h
@@ -100,15 +100,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
       if ((addr == 0x239) && (bus == 0)) {
         acc_main_on = GET_BIT(to_push, 17U) != 0U;
       }
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     generic_rx_checks((addr == 0x169) && (bus == 0));

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
@@ -112,12 +112,9 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
       update_sample(&torque_driver, torque_driver_new);
     }
 
-    if ((addr == MSG_SUBARU_ES_LKAS_State) && (bus == SUBARU_CAM_BUS)) {
+    if ((addr == MSG_SUBARU_ES_LKAS_State) && (bus == SUBARU_CAM_BUS) && mads_enabled) {
       bool lkas_pressed = (GET_BYTE(to_push, 2) & 0x0C) > 0; // LKAS_Dash_State signal
-      if (lkas_pressed && !lkas_pressed_prev && mads_enabled) {
-        controls_allowed = 1;
-      }
-      lkas_pressed_prev = lkas_pressed;
+      mads_lkas_button_check(lkas_pressed);
     }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
@@ -126,15 +123,7 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       acc_main_on = GET_BIT(to_push, 40U) != 0U;
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     // 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
@@ -66,15 +66,7 @@ static int subaru_preglobal_rx_hook(CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       acc_main_on = GET_BIT(to_push, 48U) != 0U;
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     // update vehicle moving with any non-zero wheel speed

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -80,22 +80,16 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
   bool valid = addr_safety_check(to_push, &toyota_rx_checks,
                                  toyota_get_checksum, toyota_compute_checksum, NULL, NULL);
 
-  if (valid && (GET_BUS(to_push) == 2U)) {
+  if (valid && (GET_BUS(to_push) == 2U) && mads_enabled) {
     int addr = GET_ADDR(to_push);
     if ((addr == 0x412) && !toyota_mads_lta_msg) {
-      bool set_me = (GET_BYTE(to_push, 0) & 0xC0) > 0; // LKAS_HUD
-      if (set_me && !set_me_prev && mads_enabled) {
-        controls_allowed = 1;
-      }
-      set_me_prev = set_me;
+      bool lkas_pressed = (GET_BYTE(to_push, 0) & 0xC0) > 0; // LKAS_HUD
+      mads_lkas_button_check(lkas_pressed);
     }
 
     if ((addr == 0x412) && toyota_mads_lta_msg) {
-      bool set_me = (GET_BYTE(to_push, 3) & 0x40) > 0; // LDA_ON_MESSAGE
-      if (set_me && !set_me_prev && mads_enabled) {
-        controls_allowed = 1;
-      }
-      set_me_prev = set_me;
+      bool lkas_pressed = (GET_BYTE(to_push, 3) & 0x40) > 0; // LDA_ON_MESSAGE
+      mads_lkas_button_check(lkas_pressed);
     }
   } else if (valid && (GET_BUS(to_push) == 0U)) {
     int addr = GET_ADDR(to_push);
@@ -131,15 +125,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
 
     if (addr == 0x1D3) {
       acc_main_on = GET_BIT(to_push, 15U) != 0U;
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = 1;
-      }
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = 0;
-        controls_allowed_long = 0;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     if (addr == 0xaa) {

diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h
@@ -156,16 +156,7 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) {
         pcm_cruise_check(cruise_engaged);
       }
 
-      if (acc_main_on && mads_enabled) {
-        controls_allowed = true;
-      }
-
-      if (!acc_main_on && acc_main_on_prev) {
-        disengageFromBrakes = false;
-        controls_allowed = false;
-        controls_allowed_long = false;
-      }
-      acc_main_on_prev = acc_main_on;
+      mads_acc_main_check(acc_main_on);
     }
 
     if (addr == MSG_GRA_ACC_01) {

diff --git a/board/safety/safety_volkswagen_pq.h b/board/safety/safety_volkswagen_pq.h
@@ -129,15 +129,7 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
         // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off
         // Signal: Motor_5.GRA_Hauptschalter
         acc_main_on = GET_BIT(to_push, 50U);
-        if (acc_main_on && mads_enabled) {
-          controls_allowed = 1;
-        }
-        if (!acc_main_on && acc_main_on_prev) {
-          disengageFromBrakes = false;
-          controls_allowed = 0;
-          controls_allowed_long = 0;
-        }
-        acc_main_on_prev = acc_main_on;
+        mads_acc_main_check(acc_main_on);
       }
 
       if (addr == MSG_GRA_NEU) {

diff --git a/board/safety_declarations.h b/board/safety_declarations.h
@@ -198,8 +198,6 @@ bool regen_braking = false;
 bool regen_braking_prev = false;
 bool cruise_engaged_prev = false;
 bool acc_main_on_prev = false;
-bool lfa_pressed_prev = false;
-bool set_me_prev = false;
 bool lkas_pressed_prev = false;
 struct sample_t vehicle_speed;
 bool vehicle_moving = false;