From d5b884f824d9218ba5cfd8bf2720c21f6de56306 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Sat, 16 Jun 2018 20:56:02 -0700 Subject: [PATCH] Squashed 'panda/' changes from ef880b76..0dcd84d7 0dcd84d7 Toyota safety: integer division bug 9a268f33 Toyota Safety: cleaner var types 8638650d bump panda version 9ab6a562 gmlan recv test a1a2d979 gmlan test 8efa3897 detect ack f5fab4b4 nicer err ad4d4231 add gmlan fail count bb41ff75 test 998f7c01 oops, set recessive 80051bea autoretry on chime 813218de GM: allowing higher brakes in Volt, so decel can reach between 3 and 3.5 m/s2 74ad3d65 GM: max param definitions 38a9ea9a added gm safety for steering (#123) bf5db45a Safety: made the driver steer check common so it can be shared across multiple safety files ef079e6d Safety: made rate limit check also common dc3cc240 Safety: made common the max torque check as well dbc3568a removing extra spaces 1966bdf3 Safety: made real time rate limit check a shared function e2144776 use timer for can bitbanging cb927337 minor bitbang refactor ed2920cf support extended addressing in canbitbang 36df0996 move speed be46c7a3 Merge pull request #122 from commaai/gmbitbang 7edc88e5 put that back fa66e4b7 Revert "handle rollover" 2ce3a26a handle rollover 223a1fb6 cleanin it up 1ba79077 that space tho d917386b bitbanging works 74af4417 can crc 932d7278 bit stuffing support be225227 bros ok match bros 55da0b65 rigol yea, dj pauly d yea a5775835 working on gmbitbang 875c2bd3 Cadillac: block OP messages if OP is on 7caba241 Addition to Bosch safety to support Hatchback (#111) 63ca46bc modify before we forward bf70f515 Safety: increase buffer for sampled signals. TBD a violation feedback from board to prevent car faults b0541a83 Cadillac: monitoring the 4 torque messages independently cd1dba9f Cadillac: fixed bug in regression safety ca0b6beb Cadillac: fixed typo. Need better regression tests to catch this d9f1e616 Cadillac: simplified the ignition code by removing the timeout logic and resetting controls_allowed = 0 at each init 293fd1ac GM: using real ignition logic. Creedit to Jamezz 8fa507b6 GM: simplified max steer check logic, Cadillac: fixed can parsing bug c7e2c2d6 Cadillac (#119) 83bcaa39 small logic cleanup (#118) 9d92bf27 Cadillac: need to specify car name in const 79ab5af8 Toyota: moved common functions into safety header file 40c8ddaf Cadillac ignition: simplified logic 69be556d Cadillac: better ignition logic d176220c Ignition: made a default hook for GPIO bea51874 Cadillac: added max steer safety dbc11a17 Cadillac: always controls allowed for now ace232a9 Cadillac: ignition bug e2c89d6b Cadillac: changed ignition logic to be based on can presence 528f901b Cadillac: simpler ignition logic 4e79ecf1 Cadillac: added safety file placeholder git-subtree-dir: panda git-subtree-split: 0dcd84d7912cd72d3aeaad4653007d1f178a1567 --- VERSION | 2 +- board/drivers/can.h | 16 +- board/drivers/canbitbang.h | 203 ++++++++++++++++++++++ board/gpio.h | 2 +- board/safety.h | 100 +++++++++++ board/safety/safety_cadillac.h | 131 +++++++++++++++ board/safety/safety_defaults.h | 15 +- board/safety/safety_elm327.h | 6 +- board/safety/safety_ford.h | 6 +- board/safety/safety_gm.h | 90 +++++++--- board/safety/safety_honda.h | 33 ++-- board/safety/safety_toyota.h | 94 +++-------- board/safety/safety_toyota_ipas.h | 9 +- tests/gmbitbang/recv.py | 17 ++ tests/gmbitbang/rigol.py | 35 ++++ tests/gmbitbang/test.py | 33 ++++ tests/gmbitbang/test_one.py | 23 +++ tests/gmbitbang/test_packer.c | 32 ++++ tests/safety/libpandasafety_py.py | 18 ++ tests/safety/test.c | 55 ++++++ tests/safety/test_cadillac.py | 182 ++++++++++++++++++++ tests/safety/test_gm.py | 270 ++++++++++++++++++++++++++++++ tests/safety/test_honda.py | 51 +++++- tests/safety/test_toyota.py | 7 +- 24 files changed, 1291 insertions(+), 139 deletions(-) create mode 100644 board/drivers/canbitbang.h create mode 100644 board/safety/safety_cadillac.h create mode 100755 tests/gmbitbang/recv.py create mode 100755 tests/gmbitbang/rigol.py create mode 100755 tests/gmbitbang/test.py create mode 100755 tests/gmbitbang/test_one.py create mode 100644 tests/gmbitbang/test_packer.c create mode 100644 tests/safety/test_cadillac.py create mode 100644 tests/safety/test_gm.py 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())