From eded314669e5fa02429c662b578400cac31f35b4 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Fri, 8 May 2020 11:03:24 -0700 Subject: [PATCH] Squashed 'panda/' changes from 0696730c..ac1b64e6 ac1b64e6 Fix CAN Ignition for Black Panda and Uno (#526) 273e3882 When initializing all the CAN busses, make sure the are also cleared (#527) c2bea78f Fix python library on Windows (#523) 0a123b18 that too ba6355d4 unused lines c9102c00 Chrysler: use can packer in safety tests (#522) 9874e733 Abstract steering safety tests for Toyota and Chrysler (#520) 2299ecff Block 0xe5 (Honda Bosch) at the panda/uno. Only allow static values. (#515) 35173061 Subaru: fix steer torque scaling (#501) 0bc864b3 Make torque-based steering state global (#518) d9355c41 Make cruise_engaged_prev a global + test case for it (#519) 21153764 Abstract sample speed test (#516) 11dc9054 remove unused function e5a586ee Abstract gas interceptor tests (#517) 1dbed65e Safety Test Refactor: Honda (#495) 0632710a base class for different panda safety tests bd98fe60 safety tests: use shorter function name ba59ada0 No ESP in non-white (#514) c3336180 Fix the CAN init fix (#513) 884afa0e Safety Test Refactor: Chrysler and Volkswagen PQ (#508) d77b72d1 Safety Test Refactor: Nissan (#510) 4c7755c4 Match Panda DFU entry fix in "make recover" process (#509) 0336f625 Pedal gas pressed safety limits (#507) 715b1a16 Hyundai-Kia-Genesis (HKG) (#503) 6f105e82 Safety Test Refactor: Subaru (#502) 57cc954f Safety Test Refactor: GM (#504) dd01c3b9 Safety Test Refactor: Hyundai (#505) 592c2c86 add support to can_unique.py for Cabana CSV format. (#506) ccf13b7a No more infinite while loops in CAN init (#499) 6c442b4c Safety Test Refactor: Volkswagen MQB (#493) f07a6ee7 panda recover should go through bootstub first (#498) 8cc3a357 remove cadillac (#496) 62e4d3c3 Chrysler: fix missing button signal on TX (#490) abce8f32 Safety Test Refactor: Toyota + support code (#491) 500370ae Make sure relay faults make it to the health packet (#492) bc90b60f toyota: use universal gas pressed bit (#488) 74d10ccd Fixed possible race condition (#487) a05361eb cleanup safety_replay dockerfile (#486) fe73dcc9 Openpilot-tools is deprecated (#484) da8e00f1 TX message guaranteed delivery (#421) d8f61849 Add ISO number for longitudinal limits flag comment 6a60b781 touch ups 2ce65361 comments on unsafe flags d8801345 remove from there as well 055ea07e remove that unsafe flag since it isn't implemented and it's unclear how to 4e98bbe8 Apply unsafe allow gas mode to all cars. (#480) 0c2c1494 Fixing libusb busy error (#174) 753c42cf Update Board Mac SDK Install script to work on clean mac (#146) b9a9ea39 Unsafe gas disengage mods, fix test compile warning (#481) 08ef92d5 Safety model for Volkswagen PQ35/PQ46/NMS (#474) 51e0a55d Support code for unsafe mode unit tests (#478) 5325b62b current_safety_mode 7908b722 update updating unsafe mode 98503e86 disable stock honda AEB in unsafe mode (#477) 01b2ccbe one more 9a30265a weak steering while not engaged 577f10b1 added options for unsafe mode 83cf7bf4 update comment 4556e749 enable unsafe mode, toggle for use by forks that so choose de89fcdc Nissan leaf (#473) git-subtree-dir: panda git-subtree-split: ac1b64e628d123ae243551e9b154c242da2dc060 --- .circleci/config.yml | 2 +- VERSION | 2 +- board/boards/black.h | 10 +- board/boards/uno.h | 7 +- board/build.mk | 2 +- board/drivers/can.h | 67 ++- board/drivers/llcan.h | 151 +++--- board/drivers/usb.h | 25 +- board/get_sdk_mac.sh | 2 + board/main.c | 18 +- board/pedal/main.c | 4 +- board/safety.h | 14 +- board/safety/safety_cadillac.h | 125 ----- board/safety/safety_chrysler.h | 42 +- board/safety/safety_defaults.h | 4 +- board/safety/safety_ford.h | 18 +- board/safety/safety_gm.h | 42 +- board/safety/safety_honda.h | 80 ++-- board/safety/safety_hyundai.h | 55 ++- board/safety/safety_mazda.h | 5 +- board/safety/safety_nissan.h | 96 ++-- board/safety/safety_subaru.h | 51 +- board/safety/safety_toyota.h | 90 ++-- board/safety/safety_volkswagen.h | 204 +++++++- board/safety_declarations.h | 32 +- board/spi_flasher.h | 1 + examples/can_unique.py | 73 ++- python/__init__.py | 19 +- python/flash_release.py | 29 +- tests/automated/7_can_loopback.py | 42 ++ tests/bulk_write_test.py | 38 ++ tests/safety/Dockerfile | 34 +- tests/safety/common.py | 450 +++++++++++++++--- tests/safety/libpandasafety_py.py | 55 +-- tests/safety/test.c | 230 +++------ tests/safety/test_cadillac.py | 184 -------- tests/safety/test_chrysler.py | 266 ++--------- tests/safety/test_gm.py | 270 +++++------ tests/safety/test_honda.py | 514 +++++++++------------ tests/safety/test_hyundai.py | 199 +++----- tests/safety/test_nissan.py | 187 +++----- tests/safety/test_subaru.py | 267 ++++------- tests/safety/test_toyota.py | 360 +++------------ tests/safety/test_volkswagen_mqb.py | 347 +++++--------- tests/safety/test_volkswagen_pq.py | 278 +++++++++++ tests/safety_replay/Dockerfile | 29 +- tests/safety_replay/requirements_extra.txt | 3 + tests/safety_replay/test_safety_replay.py | 6 +- 48 files changed, 2468 insertions(+), 2561 deletions(-) delete mode 100644 board/safety/safety_cadillac.h create mode 100755 tests/bulk_write_test.py delete mode 100644 tests/safety/test_cadillac.py mode change 100644 => 100755 tests/safety/test_toyota.py create mode 100644 tests/safety/test_volkswagen_pq.py diff --git a/.circleci/config.yml b/.circleci/config.yml index a4ae26efe51f35..52e456066c6290 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -11,7 +11,7 @@ jobs: - run: name: Run safety test command: | - docker run panda_safety /bin/bash -c "cd /panda/tests/safety; PYTHONPATH=/ ./test.sh" + docker run panda_safety /bin/bash -c "cd /openpilot/panda/tests/safety; PYTHONPATH=/openpilot ./test.sh" misra-c2012: machine: diff --git a/VERSION b/VERSION index 0d687f1e2ba8c5..ad90322ab87e78 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.7.3 \ No newline at end of file +v1.7.5 \ No newline at end of file diff --git a/board/boards/black.h b/board/boards/black.h index 97f322f69b04b0..57305980c48f68 100644 --- a/board/boards/black.h +++ b/board/boards/black.h @@ -23,9 +23,13 @@ void black_enable_can_transciever(uint8_t transciever, bool enabled) { } void black_enable_can_transcievers(bool enabled) { - uint8_t t1 = enabled ? 1U : 2U; // leave transciever 1 enabled to detect CAN ignition - for(uint8_t i=t1; i<=4U; i++) { - black_enable_can_transciever(i, enabled); + for(uint8_t i=1U; i<=4U; i++){ + // Leave main CAN always on for CAN-based ignition detection + if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ + black_enable_can_transciever(i, true); + } else { + black_enable_can_transciever(i, enabled); + } } } diff --git a/board/boards/uno.h b/board/boards/uno.h index a6a06eff3e9974..de430cb572749d 100644 --- a/board/boards/uno.h +++ b/board/boards/uno.h @@ -26,7 +26,12 @@ void uno_enable_can_transciever(uint8_t transciever, bool enabled) { void uno_enable_can_transcievers(bool enabled) { for(uint8_t i=1U; i<=4U; i++){ - uno_enable_can_transciever(i, enabled); + // Leave main CAN always on for CAN-based ignition detection + if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){ + uno_enable_can_transciever(i, true); + } else { + uno_enable_can_transciever(i, enabled); + } } } diff --git a/board/build.mk b/board/build.mk index 21daf53ad02710..5e205f0287cf8d 100644 --- a/board/build.mk +++ b/board/build.mk @@ -42,7 +42,7 @@ bin: obj/$(PROJ_NAME).bin # this flashes everything recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin - -PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootloader=True)" + -PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" sleep 1.0 $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin diff --git a/board/drivers/can.h b/board/drivers/can.h index 93db2565ca5342..3a6eeface9978f 100644 --- a/board/drivers/can.h +++ b/board/drivers/can.h @@ -26,8 +26,9 @@ extern uint32_t can_speed[4]; void can_set_forwarding(int from, int to); -void can_init(uint8_t can_number); +bool can_init(uint8_t can_number); void can_init_all(void); +bool can_tx_check_min_slots_free(uint32_t min); void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook); bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem); @@ -107,6 +108,20 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { return ret; } +uint32_t can_slots_empty(can_ring *q) { + uint32_t ret = 0; + + ENTER_CRITICAL(); + if (q->w_ptr >= q->r_ptr) { + ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr; + } else { + ret = q->r_ptr - q->w_ptr - 1U; + } + EXIT_CRITICAL(); + + return ret; +} + void can_clear(can_ring *q) { ENTER_CRITICAL(); q->w_ptr = 0; @@ -133,27 +148,27 @@ uint32_t can_speed[] = {5000, 5000, 5000, 333}; #define CANIF_FROM_CAN_NUM(num) (cans[num]) #define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2)) -#define CAN_NAME_FROM_CANIF(CAN) ((CAN)==CAN1 ? "CAN1" : ((CAN) == CAN2 ? "CAN2" : "CAN3")) #define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num]) #define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num]) void process_can(uint8_t can_number); -void can_set_speed(uint8_t can_number) { +bool can_set_speed(uint8_t can_number) { + bool ret = true; CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - if (!llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number))) { - puts("CAN init FAILED!!!!!\n"); - puth(can_number); puts(" "); - puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); - } + ret &= llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number)); + return ret; } void can_init_all(void) { + bool ret = true; for (uint8_t i=0U; i < CAN_MAX; i++) { - can_init(i); + can_clear(can_queues[i]); + ret &= can_init(i); } + UNUSED(ret); } void can_flip_buses(uint8_t bus1, uint8_t bus2){ @@ -179,7 +194,8 @@ void can_set_gmlan(uint8_t bus) { bus_lookup[prev_bus] = prev_bus; can_num_lookup[prev_bus] = prev_bus; can_num_lookup[3] = -1; - can_init(prev_bus); + bool ret = can_init(prev_bus); + UNUSED(ret); break; default: // GMLAN was not set on either BUS 1 or 2 @@ -198,7 +214,8 @@ void can_set_gmlan(uint8_t bus) { bus_lookup[bus] = 3; can_num_lookup[bus] = -1; can_num_lookup[3] = bus; - can_init(bus); + bool ret = can_init(bus); + UNUSED(ret); break; case 0xFF: //-1 unsigned break; @@ -317,6 +334,10 @@ void process_can(uint8_t can_number) { CAN->sTxMailBox[0].TDHR = to_send.RDHR; CAN->sTxMailBox[0].TDTR = to_send.RDTR; CAN->sTxMailBox[0].TIR = to_send.RIR; + + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) { + usb_outep3_resume_if_paused(); + } } } @@ -342,11 +363,6 @@ void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) { // GTW_status ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0; } - // Cadillac exception - if ((addr == 0x160) && (len == 5)) { - // this message isn't all zeros when ignition is on - ignition_can = GET_BYTES_04(to_push) != 0; - } } } @@ -405,6 +421,14 @@ void CAN3_TX_IRQ_Handler(void) { process_can(2); } void CAN3_RX0_IRQ_Handler(void) { can_rx(2); } void CAN3_SCE_IRQ_Handler(void) { can_sce(CAN3); } +bool can_tx_check_min_slots_free(uint32_t min) { + return + (can_slots_empty(&can_tx1_q) >= min) && + (can_slots_empty(&can_tx2_q) >= min) && + (can_slots_empty(&can_tx3_q) >= min) && + (can_slots_empty(&can_txgmlan_q) >= min); +} + void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) { if (skip_tx_hook || safety_tx_hook(to_push) != 0) { if (bus_number < BUS_MAX) { @@ -425,7 +449,9 @@ void can_set_forwarding(int from, int to) { can_forwarding[from] = to; } -void can_init(uint8_t can_number) { +bool can_init(uint8_t can_number) { + bool ret = false; + REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) @@ -438,12 +464,11 @@ void can_init(uint8_t can_number) { if (can_number != 0xffU) { CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); - can_set_speed(can_number); - - llcan_init(CAN); - + ret &= can_set_speed(can_number); + ret &= llcan_init(CAN); // in case there are queued up messages process_can(can_number); } + return ret; } diff --git a/board/drivers/llcan.h b/board/drivers/llcan.h index 5e9f276e583c6b..e467d67bc164ae 100644 --- a/board/drivers/llcan.h +++ b/board/drivers/llcan.h @@ -11,82 +11,121 @@ #define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF) #define GET_LEN(msg) ((msg)->RDTR & 0xF) #define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21)) -#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU)) +#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU)) #define GET_BYTES_04(msg) ((msg)->RDLR) #define GET_BYTES_48(msg) ((msg)->RDHR) +#define CAN_INIT_TIMEOUT_MS 500U +#define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==CAN1) ? "CAN1" : (((CAN_DEV) == CAN2) ? "CAN2" : "CAN3")) + void puts(const char *a); bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool silent) { + bool ret = true; + // initialization mode register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_INRQ, 0x180FFU); - while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); - - // set time quanta from defines - register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | - (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | - (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU); - - // silent loopback mode for debugging - if (loopback) { - register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM | CAN_BTR_LBKM); - } - if (silent) { - register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM); + uint32_t timeout_counter = 0U; + while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){ + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ + puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" set_speed timed out (1)!\n"); + ret = false; + break; + } } - // reset - register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU); - - #define CAN_TIMEOUT 1000000 - int tmp = 0; - bool ret = false; - while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK) && (tmp < CAN_TIMEOUT)) tmp++; - if (tmp < CAN_TIMEOUT) { - ret = true; + if(ret){ + // set time quanta from defines + register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | + (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | + (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU); + + // silent loopback mode for debugging + if (loopback) { + register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM | CAN_BTR_LBKM); + } + if (silent) { + register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM); + } + + // reset + register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU); + + timeout_counter = 0U; + while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) { + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ + puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" set_speed timed out (2)!\n"); + ret = false; + break; + } + } } return ret; } -void llcan_init(CAN_TypeDef *CAN_obj) { +bool llcan_init(CAN_TypeDef *CAN_obj) { + bool ret = true; + // Enter init mode register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); // Wait for INAK bit to be set - while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {} - - // no mask - // For some weird reason some of these registers do not want to set properly on CAN2 and CAN3. Probably something to do with the single/dual mode and their different filters. - CAN_obj->sFilterRegister[0].FR1 = 0U; - CAN_obj->sFilterRegister[0].FR2 = 0U; - CAN_obj->sFilterRegister[14].FR1 = 0U; - CAN_obj->sFilterRegister[14].FR2 = 0U; - CAN_obj->FA1R |= 1U | (1U << 14); - - // Exit init mode, do not wait - register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); - - // enable certain CAN interrupts - register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_WKUIE); - - if (CAN_obj == CAN1) { - NVIC_EnableIRQ(CAN1_TX_IRQn); - NVIC_EnableIRQ(CAN1_RX0_IRQn); - NVIC_EnableIRQ(CAN1_SCE_IRQn); - } else if (CAN_obj == CAN2) { - NVIC_EnableIRQ(CAN2_TX_IRQn); - NVIC_EnableIRQ(CAN2_RX0_IRQn); - NVIC_EnableIRQ(CAN2_SCE_IRQn); -#ifdef CAN3 - } else if (CAN_obj == CAN3) { - NVIC_EnableIRQ(CAN3_TX_IRQn); - NVIC_EnableIRQ(CAN3_RX0_IRQn); - NVIC_EnableIRQ(CAN3_SCE_IRQn); -#endif - } else { - puts("Invalid CAN: initialization failed\n"); + uint32_t timeout_counter = 0U; + while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) { + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ + puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" initialization timed out!\n"); + ret = false; + break; + } } + + if(ret){ + // no mask + // For some weird reason some of these registers do not want to set properly on CAN2 and CAN3. Probably something to do with the single/dual mode and their different filters. + CAN_obj->sFilterRegister[0].FR1 = 0U; + CAN_obj->sFilterRegister[0].FR2 = 0U; + CAN_obj->sFilterRegister[14].FR1 = 0U; + CAN_obj->sFilterRegister[14].FR2 = 0U; + CAN_obj->FA1R |= 1U | (1U << 14); + + // Exit init mode, do not wait + register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); + + // enable certain CAN interrupts + register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_WKUIE); + + if (CAN_obj == CAN1) { + NVIC_EnableIRQ(CAN1_TX_IRQn); + NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN1_SCE_IRQn); + } else if (CAN_obj == CAN2) { + NVIC_EnableIRQ(CAN2_TX_IRQn); + NVIC_EnableIRQ(CAN2_RX0_IRQn); + NVIC_EnableIRQ(CAN2_SCE_IRQn); + #ifdef CAN3 + } else if (CAN_obj == CAN3) { + NVIC_EnableIRQ(CAN3_TX_IRQn); + NVIC_EnableIRQ(CAN3_RX0_IRQn); + NVIC_EnableIRQ(CAN3_SCE_IRQn); + #endif + } else { + puts("Invalid CAN: initialization failed\n"); + } + } + return ret; } void llcan_clear_send(CAN_TypeDef *CAN_obj) { diff --git a/board/drivers/usb.h b/board/drivers/usb.h index a970194ffd284e..4cfd90df289088 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -23,12 +23,16 @@ typedef union _USB_Setup { } USB_Setup_TypeDef; +#define MAX_CAN_MSGS_PER_BULK_TRANSFER 4U + void usb_init(void); int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired); int usb_cb_ep1_in(void *usbdata, int len, bool hardwired); void usb_cb_ep2_out(void *usbdata, int len, bool hardwired); void usb_cb_ep3_out(void *usbdata, int len, bool hardwired); +void usb_cb_ep3_out_complete(void); void usb_cb_enumeration_complete(void); +void usb_outep3_resume_if_paused(void); // **** supporting defines **** @@ -380,6 +384,7 @@ USB_Setup_TypeDef setup; uint8_t usbdata[0x100]; uint8_t* ep0_txdata = NULL; uint16_t ep0_txlen = 0; +bool outep3_processing = false; // Store the current interface alt setting. int current_int0_alt_setting = 0; @@ -744,6 +749,7 @@ void usb_irqhandler(void) { } if (endpoint == 3) { + outep3_processing = true; usb_cb_ep3_out(usbdata, len, 1); } } else if (status == STS_SETUP_UPDT) { @@ -816,15 +822,17 @@ void usb_irqhandler(void) { #ifdef DEBUG_USB puts(" OUT3 PACKET XFRC\n"); #endif - USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; - USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + // NAK cleared by process_can (if tx buffers have room) + outep3_processing = false; + usb_cb_ep3_out_complete(); } else if ((USBx_OUTEP(3)->DOEPINT & 0x2000) != 0) { #ifdef DEBUG_USB puts(" OUT3 PACKET WTF\n"); #endif // if NAK was set trigger this, unknown interrupt - USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; - USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + // TODO: why was this here? fires when TX buffers when we can't clear NAK + // USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; + // USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; } else if ((USBx_OUTEP(3)->DOEPINT) != 0) { puts("OUTEP3 error "); puth(USBx_OUTEP(3)->DOEPINT); @@ -932,6 +940,15 @@ void usb_irqhandler(void) { //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF); } +void usb_outep3_resume_if_paused() { + ENTER_CRITICAL(); + if (!outep3_processing && (USBx_OUTEP(3)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0) { + USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; + USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + } + EXIT_CRITICAL(); +} + void OTG_FS_IRQ_Handler(void) { NVIC_DisableIRQ(OTG_FS_IRQn); //__disable_irq(); diff --git a/board/get_sdk_mac.sh b/board/get_sdk_mac.sh index a0a919f7d85e04..24b93b540dba19 100755 --- a/board/get_sdk_mac.sh +++ b/board/get_sdk_mac.sh @@ -1,5 +1,7 @@ #!/bin/bash # Need formula for gcc +sudo easy_install pip +/usr/bin/ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)" brew tap ArmMbed/homebrew-formulae brew install python dfu-util arm-none-eabi-gcc pip install --user libusb1 pycrypto requests diff --git a/board/main.c b/board/main.c index 8bc4ac78f1b43b..79cccec3d31a03 100644 --- a/board/main.c +++ b/board/main.c @@ -235,6 +235,12 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { } } +void usb_cb_ep3_out_complete() { + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) { + usb_outep3_resume_if_paused(); + } +} + void usb_cb_enumeration_complete() { puts("USB enumeration complete\n"); is_enumerated = 1; @@ -466,7 +472,17 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) case 0xde: if (setup->b.wValue.w < BUS_MAX) { can_speed[setup->b.wValue.w] = setup->b.wIndex.w; - can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); + UNUSED(ret); + } + break; + // **** 0xdf: set unsafe mode + case 0xdf: + // you can only set this if you are in a non car safety mode + if ((current_safety_mode == SAFETY_SILENT) || + (current_safety_mode == SAFETY_NOOUTPUT) || + (current_safety_mode == SAFETY_ELM327)) { + unsafe_mode = setup->b.wValue.w; } break; // **** 0xe0: uart read diff --git a/board/pedal/main.c b/board/pedal/main.c index 3192f4b2bd2f77..3d7bd03a271446 100644 --- a/board/pedal/main.c +++ b/board/pedal/main.c @@ -76,6 +76,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { UNUSED(len); UNUSED(hardwired); } +void usb_cb_ep3_out_complete(void) {} void usb_cb_enumeration_complete(void) {} int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) { @@ -315,7 +316,8 @@ int main(void) { puts("Failed to set llcan speed"); } - llcan_init(CAN1); + bool ret = llcan_init(CAN1); + UNUSED(ret); // 48mhz / 65536 ~= 732 timer_init(TIM3, 15); diff --git a/board/safety.h b/board/safety.h index 407f33f6695ee8..d2d107d66a2772 100644 --- a/board/safety.h +++ b/board/safety.h @@ -8,7 +8,6 @@ #include "safety/safety_gm_ascm.h" #include "safety/safety_gm.h" #include "safety/safety_ford.h" -#include "safety/safety_cadillac.h" #include "safety/safety_hyundai.h" #include "safety/safety_chrysler.h" #include "safety/safety_subaru.h" @@ -25,7 +24,6 @@ #define SAFETY_GM 4U #define SAFETY_HONDA_BOSCH_GIRAFFE 5U #define SAFETY_FORD 6U -#define SAFETY_CADILLAC 7U #define SAFETY_HYUNDAI 8U #define SAFETY_CHRYSLER 9U #define SAFETY_TESLA 10U @@ -37,6 +35,7 @@ #define SAFETY_GM_ASCM 18U #define SAFETY_NOOUTPUT 19U #define SAFETY_HONDA_BOSCH_HARNESS 20U +#define SAFETY_VOLKSWAGEN_PQ 21U #define SAFETY_SUBARU_LEGACY 22U uint16_t current_safety_mode = SAFETY_SILENT; @@ -183,6 +182,15 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, return is_msg_valid(rx_checks, index); } +void relay_malfunction_set(void) { + relay_malfunction = true; + fault_occurred(FAULT_RELAY_MALFUNCTION); +} + +void relay_malfunction_reset(void) { + relay_malfunction = false; + fault_recovered(FAULT_RELAY_MALFUNCTION); +} typedef struct { uint16_t id; @@ -203,9 +211,9 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks}, {SAFETY_MAZDA, &mazda_hooks}, {SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks}, + {SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks}, {SAFETY_NOOUTPUT, &nooutput_hooks}, #ifdef ALLOW_DEBUG - {SAFETY_CADILLAC, &cadillac_hooks}, {SAFETY_TESLA, &tesla_hooks}, {SAFETY_NISSAN, &nissan_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h deleted file mode 100644 index 0f500a8cd10327..00000000000000 --- a/board/safety/safety_cadillac.h +++ /dev/null @@ -1,125 +0,0 @@ -#define CADILLAC_TORQUE_MSG_N 4 // 4 torque messages: 0x151, 0x152, 0x153, 0x154 - -const AddrBus CADILLAC_TX_MSGS[] = {{0x151, 2}, {0x152, 0}, {0x153, 2}, {0x154, 0}}; -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 uint32_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_cruise_engaged_last = 0; -int cadillac_rt_torque_last = 0; -const int cadillac_torque_msgs_n = 4; -int cadillac_desired_torque_last[CADILLAC_TORQUE_MSG_N] = {0}; -uint32_t cadillac_ts_last = 0; -bool cadillac_supercruise_on = 0; -struct sample_t cadillac_torque_driver; // last few driver torques measured - -int cadillac_get_torque_idx(int addr, int array_size) { - return MIN(MAX(addr - 0x151, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on... -} - -static int cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - - if (addr == 356) { - int torque_driver_new = ((GET_BYTE(to_push, 0) & 0x7U) << 8) | (GET_BYTE(to_push, 1)); - - torque_driver_new = to_signed(torque_driver_new, 11); - // update array of samples - update_sample(&cadillac_torque_driver, torque_driver_new); - } - - // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == 0x370) && (bus == 0)) { - int cruise_engaged = GET_BYTE(to_push, 2) & 0x80; // bit 23 - if (cruise_engaged && !cadillac_cruise_engaged_last) { - controls_allowed = 1; - } - 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 = (GET_BYTE(to_push, 4) & 0x10) != 0; - } - return 1; -} - -static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); - int bus = GET_BUS(to_send); - - if (!msg_allowed(addr, bus, CADILLAC_TX_MSGS, sizeof(CADILLAC_TX_MSGS) / sizeof(CADILLAC_TX_MSGS[0]))) { - tx = 0; - } - - // steer cmd checks - if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) { - int desired_torque = ((GET_BYTE(to_send, 0) & 0x3f) << 8) | GET_BYTE(to_send, 1); - int violation = 0; - uint32_t ts = TIM2->CNT; - int idx = cadillac_get_torque_idx(addr, CADILLAC_TORQUE_MSG_N); - desired_torque = to_signed(desired_torque, 14); - - if (controls_allowed) { - - // *** global torque limit check *** - violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER, -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) { - tx = 0; - } - - } - return tx; -} - -const safety_hooks cadillac_hooks = { - .init = nooutput_init, - .rx = cadillac_rx_hook, - .tx = cadillac_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .fwd = default_fwd_hook, -}; diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 0bcffd6363a217..82fcfb1495d579 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -18,12 +18,7 @@ AddrCheckStruct chrysler_rx_checks[] = { }; const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]); -int chrysler_rt_torque_last = 0; -int chrysler_desired_torque_last = 0; -int chrysler_cruise_engaged_last = 0; int chrysler_speed = 0; -uint32_t chrysler_ts_last = 0; -struct sample_t chrysler_torque_meas; // last few torques measured static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { int checksum_byte = GET_LEN(to_push) - 1; @@ -74,6 +69,8 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { chrysler_get_checksum, chrysler_compute_checksum, chrysler_get_counter); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -82,19 +79,19 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U; // update array of samples - update_sample(&chrysler_torque_meas, torque_meas_new); + update_sample(&torque_meas, torque_meas_new); } // enter controls on rising edge of ACC, exit controls on ACC off if (addr == 500) { int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7; - if (cruise_engaged && !chrysler_cruise_engaged_last) { + if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = 1; } if (!cruise_engaged) { controls_allowed = 0; } - chrysler_cruise_engaged_last = cruise_engaged; + cruise_engaged_prev = cruise_engaged; } // update speed @@ -102,12 +99,13 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4); int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4); chrysler_speed = (speed_l + speed_r) / 2; + vehicle_moving = chrysler_speed > CHRYSLER_STANDSTILL_THRSLD; } // exit controls on rising edge of gas press if (addr == 308) { bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0; - if (gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -116,7 +114,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of brake press if (addr == 320) { bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5; - if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -124,7 +122,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x292)) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -156,20 +154,20 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, CHRYSLER_MAX_STEER, -CHRYSLER_MAX_STEER); // *** torque rate limit check *** - violation |= dist_to_meas_check(desired_torque, chrysler_desired_torque_last, - &chrysler_torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR); + violation |= dist_to_meas_check(desired_torque, desired_torque_last, + &torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR); // used next time - chrysler_desired_torque_last = desired_torque; + desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, chrysler_rt_torque_last, CHRYSLER_MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, CHRYSLER_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, chrysler_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > CHRYSLER_RT_INTERVAL) { - chrysler_rt_torque_last = desired_torque; - chrysler_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -180,9 +178,9 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - chrysler_desired_torque_last = 0; - chrysler_rt_torque_last = 0; - chrysler_ts_last = ts; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } if (violation) { @@ -192,7 +190,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // FORCE CANCEL: only the cancel button press is allowed if (addr == 571) { - if (GET_BYTE(to_send, 0) != 1) { + if ((GET_BYTE(to_send, 0) != 1) || ((GET_BYTE(to_send, 1) & 1) == 1)) { tx = 0; } } diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index ba96b7dd99c874..793dc9615888ca 100644 --- a/board/safety/safety_defaults.h +++ b/board/safety/safety_defaults.h @@ -8,7 +8,7 @@ int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static void nooutput_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); } static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -42,7 +42,7 @@ const safety_hooks nooutput_hooks = { static void alloutput_init(int16_t param) { UNUSED(param); controls_allowed = true; - relay_malfunction = false; + relay_malfunction_reset(); } static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 067ed1cadd3983..3acf8eb579893b 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -7,18 +7,18 @@ // brake rising edge // brake > 0mph -bool ford_moving = false; static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); int bus = GET_BUS(to_push); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; if (addr == 0x217) { // wheel speeds are 14 bits every 16 - ford_moving = false; + vehicle_moving = false; for (int i = 0; i < 8; i += 2) { - ford_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU); + vehicle_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU); } } @@ -38,7 +38,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // speed > 0 if (addr == 0x165) { int brake_pressed = GET_BYTE(to_push, 0) & 0x20; - if (brake_pressed && (!brake_pressed_prev || ford_moving)) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -47,14 +47,14 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 0x204) { bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; } if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) { - relay_malfunction = true; + relay_malfunction_set(); } return 1; } @@ -72,7 +72,11 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && ford_moving); + int pedal_pressed = brake_pressed_prev && vehicle_moving; + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (!unsafe_allow_gas) { + pedal_pressed = pedal_pressed || gas_pressed_prev; + } bool current_controls_allowed = controls_allowed && !(pedal_pressed); if (relay_malfunction) { diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 8672cc267a663a..264df31323c2a9 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -33,17 +33,13 @@ AddrCheckStruct gm_rx_checks[] = { }; const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]); -bool gm_moving = false; -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 int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN, NULL, NULL, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -51,13 +47,13 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7); torque_driver_new = to_signed(torque_driver_new, 11); // update array of samples - update_sample(&gm_torque_driver, torque_driver_new); + update_sample(&torque_driver, torque_driver_new); } // sample speed, really only care if car is moving or not // rear left wheel speed if (addr == 842) { - gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); + vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); } // ACC steering wheel buttons @@ -82,7 +78,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // Brake pedal's potentiometer returns near-zero reading // even when pedal is not pressed bool brake_pressed = GET_BYTE(to_push, 1) >= 10; - if (brake_pressed && (!brake_pressed_prev || gm_moving)) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -91,7 +87,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 417) { bool gas_pressed = GET_BYTE(to_push, 6) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -110,7 +106,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // 384 = ASCMLKASteeringCmd // 715 = ASCMGasRegenCmd if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 384) || (addr == 715))) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -138,7 +134,11 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && gm_moving); + int pedal_pressed = brake_pressed_prev && vehicle_moving; + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (!unsafe_allow_gas) { + pedal_pressed = pedal_pressed || gas_pressed_prev; + } bool current_controls_allowed = controls_allowed && !pedal_pressed; // BRAKE: safety check @@ -168,21 +168,21 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, GM_MAX_STEER, -GM_MAX_STEER); // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, gm_desired_torque_last, &gm_torque_driver, + violation |= driver_limit_check(desired_torque, desired_torque_last, &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; + 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); + violation |= rt_rate_limit_check(desired_torque, 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); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > GM_RT_INTERVAL) { - gm_rt_torque_last = desired_torque; - gm_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -193,9 +193,9 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // 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; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } if (violation) { diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index ecac14890f68f9..9f09da95375e05 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -7,9 +7,17 @@ // brake rising edge // brake > 0mph const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}}; -const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe -const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness -const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file +const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0xE5, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe +const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0xE5, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness + +// Roughly calculated using the offsets in openpilot +5%: +// In openpilot: ((gas1_norm + gas2_norm)/2) > 15 +// gas_norm1 = ((gain_dbc1*gas1) + offset_dbc) +// gas_norm2 = ((gain_dbc2*gas2) + offset_dbc) +// assuming that 2*(gain_dbc1*gas1) == (gain_dbc2*gas2) +// In this safety: ((gas1 + (gas2/2))/2) > THRESHOLD +const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 344; +#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) // avg between 2 tracks // Nidec and Bosch giraffe have pt on bus 0 AddrCheckStruct honda_rx_checks[] = { @@ -28,7 +36,6 @@ AddrCheckStruct honda_bh_rx_checks[] = { const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]); int honda_brake = 0; -bool honda_moving = false; bool honda_alt_brake_msg = false; bool honda_fwd_brake = false; enum {HONDA_N_HW, HONDA_BG_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW; @@ -72,6 +79,8 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { honda_get_checksum, honda_compute_checksum, honda_get_counter); } + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); @@ -80,7 +89,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // sample speed if (addr == 0x158) { // first 2 bytes - honda_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); + vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); } // state machine to enter and exit controls @@ -110,7 +119,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C); if (is_user_brake_msg) { bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); - if (brake_pressed && (!brake_pressed_prev || honda_moving)) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -120,8 +129,8 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // length check because bosch hardware also uses this id (0x201 w/ len = 8) if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; - int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && + int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push); + if (!unsafe_allow_gas && (gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) { controls_allowed = 0; } @@ -132,24 +141,28 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (!gas_interceptor_detected) { if (addr == 0x17C) { bool gas_pressed = GET_BYTE(to_push, 0) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; } } - if ((bus == 2) && (addr == 0x1FA)) { - bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; - int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); - - // Forward AEB when stock braking is higher than openpilot braking - // only stop forwarding when AEB event is over - if (!honda_stock_aeb) { - honda_fwd_brake = false; - } else if (honda_stock_brake >= honda_brake) { - honda_fwd_brake = true; - } else { - // Leave Honda forward brake as is + + // disable stock Honda AEB in unsafe mode + if ( !(unsafe_mode & UNSAFE_DISABLE_STOCK_AEB) ) { + if ((bus == 2) && (addr == 0x1FA)) { + bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; + int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); + + // Forward AEB when stock braking is higher than openpilot braking + // only stop forwarding when AEB event is over + if (!honda_stock_aeb) { + honda_fwd_brake = false; + } else if (honda_stock_brake >= honda_brake) { + honda_fwd_brake = true; + } else { + // Leave Honda forward brake as is + } } } @@ -159,7 +172,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 0xE4) || (addr == 0x194))) { if (((honda_hw != HONDA_N_HW) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_N_HW) && (bus == 0))) { - relay_malfunction = true; + relay_malfunction_set(); } } } @@ -192,8 +205,11 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || - (brake_pressed_prev && honda_moving); + int pedal_pressed = brake_pressed_prev && vehicle_moving; + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (!unsafe_allow_gas) { + pedal_pressed = pedal_pressed || gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD); + } bool current_controls_allowed = controls_allowed && !(pedal_pressed); // BRAKE: safety check @@ -222,6 +238,13 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } } + // Bosch supplemental control check + if (addr == 0xE5) { + if ((GET_BYTES_04(to_send) != 0x10800004) || ((GET_BYTES_48(to_send) & 0x00FFFFFF) != 0x0)) { + tx = 0; + } + } + // GAS: safety check if (addr == 0x200) { if (!current_controls_allowed) { @@ -248,14 +271,15 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { static void honda_nidec_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); + gas_interceptor_detected = 0; honda_hw = HONDA_N_HW; honda_alt_brake_msg = false; } static void honda_bosch_giraffe_init(int16_t param) { controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); honda_hw = HONDA_BG_HW; // Checking for alternate brake override from safety parameter honda_alt_brake_msg = (param == 1) ? true : false; @@ -263,7 +287,7 @@ static void honda_bosch_giraffe_init(int16_t param) { static void honda_bosch_harness_init(int16_t param) { controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); honda_hw = HONDA_BH_HW; // Checking for alternate brake override from safety parameter honda_alt_brake_msg = (param == 1) ? true : false; @@ -305,7 +329,7 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { } if (bus_num == bus_rdr_cam) { int addr = GET_ADDR(to_fwd); - int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D); + int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D); if (!is_lkas_msg) { bus_fwd = bus_rdr_car; } diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 1b5b656ff2fab8..f27f2f0839ae78 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -6,7 +6,7 @@ const int HYUNDAI_MAX_RATE_DOWN = 7; const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph -const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}}; +const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}, {1157, 0}}; // TODO: do checksum and counter checks AddrCheckStruct hyundai_rx_checks[] = { @@ -18,44 +18,40 @@ AddrCheckStruct hyundai_rx_checks[] = { }; const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]); -int hyundai_rt_torque_last = 0; -int hyundai_desired_torque_last = 0; -int hyundai_cruise_engaged_last = 0; -int hyundai_speed = 0; -uint32_t hyundai_ts_last = 0; -struct sample_t hyundai_torque_driver; // last few driver torques measured - static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN, NULL, NULL, NULL); - if (valid && GET_BUS(to_push) == 0) { - int addr = GET_ADDR(to_push); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + + int addr = GET_ADDR(to_push); + int bus = GET_BUS(to_push); - if (addr == 897) { - int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048; + if (valid && (bus == 0)) { + if (addr == 593) { + int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ff) * 0.79) - 808; // scale down new driver torque signal to match previous one // update array of samples - update_sample(&hyundai_torque_driver, torque_driver_new); + update_sample(&torque_driver, torque_driver_new); } // enter controls on rising edge of ACC, exit controls on ACC off if (addr == 1057) { // 2 bits: 13-14 int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3; - if (cruise_engaged && !hyundai_cruise_engaged_last) { + if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = 1; } if (!cruise_engaged) { controls_allowed = 0; } - hyundai_cruise_engaged_last = cruise_engaged; + cruise_engaged_prev = cruise_engaged; } // exit controls on rising edge of gas press if (addr == 608) { bool gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -63,15 +59,16 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // sample subaru wheel speed, averaging opposite corners if (addr == 902) { - hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL + int hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL hyundai_speed /= 2; + vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD; } // exit controls on rising edge of brake press if (addr == 916) { bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0; - if (brake_pressed && (!brake_pressed_prev || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -79,7 +76,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -111,21 +108,21 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER); // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, hyundai_desired_torque_last, &hyundai_torque_driver, + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, HYUNDAI_MAX_STEER, HYUNDAI_MAX_RATE_UP, HYUNDAI_MAX_RATE_DOWN, HYUNDAI_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_DRIVER_TORQUE_FACTOR); // used next time - hyundai_desired_torque_last = desired_torque; + desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, hyundai_rt_torque_last, HYUNDAI_MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, hyundai_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > HYUNDAI_RT_INTERVAL) { - hyundai_rt_torque_last = desired_torque; - hyundai_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -136,9 +133,9 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - hyundai_desired_torque_last = 0; - hyundai_rt_torque_last = 0; - hyundai_ts_last = ts; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } if (violation) { @@ -168,7 +165,7 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 0) { bus_fwd = 2; } - if ((bus_num == 2) && (addr != 832)) { + if ((bus_num == 2) && (addr != 832) && (addr != 1157)) { bus_fwd = 0; } } diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index f6b19c1a2fab3a..8ee63849ccde1a 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -55,7 +55,7 @@ static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // if we see wheel speed msgs on MAZDA_CAM bus then relay is closed if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == MAZDA_CAM) && (addr == MAZDA_WHEEL_SPEED)) { - relay_malfunction = true; + relay_malfunction_set(); } return 1; } @@ -83,8 +83,7 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER); // *** torque rate limit check *** - int desired_torque_last = mazda_desired_torque_last; - violation |= driver_limit_check(desired_torque, desired_torque_last, &mazda_torque_driver, + violation |= driver_limit_check(desired_torque, mazda_desired_torque_last, &mazda_torque_driver, MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN, MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR); // used next time diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index baa82b8d4aa319..aca99d19b69aa9 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -9,25 +9,22 @@ const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = { {2., 7., 17.}, {5., 3.5, .5}}; -const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = { - {3.3, 12, 32}, - {540., 120., 23.}}; - const int NISSAN_DEG_TO_CAN = 100; -const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}}; +const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}, {0x280, 2}}; AddrCheckStruct nissan_rx_checks[] = { - {.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {0x29a}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {0x1b6}, .bus = 1, .expected_timestep = 10000U}, + {.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, // STEER_ANGLE_SENSOR (100Hz) + {.addr = {0x285}, .bus = 0, .expected_timestep = 20000U}, // WHEEL_SPEEDS_REAR (50Hz) + {.addr = {0x30f}, .bus = 2, .expected_timestep = 100000U}, // CRUISE_STATE (10Hz) + {.addr = {0x15c, 0x239}, .bus = 0, .expected_timestep = 20000U}, // GAS_PEDAL (100Hz / 50Hz) + {.addr = {0x454, 0x1cc}, .bus = 0, .expected_timestep = 100000U}, // DOORS_LIGHTS (10Hz) / BRAKE (100Hz) }; const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]); float nissan_speed = 0; //int nissan_controls_allowed_last = 0; uint32_t nissan_ts_angle_last = 0; -int nissan_cruise_engaged_last = 0; int nissan_desired_angle_last = 0; struct sample_t nissan_angle_meas; // last 3 steer angles @@ -38,6 +35,8 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN, NULL, NULL, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -54,16 +53,24 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { update_sample(&nissan_angle_meas, angle_meas_new); } - if (addr == 0x29a) { + if (addr == 0x285) { // Get current speed - // Factor 0.00555 - nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6; + // Factor 0.005 + nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6; + vehicle_moving = nissan_speed > 0.; } // exit controls on rising edge of gas press - if (addr == 0x15c) { - bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)); - if (gas_pressed && !gas_pressed_prev) { + // X-Trail 0x15c, Leaf 0x239 + if ((addr == 0x15c) || (addr == 0x239)) { + bool gas_pressed = true; + if (addr == 0x15c){ + gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)) > 1; + } else { + gas_pressed = GET_BYTE(to_push, 0) > 3; + } + + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -71,30 +78,38 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) { - relay_malfunction = true; + relay_malfunction_set(); } } - if (bus == 1) { - if (addr == 0x1b6) { - int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1; - if (cruise_engaged && !nissan_cruise_engaged_last) { - controls_allowed = 1; - } - if (!cruise_engaged) { - controls_allowed = 0; - } - nissan_cruise_engaged_last = cruise_engaged; + // exit controls on rising edge of brake press, or if speed > 0 and brake + // X-trail 0x454, Leaf 0x1cc + if ((addr == 0x454) || (addr == 0x1cc)) { + bool brake_pressed = true; + if (addr == 0x454){ + brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0; + } else { + brake_pressed = GET_BYTE(to_push, 0) > 3; } - // exit controls on rising edge of brake press, or if speed > 0 and brake - if (addr == 0x454) { - bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0; - if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { + controls_allowed = 0; } + brake_pressed_prev = brake_pressed; + } + + + // Handle cruise enabled + if ((bus == 2) && (addr == 0x30f)) { + bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1; + + if (cruise_engaged && !cruise_engaged_prev) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; } } return valid; @@ -133,16 +148,6 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down); int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); - // Limit maximum steering angle at current speed - int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed)); - - if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) { - highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN); - } - if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) { - lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN); - } - // check for violation; violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); @@ -183,7 +188,10 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = GET_ADDR(to_fwd); if (bus_num == 0) { - bus_fwd = 2; // ADAS + int block_msg = (addr == 0x280); // CANCEL_MSG + if (!block_msg) { + bus_fwd = 2; // ADAS + } } if (bus_num == 2) { diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 9b7665f2bcbe36..17bd699d68ec0f 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -30,13 +30,7 @@ AddrCheckStruct subaru_l_rx_checks[] = { const int SUBARU_RX_CHECK_LEN = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]); const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]); -int subaru_cruise_engaged_last = 0; -int subaru_rt_torque_last = 0; -int subaru_desired_torque_last = 0; -int subaru_speed = 0; -uint32_t subaru_ts_last = 0; bool subaru_global = false; -struct sample_t subaru_torque_driver; // last few driver torques measured static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { return (uint8_t)GET_BYTE(to_push, 0); @@ -67,6 +61,8 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { NULL, NULL, NULL); } + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); if (((addr == 0x119) && subaru_global) || @@ -74,11 +70,12 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int torque_driver_new; if (subaru_global) { torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF); + torque_driver_new = -1 * to_signed(torque_driver_new, 11); } else { torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3); + torque_driver_new = to_signed(torque_driver_new, 11); } - torque_driver_new = to_signed(torque_driver_new, 11); - update_sample(&subaru_torque_driver, torque_driver_new); + update_sample(&torque_driver, torque_driver_new); } // enter controls on rising edge of ACC, exit controls on ACC off @@ -86,26 +83,27 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { ((addr == 0x144) && !subaru_global)) { int bit_shift = subaru_global ? 9 : 17; int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1); - if (cruise_engaged && !subaru_cruise_engaged_last) { + if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = 1; } if (!cruise_engaged) { controls_allowed = 0; } - subaru_cruise_engaged_last = cruise_engaged; + cruise_engaged_prev = cruise_engaged; } // sample subaru wheel speed, averaging opposite corners if ((addr == 0x13a) && subaru_global) { - subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR + int subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL subaru_speed /= 2; + vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD; } // exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models) if ((addr == 0x139) && subaru_global) { bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0; - if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -116,7 +114,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { ((addr == 0x140) && !subaru_global)) { int byte = subaru_global ? 4 : 0; bool gas_pressed = GET_BYTE(to_push, byte) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -124,7 +122,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -151,7 +149,7 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF); bool violation = 0; uint32_t ts = TIM2->CNT; - desired_torque = to_signed(desired_torque, 13); + desired_torque = -1 * to_signed(desired_torque, 13); if (controls_allowed) { @@ -159,22 +157,21 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER); // *** torque rate limit check *** - int desired_torque_last = subaru_desired_torque_last; - violation |= driver_limit_check(desired_torque, desired_torque_last, &subaru_torque_driver, + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN, SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR); // used next time - subaru_desired_torque_last = desired_torque; + desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, subaru_rt_torque_last, SUBARU_MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, SUBARU_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, subaru_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > SUBARU_RT_INTERVAL) { - subaru_rt_torque_last = desired_torque; - subaru_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -185,9 +182,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - subaru_desired_torque_last = 0; - subaru_rt_torque_last = 0; - subaru_ts_last = ts; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } if (violation) { @@ -226,14 +223,14 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { static void subaru_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); subaru_global = true; } static void subaru_legacy_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); subaru_global = false; } diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 4f18ee678f1f20..f69e4d3c6c0385 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -14,10 +14,20 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks // longitudinal limits const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 -const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 +const int TOYOTA_MIN_ACCEL = -3000; // -3.0 m/s2 + +const int TOYOTA_ISO_MAX_ACCEL = 2000; // 2.0 m/s2 +const int TOYOTA_ISO_MIN_ACCEL = -3500; // -3.5 m/s2 const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph -const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file + +// Roughly calculated using the offsets in openpilot +5%: +// In openpilot: ((gas1_norm + gas2_norm)/2) > 15 +// gas_norm1 = ((gain_dbc*gas1) + offset1_dbc) +// gas_norm2 = ((gain_dbc*gas2) + offset2_dbc) +// In this safety: ((gas1 + gas2)/2) > THRESHOLD +const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845; +#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0}, // DSU bus 0 {0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1 @@ -35,15 +45,6 @@ const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_che // global actuation limit states int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file -// states -int toyota_desired_torque_last = 0; // last desired steer torque -int toyota_rt_torque_last = 0; // last desired torque for real time check -uint32_t toyota_ts_last = 0; -int toyota_cruise_engaged_last = 0; // cruise state -bool toyota_moving = false; -struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps - - static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); @@ -63,6 +64,8 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, toyota_rx_checks, TOYOTA_RX_CHECKS_LEN, toyota_get_checksum, toyota_compute_checksum, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -75,24 +78,32 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; // update array of sample - update_sample(&toyota_torque_meas, torque_meas_new); + update_sample(&torque_meas, torque_meas_new); // increase torque_meas by 1 to be conservative on rounding - toyota_torque_meas.min--; - toyota_torque_meas.max++; + torque_meas.min--; + torque_meas.max++; } // enter controls on rising edge of ACC, exit controls on ACC off + // exit controls on rising edge of gas press if (addr == 0x1D2) { // 5th bit is CRUISE_ACTIVE int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; if (!cruise_engaged) { controls_allowed = 0; } - if (cruise_engaged && !toyota_cruise_engaged_last) { + if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = 1; } - toyota_cruise_engaged_last = cruise_engaged; + cruise_engaged_prev = cruise_engaged; + + // handle gas_pressed + bool gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0; + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; } // sample speed @@ -103,7 +114,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int next_byte = i + 1; // hack to deal with misra 10.8 speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte) - 0x1a6f; } - toyota_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD; + vehicle_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD; } // exit controls on rising edge of brake pedal @@ -111,7 +122,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((addr == 0x224) || (addr == 0x226)) { int byte = (addr == 0x224) ? 0 : 4; bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0; - if (brake_pressed && (!brake_pressed_prev || toyota_moving)) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -120,26 +131,17 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of interceptor gas press if (addr == 0x201) { gas_interceptor_detected = 1; - int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && + int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push); + if (!unsafe_allow_gas && (gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) { controls_allowed = 0; } gas_interceptor_prev = gas_interceptor; } - // exit controls on rising edge of gas press - if (addr == 0x2C1) { - bool gas_pressed = GET_BYTE(to_push, 6) != 0; - if (gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; - } - // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -180,7 +182,10 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { tx = 0; } } - bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); + bool violation = (unsafe_mode & UNSAFE_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX)? + max_limit_check(desired_accel, TOYOTA_ISO_MAX_ACCEL, TOYOTA_ISO_MIN_ACCEL) : + max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); + if (violation) { tx = 0; } @@ -200,20 +205,20 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); // *** torque rate limit check *** - violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last, - &toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR); + violation |= dist_to_meas_check(desired_torque, desired_torque_last, + &torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR); // used next time - toyota_desired_torque_last = desired_torque; + desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, toyota_rt_torque_last, TOYOTA_MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, TOYOTA_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, toyota_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > TOYOTA_RT_INTERVAL) { - toyota_rt_torque_last = desired_torque; - toyota_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -224,9 +229,9 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - toyota_desired_torque_last = 0; - toyota_rt_torque_last = 0; - toyota_ts_last = ts; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } if (violation) { @@ -240,7 +245,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { static void toyota_init(int16_t param) { controls_allowed = 0; - relay_malfunction = 0; + relay_malfunction_reset(); + gas_interceptor_detected = 0; toyota_dbc_eps_torque_factor = param; } diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 132c10d110fd96..4063fa7d5e8ea0 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -30,12 +30,27 @@ AddrCheckStruct volkswagen_mqb_rx_checks[] = { }; const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]); +// Safety-relevant CAN messages for the Volkswagen PQ35/PQ46/NMS platforms +#define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque +#define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque +#define MSG_MOTOR_2 0x288 // RX from ECU, for CC state and brake switch state +#define MSG_MOTOR_3 0x380 // RX from ECU, for driver throttle input +#define MSG_GRA_NEU 0x38A // TX by OP, ACC control buttons for cancel/resume +#define MSG_BREMSE_3 0x4A0 // RX from ABS, for wheel speeds +#define MSG_LDW_1 0x5BE // TX by OP, Lane line recognition and text alerts + +// Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration +const AddrBus VOLKSWAGEN_PQ_TX_MSGS[] = {{MSG_HCA_1, 0}, {MSG_GRA_NEU, 0}, {MSG_GRA_NEU, 2}, {MSG_LDW_1, 0}}; +const int VOLKSWAGEN_PQ_TX_MSGS_LEN = sizeof(VOLKSWAGEN_PQ_TX_MSGS) / sizeof(VOLKSWAGEN_PQ_TX_MSGS[0]); + +AddrCheckStruct volkswagen_pq_rx_checks[] = { + {.addr = {MSG_LENKHILFE_3}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {MSG_MOTOR_2}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 20000U}, + {.addr = {MSG_MOTOR_3}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, + {.addr = {MSG_BREMSE_3}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, +}; +const int VOLKSWAGEN_PQ_RX_CHECKS_LEN = sizeof(volkswagen_pq_rx_checks) / sizeof(volkswagen_pq_rx_checks[0]); -struct sample_t volkswagen_torque_driver; // Last few driver torques measured -int volkswagen_rt_torque_last = 0; -int volkswagen_desired_torque_last = 0; -uint32_t volkswagen_ts_last = 0; -bool volkswagen_moving = false; int volkswagen_torque_msg = 0; int volkswagen_lane_msg = 0; uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR @@ -45,10 +60,17 @@ static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { return (uint8_t)GET_BYTE(to_push, 0); } -static uint8_t volkswagen_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { +static uint8_t volkswagen_mqb_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + // MQB message counters are consistently found at LSB 8. return (uint8_t)GET_BYTE(to_push, 1) & 0xFU; } +static uint8_t volkswagen_pq_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + // Few PQ messages have counters, and their offsets are inconsistent. This + // function works only for Lenkhilfe_3 at this time. + return (uint8_t)(GET_BYTE(to_push, 1) & 0xF0U) >> 4; +} + static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); @@ -62,7 +84,7 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { crc = volkswagen_crc8_lut_8h2f[crc]; } - uint8_t counter = volkswagen_get_counter(to_push); + uint8_t counter = volkswagen_mqb_get_counter(to_push); switch(addr) { case MSG_EPS_01: crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter]; @@ -84,20 +106,40 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { return crc ^ 0xFFU; } +static uint8_t volkswagen_pq_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int len = GET_LEN(to_push); + uint8_t checksum = 0U; + + for (int i = 1; i < len; i++) { + checksum ^= (uint8_t)GET_BYTE(to_push, i); + } + + return checksum; +} + static void volkswagen_mqb_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); volkswagen_torque_msg = MSG_HCA_01; volkswagen_lane_msg = MSG_LDW_02; gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f); } +static void volkswagen_pq_init(int16_t param) { + UNUSED(param); + + controls_allowed = false; + relay_malfunction_reset(); + volkswagen_torque_msg = MSG_HCA_1; + volkswagen_lane_msg = MSG_LDW_1; +} + static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN, - volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_get_counter); + volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter); if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -110,7 +152,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8); // Check for average front speed in excess of 0.3m/s, 1.08km/h // DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare - volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288; + vehicle_moving = (wheel_speed_fl + wheel_speed_fr) > 288; } // Update driver input torque samples @@ -122,7 +164,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (sign == 1) { torque_driver_new *= -1; } - update_sample(&volkswagen_torque_driver, torque_driver_new); + update_sample(&torque_driver, torque_driver_new); } // Update ACC status from drivetrain coordinator for controls-allowed state @@ -136,7 +178,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // Signal: Motor_20.MO_Fahrpedalrohwert_01 if (addr == MSG_MOTOR_20) { bool gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -146,7 +188,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // Signal: ESP_05.ESP_Fahrer_bremst if (addr == MSG_ESP_05) { bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2; - if (brake_pressed && (!brake_pressed_prev || volkswagen_moving)) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; @@ -154,7 +196,74 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // If there are HCA messages on bus 0 not sent by OP, there's a relay problem if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_HCA_01)) { - relay_malfunction = true; + relay_malfunction_set(); + } + } + return valid; +} + +static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + + bool valid = addr_safety_check(to_push, volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_RX_CHECKS_LEN, + volkswagen_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter); + + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // Update in-motion state by sampling front wheel speeds + // Signal: Bremse_3.Radgeschw__VL_4_1 (front left) + // Signal: Bremse_3.Radgeschw__VR_4_1 (front right) + if ((bus == 0) && (addr == MSG_BREMSE_3)) { + int wheel_speed_fl = (GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8)) >> 1; + int wheel_speed_fr = (GET_BYTE(to_push, 2) | (GET_BYTE(to_push, 3) << 8)) >> 1; + // Check for average front speed in excess of 0.3m/s, 1.08km/h + // DBC speed scale 0.01: 0.3m/s = 108, sum both wheels to compare + vehicle_moving = (wheel_speed_fl + wheel_speed_fr) > 216; + } + + // Update driver input torque samples + // Signal: Lenkhilfe_3.LH3_LM (absolute torque) + // Signal: Lenkhilfe_3.LH3_LMSign (direction) + if ((bus == 0) && (addr == MSG_LENKHILFE_3)) { + int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3) << 8); + int sign = (GET_BYTE(to_push, 3) & 0x4) >> 2; + if (sign == 1) { + torque_driver_new *= -1; + } + update_sample(&torque_driver, torque_driver_new); + } + + // Update ACC status from ECU for controls-allowed state + // Signal: Motor_2.GRA_Status + if ((bus == 0) && (addr == MSG_MOTOR_2)) { + int acc_status = (GET_BYTE(to_push, 2) & 0xC0) >> 6; + controls_allowed = ((acc_status == 1) || (acc_status == 2)) ? 1 : 0; + } + + // Exit controls on rising edge of gas press + // Signal: Motor_3.Fahrpedal_Rohsignal + if ((bus == 0) && (addr == MSG_MOTOR_3)) { + int gas_pressed = (GET_BYTE(to_push, 2)); + if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } + + // Exit controls on rising edge of brake press + // Signal: Motor_2.Bremslichtschalter + if ((bus == 0) && (addr == MSG_MOTOR_2)) { + bool brake_pressed = (GET_BYTE(to_push, 2) & 0x1); + if (brake_pressed && (!(brake_pressed_prev) || vehicle_moving)) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + + // If there are HCA messages on bus 0 not sent by OP, there's a relay problem + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_1)) { + relay_malfunction_set(); } } return valid; @@ -169,19 +278,19 @@ static bool volkswagen_steering_check(int desired_torque) { violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER); // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver, + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN, VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR); - volkswagen_desired_torque_last = desired_torque; + desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) { - volkswagen_rt_torque_last = desired_torque; - volkswagen_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -192,9 +301,9 @@ static bool volkswagen_steering_check(int desired_torque) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - volkswagen_desired_torque_last = 0; - volkswagen_rt_torque_last = 0; - volkswagen_ts_last = ts; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } return violation; @@ -237,6 +346,44 @@ static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return tx; } +static int volkswagen_pq_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); + int tx = 1; + + if (!msg_allowed(addr, bus, VOLKSWAGEN_PQ_TX_MSGS, VOLKSWAGEN_PQ_TX_MSGS_LEN) || relay_malfunction) { + tx = 0; + } + + // Safety check for HCA_1 Heading Control Assist torque + // Signal: HCA_1.LM_Offset (absolute torque) + // Signal: HCA_1.LM_Offsign (direction) + if (addr == MSG_HCA_1) { + int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x7F) << 8); + desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm + int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7; + if (sign == 1) { + desired_torque *= -1; + } + + if (volkswagen_steering_check(desired_torque)) { + tx = 0; + } + } + + // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off. + // This avoids unintended engagements while still allowing resume spam + if ((addr == MSG_GRA_NEU) && !controls_allowed) { + // disallow resume and set: bits 16 and 17 + if ((GET_BYTE(to_send, 2) & 0x3) != 0) { + tx = 0; + } + } + + // 1 allows the message through + return tx; +} + static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = GET_ADDR(to_fwd); int bus_fwd = -1; @@ -275,3 +422,14 @@ const safety_hooks volkswagen_mqb_hooks = { .addr_check = volkswagen_mqb_rx_checks, .addr_check_len = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]), }; + +// Volkswagen PQ35/PQ46/NMS platforms +const safety_hooks volkswagen_pq_hooks = { + .init = volkswagen_pq_init, + .rx = volkswagen_pq_rx_hook, + .tx = volkswagen_pq_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = volkswagen_fwd_hook, + .addr_check = volkswagen_pq_rx_checks, + .addr_check_len = sizeof(volkswagen_pq_rx_checks) / sizeof(volkswagen_pq_rx_checks[0]), +}; diff --git a/board/safety_declarations.h b/board/safety_declarations.h index aa9f7fdfe43704..be8be93b78023b 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -61,6 +61,8 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push), uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push), uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push)); +void relay_malfunction_set(void); +void relay_malfunction_reset(void); typedef void (*safety_hook_init)(int16_t param); typedef int (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); @@ -87,11 +89,35 @@ bool gas_interceptor_detected = false; int gas_interceptor_prev = 0; bool gas_pressed_prev = false; bool brake_pressed_prev = false; +bool cruise_engaged_prev = false; +bool vehicle_moving = false; + +// for torque-based safety modes +int desired_torque_last = 0; // last desired steer torque +int rt_torque_last = 0; // last desired torque for real time check +struct sample_t torque_meas; // last 3 motor torques produced by the eps +struct sample_t torque_driver; // last 3 driver torques measured +uint32_t ts_last = 0; + +// This can be set with a USB command +// It enables features we consider to be unsafe, but understand others may have different opinions +// It is always 0 on mainline comma.ai openpilot + +// If using this flag, be very careful about what happens if your fork wants to brake while the +// user is pressing the gas. Tesla is careful with this. +#define UNSAFE_DISABLE_DISENGAGE_ON_GAS 1 + +// If using this flag, make sure to communicate to your users that a stock safety feature is now disabled. +#define UNSAFE_DISABLE_STOCK_AEB 2 + +// If using this flag, be aware that harder braking is more likely to lead to rear endings, +// and that alone this flag doesn't make braking compliant because there's also a time element. +// See ISO 15622:2018 for more information. +#define UNSAFE_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX 8 + +int unsafe_mode = 0; // time since safety mode has been changed uint32_t safety_mode_cnt = 0U; // allow 1s of transition timeout after relay changes state before assessing malfunctioning const uint32_t RELAY_TRNS_TIMEOUT = 1U; - -// avg between 2 tracks -#define GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) diff --git a/board/spi_flasher.h b/board/spi_flasher.h index 1c703516ac1731..b0511c14efd2ca 100644 --- a/board/spi_flasher.h +++ b/board/spi_flasher.h @@ -110,6 +110,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { UNUSED(len); UNUSED(hardwired); } +void usb_cb_ep3_out_complete(void) {} int is_enumerated = 0; void usb_cb_enumeration_complete(void) { diff --git a/examples/can_unique.py b/examples/can_unique.py index fdd586c3d3517d..0b94076f3e6ddc 100755 --- a/examples/can_unique.py +++ b/examples/can_unique.py @@ -4,7 +4,9 @@ # messages, print which bits in the interesting file have never appeared # in the background files. -# Expects the CSV file to be in the format from can_logger.py +# Expects the CSV file to be in one of the following formats: + +# can_logger.py # Bus,MessageID,Message,MessageLength # 0,0x292,0x040000001068,6 @@ -12,6 +14,10 @@ # Bus,MessageID,Message # 0,344,c000c00000000000 +# Cabana "Save Log" format +# time,addr,bus,data +# 240.47911496100002,53,0,0acc0ade0074bf9e + import csv import sys @@ -45,30 +51,49 @@ def __init__(self): def load(self, filename): """Given a CSV file, adds information about message IDs and their values.""" - with open(filename, 'rb') as input: + with open(filename, 'r') as input: reader = csv.reader(input) - next(reader, None) # skip the CSV header - for row in reader: - bus = row[0] - if row[1].startswith('0x'): - message_id = row[1][2:] # remove leading '0x' - else: - message_id = hex(int(row[1]))[2:] # old message IDs are in decimal - message_id = '%s:%s' % (bus, message_id) - if row[1].startswith('0x'): - data = row[2][2:] # remove leading '0x' - else: - data = row[2] - if message_id not in self.messages: - self.messages[message_id] = Message(message_id) - message = self.messages[message_id] - if data not in self.messages[message_id].data: - message.data[data] = True - bytes = bytearray.fromhex(data) - for i in range(len(bytes)): - message.ones[i] = message.ones[i] | int(bytes[i]) - # Inverts the data and masks it to a byte to get the zeros as ones. - message.zeros[i] = message.zeros[i] | ( (~int(bytes[i])) & 0xff) + header = next(reader, None) + if header[0] == 'time': + self.cabana(reader) + else: + self.logger(reader) + + def cabana(self, reader): + for row in reader: + bus = row[2] + message_id = hex(int(row[1]))[2:] + message_id = '%s:%s' % (bus, message_id) + data = row[3] + self.store(message_id, data) + + def logger(self, reader): + for row in reader: + bus = row[0] + if row[1].startswith('0x'): + message_id = row[1][2:] # remove leading '0x' + else: + message_id = hex(int(row[1]))[2:] # old message IDs are in decimal + message_id = '%s:%s' % (bus, message_id) + if row[1].startswith('0x'): + data = row[2][2:] # remove leading '0x' + else: + data = row[2] + self.store(message_id, data) + + def store(self, message_id, data): + if message_id not in self.messages: + self.messages[message_id] = Message(message_id) + message = self.messages[message_id] + if data not in self.messages[message_id].data: + message.data[data] = True + bytes = bytearray.fromhex(data) + for i in range(len(bytes)): + message.ones[i] = message.ones[i] | int(bytes[i]) + # Inverts the data and masks it to a byte to get the zeros as ones. + message.zeros[i] = message.zeros[i] | ( (~int(bytes[i])) & 0xff) + + def PrintUnique(interesting_file, background_files): background = Info() diff --git a/python/__init__.py b/python/__init__.py index c0e27e8d6a775e..e8525a1764e9a6 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -9,6 +9,7 @@ import time import traceback import subprocess +import sys from .dfu import PandaDFU from .esptool import ESPROM, CesantaFlasher # noqa: F401 from .flash_release import flash_release # noqa: F401 @@ -117,7 +118,6 @@ class Panda(object): SAFETY_GM = 4 SAFETY_HONDA_BOSCH_GIRAFFE = 5 SAFETY_FORD = 6 - SAFETY_CADILLAC = 7 SAFETY_HYUNDAI = 8 SAFETY_CHRYSLER = 9 SAFETY_TESLA = 10 @@ -129,6 +129,7 @@ class Panda(object): SAFETY_GM_ASCM = 18 SAFETY_NOOUTPUT = 19 SAFETY_HONDA_BOSCH_HARNESS = 20 + SAFETY_VOLKSWAGEN_PQ = 21 SAFETY_SUBARU_LEGACY = 22 SERIAL_DEBUG = 0 @@ -187,6 +188,8 @@ def connect(self, claim=True, wait=False): self.bootstub = device.getProductID() == 0xddee self.legacy = (device.getbcdDevice() != 0x2300) self._handle = device.open() + if not sys.platform in ["win32", "cygwin", "msys"]: + self._handle.setAutoDetachKernelDriver(True) if claim: self._handle.claimInterface(0) #self._handle.setInterfaceAltSetting(0, 0) #Issue in USB stack @@ -296,6 +299,7 @@ def flash(self, fn=None, code=None, reconnect=True): self.reconnect() def recover(self, timeout=None): + self.reset(enter_bootstub=True) self.reset(enter_bootloader=True) t_start = time.time() while len(PandaDFU.list()) == 0: @@ -476,7 +480,12 @@ def set_uart_callback(self, uart, install): # ******************* can ******************* - def can_send_many(self, arr): + # The panda will NAK CAN writes when there is CAN congestion. + # libusb will try to send it again, with a max timeout. + # Timeout is in ms. If set to 0, the timeout is infinite. + CAN_SEND_TIMEOUT_MS = 10 + + def can_send_many(self, arr, timeout=CAN_SEND_TIMEOUT_MS): snds = [] transmit = 1 extended = 4 @@ -499,13 +508,13 @@ def can_send_many(self, arr): for s in snds: self._handle.bulkWrite(3, s) else: - self._handle.bulkWrite(3, b''.join(snds)) + self._handle.bulkWrite(3, b''.join(snds), timeout=timeout) break except (usb1.USBErrorIO, usb1.USBErrorOverflow): print("CAN: BAD SEND MANY, RETRYING") - def can_send(self, addr, dat, bus): - self.can_send_many([[addr, None, dat, bus]]) + def can_send(self, addr, dat, bus, timeout=CAN_SEND_TIMEOUT_MS): + self.can_send_many([[addr, None, dat, bus]], timeout=timeout) def can_recv(self): dat = bytearray() diff --git a/python/flash_release.py b/python/flash_release.py index b50c9b36b3af78..85bc55368fd688 100755 --- a/python/flash_release.py +++ b/python/flash_release.py @@ -65,19 +65,22 @@ def status(x): panda.close() # flashing ESP - status("4. Flashing ESP (slow!)") - align = lambda x, sz=0x1000: x+"\xFF"*((sz-len(x)) % sz) - esp = ESPROM(st_serial) - esp.connect() - flasher = CesantaFlasher(esp, 230400) - flasher.flash_write(0x0, align(code_boot_15), True) - flasher.flash_write(0x1000, align(code_user1), True) - flasher.flash_write(0x81000, align(code_user2), True) - flasher.flash_write(0x3FE000, "\xFF"*0x1000) - flasher.boot_fw() - del flasher - del esp - time.sleep(1) + if panda.is_white(): + status("4. Flashing ESP (slow!)") + align = lambda x, sz=0x1000: x+"\xFF"*((sz-len(x)) % sz) + esp = ESPROM(st_serial) + esp.connect() + flasher = CesantaFlasher(esp, 230400) + flasher.flash_write(0x0, align(code_boot_15), True) + flasher.flash_write(0x1000, align(code_user1), True) + flasher.flash_write(0x81000, align(code_user2), True) + flasher.flash_write(0x3FE000, "\xFF"*0x1000) + flasher.boot_fw() + del flasher + del esp + time.sleep(1) + else: + status("4. No ESP in non-white panda") # check for connection status("5. Verifying version") diff --git a/tests/automated/7_can_loopback.py b/tests/automated/7_can_loopback.py index cb9f5570b5551f..0b0df1bf11fcea 100644 --- a/tests/automated/7_can_loopback.py +++ b/tests/automated/7_can_loopback.py @@ -1,6 +1,7 @@ import os import time import random +import threading from panda import Panda from nose.tools import assert_equal, assert_less, assert_greater from .helpers import panda_jungle, start_heartbeat_thread, reset_pandas, time_many_sends, test_all_pandas, test_all_gen2_pandas, clear_can_buffers, panda_connect_and_init @@ -200,3 +201,44 @@ def test(p_send, p_recv): finally: # Set back to silent mode p.set_safety_mode(Panda.SAFETY_SILENT) + +@test_all_pandas +@panda_connect_and_init +def test_bulk_write(p): + # The TX buffers on pandas is 0x100 in length. + NUM_MESSAGES_PER_BUS = 10000 + + def flood_tx(panda): + print('Sending!') + msg = b"\xaa"*4 + packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS + + # Disable timeout + panda.can_send_many(packet, timeout=0) + print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!") + + # Start heartbeat + start_heartbeat_thread(p) + + # Set safety mode and power saving + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p.set_power_save(False) + + # Start transmisson + threading.Thread(target=flood_tx, args=(p,)).start() + + # Receive as much as we can in a few second time period + rx = [] + old_len = 0 + start_time = time.time() + while time.time() - start_time < 2 or len(rx) > old_len: + old_len = len(rx) + rx.extend(panda_jungle.can_recv()) + print(f"Received {len(rx)} messages") + + # All messages should have been received + if len(rx) != 3*NUM_MESSAGES_PER_BUS: + Exception("Did not receive all messages!") + + # Set back to silent mode + p.set_safety_mode(Panda.SAFETY_SILENT) diff --git a/tests/bulk_write_test.py b/tests/bulk_write_test.py new file mode 100755 index 00000000000000..43a52bce57e22c --- /dev/null +++ b/tests/bulk_write_test.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import time +import threading + +from panda import Panda + +# The TX buffers on pandas is 0x100 in length. +NUM_MESSAGES_PER_BUS = 10000 + +def flood_tx(panda): + print('Sending!') + msg = b"\xaa"*4 + packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS + panda.can_send_many(packet) + print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!") + +if __name__ == "__main__": + serials = Panda.list() + if len(serials) != 2: + raise Exception("Connect two pandas to perform this test!") + + sender = Panda(serials[0]) + receiver = Panda(serials[1]) + + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + receiver.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # Start transmisson + threading.Thread(target=flood_tx, args=(sender,)).start() + + # Receive as much as we can in a few second time period + rx = [] + old_len = 0 + start_time = time.time() + while time.time() - start_time < 2 or len(rx) > old_len: + old_len = len(rx) + rx.extend(receiver.can_recv()) + print(f"Received {len(rx)} messages") diff --git a/tests/safety/Dockerfile b/tests/safety/Dockerfile index 3c70d253411b26..524cfc3991a071 100644 --- a/tests/safety/Dockerfile +++ b/tests/safety/Dockerfile @@ -1,6 +1,20 @@ FROM ubuntu:16.04 -RUN apt-get update && apt-get install -y clang make python python-pip git curl locales zlib1g-dev libffi-dev bzip2 libssl-dev libbz2-dev libusb-1.0-0 +RUN apt-get update && apt-get install -y \ + autoconf \ + clang \ + curl \ + git \ + libtool \ + libssl-dev \ + libusb-1.0-0 \ + libzmq3-dev \ + locales \ + make \ + python \ + python-pip \ + wget \ + zlib1g-dev RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen ENV LANG en_US.UTF-8 @@ -17,4 +31,20 @@ RUN pyenv rehash COPY requirements.txt /tmp/ RUN pip install -r /tmp/requirements.txt -COPY . /panda +WORKDIR /openpilot +RUN git clone https://github.com/commaai/opendbc.git || true +WORKDIR /openpilot/opendbc +RUN git pull && git checkout 7f3b1774dd248d4ebad91cc9de0fb1c561fab54b +WORKDIR /openpilot +RUN git clone https://github.com/commaai/cereal.git +WORKDIR /openpilot/cereal +RUN git pull && git checkout e370f79522ff7fc0b16f33f4fef420be48061206 +RUN /openpilot/cereal/install_capnp.sh + +RUN pip install -r /openpilot/opendbc/requirements.txt + +WORKDIR /openpilot +RUN cp /openpilot/opendbc/SConstruct /openpilot +COPY . /openpilot/panda + +RUN scons -c && scons -j$(nproc) diff --git a/tests/safety/common.py b/tests/safety/common.py index e0296d3ac11d54..8d81f115c18b77 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -1,70 +1,410 @@ +import abc +import struct +import unittest +import numpy as np +from opendbc.can.packer import CANPacker # pylint: disable=import-error from panda.tests.safety import libpandasafety_py MAX_WRONG_COUNTERS = 5 -def make_msg(bus, addr, length=8): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') +class UNSAFE_MODE: + DEFAULT = 0 + DISABLE_DISENGAGE_ON_GAS = 1 + DISABLE_STOCK_AEB = 2 + RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 + +def package_can_msg(msg): + addr, _, dat, bus = msg + rdlr, rdhr = struct.unpack('II', dat.ljust(8, b'\x00')) + + ret = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') if addr >= 0x800: - to_send[0].RIR = (addr << 3) | 5 + ret[0].RIR = (addr << 3) | 5 else: - to_send[0].RIR = (addr << 21) | 1 - to_send[0].RDTR = length - to_send[0].RDTR |= bus << 4 - - return to_send - -class StdTest: - @staticmethod - def test_relay_malfunction(test, addr, bus=0): - # input is a test class and the address that, if seen on specified bus, triggers - # the relay_malfunction protection logic: both tx_hook and fwd_hook are - # expected to return failure - test.assertFalse(test.safety.get_relay_malfunction()) - test.safety.safety_rx_hook(make_msg(bus, addr, 8)) - test.assertTrue(test.safety.get_relay_malfunction()) + ret[0].RIR = (addr << 21) | 1 + ret[0].RDTR = len(dat) | ((bus & 0xF) << 4) + ret[0].RDHR = rdhr + ret[0].RDLR = rdlr + + return ret + +def make_msg(bus, addr, length=8): + return package_can_msg([addr, 0, b'\x00'*length, bus]) + +class CANPackerPanda(CANPacker): + def make_can_msg_panda(self, name_or_addr, bus, values, counter=-1): + msg = self.make_can_msg(name_or_addr, bus, values, counter=-1) + return package_can_msg(msg) + +class PandaSafetyTestBase(unittest.TestCase): + @classmethod + def setUpClass(cls): + if cls.__name__ == "PandaSafetyTestBase": + cls.safety = None + raise unittest.SkipTest + + def _rx(self, msg): + return self.safety.safety_rx_hook(msg) + + def _tx(self, msg): + return self.safety.safety_tx_hook(msg) + +class InterceptorSafetyTest(PandaSafetyTestBase): + + INTERCEPTOR_THRESHOLD = 0 + + @classmethod + def setUpClass(cls): + if cls.__name__ == "InterceptorSafetyTest": + cls.safety = None + raise unittest.SkipTest + + @abc.abstractmethod + def _interceptor_msg(self, gas, addr): + pass + + def test_prev_gas_interceptor(self): + self._rx(self._interceptor_msg(0x0, 0x201)) + self.assertFalse(self.safety.get_gas_interceptor_prev()) + self._rx(self._interceptor_msg(0x1000, 0x201)) + self.assertTrue(self.safety.get_gas_interceptor_prev()) + self._rx(self._interceptor_msg(0x0, 0x201)) + self.safety.set_gas_interceptor_detected(False) + + def test_disengage_on_gas_interceptor(self): + for g in range(0, 0x1000): + self._rx(self._interceptor_msg(0, 0x201)) + self.safety.set_controls_allowed(True) + self._rx(self._interceptor_msg(g, 0x201)) + remain_enabled = g <= self.INTERCEPTOR_THRESHOLD + self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) + self._rx(self._interceptor_msg(0, 0x201)) + self.safety.set_gas_interceptor_detected(False) + + def test_unsafe_mode_no_disengage_on_gas_interceptor(self): + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + for g in range(0, 0x1000): + self._rx(self._interceptor_msg(g, 0x201)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._interceptor_msg(0, 0x201)) + self.safety.set_gas_interceptor_detected(False) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + + def test_allow_engage_with_gas_interceptor_pressed(self): + self._rx(self._interceptor_msg(0x1000, 0x201)) + self.safety.set_controls_allowed(1) + self._rx(self._interceptor_msg(0x1000, 0x201)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._interceptor_msg(0, 0x201)) + + def test_gas_interceptor_safety_check(self): + for gas in np.arange(0, 4000, 100): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + if controls_allowed: + send = True + else: + send = gas == 0 + self.assertEqual(send, self._tx(self._interceptor_msg(gas, 0x200))) + + +class TorqueSteeringSafetyTest(PandaSafetyTestBase): + + MAX_RATE_UP = 0 + MAX_RATE_DOWN = 0 + MAX_TORQUE = 0 + MAX_RT_DELTA = 0 + RT_INTERVAL = 0 + MAX_TORQUE_ERROR = 0 + TORQUE_MEAS_TOLERANCE = 0 + + @classmethod + def setUpClass(cls): + if cls.__name__ == "TorqueSteeringSafetyTest": + cls.safety = None + raise unittest.SkipTest + + @abc.abstractmethod + def _torque_meas_msg(self, torque): + pass + + @abc.abstractmethod + def _torque_msg(self, torque): + pass + + def _set_prev_torque(self, t): + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) + self.safety.set_torque_meas(t, t) + + def test_steer_safety_check(self): + for enabled in [0, 1]: + for t in range(-self.MAX_TORQUE*2, self.MAX_TORQUE*2): + self.safety.set_controls_allowed(enabled) + self._set_prev_torque(t) + if abs(t) > self.MAX_TORQUE or (not enabled and abs(t) > 0): + self.assertFalse(self._tx(self._torque_msg(t))) + else: + self.assertTrue(self._tx(self._torque_msg(t))) + + def test_torque_absolute_limits(self): + for controls_allowed in [True, False]: + for torque in np.arange(-self.MAX_TORQUE - 1000, self.MAX_TORQUE + 1000, self.MAX_RATE_UP): + self.safety.set_controls_allowed(controls_allowed) + self.safety.set_rt_torque_last(torque) + self.safety.set_torque_meas(torque, torque) + self.safety.set_desired_torque_last(torque - self.MAX_RATE_UP) + + if controls_allowed: + send = (-self.MAX_TORQUE <= torque <= self.MAX_TORQUE) + else: + send = torque == 0 + + self.assertEqual(send, self._tx(self._torque_msg(torque))) + + def test_non_realtime_limit_up(self): + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self._tx(self._torque_msg(self.MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self._tx(self._torque_msg(self.MAX_RATE_UP + 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_controls_allowed(True) + + torque_meas = self.MAX_TORQUE - self.MAX_TORQUE_ERROR - 50 + + self.safety.set_rt_torque_last(self.MAX_TORQUE) + self.safety.set_torque_meas(torque_meas, torque_meas) + self.safety.set_desired_torque_last(self.MAX_TORQUE) + self.assertTrue(self._tx(self._torque_msg(self.MAX_TORQUE - self.MAX_RATE_DOWN))) + + self.safety.set_rt_torque_last(self.MAX_TORQUE) + self.safety.set_torque_meas(torque_meas, torque_meas) + self.safety.set_desired_torque_last(self.MAX_TORQUE) + self.assertFalse(self._tx(self._torque_msg(self.MAX_TORQUE - self.MAX_RATE_DOWN + 1))) + + def test_exceed_torque_sensor(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self._set_prev_torque(0) + for t in np.arange(0, self.MAX_TORQUE_ERROR + 2, 2): # step needs to be smaller than MAX_TORQUE_ERROR + t *= sign + self.assertTrue(self._tx(self._torque_msg(t))) + + self.assertFalse(self._tx(self._torque_msg(sign * (self.MAX_TORQUE_ERROR + 2)))) + + def test_realtime_limit_up(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self.safety.init_tests() + self._set_prev_torque(0) + for t in np.arange(0, self.MAX_RT_DELTA+1, 1): + t *= sign + self.safety.set_torque_meas(t, t) + self.assertTrue(self._tx(self._torque_msg(t))) + self.assertFalse(self._tx(self._torque_msg(sign * (self.MAX_RT_DELTA + 1)))) + + self._set_prev_torque(0) + for t in np.arange(0, self.MAX_RT_DELTA+1, 1): + t *= sign + self.safety.set_torque_meas(t, t) + self.assertTrue(self._tx(self._torque_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(self.RT_INTERVAL + 1) + self.assertTrue(self._tx(self._torque_msg(sign * self.MAX_RT_DELTA))) + self.assertTrue(self._tx(self._torque_msg(sign * (self.MAX_RT_DELTA + 1)))) + + def test_torque_measurements(self): + trq = 50 + for t in [trq, -trq, 0, 0, 0, 0]: + self._rx(self._torque_meas_msg(t)) + + max_range = range(trq, trq + self.TORQUE_MEAS_TOLERANCE + 1) + min_range = range(-(trq+self.TORQUE_MEAS_TOLERANCE), -trq + 1) + self.assertTrue(self.safety.get_torque_meas_min() in min_range) + self.assertTrue(self.safety.get_torque_meas_max() in max_range) + + max_range = range(0, self.TORQUE_MEAS_TOLERANCE+1) + min_range = range(-(trq+self.TORQUE_MEAS_TOLERANCE), -trq + 1) + self._rx(self._torque_meas_msg(0)) + self.assertTrue(self.safety.get_torque_meas_min() in min_range) + self.assertTrue(self.safety.get_torque_meas_max() in max_range) + + max_range = range(0, self.TORQUE_MEAS_TOLERANCE+1) + min_range = range(-self.TORQUE_MEAS_TOLERANCE, 0 + 1) + self._rx(self._torque_meas_msg(0)) + self.assertTrue(self.safety.get_torque_meas_min() in min_range) + self.assertTrue(self.safety.get_torque_meas_max() in max_range) + + +class PandaSafetyTest(PandaSafetyTestBase): + TX_MSGS = None + STANDSTILL_THRESHOLD = None + GAS_PRESSED_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = None + RELAY_MALFUNCTION_BUS = None + FWD_BLACKLISTED_ADDRS = {} # {bus: [addr]} + FWD_BUS_LOOKUP = {} + + @classmethod + def setUpClass(cls): + if cls.__name__ == "PandaSafetyTest": + cls.safety = None + raise unittest.SkipTest + + @abc.abstractmethod + def _brake_msg(self, brake): + pass + + @abc.abstractmethod + def _speed_msg(self, speed): + pass + + @abc.abstractmethod + def _gas_msg(self, speed): + pass + + @abc.abstractmethod + def _pcm_status_msg(self, enable): + pass + + # ***** standard tests for all safety modes ***** + + def test_relay_malfunction(self): + # each car has an addr that is used to detect relay malfunction + # if that addr is seen on specified bus, triggers the relay malfunction + # protection logic: both tx_hook and fwd_hook are expected to return failure + self.assertFalse(self.safety.get_relay_malfunction()) + self._rx(make_msg(self.RELAY_MALFUNCTION_BUS, self.RELAY_MALFUNCTION_ADDR, 8)) + self.assertTrue(self.safety.get_relay_malfunction()) for a in range(1, 0x800): for b in range(0, 3): - test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8))) - test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8))) - - @staticmethod - def test_manually_enable_controls_allowed(test): - test.safety.set_controls_allowed(1) - test.assertTrue(test.safety.get_controls_allowed()) - test.safety.set_controls_allowed(0) - test.assertFalse(test.safety.get_controls_allowed()) - - @staticmethod - def test_spam_can_buses(test, TX_MSGS): + self.assertFalse(self._tx(make_msg(b, a, 8))) + self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, a, 8))) + + def test_fwd_hook(self): + # some safety modes don't forward anything, while others blacklist msgs + for bus in range(0x0, 0x3): + for addr in range(0x1, 0x800): + # assume len 8 + msg = make_msg(bus, addr, 8) + fwd_bus = self.FWD_BUS_LOOKUP.get(bus, -1) + if bus in self.FWD_BLACKLISTED_ADDRS and addr in self.FWD_BLACKLISTED_ADDRS[bus]: + fwd_bus = -1 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(bus, msg)) + + def test_spam_can_buses(self): for addr in range(1, 0x800): for bus in range(0, 4): - if all(addr != m[0] or bus != m[1] for m in TX_MSGS): - test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8))) + if all(addr != m[0] or bus != m[1] for m in self.TX_MSGS): + self.assertFalse(self._tx(make_msg(bus, addr, 8))) + + 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) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_prev_gas(self): + self.assertFalse(self.safety.get_gas_pressed_prev()) + for pressed in [self.GAS_PRESSED_THRESHOLD+1, 0]: + self._rx(self._gas_msg(pressed)) + self.assertEqual(bool(pressed), self.safety.get_gas_pressed_prev()) + + def test_allow_engage_with_gas_pressed(self): + self._rx(self._gas_msg(1)) + self.safety.set_controls_allowed(True) + self._rx(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) - @staticmethod - def test_allow_brake_at_zero_speed(test): + def test_disengage_on_gas(self): + self._rx(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self._rx(self._gas_msg(self.GAS_PRESSED_THRESHOLD+1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_unsafe_mode_no_disengage_on_gas(self): + self._rx(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self._rx(self._gas_msg(self.GAS_PRESSED_THRESHOLD+1)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_prev_brake(self): + self.assertFalse(self.safety.get_brake_pressed_prev()) + for pressed in [True, False]: + self._rx(self._brake_msg(not pressed)) + self.assertEqual(not pressed, self.safety.get_brake_pressed_prev()) + self._rx(self._brake_msg(pressed)) + self.assertEqual(pressed, self.safety.get_brake_pressed_prev()) + + def test_enable_control_allowed_from_cruise(self): + self._rx(self._pcm_status_msg(False)) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._pcm_status_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disable_control_allowed_from_cruise(self): + self.safety.set_controls_allowed(1) + self._rx(self._pcm_status_msg(False)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_cruise_engaged_prev(self): + for engaged in [True, False]: + self._rx(self._pcm_status_msg(engaged)) + self.assertEqual(engaged, self.safety.get_cruise_engaged_prev()) + self._rx(self._pcm_status_msg(not engaged)) + self.assertEqual(not engaged, self.safety.get_cruise_engaged_prev()) + + def test_allow_brake_at_zero_speed(self): # Brake was already pressed - test.safety.safety_rx_hook(test._speed_msg(0)) - test.safety.safety_rx_hook(test._brake_msg(1)) - test.safety.set_controls_allowed(1) - test.safety.safety_rx_hook(test._brake_msg(1)) - test.assertTrue(test.safety.get_controls_allowed()) - test.safety.safety_rx_hook(test._brake_msg(0)) - test.assertTrue(test.safety.get_controls_allowed()) + self._rx(self._speed_msg(0)) + self._rx(self._brake_msg(1)) + self.safety.set_controls_allowed(1) + self._rx(self._brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._brake_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) # rising edge of brake should disengage - test.safety.safety_rx_hook(test._brake_msg(1)) - test.assertFalse(test.safety.get_controls_allowed()) - test.safety.safety_rx_hook(test._brake_msg(0)) # reset no brakes + self._rx(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._brake_msg(0)) # reset no brakes - @staticmethod - def test_not_allow_brake_when_moving(test, standstill_threshold): + def test_not_allow_brake_when_moving(self): # Brake was already pressed - test.safety.safety_rx_hook(test._brake_msg(1)) - test.safety.set_controls_allowed(1) - test.safety.safety_rx_hook(test._speed_msg(standstill_threshold)) - test.safety.safety_rx_hook(test._brake_msg(1)) - test.assertTrue(test.safety.get_controls_allowed()) - test.safety.safety_rx_hook(test._speed_msg(standstill_threshold + 1)) - test.safety.safety_rx_hook(test._brake_msg(1)) - test.assertFalse(test.safety.get_controls_allowed()) - test.safety.safety_rx_hook(test._speed_msg(0)) + self._rx(self._brake_msg(1)) + self.safety.set_controls_allowed(1) + self._rx(self._speed_msg(self.STANDSTILL_THRESHOLD)) + self._rx(self._brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._speed_msg(self.STANDSTILL_THRESHOLD + 1)) + self._rx(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._speed_msg(0)) + + def test_sample_speed(self): + self.assertFalse(self.safety.get_vehicle_moving()) + + # not moving + self.safety.safety_rx_hook(self._speed_msg(0)) + self.assertFalse(self.safety.get_vehicle_moving()) + + # speed is at threshold + self.safety.safety_rx_hook(self._speed_msg(self.STANDSTILL_THRESHOLD)) + self.assertFalse(self.safety.get_vehicle_moving()) + + # past threshold + self.safety.safety_rx_hook(self._speed_msg(self.STANDSTILL_THRESHOLD+1)) + self.assertTrue(self.safety.get_vehicle_moving()) diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 3b552d0f80ed62..9898e6df8924b9 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -32,6 +32,8 @@ void set_controls_allowed(bool c); bool get_controls_allowed(void); +void set_unsafe_mode(int mode); +int get_unsafe_mode(void); void set_relay_malfunction(bool c); bool get_relay_malfunction(void); void set_gas_interceptor_detected(bool c); @@ -39,6 +41,18 @@ int get_gas_interceptor_prev(void); bool get_gas_pressed_prev(void); bool get_brake_pressed_prev(void); + +void set_torque_meas(int min, int max); +int get_torque_meas_min(void); +int get_torque_meas_max(void); +void set_torque_driver(int min, int max); +int get_torque_driver_min(void); +int get_torque_driver_max(void); +void set_desired_torque_last(int t); +void set_rt_torque_last(int t); + +bool get_cruise_engaged_prev(void); +bool get_vehicle_moving(void); int get_hw_type(void); void set_timer(uint32_t t); @@ -47,53 +61,16 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); int set_safety_hooks(uint16_t mode, int16_t param); -void init_tests_toyota(void); -int get_toyota_torque_meas_min(void); -int get_toyota_torque_meas_max(void); -void set_toyota_torque_meas(int min, int max); -void set_toyota_desired_torque_last(int t); -void set_toyota_rt_torque_last(int t); +void init_tests(void); void init_tests_honda(void); -bool get_honda_moving(void); void set_honda_fwd_brake(bool); void set_honda_alt_brake_msg(bool); int get_honda_hw(void); -void init_tests_cadillac(void); -void set_cadillac_desired_torque_last(int t); -void set_cadillac_rt_torque_last(int t); -void set_cadillac_torque_driver(int min, int max); - -void init_tests_gm(void); -void set_gm_desired_torque_last(int t); -void set_gm_rt_torque_last(int t); -void set_gm_torque_driver(int min, int max); - -void init_tests_hyundai(void); -void set_hyundai_desired_torque_last(int t); -void set_hyundai_rt_torque_last(int t); -void set_hyundai_torque_driver(int min, int max); - void init_tests_chrysler(void); -void set_chrysler_desired_torque_last(int t); -void set_chrysler_rt_torque_last(int t); -int get_chrysler_torque_meas_min(void); -int get_chrysler_torque_meas_max(void); -void set_chrysler_torque_meas(int min, int max); - -void init_tests_subaru(void); -void set_subaru_desired_torque_last(int t); -void set_subaru_rt_torque_last(int t); -bool get_subaru_global(void); -void init_tests_volkswagen(void); -int get_volkswagen_torque_driver_min(void); -int get_volkswagen_torque_driver_max(void); -bool get_volkswagen_moving(void); -void set_volkswagen_desired_torque_last(int t); -void set_volkswagen_rt_torque_last(int t); -void set_volkswagen_torque_driver(int min, int max); +bool get_subaru_global(void); void init_tests_nissan(void); void set_nissan_desired_angle_last(int t); diff --git a/tests/safety/test.c b/tests/safety/test.c index 9c7955b2869ce0..52d2ea7f1a1080 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -23,13 +23,8 @@ typedef struct uint32_t CNT; } TIM_TypeDef; -struct sample_t toyota_torque_meas; -struct sample_t cadillac_torque_driver; -struct sample_t gm_torque_driver; -struct sample_t hyundai_torque_driver; -struct sample_t chrysler_torque_meas; -struct sample_t subaru_torque_driver; -struct sample_t volkswagen_torque_driver; +struct sample_t torque_meas; +struct sample_t torque_driver; TIM_TypeDef timer; TIM_TypeDef *TIM2 = &timer; @@ -62,6 +57,13 @@ uint8_t hw_type = HW_TYPE_UNKNOWN; ({ __typeof__ (a) _a = (a); \ (_a > 0) ? _a : (-_a); }) +// from faults.h +#define FAULT_RELAY_MALFUNCTION (1U << 0) +void fault_occurred(uint32_t fault) { +} +void fault_recovered(uint32_t fault) { +} + // from llcan.h #define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF) #define GET_LEN(msg) ((msg)->RDTR & 0xf) @@ -72,7 +74,9 @@ uint8_t hw_type = HW_TYPE_UNKNOWN; #define UNUSED(x) (void)(x) +#ifndef PANDA #define PANDA +#endif #define NULL ((void*)0) #define static #include "safety.h" @@ -81,6 +85,10 @@ void set_controls_allowed(bool c){ controls_allowed = c; } +void set_unsafe_mode(int mode){ + unsafe_mode = mode; +} + void set_relay_malfunction(bool c){ relay_malfunction = c; } @@ -93,6 +101,10 @@ bool get_controls_allowed(void){ return controls_allowed; } +int get_unsafe_mode(void){ + return unsafe_mode; +} + bool get_relay_malfunction(void){ return relay_malfunction; } @@ -113,6 +125,14 @@ bool get_brake_pressed_prev(void){ return brake_pressed_prev; } +bool get_cruise_engaged_prev(void){ + return cruise_engaged_prev; +} + +bool get_vehicle_moving(void){ + return vehicle_moving; +} + int get_hw_type(void){ return hw_type; } @@ -125,122 +145,38 @@ void set_timer(uint32_t t){ timer.CNT = t; } -void set_toyota_torque_meas(int min, int max){ - toyota_torque_meas.min = min; - toyota_torque_meas.max = max; -} - -void set_cadillac_torque_driver(int min, int max){ - 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; -} - -void set_hyundai_torque_driver(int min, int max){ - hyundai_torque_driver.min = min; - hyundai_torque_driver.max = max; -} - -void set_chrysler_torque_meas(int min, int max){ - chrysler_torque_meas.min = min; - chrysler_torque_meas.max = max; +void set_torque_meas(int min, int max){ + torque_meas.min = min; + torque_meas.max = max; } -void set_volkswagen_torque_driver(int min, int max){ - volkswagen_torque_driver.min = min; - volkswagen_torque_driver.max = max; +int get_torque_meas_min(void){ + return torque_meas.min; } -int get_volkswagen_torque_driver_min(void){ - return volkswagen_torque_driver.min; +int get_torque_meas_max(void){ + return torque_meas.max; } -int get_volkswagen_torque_driver_max(void){ - return volkswagen_torque_driver.max; +void set_torque_driver(int min, int max){ + torque_driver.min = min; + torque_driver.max = max; } -int get_chrysler_torque_meas_min(void){ - return chrysler_torque_meas.min; +int get_torque_driver_min(void){ + return torque_driver.min; } -int get_chrysler_torque_meas_max(void){ - return chrysler_torque_meas.max; +int get_torque_driver_max(void){ + return torque_driver.max; } -int get_toyota_torque_meas_min(void){ - return toyota_torque_meas.min; +void set_rt_torque_last(int t){ + rt_torque_last = t; } -int get_toyota_torque_meas_max(void){ - return toyota_torque_meas.max; -} - -void set_toyota_rt_torque_last(int t){ - toyota_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_hyundai_rt_torque_last(int t){ - hyundai_rt_torque_last = t; -} - -void set_chrysler_rt_torque_last(int t){ - chrysler_rt_torque_last = t; -} - -void set_subaru_rt_torque_last(int t){ - subaru_rt_torque_last = t; -} - -void set_volkswagen_rt_torque_last(int t){ - volkswagen_rt_torque_last = t; -} - -void set_toyota_desired_torque_last(int t){ - toyota_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; -} - -void set_hyundai_desired_torque_last(int t){ - hyundai_desired_torque_last = t; -} - -void set_chrysler_desired_torque_last(int t){ - chrysler_desired_torque_last = t; -} - -void set_subaru_desired_torque_last(int t){ - subaru_desired_torque_last = t; -} - -void set_volkswagen_desired_torque_last(int t){ - volkswagen_desired_torque_last = t; -} - -int get_volkswagen_moving(void){ - return volkswagen_moving; -} - -bool get_honda_moving(void){ - return honda_moving; +void set_desired_torque_last(int t){ + desired_torque_last = t; } void set_honda_alt_brake_msg(bool c){ @@ -265,83 +201,25 @@ void init_tests(void){ safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic gas_pressed_prev = false; brake_pressed_prev = false; -} - -void init_tests_toyota(void){ - init_tests(); - toyota_torque_meas.min = 0; - toyota_torque_meas.max = 0; - toyota_desired_torque_last = 0; - toyota_rt_torque_last = 0; - toyota_ts_last = 0; - set_timer(0); -} - -void init_tests_cadillac(void){ - init_tests(); - 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){ - init_tests(); - 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_hyundai(void){ - init_tests(); - hyundai_torque_driver.min = 0; - hyundai_torque_driver.max = 0; - hyundai_desired_torque_last = 0; - hyundai_rt_torque_last = 0; - hyundai_ts_last = 0; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = 0; + torque_driver.min = 0; + torque_driver.max = 0; + torque_meas.min = 0; + torque_meas.max = 0; + vehicle_moving = false; + unsafe_mode = 0; set_timer(0); } void init_tests_chrysler(void){ init_tests(); chrysler_speed = 0; - chrysler_torque_meas.min = 0; - chrysler_torque_meas.max = 0; - chrysler_desired_torque_last = 0; - chrysler_rt_torque_last = 0; - chrysler_ts_last = 0; - set_timer(0); -} - -void init_tests_subaru(void){ - init_tests(); - subaru_torque_driver.min = 0; - subaru_torque_driver.max = 0; - subaru_desired_torque_last = 0; - subaru_rt_torque_last = 0; - subaru_ts_last = 0; - set_timer(0); -} - -void init_tests_volkswagen(void){ - init_tests(); - volkswagen_moving = false; - volkswagen_torque_driver.min = 0; - volkswagen_torque_driver.max = 0; - volkswagen_desired_torque_last = 0; - volkswagen_rt_torque_last = 0; - volkswagen_ts_last = 0; - set_timer(0); } void init_tests_honda(void){ init_tests(); - honda_moving = false; honda_fwd_brake = false; } diff --git a/tests/safety/test_cadillac.py b/tests/safety/test_cadillac.py deleted file mode 100644 index e4e75f15bdf4a9..00000000000000 --- a/tests/safety/test_cadillac.py +++ /dev/null @@ -1,184 +0,0 @@ -#!/usr/bin/env python3 -import unittest -import numpy as np -from panda import Panda -from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import make_msg, StdTest - - -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 - -TX_MSGS = [[0x151, 2], [0x152, 0], [0x153, 2], [0x154, 0]] - -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.set_safety_hooks(Panda.SAFETY_CADILLAC, 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): - t = twos_comp(torque, 11) - to_send = make_msg(0, 0x164) - to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8) - return to_send - - def _torque_msg(self, torque): - to_send = make_msg(2, 0x151) - t = twos_comp(torque, 14) - to_send[0].RDLR = ((t >> 8) & 0x3F) | ((t & 0xFF) << 8) - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) - - def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x370) - to_push[0].RDLR = 0x800000 - self.safety.safety_rx_hook(to_push) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x370) - self.safety.set_controls_allowed(1) - self.safety.safety_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.safety_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.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) - self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP))) - - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) - self.safety.set_controls_allowed(True) - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_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.safety_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.safety_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.safety_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.safety_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.safety_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.safety_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.safety_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.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_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.safety_tx_hook(self._torque_msg(t))) - - # Increase timer to update rt_torque_last - self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - - def test_fwd_hook(self): - # nothing allowed - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - for b in buss: - for m in msgs: - # assume len 8 - self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - - -if __name__ == "__main__": - unittest.main() diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index c66b90b6ba3eb0..369f1f98091387 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -1,258 +1,84 @@ #!/usr/bin/env python3 import unittest -import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda -MAX_RATE_UP = 3 -MAX_RATE_DOWN = 3 -MAX_STEER = 261 -MAX_RT_DELTA = 112 -RT_INTERVAL = 250000 +class TestChryslerSafety(common.PandaSafetyTest, common.TorqueSteeringSafetyTest): + TX_MSGS = [[571, 0], [658, 0], [678, 0]] + STANDSTILL_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = 0x292 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [658, 678]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} -MAX_TORQUE_ERROR = 80 + MAX_RATE_UP = 3 + MAX_RATE_DOWN = 3 + MAX_TORQUE = 261 + MAX_RT_DELTA = 112 + RT_INTERVAL = 250000 + MAX_TORQUE_ERROR = 80 -TX_MSGS = [[571, 0], [658, 0], [678, 0]] - -def chrysler_checksum(msg, len_msg): - checksum = 0xFF - for idx in range(0, len_msg-1): - curr = (msg.RDLR >> (8*idx)) if idx < 4 else (msg.RDHR >> (8*(idx - 4))) - curr &= 0xFF - shift = 0x80 - for i in range(0, 8): - bit_sum = curr & shift - temp_chk = checksum & 0x80 - if (bit_sum != 0): - bit_sum = 0x1C - if (temp_chk != 0): - bit_sum = 1 - checksum = checksum << 1 - temp_chk = checksum | 1 - bit_sum ^= temp_chk - else: - if (temp_chk != 0): - bit_sum = 0x1D - checksum = checksum << 1 - bit_sum ^= checksum - checksum = bit_sum - shift = shift >> 1 - return ~checksum & 0xFF - -class TestChryslerSafety(unittest.TestCase): cnt_torque_meas = 0 cnt_gas = 0 cnt_cruise = 0 cnt_brake = 0 - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0) - cls.safety.init_tests_chrysler() + def setUp(self): + self.packer = CANPackerPanda("chrysler_pacifica_2017_hybrid") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0) + self.safety.init_tests_chrysler() - def _button_msg(self, buttons): - to_send = make_msg(0, 571) - to_send[0].RDLR = buttons - return to_send + def _button_msg(self, cancel): + values = {"ACC_CANCEL": cancel} + return self.packer.make_can_msg_panda("WHEEL_BUTTONS", 0, values) - def _cruise_msg(self, active): - to_send = make_msg(0, 500) - to_send[0].RDLR = 0x380000 if active else 0 - to_send[0].RDHR |= (self.cnt_cruise % 16) << 20 - to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + def _pcm_status_msg(self, active): + values = {"ACC_STATUS_2": 0x7 if active else 0, \ + "COUNTER": self.cnt_cruise % 16} self.__class__.cnt_cruise += 1 - return to_send + return self.packer.make_can_msg_panda("ACC_2", 0, values) def _speed_msg(self, speed): - speed = int(speed / 0.071028) - to_send = make_msg(0, 514, 4) - to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \ - ((speed & 0xFF0) << 12) + ((speed & 0xF) << 28) - return to_send + values = {"SPEED_LEFT": speed, "SPEED_RIGHT": speed} + return self.packer.make_can_msg_panda("SPEED_1", 0, values) def _gas_msg(self, gas): - to_send = make_msg(0, 308) - to_send[0].RDHR = (gas & 0x7F) << 8 - to_send[0].RDHR |= (self.cnt_gas % 16) << 20 + values = {"ACCEL_134": gas, "COUNTER": self.cnt_gas % 16} self.__class__.cnt_gas += 1 - return to_send + return self.packer.make_can_msg_panda("ACCEL_GAS_134", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 320) - to_send[0].RDLR = 5 if brake else 0 - to_send[0].RDHR |= (self.cnt_brake % 16) << 20 - to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + values = {"BRAKE_PRESSED_2": 5 if brake else 0, \ + "COUNTER": self.cnt_brake % 16} self.__class__.cnt_brake += 1 - return to_send - - def _set_prev_torque(self, t): - self.safety.set_chrysler_desired_torque_last(t) - self.safety.set_chrysler_rt_torque_last(t) - self.safety.set_chrysler_torque_meas(t, t) + return self.packer.make_can_msg_panda("BRAKE_2", 0, values) def _torque_meas_msg(self, torque): - to_send = make_msg(0, 544) - to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8) - to_send[0].RDHR |= (self.cnt_torque_meas % 16) << 20 - to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + values = {"TORQUE_MOTOR": torque, "COUNTER": self.cnt_torque_meas % 16} self.__class__.cnt_torque_meas += 1 - return to_send + return self.packer.make_can_msg_panda("EPS_STATUS", 0, values) def _torque_msg(self, torque): - to_send = make_msg(0, 0x292) - to_send[0].RDLR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8) - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x292) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_steer_safety_check(self): - for enabled in [0, 1]: - for t in range(-MAX_STEER*2, MAX_STEER*2): - 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.safety_tx_hook(self._torque_msg(t))) - else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + values = {"LKAS_STEERING_TORQUE": torque} + return self.packer.make_can_msg_panda("LKAS_COMMAND", 0, values) - def test_enable_control_allowed_from_cruise(self): - to_push = self._cruise_msg(True) - self.safety.safety_rx_hook(to_push) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - to_push = self._cruise_msg(False) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_gas_disable(self): + def test_disengage_on_gas(self): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._speed_msg(2.2)) - self.safety.safety_rx_hook(self._gas_msg(1)) + self._rx(self._speed_msg(2.1)) + self._rx(self._gas_msg(1)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.safety.safety_rx_hook(self._speed_msg(2.3)) - self.safety.safety_rx_hook(self._gas_msg(1)) + self._rx(self._gas_msg(0)) + self._rx(self._speed_msg(2.2)) + self._rx(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, 0) - - def test_non_realtime_limit_up(self): - self.safety.set_controls_allowed(True) - - self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) - - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) - - def test_non_realtime_limit_down(self): - self.safety.set_controls_allowed(True) - - self.safety.set_chrysler_rt_torque_last(MAX_STEER) - torque_meas = MAX_STEER - MAX_TORQUE_ERROR - 20 - self.safety.set_chrysler_torque_meas(torque_meas, torque_meas) - self.safety.set_chrysler_desired_torque_last(MAX_STEER) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN))) - - self.safety.set_chrysler_rt_torque_last(MAX_STEER) - self.safety.set_chrysler_torque_meas(torque_meas, torque_meas) - self.safety.set_chrysler_desired_torque_last(MAX_STEER) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1))) - - def test_exceed_torque_sensor(self): - self.safety.set_controls_allowed(True) - - for sign in [-1, 1]: - self._set_prev_torque(0) - for t in np.arange(0, MAX_TORQUE_ERROR + 2, 2): # step needs to be smaller than MAX_TORQUE_ERROR - t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2)))) - - def test_realtime_limit_up(self): - self.safety.set_controls_allowed(True) - - for sign in [-1, 1]: - self.safety.init_tests_chrysler() - self._set_prev_torque(0) - for t in np.arange(0, MAX_RT_DELTA+1, 1): - t *= sign - self.safety.set_chrysler_torque_meas(t, t) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_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, 1): - t *= sign - self.safety.set_chrysler_torque_meas(t, t) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - # Increase timer to update rt_torque_last - self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * MAX_RT_DELTA))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - def test_torque_measurements(self): - self.safety.safety_rx_hook(self._torque_meas_msg(50)) - self.safety.safety_rx_hook(self._torque_meas_msg(-50)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - - self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min()) - self.assertEqual(50, self.safety.get_chrysler_torque_meas_max()) - - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(0, self.safety.get_chrysler_torque_meas_max()) - self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min()) - - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(0, self.safety.get_chrysler_torque_meas_max()) - self.assertEqual(0, self.safety.get_chrysler_torque_meas_min()) - def test_cancel_button(self): - CANCEL = 1 - for b in range(0, 0xff): - if b == CANCEL: - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(b))) - else: - self.assertFalse(self.safety.safety_tx_hook(self._button_msg(b))) - - def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - blocked_msgs = [658, 678] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + for cancel in [True, False]: + self.assertEqual(cancel, self._tx(self._button_msg(cancel))) if __name__ == "__main__": diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py index 34d92357c79d9f..53433552924b05 100644 --- a/tests/safety/test_gm.py +++ b/tests/safety/test_gm.py @@ -3,7 +3,8 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, UNSAFE_MODE MAX_RATE_UP = 7 MAX_RATE_DOWN = 17 @@ -15,141 +16,104 @@ MAX_RT_DELTA = 128 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 50; -DRIVER_TORQUE_FACTOR = 4; - -TX_MSGS = [[384, 0], [1033, 0], [1034, 0], [715, 0], [880, 0], # pt bus - [161, 1], [774, 1], [776, 1], [784, 1], # obs bus - [789, 2], # ch bus - [0x104c006c, 3], [0x10400060]] # gmlan - -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.set_safety_hooks(Panda.SAFETY_GM, 0) - cls.safety.init_tests_gm() +DRIVER_TORQUE_ALLOWANCE = 50 +DRIVER_TORQUE_FACTOR = 4 + +class TestGmSafety(common.PandaSafetyTest): + TX_MSGS = [[384, 0], [1033, 0], [1034, 0], [715, 0], [880, 0], # pt bus + [161, 1], [774, 1], [776, 1], [784, 1], # obs bus + [789, 2], # ch bus + [0x104c006c, 3], [0x10400060]] # gmlan + STANDSTILL_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = 384 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {} + FWD_BUS_LOOKUP = {} + + def setUp(self): + self.packer = CANPackerPanda("gm_global_a_powertrain") + self.packer_chassis = CANPackerPanda("gm_global_a_chassis") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_GM, 0) + self.safety.init_tests() + + # override these tests from PandaSafetyTest, GM uses button enable + def test_disable_control_allowed_from_cruise(self): pass + def test_enable_control_allowed_from_cruise(self): pass + def test_cruise_engaged_prev(self): pass def _speed_msg(self, speed): - to_send = make_msg(0, 842) - to_send[0].RDLR = speed - return to_send + values = {"%sWheelSpd"%s: speed for s in ["RL", "RR"]} + return self.packer.make_can_msg_panda("EBCMWheelSpdRear", 0, values) def _button_msg(self, buttons): - to_send = make_msg(0, 481) - to_send[0].RDHR = buttons << 12 - return to_send + values = {"ACCButtons": buttons} + return self.packer.make_can_msg_panda("ASCMSteeringButton", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 241) - to_send[0].RDLR = 0xa00 if brake else 0x900 - return to_send + # GM safety has a brake threshold of 10 + values = {"BrakePedalPosition": 10 if brake else 0} + return self.packer.make_can_msg_panda("EBCMBrakePedalPosition", 0, values) def _gas_msg(self, gas): - to_send = make_msg(0, 417) - to_send[0].RDHR = (1 << 16) if gas else 0 - return to_send + values = {"AcceleratorPedal": 1 if gas else 0} + return self.packer.make_can_msg_panda("AcceleratorPedal", 0, values) def _send_brake_msg(self, brake): - to_send = make_msg(2, 789) - brake = (-brake) & 0xfff - to_send[0].RDLR = (brake >> 8) | ((brake &0xff) << 8) - return to_send + values = {"FrictionBrakeCmd": -brake} + return self.packer_chassis.make_can_msg_panda("EBCMFrictionBrakeCmd", 2, values) def _send_gas_msg(self, gas): - to_send = make_msg(0, 715) - to_send[0].RDLR = ((gas & 0x1f) << 27) | ((gas & 0xfe0) << 11) - return to_send + values = {"GasRegenCmd": gas} + return self.packer.make_can_msg_panda("ASCMGasRegenCmd", 0, values) def _set_prev_torque(self, t): - self.safety.set_gm_desired_torque_last(t) - self.safety.set_gm_rt_torque_last(t) + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) def _torque_driver_msg(self, torque): - t = twos_comp(torque, 11) - to_send = make_msg(0, 388) - to_send[0].RDHR = (((t >> 8) & 0x7) << 16) | ((t & 0xFF) << 24) - return to_send + values = {"LKADriverAppldTrq": torque} + return self.packer.make_can_msg_panda("PSCMStatus", 0, values) def _torque_msg(self, torque): - t = twos_comp(torque, 11) - to_send = make_msg(0, 384) - to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8) - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 384) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) + values = {"LKASteeringCmd": torque} + return self.packer.make_can_msg_panda("ASCMLKASteeringCmd", 0, values) def test_resume_button(self): RESUME_BTN = 2 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(RESUME_BTN)) + self._rx(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.safety_rx_hook(self._button_msg(SET_BTN)) + self._rx(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.safety_rx_hook(self._button_msg(CANCEL_BTN)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, 0) - - def test_disengage_on_gas(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(True)) + self._rx(self._button_msg(CANCEL_BTN)) self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(False)) - - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._gas_msg(True)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_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.safety_tx_hook(self._send_brake_msg(b))) + self.assertFalse(self._tx(self._send_brake_msg(b))) else: - self.assertTrue(self.safety.safety_tx_hook(self._send_brake_msg(b))) + self.assertTrue(self._tx(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.safety_tx_hook(self._send_gas_msg(g))) + self.assertFalse(self._tx(self._send_gas_msg(g))) else: - self.assertTrue(self.safety.safety_tx_hook(self._send_gas_msg(g))) + self.assertTrue(self._tx(self._send_gas_msg(g))) def test_steer_safety_check(self): for enabled in [0, 1]: @@ -157,30 +121,27 @@ def test_steer_safety_check(self): 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.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self._tx(self._torque_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + self.assertTrue(self._tx(self._torque_msg(t))) def test_non_realtime_limit_up(self): - self.safety.set_gm_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(MAX_RATE_UP))) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(-MAX_RATE_UP))) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self._tx(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_torque_driver(0, 0) self.safety.set_controls_allowed(True) def test_against_torque_driver(self): @@ -189,12 +150,12 @@ def test_against_torque_driver(self): 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.safety.set_torque_driver(t, t) self._set_prev_torque(MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign))) + self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign))) - self.safety.set_gm_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) + self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self._tx(self._torque_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -202,56 +163,99 @@ def test_against_torque_driver(self): 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.safety_tx_hook(self._torque_msg(torque_desired))) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self._tx(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.safety_tx_hook(self._torque_msg(torque_desired + delta))) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self._tx(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.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(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.safety_tx_hook(self._torque_msg(0))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(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.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self._tx(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.safety.init_tests() self._set_prev_torque(0) - self.safety.set_gm_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._torque_msg(t))) + self.assertFalse(self._tx(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.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self._tx(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - - def test_fwd_hook(self): - # nothing allowed - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - for b in buss: - for m in msgs: - # assume len 8 - self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - + self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + + + def test_tx_hook_on_pedal_pressed(self): + for pedal in ['brake', 'gas']: + if pedal == 'brake': + # brake_pressed_prev and vehicle_moving + self._rx(self._speed_msg(100)) + self._rx(self._brake_msg(MAX_BRAKE)) + elif pedal == 'gas': + # gas_pressed_prev + self._rx(self._gas_msg(MAX_GAS)) + + self.safety.set_controls_allowed(1) + self.assertFalse(self._tx(self._send_brake_msg(MAX_BRAKE))) + self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP))) + self.assertFalse(self._tx(self._send_gas_msg(MAX_GAS))) + + # reset status + self.safety.set_controls_allowed(0) + self._tx(self._send_brake_msg(0)) + self._tx(self._torque_msg(0)) + if pedal == 'brake': + self._rx(self._speed_msg(0)) + self._rx(self._brake_msg(0)) + elif pedal == 'gas': + self._rx(self._gas_msg(0)) + + def test_tx_hook_on_pedal_pressed_on_unsafe_gas_mode(self): + for pedal in ['brake', 'gas']: + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + if pedal == 'brake': + # brake_pressed_prev and vehicle_moving + self._rx(self._speed_msg(100)) + self._rx(self._brake_msg(MAX_BRAKE)) + allow_ctrl = False + elif pedal == 'gas': + # gas_pressed_prev + self._rx(self._gas_msg(MAX_GAS)) + allow_ctrl = True + + self.safety.set_controls_allowed(1) + self.assertEqual(allow_ctrl, self._tx(self._send_brake_msg(MAX_BRAKE))) + self.assertEqual(allow_ctrl, self._tx(self._torque_msg(MAX_RATE_UP))) + self.assertEqual(allow_ctrl, self._tx(self._send_gas_msg(MAX_GAS))) + + # reset status + self.safety.set_controls_allowed(0) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + self._tx(self._send_brake_msg(0)) + self._tx(self._torque_msg(0)) + if pedal == 'brake': + self._rx(self._speed_msg(0)) + self._rx(self._brake_msg(0)) + elif pedal == 'gas': + self._rx(self._gas_msg(0)) if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index adf02fd5cc0dbb..5cd2ba05f2cdd2 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -3,285 +3,115 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, make_msg, \ + MAX_WRONG_COUNTERS, UNSAFE_MODE MAX_BRAKE = 255 -INTERCEPTOR_THRESHOLD = 328 -N_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]] -BH_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness -BG_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe - +class Btn: + CANCEL = 2 + SET = 3 + RESUME = 4 HONDA_N_HW = 0 HONDA_BG_HW = 1 HONDA_BH_HW = 2 -def honda_checksum(msg, addr, len_msg): - checksum = 0 - while addr > 0: - checksum += addr - addr >>= 4 - for i in range (0, 2*len_msg): - if i < 8: - checksum += (msg.RDLR >> (4 * i)) - else: - checksum += (msg.RDHR >> (4 * (i - 8))) - return (8 - checksum) & 0xF - -class TestHondaSafety(unittest.TestCase): +class TestHondaSafety(common.PandaSafetyTest): cnt_speed = 0 cnt_gas = 0 cnt_button = 0 + PT_BUS = 0 + @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0) - cls.safety.init_tests_honda() + def setUpClass(cls): + if cls.__name__ == "TestHondaSafety": + cls.packer = None + cls.safety = None + raise unittest.SkipTest + + # override these inherited tests. honda doesn't use pcm enable + def test_disable_control_allowed_from_cruise(self): pass + def test_enable_control_allowed_from_cruise(self): pass + def test_cruise_engaged_prev(self): pass def _speed_msg(self, speed): - bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 - to_send = make_msg(bus, 0x158) - to_send[0].RDLR = speed - to_send[0].RDHR |= (self.cnt_speed % 4) << 28 - to_send[0].RDHR |= honda_checksum(to_send[0], 0x158, 8) << 24 + values = {"XMISSION_SPEED": speed, "COUNTER": self.cnt_speed % 4} self.__class__.cnt_speed += 1 - return to_send + return self.packer.make_can_msg_panda("ENGINE_DATA", self.PT_BUS, values) - def _button_msg(self, buttons, addr): - bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 - to_send = make_msg(bus, addr) - to_send[0].RDLR = buttons << 5 - to_send[0].RDHR |= (self.cnt_button % 4) << 28 - to_send[0].RDHR |= honda_checksum(to_send[0], addr, 8) << 24 + def _button_msg(self, buttons): + values = {"CRUISE_BUTTONS": buttons, "COUNTER": self.cnt_button % 4} self.__class__.cnt_button += 1 - return to_send + return self.packer.make_can_msg_panda("SCM_BUTTONS", self.PT_BUS, values) def _brake_msg(self, brake): - bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 - to_send = make_msg(bus, 0x17C) - to_send[0].RDHR = 0x200000 if brake else 0 - to_send[0].RDHR |= (self.cnt_gas % 4) << 28 - to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24 + values = {"BRAKE_PRESSED": brake, "COUNTER": self.cnt_gas % 4} self.__class__.cnt_gas += 1 - return to_send - - def _alt_brake_msg(self, brake): - to_send = make_msg(0, 0x1BE) - to_send[0].RDLR = 0x10 if brake else 0 - return to_send + return self.packer.make_can_msg_panda("POWERTRAIN_DATA", self.PT_BUS, values) def _gas_msg(self, gas): - bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 - to_send = make_msg(bus, 0x17C) - to_send[0].RDLR = 1 if gas else 0 - to_send[0].RDHR |= (self.cnt_gas % 4) << 28 - to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24 + values = {"PEDAL_GAS": gas, "COUNTER": self.cnt_gas % 4} self.__class__.cnt_gas += 1 - return to_send + return self.packer.make_can_msg_panda("POWERTRAIN_DATA", self.PT_BUS, values) def _send_brake_msg(self, brake): - to_send = make_msg(0, 0x1FA) - to_send[0].RDLR = ((brake & 0x3) << 14) | ((brake & 0x3FF) >> 2) - return to_send - - def _send_interceptor_msg(self, gas, addr): - to_send = make_msg(0, addr, 6) - gas2 = gas * 2 - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ - ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) - return to_send + values = {} + if self.safety.get_honda_hw() == HONDA_N_HW: + values = {"COMPUTER_BRAKE": brake} + return self.packer.make_can_msg_panda("BRAKE_COMMAND", 0, values) def _send_steer_msg(self, steer): - bus = 2 if self.safety.get_honda_hw() == HONDA_BG_HW else 0 - to_send = make_msg(bus, 0xE4, 6) - to_send[0].RDLR = steer - return to_send - - def test_spam_can_buses(self): - hw_type = self.safety.get_honda_hw() - if hw_type == HONDA_N_HW: - tx_msgs = N_TX_MSGS - elif hw_type == HONDA_BH_HW: - tx_msgs = BH_TX_MSGS - elif hw_type == HONDA_BG_HW: - tx_msgs = BG_TX_MSGS - StdTest.test_spam_can_buses(self, tx_msgs) - - def test_relay_malfunction(self): - hw = self.safety.get_honda_hw() - bus = 2 if hw == HONDA_BG_HW else 0 - StdTest.test_relay_malfunction(self, 0xE4, bus=bus) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + values = {"STEER_TORQUE": steer} + return self.packer.make_can_msg_panda("STEERING_CONTROL", 0, values) def test_resume_button(self): - RESUME_BTN = 4 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x296)) + self._rx(self._button_msg(Btn.RESUME)) self.assertTrue(self.safety.get_controls_allowed()) def test_set_button(self): - SET_BTN = 3 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) + self._rx(self._button_msg(Btn.SET)) self.assertTrue(self.safety.get_controls_allowed()) def test_cancel_button(self): - CANCEL_BTN = 2 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x296)) + self._rx(self._button_msg(Btn.CANCEL)) self.assertFalse(self.safety.get_controls_allowed()) - def test_sample_speed(self): - self.assertEqual(0, self.safety.get_honda_moving()) - self.safety.safety_rx_hook(self._speed_msg(100)) - self.assertEqual(1, self.safety.get_honda_moving()) - - def test_prev_brake(self): - self.assertFalse(self.safety.get_brake_pressed_prev()) - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_brake_pressed_prev()) - def test_disengage_on_brake(self): self.safety.set_controls_allowed(1) - self.safety.safety_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.safety_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.safety_rx_hook(self._alt_brake_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, 0) - - def test_prev_gas(self): - self.safety.safety_rx_hook(self._gas_msg(False)) - self.assertFalse(self.safety.get_gas_pressed_prev()) - self.safety.safety_rx_hook(self._gas_msg(True)) - self.assertTrue(self.safety.get_gas_pressed_prev()) - - def test_prev_gas_interceptor(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) - self.assertFalse(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - self.assertTrue(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - - def test_disengage_on_gas(self): - self.safety.safety_rx_hook(self._gas_msg(0)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(1)) + self._rx(self._brake_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._gas_msg(1)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disengage_on_gas_interceptor(self): - for g in range(0, 0x1000): - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) - remain_enabled = g <= INTERCEPTOR_THRESHOLD - self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - - def test_allow_engage_with_gas_interceptor_pressed(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - - def test_brake_safety_check(self): - hw = self.safety.get_honda_hw() - if hw == HONDA_N_HW: - for fwd_brake in [False, True]: - self.safety.set_honda_fwd_brake(fwd_brake) - for brake in np.arange(0, MAX_BRAKE + 10, 1): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if fwd_brake: - send = False # block openpilot brake msg when fwd'ing stock msg - elif controls_allowed: - send = MAX_BRAKE >= brake >= 0 - else: - send = brake == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake))) - self.safety.set_honda_fwd_brake(False) - - def test_gas_interceptor_safety_check(self): - if self.safety.get_honda_hw() == HONDA_N_HW: - for gas in np.arange(0, 4000, 100): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed: - send = True - else: - send = gas == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200))) - def test_steer_safety_check(self): self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._send_steer_msg(0x0000))) - self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000))) - - def test_spam_cancel_safety_check(self): - hw = self.safety.get_honda_hw() - if hw != HONDA_N_HW: - RESUME_BTN = 4 - SET_BTN = 3 - CANCEL_BTN = 2 - BUTTON_MSG = 0x296 - self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG))) - self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) - self.assertFalse(self.safety.safety_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.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) + self.assertTrue(self._tx(self._send_steer_msg(0x0000))) + self.assertFalse(self._tx(self._send_steer_msg(0x1000))) def test_rx_hook(self): + # TODO: move this test to common # checksum checks - SET_BTN = 3 - for msg in ["btn1", "btn2", "gas", "speed"]: + for msg in ["btn", "gas", "speed"]: self.safety.set_controls_allowed(1) - if msg == "btn1": - if self.safety.get_honda_hw() == HONDA_N_HW: - to_push = self._button_msg(SET_BTN, 0x1A6) # only in Honda_NIDEC - else: - continue - if msg == "btn2": - to_push = self._button_msg(SET_BTN, 0x296) + # TODO: add this coverage back by re-running all tests with the acura dbc + # to_push = self._button_msg(Btn.SET, 0x1A6) # only in Honda_NIDEC + if msg == "btn": + to_push = self._button_msg(Btn.SET) if msg == "gas": to_push = self._gas_msg(0) if msg == "speed": to_push = self._speed_msg(0) - self.assertTrue(self.safety.safety_rx_hook(to_push)) - to_push[0].RDHR = 0 # invalidate checksum - self.assertFalse(self.safety.safety_rx_hook(to_push)) - self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self._rx(to_push)) + if msg != "btn": + to_push[0].RDHR = 0 # invalidate checksum + self.assertFalse(self._rx(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) # counter # reset wrong_counters to zero by sending valid messages @@ -291,88 +121,194 @@ def test_rx_hook(self): self.__class__.cnt_button += 1 if i < MAX_WRONG_COUNTERS: self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) - self.safety.safety_rx_hook(self._speed_msg(0)) - self.safety.safety_rx_hook(self._gas_msg(0)) + self._rx(self._button_msg(Btn.SET)) + self._rx(self._speed_msg(0)) + self._rx(self._gas_msg(0)) else: - self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))) - self.assertFalse(self.safety.safety_rx_hook(self._speed_msg(0))) - self.assertFalse(self.safety.safety_rx_hook(self._gas_msg(0))) + self.assertFalse(self._rx(self._button_msg(Btn.SET))) + self.assertFalse(self._rx(self._speed_msg(0))) + self.assertFalse(self._rx(self._gas_msg(0))) self.assertFalse(self.safety.get_controls_allowed()) # restore counters for future tests with a couple of good messages for i in range(2): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) - self.safety.safety_rx_hook(self._speed_msg(0)) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) + self._rx(self._button_msg(Btn.SET)) + self._rx(self._speed_msg(0)) + self._rx(self._gas_msg(0)) + self._rx(self._button_msg(Btn.SET)) self.assertTrue(self.safety.get_controls_allowed()) + def test_tx_hook_on_pedal_pressed(self): + for mode in [UNSAFE_MODE.DEFAULT, UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS]: + for pedal in ['brake', 'gas']: + self.safety.set_unsafe_mode(mode) + allow_ctrl = False + if pedal == 'brake': + # brake_pressed_prev and vehicle_moving + self._rx(self._speed_msg(100)) + self._rx(self._brake_msg(1)) + elif pedal == 'gas': + # gas_pressed_prev + self._rx(self._gas_msg(1)) + allow_ctrl = mode == UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS + + self.safety.set_controls_allowed(1) + hw = self.safety.get_honda_hw() + if hw == HONDA_N_HW: + self.safety.set_honda_fwd_brake(False) + self.assertEqual(allow_ctrl, self._tx(self._send_brake_msg(MAX_BRAKE))) + self.assertEqual(allow_ctrl, self._tx(self._send_steer_msg(0x1000))) + + # reset status + self.safety.set_controls_allowed(0) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + self._tx(self._send_brake_msg(0)) + self._tx(self._send_steer_msg(0)) + if pedal == 'brake': + self._rx(self._speed_msg(0)) + self._rx(self._brake_msg(0)) + elif pedal == 'gas': + self._rx(self._gas_msg(0)) + + +class TestHondaNidecSafety(TestHondaSafety, common.InterceptorSafetyTest): + + TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]] + STANDSTILL_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = 0xE4 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x194, 0x33D, 0x30C]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + INTERCEPTOR_THRESHOLD = 344 + + def setUp(self): + self.packer = CANPackerPanda("honda_civic_touring_2016_can_generated") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0) + self.safety.init_tests_honda() + + # Honda gas gains are the different + def _interceptor_msg(self, gas, addr): + to_send = make_msg(0, addr, 6) + gas2 = gas * 2 + to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ + ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) + return to_send def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - fwd_brake = [False, True] - - for f in fwd_brake: - self.safety.set_honda_fwd_brake(f) - blocked_msgs = [0xE4, 0x194, 0x33D] - blocked_msgs += [0x30C] - if not f: - blocked_msgs += [0x1FA] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + # normal operation, not forwarding AEB + self.FWD_BLACKLISTED_ADDRS[2].append(0x1FA) + self.safety.set_honda_fwd_brake(False) + super().test_fwd_hook() + + # TODO: test latching until AEB event is over? + # forwarding AEB brake signal + self.FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x194, 0x33D, 0x30C]} + self.safety.set_honda_fwd_brake(True) + super().test_fwd_hook() + def test_brake_safety_check(self): + for fwd_brake in [False, True]: + self.safety.set_honda_fwd_brake(fwd_brake) + for brake in np.arange(0, MAX_BRAKE + 10, 1): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + if fwd_brake: + send = False # block openpilot brake msg when fwd'ing stock msg + elif controls_allowed: + send = MAX_BRAKE >= brake >= 0 + else: + send = brake == 0 + self.assertEqual(send, self._tx(self._send_brake_msg(brake))) self.safety.set_honda_fwd_brake(False) + def test_tx_hook_on_interceptor_pressed(self): + for mode in [UNSAFE_MODE.DEFAULT, UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS]: + self.safety.set_unsafe_mode(mode) + # gas_interceptor_prev > INTERCEPTOR_THRESHOLD + self._rx(self._interceptor_msg(self.INTERCEPTOR_THRESHOLD+1, 0x201)) + self._rx(self._interceptor_msg(self.INTERCEPTOR_THRESHOLD+1, 0x201)) + allow_ctrl = mode == UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS -class TestHondaBoschGiraffeSafety(TestHondaSafety): - @classmethod - def setUp(cls): - TestHondaSafety.setUp() - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0) - cls.safety.init_tests_honda() + self.safety.set_controls_allowed(1) + self.safety.set_honda_fwd_brake(False) + self.assertEqual(allow_ctrl, self._tx(self._send_brake_msg(MAX_BRAKE))) + self.assertEqual(allow_ctrl, self._tx(self._interceptor_msg(self.INTERCEPTOR_THRESHOLD, 0x200))) + self.assertEqual(allow_ctrl, self._tx(self._send_steer_msg(0x1000))) + + # reset status + self.safety.set_controls_allowed(0) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + self._tx(self._send_brake_msg(0)) + self._tx(self._send_steer_msg(0)) + self._tx(self._interceptor_msg(0, 0x200)) + self.safety.set_gas_interceptor_detected(False) + + +class TestHondaBoschHarnessSafety(TestHondaSafety): + TX_MSGS = [[0xE4, 0], [0xE5, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness + STANDSTILL_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = 0xE4 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0xE5, 0x33D]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + PT_BUS = 1 + + def setUp(self): + self.packer = CANPackerPanda("honda_accord_s2t_2018_can_generated") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_HARNESS, 0) + self.safety.init_tests_honda() + + def _alt_brake_msg(self, brake): + to_send = make_msg(0, 0x1BE) + to_send[0].RDLR = 0x10 if brake else 0 + return to_send + + def test_spam_cancel_safety_check(self): + self.safety.set_controls_allowed(0) + self.assertTrue(self._tx(self._button_msg(Btn.CANCEL))) + self.assertFalse(self._tx(self._button_msg(Btn.RESUME))) + self.assertFalse(self._tx(self._button_msg(Btn.SET))) + # do not block resume if we are engaged already + self.safety.set_controls_allowed(1) + self.assertTrue(self._tx(self._button_msg(Btn.RESUME))) + + def test_alt_disengage_on_brake(self): + self.safety.set_honda_alt_brake_msg(1) + self.safety.set_controls_allowed(1) + self._rx(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._rx(self._alt_brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + + +class TestHondaBoschGiraffeSafety(TestHondaBoschHarnessSafety): + TX_MSGS = [[0xE4, 2], [0xE5, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe + STANDSTILL_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = 0xE4 + RELAY_MALFUNCTION_BUS = 2 + FWD_BLACKLISTED_ADDRS = {1: [0xE4, 0xE5, 0x33D]} + FWD_BUS_LOOKUP = {1: 2, 2: 1} + + PT_BUS = 0 + + def setUp(self): + super().setUp() + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0) + self.safety.init_tests_honda() + + def _send_steer_msg(self, steer): + values = {"STEER_TORQUE": steer} + return self.packer.make_can_msg_panda("STEERING_CONTROL", 2, values) - def test_fwd_hook(self): - buss = range(0x0, 0x3) - msgs = range(0x1, 0x800) - hw = self.safety.get_honda_hw() - bus_rdr_cam = 2 if hw == HONDA_BH_HW else 1 - bus_rdr_car = 0 if hw == HONDA_BH_HW else 2 - bus_pt = 1 if hw == HONDA_BH_HW else 0 - - blocked_msgs = [0xE4, 0x33D] - for b in buss: - for m in msgs: - if b == bus_pt: - fwd_bus = -1 - elif b == bus_rdr_cam: - fwd_bus = -1 if m in blocked_msgs else bus_rdr_car - elif b == bus_rdr_car: - fwd_bus = bus_rdr_cam - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - - -class TestHondaBoschHarnessSafety(TestHondaBoschGiraffeSafety): - @classmethod - def setUp(cls): - TestHondaBoschGiraffeSafety.setUp() - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_HARNESS, 0) - cls.safety.init_tests_honda() if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 2d022759e102f2..b0ba85eea4233e 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -3,7 +3,8 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 @@ -12,75 +13,57 @@ MAX_RT_DELTA = 112 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 50; -DRIVER_TORQUE_FACTOR = 2; +DRIVER_TORQUE_ALLOWANCE = 50 +DRIVER_TORQUE_FACTOR = 2 -SPEED_THRESHOLD = 30 # ~1kph -TX_MSGS = [[832, 0], [1265, 0]] +class TestHyundaiSafety(common.PandaSafetyTest): + TX_MSGS = [[832, 0], [1265, 0], [1157, 0]] + STANDSTILL_THRESHOLD = 30 # ~1kph + RELAY_MALFUNCTION_ADDR = 832 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [832, 1157]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} -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 TestHyundaiSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0) - cls.safety.init_tests_hyundai() + def setUp(self): + self.packer = CANPackerPanda("hyundai_kia_generic") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0) + self.safety.init_tests() def _button_msg(self, buttons): - to_send = make_msg(0, 1265) - to_send[0].RDLR = buttons - return to_send + values = {"CF_Clu_CruiseSwState": buttons} + return self.packer.make_can_msg_panda("CLU11", 0, values) def _gas_msg(self, val): - to_send = make_msg(0, 608) - to_send[0].RDHR = (val & 0x3) << 30; - return to_send + values = {"CF_Ems_AclAct": val} + return self.packer.make_can_msg_panda("EMS16", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 916) - to_send[0].RDHR = brake << 23; - return to_send + values = {"DriverBraking": brake} + return self.packer.make_can_msg_panda("TCS13", 0, values) def _speed_msg(self, speed): - to_send = make_msg(0, 902) - to_send[0].RDLR = speed & 0x3FFF; - to_send[0].RDHR = (speed & 0x3FFF) << 16; - return to_send + # panda safety doesn't scale, so undo the scaling + values = {"WHL_SPD_%s"%s: speed*0.03125 for s in ["FL", "FR", "RL", "RR"]} + return self.packer.make_can_msg_panda("WHL_SPD11", 0, values) + + def _pcm_status_msg(self, enabled): + values = {"ACCMode": enabled} + return self.packer.make_can_msg_panda("SCC12", 0, values) def _set_prev_torque(self, t): - self.safety.set_hyundai_desired_torque_last(t) - self.safety.set_hyundai_rt_torque_last(t) + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) + # TODO: this is unused def _torque_driver_msg(self, torque): - to_send = make_msg(0, 897) - to_send[0].RDLR = (torque + 2048) << 11 - return to_send + values = {"CR_Mdps_StrColTq": torque} + return self.packer.make_can_msg_panda("MDPS12", 0, values) def _torque_msg(self, torque): - to_send = make_msg(0, 832) - to_send[0].RDLR = (torque + 1024) << 16 - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 832) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) + values = {"CR_Lkas_StrToqReq": torque} + return self.packer.make_can_msg_panda("LKAS11", 0, values) def test_steer_safety_check(self): for enabled in [0, 1]: @@ -88,53 +71,27 @@ def test_steer_safety_check(self): 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.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self._tx(self._torque_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) - - def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 1057) - to_push[0].RDLR = 1 << 13 - self.safety.safety_rx_hook(to_push) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 1057) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_disengage_on_gas(self): - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) + self.assertTrue(self._tx(self._torque_msg(t))) def test_non_realtime_limit_up(self): - self.safety.set_hyundai_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(MAX_RATE_UP))) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(-MAX_RATE_UP))) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self._tx(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): - self.safety.set_hyundai_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) self.safety.set_controls_allowed(True) def test_against_torque_driver(self): @@ -143,12 +100,12 @@ def test_against_torque_driver(self): for sign in [-1, 1]: for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): t *= -sign - self.safety.set_hyundai_torque_driver(t, t) + self.safety.set_torque_driver(t, t) self._set_prev_torque(MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign))) + self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign))) - self.safety.set_hyundai_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) + self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self._tx(self._torque_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -156,44 +113,44 @@ def test_against_torque_driver(self): torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign delta = 1 * sign self._set_prev_torque(torque_desired) - self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self._tx(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) - self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self._tx(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(self._torque_msg(0))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self._tx(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_hyundai() + self.safety.init_tests() self._set_prev_torque(0) - self.safety.set_hyundai_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._torque_msg(t))) + self.assertFalse(self._tx(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.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self._tx(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) def test_spam_cancel_safety_check(self): @@ -201,30 +158,12 @@ def test_spam_cancel_safety_check(self): SET_BTN = 2 CANCEL_BTN = 4 self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN))) - self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN))) - self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN))) + self.assertTrue(self._tx(self._button_msg(CANCEL_BTN))) + self.assertFalse(self._tx(self._button_msg(RESUME_BTN))) + self.assertFalse(self._tx(self._button_msg(SET_BTN))) # do not block resume if we are engaged already self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN))) - - def test_fwd_hook(self): - - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - blocked_msgs = [832] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + self.assertTrue(self._tx(self._button_msg(RESUME_BTN))) if __name__ == "__main__": diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index ab826fffbd2a03..853899ae789ff0 100644 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -3,43 +3,37 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda -ANGLE_MAX_BP = [1.3, 10., 30.] -ANGLE_MAX_V = [540., 120., 23.] ANGLE_DELTA_BP = [0., 5., 15.] ANGLE_DELTA_V = [5., .8, .15] # windup limit ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit -TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]] - -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 + return 1 if a > 0 else -1 -class TestNissanSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) - cls.safety.init_tests_nissan() +class TestNissanSafety(common.PandaSafetyTest): - def _angle_meas_msg(self, angle): - to_send = make_msg(0, 0x2) - angle = int(angle * -10) - t = twos_comp(angle, 16) - to_send[0].RDLR = t & 0xFFFF + TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]] + STANDSTILL_THRESHOLD = 0 + GAS_PRESSED_THRESHOLD = 1 + RELAY_MALFUNCTION_ADDR = 0x169 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2b1, 0x4cc]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("nissan_x_trail_2017") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) + self.safety.init_tests_nissan() - return to_send + def _angle_meas_msg(self, angle): + values = {"STEER_ANGLE": angle} + return self.packer.make_can_msg_panda("STEER_ANGLE_SENSOR", 0, values) def _set_prev_angle(self, t): t = int(t * -100) @@ -47,152 +41,111 @@ def _set_prev_angle(self, t): def _angle_meas_msg_array(self, angle): for i in range(6): - self.safety.safety_rx_hook(self._angle_meas_msg(angle)) - - def _lkas_state_msg(self, state): - to_send = make_msg(0, 0x1b6) - to_send[0].RDHR = (state & 0x1) << 6 + self._rx(self._angle_meas_msg(angle)) - return to_send + def _pcm_status_msg(self, enabled): + values = {"CRUISE_ENABLED": enabled} + return self.packer.make_can_msg_panda("CRUISE_STATE", 2, values) def _lkas_control_msg(self, angle, state): - to_send = make_msg(0, 0x169) - angle = int((angle - 1310) * -100) - to_send[0].RDLR = ((angle & 0x3FC00) >> 10) | ((angle & 0x3FC) << 6) | ((angle & 0x3) << 16) - to_send[0].RDHR = ((state & 0x1) << 20) - - return to_send + values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": state} + return self.packer.make_can_msg_panda("LKAS", 0, values) def _speed_msg(self, speed): - to_send = make_msg(0, 0x29a) - speed = int(speed / 0.00555 * 3.6) - to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8) - - return to_send + # TODO: why the 3.6? m/s to kph? not in dbc + values = {"WHEEL_SPEED_%s"%s: speed*3.6 for s in ["RR", "RL"]} + return self.packer.make_can_msg_panda("WHEEL_SPEEDS_REAR", 0, values) def _brake_msg(self, brake): - to_send = make_msg(1, 0x454) - to_send[0].RDLR = ((brake & 0x1) << 23) - - return to_send - - def _send_gas_cmd(self, gas): - to_send = make_msg(0, 0x15c) - to_send[0].RDHR = ((gas & 0x3fc) << 6) | ((gas & 0x3) << 22) + values = {"USER_BRAKE_PRESSED": brake} + return self.packer.make_can_msg_panda("DOORS_LIGHTS", 1, values) - return to_send + def _gas_msg(self, gas): + values = {"GAS_PEDAL": gas} + return self.packer.make_can_msg_panda("GAS_PEDAL", 0, values) - def _acc_button_cmd(self, buttons): - to_send = make_msg(2, 0x20b) - to_send[0].RDLR = (buttons << 8) - - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) + def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0): + no_button = not any([cancel, propilot, flw_dist, _set, res]) + values = {"CANCEL_BUTTON": cancel, "PROPILOT_BUTTON": propilot, \ + "FOLLOW_DISTANCE_BUTTON": flw_dist, "SET_BUTTON": _set, \ + "RES_BUTTON": res, "NO_BUTTON_PRESSED": no_button} + return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 2, values) def test_angle_cmd_when_enabled(self): - # when controls are allowed, angle cmd rate limit is enforced - # test 1: no limitations if we stay within limits - speeds = [0., 1., 5., 10., 15., 100.] + speeds = [0., 1., 5., 10., 15., 50.] angles = [-300, -100, -10, 0, 10, 100, 300] for a in angles: for s in speeds: max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - angle_lim = np.interp(s, ANGLE_MAX_BP, ANGLE_MAX_V) # first test against false positives self._angle_meas_msg_array(a) - self.safety.safety_rx_hook(self._speed_msg(s)) + self._rx(self._speed_msg(s)) - self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self._set_prev_angle(a) self.safety.set_controls_allowed(1) - self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( - np.clip(a + sign(a) * max_delta_up, -angle_lim, angle_lim), 1))) + # Stay within limits + # Up + self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook( - self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + + # Don't change + self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( - np.clip(a - sign(a) * max_delta_down, -angle_lim, angle_lim), 1))) + + # Down + self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) self.assertTrue(self.safety.get_controls_allowed()) - # now inject too high rates - self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * - (max_delta_up + 1), 1))) + # Inject too high rates + # Up + self.assertEqual(False, self._tx(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1), 1))) self.assertFalse(self.safety.get_controls_allowed()) + + # Don't change self.safety.set_controls_allowed(1) - self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self._set_prev_angle(a) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook( - self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * - (max_delta_down + 1), 1))) + + # Down + self.assertEqual(False, self._tx(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1), 1))) self.assertFalse(self.safety.get_controls_allowed()) # Check desired steer should be the same as steer angle when controls are off self.safety.set_controls_allowed(0) - self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 0))) + self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0))) def test_angle_cmd_when_disabled(self): self.safety.set_controls_allowed(0) self._set_prev_angle(0) - self.assertFalse(self.safety.safety_tx_hook(self._lkas_control_msg(0, 1))) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, 0) - - def test_gas_rising_edge(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_gas_cmd(100)) + self.assertFalse(self._tx(self._lkas_control_msg(0, 1))) self.assertFalse(self.safety.get_controls_allowed()) def test_acc_buttons(self): self.safety.set_controls_allowed(1) - self.safety.safety_tx_hook(self._acc_button_cmd(0x2)) # Cancel button + self._tx(self._acc_button_cmd(cancel=1)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_tx_hook(self._acc_button_cmd(0x1)) # ProPilot button + self._tx(self._acc_button_cmd(propilot=1)) self.assertFalse(self.safety.get_controls_allowed()) self.safety.set_controls_allowed(1) - self.safety.safety_tx_hook(self._acc_button_cmd(0x4)) # Follow Distance button + self._tx(self._acc_button_cmd(flw_dist=1)) self.assertFalse(self.safety.get_controls_allowed()) self.safety.set_controls_allowed(1) - self.safety.safety_tx_hook(self._acc_button_cmd(0x8)) # Set button + self._tx(self._acc_button_cmd(_set=1)) self.assertFalse(self.safety.get_controls_allowed()) self.safety.set_controls_allowed(1) - self.safety.safety_tx_hook(self._acc_button_cmd(0x10)) # Res button + self._tx(self._acc_button_cmd(res=1)) self.assertFalse(self.safety.get_controls_allowed()) self.safety.set_controls_allowed(1) - self.safety.safety_tx_hook(self._acc_button_cmd(0x20)) # No button pressed + self._tx(self._acc_button_cmd()) self.assertFalse(self.safety.get_controls_allowed()) - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x169) - - def test_fwd_hook(self): - - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - blocked_msgs = [0x169,0x2b1,0x4cc] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index d68090d19eb4d9..08bb25bc1505ef 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -3,7 +3,8 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda MAX_RATE_UP = 50 MAX_RATE_DOWN = 70 @@ -12,181 +13,92 @@ MAX_RT_DELTA = 940 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 60; -DRIVER_TORQUE_FACTOR = 10; +DRIVER_TORQUE_ALLOWANCE = 60 +DRIVER_TORQUE_FACTOR = 10 -SPEED_THRESHOLD = 20 # 1kph (see dbc file) -TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] -TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]] - -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 - -def subaru_checksum(msg, addr, len_msg): - checksum = addr + (addr >> 8) - for i in range(len_msg): - if i < 4: - checksum += (msg.RDLR >> (8 * i)) - else: - checksum += (msg.RDHR >> (8 * (i - 4))) - return checksum & 0xff - - -class TestSubaruSafety(unittest.TestCase): +class TestSubaruSafety(common.PandaSafetyTest): cnt_gas = 0 cnt_torque_driver = 0 cnt_cruise = 0 cnt_speed = 0 cnt_brake = 0 - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0) - cls.safety.init_tests_subaru() + TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] + STANDSTILL_THRESHOLD = 20 # 1kph (see dbc file) + RELAY_MALFUNCTION_ADDR = 0x122 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [290, 545, 802]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("subaru_global_2017") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0) + self.safety.init_tests() def _set_prev_torque(self, t): - self.safety.set_subaru_desired_torque_last(t) - self.safety.set_subaru_rt_torque_last(t) + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) def _torque_driver_msg(self, torque): - t = twos_comp(torque, 11) - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x119) - to_send[0].RDLR = ((t & 0x7FF) << 16) - to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8) - self.__class__.cnt_torque_driver += 1 - else: - to_send = make_msg(0, 0x371) - to_send[0].RDLR = (t & 0x7) << 29 - to_send[0].RDHR = (t >> 3) & 0xFF - return to_send + values = {"Steer_Torque_Sensor": torque, "counter": self.cnt_torque_driver % 4} + self.__class__.cnt_torque_driver += 1 + return self.packer.make_can_msg_panda("Steering_Torque", 0, values) def _speed_msg(self, speed): - speed &= 0x1FFF - to_send = make_msg(0, 0x13a) - to_send[0].RDLR = speed << 12 - to_send[0].RDHR = speed << 6 - to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8) + # subaru safety doesn't use the scaled value, so undo the scaling + values = {s: speed*0.057 for s in ["FR", "FL", "RR", "RL"]} + values["Counter"] = self.cnt_speed % 4 self.__class__.cnt_speed += 1 - return to_send + return self.packer.make_can_msg_panda("Wheel_Speeds", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 0x139) - to_send[0].RDHR = (brake << 4) & 0xFFF - to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8) + values = {"Brake_Pedal": brake, "Counter": self.cnt_brake % 4} self.__class__.cnt_brake += 1 - return to_send + return self.packer.make_can_msg_panda("Brake_Pedal", 0, values) def _torque_msg(self, torque): - t = twos_comp(torque, 13) - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x122) - to_send[0].RDLR = (t << 16) - else: - to_send = make_msg(0, 0x164) - to_send[0].RDLR = (t << 8) - return to_send + values = {"LKAS_Output": torque} + return self.packer.make_can_msg_panda("ES_LKAS", 0, values) def _gas_msg(self, gas): - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x40) - to_send[0].RDHR = gas & 0xFF - to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8) - self.__class__.cnt_gas += 1 - else: - to_send = make_msg(0, 0x140) - to_send[0].RDLR = gas & 0xFF - return to_send - - def _cruise_msg(self, cruise): - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x240) - to_send[0].RDHR = cruise << 9 - to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8) - self.__class__.cnt_cruise += 1 - else: - to_send = make_msg(0, 0x144) - to_send[0].RDHR = cruise << 17 - return to_send + values = {"Throttle_Pedal": gas, "Counter": self.cnt_gas % 4} + self.__class__.cnt_gas += 1 + return self.packer.make_can_msg_panda("Throttle", 0, values) + + def _pcm_status_msg(self, cruise): + values = {"Cruise_Activated": cruise, "Counter": self.cnt_cruise % 4} + self.__class__.cnt_cruise += 1 + return self.packer.make_can_msg_panda("CruiseControl", 0, values) def _set_torque_driver(self, min_t, max_t): for i in range(0, 5): - self.safety.safety_rx_hook(self._torque_driver_msg(min_t)) - self.safety.safety_rx_hook(self._torque_driver_msg(max_t)) - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS if self.safety.get_subaru_global() else TX_L_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x122 if self.safety.get_subaru_global() else 0x164) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_enable_control_allowed_from_cruise(self): - self.safety.safety_rx_hook(self._cruise_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._cruise_msg(False)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_disengage_on_gas(self): - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - if (self.safety.get_subaru_global()): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) + self._rx(self._torque_driver_msg(min_t)) + self._rx(self._torque_driver_msg(max_t)) def test_steer_safety_check(self): for enabled in [0, 1]: for t in range(-3000, 3000): 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.safety_tx_hook(self._torque_msg(t))) - else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + block = abs(t) > MAX_STEER or (not enabled and abs(t) > 0) + self.assertEqual(not block, self._tx(self._torque_msg(t))) def test_non_realtime_limit_up(self): self._set_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(MAX_RATE_UP))) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(-MAX_RATE_UP))) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self._tx(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self._set_torque_driver(0, 0) @@ -200,10 +112,10 @@ def test_against_torque_driver(self): t *= -sign self._set_torque_driver(t, t) self._set_prev_torque(MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign))) + self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign))) self._set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) + self.assertFalse(self._tx(self._torque_msg(-MAX_STEER))) # arbitrary high driver torque to ensure max steer torque is allowed max_driver_torque = int(MAX_STEER / DRIVER_TORQUE_FACTOR + DRIVER_TORQUE_ALLOWANCE + 1) @@ -215,67 +127,84 @@ def test_against_torque_driver(self): delta = 1 * sign self._set_prev_torque(torque_desired) self._set_torque_driver(-driver_torque, -driver_torque) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) + self.assertTrue(self._tx(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) self._set_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) + self.assertFalse(self._tx(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) + self.assertTrue(self._tx(self._torque_msg(0))) self._set_prev_torque(MAX_STEER * sign) self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + self.assertFalse(self._tx(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_subaru() + self.safety.init_tests() self._set_prev_torque(0) self._set_torque_driver(0, 0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._torque_msg(t))) + self.assertFalse(self._tx(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.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self._tx(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - - def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - blocked_msgs = [290, 545, 802] if self.safety.get_subaru_global() else [356, 545, 802] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + class TestSubaruLegacySafety(TestSubaruSafety): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0) - cls.safety.init_tests_subaru() + + TX_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]] + RELAY_MALFUNCTION_ADDR = 0x164 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [356, 545, 802]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("subaru_outback_2015_eyesight") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0) + self.safety.init_tests() + + # subaru legacy safety doesn't have brake checks + def test_prev_brake(self): pass + def test_not_allow_brake_when_moving(self): pass + def test_allow_brake_at_zero_speed(self): pass + # doesn't have speed checks either + def test_sample_speed(self): pass + + def _torque_driver_msg(self, torque): + # TODO: figure out if this scaling factor from the DBC is right. + # if it is, remove the scaling from here and put it in the safety code + values = {"Steer_Torque_Sensor": torque*8} + return self.packer.make_can_msg_panda("Steering_Torque", 0, values) + + def _torque_msg(self, torque): + values = {"LKAS_Command": torque} + return self.packer.make_can_msg_panda("ES_LKAS", 0, values) + + def _gas_msg(self, gas): + values = {"Throttle_Pedal": gas} + return self.packer.make_can_msg_panda("Throttle", 0, values) + + def _pcm_status_msg(self, cruise): + values = {"Cruise_Activated": cruise} + return self.packer.make_can_msg_panda("CruiseControl", 0, values) + if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py old mode 100644 new mode 100755 index bc0795595e700a..8aee8e7f364e1b --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -3,294 +3,93 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg - -MAX_RATE_UP = 10 -MAX_RATE_DOWN = 25 -MAX_TORQUE = 1500 - -MAX_ACCEL = 1500 -MIN_ACCEL = -3000 - -MAX_RT_DELTA = 375 -RT_INTERVAL = 250000 - -STANDSTILL_THRESHOLD = 100 # 1kph - -MAX_TORQUE_ERROR = 350 -INTERCEPTOR_THRESHOLD = 475 - -TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 - [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 - [0x2E4, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], # LKAS + ACC - [0x200, 0], [0x750, 0]]; # interceptor + blindspot monitor - - -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 - -def toyota_checksum(msg, addr, len_msg): - checksum = (len_msg + addr + (addr >> 8)) - for i in range(len_msg): - if i < 4: - checksum += (msg.RDLR >> (8 * i)) - else: - checksum += (msg.RDHR >> (8 * (i - 4))) - return checksum & 0xff - - -class TestToyotaSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 100) - cls.safety.init_tests_toyota() - - def _set_prev_torque(self, t): - self.safety.set_toyota_desired_torque_last(t) - self.safety.set_toyota_rt_torque_last(t) - self.safety.set_toyota_torque_meas(t, t) +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, make_msg, UNSAFE_MODE + +MAX_ACCEL = 1.5 +MIN_ACCEL = -3.0 + +ISO_MAX_ACCEL = 2.0 +ISO_MIN_ACCEL = -3.5 + +class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, \ + common.TorqueSteeringSafetyTest): + + TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 + [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 + [0x2E4, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], # LKAS + ACC + [0x200, 0], [0x750, 0]] # interceptor + blindspot monitor + STANDSTILL_THRESHOLD = 1 # 1kph + RELAY_MALFUNCTION_ADDR = 0x2E4 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x343]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + INTERCEPTOR_THRESHOLD = 845 + + MAX_RATE_UP = 10 + MAX_RATE_DOWN = 25 + MAX_TORQUE = 1500 + MAX_RT_DELTA = 375 + RT_INTERVAL = 250000 + MAX_TORQUE_ERROR = 350 + TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conversative for rounding + + def setUp(self): + self.packer = CANPackerPanda("toyota_prius_2017_pt_generated") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 66) + self.safety.init_tests() def _torque_meas_msg(self, torque): - t = twos_comp(torque, 16) - to_send = make_msg(0, 0x260) - to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16) - to_send[0].RDHR |= toyota_checksum(to_send[0], 0x260, 8) << 24 - return to_send + values = {"STEER_TORQUE_EPS": torque} + return self.packer.make_can_msg_panda("STEER_TORQUE_SENSOR", 0, values) def _torque_msg(self, torque): - t = twos_comp(torque, 16) - to_send = make_msg(0, 0x2E4) - to_send[0].RDLR = t | ((t & 0xFF) << 16) - return to_send + values = {"STEER_TORQUE_CMD": torque} + return self.packer.make_can_msg_panda("STEERING_LKA", 0, values) def _accel_msg(self, accel): - to_send = make_msg(0, 0x343) - a = twos_comp(accel, 16) - to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) - return to_send + values = {"ACCEL_CMD": accel} + return self.packer.make_can_msg_panda("ACC_CONTROL", 0, values) def _speed_msg(self, s): - offset = (0x6f << 8) + 0x1a # there is a 0x1a6f offset in the signal - to_send = make_msg(0, 0xaa) - to_send[0].RDLR = ((s & 0xFF) << 8 | (s >> 8)) + offset - to_send[0].RDLR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16) - to_send[0].RDHR = ((s & 0xFF) << 8 | (s >> 8)) + offset - to_send[0].RDHR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16) - return to_send + values = {("WHEEL_SPEED_%s"%n): s for n in ["FR", "FL", "RR", "RL"]} + return self.packer.make_can_msg_panda("WHEEL_SPEEDS", 0, values) - def _brake_msg(self, brake): - to_send = make_msg(0, 0x226) - to_send[0].RDHR = brake << 5 - to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24 - return to_send + def _brake_msg(self, pressed): + values = {"BRAKE_PRESSED": pressed} + return self.packer.make_can_msg_panda("BRAKE_MODULE", 0, values) - def _send_gas_msg(self, gas): - to_send = make_msg(0, 0x2C1) - to_send[0].RDHR = (gas & 0xFF) << 16 - return to_send + def _gas_msg(self, pressed): + cruise_active = self.safety.get_controls_allowed() + values = {"GAS_RELEASED": not pressed, "CRUISE_ACTIVE": cruise_active} + return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values) - def _send_interceptor_msg(self, gas, addr): - gas2 = gas * 2 + def _pcm_status_msg(self, cruise_on): + values = {"CRUISE_ACTIVE": cruise_on} + return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values) + + # Toyota gas gains are the same + def _interceptor_msg(self, gas, addr): to_send = make_msg(0, addr, 6) to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ - ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) - return to_send - - def _pcm_cruise_msg(self, cruise_on): - to_send = make_msg(0, 0x1D2) - to_send[0].RDLR = cruise_on << 5 - to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x1D2, 8) << 24) + ((gas & 0xff) << 24) | ((gas & 0xff00) << 8) return to_send - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x2E4) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) - - def test_enable_control_allowed_from_cruise(self): - self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) - self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._pcm_cruise_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_prev_gas(self): - for g in range(0, 256): - self.safety.safety_rx_hook(self._send_gas_msg(g)) - self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) - - def test_prev_gas_interceptor(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) - self.assertFalse(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - self.assertTrue(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - - def test_disengage_on_gas(self): - self.safety.safety_rx_hook(self._send_gas_msg(0)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disengage_on_gas_interceptor(self): - for g in range(0, 0x1000): - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) - remain_enabled = g <= INTERCEPTOR_THRESHOLD - self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, STANDSTILL_THRESHOLD) - - def test_allow_engage_with_gas_interceptor_pressed(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - def test_accel_actuation_limits(self): - for accel in np.arange(MIN_ACCEL - 1000, MAX_ACCEL + 1000, 100): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed: - send = MIN_ACCEL <= accel <= MAX_ACCEL - else: - send = accel == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._accel_msg(accel))) + limits = ((MIN_ACCEL, MAX_ACCEL, UNSAFE_MODE.DEFAULT), + (ISO_MIN_ACCEL, ISO_MAX_ACCEL, UNSAFE_MODE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX)) - 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): + for min_accel, max_accel, unsafe_mode in limits: + for accel in np.arange(min_accel - 1, max_accel + 1, 0.1): + for controls_allowed in [True, False]: self.safety.set_controls_allowed(controls_allowed) - self.safety.set_toyota_rt_torque_last(torque) - self.safety.set_toyota_torque_meas(torque, torque) - self.safety.set_toyota_desired_torque_last(torque - MAX_RATE_UP) - + self.safety.set_unsafe_mode(unsafe_mode) if controls_allowed: - send = (-MAX_TORQUE <= torque <= MAX_TORQUE) + should_tx = int(min_accel*1000) <= int(accel*1000) <= int(max_accel*1000) else: - send = torque == 0 - - self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque))) - - def test_non_realtime_limit_up(self): - self.safety.set_controls_allowed(True) - - self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) - - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) - - def test_non_realtime_limit_down(self): - self.safety.set_controls_allowed(True) - - self.safety.set_toyota_rt_torque_last(1000) - self.safety.set_toyota_torque_meas(500, 500) - self.safety.set_toyota_desired_torque_last(1000) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN))) - - self.safety.set_toyota_rt_torque_last(1000) - self.safety.set_toyota_torque_meas(500, 500) - self.safety.set_toyota_desired_torque_last(1000) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) - - def test_exceed_torque_sensor(self): - self.safety.set_controls_allowed(True) - - for sign in [-1, 1]: - self._set_prev_torque(0) - for t in np.arange(0, MAX_TORQUE_ERROR + 10, 10): - t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) - - def test_realtime_limit_up(self): - self.safety.set_controls_allowed(True) - - for sign in [-1, 1]: - self.safety.init_tests_toyota() - self._set_prev_torque(0) - for t in np.arange(0, 380, 10): - t *= sign - self.safety.set_toyota_torque_meas(t, t) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * 380))) - - self._set_prev_torque(0) - for t in np.arange(0, 370, 10): - t *= sign - self.safety.set_toyota_torque_meas(t, t) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - # Increase timer to update rt_torque_last - self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * 370))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * 380))) - - def test_torque_measurements(self): - self.safety.safety_rx_hook(self._torque_meas_msg(50)) - self.safety.safety_rx_hook(self._torque_meas_msg(-50)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - - self.assertEqual(-51, self.safety.get_toyota_torque_meas_min()) - self.assertEqual(51, self.safety.get_toyota_torque_meas_max()) - - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(1, self.safety.get_toyota_torque_meas_max()) - self.assertEqual(-51, self.safety.get_toyota_torque_meas_min()) - - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(1, self.safety.get_toyota_torque_meas_max()) - self.assertEqual(-1, self.safety.get_toyota_torque_meas_min()) - - def test_gas_interceptor_safety_check(self): - - self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0, 0x200))) - self.assertFalse(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200))) - self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200))) + should_tx = np.isclose(accel, 0, atol=0.0001) + self.assertEqual(should_tx, self._tx(self._accel_msg(accel))) def test_rx_hook(self): # checksum checks @@ -299,31 +98,12 @@ def test_rx_hook(self): if msg == "trq": to_push = self._torque_meas_msg(0) if msg == "pcm": - to_push = self._pcm_cruise_msg(1) - self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push = self._pcm_status_msg(True) + self.assertTrue(self._rx(to_push)) to_push[0].RDHR = 0 - self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self._rx(to_push)) self.assertFalse(self.safety.get_controls_allowed()) - def test_fwd_hook(self): - - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - blocked_msgs = [0x2E4, 0x412, 0x191] - blocked_msgs += [0x343] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py index a413fd445b05a9..237891d4ac1ef6 100644 --- a/tests/safety/test_volkswagen_mqb.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -1,10 +1,10 @@ #!/usr/bin/env python3 import unittest import numpy as np -import crcmod from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, MAX_WRONG_COUNTERS MAX_RATE_UP = 4 MAX_RATE_DOWN = 10 @@ -24,42 +24,7 @@ MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts -# Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration -TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]] - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -# Python crcmod works differently somehow from every other CRC calculator. The -# implied leading 1 on the polynomial isn't a problem, but to get the right -# result for CRC-8H2F/AUTOSAR, we have to feed it initCrc 0x00 instead of 0xFF. -volkswagen_crc_8h2f = crcmod.mkCrcFun(0x12F, initCrc=0x00, rev=False, xorOut=0xFF) - -def volkswagen_mqb_crc(msg, addr, len_msg): - # This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation of - # this algorithm for a version with explanatory comments. - msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little') - counter = (msg.RDLR & 0xF00) >> 8 - if addr == MSG_EPS_01: - magic_pad = b'\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5'[counter] - elif addr == MSG_ESP_05: - magic_pad = b'\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07'[counter] - elif addr == MSG_TSK_06: - magic_pad = b'\xC4\xE2\x4F\xE4\xF8\x2F\x56\x81\x9F\xE5\x83\x44\x05\x3F\x97\xDF'[counter] - elif addr == MSG_MOTOR_20: - magic_pad = b'\xE9\x65\xAE\x6B\x7B\x35\xE5\x5F\x4E\xC7\x86\xA2\xBB\xDD\xEB\xB4'[counter] - elif addr == MSG_HCA_01: - magic_pad = b'\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA'[counter] - elif addr == MSG_GRA_ACC_01: - magic_pad = b'\x6A\x38\xB4\x27\x22\xEF\xE1\xBB\xF8\x80\x84\x49\xC7\x9E\x1E\x2B'[counter] - else: - magic_pad = None - return volkswagen_crc_8h2f(msg_bytes[1:len_msg] + magic_pad.to_bytes(1, 'little')) - -class TestVolkswagenMqbSafety(unittest.TestCase): +class TestVolkswagenMqbSafety(common.PandaSafetyTest): cnt_eps_01 = 0 cnt_esp_05 = 0 cnt_tsk_06 = 0 @@ -67,187 +32,118 @@ class TestVolkswagenMqbSafety(unittest.TestCase): cnt_hca_01 = 0 cnt_gra_acc_01 = 0 - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0) - cls.safety.init_tests_volkswagen() + # Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep + # compatibility with gateway and camera integration + TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]] + STANDSTILL_THRESHOLD = 1 + RELAY_MALFUNCTION_ADDR = MSG_HCA_01 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_01, MSG_LDW_02]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("vw_mqb_2010") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0) + self.safety.init_tests() + + # override these inherited tests from PandaSafetyTest + def test_cruise_engaged_prev(self): pass def _set_prev_torque(self, t): - self.safety.set_volkswagen_desired_torque_last(t) - self.safety.set_volkswagen_rt_torque_last(t) + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) # Wheel speeds _esp_19_msg def _speed_msg(self, speed): - wheel_speed_scaled = int(speed / 0.0075) - to_send = make_msg(0, MSG_ESP_19) - to_send[0].RDLR = wheel_speed_scaled | (wheel_speed_scaled << 16) - to_send[0].RDHR = wheel_speed_scaled | (wheel_speed_scaled << 16) - return to_send + values = {"ESP_%s_Radgeschw_02"%s: speed for s in ["HL", "HR", "VL", "VR"]} + return self.packer.make_can_msg_panda("ESP_19", 0, values) # Brake light switch _esp_05_msg def _brake_msg(self, brake): - to_send = make_msg(0, MSG_ESP_05) - to_send[0].RDLR = (0x1 << 26) if brake else 0 - to_send[0].RDLR |= (self.cnt_esp_05 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_ESP_05, 8) + values = {"ESP_Fahrer_bremst": brake, "COUNTER": self.cnt_esp_05 % 16} self.__class__.cnt_esp_05 += 1 - return to_send + return self.packer.make_can_msg_panda("ESP_05", 0, values) + + # Driver throttle input + def _gas_msg(self, gas): + values = {"MO_Fahrpedalrohwert_01": gas, "COUNTER": self.cnt_motor_20 % 16} + self.__class__.cnt_motor_20 += 1 + return self.packer.make_can_msg_panda("Motor_20", 0, values) + + # ACC engagement status + def _pcm_status_msg(self, enable): + values = {"TSK_Status": 3 if enable else 1, "COUNTER": self.cnt_tsk_06 % 16} + self.__class__.cnt_tsk_06 += 1 + return self.packer.make_can_msg_panda("TSK_06", 0, values) # Driver steering input torque def _eps_01_msg(self, torque): - to_send = make_msg(0, MSG_EPS_01) - t = abs(torque) - to_send[0].RDHR = ((t & 0x1FFF) << 8) - if torque < 0: - to_send[0].RDHR |= 0x1 << 23 - to_send[0].RDLR |= (self.cnt_eps_01 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_EPS_01, 8) + values = {"Driver_Strain": abs(torque), "Driver_Strain_VZ": torque < 0, + "COUNTER": self.cnt_eps_01 % 16} self.__class__.cnt_eps_01 += 1 - return to_send + return self.packer.make_can_msg_panda("EPS_01", 0, values) # openpilot steering output torque def _hca_01_msg(self, torque): - to_send = make_msg(0, MSG_HCA_01) - t = abs(torque) - to_send[0].RDLR = (t & 0xFFF) << 16 - if torque < 0: - to_send[0].RDLR |= 0x1 << 31 - to_send[0].RDLR |= (self.cnt_hca_01 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_HCA_01, 8) + values = {"Assist_Torque": abs(torque), "Assist_VZ": torque < 0, + "COUNTER": self.cnt_hca_01 % 16} self.__class__.cnt_hca_01 += 1 - return to_send - - # ACC engagement status - def _tsk_06_msg(self, status): - to_send = make_msg(0, MSG_TSK_06) - to_send[0].RDLR = (status & 0x7) << 24 - to_send[0].RDLR |= (self.cnt_tsk_06 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_TSK_06, 8) - self.__class__.cnt_tsk_06 += 1 - return to_send - - # Driver throttle input - def _motor_20_msg(self, gas): - to_send = make_msg(0, MSG_MOTOR_20) - to_send[0].RDLR = (gas & 0xFF) << 12 - to_send[0].RDLR |= (self.cnt_motor_20 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_MOTOR_20, 8) - self.__class__.cnt_motor_20 += 1 - return to_send + return self.packer.make_can_msg_panda("HCA_01", 0, values) # Cruise control buttons - def _gra_acc_01_msg(self, bit): - to_send = make_msg(2, MSG_GRA_ACC_01) - to_send[0].RDLR = 1 << bit - to_send[0].RDLR |= (self.cnt_gra_acc_01 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_GRA_ACC_01, 8) + def _gra_acc_01_msg(self, cancel=0, resume=0, _set=0): + values = {"GRA_Abbrechen": cancel, "GRA_Tip_Setzen": _set, + "GRA_Tip_Wiederaufnahme": resume, "COUNTER": self.cnt_gra_acc_01 % 16} self.__class__.cnt_gra_acc_01 += 1 - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, MSG_HCA_01) - - def test_prev_gas(self): - for g in range(0, 256): - self.safety.safety_rx_hook(self._motor_20_msg(g)) - self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) + return self.packer.make_can_msg_panda("GRA_ACC_01", 0, values) def test_enable_control_allowed_from_cruise(self): self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._tsk_06_msg(3)) + self._rx(self._pcm_status_msg(True)) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._tsk_06_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_sample_speed(self): - # Stationary - self.safety.safety_rx_hook(self._speed_msg(0)) - self.assertEqual(0, self.safety.get_volkswagen_moving()) - # 1 km/h, just under 0.3 m/s safety grace threshold - self.safety.safety_rx_hook(self._speed_msg(1)) - self.assertEqual(0, self.safety.get_volkswagen_moving()) - # 2 km/h, just over 0.3 m/s safety grace threshold - self.safety.safety_rx_hook(self._speed_msg(2)) - self.assertEqual(1, self.safety.get_volkswagen_moving()) - # 144 km/h, openpilot V_CRUISE_MAX - self.safety.safety_rx_hook(self._speed_msg(144)) - self.assertEqual(1, self.safety.get_volkswagen_moving()) - - def test_prev_brake(self): - self.assertFalse(self.safety.get_brake_pressed_prev()) - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_brake_pressed_prev()) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, 1) - - def test_disengage_on_gas(self): - self.safety.safety_rx_hook(self._motor_20_msg(0)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._motor_20_msg(1)) + self._rx(self._pcm_status_msg(False)) self.assertFalse(self.safety.get_controls_allowed()) - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._motor_20_msg(1)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._motor_20_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._motor_20_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - def test_steer_safety_check(self): for enabled in [0, 1]: for t in range(-500, 500): 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.safety_tx_hook(self._hca_01_msg(t))) + self.assertFalse(self._tx(self._hca_01_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + self.assertTrue(self._tx(self._hca_01_msg(t))) def test_spam_cancel_safety_check(self): - BIT_CANCEL = 13 - BIT_RESUME = 19 - BIT_SET = 16 self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_CANCEL))) - self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME))) - self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_SET))) + self.assertTrue(self._tx(self._gra_acc_01_msg(cancel=1))) + self.assertFalse(self._tx(self._gra_acc_01_msg(resume=1))) + self.assertFalse(self._tx(self._gra_acc_01_msg(_set=1))) # do not block resume if we are engaged already self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME))) + self.assertTrue(self._tx(self._gra_acc_01_msg(resume=1))) def test_non_realtime_limit_up(self): - self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP))) + self.assertTrue(self._tx(self._hca_01_msg(MAX_RATE_UP))) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP))) + self.assertTrue(self._tx(self._hca_01_msg(-MAX_RATE_UP))) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP + 1))) + self.assertFalse(self._tx(self._hca_01_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self._tx(self._hca_01_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): - self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) self.safety.set_controls_allowed(True) def test_against_torque_driver(self): @@ -256,12 +152,12 @@ def test_against_torque_driver(self): for sign in [-1, 1]: for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): t *= -sign - self.safety.set_volkswagen_torque_driver(t, t) + self.safety.set_torque_driver(t, t) self._set_prev_torque(MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_STEER * sign))) + self.assertTrue(self._tx(self._hca_01_msg(MAX_STEER * sign))) - self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_STEER))) + self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self._tx(self._hca_01_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -269,62 +165,62 @@ def test_against_torque_driver(self): torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign delta = 1 * sign self._set_prev_torque(torque_desired) - self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired))) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self._tx(self._hca_01_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) - self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired + delta))) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self._tx(self._hca_01_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(0))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(self._hca_01_msg(0))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self._tx(self._hca_01_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_volkswagen() + self.safety.init_tests() self._set_prev_torque(0) - self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_torque_driver(0, 0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._hca_01_msg(t))) + self.assertFalse(self._tx(self._hca_01_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.safety_tx_hook(self._hca_01_msg(t))) + self.assertTrue(self._tx(self._hca_01_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._hca_01_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self._tx(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) def test_torque_measurements(self): - self.safety.safety_rx_hook(self._eps_01_msg(50)) - self.safety.safety_rx_hook(self._eps_01_msg(-50)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(50)) + self._rx(self._eps_01_msg(-50)) + self._rx(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(0)) - self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min()) - self.assertEqual(50, self.safety.get_volkswagen_torque_driver_max()) + self.assertEqual(-50, self.safety.get_torque_driver_min()) + self.assertEqual(50, self.safety.get_torque_driver_max()) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) - self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min()) + self._rx(self._eps_01_msg(0)) + self.assertEqual(0, self.safety.get_torque_driver_max()) + self.assertEqual(-50, self.safety.get_torque_driver_min()) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) - self.assertEqual(0, self.safety.get_volkswagen_torque_driver_min()) + self._rx(self._eps_01_msg(0)) + self.assertEqual(0, self.safety.get_torque_driver_max()) + self.assertEqual(0, self.safety.get_torque_driver_min()) def test_rx_hook(self): # checksum checks @@ -338,12 +234,12 @@ def test_rx_hook(self): if msg == MSG_ESP_05: to_push = self._brake_msg(False) if msg == MSG_TSK_06: - to_push = self._tsk_06_msg(3) + to_push = self._pcm_status_msg(True) if msg == MSG_MOTOR_20: - to_push = self._motor_20_msg(0) - self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push = self._gas_msg(0) + self.assertTrue(self._rx(to_push)) to_push[0].RDHR ^= 0xFF - self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self._rx(to_push)) self.assertFalse(self.safety.get_controls_allowed()) # counter @@ -355,43 +251,26 @@ def test_rx_hook(self): self.__class__.cnt_motor_20 += 1 if i < MAX_WRONG_COUNTERS: self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._brake_msg(False)) - self.safety.safety_rx_hook(self._tsk_06_msg(3)) - self.safety.safety_rx_hook(self._motor_20_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._brake_msg(False)) + self._rx(self._pcm_status_msg(True)) + self._rx(self._gas_msg(0)) else: - self.assertFalse(self.safety.safety_rx_hook(self._eps_01_msg(0))) - self.assertFalse(self.safety.safety_rx_hook(self._brake_msg(False))) - self.assertFalse(self.safety.safety_rx_hook(self._tsk_06_msg(3))) - self.assertFalse(self.safety.safety_rx_hook(self._motor_20_msg(0))) + self.assertFalse(self._rx(self._eps_01_msg(0))) + self.assertFalse(self._rx(self._brake_msg(False))) + self.assertFalse(self._rx(self._pcm_status_msg(True))) + self.assertFalse(self._rx(self._gas_msg(0))) self.assertFalse(self.safety.get_controls_allowed()) # restore counters for future tests with a couple of good messages for i in range(2): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._brake_msg(False)) - self.safety.safety_rx_hook(self._tsk_06_msg(3)) - self.safety.safety_rx_hook(self._motor_20_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._brake_msg(False)) + self._rx(self._pcm_status_msg(True)) + self._rx(self._gas_msg(0)) self.assertTrue(self.safety.get_controls_allowed()) - def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - blocked_msgs_0to2 = [] - blocked_msgs_2to0 = [MSG_HCA_01, MSG_LDW_02] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = -1 if m in blocked_msgs_0to2 else 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs_2to0 else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_volkswagen_pq.py b/tests/safety/test_volkswagen_pq.py new file mode 100644 index 00000000000000..f30d19f3f82af2 --- /dev/null +++ b/tests/safety/test_volkswagen_pq.py @@ -0,0 +1,278 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from panda import Panda +from panda.tests.safety import libpandasafety_py +import panda.tests.safety.common as common +from panda.tests.safety.common import make_msg, MAX_WRONG_COUNTERS + +MAX_RATE_UP = 4 +MAX_RATE_DOWN = 10 +MAX_STEER = 300 +MAX_RT_DELTA = 75 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 80 +DRIVER_TORQUE_FACTOR = 3 + +MSG_LENKHILFE_3 = 0x0D0 # RX from EPS, for steering angle and driver steering torque +MSG_HCA_1 = 0x0D2 # TX by OP, Heading Control Assist steering torque +MSG_MOTOR_2 = 0x288 # RX from ECU, for CC state and brake switch state +MSG_MOTOR_3 = 0x380 # RX from ECU, for driver throttle input +MSG_GRA_NEU = 0x38A # TX by OP, ACC control buttons for cancel/resume +MSG_BREMSE_3 = 0x4A0 # RX from ABS, for wheel speeds +MSG_LDW_1 = 0x5BE # TX by OP, Lane line recognition and text alerts + + +def volkswagen_pq_checksum(msg, addr, len_msg): + msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little') + msg_bytes = msg_bytes[1:len_msg] + + checksum = 0 + for i in msg_bytes: + checksum ^= i + return checksum + +class TestVolkswagenPqSafety(common.PandaSafetyTest): + cruise_engaged = False + brake_pressed = False + cnt_lenkhilfe_3 = 0 + cnt_hca_1 = 0 + + # Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration + TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0]] + STANDSTILL_THRESHOLD = 1 + RELAY_MALFUNCTION_ADDR = MSG_HCA_1 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_PQ, 0) + self.safety.init_tests() + + # override these inherited tests from PandaSafetyTest + def test_cruise_engaged_prev(self): pass + + def _set_prev_torque(self, t): + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) + + # Wheel speeds (Bremse_3) + def _speed_msg(self, speed): + wheel_speed_scaled = int(speed / 0.01) + to_send = make_msg(0, MSG_BREMSE_3) + to_send[0].RDLR = (wheel_speed_scaled | (wheel_speed_scaled << 16)) << 1 + to_send[0].RDHR = (wheel_speed_scaled | (wheel_speed_scaled << 16)) << 1 + return to_send + + # Brake light switch (shared message Motor_2) + def _brake_msg(self, brake): + to_send = make_msg(0, MSG_MOTOR_2) + to_send[0].RDLR = (0x1 << 16) if brake else 0 + # since this siganl's used for engagement status, preserve current state + to_send[0].RDLR |= (self.safety.get_controls_allowed() & 0x3) << 22 + return to_send + + # ACC engaged status (shared message Motor_2) + def _pcm_status_msg(self, cruise): + self.__class__.cruise_engaged = cruise + return self._motor_2_msg() + + # Driver steering input torque + def _lenkhilfe_3_msg(self, torque): + to_send = make_msg(0, MSG_LENKHILFE_3) + t = abs(torque) + to_send[0].RDLR = ((t & 0x3FF) << 16) + if torque < 0: + to_send[0].RDLR |= 0x1 << 26 + to_send[0].RDLR |= (self.cnt_lenkhilfe_3 % 16) << 12 + to_send[0].RDLR |= volkswagen_pq_checksum(to_send[0], MSG_LENKHILFE_3, 8) + self.__class__.cnt_lenkhilfe_3 += 1 + return to_send + + # openpilot steering output torque + def _hca_1_msg(self, torque): + to_send = make_msg(0, MSG_HCA_1) + t = abs(torque) << 5 # DBC scale from centi-Nm to PQ network (approximated) + to_send[0].RDLR = (t & 0x7FFF) << 16 + if torque < 0: + to_send[0].RDLR |= 0x1 << 31 + to_send[0].RDLR |= (self.cnt_hca_1 % 16) << 8 + to_send[0].RDLR |= volkswagen_pq_checksum(to_send[0], MSG_HCA_1, 8) + self.__class__.cnt_hca_1 += 1 + return to_send + + # ACC engagement and brake light switch status + # Called indirectly for compatibility with common.py tests + def _motor_2_msg(self): + to_send = make_msg(0, MSG_MOTOR_2) + to_send[0].RDLR = (0x1 << 16) if self.__class__.brake_pressed else 0 + to_send[0].RDLR |= (self.__class__.cruise_engaged & 0x3) << 22 + return to_send + + # Driver throttle input (motor_3) + def _gas_msg(self, gas): + to_send = make_msg(0, MSG_MOTOR_3) + to_send[0].RDLR = (gas & 0xFF) << 16 + return to_send + + # Cruise control buttons + def _gra_neu_msg(self, bit): + to_send = make_msg(2, MSG_GRA_NEU) + to_send[0].RDLR = 1 << bit + to_send[0].RDLR |= volkswagen_pq_checksum(to_send[0], MSG_GRA_NEU, 8) + return to_send + + def test_steer_safety_check(self): + for enabled in [0, 1]: + for t in range(-500, 500): + 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._tx(self._hca_1_msg(t))) + else: + self.assertTrue(self._tx(self._hca_1_msg(t))) + + def test_spam_cancel_safety_check(self): + BIT_CANCEL = 9 + BIT_SET = 16 + BIT_RESUME = 17 + self.safety.set_controls_allowed(0) + self.assertTrue(self._tx(self._gra_neu_msg(BIT_CANCEL))) + self.assertFalse(self._tx(self._gra_neu_msg(BIT_RESUME))) + self.assertFalse(self._tx(self._gra_neu_msg(BIT_SET))) + # do not block resume if we are engaged already + self.safety.set_controls_allowed(1) + self.assertTrue(self._tx(self._gra_neu_msg(BIT_RESUME))) + + def test_non_realtime_limit_up(self): + self.safety.set_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self._tx(self._hca_1_msg(MAX_RATE_UP))) + self._set_prev_torque(0) + self.assertTrue(self._tx(self._hca_1_msg(-MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self._tx(self._hca_1_msg(MAX_RATE_UP + 1))) + self.safety.set_controls_allowed(True) + self._set_prev_torque(0) + self.assertFalse(self._tx(self._hca_1_msg(-MAX_RATE_UP - 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_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_torque_driver(t, t) + self._set_prev_torque(MAX_STEER * sign) + self.assertTrue(self._tx(self._hca_1_msg(MAX_STEER * sign))) + + self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self._tx(self._hca_1_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_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self._tx(self._hca_1_msg(torque_desired))) + self._set_prev_torque(torque_desired + delta) + self.safety.set_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self._tx(self._hca_1_msg(torque_desired + delta))) + + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(self._hca_1_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self._tx(self._hca_1_msg(0))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self._tx(self._hca_1_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() + self._set_prev_torque(0) + self.safety.set_torque_driver(0, 0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self._tx(self._hca_1_msg(t))) + self.assertFalse(self._tx(self._hca_1_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._tx(self._hca_1_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(RT_INTERVAL + 1) + self.assertTrue(self._tx(self._hca_1_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self._tx(self._hca_1_msg(sign * (MAX_RT_DELTA + 1)))) + + def test_torque_measurements(self): + self._rx(self._lenkhilfe_3_msg(50)) + self._rx(self._lenkhilfe_3_msg(-50)) + self._rx(self._lenkhilfe_3_msg(0)) + self._rx(self._lenkhilfe_3_msg(0)) + self._rx(self._lenkhilfe_3_msg(0)) + self._rx(self._lenkhilfe_3_msg(0)) + + self.assertEqual(-50, self.safety.get_torque_driver_min()) + self.assertEqual(50, self.safety.get_torque_driver_max()) + + self._rx(self._lenkhilfe_3_msg(0)) + self.assertEqual(0, self.safety.get_torque_driver_max()) + self.assertEqual(-50, self.safety.get_torque_driver_min()) + + self._rx(self._lenkhilfe_3_msg(0)) + self.assertEqual(0, self.safety.get_torque_driver_max()) + self.assertEqual(0, self.safety.get_torque_driver_min()) + + def test_rx_hook(self): + # checksum checks + # TODO: Would be ideal to check non-checksum non-counter messages as well, + # but I'm not sure if we can easily validate Panda's simple temporal + # reception-rate check here. + for msg in [MSG_LENKHILFE_3]: + self.safety.set_controls_allowed(1) + if msg == MSG_LENKHILFE_3: + to_push = self._lenkhilfe_3_msg(0) + self.assertTrue(self._rx(to_push)) + to_push[0].RDHR ^= 0xFF + self.assertFalse(self._rx(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + + # counter + # reset wrong_counters to zero by sending valid messages + for i in range(MAX_WRONG_COUNTERS + 1): + self.__class__.cnt_lenkhilfe_3 += 1 + if i < MAX_WRONG_COUNTERS: + self.safety.set_controls_allowed(1) + self._rx(self._lenkhilfe_3_msg(0)) + else: + self.assertFalse(self._rx(self._lenkhilfe_3_msg(0))) + self.assertFalse(self.safety.get_controls_allowed()) + + # restore counters for future tests with a couple of good messages + for i in range(2): + self.safety.set_controls_allowed(1) + self._rx(self._lenkhilfe_3_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety_replay/Dockerfile b/tests/safety_replay/Dockerfile index b0b5f0c2710bfa..09236e6a2adee4 100644 --- a/tests/safety_replay/Dockerfile +++ b/tests/safety_replay/Dockerfile @@ -1,6 +1,21 @@ FROM ubuntu:16.04 -RUN apt-get update && apt-get install -y make clang python python-pip git libarchive-dev libusb-1.0-0 locales curl zlib1g-dev libffi-dev bzip2 libssl-dev libbz2-dev +RUN apt-get update && apt-get install -y \ + bzip2 \ + clang \ + curl \ + git \ + libarchive-dev \ + libbz2-dev \ + libcurl4-openssl-dev \ + libffi-dev \ + libssl-dev \ + libusb-1.0-0 \ + locales \ + make \ + python \ + python-pip \ + zlib1g-dev RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen ENV LANG en_US.UTF-8 @@ -21,15 +36,11 @@ RUN pip install -r requirements_extra.txt COPY tests/safety_replay/install_capnp.sh install_capnp.sh RUN ./install_capnp.sh -RUN mkdir /openpilot +RUN git clone https://github.com/commaai/openpilot.git || true WORKDIR /openpilot -RUN git clone https://github.com/commaai/cereal.git || true -WORKDIR /openpilot/cereal -RUN git pull -RUN git checkout bb2cc7572de99becce1bfbae63f3b38d5464e162 +RUN git pull && git checkout f9257fc75f68c673f9e433985fbe739f23310bb4 +RUN git submodule update --init cereal + COPY . /openpilot/panda WORKDIR /openpilot/panda/tests/safety_replay -RUN git clone https://github.com/commaai/openpilot-tools.git tools || true -WORKDIR tools -RUN git checkout d69c6bc85f221766305ec53956e9a1d3bf283160 diff --git a/tests/safety_replay/requirements_extra.txt b/tests/safety_replay/requirements_extra.txt index b04b7674d76efa..8034efc89e0ded 100644 --- a/tests/safety_replay/requirements_extra.txt +++ b/tests/safety_replay/requirements_extra.txt @@ -2,3 +2,6 @@ aenum subprocess32 libarchive pycapnp +pycurl +tenacity +atomicwrites diff --git a/tests/safety_replay/test_safety_replay.py b/tests/safety_replay/test_safety_replay.py index 81b9f5b13e9d54..1ba800a7d63177 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -15,11 +15,12 @@ ("38bfd238edecbcd7|2019-06-07--10-15-25.bz2", Panda.SAFETY_TOYOTA, 66), # TOYOTA.PRIUS ("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", Panda.SAFETY_GM, 0), # GM.VOLT ("0375fdf7b1ce594d|2019-05-21--20-10-33.bz2", Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 1), # HONDA.ACCORD - ("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SANTA_FE ("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID ("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, 0), # SUBARU.IMPREZA - ("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF + ("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF (MK7) + ("d12cd943127f267b|2020-03-27--15-57-18.bz2", Panda.SAFETY_VOLKSWAGEN_PQ, 0), # 2009 VW Passat R36 (B6), supporting OP port not yet upstreamed ("fbbfa6af821552b9|2020-03-03--08-09-43.bz2", Panda.SAFETY_NISSAN, 0), # NISSAN.XTRAIL + ("5b7c365c50084530_2020-04-15--16-13-24--3--rlog.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SONATA ] if __name__ == "__main__": @@ -39,4 +40,3 @@ for f in failed: print("\n**** failed on %s ****" % f) assert len(failed) == 0, "\nfailed on %d logs" % len(failed) -