Safety code testing (#104)
* inital infrastructure for panda safety testing * add test for toyota acceleration * test for non real time torque rate limits and refactoring * add test for cruise disable * fix toyota limit down * add tests for realtime limits * test for torque measurements * fix toyota test setup * honda button logic * test for brake logic * tests for gas logic * test steer, gas and brake message contents * add test script * fix hardcoded limitsmaster
parent
f4efd1f29d
commit
1c88caf917
|
@ -2,6 +2,8 @@
|
||||||
.*.swp
|
.*.swp
|
||||||
.*.swo
|
.*.swo
|
||||||
*.o
|
*.o
|
||||||
|
*.so
|
||||||
|
*.d
|
||||||
a.out
|
a.out
|
||||||
*~
|
*~
|
||||||
.#*
|
.#*
|
||||||
|
|
|
@ -0,0 +1,18 @@
|
||||||
|
CC = clang
|
||||||
|
CCFLAGS = -O3 -fPIC -I.
|
||||||
|
|
||||||
|
.PHONY: all
|
||||||
|
all: libpandasafety.so
|
||||||
|
|
||||||
|
libpandasafety.so: test.o
|
||||||
|
$(CC) -shared -o '$@' $^ -lm
|
||||||
|
|
||||||
|
test.o: test.c
|
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CCFLAGS) -MMD -c -I../../board -o '$@' '$<'
|
||||||
|
|
||||||
|
.PHONY: clean
|
||||||
|
clean:
|
||||||
|
rm -f libpandasafety.so test.o test.d
|
||||||
|
|
||||||
|
-include test.d
|
|
@ -0,0 +1,56 @@
|
||||||
|
import os
|
||||||
|
import subprocess
|
||||||
|
|
||||||
|
from cffi import FFI
|
||||||
|
|
||||||
|
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||||
|
libpandasafety_fn = os.path.join(can_dir, "libpandasafety.so")
|
||||||
|
subprocess.check_call(["make"], cwd=can_dir)
|
||||||
|
|
||||||
|
ffi = FFI()
|
||||||
|
ffi.cdef("""
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t TIR; /*!< CAN TX mailbox identifier register */
|
||||||
|
uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */
|
||||||
|
uint32_t TDLR; /*!< CAN mailbox data low register */
|
||||||
|
uint32_t TDHR; /*!< CAN mailbox data high register */
|
||||||
|
} CAN_TxMailBox_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */
|
||||||
|
uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
|
||||||
|
uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
|
||||||
|
uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
|
||||||
|
} CAN_FIFOMailBox_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t CNT;
|
||||||
|
} TIM_TypeDef;
|
||||||
|
|
||||||
|
void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
||||||
|
int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||||
|
void toyota_init(int16_t param);
|
||||||
|
void set_controls_allowed(int c);
|
||||||
|
int get_controls_allowed(void);
|
||||||
|
void init_tests_toyota(void);
|
||||||
|
void set_timer(int t);
|
||||||
|
void set_torque_meas(int min, int max);
|
||||||
|
void set_rt_torque_last(int t);
|
||||||
|
void set_desired_torque_last(int t);
|
||||||
|
int get_torque_meas_min(void);
|
||||||
|
int get_torque_meas_max(void);
|
||||||
|
|
||||||
|
void init_tests_honda(void);
|
||||||
|
int get_ego_speed(void);
|
||||||
|
void honda_init(int16_t param);
|
||||||
|
void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
||||||
|
int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||||
|
int get_brake_prev(void);
|
||||||
|
int get_gas_prev(void);
|
||||||
|
|
||||||
|
""")
|
||||||
|
|
||||||
|
libpandasafety = ffi.dlopen(libpandasafety_fn)
|
|
@ -0,0 +1,101 @@
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t TIR; /*!< CAN TX mailbox identifier register */
|
||||||
|
uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */
|
||||||
|
uint32_t TDLR; /*!< CAN mailbox data low register */
|
||||||
|
uint32_t TDHR; /*!< CAN mailbox data high register */
|
||||||
|
} CAN_TxMailBox_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */
|
||||||
|
uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
|
||||||
|
uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
|
||||||
|
uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
|
||||||
|
} CAN_FIFOMailBox_TypeDef;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
uint32_t CNT;
|
||||||
|
} TIM_TypeDef;
|
||||||
|
|
||||||
|
TIM_TypeDef timer;
|
||||||
|
TIM_TypeDef *TIM2 = &timer;
|
||||||
|
|
||||||
|
#define min(a,b) \
|
||||||
|
({ __typeof__ (a) _a = (a); \
|
||||||
|
__typeof__ (b) _b = (b); \
|
||||||
|
_a < _b ? _a : _b; })
|
||||||
|
|
||||||
|
#define max(a,b) \
|
||||||
|
({ __typeof__ (a) _a = (a); \
|
||||||
|
__typeof__ (b) _b = (b); \
|
||||||
|
_a > _b ? _a : _b; })
|
||||||
|
|
||||||
|
|
||||||
|
#define static
|
||||||
|
#include "safety.h"
|
||||||
|
|
||||||
|
void set_controls_allowed(int c){
|
||||||
|
controls_allowed = c;
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_controls_allowed(void){
|
||||||
|
return controls_allowed;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_timer(int t){
|
||||||
|
timer.CNT = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_torque_meas(int min, int max){
|
||||||
|
torque_meas_min = min;
|
||||||
|
torque_meas_max = max;
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_torque_meas_min(void){
|
||||||
|
return torque_meas_min;
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_torque_meas_max(void){
|
||||||
|
return torque_meas_max;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_rt_torque_last(int t){
|
||||||
|
rt_torque_last = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_desired_torque_last(int t){
|
||||||
|
desired_torque_last = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_ego_speed(void){
|
||||||
|
return ego_speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_brake_prev(void){
|
||||||
|
return brake_prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_gas_prev(void){
|
||||||
|
return gas_prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
void init_tests_toyota(void){
|
||||||
|
torque_meas_min = 0;
|
||||||
|
torque_meas_max = 0;
|
||||||
|
desired_torque_last = 0;
|
||||||
|
rt_torque_last = 0;
|
||||||
|
ts_last = 0;
|
||||||
|
set_timer(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void init_tests_honda(void){
|
||||||
|
ego_speed = 0;
|
||||||
|
gas_interceptor_detected = 0;
|
||||||
|
brake_prev = 0;
|
||||||
|
gas_prev = 0;
|
||||||
|
}
|
|
@ -0,0 +1,2 @@
|
||||||
|
#!/usr/bin/env sh
|
||||||
|
python -m unittest discover .
|
|
@ -0,0 +1,148 @@
|
||||||
|
#!/usr/bin/env python2
|
||||||
|
import unittest
|
||||||
|
import numpy as np
|
||||||
|
import libpandasafety_py
|
||||||
|
|
||||||
|
|
||||||
|
class TestHondaSafety(unittest.TestCase):
|
||||||
|
@classmethod
|
||||||
|
def setUp(cls):
|
||||||
|
cls.safety = libpandasafety_py.libpandasafety
|
||||||
|
cls.safety.honda_init(0)
|
||||||
|
cls.safety.init_tests_honda()
|
||||||
|
|
||||||
|
def _speed_msg(self, speed):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x158 << 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 = 0x1A6 << 21
|
||||||
|
to_send[0].RDLR = buttons << 5
|
||||||
|
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _brake_msg(self, brake):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x17C << 21
|
||||||
|
to_send[0].RDHR = 0x200000 if brake else 0
|
||||||
|
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _gas_msg(self, gas):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x17C << 21
|
||||||
|
to_send[0].RDLR = 1 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 = 0x1FA << 21
|
||||||
|
to_send[0].RDLR = brake
|
||||||
|
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _send_gas_msg(self, gas):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x200 << 21
|
||||||
|
to_send[0].RDLR = gas
|
||||||
|
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _send_steer_msg(self, steer):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0xE4 << 21
|
||||||
|
to_send[0].RDLR = steer
|
||||||
|
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self):
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_resume_button(self):
|
||||||
|
RESUME_BTN = 4
|
||||||
|
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_set_button(self):
|
||||||
|
SET_BTN = 3
|
||||||
|
self.safety.honda_rx_hook(self._button_msg(SET_BTN))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_cancel_button(self):
|
||||||
|
CANCEL_BTN = 2
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.honda_rx_hook(self._button_msg(CANCEL_BTN))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_sample_speed(self):
|
||||||
|
self.assertEqual(0, self.safety.get_ego_speed())
|
||||||
|
self.safety.honda_rx_hook(self._speed_msg(100))
|
||||||
|
self.assertEqual(100, self.safety.get_ego_speed())
|
||||||
|
|
||||||
|
def test_prev_brake(self):
|
||||||
|
self.assertFalse(self.safety.get_brake_prev())
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True))
|
||||||
|
self.assertTrue(self.safety.get_brake_prev())
|
||||||
|
|
||||||
|
def test_disengage_on_brake(self):
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(1))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_allow_brake_at_zero_speed(self):
|
||||||
|
# Brake was already pressed
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_not_allow_brake_when_moving(self):
|
||||||
|
# Brake was already pressed
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True))
|
||||||
|
self.safety.honda_rx_hook(self._speed_msg(100))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
|
||||||
|
self.safety.honda_rx_hook(self._brake_msg(True))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_prev_gas(self):
|
||||||
|
self.assertFalse(self.safety.get_gas_prev())
|
||||||
|
self.safety.honda_rx_hook(self._gas_msg(True))
|
||||||
|
self.assertTrue(self.safety.get_gas_prev())
|
||||||
|
|
||||||
|
def test_disengage_on_gas(self):
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.honda_rx_hook(self._gas_msg(1))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_allow_engage_with_gas_pressed(self):
|
||||||
|
self.safety.honda_rx_hook(self._gas_msg(1))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.honda_rx_hook(self._gas_msg(1))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_brake_safety_check(self):
|
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000)))
|
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
|
||||||
|
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
|
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0)))
|
||||||
|
|
||||||
|
def test_gas_safety_check(self):
|
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000)))
|
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000)))
|
||||||
|
|
||||||
|
def test_steer_safety_check(self):
|
||||||
|
self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000)))
|
||||||
|
self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000)))
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
|
@ -0,0 +1,185 @@
|
||||||
|
#!/usr/bin/env python2
|
||||||
|
import unittest
|
||||||
|
import numpy as np
|
||||||
|
import libpandasafety_py
|
||||||
|
|
||||||
|
MAX_RATE_UP = 10
|
||||||
|
MAX_RATE_DOWN = 25
|
||||||
|
MAX_TORQUE = 1500
|
||||||
|
|
||||||
|
MAX_ACCEL = 1500
|
||||||
|
MIN_ACCEL = -3000
|
||||||
|
|
||||||
|
MAX_RT_DELTA = 375
|
||||||
|
RT_INTERVAL = 250000
|
||||||
|
|
||||||
|
MAX_TORQUE_ERROR = 350
|
||||||
|
|
||||||
|
def twos_comp(val, bits):
|
||||||
|
if val >= 0:
|
||||||
|
return val
|
||||||
|
else:
|
||||||
|
return (2**bits) + val
|
||||||
|
|
||||||
|
|
||||||
|
class TestToyotaSafety(unittest.TestCase):
|
||||||
|
@classmethod
|
||||||
|
def setUp(cls):
|
||||||
|
cls.safety = libpandasafety_py.libpandasafety
|
||||||
|
cls.safety.toyota_init(100)
|
||||||
|
cls.safety.init_tests_toyota()
|
||||||
|
|
||||||
|
def _set_prev_torque(self, t):
|
||||||
|
self.safety.set_desired_torque_last(t)
|
||||||
|
self.safety.set_rt_torque_last(t)
|
||||||
|
self.safety.set_torque_meas(t, t)
|
||||||
|
|
||||||
|
def _torque_meas_msg(self, torque):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x260 << 21
|
||||||
|
|
||||||
|
t = twos_comp(torque, 16)
|
||||||
|
to_send[0].RDHR = t | ((t & 0xFF) << 16)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _torque_msg(self, torque):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x2E4 << 21
|
||||||
|
|
||||||
|
t = twos_comp(torque, 16)
|
||||||
|
to_send[0].RDLR = t | ((t & 0xFF) << 16)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _accel_msg(self, accel):
|
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_send[0].RIR = 0x343 << 21
|
||||||
|
|
||||||
|
a = twos_comp(accel, 16)
|
||||||
|
to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8)
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self):
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_manually_enable_controls_allowed(self):
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_enable_control_allowed_from_cruise(self):
|
||||||
|
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_push[0].RIR = 0x1D2 << 21
|
||||||
|
to_push[0].RDHR = 0xF00000
|
||||||
|
|
||||||
|
self.safety.toyota_rx_hook(to_push)
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_disable_control_allowed_from_cruise(self):
|
||||||
|
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||||
|
to_push[0].RIR = 0x1D2 << 21
|
||||||
|
to_push[0].RDHR = 0
|
||||||
|
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.toyota_rx_hook(to_push)
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_accel_actuation_limits(self):
|
||||||
|
for accel in np.arange(MIN_ACCEL - 1000, MAX_ACCEL + 1000, 100):
|
||||||
|
for controls_allowed in [True, False]:
|
||||||
|
self.safety.set_controls_allowed(controls_allowed)
|
||||||
|
|
||||||
|
if controls_allowed:
|
||||||
|
send = MIN_ACCEL <= accel <= MAX_ACCEL
|
||||||
|
else:
|
||||||
|
send = accel == 0
|
||||||
|
self.assertEqual(send, self.safety.toyota_tx_hook(self._accel_msg(accel)))
|
||||||
|
|
||||||
|
def test_torque_absolute_limits(self):
|
||||||
|
for controls_allowed in [True, False]:
|
||||||
|
for torque in np.arange(-MAX_TORQUE - 1000, MAX_TORQUE + 1000, MAX_RATE_UP):
|
||||||
|
self.safety.set_controls_allowed(controls_allowed)
|
||||||
|
self.safety.set_rt_torque_last(torque)
|
||||||
|
self.safety.set_torque_meas(torque, torque)
|
||||||
|
self.safety.set_desired_torque_last(torque - MAX_RATE_UP)
|
||||||
|
|
||||||
|
if controls_allowed:
|
||||||
|
send = (-MAX_TORQUE <= torque <= MAX_TORQUE)
|
||||||
|
else:
|
||||||
|
send = torque == 0
|
||||||
|
|
||||||
|
self.assertEqual(send, self.safety.toyota_tx_hook(self._torque_msg(torque)))
|
||||||
|
|
||||||
|
def test_non_realtime_limit_up(self):
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP)))
|
||||||
|
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
|
||||||
|
|
||||||
|
def test_non_realtime_limit_down(self):
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
self.safety.set_rt_torque_last(1000)
|
||||||
|
self.safety.set_torque_meas(500, 500)
|
||||||
|
self.safety.set_desired_torque_last(1000)
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN)))
|
||||||
|
|
||||||
|
self.safety.set_rt_torque_last(1000)
|
||||||
|
self.safety.set_torque_meas(500, 500)
|
||||||
|
self.safety.set_desired_torque_last(1000)
|
||||||
|
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1)))
|
||||||
|
|
||||||
|
def test_exceed_torque_sensor(self):
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
for sign in [-1, 1]:
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
for t in np.arange(0, MAX_TORQUE_ERROR + 10, 10):
|
||||||
|
t *= sign
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
|
||||||
|
|
||||||
|
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10))))
|
||||||
|
|
||||||
|
def test_realtime_limit_up(self):
|
||||||
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
||||||
|
for sign in [-1, 1]:
|
||||||
|
self.safety.init_tests_toyota()
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
for t in np.arange(0, 380, 10):
|
||||||
|
t *= sign
|
||||||
|
self.safety.set_torque_meas(t, t)
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
|
||||||
|
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * 380)))
|
||||||
|
|
||||||
|
self._set_prev_torque(0)
|
||||||
|
for t in np.arange(0, 370, 10):
|
||||||
|
t *= sign
|
||||||
|
self.safety.set_torque_meas(t, t)
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
|
||||||
|
|
||||||
|
# Increase timer to update rt_torque_last
|
||||||
|
self.safety.set_timer(RT_INTERVAL + 1)
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 370)))
|
||||||
|
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 380)))
|
||||||
|
|
||||||
|
def test_torque_measurements(self):
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(50))
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(-50))
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
|
||||||
|
|
||||||
|
self.assertEqual(-51, self.safety.get_torque_meas_min())
|
||||||
|
self.assertEqual(51, self.safety.get_torque_meas_max())
|
||||||
|
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
|
||||||
|
self.assertEqual(-1, self.safety.get_torque_meas_max())
|
||||||
|
self.assertEqual(-51, self.safety.get_torque_meas_min())
|
||||||
|
|
||||||
|
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
|
||||||
|
self.assertEqual(-1, self.safety.get_torque_meas_max())
|
||||||
|
self.assertEqual(-1, self.safety.get_torque_meas_min())
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
Loading…
Reference in New Issue