Skip to content

Commit

Permalink
print safety check violations
Browse files Browse the repository at this point in the history
  • Loading branch information
MoreTore committed Jul 19, 2023
1 parent 93e0ff9 commit e1b7856
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 28 deletions.
1 change: 1 addition & 0 deletions board/config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
81 changes: 53 additions & 28 deletions board/safety.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

Expand All @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit e1b7856

Please sign in to comment.