added gm safety for steering (#123)
* added gm safety for steering * safety tests done for safety_gmmaster
parent
bf5db45ab1
commit
38a9ea9ad9
|
@ -1,4 +1,4 @@
|
||||||
const int CADILLAC_STEER_MAX = 150; // 1s
|
const int CADILLAC_MAX_STEER = 150; // 1s
|
||||||
// real time torque limit to prevent controls spamming
|
// real time torque limit to prevent controls spamming
|
||||||
// the real time limit is 1500/sec
|
// the real time limit is 1500/sec
|
||||||
const int CADILLAC_MAX_RT_DELTA = 75; // max delta torque allowed for real time checks
|
const int CADILLAC_MAX_RT_DELTA = 75; // max delta torque allowed for real time checks
|
||||||
|
@ -14,8 +14,7 @@ int cadillac_rt_torque_last = 0;
|
||||||
int cadillac_desired_torque_last[4] = {0}; // 4 torque messages
|
int cadillac_desired_torque_last[4] = {0}; // 4 torque messages
|
||||||
uint32_t cadillac_ts_last = 0;
|
uint32_t cadillac_ts_last = 0;
|
||||||
int cadillac_supercruise_on = 0;
|
int cadillac_supercruise_on = 0;
|
||||||
|
struct sample_t cadillac_torque_driver; // last few driver torques measured
|
||||||
struct sample_t cadillac_torque_driver; // last 3 driver torques measured
|
|
||||||
|
|
||||||
int cadillac_get_torque_idx(uint32_t addr) {
|
int cadillac_get_torque_idx(uint32_t addr) {
|
||||||
if (addr==0x151) return 0;
|
if (addr==0x151) return 0;
|
||||||
|
@ -60,7 +59,7 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||||
uint32_t addr = to_send->RIR >> 21;
|
uint32_t addr = to_send->RIR >> 21;
|
||||||
|
|
||||||
// block steering cmd above 150
|
// steer cmd checks
|
||||||
if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
|
if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
|
||||||
int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
|
int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
|
||||||
int violation = 0;
|
int violation = 0;
|
||||||
|
@ -71,12 +70,12 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||||
if (controls_allowed) {
|
if (controls_allowed) {
|
||||||
|
|
||||||
// *** global torque limit check ***
|
// *** global torque limit check ***
|
||||||
violation |= max_limit_check(desired_torque, CADILLAC_STEER_MAX);
|
violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER);
|
||||||
|
|
||||||
// *** torque rate limit check ***
|
// *** torque rate limit check ***
|
||||||
int desired_torque_last = cadillac_desired_torque_last[idx];
|
int desired_torque_last = cadillac_desired_torque_last[idx];
|
||||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &cadillac_torque_driver,
|
violation |= driver_limit_check(desired_torque, desired_torque_last, &cadillac_torque_driver,
|
||||||
CADILLAC_STEER_MAX, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN,
|
CADILLAC_MAX_STEER, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN,
|
||||||
CADILLAC_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR);
|
CADILLAC_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR);
|
||||||
|
|
||||||
// used next time
|
// used next time
|
||||||
|
@ -87,7 +86,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||||
|
|
||||||
// every RT_INTERVAL set the new limits
|
// every RT_INTERVAL set the new limits
|
||||||
uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
|
uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
|
||||||
if (ts_elapsed > RT_INTERVAL) {
|
if (ts_elapsed > CADILLAC_RT_INTERVAL) {
|
||||||
cadillac_rt_torque_last = desired_torque;
|
cadillac_rt_torque_last = desired_torque;
|
||||||
cadillac_ts_last = ts;
|
cadillac_ts_last = ts;
|
||||||
}
|
}
|
||||||
|
@ -98,8 +97,8 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||||
violation = 1;
|
violation = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset to 0 if either controls is not allowed or there's a violation
|
// reset to 0 if either controls is not allowed or there's a violation
|
||||||
if (violation || !controls_allowed) {
|
if (violation || !controls_allowed) {
|
||||||
cadillac_desired_torque_last[idx] = 0;
|
cadillac_desired_torque_last[idx] = 0;
|
||||||
cadillac_rt_torque_last = 0;
|
cadillac_rt_torque_last = 0;
|
||||||
cadillac_ts_last = ts;
|
cadillac_ts_last = ts;
|
||||||
|
|
|
@ -8,15 +8,24 @@
|
||||||
// brake rising edge
|
// brake rising edge
|
||||||
// brake > 0mph
|
// brake > 0mph
|
||||||
|
|
||||||
// gm_: poor man's namespacing
|
const int GM_MAX_STEER = 255;
|
||||||
|
const int GM_MAX_RT_DELTA = 128; // max delta torque allowed for real time checks
|
||||||
|
const int32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||||
|
const int GM_MAX_RATE_UP = 7;
|
||||||
|
const int GM_MAX_RATE_DOWN = 17;
|
||||||
|
const int GM_DRIVER_TORQUE_ALLOWANCE = 50;
|
||||||
|
const int GM_DRIVER_TORQUE_FACTOR = 4;
|
||||||
|
|
||||||
int gm_brake_prev = 0;
|
int gm_brake_prev = 0;
|
||||||
int gm_gas_prev = 0;
|
int gm_gas_prev = 0;
|
||||||
int gm_speed = 0;
|
int gm_speed = 0;
|
||||||
|
|
||||||
// silence everything if stock ECUs are still online
|
// silence everything if stock ECUs are still online
|
||||||
int gm_ascm_detected = 0;
|
int gm_ascm_detected = 0;
|
||||||
|
|
||||||
int gm_ignition_started = 0;
|
int gm_ignition_started = 0;
|
||||||
|
int gm_rt_torque_last = 0;
|
||||||
|
int gm_desired_torque_last = 0;
|
||||||
|
uint32_t gm_ts_last = 0;
|
||||||
|
struct sample_t gm_torque_driver; // last few driver torques measured
|
||||||
|
|
||||||
static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
int bus_number = (to_push->RDTR >> 4) & 0xFF;
|
int bus_number = (to_push->RDTR >> 4) & 0xFF;
|
||||||
|
@ -31,6 +40,13 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
addr = to_push->RIR >> 21;
|
addr = to_push->RIR >> 21;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (addr == 388) {
|
||||||
|
int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF);
|
||||||
|
torque_driver_new = to_signed(torque_driver_new, 11);
|
||||||
|
// update array of samples
|
||||||
|
update_sample(&gm_torque_driver, torque_driver_new);
|
||||||
|
}
|
||||||
|
|
||||||
if (addr == 0x1f1 && bus_number == 0) {
|
if (addr == 0x1f1 && bus_number == 0) {
|
||||||
//Bit 5 should be ignition "on"
|
//Bit 5 should be ignition "on"
|
||||||
//Backup plan is Bit 2 (accessory power)
|
//Backup plan is Bit 2 (accessory power)
|
||||||
|
@ -136,13 +152,49 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||||
// LKA STEER: safety check
|
// LKA STEER: safety check
|
||||||
if (addr == 384) {
|
if (addr == 384) {
|
||||||
int rdlr = to_send->RDLR;
|
int rdlr = to_send->RDLR;
|
||||||
int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
|
int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
|
||||||
steer = to_signed(steer, 11);
|
uint32_t ts = TIM2->CNT;
|
||||||
int max_steer = 255;
|
int violation = 0;
|
||||||
|
desired_torque = to_signed(desired_torque, 11);
|
||||||
|
|
||||||
if (current_controls_allowed) {
|
if (current_controls_allowed) {
|
||||||
if ((steer > max_steer) || (steer < -max_steer)) return 0;
|
|
||||||
} else {
|
// *** global torque limit check ***
|
||||||
if (steer != 0) return 0;
|
violation |= max_limit_check(desired_torque, GM_MAX_STEER);
|
||||||
|
|
||||||
|
// *** torque rate limit check ***
|
||||||
|
violation |= driver_limit_check(desired_torque, gm_desired_torque_last, &gm_torque_driver,
|
||||||
|
GM_MAX_STEER, GM_MAX_RATE_UP, GM_MAX_RATE_DOWN,
|
||||||
|
GM_DRIVER_TORQUE_ALLOWANCE, GM_DRIVER_TORQUE_FACTOR);
|
||||||
|
|
||||||
|
// used next time
|
||||||
|
gm_desired_torque_last = desired_torque;
|
||||||
|
|
||||||
|
// *** torque real time rate limit check ***
|
||||||
|
violation |= rt_rate_limit_check(desired_torque, gm_rt_torque_last, GM_MAX_RT_DELTA);
|
||||||
|
|
||||||
|
// every RT_INTERVAL set the new limits
|
||||||
|
uint32_t ts_elapsed = get_ts_elapsed(ts, gm_ts_last);
|
||||||
|
if (ts_elapsed > GM_RT_INTERVAL) {
|
||||||
|
gm_rt_torque_last = desired_torque;
|
||||||
|
gm_ts_last = ts;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// no torque if controls is not allowed
|
||||||
|
if (!current_controls_allowed && (desired_torque != 0)) {
|
||||||
|
violation = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// reset to 0 if either controls is not allowed or there's a violation
|
||||||
|
if (violation || !current_controls_allowed) {
|
||||||
|
gm_desired_torque_last = 0;
|
||||||
|
gm_rt_torque_last = 0;
|
||||||
|
gm_ts_last = ts;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (violation) {
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -40,6 +40,7 @@ void init_tests_toyota(void);
|
||||||
void set_timer(int t);
|
void set_timer(int t);
|
||||||
void set_torque_meas(int min, int max);
|
void set_torque_meas(int min, int max);
|
||||||
void set_cadillac_torque_driver(int min, int max);
|
void set_cadillac_torque_driver(int min, int max);
|
||||||
|
void set_gm_torque_driver(int min, int max);
|
||||||
void set_rt_torque_last(int t);
|
void set_rt_torque_last(int t);
|
||||||
void set_desired_torque_last(int t);
|
void set_desired_torque_last(int t);
|
||||||
int get_torque_meas_min(void);
|
int get_torque_meas_min(void);
|
||||||
|
@ -62,6 +63,13 @@ int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||||
void set_cadillac_desired_torque_last(int t);
|
void set_cadillac_desired_torque_last(int t);
|
||||||
void set_cadillac_rt_torque_last(int t);
|
void set_cadillac_rt_torque_last(int t);
|
||||||
|
|
||||||
|
void init_tests_gm(void);
|
||||||
|
void gm_init(int16_t param);
|
||||||
|
void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
||||||
|
int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||||
|
void set_gm_desired_torque_last(int t);
|
||||||
|
void set_gm_rt_torque_last(int t);
|
||||||
|
|
||||||
void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
||||||
int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||||
|
|
||||||
|
|
|
@ -24,6 +24,7 @@ typedef struct
|
||||||
|
|
||||||
struct sample_t torque_meas;
|
struct sample_t torque_meas;
|
||||||
struct sample_t cadillac_torque_driver;
|
struct sample_t cadillac_torque_driver;
|
||||||
|
struct sample_t gm_torque_driver;
|
||||||
|
|
||||||
TIM_TypeDef timer;
|
TIM_TypeDef timer;
|
||||||
TIM_TypeDef *TIM2 = &timer;
|
TIM_TypeDef *TIM2 = &timer;
|
||||||
|
@ -69,6 +70,11 @@ void set_cadillac_torque_driver(int min, int max){
|
||||||
cadillac_torque_driver.max = max;
|
cadillac_torque_driver.max = max;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void set_gm_torque_driver(int min, int max){
|
||||||
|
gm_torque_driver.min = min;
|
||||||
|
gm_torque_driver.max = max;
|
||||||
|
}
|
||||||
|
|
||||||
int get_torque_meas_min(void){
|
int get_torque_meas_min(void){
|
||||||
return torque_meas.min;
|
return torque_meas.min;
|
||||||
}
|
}
|
||||||
|
@ -85,6 +91,10 @@ void set_cadillac_rt_torque_last(int t){
|
||||||
cadillac_rt_torque_last = t;
|
cadillac_rt_torque_last = t;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void set_gm_rt_torque_last(int t){
|
||||||
|
gm_rt_torque_last = t;
|
||||||
|
}
|
||||||
|
|
||||||
void set_desired_torque_last(int t){
|
void set_desired_torque_last(int t){
|
||||||
desired_torque_last = t;
|
desired_torque_last = t;
|
||||||
}
|
}
|
||||||
|
@ -93,6 +103,11 @@ void set_cadillac_desired_torque_last(int t){
|
||||||
for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = t;
|
for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = t;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void set_gm_desired_torque_last(int t){
|
||||||
|
gm_desired_torque_last = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
int get_ego_speed(void){
|
int get_ego_speed(void){
|
||||||
return ego_speed;
|
return ego_speed;
|
||||||
}
|
}
|
||||||
|
@ -131,6 +146,15 @@ void init_tests_cadillac(void){
|
||||||
set_timer(0);
|
set_timer(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void init_tests_gm(void){
|
||||||
|
gm_torque_driver.min = 0;
|
||||||
|
gm_torque_driver.max = 0;
|
||||||
|
gm_desired_torque_last = 0;
|
||||||
|
gm_rt_torque_last = 0;
|
||||||
|
gm_ts_last = 0;
|
||||||
|
set_timer(0);
|
||||||
|
}
|
||||||
|
|
||||||
void init_tests_honda(void){
|
void init_tests_honda(void){
|
||||||
ego_speed = 0;
|
ego_speed = 0;
|
||||||
gas_interceptor_detected = 0;
|
gas_interceptor_detected = 0;
|
||||||
|
|
|
@ -0,0 +1,270 @@
|
||||||
|
#!/usr/bin/env python2
|
||||||
|
import unittest
|
||||||
|
import numpy as np
|
||||||
|
import libpandasafety_py
|
||||||
|
|
||||||
|
MAX_RATE_UP = 7
|
||||||
|
MAX_RATE_DOWN = 17
|
||||||
|
MAX_STEER = 255
|
||||||
|
MAX_BRAKE = 255
|
||||||
|
MAX_GAS = 3072
|
||||||
|
MAX_REGEN = 1404
|
||||||
|
|
||||||
|
MAX_RT_DELTA = 128
|
||||||
|
RT_INTERVAL = 250000
|
||||||
|
|
||||||
|
DRIVER_TORQUE_ALLOWANCE = 50;
|
||||||
|
DRIVER_TORQUE_FACTOR = 4;
|
||||||
|
|
||||||
|
def twos_comp(val, bits):
|
||||||
|
if val >= 0:
|
||||||
|
return val
|
||||||
|
else:
|
||||||
|
return (2**bits) + val
|
||||||
|
|
||||||
|
def sign(a):
|
||||||
|
if a > 0:
|
||||||
|
return 1
|
||||||
|
else:
|
||||||
|
return -1
|
||||||
|
|
||||||
|
class TestGmSafety(unittest.TestCase):
|
||||||
|
@classmethod
|
||||||
|
def setUp(cls):
|
||||||
|
cls.safety = libpandasafety_py.libpandasafety
|
||||||
|
cls.safety.gm_init(0)
|
||||||
|
cls.safety.init_tests_gm()
|
||||||
|
|
||||||
|
def _speed_msg(self, speed):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 842 << 21
|
||||||
|
to_send[0].RDLR = speed
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _button_msg(self, buttons):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 481 << 21
|
||||||
|
to_send[0].RDHR = buttons << 12
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _brake_msg(self, brake):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 241 << 21
|
||||||
|
to_send[0].RDLR = 0xa00 if brake else 0x900
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _gas_msg(self, gas):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 417 << 21
|
||||||
|
to_send[0].RDHR = (1 << 16) if gas else 0
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _send_brake_msg(self, brake):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 789 << 21
|
||||||
|
brake = (-brake) & 0xfff
|
||||||
|
to_send[0].RDLR = (brake >> 8) | ((brake &0xff) << 8)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _send_gas_msg(self, gas):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 715 << 21
|
||||||
|
to_send[0].RDLR = ((gas & 0x1f) << 27) | ((gas & 0xfe0) << 11)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _set_prev_torque(self, t):
|
||||||
|
self.safety.set_gm_desired_torque_last(t)
|
||||||
|
self.safety.set_gm_rt_torque_last(t)
|
||||||
|
|
||||||
|
def _torque_driver_msg(self, torque):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 388 << 21
|
||||||
|
|
||||||
|
t = twos_comp(torque, 11)
|
||||||
|
to_send[0].RDHR = (((t >> 8) & 0x7) << 16) | ((t & 0xFF) << 24)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _torque_driver_msg_array(self, torque):
|
||||||
|
for i in range(3):
|
||||||
|
self.safety.gm_ipas_rx_hook(self._torque_driver_msg(torque))
|
||||||
|
|
||||||
|
def _torque_msg(self, torque):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 384 << 21
|
||||||
|
|
||||||
|
t = twos_comp(torque, 11)
|
||||||
|
to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self):
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_resume_button(self):
|
||||||
|
RESUME_BTN = 2
|
||||||
|
self.safety.set_controls_allowed(0)
|
||||||
|
self.safety.gm_rx_hook(self._button_msg(RESUME_BTN))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_set_button(self):
|
||||||
|
SET_BTN = 3
|
||||||
|
self.safety.set_controls_allowed(0)
|
||||||
|
self.safety.gm_rx_hook(self._button_msg(SET_BTN))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_cancel_button(self):
|
||||||
|
CANCEL_BTN = 6
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.gm_rx_hook(self._button_msg(CANCEL_BTN))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_disengage_on_brake(self):
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(True))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_allow_brake_at_zero_speed(self):
|
||||||
|
# Brake was already pressed
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(True))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(True))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(False))
|
||||||
|
|
||||||
|
def test_not_allow_brake_when_moving(self):
|
||||||
|
# Brake was already pressed
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(True))
|
||||||
|
self.safety.gm_rx_hook(self._speed_msg(100))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(True))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
self.safety.gm_rx_hook(self._brake_msg(False))
|
||||||
|
|
||||||
|
def test_disengage_on_gas(self):
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.gm_rx_hook(self._gas_msg(True))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
self.safety.gm_rx_hook(self._gas_msg(False))
|
||||||
|
|
||||||
|
def test_allow_engage_with_gas_pressed(self):
|
||||||
|
self.safety.gm_rx_hook(self._gas_msg(True))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.gm_rx_hook(self._gas_msg(True))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
self.safety.gm_rx_hook(self._gas_msg(False))
|
||||||
|
|
||||||
|
def test_brake_safety_check(self):
|
||||||
|
for enabled in [0, 1]:
|
||||||
|
for b in range(0, 500):
|
||||||
|
self.safety.set_controls_allowed(enabled)
|
||||||
|
if abs(b) > MAX_BRAKE or (not enabled and b != 0):
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._send_brake_msg(b)))
|
||||||
|
else:
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._send_brake_msg(b)))
|
||||||
|
|
||||||
|
def test_gas_safety_check(self):
|
||||||
|
for enabled in [0, 1]:
|
||||||
|
for g in range(0, 2**12-1):
|
||||||
|
self.safety.set_controls_allowed(enabled)
|
||||||
|
if abs(g) > MAX_GAS or (not enabled and g != MAX_REGEN):
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._send_gas_msg(g)))
|
||||||
|
else:
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._send_gas_msg(g)))
|
||||||
|
|
||||||
|
def test_steer_safety_check(self):
|
||||||
|
for enabled in [0, 1]:
|
||||||
|
for t in range(-0x200, 0x200):
|
||||||
|
self.safety.set_controls_allowed(enabled)
|
||||||
|
self._set_prev_torque(t)
|
||||||
|
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(t)))
|
||||||
|
else:
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t)))
|
||||||
|
|
||||||
|
def test_manually_enable_controls_allowed(self):
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
self.safety.set_controls_allowed(0)
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_non_realtime_limit_up(self):
|
||||||
|
self.safety.set_gm_torque_driver(0, 0)
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(MAX_RATE_UP)))
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(-MAX_RATE_UP)))
|
||||||
|
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||||
|
|
||||||
|
def test_non_realtime_limit_down(self):
|
||||||
|
self.safety.set_gm_torque_driver(0, 0)
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
def test_against_torque_driver(self):
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
for sign in [-1, 1]:
|
||||||
|
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||||
|
t *= -sign
|
||||||
|
self.safety.set_gm_torque_driver(t, t)
|
||||||
|
self._set_prev_torque(MAX_STEER * sign)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||||
|
|
||||||
|
self.safety.set_gm_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||||
|
|
||||||
|
# spot check some individual cases
|
||||||
|
for sign in [-1, 1]:
|
||||||
|
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign
|
||||||
|
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
|
||||||
|
delta = 1 * sign
|
||||||
|
self._set_prev_torque(torque_desired)
|
||||||
|
self.safety.set_gm_torque_driver(-driver_torque, -driver_torque)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(torque_desired)))
|
||||||
|
self._set_prev_torque(torque_desired + delta)
|
||||||
|
self.safety.set_gm_torque_driver(-driver_torque, -driver_torque)
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||||
|
|
||||||
|
self._set_prev_torque(MAX_STEER * sign)
|
||||||
|
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||||
|
self._set_prev_torque(MAX_STEER * sign)
|
||||||
|
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(0)))
|
||||||
|
self._set_prev_torque(MAX_STEER * sign)
|
||||||
|
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||||
|
|
||||||
|
|
||||||
|
def test_realtime_limits(self):
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
for sign in [-1, 1]:
|
||||||
|
self.safety.init_tests_gm()
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.safety.set_gm_torque_driver(0, 0)
|
||||||
|
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||||
|
t *= sign
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t)))
|
||||||
|
self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||||
|
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||||
|
t *= sign
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t)))
|
||||||
|
|
||||||
|
# Increase timer to update rt_torque_last
|
||||||
|
self.safety.set_timer(RT_INTERVAL + 1)
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||||
|
self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
|
@ -72,11 +72,13 @@ class TestHondaSafety(unittest.TestCase):
|
||||||
|
|
||||||
def test_resume_button(self):
|
def test_resume_button(self):
|
||||||
RESUME_BTN = 4
|
RESUME_BTN = 4
|
||||||
|
self.safety.set_controls_allowed(0)
|
||||||
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
|
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
|
||||||
self.assertTrue(self.safety.get_controls_allowed())
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
def test_set_button(self):
|
def test_set_button(self):
|
||||||
SET_BTN = 3
|
SET_BTN = 3
|
||||||
|
self.safety.set_controls_allowed(0)
|
||||||
self.safety.honda_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
self.safety.honda_rx_hook(self._button_msg(SET_BTN, 0x1A6))
|
||||||
self.assertTrue(self.safety.get_controls_allowed())
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
@ -119,6 +121,7 @@ class TestHondaSafety(unittest.TestCase):
|
||||||
|
|
||||||
self.safety.honda_rx_hook(self._brake_msg(True))
|
self.safety.honda_rx_hook(self._brake_msg(True))
|
||||||
self.assertTrue(self.safety.get_controls_allowed())
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(False)) # reset no brakes
|
||||||
|
|
||||||
def test_not_allow_brake_when_moving(self):
|
def test_not_allow_brake_when_moving(self):
|
||||||
# Brake was already pressed
|
# Brake was already pressed
|
||||||
|
@ -154,10 +157,12 @@ class TestHondaSafety(unittest.TestCase):
|
||||||
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0)))
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0)))
|
||||||
|
|
||||||
def test_gas_safety_check(self):
|
def test_gas_safety_check(self):
|
||||||
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000)))
|
self.safety.set_controls_allowed(0)
|
||||||
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
|
self.assertTrue(self.safety.honda_tx_hook(self._send_gas_msg(0x0000)))
|
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_gas_msg(0x1000)))
|
||||||
|
|
||||||
def test_steer_safety_check(self):
|
def test_steer_safety_check(self):
|
||||||
|
self.safety.set_controls_allowed(0)
|
||||||
self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000)))
|
self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000)))
|
||||||
self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000)))
|
self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000)))
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue