diff --git a/board/config.h b/board/config.h index 903161876c..f484e46c02 100644 --- a/board/config.h +++ b/board/config.h @@ -9,6 +9,7 @@ //#define DEBUG_FAULTS //#define DEBUG_COMMS //#define DEBUG_FAN +//#define DEBUG_SAFETY #define CAN_INIT_TIMEOUT_MS 500U #define DEEPSLEEP_WAKEUP_DELAY 3U diff --git a/board/safety.h b/board/safety.h index d8d14ba5de..6d53bc8819 100644 --- a/board/safety.h +++ b/board/safety.h @@ -53,6 +53,10 @@ #define SAFETY_HYUNDAI_CANFD 28U #define SAFETY_MAZDA_2019 29U +#define TORQUE_CHK_LOG_TAG "steer_torque_cmd_checks violation: " +#define ANGLE_CHK_LOG_TAG "steer_angle_cmd_checks violation: " +#define LONG_CHK_LOG_TAG "longitudinal checks violation: " + uint16_t current_safety_mode = SAFETY_SILENT; uint16_t current_safety_param = 0; const safety_hooks *current_hooks = &nooutput_hooks; @@ -502,27 +506,40 @@ int ROUND(float val) { return val + ((val > 0.0) ? 0.5 : -0.5); } +static void safety_print(const char *a){ +#ifdef DEBUG_SAFETY + print(a); +#else + UNUSED(a); +#endif +} + // Safety checks for longitudinal actuation bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) { bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel); bool accel_inactive = desired_accel == limits.inactive_accel; - return !(accel_valid || accel_inactive); + return !(accel_valid || accel_inactive) + ? (safety_print(LONG_CHK_LOG_TAG "accel\n"), true) : false; } bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits) { - return !get_longitudinal_allowed() && (desired_speed != limits.inactive_speed); + return (!get_longitudinal_allowed() && (desired_speed != limits.inactive_speed)) + ? (safety_print(LONG_CHK_LOG_TAG "desired_speed exceeded inactive_speed\n"), true) : false; } bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) { bool gas_valid = get_longitudinal_allowed() && !max_limit_check(desired_gas, limits.max_gas, limits.min_gas); bool gas_inactive = desired_gas == limits.inactive_gas; - return !(gas_valid || gas_inactive); + return !(gas_valid || gas_inactive) + ? (safety_print(LONG_CHK_LOG_TAG "gas\n"), true) : false; } bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits) { bool violation = false; - violation |= !get_longitudinal_allowed() && (desired_brake != 0); - violation |= desired_brake > limits.max_brake; + violation |= (!get_longitudinal_allowed() && (desired_brake != 0)) + ? (safety_print(LONG_CHK_LOG_TAG "desired_brake != 0\n"), true) : false; + violation |= (desired_brake > limits.max_brake) + ? (safety_print(LONG_CHK_LOG_TAG "desired_brake > max_brake\n"), true) : false; return violation; } @@ -537,21 +554,25 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi if (controls_allowed) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer); + violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer) + ? (safety_print(TORQUE_CHK_LOG_TAG "max_limit exceeded\n"), true) : false; // *** torque rate limit check *** if (limits.type == TorqueDriverLimited) { violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, limits.max_steer, limits.max_rate_up, limits.max_rate_down, - limits.driver_torque_allowance, limits.driver_torque_factor); + limits.driver_torque_allowance, limits.driver_torque_factor) + ? (safety_print(TORQUE_CHK_LOG_TAG "driver_limit exceeded\n"), true) : false; } else { violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, - limits.max_rate_up, limits.max_rate_down, limits.max_torque_error); + limits.max_rate_up, limits.max_rate_down, limits.max_torque_error) + ? (safety_print(TORQUE_CHK_LOG_TAG "dist_to_meas exceeded\n"), true) : false; } desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta) + ? (safety_print(TORQUE_CHK_LOG_TAG "rt_rate_limit exceeded\n"), true) : false; // every RT_INTERVAL set the new limits uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); @@ -562,36 +583,31 @@ 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)) { - violation = true; - } + violation |= (!controls_allowed && (desired_torque != 0)) + ? (safety_print(TORQUE_CHK_LOG_TAG "desired_torque != 0\n"), true) : false; // certain safety modes set their steer request bit low for one or more frame at a // predefined max frequency to avoid steering faults in certain situations bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0); if (!limits.has_steer_req_tolerance) { - if (steer_req_mismatch) { - violation = true; - } + violation |= (steer_req_mismatch) + ? (safety_print(TORQUE_CHK_LOG_TAG "steer_req_mismatch\n"), true) : false; } else { if (steer_req_mismatch) { if (invalid_steer_req_count == 0) { // disallow torque cut if not enough recent matching steer_req messages - if (valid_steer_req_count < limits.min_valid_request_frames) { - violation = true; - } + violation |= (valid_steer_req_count < limits.min_valid_request_frames) + ? (safety_print(TORQUE_CHK_LOG_TAG "valid_steer_req_count too low\n"), true) : false; // or we've cut torque too recently in time uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last); - if (ts_elapsed < limits.min_valid_request_rt_interval) { - violation = true; - } + violation |= (ts_elapsed < limits.min_valid_request_rt_interval) + ? (safety_print(TORQUE_CHK_LOG_TAG "cut torque too recently\n"), true) : false; } else { // or we're cutting more frames consecutively than allowed - if (invalid_steer_req_count >= limits.max_invalid_request_frames) { - violation = true; - } + violation |= (invalid_steer_req_count >= limits.max_invalid_request_frames) + ? (safety_print(TORQUE_CHK_LOG_TAG "cut more consecutive frames than allowed\n"), true) : false; } valid_steer_req_count = 0; @@ -652,18 +668,27 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const } // check for violation; - violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); + violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle) + ? (safety_print(ANGLE_CHK_LOG_TAG "steer control enabled, max_limit exceeded\n"), true) : false; + } desired_angle_last = desired_angle; // Angle should either be 0 or same as current angle while not steering if (!steer_control_enabled) { - violation |= (limits.inactive_angle_is_zero ? (desired_angle != 0) : - max_limit_check(desired_angle, angle_meas.max + 1, angle_meas.min - 1)); + if (limits.inactive_angle_is_zero) { + violation |= (desired_angle != 0) + ? (safety_print(ANGLE_CHK_LOG_TAG "steer control disabled, desired_angle != 0\n"), true) : false; + } else { + violation |= (max_limit_check(desired_angle, angle_meas.max + 1, angle_meas.min - 1)) + ? (safety_print(ANGLE_CHK_LOG_TAG "steer control disabled, max_limit exceed"), true) : false; + } } // No angle control allowed when controls are not allowed - violation |= !controls_allowed && steer_control_enabled; + violation |= (!controls_allowed && steer_control_enabled) + ? (safety_print(ANGLE_CHK_LOG_TAG "steer_control_enabled\n"), true) : false; + return violation; }