diff --git a/board/safety.h b/board/safety.h index bfab19b77..c811e64fe 100644 --- a/board/safety.h +++ b/board/safety.h @@ -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) { diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 8e4800a5b..00ce4f848 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -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; } } diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 947ad7d22..1835e76ea 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -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 diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index a8f4a0606..ff75cb672 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -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)))