mawei
/
panda
mirror of https://github.com/commaai/panda.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ford safety: curvature error limit (#1353) 

* set ford vehicle speed

* parse yaw rate signals

* misra

* misra

* misra

* misra

* draft

* update module

* already checked

* and set it properly

* some stuff

* draft

* clean up (will fail tests because we don't send yaw rate yet)

* could do something like this

* this is better and less prone to bugs

* match simple op limiting, debugging

* set checksum for messages in tests

* clean up

* fix that

* one m/s fudge

* fix sign of yaw rate

* interpolate detects size

* forgot OP flips the curvature sign. it matches yaw on can

* all my debugging crap

* make replay work for ford

* fix panda blocking messages (array is fixed size so size-1 is 0 rate at high speed)

* uncomment safety test limits

* revert

* round for zero blocked msgs

* fix limits

* meas safety checks that down rate is >=, not <

* test pass

* lots of comments and draft what one meas torque check would look like

* fix that

* add curvature meas

* add debugging stuff

* Revert "add debugging stuff"

This reverts commit 449783fc62.

* messy but at least one test passes now

* draft

* add max_steer

* some safety clean up

* and that

* start with a test that works

* another test that works (sort of, we need more strict panda safety without false positives)

* no max curvature check (not safety related), allow any rate limits

* add new function

* also need to consider max val here, since OP will send up to that

* and now use the function

* lower to 10

* compilation fixes

* clean up (no rate limiting)

* remove that too

* curvature diff test

* more clean up

* debug

* ?

* better names

* more official

* use _curvature_meas_msg_array here

* bit faster

* no i don't

* revert that

* why not just use angle_meas?

* bb ll

* bb deb

* clean up debug vals

* more

* revert replay drive debugging changes

* Update board/safety.h

* rm line

* only need to round the final thing

* not needed, under 10 ms

* make a class variable

* fix a misra?

* another misra?

better

* ?

* 12.1

* need to explicitly convert

* add one to not false trigger the violation (float rounding)

* not really needed

* rm line

* cmt

* use clamp

* rename

* in struct

* comment

* use max_limit_check

* draft clean up

* Revert "draft clean up"

This reverts commit d1a0e8acd1.

* make a global

make a global

* this is fine
This commit is contained in:
Shane Smiskol 2023-04-27 00:40:29 -07:00 committed by GitHub
parent 4269b74a84
commit c9c3cb38f6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 87 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
board/safety.h
View File

@ -411,7 +411,19 @@ bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
return (val > MAX_VAL) || (val < MIN_VAL);
}
// check that commanded value isn't too far from measured
// check that commanded angle value isn't too far from measured
bool angle_dist_to_meas_check(int val, struct sample_t *val_meas, const int MAX_ERROR, const int MAX_VAL) {
// val must always be near val_meas, limited to the maximum value
// add 1 to not false trigger the violation
int highest_allowed = CLAMP(val_meas->max + MAX_ERROR + 1, -MAX_VAL, MAX_VAL);
int lowest_allowed = CLAMP(val_meas->min - MAX_ERROR - 1, -MAX_VAL, MAX_VAL);
// check for violation
return max_limit_check(val, highest_allowed, lowest_allowed);
}
// check that commanded torque value isn't too far from measured
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) {

49
board/safety/safety_ford.h
View File

@ -124,6 +124,7 @@ static bool ford_get_quality_flag_valid(CANPacket_t *to_push) {
#define INACTIVE_PATH_OFFSET 512U
#define INACTIVE_PATH_ANGLE 1000U
#define FORD_MAX_SPEED_DELTA 2.0 // m/s
#define FORD_CURVATURE_DELTA_LIMIT_SPEED 10.0 // m/s
static bool ford_lkas_msg_check(int addr) {
return (addr == MSG_ACCDATA_3)
@ -132,6 +133,13 @@ static bool ford_lkas_msg_check(int addr) {
|| (addr == MSG_IPMA_Data);
}
// Curvature rate limits
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
};
static int ford_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &ford_rx_checks,
ford_get_checksum, ford_compute_checksum, ford_get_counter, ford_get_quality_flag_valid);
@ -151,6 +159,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
vehicle_speed = ((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6;
}
// Check vehicle speed against a second source
if (addr == MSG_EngVehicleSpThrottle2) {
// Disable controls if speeds from ABS and PCM ECUs are too far apart.
// Signal: Veh_V_ActlEng
@ -160,6 +169,17 @@ static int ford_rx_hook(CANPacket_t *to_push) {
}
}
// Update vehicle yaw rate
if (addr == MSG_Yaw_Data_FD1) {
// Signal: VehYaw_W_Actl
float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / MAX(vehicle_speed, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
int current_curvature_can = current_curvature * (float)FORD_STEERING_LIMITS.angle_deg_to_can +
((current_curvature > 0.) ? 0.5 : -0.5);
update_sample(&angle_meas, current_curvature_can);
}
// Update gas pedal
if (addr == MSG_EngVehicleSpThrottle) {
// Pedal position: (0.1 * val) in percent
@ -225,20 +245,29 @@ static int ford_tx_hook(CANPacket_t *to_send) {
// Safety check for LateralMotionControl action
if (addr == MSG_LateralMotionControl) {
// Signal: LatCtl_D_Rq
unsigned int steer_control_type = (GET_BYTE(to_send, 4) >> 2) & 0x7U;
unsigned int curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5);
unsigned int curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2);
unsigned int path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5);
unsigned int path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6);
bool steer_control_enabled = ((GET_BYTE(to_send, 4) >> 2) & 0x7U) != 0U;
unsigned int raw_curvature = (GET_BYTE(to_send, 0) << 3) | (GET_BYTE(to_send, 1) >> 5);
unsigned int raw_curvature_rate = ((GET_BYTE(to_send, 1) & 0x1FU) << 8) | GET_BYTE(to_send, 2);
unsigned int raw_path_angle = (GET_BYTE(to_send, 3) << 3) | (GET_BYTE(to_send, 4) >> 5);
unsigned int raw_path_offset = (GET_BYTE(to_send, 5) << 2) | (GET_BYTE(to_send, 6) >> 6);
// These signals are not yet tested with the current safety limits
if ((curvature_rate != INACTIVE_CURVATURE_RATE) || (path_angle != INACTIVE_PATH_ANGLE) || (path_offset != INACTIVE_PATH_OFFSET)) {
tx = 0;
bool violation = (raw_curvature_rate != INACTIVE_CURVATURE_RATE) || (raw_path_angle != INACTIVE_PATH_ANGLE) || (raw_path_offset != INACTIVE_PATH_OFFSET);
int desired_curvature = raw_curvature - INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
if (controls_allowed) {
if (vehicle_speed > FORD_CURVATURE_DELTA_LIMIT_SPEED) {
violation |= angle_dist_to_meas_check(desired_curvature, &angle_meas,
FORD_STEERING_LIMITS.max_angle_error, FORD_STEERING_LIMITS.max_steer);
}
}
// No steer control allowed when controls are not allowed
bool steer_control_enabled = (steer_control_type != 0U) || (curvature != INACTIVE_CURVATURE);
if (!controls_allowed && steer_control_enabled) {
// No curvature command if controls is not allowed
if (!controls_allowed && ((desired_curvature != 0) || steer_control_enabled)) {
violation = true;
}
if (violation) {
tx = 0;
}
}

5
board/safety_declarations.h
View File

@ -68,6 +68,7 @@ typedef struct {
const int angle_deg_to_can;
const struct lookup_t angle_rate_up_lookup;
const struct lookup_t angle_rate_down_lookup;
const int max_angle_error; // used to limit error between meas and cmd
} SteeringLimits;
typedef struct {
@ -124,6 +125,8 @@ uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);
void update_sample(struct sample_t *sample, int sample_new);
bool max_limit_check(int val, const int MAX, const int MIN);
bool angle_dist_to_meas_check(int val, struct sample_t *val_meas,
const int MAX_ERROR, const int MAX_VAL);
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);
bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
@ -208,7 +211,7 @@ uint32_t heartbeat_engaged_mismatches = 0; // count of mismatches between heart
// for safety modes with angle steering control
uint32_t ts_angle_last = 0;
int desired_angle_last = 0;
struct sample_t angle_meas; // last 6 steer angles
struct sample_t angle_meas; // last 6 steer angles/curvatures
// This can be set with a USB command
// It enables features that allow alternative experiences, like not disengaging on gas press

31
tests/safety/test_ford.py
View File

@ -50,6 +50,11 @@ def checksum(msg):
return addr, t, ret, bus
def round_curvature_can(curvature):
# rounds curvature as if it was sent on CAN
return round(curvature * 5, 4) / 5
class Buttons:
CANCEL = 0
RESUME = 1
@ -68,8 +73,15 @@ class TestFordSafety(common.PandaSafetyTest):
FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]}
FWD_BUS_LOOKUP = {0: 2, 2: 0}
# Max allowed delta between car speeds
MAX_SPEED_DELTA = 2.0 # m/s
# Curvature control limits
DEG_TO_CAN = 50000 # 1 / (2e-5) rad to can
MAX_CURVATURE = 0.02
MAX_CURVATURE_DELTA = 0.002
CURVATURE_DELTA_LIMIT_SPEED = 10.0 # m/s
cnt_speed = 0
cnt_speed_2 = 0
cnt_yaw_rate = 0
@ -80,6 +92,11 @@ class TestFordSafety(common.PandaSafetyTest):
self.safety.set_safety_hooks(Panda.SAFETY_FORD, 0)
self.safety.init_tests()
def _reset_curvature_measurements(self, curvature, speed):
self._rx(self._speed_msg(speed))
for _ in range(6):
self._rx(self._yaw_rate_msg(curvature, speed))
# Driver brake pedal
def _user_brake_msg(self, brake: bool):
# brake pedal and cruise state share same message, so we have to send
@ -213,6 +230,20 @@ class TestFordSafety(common.PandaSafetyTest):
should_tx = should_tx and (not enabled or controls_allowed)
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)
for speed in np.linspace(0, 50, 11):
for initial_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 51):
self._reset_curvature_measurements(initial_curvature, speed)
limit_command = speed > self.CURVATURE_DELTA_LIMIT_SPEED
for new_curvature in np.linspace(-self.MAX_CURVATURE, self.MAX_CURVATURE, 51):
too_far_away = round_curvature_can(abs(new_curvature - initial_curvature)) > self.MAX_CURVATURE_DELTA
should_tx = not limit_command or not too_far_away
self.assertEqual(should_tx, self._tx(self._tja_command_msg(True, 0, 0, new_curvature, 0)))
def test_prevent_lkas_action(self):
self.safety.set_controls_allowed(1)
self.assertFalse(self._tx(self._lkas_command_msg(1)))