Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MADS: Steering Mode on Brake Pedal Press #89

Merged
merged 1 commit into from
Mar 19, 2025
Merged

MADS: Steering Mode on Brake Pedal Press #89

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions opendbc/safety/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,5 +11,6 @@ class ALTERNATIVE_EXPERIENCE:
ALLOW_AEB = 16

# sunnypilot
ENABLE_MADS = 2 ** 10
DISENGAGE_LATERAL_ON_BRAKE = 2 ** 11
ENABLE_MADS = 1024
MADS_DISENGAGE_LATERAL_ON_BRAKE = 2048
MADS_PAUSE_LATERAL_ON_BRAKE = 4096
14 changes: 9 additions & 5 deletions opendbc/safety/sunnypilot/safety_mads.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ inline void m_mads_state_init(void) {

m_mads_state.system_enabled = false;
m_mads_state.disengage_lateral_on_brake = false;
m_mads_state.pause_lateral_on_brake = false;

m_mads_state.acc_main.previous = false;
m_mads_state.acc_main.transition = MADS_EDGE_NO_CHANGE;
Expand Down Expand Up @@ -96,15 +97,16 @@ inline void m_update_control_state(void) {
}

// Secondary control conditions - only checked if primary conditions don't block further control processing
if (allowed && m_mads_state.disengage_lateral_on_brake) {
if (allowed && (m_mads_state.disengage_lateral_on_brake || m_mads_state.pause_lateral_on_brake)) {
// Brake rising edge immediately blocks controls
// Brake release might request controls if brake was the ONLY reason for disengagement
if (m_mads_state.braking.transition == MADS_EDGE_RISING) {
mads_exit_controls(MADS_DISENGAGE_REASON_BRAKE);
allowed = false;
} else if ((m_mads_state.braking.transition == MADS_EDGE_FALLING) &&
(m_mads_state.current_disengage.active_reason == MADS_DISENGAGE_REASON_BRAKE) &&
(m_mads_state.current_disengage.pending_reasons == MADS_DISENGAGE_REASON_BRAKE)) {
(m_mads_state.current_disengage.pending_reasons == MADS_DISENGAGE_REASON_BRAKE) &&
m_mads_state.pause_lateral_on_brake) {
m_mads_state.controls_requested_lat = true;
} else if (m_mads_state.braking.current) {
allowed = false;
Expand Down Expand Up @@ -138,15 +140,17 @@ inline void mads_heartbeat_engaged_check(void) {

inline void mads_set_alternative_experience(const int *mode) {
const bool mads_enabled = (*mode & ALT_EXP_ENABLE_MADS) != 0;
const bool disengage_lateral_on_brake = (*mode & ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE) != 0;
const bool disengage_lateral_on_brake = (*mode & ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE) != 0;
const bool pause_lateral_on_brake = (*mode & ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE) != 0;

mads_set_system_state(mads_enabled, disengage_lateral_on_brake);
mads_set_system_state(mads_enabled, disengage_lateral_on_brake, pause_lateral_on_brake);
}

extern inline void mads_set_system_state(const bool enabled, const bool disengage_lateral_on_brake) {
extern inline void mads_set_system_state(const bool enabled, const bool disengage_lateral_on_brake, const bool pause_lateral_on_brake) {
m_mads_state_init();
m_mads_state.system_enabled = enabled;
m_mads_state.disengage_lateral_on_brake = disengage_lateral_on_brake;
m_mads_state.pause_lateral_on_brake = pause_lateral_on_brake;
}

inline void mads_exit_controls(const DisengageReason reason) {
Expand Down
8 changes: 5 additions & 3 deletions opendbc/safety/sunnypilot/safety_mads_declarations.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,8 @@ typedef enum __attribute__((packed)) {
// ===============================

#define ALT_EXP_ENABLE_MADS 1024
#define ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE 2048
#define ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE 2048
#define ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE 4096

#define MISMATCH_DEFAULT_THRESHOLD 25

Expand Down Expand Up @@ -75,6 +76,7 @@ typedef struct {

bool system_enabled : 1;
bool disengage_lateral_on_brake : 1;
bool pause_lateral_on_brake : 1;
bool controls_requested_lat : 1;
bool controls_allowed_lat : 1;
} MADSState;
Expand All @@ -94,7 +96,7 @@ extern uint32_t heartbeat_engaged_mads_mismatches;
// External Function Declarations (kept as needed)
// ===============================

extern void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake);
extern void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
extern void mads_set_alternative_experience(const int *mode);
extern void mads_state_update(bool op_vehicle_moving, bool op_acc_main, bool op_allowed, bool is_braking);
extern void mads_exit_controls(DisengageReason reason);
Expand All @@ -109,4 +111,4 @@ extern EdgeTransition m_get_edge_transition(bool current, bool last);
extern void m_mads_state_init(void);
extern void m_update_button_state(ButtonStateTracking *button_state);
extern void m_update_binary_state(BinaryStateTracking *state);
extern void m_update_control_state(void);
extern void m_update_control_state(void);
8 changes: 4 additions & 4 deletions opendbc/safety/tests/hyundai_common.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,7 @@ def test_main_cruise_button(self):

# Test initial state
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False)
self.safety.set_mads_params(enable_mads, False, False)

self.assertFalse(self.safety.get_acc_main_on())

Expand Down Expand Up @@ -191,7 +191,7 @@ def test_acc_main_sync_mismatches_reset(self):
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)

self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False)
self.safety.set_mads_params(enable_mads, False, False)

# Initial state
self._rx(self._main_cruise_button_msg(0))
Expand Down Expand Up @@ -226,7 +226,7 @@ def test_acc_main_sync_mismatch_counter(self):
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)

self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False)
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_controls_allowed_lat(True)

# Start with matched states
Expand Down Expand Up @@ -269,7 +269,7 @@ def test_acc_main_sync_mismatch_recovery(self):
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)

self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False)
self.safety.set_mads_params(enable_mads, False, False)

# Create initial mismatch
self._rx(self._main_cruise_button_msg(1)) # Press button
Expand Down
14 changes: 9 additions & 5 deletions opendbc/safety/tests/libsafety/safety_helpers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ bool get_enable_mads(void){
return get_mads_state()->system_enabled;
}

bool get_disengage_lateral_on_brake(void){
return get_mads_state()->disengage_lateral_on_brake;
bool get_pause_lateral_on_brake(void){
return get_mads_state()->pause_lateral_on_brake;
}

int get_alternative_experience(void){
Expand Down Expand Up @@ -238,13 +238,17 @@ void set_controls_requested_lat(bool c){
m_mads_state.controls_requested_lat = c;
}

void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake){
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake){
alternative_experience = 0;
if (enable_mads) {
alternative_experience |= ALT_EXP_ENABLE_MADS;

if (disengage_lateral_on_brake)
alternative_experience |= ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE;
if (disengage_lateral_on_brake) {
alternative_experience |= ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE;
} else if (pause_lateral_on_brake) {
alternative_experience |= ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE;
} else {
}
}

mads_set_alternative_experience(&alternative_experience);
Expand Down
8 changes: 4 additions & 4 deletions opendbc/safety/tests/libsafety/safety_helpers.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ def setup_safety_helpers(ffi):
int get_honda_hw(void);

bool get_enable_mads(void);
bool get_disengage_lateral_on_brake(void);
bool get_pause_lateral_on_brake(void);
void set_mads_button_press(int mads_button_press);
void set_controls_allowed_lat(bool c);
void set_controls_requested_lat(bool c);
Expand All @@ -68,7 +68,7 @@ def setup_safety_helpers(ffi):
int mads_get_current_disengage_reason(void);
int get_temp_debug(void);
uint32_t get_acc_main_on_mismatches(void);
void set_mads_params(bool enable_mads, bool disengage_lat_on_brake);
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
void set_heartbeat_engaged_mads(bool c);
void mads_heartbeat_engaged_check(void);
""")
Expand Down Expand Up @@ -132,14 +132,14 @@ def get_honda_hw(self) -> int: ...

def set_mads_button_press(self, mads_button_press: int) -> None: ...
def get_enable_mads(self) -> bool: ...
def get_disengage_lateral_on_brake(self) -> bool: ...
def get_pause_lateral_on_brake(self) -> bool: ...

def get_mads_button_press(self) -> int: ...
def mads_set_current_disengage_reason(self, reason: int) -> None: ...
def mads_get_current_disengage_reason(self) -> int: ...
def get_acc_main_on_mismatches(self) -> int: ...

def set_mads_params(self, enable_mads: bool, disengage_lat_on_brake: bool) -> None: ...
def set_mads_params(self, enable_mads: bool, disengage_lateral_on_brake: bool, pause_lateral_on_brake: bool) -> None: ...
def set_heartbeat_engaged_mads(self, c: bool) -> None: ...
def mads_heartbeat_engaged_check(self) -> None: ...
# def get_temp_debug(self) -> int: ...
Loading