diff --git a/VERSION b/VERSION index 9cb4db99be..efdb8b1801 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.1.1 \ No newline at end of file +v1.1.2 \ No newline at end of file diff --git a/board/drivers/can.h b/board/drivers/can.h index 839e46392b..cd809704b9 100644 --- a/board/drivers/can.h +++ b/board/drivers/can.h @@ -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)); + } } } } diff --git a/board/drivers/canbitbang.h b/board/drivers/canbitbang.h new file mode 100644 index 0000000000..5226366b17 --- /dev/null +++ b/board/drivers/canbitbang.h @@ -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<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 + diff --git a/board/gpio.h b/board/gpio.h index 7b2812e7b4..6112f6f887 100644 --- a/board/gpio.h +++ b/board/gpio.h @@ -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; diff --git a/board/safety.h b/board/safety.h index 89b9e090ea..411d757a9b 100644 --- a/board/safety.h +++ b/board/safety.h @@ -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); +} diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h new file mode 100644 index 0000000000..dfc15d0a35 --- /dev/null +++ b/board/safety/safety_cadillac.h @@ -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, +}; diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index 16ab3643f8..8042cc3ce0 100644 --- a/board/safety/safety_defaults.h +++ b/board/safety/safety_defaults.h @@ -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, }; diff --git a/board/safety/safety_elm327.h b/board/safety/safety_elm327.h index 0b23fa5dac..98dce6532a 100644 --- a/board/safety/safety_elm327.h +++ b/board/safety/safety_elm327.h @@ -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, }; diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 55b9900387..804064842b 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -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, }; diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 03c36d6255..e70332c1eb 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -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; } } diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index bc6ce6e9c0..b64e45ce6d 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -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, }; diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index c78e090d09..da4bd388dd 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -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, }; diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h index 6b96c3cdd6..35b7e2b597 100644 --- a/board/safety/safety_toyota_ipas.h +++ b/board/safety/safety_toyota_ipas.h @@ -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, }; diff --git a/tests/gmbitbang/recv.py b/tests/gmbitbang/recv.py new file mode 100755 index 0000000000..6eb70aa4ad --- /dev/null +++ b/tests/gmbitbang/recv.py @@ -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 diff --git a/tests/gmbitbang/rigol.py b/tests/gmbitbang/rigol.py new file mode 100755 index 0000000000..3d690fdd84 --- /dev/null +++ b/tests/gmbitbang/rigol.py @@ -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 + diff --git a/tests/gmbitbang/test.py b/tests/gmbitbang/test.py new file mode 100755 index 0000000000..652ac1ddd8 --- /dev/null +++ b/tests/gmbitbang/test.py @@ -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) + diff --git a/tests/gmbitbang/test_one.py b/tests/gmbitbang/test_one.py new file mode 100755 index 0000000000..a398e27803 --- /dev/null +++ b/tests/gmbitbang/test_one.py @@ -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) + diff --git a/tests/gmbitbang/test_packer.c b/tests/gmbitbang/test_packer.c new file mode 100644 index 0000000000..f056dd4812 --- /dev/null +++ b/tests/gmbitbang/test_packer.c @@ -0,0 +1,32 @@ +#include +#include + +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; +} + + + diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 2d908488a1..8840c66f31 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -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); diff --git a/tests/safety/test.c b/tests/safety/test.c index eb19ab28c0..f0f2af506d 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -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; diff --git a/tests/safety/test_cadillac.py b/tests/safety/test_cadillac.py new file mode 100644 index 0000000000..53c943e966 --- /dev/null +++ b/tests/safety/test_cadillac.py @@ -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() diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py new file mode 100644 index 0000000000..f11360dbe2 --- /dev/null +++ b/tests/safety/test_gm.py @@ -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() diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index fb169af7e3..143916d609 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -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() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index 1f7a9d0ac0..ad86be63be 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -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())