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

Ford: lateral motion control safety (#1228) 

* assert the other signals are zeroed

* add to tja function

* fix misra

* test

* should tx

* ?

* rm line

* up here

* Update tests/safety/test_ford.py

* faster

* adjust round
This commit is contained in:
Shane Smiskol 2023-02-02 23:02:03 -08:00 committed by GitHub
parent 6ec0c80754
commit 0f964c6db3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 36 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
board/safety/safety_ford.h
View File

@ -30,6 +30,10 @@ AddrCheckStruct ford_addr_checks[] = {
#define FORD_ADDR_CHECK_LEN (sizeof(ford_addr_checks) / sizeof(ford_addr_checks[0]))
addr_checks ford_rx_checks = {ford_addr_checks, FORD_ADDR_CHECK_LEN};
#define INACTIVE_CURVATURE 1000U
#define INACTIVE_CURVATURE_RATE 4096U
#define INACTIVE_PATH_OFFSET 512U
#define INACTIVE_PATH_ANGLE 1000U
static bool ford_lkas_msg_check(int addr) {
return (addr == MSG_ACCDATA_3)
@ -116,9 +120,18 @@ static int ford_tx_hook(CANPacket_t *to_send) {
if (addr == MSG_LateralMotionControl) {
// Signal: LatCtl_D_Rq
unsigned int steer_control_type = (GET_BYTE(to_send, 4) >> 2) & 0x7U;
bool steer_control_enabled = steer_control_type != 0U;
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);
// 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;
}
// 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) {
tx = 0;
}

29
tests/safety/test_ford.py
View File

@ -1,4 +1,5 @@
#!/usr/bin/env python3
import numpy as np
import unittest
import panda.tests.safety.common as common
@ -80,9 +81,13 @@ class TestFordSafety(common.PandaSafetyTest):
return self.packer.make_can_msg_panda("Lane_Assist_Data1", 0, values)
# TJA command
def _tja_command_msg(self, enabled: bool):
def _tja_command_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float):
values = {
"LatCtl_D_Rq": 1 if enabled else 0,
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return self.packer.make_can_msg_panda("LateralMotionControl", 0, values)
@ -96,13 +101,23 @@ class TestFordSafety(common.PandaSafetyTest):
return self.packer.make_can_msg_panda("Steering_Data_FD1", bus, values)
def test_steer_allowed(self):
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._tja_command_msg(1)))
self.assertTrue(self.safety.get_controls_allowed())
path_offsets = np.arange(-5.12, 5.11, 1).round()
path_angles = np.arange(-0.5, 0.5235, 0.1).round(1)
curvature_rates = np.arange(-0.001024, 0.00102375, 0.001).round(3)
curvatures = np.arange(-0.02, 0.02094, 0.01).round(2)
self.safety.set_controls_allowed(0)
self.assertFalse(self._tx(self._tja_command_msg(1)))
self.assertFalse(self.safety.get_controls_allowed())
for controls_allowed in (True, False):
for steer_control_enabled in (True, False):
for path_offset in path_offsets:
for path_angle in path_angles:
for curvature_rate in curvature_rates:
for curvature in curvatures:
self.safety.set_controls_allowed(controls_allowed)
enabled = steer_control_enabled or curvature != 0
should_tx = path_offset == 0 and path_angle == 0 and curvature_rate == 0
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_prevent_lkas_action(self):
self.safety.set_controls_allowed(1)