From 94cd9a0788070e4e802e8a6a1c1cc7dae95e8f70 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 4 May 2023 04:59:35 +0000 Subject: [PATCH] Ford: curvature rate limits (#1258) * ford curvature rate limits draft * make common angle tests work with curvature * comment * no need for this * extra line * fix test * generic curvature test class * more reasonable limits * adjust limits * draft * works * works * clean up * add vehicle speed * works * clean up * clean up * more clean up * more clean up * lower * double * add updated bp * remove * can clean that up * draft * this works! * think that's the correct placement * try this * closer * use min * add/sub one to not falsely trip * remove old angle error safety * i'm not sure if clamp is more readable * fix that * fix * stash * fix these tests * ternary * floats are a pain * draft, works kinda * even better * round that * need tolerance * this should work (adding tol=1 wouldn't let us have multiple rate limits) * test works * clamp breaks if low is higher than high :((( down from 150 blocked msgs to 7! * no blocked msgs!!! * test a whole bunch * stash * stash * clean up test * clean up test to be more like torque (+ speeds) * clean up * cmt * test up * up and down are good * rename and remove * this is tested * uncomment * this is tested by ensuring we move towards error at a minimum rate * not used any more * revert common * clean up test_ford a bit more * some clean up (combine variables where it makes sense) * yeah can't use clamp since min isn't always < max, min(max(.. handles this * clean up * revert that * comments * cmt * another * that's old * misra! * Update board/safety/safety_ford.h * Update board/safety/safety_ford.h * add todo, fix test case * more clear, matches panda * add comment * Update tests/safety/test_ford.py * more fine speed increments * rm comment * better names * this is expected behavior (tested by common checks) * CURVATURE_ERROR_LIMIT_SPEED * better name? * pretty clean! * same for up * only used in one place now * these are now clear * common term * make vehicle_speed a sample_t * need to use values[0] * speed is a float * Revert "speed is a float" This reverts commit 01af02f1d35026d983cc2ba6d9ba364c87c800ee. * Revert "need to use values[0]" This reverts commit 8f6d68345a92f7285d07ca071285e903ed7871bb. * Revert "make vehicle_speed a sample_t" This reverts commit ecd8dc86b6b97cc8bff7da697353a8d90c358b12. * safety fixes for new speed sample * test fixes for new speed sample * fix misra and make intermediate variable * this isn't needed --- board/safety.h | 34 ++++++++++------- board/safety/safety_ford.h | 9 ++++- board/safety_declarations.h | 1 - tests/safety/test_ford.py | 74 +++++++++++++++++++++++++++---------- 4 files changed, 83 insertions(+), 35 deletions(-) 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)