diff --git a/board/safety.h b/board/safety.h index 753fbf8b..426800cc 100644 --- a/board/safety.h +++ b/board/safety.h @@ -616,7 +616,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) { bool violation = false; - if (!limits.disable_angle_rate_limits && controls_allowed && steer_control_enabled) { + if (controls_allowed && steer_control_enabled) { // convert floating point angle rate limits to integers in the scale of the desired angle on CAN, // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are // always slightly above openpilot's in case we read an updated speed in between angle commands @@ -628,24 +628,30 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); + // check that commanded angle value isn't too far from measured, used to limit torque for some safety modes + // ensure we start moving in direction of meas while respecting rate limits if error is exceeded + if (limits.enforce_angle_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) { + // the rate limits above are liberally above openpilot's to avoid false positives. + // likewise, allow a lower rate for moving towards meas when error is exceeded + int delta_angle_up_lower = interpolate(limits.angle_rate_up_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can; + int delta_angle_down_lower = interpolate(limits.angle_rate_down_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can; + + int highest_desired_angle_lower = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up_lower : delta_angle_down_lower); + int lowest_desired_angle_lower = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down_lower : delta_angle_up_lower); + + lowest_desired_angle = MIN(MAX(lowest_desired_angle, angle_meas.min - limits.max_angle_error - 1), highest_desired_angle_lower); + highest_desired_angle = MAX(MIN(highest_desired_angle, angle_meas.max + limits.max_angle_error + 1), lowest_desired_angle_lower); + + // don't enforce above the max steer + lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_steer, limits.max_steer); + highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_steer, limits.max_steer); + } + // check for violation; violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); } desired_angle_last = desired_angle; - // check that commanded angle value isn't too far from measured, used to limit torque for some safety modes - if (limits.enforce_angle_error && controls_allowed && steer_control_enabled) { - if ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed) { - // val must always be near angle_meas, limited to the maximum value - // add 1 to not false trigger the violation - int highest_allowed = CLAMP(angle_meas.max + limits.max_angle_error + 1, -limits.max_steer, limits.max_steer); - int lowest_allowed = CLAMP(angle_meas.min - limits.max_angle_error - 1, -limits.max_steer, limits.max_steer); - - // check for violation - violation |= max_limit_check(desired_angle, highest_allowed, lowest_allowed); - } - } - // 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) : diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 0fbbf9cb..867b335d 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -138,11 +138,18 @@ const SteeringLimits FORD_STEERING_LIMITS = { .max_steer = 1000, .angle_deg_to_can = 50000, // 1 / (2e-5) rad to can .max_angle_error = 100, // 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can + .angle_rate_up_lookup = { + {5., 25., 25.}, + {0.0002, 0.0001, 0.0001} + }, + .angle_rate_down_lookup = { + {5., 25., 25.}, + {0.000225, 0.00015, 0.00015} + }, // no blending at low speed due to lack of torque wind-up and inaccurate current curvature .angle_error_min_speed = 10.0, // m/s - .disable_angle_rate_limits = true, .enforce_angle_error = true, .inactive_angle_is_zero = true, }; diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 4de886c6..a35f653f 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -73,7 +73,6 @@ typedef struct { const int max_angle_error; // used to limit error between meas and cmd while enabled const float angle_error_min_speed; // minimum speed to start limiting angle error - const bool disable_angle_rate_limits; const bool enforce_angle_error; // enables max_angle_error check const bool inactive_angle_is_zero; // if false, enforces angle near meas when disabled (default) } SteeringLimits; diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index 798210b8..828d7b88 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -77,6 +77,10 @@ class TestFordSafety(common.PandaSafetyTest): MAX_CURVATURE_ERROR = 0.002 CURVATURE_ERROR_MIN_SPEED = 10.0 # m/s + ANGLE_RATE_BP = [5., 25., 25.] + ANGLE_RATE_UP = [0.0002, 0.0001, 0.0001] # windup limit + ANGLE_RATE_DOWN = [0.000225, 0.00015, 0.00015] # unwind limit + cnt_speed = 0 cnt_speed_2 = 0 cnt_yaw_rate = 0 @@ -87,9 +91,13 @@ class TestFordSafety(common.PandaSafetyTest): self.safety.set_safety_hooks(Panda.SAFETY_FORD, 0) self.safety.init_tests() + def _set_prev_desired_angle(self, t): + t = round(t * self.DEG_TO_CAN) + self.safety.set_desired_angle_last(t) + def _reset_curvature_measurement(self, curvature, speed): - self._rx(self._speed_msg(speed)) for _ in range(6): + self._rx(self._speed_msg(speed)) self._rx(self._yaw_rate_msg(curvature, speed)) # Driver brake pedal @@ -240,6 +248,7 @@ class TestFordSafety(common.PandaSafetyTest): for curvature_rate in curvature_rates: for curvature in curvatures: self.safety.set_controls_allowed(controls_allowed) + self._set_prev_desired_angle(curvature) self._reset_curvature_measurement(curvature, speed) should_tx = path_offset == 0 and path_angle == 0 and curvature_rate == 0 @@ -251,28 +260,55 @@ class TestFordSafety(common.PandaSafetyTest): curvature=curvature): self.assertEqual(should_tx, self._tx(self._tja_command_msg(steer_control_enabled, path_offset, path_angle, curvature, curvature_rate))) - def test_steer_meas_delta(self): - """This safety model enforces a maximum distance from measured and commanded curvature, only above a certain speed""" - self.safety.set_controls_allowed(1) + def test_curvature_rate_limit_up(self): + """ + When the curvature error is exceeded, commanded curvature must start moving towards meas respecting rate limits. + Since panda allows higher rate limits to avoid false positives, we need to allow a lower rate to move towards meas. + """ + self.safety.set_controls_allowed(True) + small_curvature = 2 / self.DEG_TO_CAN # significant small amount of curvature to cross boundary - for steer_control_enabled in (True, False): - for speed in np.linspace(0, 50, 11): - for initial_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 21): - self._reset_curvature_measurement(initial_curvature, speed) + for speed in np.arange(0, 40, 0.5): + limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED + max_delta_up = np.interp(speed, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP) + max_delta_up_lower = np.interp(speed + 1, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP) - limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED - for new_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 41): - too_far_away = round(abs(new_curvature - initial_curvature), 5) > self.MAX_CURVATURE_ERROR + cases = [ + (not limit_command, 0), + (not limit_command, max_delta_up_lower - small_curvature), + (True, max_delta_up_lower), + (True, max_delta_up), + (False, max_delta_up + small_curvature), + ] - if steer_control_enabled: - should_tx = not limit_command or not too_far_away - else: - # enforce angle error limit is disabled when steer request bit is 0 - should_tx = new_curvature == 0 + for sign in (-1, 1): + self._reset_curvature_measurement(sign * (self.MAX_CURVATURE_ERROR + 1e-3), speed) + for should_tx, curvature in cases: + self._set_prev_desired_angle(sign * small_curvature) + self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * (small_curvature + curvature), 0))) - with self.subTest(steer_control_enabled=steer_control_enabled, speed=speed, - initial_curvature=initial_curvature, new_curvature=new_curvature): - self.assertEqual(should_tx, self._tx(self._tja_command_msg(steer_control_enabled, 0, 0, new_curvature, 0))) + def test_curvature_rate_limit_down(self): + self.safety.set_controls_allowed(True) + small_curvature = 2 / self.DEG_TO_CAN # significant small amount of curvature to cross boundary + + for speed in np.arange(0, 40, 0.5): + limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED + max_delta_down = np.interp(speed, self.ANGLE_RATE_BP, self.ANGLE_RATE_DOWN) + max_delta_down_lower = np.interp(speed + 1, self.ANGLE_RATE_BP, self.ANGLE_RATE_DOWN) + + cases = [ + (not limit_command, self.MAX_CURVATURE), + (not limit_command, self.MAX_CURVATURE - max_delta_down_lower + small_curvature), + (True, self.MAX_CURVATURE - max_delta_down_lower), + (True, self.MAX_CURVATURE - max_delta_down), + (False, self.MAX_CURVATURE - max_delta_down - small_curvature), + ] + + for sign in (-1, 1): + self._reset_curvature_measurement(sign * (self.MAX_CURVATURE - self.MAX_CURVATURE_ERROR - 1e-3), speed) + for should_tx, curvature in cases: + self._set_prev_desired_angle(sign * self.MAX_CURVATURE) + self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, sign * curvature, 0))) def test_prevent_lkas_action(self): self.safety.set_controls_allowed(1)