retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Reset state on safety mode init (#542) 

* reset state on safety mode init

* more global state

* reset message seen too

* misra
master
Adeeb 2020-05-26 14:23:39 -07:00 committed by GitHub
parent d4f3f15c33
commit 9ebde2535c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 57 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
board/safety.h
View File

@ -239,7 +239,29 @@ const safety_hook_config safety_hook_registry[] = {
};
int set_safety_hooks(uint16_t mode, int16_t param) {
safety_mode_cnt = 0U; // reset safety mode timer
// reset state set by safety mode
safety_mode_cnt = 0U;
relay_malfunction = false;
gas_interceptor_detected = false;
gas_interceptor_prev = 0;
gas_pressed_prev = false;
brake_pressed_prev = false;
cruise_engaged_prev = false;
vehicle_speed = 0;
vehicle_moving = false;
desired_torque_last = 0;
rt_torque_last = 0;
ts_angle_last = 0;
desired_angle_last = 0;
ts_last = 0;
torque_meas.max = 0;
torque_meas.max = 0;
torque_driver.min = 0;
torque_driver.max = 0;
angle_meas.min = 0;
angle_meas.max = 0;
int set_status = -1; // not set
int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
for (int i = 0; i < hook_config_count; i++) {
@ -247,7 +269,12 @@ int set_safety_hooks(uint16_t mode, int16_t param) {
current_hooks = safety_hook_registry[i].hooks;
current_safety_mode = safety_hook_registry[i].id;
set_status = 0; // set
break;
}
// reset message index and seen flags in addr struct
for (int j = 0; j < safety_hook_registry[i].hooks->addr_check_len; j++) {
safety_hook_registry[i].hooks->addr_check[j].index = 0;
safety_hook_registry[i].hooks->addr_check[j].msg_seen = false;
}
}
if ((set_status == 0) && (current_hooks->init != NULL)) {

8
board/safety/safety_chrysler.h
View File

@ -17,8 +17,6 @@ AddrCheckStruct chrysler_rx_checks[] = {
};
const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]);
int chrysler_speed = 0;
static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
int checksum_byte = GET_LEN(to_push) - 1;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
@ -97,14 +95,14 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (addr == 514) {
int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
chrysler_speed = (speed_l + speed_r) / 2;
vehicle_moving = chrysler_speed > CHRYSLER_STANDSTILL_THRSLD;
vehicle_speed = (speed_l + speed_r) / 2;
vehicle_moving = (int)vehicle_speed > CHRYSLER_STANDSTILL_THRSLD;
}
// exit controls on rising edge of gas press
if (addr == 308) {
bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0;
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && ((int)vehicle_speed > CHRYSLER_GAS_THRSLD)) {
controls_allowed = 0;
}
gas_pressed_prev = gas_pressed;

29
board/safety/safety_nissan.h
View File

@ -24,13 +24,6 @@ AddrCheckStruct nissan_rx_checks[] = {
};
const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);
float nissan_speed = 0;
//int nissan_controls_allowed_last = 0;
uint32_t nissan_ts_angle_last = 0;
int nissan_desired_angle_last = 0;
struct sample_t nissan_angle_meas; // last 3 steer angles
static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
@ -52,14 +45,14 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
angle_meas_new = to_signed(angle_meas_new, 16) * 10;
// update array of samples
update_sample(&nissan_angle_meas, angle_meas_new);
update_sample(&angle_meas, angle_meas_new);
}
if (addr == 0x285) {
// Get current speed
// Factor 0.005
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6;
vehicle_moving = nissan_speed > 0.;
vehicle_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6;
vehicle_moving = vehicle_speed > 0.;
}
// exit controls on rising edge of gas press
@ -142,24 +135,22 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (controls_allowed && lka_active) {
// add 1 to not false trigger the violation
float delta_angle_float;
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, vehicle_speed) * NISSAN_DEG_TO_CAN) + 1.;
int delta_angle_up = (int)(delta_angle_float);
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, vehicle_speed) * NISSAN_DEG_TO_CAN) + 1.;
int delta_angle_down = (int)(delta_angle_float);
int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
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 for violation;
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
//nissan_controls_allowed_last = controls_allowed;
}
nissan_desired_angle_last = desired_angle;
desired_angle_last = desired_angle;
// desired steer angle should be the same as steer angle measured when controls are off
if ((!controls_allowed) &&
((desired_angle < (nissan_angle_meas.min - 1)) ||
(desired_angle > (nissan_angle_meas.max + 1)))) {
((desired_angle < (angle_meas.min - 1)) ||
(desired_angle > (angle_meas.max + 1)))) {
violation = 1;
}

8
board/safety_declarations.h
View File

@ -98,15 +98,21 @@ int gas_interceptor_prev = 0;
bool gas_pressed_prev = false;
bool brake_pressed_prev = false;
bool cruise_engaged_prev = false;
float vehicle_speed = 0;
bool vehicle_moving = false;
// for torque-based safety modes
// for safety modes with torque steering control
int desired_torque_last = 0; // last desired steer torque
int rt_torque_last = 0; // last desired torque for real time check
struct sample_t torque_meas; // last 3 motor torques produced by the eps
struct sample_t torque_driver; // last 3 driver torques measured
uint32_t ts_last = 0;
// for safety modes with angle steering control
uint32_t ts_angle_last = 0;
int desired_angle_last = 0;
struct sample_t angle_meas; // last 3 steer angles
// This can be set with a USB command
// It enables features we consider to be unsafe, but understand others may have different opinions
// It is always 0 on mainline comma.ai openpilot

7
tests/safety/libpandasafety_py.py
View File

@ -50,6 +50,7 @@ int get_torque_driver_min(void);
int get_torque_driver_max(void);
void set_desired_torque_last(int t);
void set_rt_torque_last(int t);
void set_desired_angle_last(int t);
bool get_cruise_engaged_prev(void);
bool get_vehicle_moving(void);
@ -69,13 +70,7 @@ void set_honda_alt_brake_msg(bool);
void set_honda_bosch_long(bool c);
int get_honda_hw(void);
void init_tests_chrysler(void);
bool get_subaru_global(void);
void init_tests_nissan(void);
void set_nissan_desired_angle_last(int t);
""")
libpandasafety = ffi.dlopen(libpandasafety_fn)

31
tests/safety/test.c
View File

@ -180,6 +180,10 @@ void set_desired_torque_last(int t){
desired_torque_last = t;
}
void set_desired_angle_last(int t){
desired_angle_last = t;
}
void set_honda_alt_brake_msg(bool c){
honda_alt_brake_msg = c;
}
@ -196,46 +200,19 @@ void set_honda_fwd_brake(bool c){
honda_fwd_brake = c;
}
void set_nissan_desired_angle_last(int t){
nissan_desired_angle_last = t;
}
void init_tests(void){
// get HW_TYPE from env variable set in test.sh
hw_type = atoi(getenv("HW_TYPE"));
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
gas_pressed_prev = false;
brake_pressed_prev = false;
desired_torque_last = 0;
rt_torque_last = 0;
ts_last = 0;
torque_driver.min = 0;
torque_driver.max = 0;
torque_meas.min = 0;
torque_meas.max = 0;
vehicle_moving = false;
unsafe_mode = 0;
set_timer(0);
}
void init_tests_chrysler(void){
init_tests();
chrysler_speed = 0;
}
void init_tests_honda(void){
init_tests();
honda_fwd_brake = false;
}
void init_tests_nissan(void){
init_tests();
nissan_angle_meas.min = 0;
nissan_angle_meas.max = 0;
nissan_desired_angle_last = 0;
set_timer(0);
}
void set_gmlan_digital_output(int to_set){
}

2
tests/safety/test_chrysler.py
View File

@ -30,7 +30,7 @@ class TestChryslerSafety(common.PandaSafetyTest, common.TorqueSteeringSafetyTest
self.packer = CANPackerPanda("chrysler_pacifica_2017_hybrid")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0)
self.safety.init_tests_chrysler()
self.safety.init_tests()
def _button_msg(self, cancel):
values = {"ACC_CANCEL": cancel}

4
tests/safety/test_nissan.py
View File

@ -29,7 +29,7 @@ class TestNissanSafety(common.PandaSafetyTest):
self.packer = CANPackerPanda("nissan_x_trail_2017")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0)
self.safety.init_tests_nissan()
self.safety.init_tests()
def _angle_meas_msg(self, angle):
values = {"STEER_ANGLE": angle}
@ -37,7 +37,7 @@ class TestNissanSafety(common.PandaSafetyTest):
def _set_prev_angle(self, t):
t = int(t * -100)
self.safety.set_nissan_desired_angle_last(t)
self.safety.set_desired_angle_last(t)
def _angle_meas_msg_array(self, angle):
for i in range(6):