From c0c1cc254a7364fcc60c679a312803fb4c540c0b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 25 Nov 2024 23:29:00 -0500 Subject: [PATCH] move to safety_mads --- board/safety.h | 12 +--- board/safety/safety_hyundai.h | 2 +- board/safety/safety_hyundai_canfd.h | 2 +- board/safety/safety_hyundai_common.h | 12 ---- board/safety_declarations.h | 27 ------- board/sunnypilot/safety_common.h | 50 ------------- board/sunnypilot/safety_mads.h | 103 +++++++++++++++++++++++++++ 7 files changed, 106 insertions(+), 102 deletions(-) delete mode 100644 board/sunnypilot/safety_common.h create mode 100644 board/sunnypilot/safety_mads.h diff --git a/board/safety.h b/board/safety.h index 97c1c16a28..be67869a25 100644 --- a/board/safety.h +++ b/board/safety.h @@ -3,7 +3,7 @@ #include "safety_declarations.h" #include "can.h" -#include "sunnypilot/safety_common.h" +#include "sunnypilot/safety_mads.h" // include the safety policies. #include "safety/safety_defaults.h" @@ -83,16 +83,6 @@ bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 int cruise_button_prev = 0; bool safety_rx_checks_invalid = false; -// mads -bool enable_mads = false; -bool disengage_lateral_on_brake = false; -bool disengaged_from_brakes = false; -bool controls_allowed_lat = false; -bool lkas_button = false; -bool acc_main_on_prev = false; -bool main_button_prev = false; -uint32_t acc_main_on_mismatches = 0; - // for safety modes with torque steering control int desired_torque_last = 0; // last desired steer torque int rt_torque_last = 0; // last desired torque for real time check diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 9cfe3a4b4b..46ff8590bb 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -210,7 +210,7 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) { if (addr == 0x420) { bool acc_main_on_tx = GET_BIT(to_send, 0U); - hyundai_common_reset_acc_main(acc_main_on_tx); + reset_acc_main(acc_main_on_tx); } // ACCEL: safety check diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index 1d54371990..44f13ccefb 100644 --- a/board/safety/safety_hyundai_canfd.h +++ b/board/safety/safety_hyundai_canfd.h @@ -204,7 +204,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) { } bool acc_main_on_tx = GET_BIT(to_send, 66U); - hyundai_common_reset_acc_main(acc_main_on_tx); + reset_acc_main(acc_main_on_tx); } return tx; diff --git a/board/safety/safety_hyundai_common.h b/board/safety/safety_hyundai_common.h index 4bef6c0648..5b81c618df 100644 --- a/board/safety/safety_hyundai_common.h +++ b/board/safety/safety_hyundai_common.h @@ -137,15 +137,3 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *to_push) { return crc; } - -void hyundai_common_reset_acc_main(const bool acc_main_on_tx) { - if (acc_main_on && !acc_main_on_tx) { - acc_main_on_mismatches += 1U; - if (acc_main_on_mismatches >= 25U) { - acc_main_on = acc_main_on_tx; - mads_check_acc_main(); - } - } else { - acc_main_on_mismatches = 0U; - } -} diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 3c0e277959..b0e9588e95 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -207,23 +207,6 @@ extern bool acc_main_on; // referred to as "ACC off" in ISO 15622:2018 extern int cruise_button_prev; extern bool safety_rx_checks_invalid; -// mads -void mads_set_state(bool state); -void mads_check_acc_main(void); -void mads_check_lkas_button(void); -void mads_exit_controls(void); -void mads_resume_controls(void); -void check_braking_condition(bool state, bool state_prev); - -extern bool enable_mads; -extern bool disengage_lateral_on_brake; -extern bool disengaged_from_brakes; -extern bool controls_allowed_lat; -extern bool lkas_button; -extern bool acc_main_on_prev; -extern bool main_button_prev; -extern uint32_t acc_main_on_mismatches; - // for safety modes with torque steering control extern int desired_torque_last; // last desired steer torque extern int rt_torque_last; // last desired torque for real time check @@ -261,16 +244,6 @@ extern struct sample_t angle_meas; // last 6 steer angles/curvatures // This flag allows AEB to be commanded from openpilot. #define ALT_EXP_ALLOW_AEB 16 -// Enable the ability to enable sunnypilot Automatic Lane Centering and ACC/SCC independently of each other. This -// will enable MADS and allow other features to be used. -// Enable the ability to re-engage sunnypilot Automatic Lane Centering only (NOT ACC/SCC) on brake release while MADS -// is enabled. -#define ALT_EXP_ENABLE_MADS 1024 - -// Enable the ability to disable disengaging lateral control on brake press while MADS is enabled. -// The feature must be gated behind this flag per geohot's comment on the comma community Discord server. -#define ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE 2048 - extern int alternative_experience; // time since safety mode has been changed diff --git a/board/sunnypilot/safety_common.h b/board/sunnypilot/safety_common.h deleted file mode 100644 index b11f6a99a9..0000000000 --- a/board/sunnypilot/safety_common.h +++ /dev/null @@ -1,50 +0,0 @@ -#pragma once - -void mads_set_state(bool state) { - controls_allowed_lat = state; - disengaged_from_brakes = state; -} - -void mads_check_acc_main(void) { - if (acc_main_on && enable_mads) { - controls_allowed_lat = true; - } - - if (!acc_main_on && acc_main_on_prev) { - controls_allowed = false; - mads_set_state(false); - } - acc_main_on_prev = acc_main_on; -} - -void mads_check_lkas_button(void) { - if (lkas_button && enable_mads) { - controls_allowed_lat = true; - } -} - -void mads_exit_controls(void) { - if (controls_allowed_lat) { - disengaged_from_brakes = true; - controls_allowed_lat = false; - } -} - -void mads_resume_controls(void) { - if (disengaged_from_brakes) { - controls_allowed_lat = true; - disengaged_from_brakes = false; - } -} - -void check_braking_condition(bool state, bool state_prev) { - if (state && (!state_prev || vehicle_moving)) { - controls_allowed = false; - if (disengage_lateral_on_brake) { - mads_exit_controls(); - } - } else if (!state && disengage_lateral_on_brake) { - mads_resume_controls(); - } else { - } -} diff --git a/board/sunnypilot/safety_mads.h b/board/sunnypilot/safety_mads.h new file mode 100644 index 0000000000..b219929812 --- /dev/null +++ b/board/sunnypilot/safety_mads.h @@ -0,0 +1,103 @@ +#pragma once + +void mads_set_state(bool state); +void mads_check_acc_main(void); +void mads_check_lkas_button(void); +void mads_exit_controls(void); +void mads_resume_controls(void); +void check_braking_condition(bool state, bool state_prev); + +extern bool enable_mads; +bool enable_mads = false; + +extern bool disengage_lateral_on_brake; +bool disengage_lateral_on_brake = false; + +extern bool disengaged_from_brakes; +bool disengaged_from_brakes = false; + +extern bool controls_allowed_lat; +bool controls_allowed_lat = false; + +extern bool lkas_button; +bool lkas_button = false; + +extern bool acc_main_on_prev; +bool acc_main_on_prev = false; + +extern bool main_button_prev; +bool main_button_prev = false; + +extern uint32_t acc_main_on_mismatches; +uint32_t acc_main_on_mismatches = 0; + +// Enable the ability to enable sunnypilot Automatic Lane Centering and ACC/SCC independently of each other. This +// will enable MADS and allow other features to be used. +// Enable the ability to re-engage sunnypilot Automatic Lane Centering only (NOT ACC/SCC) on brake release while MADS +// is enabled. +#define ALT_EXP_ENABLE_MADS 1024 + +// Enable the ability to disable disengaging lateral control on brake press while MADS is enabled. +// The feature must be gated behind this flag per geohot's comment on the comma community Discord server. +#define ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE 2048 + +void mads_set_state(bool state) { + controls_allowed_lat = state; + disengaged_from_brakes = state; +} + +void mads_check_acc_main(void) { + if (acc_main_on && enable_mads) { + controls_allowed_lat = true; + } + + if (!acc_main_on && acc_main_on_prev) { + controls_allowed = false; + mads_set_state(false); + } + acc_main_on_prev = acc_main_on; +} + +void mads_check_lkas_button(void) { + if (lkas_button && enable_mads) { + controls_allowed_lat = true; + } +} + +void mads_exit_controls(void) { + if (controls_allowed_lat) { + disengaged_from_brakes = true; + controls_allowed_lat = false; + } +} + +void mads_resume_controls(void) { + if (disengaged_from_brakes) { + controls_allowed_lat = true; + disengaged_from_brakes = false; + } +} + +void check_braking_condition(bool state, bool state_prev) { + if (state && (!state_prev || vehicle_moving)) { + controls_allowed = false; + if (disengage_lateral_on_brake) { + mads_exit_controls(); + } + } else if (!state && disengage_lateral_on_brake) { + mads_resume_controls(); + } else { + } +} + +void reset_acc_main(const bool acc_main_on_tx) { + if (acc_main_on && !acc_main_on_tx) { + acc_main_on_mismatches += 1U; + if (acc_main_on_mismatches >= 25U) { + acc_main_on = acc_main_on_tx; + mads_check_acc_main(); + } + } else { + acc_main_on_mismatches = 0U; + } +}