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

Merge panda subtree

Vehicle Researcher 2018-06-16 20:56:03 -07:00
parent 2751d87d01 d5b884f824
commit c210011c84
24 changed files with 1291 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
panda/VERSION
View File

@ -1 +1 @@
v1.1.1
v1.1.2

16
panda/board/drivers/can.h
View File

@ -415,6 +415,9 @@ void can_rx(uint8_t can_number) {
to_push.RDLR = CAN->sFIFOMailBox[0].RDLR;
to_push.RDHR = CAN->sFIFOMailBox[0].RDHR;
// modify RDTR for our API
to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
// forwarding (panda only)
#ifdef PANDA
int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
@ -428,8 +431,6 @@ void can_rx(uint8_t can_number) {
}
#endif
// modify RDTR for our API
to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
safety_rx_hook(&to_push);
#ifdef PANDA
@ -460,14 +461,21 @@ void CAN3_SCE_IRQHandler() { can_sce(CAN3); }
#endif
#include "canbitbang.h"
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
if (safety_tx_hook(to_push) && !can_autobaud_enabled[bus_number]) {
if (bus_number < BUS_MAX) {
// add CAN packet to send queue
// bus number isn't passed through
to_push->RDTR &= 0xF;
can_push(can_queues[bus_number], to_push);
process_can(CAN_NUM_FROM_BUS_NUM(bus_number));
if (bus_number == 3 && can_num_lookup[3] == 0xFF) {
// TODO: why uint8 bro? only int8?
bitbang_gmlan(to_push);
} else {
can_push(can_queues[bus_number], to_push);
process_can(CAN_NUM_FROM_BUS_NUM(bus_number));
}
}
}
}

203
panda/board/drivers/canbitbang.h 100644
View File

@ -0,0 +1,203 @@
#define MAX_BITS_CAN_PACKET (200)
// returns out_len
int do_bitstuff(char *out, char *in, int in_len) {
int last_bit = -1;
int bit_cnt = 0;
int j = 0;
for (int i = 0; i < in_len; i++) {
char bit = in[i];
out[j++] = bit;
// do the stuffing
if (bit == last_bit) {
bit_cnt++;
if (bit_cnt == 5) {
// 5 in a row the same, do stuff
last_bit = !bit;
out[j++] = last_bit;
bit_cnt = 1;
}
} else {
// this is a new bit
last_bit = bit;
bit_cnt = 1;
}
}
return j;
}
int append_crc(char *in, int in_len) {
int crc = 0;
for (int i = 0; i < in_len; i++) {
crc <<= 1;
if (in[i] ^ ((crc>>15)&1)) {
crc = crc ^ 0x4599;
}
crc &= 0x7fff;
}
for (int i = 14; i >= 0; i--) {
in[in_len++] = (crc>>i)&1;
}
return in_len;
}
int append_bits(char *in, int in_len, char *app, int app_len) {
for (int i = 0; i < app_len; i++) {
in[in_len++] = app[i];
}
return in_len;
}
int append_int(char *in, int in_len, int val, int val_len) {
for (int i = val_len-1; i >= 0; i--) {
in[in_len++] = (val&(1<<i)) != 0;
}
return in_len;
}
int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
char pkt[MAX_BITS_CAN_PACKET];
char footer[] = {
1, // CRC delimiter
1, // ACK
1, // ACK delimiter
1,1,1,1,1,1,1, // EOF
1,1,1, // IFS
};
int len = 0;
// test packet
int dlc_len = to_bang->RDTR & 0xF;
len = append_int(pkt, len, 0, 1); // Start-of-frame
if (to_bang->RIR & 4) {
// extended identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 3, 2); // SRR+IDE
len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1<<18)-1), 18); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+r1+r0
} else {
// standard identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved
}
len = append_int(pkt, len, dlc_len, 4); // Data length code
// append data
for (int i = 0; i < dlc_len; i++) {
unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
len = append_int(pkt, len, dat, 8);
}
// append crc
len = append_crc(pkt, len);
// do bitstuffing
len = do_bitstuff(out, pkt, len);
// append footer
len = append_bits(out, len, footer, sizeof(footer));
return len;
}
// hardware stuff below this line
#ifdef PANDA
void set_bitbanged_gmlan(int val) {
if (val) {
GPIOB->ODR |= (1 << 13);
} else {
GPIOB->ODR &= ~(1 << 13);
}
}
char pkt_stuffed[MAX_BITS_CAN_PACKET];
int gmlan_sending = -1;
int gmlan_sendmax = -1;
int gmlan_silent_count = 0;
int gmlan_fail_count = 0;
#define REQUIRED_SILENT_TIME 10
#define MAX_FAIL_COUNT 10
void TIM4_IRQHandler(void) {
if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) {
int read = get_gpio_input(GPIOB, 12);
if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
if (read == 0) {
gmlan_silent_count = 0;
} else {
gmlan_silent_count++;
}
} else if (gmlan_silent_count == REQUIRED_SILENT_TIME) {
int retry = 0;
// in send loop
if (gmlan_sending > 0 && // not first bit
(read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant
gmlan_sending != (gmlan_sendmax-11)) { //not ack bit
puts("GMLAN ERR: bus driven at ");
puth(gmlan_sending);
puts("\n");
retry = 1;
} else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK
puts("GMLAN ERR: didn't recv ACK\n");
retry = 1;
}
if (retry) {
// reset sender (retry after 7 silent)
set_bitbanged_gmlan(1); // recessive
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_fail_count++;
if (gmlan_fail_count == MAX_FAIL_COUNT) {
puts("GMLAN ERR: giving up send\n");
}
} else {
set_bitbanged_gmlan(pkt_stuffed[gmlan_sending]);
gmlan_sending++;
}
}
if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) {
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_INPUT);
TIM4->DIER = 0; // no update interrupt
TIM4->CR1 = 0; // disable timer
gmlan_sendmax = -1; // exit
}
}
TIM4->SR = 0;
}
void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
// TODO: make failure less silent
if (gmlan_sendmax != -1) return;
int len = get_bit_message(pkt_stuffed, to_bang);
gmlan_fail_count = 0;
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_sendmax = len;
// setup for bitbang loop
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
// setup
TIM4->PSC = 48-1; // tick on 1 us
TIM4->CR1 = TIM_CR1_CEN; // enable
TIM4->ARR = 30-1; // 33.3 kbps
// in case it's disabled
NVIC_EnableIRQ(TIM4_IRQn);
// run the interrupt
TIM4->DIER = TIM_DIER_UIE; // update interrupt
TIM4->SR = 0;
}
#endif

2
panda/board/gpio.h
View File

@ -119,7 +119,7 @@ void periph_init() {
RCC->APB1ENR |= RCC_APB1ENR_DACEN;
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
//RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;

100
panda/board/safety.h
View File

@ -1,7 +1,24 @@
// sample struct that keeps 3 samples in memory
struct sample_t {
int values[6];
int min;
int max;
} sample_t_default = {{0}, 0, 0};
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
int safety_ignition_hook();
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);
void update_sample(struct sample_t *sample, int sample_new);
int max_limit_check(int val, const int MAX);
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@ -31,6 +48,7 @@ int controls_allowed = 0;
#endif
#include "safety/safety_gm.h"
#include "safety/safety_ford.h"
#include "safety/safety_cadillac.h"
#include "safety/safety_elm327.h"
const safety_hooks *current_hooks = &nooutput_hooks;
@ -68,6 +86,7 @@ typedef struct {
#define SAFETY_GM 3
#define SAFETY_HONDA_BOSCH 4
#define SAFETY_FORD 5
#define SAFETY_CADILLAC 6
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@ -80,6 +99,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_TOYOTA, &toyota_hooks},
{SAFETY_GM, &gm_hooks},
{SAFETY_FORD, &ford_hooks},
{SAFETY_CADILLAC, &cadillac_hooks},
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
#ifdef PANDA
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
@ -101,3 +121,83 @@ int safety_set_mode(uint16_t mode, int16_t param) {
return -1;
}
// compute the time elapsed (in microseconds) from 2 counter samples
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts;
}
// convert a trimmed integer to signed 32 bit int
int to_signed(int d, int bits) {
if (d >= (1 << (bits - 1))) {
d -= (1 << bits);
}
return d;
}
// given a new sample, update the smaple_t struct
void update_sample(struct sample_t *sample, int sample_new) {
for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) {
sample->values[i] = sample->values[i-1];
}
sample->values[0] = sample_new;
// get the minimum and maximum measured samples
sample->min = sample->max = sample->values[0];
for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) {
if (sample->values[i] < sample->min) sample->min = sample->values[i];
if (sample->values[i] > sample->max) sample->max = sample->values[i];
}
}
int max_limit_check(int val, const int MAX) {
return (val > MAX) | (val < -MAX);
}
// check that commanded value isn't too far from measured
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
// *** val rate limit check ***
int16_t highest_allowed_val = max(val_last, 0) + MAX_RATE_UP;
int16_t lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP;
// if we've exceeded the meas val, we must start moving toward 0
highest_allowed_val = min(highest_allowed_val, max(val_last - MAX_RATE_DOWN, max(val_meas->max, 0) + MAX_ERROR));
lowest_allowed_val = max(lowest_allowed_val, min(val_last + MAX_RATE_DOWN, min(val_meas->min, 0) - MAX_ERROR));
// check for violation
return (val < lowest_allowed_val) || (val > highest_allowed_val);
}
// check that commanded value isn't fighting against driver
int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {
int highest_allowed = max(val_last, 0) + MAX_RATE_UP;
int lowest_allowed = min(val_last, 0) - MAX_RATE_UP;
int driver_max_limit = MAX + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
int driver_min_limit = -MAX + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
// if we've exceeded the applied torque, we must start moving toward 0
highest_allowed = min(highest_allowed, max(val_last - MAX_RATE_DOWN,
max(driver_max_limit, 0)));
lowest_allowed = max(lowest_allowed, min(val_last + MAX_RATE_DOWN,
min(driver_min_limit, 0)));
// check for violation
return (val < lowest_allowed) || (val > highest_allowed);
}
// real time check, mainly used for steer torque rate limiter
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// *** torque real time rate limit check ***
int16_t highest_val = max(val_last, 0) + MAX_RT_DELTA;
int16_t lowest_val = min(val_last, 0) - MAX_RT_DELTA;
// check for violation
return (val < lowest_val) || (val > highest_val);
}

131
panda/board/safety/safety_cadillac.h 100644
View File

@ -0,0 +1,131 @@
const int CADILLAC_MAX_STEER = 150; // 1s
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int CADILLAC_MAX_RT_DELTA = 75; // max delta torque allowed for real time checks
const int32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks
const int CADILLAC_MAX_RATE_UP = 2;
const int CADILLAC_MAX_RATE_DOWN = 5;
const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50;
const int CADILLAC_DRIVER_TORQUE_FACTOR = 4;
int cadillac_ign = 0;
int cadillac_cruise_engaged_last = 0;
int cadillac_rt_torque_last = 0;
int cadillac_desired_torque_last[4] = {0}; // 4 torque messages
uint32_t cadillac_ts_last = 0;
int cadillac_supercruise_on = 0;
struct sample_t cadillac_torque_driver; // last few driver torques measured
int cadillac_get_torque_idx(uint32_t addr) {
if (addr==0x151) return 0;
else if (addr==0x152) return 1;
else if (addr==0x153) return 2;
else return 3;
}
static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;
if (addr == 356) {
int torque_driver_new = ((to_push->RDLR & 0x7) << 8) | ((to_push->RDLR >> 8) & 0xFF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&cadillac_torque_driver, torque_driver_new);
}
// this message isn't all zeros when ignition is on
if (addr == 0x160 && bus_number == 0) {
cadillac_ign = to_push->RDLR > 0;
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x370) && (bus_number == 0)) {
int cruise_engaged = to_push->RDLR & 0x800000; // bit 23
if (cruise_engaged && !cadillac_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
controls_allowed = 0;
}
cadillac_cruise_engaged_last = cruise_engaged;
}
// know supercruise mode and block openpilot msgs if on
if ((addr == 0x152) || (addr == 0x154)) {
cadillac_supercruise_on = (to_push->RDHR>>4) & 0x1;
}
}
static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t addr = to_send->RIR >> 21;
// steer cmd checks
if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
int violation = 0;
uint32_t ts = TIM2->CNT;
int idx = cadillac_get_torque_idx(addr);
desired_torque = to_signed(desired_torque, 14);
if (controls_allowed) {
// *** global torque limit check ***
violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER);
// *** torque rate limit check ***
int desired_torque_last = cadillac_desired_torque_last[idx];
violation |= driver_limit_check(desired_torque, desired_torque_last, &cadillac_torque_driver,
CADILLAC_MAX_STEER, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN,
CADILLAC_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR);
// used next time
cadillac_desired_torque_last[idx] = desired_torque;
// *** torque real time rate limit check ***
violation |= rt_rate_limit_check(desired_torque, cadillac_rt_torque_last, CADILLAC_MAX_RT_DELTA);
// every RT_INTERVAL set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
if (ts_elapsed > CADILLAC_RT_INTERVAL) {
cadillac_rt_torque_last = desired_torque;
cadillac_ts_last = ts;
}
}
// no torque if controls is not allowed
if (!controls_allowed && (desired_torque != 0)) {
violation = 1;
}
// reset to 0 if either controls is not allowed or there's a violation
if (violation || !controls_allowed) {
cadillac_desired_torque_last[idx] = 0;
cadillac_rt_torque_last = 0;
cadillac_ts_last = ts;
}
if (violation || cadillac_supercruise_on) {
return false;
}
}
return true;
}
static void cadillac_init(int16_t param) {
controls_allowed = 0;
cadillac_ign = 0;
}
static int cadillac_ign_hook() {
return cadillac_ign;
}
const safety_hooks cadillac_hooks = {
.init = cadillac_init,
.rx = cadillac_rx_hook,
.tx = cadillac_tx_hook,
.tx_lin = alloutput_tx_lin_hook,
.ignition = cadillac_ign_hook,
.fwd = alloutput_fwd_hook,
};

15
panda/board/safety/safety_defaults.h
View File

@ -1,5 +1,9 @@
void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {}
int default_ign_hook() {
return -1; // use GPIO to determine ignition
}
// *** no output safety mode ***
static void nooutput_init(int16_t param) {
@ -14,9 +18,6 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
return false;
}
static int nooutput_ign_hook() {
return -1;
}
static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
@ -26,7 +27,7 @@ const safety_hooks nooutput_hooks = {
.rx = default_rx_hook,
.tx = nooutput_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = nooutput_ign_hook,
.ignition = default_ign_hook,
.fwd = nooutput_fwd_hook,
};
@ -44,10 +45,6 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
return true;
}
static int alloutput_ign_hook() {
return -1;
}
static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
@ -57,7 +54,7 @@ const safety_hooks alloutput_hooks = {
.rx = default_rx_hook,
.tx = alloutput_tx_hook,
.tx_lin = alloutput_tx_lin_hook,
.ignition = alloutput_ign_hook,
.ignition = default_ign_hook,
.fwd = alloutput_fwd_hook,
};

6
panda/board/safety/safety_elm327.h
View File

@ -31,10 +31,6 @@ static void elm327_init(int16_t param) {
controls_allowed = 1;
}
static int elm327_ign_hook() {
return -1;
}
static int elm327_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
@ -44,6 +40,6 @@ const safety_hooks elm327_hooks = {
.rx = elm327_rx_hook,
.tx = elm327_tx_hook,
.tx_lin = elm327_tx_lin_hook,
.ignition = elm327_ign_hook,
.ignition = default_ign_hook,
.fwd = elm327_fwd_hook,
};

6
panda/board/safety/safety_ford.h
View File

@ -96,15 +96,11 @@ static int ford_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
static int ford_ign_hook() {
return -1;
}
const safety_hooks ford_hooks = {
.init = ford_init,
.rx = ford_rx_hook,
.tx = ford_tx_hook,
.tx_lin = ford_tx_lin_hook,
.ignition = ford_ign_hook,
.ignition = default_ign_hook,
.fwd = ford_fwd_hook,
};

90
panda/board/safety/safety_gm.h
View File

@ -8,15 +8,27 @@
// brake rising edge
// 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;
const int GM_MAX_GAS = 3072;
const int GM_MAX_REGEN = 1404;
const int GM_MAX_BRAKE = 350;
int gm_brake_prev = 0;
int gm_gas_prev = 0;
int gm_speed = 0;
// silence everything if stock ECUs are still online
int gm_ascm_detected = 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) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
@ -31,10 +43,18 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
addr = to_push->RIR >> 21;
}
if (addr == 0x135 && bus_number == 0) {
//Gear selector (used for determining ignition)
int gear = to_push->RDLR & 0x7;
gm_ignition_started = gear > 0; //Park = 0. If out of park, we're "on."
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) {
//Bit 5 should be ignition "on"
//Backup plan is Bit 2 (accessory power)
uint32_t ign = (to_push->RDLR) & 0x20;
gm_ignition_started = ign > 0;
}
// sample speed, really only care if car is moving or not
@ -126,7 +146,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int brake = ((rdlr & 0xF) << 8) + ((rdlr & 0xFF00) >> 8);
brake = (0x1000 - brake) & 0xFFF;
if (current_controls_allowed) {
if (brake > 255) return 0;
if (brake > GM_MAX_BRAKE) return 0;
} else {
if (brake != 0) return 0;
}
@ -135,17 +155,49 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// LKA STEER: safety check
if (addr == 384) {
int rdlr = to_send->RDLR;
int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
int max_steer = 255;
int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
uint32_t ts = TIM2->CNT;
int violation = 0;
desired_torque = to_signed(desired_torque, 11);
if (current_controls_allowed) {
// Signed arithmetic
if (steer & 0x400) {
if (steer < (0x800 - max_steer)) return 0;
} else {
if (steer > max_steer) return 0;
// *** global torque limit check ***
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;
}
} else {
if (steer != 0) return 0;
}
// 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;
}
}
@ -158,11 +210,11 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int gas_regen = ((rdlr & 0x7F0000) >> 11) + ((rdlr & 0xF8000000) >> 27);
int apply = rdlr & 1;
if (current_controls_allowed) {
if (gas_regen > 3072) return 0;
if (gas_regen > GM_MAX_GAS) return 0;
} else {
// Disabled message is !engaed with gas
// value that corresponds to max regen.
if (apply || gas_regen != 1404) return 0;
if (apply || gas_regen != GM_MAX_REGEN) return 0;
}
}

33
panda/board/safety/safety_honda.h
View File

@ -16,6 +16,7 @@ int gas_interceptor_prev = 0;
int ego_speed = 0;
// TODO: auto-detect bosch hardware based on CAN messages?
bool bosch_hardware = false;
bool honda_alt_brake_msg = false;
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
@ -36,11 +37,14 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
}
// user brake signal is different for nidec vs bosch hardware
// nidec hardware: 0x17C bit 53
// bosch hardware: 0x1BE bit 4
#define IS_USER_BRAKE_MSG(to_push) (!bosch_hardware ? to_push->RIR>>21 == 0x17C : to_push->RIR>>21 == 0x1BE)
#define USER_BRAKE_VALUE(to_push) (!bosch_hardware ? to_push->RDHR & 0x200000 : to_push->RDLR & 0x10)
// user brake signal on 0x17C reports applied brake from computer brake on accord
// and crv, which prevents the usual brake safety from working correctly. these
// cars have a signal on 0x1BE which only detects user's brake being applied so
// in these cases, this is used instead.
// most hondas: 0x17C bit 53
// accord, crv: 0x1BE bit 4
#define IS_USER_BRAKE_MSG(to_push) (!honda_alt_brake_msg ? to_push->RIR>>21 == 0x17C : to_push->RIR>>21 == 0x1BE)
#define USER_BRAKE_VALUE(to_push) (!honda_alt_brake_msg ? to_push->RDHR & 0x200000 : to_push->RDLR & 0x10)
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (IS_USER_BRAKE_MSG(to_push)) {
@ -115,6 +119,14 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
}
}
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
if (((to_send->RIR>>21) == 0x296) && bosch_hardware &&
!current_controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
if (((to_send->RDLR >> 5) & 0x7) != 2) return 0;
}
// 1 allows the message through
return true;
@ -128,28 +140,27 @@ static int honda_tx_lin_hook(int lin_num, uint8_t *data, int len) {
static void honda_init(int16_t param) {
controls_allowed = 0;
bosch_hardware = false;
honda_alt_brake_msg = false;
}
static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
static int honda_ign_hook() {
return -1;
}
const safety_hooks honda_hooks = {
.init = honda_init,
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.tx_lin = honda_tx_lin_hook,
.ignition = honda_ign_hook,
.ignition = default_ign_hook,
.fwd = honda_fwd_hook,
};
static void honda_bosch_init(int16_t param) {
controls_allowed = 0;
bosch_hardware = true;
// Checking for alternate brake override from safety parameter
honda_alt_brake_msg = param == 1 ? true : false;
}
static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
@ -165,6 +176,6 @@ const safety_hooks honda_bosch_hooks = {
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.tx_lin = honda_tx_lin_hook,
.ignition = honda_ign_hook,
.ignition = default_ign_hook,
.fwd = honda_bosch_fwd_hook,
};

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

@ -1,64 +1,45 @@
// track the torque measured for limiting
struct sample_t {
int values[3];
int min;
int max;
} sample_t_default = {{0, 0, 0}, 0, 0};
struct sample_t torque_meas; // last 3 motor torques produced by the eps
// global torque limit
const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever
const int 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 int32_t MAX_RATE_UP = 10; // ramp up slow
const int32_t MAX_RATE_DOWN = 25; // ramp down fast
const int32_t MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor
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
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int32_t MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int32_t RT_INTERVAL = 250000; // 250ms between real time checks
const int MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int RT_INTERVAL = 250000; // 250ms between real time checks
// longitudinal limits
const int16_t MAX_ACCEL = 1500; // 1.5 m/s2
const int16_t MIN_ACCEL = -3000; // 3.0 m/s2
const int MAX_ACCEL = 1500; // 1.5 m/s2
const int MIN_ACCEL = -3000; // 3.0 m/s2
// global actuation limit state
int actuation_limits = 1; // by default steer limits are imposed
int16_t dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
int dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
// state of torque limits
int16_t desired_torque_last = 0; // last desired steer torque
int16_t rt_torque_last = 0; // last desired torque for real time check
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
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts;
}
void update_sample(struct sample_t *sample, int sample_new) {
for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) {
sample->values[i] = sample->values[i-1];
}
sample->values[0] = sample_new;
// get the minimum and maximum measured torque over the last 3 frames
sample->min = sample->max = sample->values[0];
for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) {
if (sample->values[i] < sample->min) sample->min = sample->values[i];
if (sample->values[i] > sample->max) sample->max = sample->values[i];
}
}
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// get eps motor torque (0.66 factor in dbc)
if ((to_push->RIR>>21) == 0x260) {
int16_t torque_meas_new_16 = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
torque_meas_new = to_signed(torque_meas_new, 16);
// scale by dbc_factor
torque_meas_new = (torque_meas_new * dbc_eps_torque_factor) / 100;
// increase torque_meas by 1 to be conservative on rounding
int torque_meas_new = ((int)(torque_meas_new_16) * dbc_eps_torque_factor / 100) + (torque_meas_new_16 > 0 ? 1 : -1);
torque_meas_new += (torque_meas_new > 0 ? 1 : -1);
// update array of sample
update_sample(&torque_meas, torque_meas_new);
@ -87,7 +68,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// ACCEL: safety check on byte 1-2
if ((to_send->RIR>>21) == 0x343) {
int16_t desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
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) {
if ((desired_accel > MAX_ACCEL) || (desired_accel < MIN_ACCEL)) {
return 0;
@ -99,8 +81,9 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// STEER: safety check on bytes 2-3
if ((to_send->RIR>>21) == 0x2E4) {
int16_t desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF);
int16_t violation = 0;
int desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF);
desired_torque = to_signed(desired_torque, 16);
int violation = 0;
uint32_t ts = TIM2->CNT;
@ -108,35 +91,16 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (controls_allowed && actuation_limits) {
// *** global torque limit check ***
if (desired_torque < -MAX_TORQUE) violation = 1;
if (desired_torque > MAX_TORQUE) violation = 1;
violation |= max_limit_check(desired_torque, MAX_TORQUE);
// *** torque rate limit check ***
int16_t highest_allowed_torque = max(desired_torque_last, 0) + MAX_RATE_UP;
int16_t lowest_allowed_torque = min(desired_torque_last, 0) - MAX_RATE_UP;
// if we've exceeded the applied torque, we must start moving toward 0
highest_allowed_torque = min(highest_allowed_torque, max(desired_torque_last - MAX_RATE_DOWN, max(torque_meas.max, 0) + MAX_TORQUE_ERROR));
lowest_allowed_torque = max(lowest_allowed_torque, min(desired_torque_last + MAX_RATE_DOWN, min(torque_meas.min, 0) - MAX_TORQUE_ERROR));
// check for violation
if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) {
violation = 1;
}
violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, MAX_RATE_UP, MAX_RATE_DOWN, MAX_TORQUE_ERROR);
// used next time
desired_torque_last = desired_torque;
// *** torque real time rate limit check ***
int16_t highest_rt_torque = max(rt_torque_last, 0) + MAX_RT_DELTA;
int16_t lowest_rt_torque = min(rt_torque_last, 0) - MAX_RT_DELTA;
// check for violation
if ((desired_torque < lowest_rt_torque) || (desired_torque > highest_rt_torque)) {
violation = 1;
}
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, MAX_RT_DELTA);
// every RT_INTERVAL set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
@ -179,10 +143,6 @@ static void toyota_init(int16_t param) {
dbc_eps_torque_factor = param;
}
static int toyota_ign_hook() {
return -1;
}
static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return -1;
}
@ -192,7 +152,7 @@ const safety_hooks toyota_hooks = {
.rx = toyota_rx_hook,
.tx = toyota_tx_hook,
.tx_lin = toyota_tx_lin_hook,
.ignition = toyota_ign_hook,
.ignition = default_ign_hook,
.fwd = toyota_fwd_hook,
};
@ -207,6 +167,6 @@ const safety_hooks toyota_nolimits_hooks = {
.rx = toyota_rx_hook,
.tx = toyota_tx_hook,
.tx_lin = toyota_tx_lin_hook,
.ignition = toyota_ign_hook,
.ignition = default_ign_hook,
.fwd = toyota_fwd_hook,
};

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

@ -35,13 +35,6 @@ uint32_t ts_angle_last = 0;
int controls_allowed_last = 0;
int to_signed(int d, int bits) {
if (d >= (1 << (bits - 1))) {
d -= (1 << bits);
}
return d;
}
// interp function that holds extreme values
float interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
@ -187,7 +180,7 @@ const safety_hooks toyota_ipas_hooks = {
.rx = toyota_ipas_rx_hook,
.tx = toyota_ipas_tx_hook,
.tx_lin = toyota_tx_lin_hook,
.ignition = toyota_ign_hook,
.ignition = default_ign_hook,
.fwd = toyota_fwd_hook,
};

17
panda/tests/gmbitbang/recv.py 100755
View File

@ -0,0 +1,17 @@
#!/usr/bin/env python
import time
from panda import Panda
p = Panda()
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
p.set_gmlan(bus=2)
#p.can_send(0xaaa, "\x00\x00", bus=3)
last_add = None
while 1:
ret = p.can_recv()
if len(ret) > 0:
add = ret[0][0]
if last_add is not None and add != last_add+1:
print "MISS %d %d" % (last_add, add)
last_add = add
print ret

35
panda/tests/gmbitbang/rigol.py 100755
View File

@ -0,0 +1,35 @@
#!/usr/bin/env python
import numpy as np
import visa
import matplotlib.pyplot as plt
resources = visa.ResourceManager()
print resources.list_resources()
scope = resources.open_resource('USB0::0x1AB1::0x04CE::DS1ZA184652242::INSTR', timeout=2000, chunk_size=1024000)
print(scope.query('*IDN?').strip())
#voltscale = scope.ask_for_values(':CHAN1:SCAL?')[0]
#voltoffset = scope.ask_for_values(":CHAN1:OFFS?")[0]
#scope.write(":STOP")
scope.write(":WAV:POIN:MODE RAW")
scope.write(":WAV:DATA? CHAN1")[10:]
rawdata = scope.read_raw()
data = np.frombuffer(rawdata, 'B')
print data.shape
s1 = data[0:650]
s2 = data[650:]
s1i = np.argmax(s1 > 100)
s2i = np.argmax(s2 > 100)
s1 = s1[s1i:]
s2 = s2[s2i:]
plt.plot(s1)
plt.plot(s2)
plt.show()
#data = (data - 130.0 - voltoffset/voltscale*25) / 25 * voltscale
print data

33
panda/tests/gmbitbang/test.py 100755
View File

@ -0,0 +1,33 @@
#!/usr/bin/env python
import time
from panda import Panda
p1 = Panda('380016000551363338383037')
p2 = Panda('430026000951363338383037')
# this is a test, no safety
p1.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
p2.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# get versions
print(p1.get_version())
print(p2.get_version())
# this sets bus 2 to actually be GMLAN
p2.set_gmlan(bus=2)
# send w bitbang then without
#iden = 123
iden = 18000
#dat = "\x01\x02"
dat = "\x01\x02\x03\x04\x05\x06\x07\x08"
while 1:
iden += 1
p1.set_gmlan(bus=None)
p1.can_send(iden, dat, bus=3)
#p1.set_gmlan(bus=2)
#p1.can_send(iden, dat, bus=3)
time.sleep(0.01)
print p2.can_recv()
#exit(0)

23
panda/tests/gmbitbang/test_one.py 100755
View File

@ -0,0 +1,23 @@
#!/usr/bin/env python
import time
from panda import Panda
p = Panda()
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# ack any crap on bus
p.set_gmlan(bus=2)
time.sleep(0.1)
while len(p.can_recv()) > 0:
print "clearing"
time.sleep(0.1)
print "cleared"
p.set_gmlan(bus=None)
iden = 18000
dat = "\x01\x02\x03\x04\x05\x06\x07\x08"
while 1:
iden += 1
p.can_send(iden, dat, bus=3)
time.sleep(0.01)

32
panda/tests/gmbitbang/test_packer.c 100644
View File

@ -0,0 +1,32 @@
#include <stdio.h>
#include <stdint.h>
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;
#include "../../board/drivers/canbitbang.h"
int main() {
char out[300];
CAN_FIFOMailBox_TypeDef to_bang = {0};
to_bang.RIR = 20 << 21;
to_bang.RDTR = 1;
to_bang.RDLR = 1;
int len = get_bit_message(out, &to_bang);
printf("T:");
for (int i = 0; i < len; i++) {
printf("%d", out[i]);
}
printf("\n");
printf("R:0000010010100000100010000010011110111010100111111111111111");
printf("\n");
return 0;
}

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

@ -39,6 +39,8 @@ 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_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);
@ -51,6 +53,22 @@ 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);
void set_honda_alt_brake_msg(bool);
void set_bosch_hardware(bool);
void init_tests_cadillac(void);
void cadillac_init(int16_t param);
void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
void set_cadillac_desired_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);
int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);

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

@ -23,6 +23,8 @@ typedef struct
} TIM_TypeDef;
struct sample_t torque_meas;
struct sample_t cadillac_torque_driver;
struct sample_t gm_torque_driver;
TIM_TypeDef timer;
TIM_TypeDef *TIM2 = &timer;
@ -63,6 +65,16 @@ void set_torque_meas(int min, int max){
torque_meas.max = max;
}
void set_cadillac_torque_driver(int min, int max){
cadillac_torque_driver.min = min;
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){
return torque_meas.min;
}
@ -75,10 +87,27 @@ void set_rt_torque_last(int t){
rt_torque_last = t;
}
void set_cadillac_rt_torque_last(int 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){
desired_torque_last = t;
}
void set_cadillac_desired_torque_last(int 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){
return ego_speed;
}
@ -91,6 +120,14 @@ int get_gas_prev(void){
return gas_prev;
}
void set_honda_alt_brake_msg(bool c){
honda_alt_brake_msg = c;
}
void set_bosch_hardware(bool c){
bosch_hardware = c;
}
void init_tests_toyota(void){
torque_meas.min = 0;
torque_meas.max = 0;
@ -100,6 +137,24 @@ void init_tests_toyota(void){
set_timer(0);
}
void init_tests_cadillac(void){
cadillac_torque_driver.min = 0;
cadillac_torque_driver.max = 0;
for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = 0;
cadillac_rt_torque_last = 0;
cadillac_ts_last = 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){
ego_speed = 0;
gas_interceptor_detected = 0;

182
panda/tests/safety/test_cadillac.py 100644
View File

@ -0,0 +1,182 @@
#!/usr/bin/env python2
import unittest
import numpy as np
import libpandasafety_py
MAX_RATE_UP = 2
MAX_RATE_DOWN = 5
MAX_TORQUE = 150
MAX_RT_DELTA = 75
RT_INTERVAL = 250000
DRIVER_TORQUE_ALLOWANCE = 50;
DRIVER_TORQUE_FACTOR = 4;
IPAS_OVERRIDE_THRESHOLD = 200
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 TestCadillacSafety(unittest.TestCase):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.cadillac_init(0)
cls.safety.init_tests_cadillac()
def _set_prev_torque(self, t):
self.safety.set_cadillac_desired_torque_last(t)
self.safety.set_cadillac_rt_torque_last(t)
def _torque_driver_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x164 << 21
t = twos_comp(torque, 11)
to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8)
return to_send
def _torque_driver_msg_array(self, torque):
for i in range(3):
self.safety.cadillac_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 = 0x151 << 21
t = twos_comp(torque, 14)
to_send[0].RDLR = ((t >> 8) & 0x3F) | ((t & 0xFF) << 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())
self.safety.set_controls_allowed(0)
def test_enable_control_allowed_from_cruise(self):
to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_push[0].RIR = 0x370 << 21
to_push[0].RDLR = 0x800000
to_push[0].RDTR = 0
self.safety.cadillac_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 = 0x370 << 21
to_push[0].RDLR = 0
to_push[0].RDTR = 0
self.safety.set_controls_allowed(1)
self.safety.cadillac_rx_hook(to_push)
self.assertFalse(self.safety.get_controls_allowed())
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_cadillac_rt_torque_last(torque)
self.safety.set_cadillac_torque_driver(0, 0)
self.safety.set_cadillac_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.cadillac_tx_hook(self._torque_msg(torque)))
def test_non_realtime_limit_up(self):
self.safety.set_cadillac_torque_driver(0, 0)
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP)))
self._set_prev_torque(0)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
self.safety.set_controls_allowed(True)
self._set_prev_torque(0)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
def test_non_realtime_limit_down(self):
self.safety.set_cadillac_torque_driver(0, 0)
self.safety.set_controls_allowed(True)
def test_exceed_torque_sensor(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_cadillac_torque_driver(t, t)
self._set_prev_torque(MAX_TORQUE * sign)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_TORQUE * sign)))
self.safety.set_cadillac_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_TORQUE)))
# spot check some individual cases
for sign in [-1, 1]:
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign
torque_desired = (MAX_TORQUE - 10 * DRIVER_TORQUE_FACTOR) * sign
delta = 1 * sign
self._set_prev_torque(torque_desired)
self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired)))
self._set_prev_torque(torque_desired + delta)
self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired + delta)))
self._set_prev_torque(MAX_TORQUE * sign)
self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(MAX_TORQUE * sign)
self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(0)))
self._set_prev_torque(MAX_TORQUE * sign)
self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign)
self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - 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_cadillac()
self._set_prev_torque(0)
self.safety.set_cadillac_torque_driver(0, 0)
for t in np.arange(0, MAX_RT_DELTA, 1):
t *= sign
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.cadillac_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.cadillac_tx_hook(self._torque_msg(t)))
# Increase timer to update rt_torque_last
self.safety.set_timer(RT_INTERVAL + 1)
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
if __name__ == "__main__":
unittest.main()

270
panda/tests/safety/test_gm.py 100644
View File

@ -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 = 350
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()

51
panda/tests/safety/test_honda.py
View File

@ -18,9 +18,9 @@ class TestHondaSafety(unittest.TestCase):
return to_send
def _button_msg(self, buttons):
def _button_msg(self, buttons, msg):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x1A6 << 21
to_send[0].RIR = msg << 21
to_send[0].RDLR = buttons << 5
return to_send
@ -32,6 +32,13 @@ class TestHondaSafety(unittest.TestCase):
return to_send
def _alt_brake_msg(self, brake):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x1BE << 21
to_send[0].RDLR = 0x10 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
@ -65,18 +72,20 @@ class TestHondaSafety(unittest.TestCase):
def test_resume_button(self):
RESUME_BTN = 4
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN))
self.safety.set_controls_allowed(0)
self.safety.honda_rx_hook(self._button_msg(RESUME_BTN, 0x1A6))
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.safety.set_controls_allowed(0)
self.safety.honda_rx_hook(self._button_msg(SET_BTN, 0x1A6))
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.safety.honda_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6))
self.assertFalse(self.safety.get_controls_allowed())
def test_sample_speed(self):
@ -94,6 +103,17 @@ class TestHondaSafety(unittest.TestCase):
self.safety.honda_rx_hook(self._brake_msg(1))
self.assertFalse(self.safety.get_controls_allowed())
def test_alt_disengage_on_brake(self):
self.safety.set_honda_alt_brake_msg(1)
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._alt_brake_msg(1))
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_honda_alt_brake_msg(0)
self.safety.set_controls_allowed(1)
self.safety.honda_rx_hook(self._alt_brake_msg(1))
self.assertTrue(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))
@ -101,6 +121,7 @@ class TestHondaSafety(unittest.TestCase):
self.safety.honda_rx_hook(self._brake_msg(True))
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):
# Brake was already pressed
@ -136,13 +157,29 @@ class TestHondaSafety(unittest.TestCase):
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)))
self.safety.set_controls_allowed(0)
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):
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000)))
self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000)))
def test_spam_cancel_safety_check(self):
RESUME_BTN = 4
SET_BTN = 3
CANCEL_BTN = 2
BUTTON_MSG = 0x296
self.safety.set_bosch_hardware(1)
self.safety.set_controls_allowed(0)
self.assertTrue(self.safety.honda_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG)))
self.assertFalse(self.safety.honda_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
self.assertFalse(self.safety.honda_tx_hook(self._button_msg(SET_BTN, BUTTON_MSG)))
# do not block resume if we are engaged already
self.safety.set_controls_allowed(1)
self.assertTrue(self.safety.honda_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
if __name__ == "__main__":
unittest.main()

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

@ -62,7 +62,7 @@ class TestToyotaSafety(unittest.TestCase):
return to_send
def _torque_driver_msg_array(self, torque):
for i in range(3):
for i in range(6):
self.safety.toyota_ipas_rx_hook(self._torque_driver_msg(torque))
def _angle_meas_msg(self, angle):
@ -74,7 +74,7 @@ class TestToyotaSafety(unittest.TestCase):
return to_send
def _angle_meas_msg_array(self, angle):
for i in range(3):
for i in range(6):
self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(angle))
def _torque_msg(self, torque):
@ -229,6 +229,9 @@ class TestToyotaSafety(unittest.TestCase):
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.safety.toyota_rx_hook(self._torque_meas_msg(0))
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())