diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml index a4ae26ef..52e45606 100644 --- a/panda/.circleci/config.yml +++ b/panda/.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/panda/VERSION b/panda/VERSION index 0d687f1e..ad90322a 100644 --- a/panda/VERSION +++ b/panda/VERSION @@ -1 +1 @@ -v1.7.3 \ No newline at end of file +v1.7.5 \ No newline at end of file diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h index 97f322f6..57305980 100644 --- a/panda/board/boards/black.h +++ b/panda/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/panda/board/boards/uno.h b/panda/board/boards/uno.h index a6a06eff..de430cb5 100644 --- a/panda/board/boards/uno.h +++ b/panda/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/panda/board/build.mk b/panda/board/build.mk index 21daf53a..5e205f02 100644 --- a/panda/board/build.mk +++ b/panda/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/panda/board/drivers/can.h b/panda/board/drivers/can.h index 93db2565..3a6eefac 100644 --- a/panda/board/drivers/can.h +++ b/panda/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/panda/board/drivers/llcan.h b/panda/board/drivers/llcan.h index 5e9f276e..e467d67b 100644 --- a/panda/board/drivers/llcan.h +++ b/panda/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); + uint32_t timeout_counter = 0U; + while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){ + // Delay for about 1ms + delay(10000); + timeout_counter++; - // 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); + 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); + 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); - #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; + // 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)) {} + uint32_t timeout_counter = 0U; + while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) { + // Delay for about 1ms + delay(10000); + timeout_counter++; - // 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"); + 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/panda/board/drivers/usb.h b/panda/board/drivers/usb.h index a970194f..4cfd90df 100644 --- a/panda/board/drivers/usb.h +++ b/panda/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/panda/board/get_sdk_mac.sh b/panda/board/get_sdk_mac.sh index a0a919f7..24b93b54 100755 --- a/panda/board/get_sdk_mac.sh +++ b/panda/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/panda/board/main.c b/panda/board/main.c index 8bc4ac78..79cccec3 100644 --- a/panda/board/main.c +++ b/panda/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/panda/board/pedal/main.c b/panda/board/pedal/main.c index 3192f4b2..3d7bd03a 100644 --- a/panda/board/pedal/main.c +++ b/panda/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/panda/board/safety.h b/panda/board/safety.h index 407f33f6..d2d107d6 100644 --- a/panda/board/safety.h +++ b/panda/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/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h deleted file mode 100644 index 0f500a8c..00000000 --- a/panda/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/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index 0bcffd63..82fcfb14 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/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/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h index ba96b7dd..793dc961 100644 --- a/panda/board/safety/safety_defaults.h +++ b/panda/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/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 067ed1ca..3acf8eb5 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/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/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 8672cc26..264df313 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/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/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index ecac1489..9f09da95 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/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/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 1b5b656f..61c3b7e8 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -6,56 +6,72 @@ 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 +// TODO: do checksum checks AddrCheckStruct hyundai_rx_checks[] = { - {.addr = {608}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {897}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {902}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {916}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {1057}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {608}, .bus = 0, .max_counter = 3U, .expected_timestep = 10000U}, + {.addr = {897}, .bus = 0, .max_counter = 255U, .expected_timestep = 10000U}, + {.addr = {902}, .bus = 0, .max_counter = 3U, .expected_timestep = 10000U}, + {.addr = {916}, .bus = 0, .max_counter = 7U, .expected_timestep = 10000U}, + {.addr = {1057}, .bus = 0, .max_counter = 15U, .expected_timestep = 20000U}, }; 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 uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t cnt; + if (addr == 608) { + cnt = (GET_BYTE(to_push, 7) >> 4) & 0x3; + } else if (addr == 897) { + cnt = GET_BYTE(to_push, 5); + } else if (addr == 902) { + cnt = (GET_BYTE(to_push, 1) >> 6) & 0x3; + } else if (addr == 916) { + cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7; + } else if (addr == 1057) { + cnt = GET_BYTE(to_push, 7) & 0xF; + } else { + cnt = 0; + } + return cnt; +} 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); + NULL, NULL, hyundai_get_counter); - if (valid && GET_BUS(to_push) == 0) { - int addr = GET_ADDR(to_push); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (addr == 897) { - int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048; + int addr = GET_ADDR(to_push); + int bus = GET_BUS(to_push); + + 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 +79,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 +96,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 +128,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 +153,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 +185,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/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index f6b19c1a..8ee63849 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/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/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index baa82b8d..aca99d19 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/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/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 9b7665f2..17bd699d 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/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/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 4f18ee67..f69e4d3c 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/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/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h index 132c10d1..4063fa7d 100644 --- a/panda/board/safety/safety_volkswagen.h +++ b/panda/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/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index aa9f7fdf..be8be93b 100644 --- a/panda/board/safety_declarations.h +++ b/panda/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/panda/board/spi_flasher.h b/panda/board/spi_flasher.h index 1c703516..b0511c14 100644 --- a/panda/board/spi_flasher.h +++ b/panda/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/panda/examples/can_unique.py b/panda/examples/can_unique.py index fdd586c3..0b94076f 100755 --- a/panda/examples/can_unique.py +++ b/panda/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 @@ class Info(): 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/panda/python/__init__.py b/panda/python/__init__.py index c0e27e8d..e8525a17 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -9,6 +9,7 @@ import os 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 @@ class Panda(object): 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 @@ class Panda(object): 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 @@ class Panda(object): # ******************* 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 @@ class Panda(object): 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/panda/python/flash_release.py b/panda/python/flash_release.py index b50c9b36..85bc5536 100755 --- a/panda/python/flash_release.py +++ b/panda/python/flash_release.py @@ -65,19 +65,22 @@ def flash_release(path=None, st_serial=None): 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/panda/tests/automated/7_can_loopback.py b/panda/tests/automated/7_can_loopback.py index cb9f5570..0b0df1bf 100644 --- a/panda/tests/automated/7_can_loopback.py +++ b/panda/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_gen2_loopback(p): 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/panda/tests/bulk_write_test.py b/panda/tests/bulk_write_test.py new file mode 100755 index 00000000..43a52bce --- /dev/null +++ b/panda/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/panda/tests/safety/Dockerfile b/panda/tests/safety/Dockerfile index 3c70d253..524cfc39 100644 --- a/panda/tests/safety/Dockerfile +++ b/panda/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/panda/tests/safety/common.py b/panda/tests/safety/common.py index e0296d3a..8d81f115 100644 --- a/panda/tests/safety/common.py +++ b/panda/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 + ret[0].RIR = (addr << 21) | 1 + ret[0].RDTR = len(dat) | ((bus & 0xF) << 4) + ret[0].RDHR = rdhr + ret[0].RDLR = rdlr - return to_send + return ret -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()) +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))) + self.assertFalse(self._tx(make_msg(b, a, 8))) + self.assertEqual(-1, self.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()) + 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)) - @staticmethod - def test_spam_can_buses(test, TX_MSGS): + 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))) - @staticmethod - def test_allow_brake_at_zero_speed(test): + 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()) + + 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/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py index 3b552d0f..9898e6df 100644 --- a/panda/tests/safety/libpandasafety_py.py +++ b/panda/tests/safety/libpandasafety_py.py @@ -32,6 +32,8 @@ typedef struct 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 @@ bool get_gas_interceptor_detetcted(void); 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,54 +61,17 @@ int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push); 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); - void init_tests_nissan(void); void set_nissan_desired_angle_last(int t); diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c index 9c7955b2..52d2ea7f 100644 --- a/panda/tests/safety/test.c +++ b/panda/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_torque_meas(int min, int max){ + torque_meas.min = min; + torque_meas.max = max; } -void set_cadillac_torque_driver(int min, int max){ - cadillac_torque_driver.min = min; - cadillac_torque_driver.max = max; +int get_torque_meas_min(void){ + return torque_meas.min; } -void set_gm_torque_driver(int min, int max){ - gm_torque_driver.min = min; - gm_torque_driver.max = max; +int get_torque_meas_max(void){ + return torque_meas.max; } -void set_hyundai_torque_driver(int min, int max){ - hyundai_torque_driver.min = min; - hyundai_torque_driver.max = max; +void set_torque_driver(int min, int max){ + torque_driver.min = min; + torque_driver.max = max; } -void set_chrysler_torque_meas(int min, int max){ - chrysler_torque_meas.min = min; - chrysler_torque_meas.max = max; +int get_torque_driver_min(void){ + return torque_driver.min; } -void set_volkswagen_torque_driver(int min, int max){ - volkswagen_torque_driver.min = min; - volkswagen_torque_driver.max = max; +int get_torque_driver_max(void){ + return torque_driver.max; } -int get_volkswagen_torque_driver_min(void){ - return volkswagen_torque_driver.min; +void set_rt_torque_last(int t){ + rt_torque_last = t; } -int get_volkswagen_torque_driver_max(void){ - return volkswagen_torque_driver.max; -} - -int get_chrysler_torque_meas_min(void){ - return chrysler_torque_meas.min; -} - -int get_chrysler_torque_meas_max(void){ - return chrysler_torque_meas.max; -} - -int get_toyota_torque_meas_min(void){ - return toyota_torque_meas.min; -} - -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/panda/tests/safety/test_cadillac.py b/panda/tests/safety/test_cadillac.py deleted file mode 100644 index e4e75f15..00000000 --- a/panda/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/panda/tests/safety/test_chrysler.py b/panda/tests/safety/test_chrysler.py index c66b90b6..369f1f98 100755 --- a/panda/tests/safety/test_chrysler.py +++ b/panda/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 + values = {"LKAS_STEERING_TORQUE": torque} + return self.packer.make_can_msg_panda("LKAS_COMMAND", 0, values) - 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) - - 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) + def test_disengage_on_gas(self): 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): - 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/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index 34d92357..53433552 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -3,7 +3,8 @@ 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, UNSAFE_MODE MAX_RATE_UP = 7 MAX_RATE_DOWN = 17 @@ -15,141 +16,104 @@ MAX_REGEN = 1404 MAX_RT_DELTA = 128 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 50; -DRIVER_TORQUE_FACTOR = 4; +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 +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 twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val + 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() -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() + # 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._rx(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.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 @@ class TestGmSafety(unittest.TestCase): 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 @@ class TestGmSafety(unittest.TestCase): 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 @@ class TestGmSafety(unittest.TestCase): 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)))) + 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_fwd_hook(self): - # nothing allowed - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) + 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)) - 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.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/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py index adf02fd5..5cd2ba05 100755 --- a/panda/tests/safety/test_honda.py +++ b/panda/tests/safety/test_honda.py @@ -3,285 +3,115 @@ 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, 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._rx(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.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 @@ class TestHondaSafety(unittest.TestCase): 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): + # 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 + + 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()) - def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - fwd_brake = [False, True] +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} - 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 + PT_BUS = 0 - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + 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() - self.safety.set_honda_fwd_brake(False) + def _send_steer_msg(self, steer): + values = {"STEER_TORQUE": steer} + return self.packer.make_can_msg_panda("STEERING_CONTROL", 2, values) -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() - - 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/panda/tests/safety/test_hyundai.py b/panda/tests/safety/test_hyundai.py index 2d022759..dad07edf 100644 --- a/panda/tests/safety/test_hyundai.py +++ b/panda/tests/safety/test_hyundai.py @@ -3,7 +3,8 @@ 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 = 7 @@ -12,75 +13,67 @@ MAX_STEER = 255 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 + cnt_gas = 0 + cnt_speed = 0 + cnt_brake = 0 + cnt_cruise = 0 -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, "AliveCounter": self.cnt_gas % 4} + self.__class__.cnt_gas += 1 + 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, "AliveCounterTCS": self.cnt_brake % 8} + self.__class__.cnt_brake += 1 + 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"]} + values["WHL_SPD_AliveCounter_LSB"] = self.cnt_speed % 4 + self.__class__.cnt_speed += 1 + return self.packer.make_can_msg_panda("WHL_SPD11", 0, values) + + def _pcm_status_msg(self, enabled): + values = {"ACCMode": enabled, "CR_VSM_Alive": self.cnt_cruise % 16} + self.__class__.cnt_cruise += 1 + 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 +81,27 @@ class TestHyundaiSafety(unittest.TestCase): 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 +110,12 @@ class TestHyundaiSafety(unittest.TestCase): 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 +123,44 @@ class TestHyundaiSafety(unittest.TestCase): 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 +168,12 @@ class TestHyundaiSafety(unittest.TestCase): 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/panda/tests/safety/test_nissan.py b/panda/tests/safety/test_nissan.py index ab826fff..853899ae 100644 --- a/panda/tests/safety/test_nissan.py +++ b/panda/tests/safety/test_nissan.py @@ -3,43 +3,37 @@ 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 -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): + + 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() 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 - - return to_send + 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 @@ class TestNissanSafety(unittest.TestCase): def _angle_meas_msg_array(self, angle): for i in range(6): - self.safety.safety_rx_hook(self._angle_meas_msg(angle)) + self._rx(self._angle_meas_msg(angle)) - def _lkas_state_msg(self, state): - to_send = make_msg(0, 0x1b6) - to_send[0].RDHR = (state & 0x1) << 6 - - 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) + 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 _send_gas_cmd(self, gas): - to_send = make_msg(0, 0x15c) - to_send[0].RDHR = ((gas & 0x3fc) << 6) | ((gas & 0x3) << 22) - - return to_send - - 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))) - 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.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))) + # 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()) - # 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))) + # Don't change + self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + + # Down + self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + + # 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/panda/tests/safety/test_subaru.py b/panda/tests/safety/test_subaru.py index d68090d1..08bb25bc 100644 --- a/panda/tests/safety/test_subaru.py +++ b/panda/tests/safety/test_subaru.py @@ -3,7 +3,8 @@ 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 = 50 MAX_RATE_DOWN = 70 @@ -12,181 +13,92 @@ MAX_STEER = 2047 MAX_RT_DELTA = 940 RT_INTERVAL = 250000 -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 +DRIVER_TORQUE_ALLOWANCE = 60 +DRIVER_TORQUE_FACTOR = 10 -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 + 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 _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 + 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 @@ class TestSubaruSafety(unittest.TestCase): 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 @@ class TestSubaruSafety(unittest.TestCase): 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)))) + 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_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))) - 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/panda/tests/safety/test_toyota.py b/panda/tests/safety/test_toyota.py old mode 100644 new mode 100755 index bc079559..8aee8e7f --- a/panda/tests/safety/test_toyota.py +++ b/panda/tests/safety/test_toyota.py @@ -3,294 +3,93 @@ 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, make_msg, UNSAFE_MODE -MAX_RATE_UP = 10 -MAX_RATE_DOWN = 25 -MAX_TORQUE = 1500 +MAX_ACCEL = 1.5 +MIN_ACCEL = -3.0 -MAX_ACCEL = 1500 -MIN_ACCEL = -3000 +ISO_MAX_ACCEL = 2.0 +ISO_MIN_ACCEL = -3.5 -MAX_RT_DELTA = 375 -RT_INTERVAL = 250000 +class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, \ + common.TorqueSteeringSafetyTest): -STANDSTILL_THRESHOLD = 100 # 1kph + 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_TORQUE_ERROR = 350 -INTERCEPTOR_THRESHOLD = 475 + 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 -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) + 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) + ((gas & 0xff) << 24) | ((gas & 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) - 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 @@ class TestToyotaSafety(unittest.TestCase): 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/panda/tests/safety/test_volkswagen_mqb.py b/panda/tests/safety/test_volkswagen_mqb.py index a413fd44..237891d4 100644 --- a/panda/tests/safety/test_volkswagen_mqb.py +++ b/panda/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_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque 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._rx(self._pcm_status_msg(False)) 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.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 @@ class TestVolkswagenMqbSafety(unittest.TestCase): 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 @@ class TestVolkswagenMqbSafety(unittest.TestCase): 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 @@ class TestVolkswagenMqbSafety(unittest.TestCase): 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 @@ class TestVolkswagenMqbSafety(unittest.TestCase): 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/panda/tests/safety/test_volkswagen_pq.py b/panda/tests/safety/test_volkswagen_pq.py new file mode 100644 index 00000000..f30d19f3 --- /dev/null +++ b/panda/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/panda/tests/safety_replay/.gitignore b/panda/tests/safety_replay/.gitignore new file mode 100644 index 00000000..192fb094 --- /dev/null +++ b/panda/tests/safety_replay/.gitignore @@ -0,0 +1 @@ +*.bz2 diff --git a/panda/tests/safety_replay/Dockerfile b/panda/tests/safety_replay/Dockerfile index b0b5f0c2..09236e6a 100644 --- a/panda/tests/safety_replay/Dockerfile +++ b/panda/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/panda/tests/safety_replay/requirements_extra.txt b/panda/tests/safety_replay/requirements_extra.txt index b04b7674..8034efc8 100644 --- a/panda/tests/safety_replay/requirements_extra.txt +++ b/panda/tests/safety_replay/requirements_extra.txt @@ -2,3 +2,6 @@ aenum subprocess32 libarchive pycapnp +pycurl +tenacity +atomicwrites diff --git a/panda/tests/safety_replay/test_safety_replay.py b/panda/tests/safety_replay/test_safety_replay.py index 81b9f5b1..1ba800a7 100755 --- a/panda/tests/safety_replay/test_safety_replay.py +++ b/panda/tests/safety_replay/test_safety_replay.py @@ -15,11 +15,12 @@ logs = [ ("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 @@ if __name__ == "__main__": for f in failed: print("\n**** failed on %s ****" % f) assert len(failed) == 0, "\nfailed on %d logs" % len(failed) -