mawei/panda-meb
1
0
Fork 0
mirror of https://github.com/infiniteCable2/panda.git synced 2026-02-18 17:23:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford: curvature rate limits (#1258)

Browse Source 
* 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
This commit is contained in:
Shane Smiskol
2023-05-04 04:59:35 +00:00
committed by GitHub
parent b44df8151a
commit 94cd9a0788
4 changed files with 83 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
board/safety.h
View File

@@ -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) :

9
board/safety/safety_ford.h
View File

@@ -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,
};

1
board/safety_declarations.h
View File

@@ -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;

74
tests/safety/test_ford.py
View File

@@ -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)