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

Merge panda subtree

Vehicle Researcher 2018-08-02 02:55:38 +00:00
parent b63d51d3ef 76494ab0be
commit 6b1efbf185
7 changed files with 112 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
panda/VERSION
View File

@ -1 +1 @@
v1.1.2
v1.1.3

98
panda/board/safety/safety_toyota.h
View File

@ -1,32 +1,34 @@
struct sample_t torque_meas; // last 3 motor torques produced by the eps
int toyota_no_dsu_car = 0; // ch-r and camry don't have the DSU
int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high?
// global torque limit
const int MAX_TORQUE = 1500; // max torque cmd allowed ever
const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever
// rate based torque limit + stay within actually applied
// packet is sent at 100hz, so this limit is 1000/sec
const int MAX_RATE_UP = 10; // ramp up slow
const int MAX_RATE_DOWN = 25; // ramp down fast
const int MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor
const int TOYOTA_MAX_RATE_UP = 10; // ramp up slow
const int TOYOTA_MAX_RATE_DOWN = 25; // ramp down fast
const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int RT_INTERVAL = 250000; // 250ms between real time checks
const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks
// longitudinal limits
const int MAX_ACCEL = 1500; // 1.5 m/s2
const int MIN_ACCEL = -3000; // 3.0 m/s2
const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2
const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2
// global actuation limit state
int actuation_limits = 1; // by default steer limits are imposed
int dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
int toyota_actuation_limits = 1; // by default steer limits are imposed
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
// state of torque limits
int desired_torque_last = 0; // last desired steer torque
int rt_torque_last = 0; // last desired torque for real time check
uint32_t ts_last = 0;
int cruise_engaged_last = 0; // cruise state
int toyota_desired_torque_last = 0; // last desired steer torque
int toyota_rt_torque_last = 0; // last desired torque for real time check
uint32_t toyota_ts_last = 0;
int toyota_cruise_engaged_last = 0; // cruise state
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
@ -36,26 +38,38 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
torque_meas_new = to_signed(torque_meas_new, 16);
// scale by dbc_factor
torque_meas_new = (torque_meas_new * dbc_eps_torque_factor) / 100;
torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100;
// increase torque_meas by 1 to be conservative on rounding
torque_meas_new += (torque_meas_new > 0 ? 1 : -1);
// update array of sample
update_sample(&torque_meas, torque_meas_new);
update_sample(&toyota_torque_meas, torque_meas_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((to_push->RIR>>21) == 0x1D2) {
// 4 bits: 55-52
int cruise_engaged = to_push->RDHR & 0xF00000;
if (cruise_engaged && !cruise_engaged_last) {
if (cruise_engaged && !toyota_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
controls_allowed = 0;
}
cruise_engaged_last = cruise_engaged;
toyota_cruise_engaged_last = cruise_engaged;
}
int bus = (to_push->RDTR >> 4) & 0xF;
// 0x680 is a radar msg only found in dsu-less cars
if ((to_push->RIR>>21) == 0x680 && (bus == 1)) {
toyota_no_dsu_car = 1;
}
// 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high
if ((to_push->RIR>>21) == 0x2E4 && (bus == 0)) {
toyota_giraffe_switch_1 = 1;
}
}
static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@ -70,8 +84,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if ((to_send->RIR>>21) == 0x343) {
int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
desired_accel = to_signed(desired_accel, 16);
if (controls_allowed && actuation_limits) {
int violation = max_limit_check(desired_accel, MAX_ACCEL, MIN_ACCEL);
if (controls_allowed && toyota_actuation_limits) {
int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) return 0;
} else if (!controls_allowed && (desired_accel != 0)) {
return 0;
@ -87,25 +101,26 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t ts = TIM2->CNT;
// only check if controls are allowed and actuation_limits are imposed
if (controls_allowed && actuation_limits) {
if (controls_allowed && toyota_actuation_limits) {
// *** global torque limit check ***
violation |= max_limit_check(desired_torque, MAX_TORQUE, -MAX_TORQUE);
violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE);
// *** torque rate limit check ***
violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, MAX_RATE_UP, MAX_RATE_DOWN, MAX_TORQUE_ERROR);
violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last,
&toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR);
// used next time
desired_torque_last = desired_torque;
toyota_desired_torque_last = desired_torque;
// *** torque real time rate limit check ***
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, MAX_RT_DELTA);
violation |= rt_rate_limit_check(desired_torque, toyota_rt_torque_last, TOYOTA_MAX_RT_DELTA);
// every RT_INTERVAL set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
if (ts_elapsed > RT_INTERVAL) {
rt_torque_last = desired_torque;
ts_last = ts;
uint32_t ts_elapsed = get_ts_elapsed(ts, toyota_ts_last);
if (ts_elapsed > TOYOTA_RT_INTERVAL) {
toyota_rt_torque_last = desired_torque;
toyota_ts_last = ts;
}
}
@ -116,9 +131,9 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// reset to 0 if either controls is not allowed or there's a violation
if (violation || !controls_allowed) {
desired_torque_last = 0;
rt_torque_last = 0;
ts_last = ts;
toyota_desired_torque_last = 0;
toyota_rt_torque_last = 0;
toyota_ts_last = ts;
}
if (violation) {
@ -138,11 +153,19 @@ static int toyota_tx_lin_hook(int lin_num, uint8_t *data, int len) {
static void toyota_init(int16_t param) {
controls_allowed = 0;
actuation_limits = 1;
dbc_eps_torque_factor = param;
toyota_actuation_limits = 1;
toyota_giraffe_switch_1 = 0;
toyota_dbc_eps_torque_factor = param;
}
static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// forward cam to radar and viceversa if car is dsu-less, except lkas cmd and hud
if ((bus_num == 0 || bus_num == 2) && toyota_no_dsu_car && !toyota_giraffe_switch_1) {
int addr = to_fwd->RIR>>21;
bool is_lkas_msg = (addr == 0x2E4 || addr == 0x412) && bus_num == 2;
return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2);
}
return -1;
}
@ -157,8 +180,9 @@ const safety_hooks toyota_hooks = {
static void toyota_nolimits_init(int16_t param) {
controls_allowed = 0;
actuation_limits = 0;
dbc_eps_torque_factor = param;
toyota_actuation_limits = 0;
toyota_giraffe_switch_1 = 0;
toyota_dbc_eps_torque_factor = param;
}
const safety_hooks toyota_nolimits_hooks = {

8
panda/board/safety/safety_toyota_ipas.h
View File

@ -2,7 +2,7 @@
// TODO: refactor to repeat less code
// IPAS override
const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
struct lookup_t {
float x[3];
@ -92,7 +92,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// every RT_INTERVAL or when controls are turned on, set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last);
if ((ts_elapsed > RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) {
if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) {
rt_angle_last = angle_meas_new;
ts_angle_last = ts;
}
@ -118,8 +118,8 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on high steering override
if (angle_control && ((torque_driver.min > IPAS_OVERRIDE_THRESHOLD) ||
(torque_driver.max < -IPAS_OVERRIDE_THRESHOLD) ||
if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
(torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
(ipas_state==5))) {
controls_allowed = 0;
}

10
panda/examples/tesla_tester.py
View File

@ -29,11 +29,11 @@ def tesla_tester():
# BDY 0x248 is the MCU_commands message, which includes folding mirrors, opening the trunk, frunk, setting the cars lock state and more. For our test, we will edit the 3rd byte, which is MCU_lockRequest. 0x01 will lock, 0x02 will unlock:
print("Unlocking Tesla...")
p.can_send(0x248, "\x00\x00\x02\x00\x00\x00\x00\x00", bus_num)
p.can_send(0x248, "\x00\x00\x02\x00\x00\x00\x00\x00", body_bus_num)
#Or, we can set the first byte, MCU_frontHoodCommand + MCU_liftgateSwitch, to 0x01 to pop the frunk, or 0x04 to open/close the trunk (0x05 should open both)
print("Opening Frunk...")
p.can_send(0x248, "\x01\x00\x00\x00\x00\x00\x00\x00", bus_num)
p.can_send(0x248, "\x01\x00\x00\x00\x00\x00\x00\x00", body_bus_num)
#Back to safety...
print("Disabling output on Panda...")
@ -41,7 +41,6 @@ def tesla_tester():
print("Reading VIN from 0x568. This is painfully slow and can take up to 3 minutes (1 minute per message; 3 messages needed for full VIN)...")
cnt = 0
vin = {}
while True:
#Read the VIN
@ -53,11 +52,10 @@ def tesla_tester():
vin_string = binascii.hexlify(dat)[2:] #rest of the string is the actual VIN data
vin[vin_index] = vin_string.decode("hex")
print("Got VIN index " + str(vin_index) + " data " + vin[vin_index])
cnt += 1
#if we have all 3 parts of the VIN, print it and break out of our while loop
if cnt == 3:
if 0 in vin and 1 in vin and 2 in vin:
print("VIN: " + vin[0] + vin[1] + vin[2][:3])
break
if __name__ == "__main__":
tesla_tester()
tesla_tester()

10
panda/tests/safety/libpandasafety_py.py
View File

@ -38,13 +38,13 @@ void reset_angle_control(void);
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_toyota_torque_meas(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_desired_torque_last(int t);
int get_torque_meas_min(void);
int get_torque_meas_max(void);
void set_toyota_rt_torque_last(int t);
void set_toyota_desired_torque_last(int t);
int get_toyota_torque_meas_min(void);
int get_toyota_torque_meas_max(void);
void init_tests_honda(void);
int get_ego_speed(void);

34
panda/tests/safety/test.c
View File

@ -22,7 +22,7 @@ typedef struct
uint32_t CNT;
} TIM_TypeDef;
struct sample_t torque_meas;
struct sample_t toyota_torque_meas;
struct sample_t cadillac_torque_driver;
struct sample_t gm_torque_driver;
@ -60,9 +60,9 @@ void set_timer(int t){
timer.CNT = t;
}
void set_torque_meas(int min, int max){
torque_meas.min = min;
torque_meas.max = max;
void set_toyota_torque_meas(int min, int max){
toyota_torque_meas.min = min;
toyota_torque_meas.max = max;
}
void set_cadillac_torque_driver(int min, int max){
@ -75,16 +75,16 @@ void set_gm_torque_driver(int min, int max){
gm_torque_driver.max = max;
}
int get_torque_meas_min(void){
return torque_meas.min;
int get_toyota_torque_meas_min(void){
return toyota_torque_meas.min;
}
int get_torque_meas_max(void){
return torque_meas.max;
int get_toyota_torque_meas_max(void){
return toyota_torque_meas.max;
}
void set_rt_torque_last(int t){
rt_torque_last = t;
void set_toyota_rt_torque_last(int t){
toyota_rt_torque_last = t;
}
void set_cadillac_rt_torque_last(int t){
@ -95,8 +95,8 @@ void set_gm_rt_torque_last(int t){
gm_rt_torque_last = t;
}
void set_desired_torque_last(int t){
desired_torque_last = t;
void set_toyota_desired_torque_last(int t){
toyota_desired_torque_last = t;
}
void set_cadillac_desired_torque_last(int t){
@ -129,11 +129,11 @@ void set_bosch_hardware(bool c){
}
void init_tests_toyota(void){
torque_meas.min = 0;
torque_meas.max = 0;
desired_torque_last = 0;
rt_torque_last = 0;
ts_last = 0;
toyota_torque_meas.min = 0;
toyota_torque_meas.max = 0;
toyota_desired_torque_last = 0;
toyota_rt_torque_last = 0;
toyota_ts_last = 0;
set_timer(0);
}

40
panda/tests/safety/test_toyota.py
View File

@ -41,9 +41,9 @@ class TestToyotaSafety(unittest.TestCase):
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)
self.safety.set_toyota_desired_torque_last(t)
self.safety.set_toyota_rt_torque_last(t)
self.safety.set_toyota_torque_meas(t, t)
def _torque_meas_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
@ -158,9 +158,9 @@ class TestToyotaSafety(unittest.TestCase):
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)
self.safety.set_toyota_rt_torque_last(torque)
self.safety.set_toyota_torque_meas(torque, torque)
self.safety.set_toyota_desired_torque_last(torque - MAX_RATE_UP)
if controls_allowed:
send = (-MAX_TORQUE <= torque <= MAX_TORQUE)
@ -181,14 +181,14 @@ class TestToyotaSafety(unittest.TestCase):
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.safety.set_toyota_rt_torque_last(1000)
self.safety.set_toyota_torque_meas(500, 500)
self.safety.set_toyota_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.safety.set_toyota_rt_torque_last(1000)
self.safety.set_toyota_torque_meas(500, 500)
self.safety.set_toyota_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):
@ -210,14 +210,14 @@ class TestToyotaSafety(unittest.TestCase):
self._set_prev_torque(0)
for t in np.arange(0, 380, 10):
t *= sign
self.safety.set_torque_meas(t, t)
self.safety.set_toyota_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.safety.set_toyota_torque_meas(t, t)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
@ -233,16 +233,16 @@ class TestToyotaSafety(unittest.TestCase):
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
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.assertEqual(-51, self.safety.get_toyota_torque_meas_min())
self.assertEqual(51, self.safety.get_toyota_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.assertEqual(-1, self.safety.get_toyota_torque_meas_max())
self.assertEqual(-51, self.safety.get_toyota_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())
self.assertEqual(-1, self.safety.get_toyota_torque_meas_max())
self.assertEqual(-1, self.safety.get_toyota_torque_meas_min())
def test_ipas_override(self):