mirror of
https://github.com/infiniteCable2/opendbc.git
synced 2026-02-18 13:03:52 +08:00
safety: namespace helpers (#2813)
* declarations * lowercase * namespace * screaming
This commit is contained in:
@@ -1,21 +1,21 @@
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define MIN(a, b) ({ \
|
||||
#define SAFETY_MIN(a, b) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
__typeof__(b) _b = (b); \
|
||||
(_a < _b) ? _a : _b; \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define MAX(a, b) ({ \
|
||||
#define SAFETY_MAX(a, b) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
__typeof__(b) _b = (b); \
|
||||
(_a > _b) ? _a : _b; \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define CLAMP(x, low, high) ({ \
|
||||
#define SAFETY_CLAMP(x, low, high) ({ \
|
||||
__typeof__(x) __x = (x); \
|
||||
__typeof__(low) __low = (low);\
|
||||
__typeof__(high) __high = (high);\
|
||||
@@ -23,29 +23,25 @@
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define ABS(a) ({ \
|
||||
#define SAFETY_ABS(a) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
(_a > 0) ? _a : (-_a); \
|
||||
})
|
||||
|
||||
// STM32 HAL defines this in production; define here for tests if missing
|
||||
#ifndef UNUSED
|
||||
#define UNUSED(x) ((void)(x))
|
||||
#endif
|
||||
#define SAFETY_UNUSED(x) ((void)(x))
|
||||
|
||||
// compute the time elapsed (in microseconds) from 2 counter samples
|
||||
// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
|
||||
static inline uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
|
||||
static inline uint32_t safety_get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
|
||||
return ts - ts_last;
|
||||
}
|
||||
|
||||
static bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
|
||||
static bool safety_max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
|
||||
return (val > MAX_VAL) || (val < MIN_VAL);
|
||||
}
|
||||
|
||||
// interp function that holds extreme values
|
||||
static float interpolate(struct lookup_t xy, float x) {
|
||||
|
||||
static float safety_interpolate(struct lookup_t xy, float x) {
|
||||
int size = sizeof(xy.x) / sizeof(xy.x[0]);
|
||||
float ret = xy.y[size - 1]; // default output is last point
|
||||
|
||||
@@ -62,7 +58,7 @@ static float interpolate(struct lookup_t xy, float x) {
|
||||
float dx = xy.x[i+1] - x0;
|
||||
float dy = xy.y[i+1] - y0;
|
||||
// dx should not be zero as xy.x is supposed to be monotonic
|
||||
dx = MAX(dx, 0.0001);
|
||||
dx = SAFETY_MAX(dx, 0.0001);
|
||||
ret = (dy * (x - x0) / dx) + y0;
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// ISO 11270
|
||||
static const float ISO_LATERAL_ACCEL = 3.0; // m/s^2
|
||||
@@ -11,15 +11,15 @@ static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
|
||||
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
|
||||
|
||||
// *** val rate limit check ***
|
||||
int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
|
||||
int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;
|
||||
|
||||
// if we've exceeded the meas val, we must start moving toward 0
|
||||
int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, MAX(val_meas->max, 0) + MAX_ERROR));
|
||||
int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR));
|
||||
int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN, SAFETY_MAX(val_meas->max, 0) + MAX_ERROR));
|
||||
int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN, SAFETY_MIN(val_meas->min, 0) - MAX_ERROR));
|
||||
|
||||
// check for violation
|
||||
return max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
return safety_max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
}
|
||||
|
||||
// check that commanded value isn't fighting against driver
|
||||
@@ -28,32 +28,32 @@ static bool driver_limit_check(int val, int val_last, const struct sample_t *val
|
||||
const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {
|
||||
|
||||
// torque delta/rate limits
|
||||
int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
|
||||
int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;
|
||||
|
||||
// driver
|
||||
int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
|
||||
int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
|
||||
|
||||
// if we've exceeded the applied torque, we must start moving toward 0
|
||||
int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN,
|
||||
MAX(driver_max_limit, 0)));
|
||||
int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN,
|
||||
MIN(driver_min_limit, 0)));
|
||||
int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN,
|
||||
SAFETY_MAX(driver_max_limit, 0)));
|
||||
int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN,
|
||||
SAFETY_MIN(driver_min_limit, 0)));
|
||||
|
||||
// check for violation
|
||||
return max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
return safety_max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
}
|
||||
|
||||
// real time check, mainly used for steer torque rate limiter
|
||||
static bool rt_torque_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
int highest_val = MAX(val_last, 0) + MAX_RT_DELTA;
|
||||
int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA;
|
||||
int highest_val = SAFETY_MAX(val_last, 0) + MAX_RT_DELTA;
|
||||
int lowest_val = SAFETY_MIN(val_last, 0) - MAX_RT_DELTA;
|
||||
|
||||
// check for violation
|
||||
return max_limit_check(val, highest_val, lowest_val);
|
||||
return safety_max_limit_check(val, highest_val, lowest_val);
|
||||
}
|
||||
|
||||
// Safety checks for torque-based steering commands
|
||||
@@ -66,12 +66,12 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
int max_torque = limits.max_torque;
|
||||
if (limits.dynamic_max_torque) {
|
||||
const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
|
||||
max_torque = interpolate(limits.max_torque_lookup, fudged_speed) + 1;
|
||||
max_torque = CLAMP(max_torque, -limits.max_torque, limits.max_torque);
|
||||
max_torque = safety_interpolate(limits.max_torque_lookup, fudged_speed) + 1;
|
||||
max_torque = SAFETY_CLAMP(max_torque, -limits.max_torque, limits.max_torque);
|
||||
}
|
||||
|
||||
// *** global torque limit check ***
|
||||
violation |= max_limit_check(desired_torque, max_torque, -max_torque);
|
||||
violation |= safety_max_limit_check(desired_torque, max_torque, -max_torque);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
if (limits.type == TorqueDriverLimited) {
|
||||
@@ -88,7 +88,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
violation |= rt_torque_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last);
|
||||
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_torque_check_last);
|
||||
if (ts_elapsed > MAX_RT_INTERVAL) {
|
||||
rt_torque_last = desired_torque;
|
||||
ts_torque_check_last = ts;
|
||||
@@ -117,7 +117,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
}
|
||||
|
||||
// or we've cut torque too recently in time
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last);
|
||||
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_steer_req_mismatch_last);
|
||||
if (ts_elapsed < limits.min_valid_request_rt_interval) {
|
||||
violation = true;
|
||||
}
|
||||
@@ -130,9 +130,9 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
|
||||
valid_steer_req_count = 0;
|
||||
ts_steer_req_mismatch_last = ts;
|
||||
invalid_steer_req_count = MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
|
||||
invalid_steer_req_count = SAFETY_MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
|
||||
} else {
|
||||
valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
|
||||
valid_steer_req_count = SAFETY_MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
|
||||
invalid_steer_req_count = 0;
|
||||
}
|
||||
}
|
||||
@@ -163,7 +163,7 @@ static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) {
|
||||
rt_angle_msgs += 1U;
|
||||
|
||||
// every RT_INTERVAL reset message counter
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_check_last);
|
||||
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_angle_check_last);
|
||||
if (ts_elapsed >= MAX_RT_INTERVAL) {
|
||||
rt_angle_msgs = 0;
|
||||
ts_angle_check_last = ts;
|
||||
@@ -182,8 +182,8 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
// always slightly above openpilot's in case we read an updated speed in between angle commands
|
||||
// TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset
|
||||
const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
|
||||
int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
int delta_angle_up = (safety_interpolate(limits.angle_rate_up_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
int delta_angle_down = (safety_interpolate(limits.angle_rate_down_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
|
||||
// allow down limits at zero since small floats from openpilot will be rounded to 0
|
||||
// TODO: openpilot should be cognizant of this and not send small floats
|
||||
@@ -195,8 +195,8 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
if (limits.enforce_angle_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) {
|
||||
// flipped fudge to avoid false positives
|
||||
const float fudged_speed_error = (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.;
|
||||
const int delta_angle_up_relaxed = (interpolate(limits.angle_rate_up_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
const int delta_angle_down_relaxed = (interpolate(limits.angle_rate_down_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
const int delta_angle_up_relaxed = (safety_interpolate(limits.angle_rate_up_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
const int delta_angle_down_relaxed = (safety_interpolate(limits.angle_rate_down_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
|
||||
// the minimum and maximum angle allowed based on the measured angle
|
||||
const int lowest_desired_angle_error = angle_meas.min - limits.max_angle_error - 1;
|
||||
@@ -205,22 +205,22 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
// the MAX is to allow the desired angle to hit the edge of the bounds and not require going under it
|
||||
if (desired_angle_last > highest_desired_angle_error) {
|
||||
const int delta = (desired_angle_last >= 0) ? delta_angle_down_relaxed : delta_angle_up_relaxed;
|
||||
highest_desired_angle = MAX(desired_angle_last - delta, highest_desired_angle_error);
|
||||
highest_desired_angle = SAFETY_MAX(desired_angle_last - delta, highest_desired_angle_error);
|
||||
|
||||
} else if (desired_angle_last < lowest_desired_angle_error) {
|
||||
const int delta = (desired_angle_last <= 0) ? delta_angle_down_relaxed : delta_angle_up_relaxed;
|
||||
lowest_desired_angle = MIN(desired_angle_last + delta, lowest_desired_angle_error);
|
||||
lowest_desired_angle = SAFETY_MIN(desired_angle_last + delta, lowest_desired_angle_error);
|
||||
|
||||
} else {
|
||||
// already inside error boundary, don't allow commanding outside it
|
||||
highest_desired_angle = MIN(highest_desired_angle, highest_desired_angle_error);
|
||||
lowest_desired_angle = MAX(lowest_desired_angle, lowest_desired_angle_error);
|
||||
highest_desired_angle = SAFETY_MIN(highest_desired_angle, highest_desired_angle_error);
|
||||
lowest_desired_angle = SAFETY_MAX(lowest_desired_angle, lowest_desired_angle_error);
|
||||
}
|
||||
|
||||
// don't enforce above the max steer
|
||||
// TODO: this should always be done
|
||||
lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
lowest_desired_angle = SAFETY_CLAMP(lowest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
highest_desired_angle = SAFETY_CLAMP(highest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
}
|
||||
|
||||
// check not above ISO 11270 lateral accel assuming worst case road roll
|
||||
@@ -230,23 +230,23 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL); // ~2.4 m/s^2
|
||||
|
||||
// Allow small tolerance by using minimum speed and rounding curvature up
|
||||
const float speed_lower = MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const float speed_upper = MAX(vehicle_speed.max / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const float speed_lower = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const float speed_upper = SAFETY_MAX(vehicle_speed.max / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const int max_curvature_upper = (MAX_LATERAL_ACCEL / (speed_lower * speed_lower) * limits.angle_deg_to_can) + 1.;
|
||||
const int max_curvature_lower = (MAX_LATERAL_ACCEL / (speed_upper * speed_upper) * limits.angle_deg_to_can) - 1.;
|
||||
|
||||
// ensure that the curvature error doesn't try to enforce above this limit
|
||||
if (desired_angle_last > 0) {
|
||||
lowest_desired_angle = CLAMP(lowest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
highest_desired_angle = CLAMP(highest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
lowest_desired_angle = SAFETY_CLAMP(lowest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
highest_desired_angle = SAFETY_CLAMP(highest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
} else {
|
||||
lowest_desired_angle = CLAMP(lowest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
highest_desired_angle = CLAMP(highest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
lowest_desired_angle = SAFETY_CLAMP(lowest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
highest_desired_angle = SAFETY_CLAMP(highest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
}
|
||||
}
|
||||
|
||||
// check for violation;
|
||||
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
}
|
||||
desired_angle_last = desired_angle;
|
||||
|
||||
@@ -255,9 +255,9 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
if (limits.inactive_angle_is_zero) {
|
||||
violation |= desired_angle != 0;
|
||||
} else {
|
||||
const int max_inactive_angle = CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -271,7 +271,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
if (limits.inactive_angle_is_zero) {
|
||||
desired_angle_last = 0;
|
||||
} else {
|
||||
desired_angle_last = CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -299,7 +299,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
// Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
|
||||
static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^3
|
||||
|
||||
const float fudged_speed = MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
|
||||
const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
|
||||
const float curvature_factor = get_curvature_factor(fudged_speed, params);
|
||||
|
||||
bool violation = false;
|
||||
@@ -318,14 +318,14 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
const int highest_desired_angle = desired_angle_last + max_angle_delta_can;
|
||||
const int lowest_desired_angle = desired_angle_last - max_angle_delta_can;
|
||||
|
||||
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
|
||||
// *** ISO lateral accel limit ***
|
||||
const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
|
||||
const float max_angle = get_angle_from_curvature(max_curvature, curvature_factor, params);
|
||||
const int max_angle_can = (max_angle * limits.angle_deg_to_can) + 1.;
|
||||
|
||||
violation |= max_limit_check(desired_angle, max_angle_can, -max_angle_can);
|
||||
violation |= safety_max_limit_check(desired_angle, max_angle_can, -max_angle_can);
|
||||
|
||||
// *** angle real time rate limit check ***
|
||||
violation |= rt_angle_rate_limit_check(limits);
|
||||
@@ -334,9 +334,9 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
|
||||
// Angle should either be 0 or same as current angle while not steering
|
||||
if (!steer_control_enabled) {
|
||||
const int max_inactive_angle = CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
}
|
||||
|
||||
// No angle control allowed when controls are not allowed
|
||||
@@ -346,7 +346,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
|
||||
// reset to current angle if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
desired_angle_last = CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
}
|
||||
|
||||
return violation;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
bool get_longitudinal_allowed(void) {
|
||||
return controls_allowed && !gas_pressed_prev;
|
||||
@@ -6,7 +6,7 @@ bool get_longitudinal_allowed(void) {
|
||||
|
||||
// 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_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
|
||||
bool accel_inactive = desired_accel == limits.inactive_accel;
|
||||
return !(accel_valid || accel_inactive);
|
||||
}
|
||||
@@ -16,13 +16,13 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit
|
||||
}
|
||||
|
||||
bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits) {
|
||||
bool transmission_rpm_valid = get_longitudinal_allowed() && !max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
|
||||
bool transmission_rpm_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
|
||||
bool transmission_rpm_inactive = desired_transmission_rpm == limits.inactive_transmission_rpm;
|
||||
return !(transmission_rpm_valid || transmission_rpm_inactive);
|
||||
}
|
||||
|
||||
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_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
|
||||
bool gas_inactive = desired_gas == limits.inactive_gas;
|
||||
return !(gas_valid || gas_inactive);
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static void body_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->addr == 0x201U) {
|
||||
@@ -32,7 +32,7 @@ static safety_config body_init(uint16_t param) {
|
||||
static const CanMsg BODY_TX_MSGS[] = {{0x250, 0, 8, .check_relay = false}, {0x250, 0, 6, .check_relay = false}, {0x251, 0, 5, .check_relay = false}, // body
|
||||
{0x1, 0, 8, .check_relay = false}}; // CAN flasher
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
safety_config ret = BUILD_SAFETY_CFG(body_rx_checks, BODY_TX_MSGS);
|
||||
ret.disable_forwarding = true;
|
||||
return ret;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
typedef struct {
|
||||
const unsigned int EPS_2;
|
||||
|
||||
@@ -1,25 +1,25 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// GCOV_EXCL_START
|
||||
// Unreachable by design (doesn't define any rx msgs)
|
||||
void default_rx_hook(const CANPacket_t *msg) {
|
||||
UNUSED(msg);
|
||||
SAFETY_UNUSED(msg);
|
||||
}
|
||||
// GCOV_EXCL_STOP
|
||||
|
||||
// *** no output safety mode ***
|
||||
|
||||
static safety_config nooutput_init(uint16_t param) {
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
return (safety_config){NULL, 0, NULL, 0, true}; // NOLINT(readability/braces)
|
||||
}
|
||||
|
||||
// GCOV_EXCL_START
|
||||
// Unreachable by design (doesn't define any tx msgs)
|
||||
static bool nooutput_tx_hook(const CANPacket_t *msg) {
|
||||
UNUSED(msg);
|
||||
SAFETY_UNUSED(msg);
|
||||
return false;
|
||||
}
|
||||
// GCOV_EXCL_STOP
|
||||
@@ -40,7 +40,7 @@ static safety_config alloutput_init(uint16_t param) {
|
||||
}
|
||||
|
||||
static bool alloutput_tx_hook(const CANPacket_t *msg) {
|
||||
UNUSED(msg);
|
||||
SAFETY_UNUSED(msg);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/defaults.h"
|
||||
|
||||
static bool elm327_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Safety-relevant CAN messages for Ford vehicles.
|
||||
#define FORD_EngBrakeData 0x165U // RX from PCM, for driver brake pedal and cruise state
|
||||
@@ -137,7 +137,7 @@ static void ford_rx_hook(const CANPacket_t *msg) {
|
||||
// Signal: VehYaw_W_Actl
|
||||
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
|
||||
float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5;
|
||||
float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
|
||||
float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
|
||||
// convert current curvature into units on CAN for comparison with desired curvature
|
||||
update_sample(&angle_meas, ROUND(current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can));
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
|
||||
#define GM_COMMON_RX_CHECKS \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// All common address checks except SCM_BUTTONS which isn't on one Nidec safety configuration
|
||||
#define HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
#define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
#define HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(bus) \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
extern uint16_t hyundai_canfd_crc_lut[256];
|
||||
uint16_t hyundai_canfd_crc_lut[256];
|
||||
@@ -92,7 +92,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
|
||||
if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || main_button) {
|
||||
hyundai_last_button_interaction = 0U;
|
||||
} else {
|
||||
hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
|
||||
hyundai_last_button_interaction = SAFETY_MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
|
||||
}
|
||||
|
||||
if (hyundai_longitudinal) {
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// CAN msgs we care about
|
||||
#define MAZDA_LKAS 0x243U
|
||||
@@ -94,7 +94,7 @@ static safety_config mazda_init(uint16_t param) {
|
||||
{.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static bool nissan_alt_eps = false;
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define PSA_STEERING 757U // RX from XXX, driver torque
|
||||
#define PSA_STEERING_ALT 773U // RX from EPS, steering angle
|
||||
@@ -124,7 +124,7 @@ static bool psa_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
static safety_config psa_init(uint16_t param) {
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
static const CanMsg PSA_TX_MSGS[] = {
|
||||
{PSA_LANE_KEEP_ASSIST, PSA_MAIN_BUS, 8, .check_relay = true}, // EPS steering
|
||||
};
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static uint8_t rivian_get_counter(const CANPacket_t *msg) {
|
||||
uint8_t cnt = 0;
|
||||
@@ -167,7 +167,7 @@ static safety_config rivian_init(uint16_t param) {
|
||||
|
||||
bool rivian_longitudinal = false;
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const int FLAG_RIVIAN_LONG_CONTROL = 1;
|
||||
rivian_longitudinal = GET_FLAG(param, FLAG_RIVIAN_LONG_CONTROL);
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \
|
||||
{ \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Preglobal platform
|
||||
// 0x161 is ES_CruiseThrottle
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static bool tesla_longitudinal = false;
|
||||
static bool tesla_stock_aeb = false;
|
||||
@@ -328,7 +328,7 @@ static safety_config tesla_init(uint16_t param) {
|
||||
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
|
||||
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_BASE_TX_MSGS \
|
||||
@@ -271,12 +271,12 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
// check if we should wind down torque
|
||||
int driver_torque = MIN(ABS(torque_driver.min), ABS(torque_driver.max));
|
||||
int driver_torque = SAFETY_MIN(SAFETY_ABS(torque_driver.min), SAFETY_ABS(torque_driver.max));
|
||||
if ((driver_torque > TOYOTA_LTA_MAX_DRIVER_TORQUE) && (torque_wind_down != 0)) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
int eps_torque = MIN(ABS(torque_meas.min), ABS(torque_meas.max));
|
||||
int eps_torque = SAFETY_MIN(SAFETY_ABS(torque_meas.min), SAFETY_ABS(torque_meas.max));
|
||||
if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
static bool volkswagen_mqb_brake_pedal_switch = false;
|
||||
@@ -26,7 +26,7 @@ static safety_config volkswagen_mqb_init(uint16_t param) {
|
||||
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
|
||||
volkswagen_set_button_prev = false;
|
||||
volkswagen_resume_button_prev = false;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
#define MSG_LENKHILFE_3 0x0D0U // RX from EPS, for steering angle and driver steering torque
|
||||
@@ -63,7 +63,7 @@ static safety_config volkswagen_pq_init(uint16_t param) {
|
||||
{.msg = {{MSG_GRA_NEU, 0, 4, 30U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
|
||||
volkswagen_set_button_prev = false;
|
||||
volkswagen_resume_button_prev = false;
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
#include "opendbc/safety/helpers.h"
|
||||
#include "opendbc/safety/lateral.h"
|
||||
#include "opendbc/safety/longitudinal.h"
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/board/can.h"
|
||||
|
||||
// all the safety modes
|
||||
@@ -142,7 +142,7 @@ static void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
|
||||
if (index != -1) {
|
||||
uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U);
|
||||
addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1;
|
||||
addr_list[index].status.wrong_counters = CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
|
||||
addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
|
||||
addr_list[index].status.last_counter = counter;
|
||||
}
|
||||
}
|
||||
@@ -313,12 +313,12 @@ void safety_tick(const safety_config *cfg) {
|
||||
uint32_t ts = microsecond_timer_get();
|
||||
if (cfg != NULL) {
|
||||
for (int i=0; i < cfg->rx_checks_len; i++) {
|
||||
uint32_t elapsed_time = get_ts_elapsed(ts, cfg->rx_checks[i].status.last_timestamp);
|
||||
uint32_t elapsed_time = safety_get_ts_elapsed(ts, cfg->rx_checks[i].status.last_timestamp);
|
||||
// lag threshold is max of: 1s and MAX_MISSED_MSGS * expected timestep.
|
||||
// Quite conservative to not risk false triggers.
|
||||
// 2s of lag is worse case, since the function is called at 1Hz
|
||||
uint32_t timestep = 1e6 / cfg->rx_checks[i].msg[cfg->rx_checks[i].status.index].frequency;
|
||||
bool lagging = elapsed_time > MAX(timestep * MAX_MISSED_MSGS, 1e6);
|
||||
bool lagging = elapsed_time > SAFETY_MAX(timestep * MAX_MISSED_MSGS, 1e6);
|
||||
cfg->rx_checks[i].status.lagging = lagging;
|
||||
if (lagging) {
|
||||
controls_allowed = false;
|
||||
@@ -479,9 +479,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
|
||||
// convert a trimmed integer to signed 32 bit int
|
||||
int to_signed(int d, int bits) {
|
||||
int d_signed = d;
|
||||
int max_value = (1 << MAX((bits - 1), 0));
|
||||
int max_value = (1 << SAFETY_MAX((bits - 1), 0));
|
||||
if (d >= max_value) {
|
||||
d_signed = d - (1 << MAX(bits, 0));
|
||||
d_signed = d - (1 << SAFETY_MAX(bits, 0));
|
||||
}
|
||||
return d_signed;
|
||||
}
|
||||
@@ -525,7 +525,7 @@ void speed_mismatch_check(const float speed_2) {
|
||||
// Disable controls if speeds from two sources are too far apart.
|
||||
// For safety modes that use speed to adjust torque or angle limits
|
||||
const float MAX_SPEED_DELTA = 2.0; // m/s
|
||||
bool is_invalid_speed = ABS(speed_2 - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > MAX_SPEED_DELTA;
|
||||
bool is_invalid_speed = SAFETY_ABS(speed_2 - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > MAX_SPEED_DELTA;
|
||||
if (is_invalid_speed) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
|
||||
@@ -3,10 +3,10 @@
|
||||
// this file is checked by cppcheck
|
||||
|
||||
// Ignore misra-c2012-8.7 as these functions are only called from libsafety
|
||||
UNUSED(heartbeat_engaged);
|
||||
SAFETY_UNUSED(heartbeat_engaged);
|
||||
|
||||
UNUSED(safety_rx_hook);
|
||||
UNUSED(safety_tx_hook);
|
||||
UNUSED(safety_fwd_hook);
|
||||
UNUSED(safety_tick);
|
||||
UNUSED(set_safety_hooks);
|
||||
SAFETY_UNUSED(safety_rx_hook);
|
||||
SAFETY_UNUSED(safety_tx_hook);
|
||||
SAFETY_UNUSED(safety_fwd_hook);
|
||||
SAFETY_UNUSED(safety_tick);
|
||||
SAFETY_UNUSED(set_safety_hooks);
|
||||
|
||||
Reference in New Issue
Block a user