From 1174c2174b641f2dd65728d167894a05b5f21639 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Mon, 8 Jul 2019 09:33:48 +1000 Subject: [PATCH 01/24] Better Panda --- openpilot_tools | 1 - panda/.circleci/config.yml | 69 +- panda/.gitignore | 5 +- panda/README.md | 2 +- panda/VERSION | 2 +- panda/board/README.md | 6 + panda/board/bootstub.c | 19 +- panda/board/build.mk | 34 +- panda/board/config.h | 10 +- panda/board/drivers/adc.h | 2 +- panda/board/drivers/can.h | 412 +++++---- panda/board/drivers/gmlan_alt.h | 75 +- panda/board/drivers/llgpio.h | 15 +- panda/board/drivers/spi.h | 15 +- panda/board/drivers/uart.h | 123 +-- panda/board/drivers/usb.h | 157 ++-- panda/board/gpio.h | 124 +-- panda/board/libc.h | 30 +- panda/board/main.c | 583 ++++++------- panda/board/pedal/Makefile | 10 +- panda/board/pedal/main.c | 122 +-- panda/board/safety.h | 182 ++-- panda/board/safety/safety_cadillac.h | 41 +- panda/board/safety/safety_chrysler.h | 79 +- panda/board/safety/safety_defaults.h | 28 +- panda/board/safety/safety_elm327.h | 64 +- panda/board/safety/safety_ford.h | 41 +- panda/board/safety/safety_gm.h | 121 +-- panda/board/safety/safety_gm_ascm.h | 39 +- panda/board/safety/safety_honda.h | 180 ++-- panda/board/safety/safety_hyundai.h | 113 +-- panda/board/safety/safety_subaru.h | 130 +-- panda/board/safety/safety_tesla.h | 793 +++++++++++++----- panda/board/safety/safety_toyota.h | 157 ++-- panda/board/safety/safety_toyota_ipas.h | 55 +- panda/board/spi_flasher.h | 21 +- panda/boardesp/elm327.c | 10 +- panda/boardesp/proxy.c | 4 +- panda/boardesp/webserver.c | 10 +- panda/common/version.mk | 22 +- panda/crypto/sha.c | 34 +- .../drivers/windows/pandaJ2534DLL/MessageRx.h | 2 +- .../pandaJ2534DLL/PandaJ2534Device.cpp | 2 +- panda/examples/query_vin_and_stats.py | 4 +- panda/python/__init__.py | 36 +- panda/python/dfu.py | 7 +- panda/python/esptool.py | 2 +- panda/python/update.py | 4 +- panda/requirements.txt | 5 +- panda/run_automated_tests.sh | 13 +- panda/setup.py | 2 +- panda/tests/automated/0_builds.py | 6 + panda/tests/automated/1_program.py | 16 +- panda/tests/automated/2_usb_to_can.py | 49 +- panda/tests/automated/3_wifi.py | 61 +- panda/tests/automated/4_wifi_functionality.py | 25 +- panda/tests/automated/5_wifi_udp.py | 43 +- panda/tests/automated/helpers.py | 133 +-- panda/tests/debug_console.py | 3 - panda/tests/elm_car_simulator.py | 6 +- panda/tests/safety/libpandasafety_py.py | 71 +- panda/tests/safety/test.c | 139 +-- panda/tests/safety/test_cadillac.py | 62 +- panda/tests/safety/test_chrysler.py | 85 +- panda/tests/safety/test_gm.py | 131 ++- panda/tests/safety/test_honda.py | 184 ++-- panda/tests/safety/test_hyundai.py | 84 +- panda/tests/safety/test_subaru.py | 66 +- panda/tests/safety/test_toyota.py | 400 +++++---- selfdrive/pandad.py | 2 +- 70 files changed, 2618 insertions(+), 2965 deletions(-) delete mode 160000 openpilot_tools diff --git a/openpilot_tools b/openpilot_tools deleted file mode 160000 index b6461274d68491..00000000000000 --- a/openpilot_tools +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b6461274d684915f39dc45efc5292ea890698da9 diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml index e36aaa142a5a18..2289ad8aa725e1 100644 --- a/panda/.circleci/config.yml +++ b/panda/.circleci/config.yml @@ -12,47 +12,6 @@ jobs: name: Run safety test command: | docker run panda_safety /bin/bash -c "cd /panda/tests/safety; ./test.sh" - - misra-c2012: - machine: - docker_layer_caching: true - steps: - - checkout - - run: - name: Build image - command: "docker build -t panda_misra -f tests/misra/Dockerfile ." - - run: - name: Run Misra C 2012 test - command: | - mkdir /tmp/misra - docker run -v /tmp/misra:/tmp/misra panda_misra /bin/bash -c "cd /panda/tests/misra; ./test_misra.sh" - - store_artifacts: - name: Store cppcheck test output - path: /tmp/misra/cppcheck_output.txt - - store_artifacts: - name: Store misra test output - path: /tmp/misra/misra_output.txt - - store_artifacts: - name: Store cppcheck safety test output - path: /tmp/misra/cppcheck_safety_output.txt - - store_artifacts: - name: Store misra safety test output - path: /tmp/misra/misra_safety_output.txt - - - strict-compiler: - machine: - docker_layer_caching: true - steps: - - checkout - - run: - name: Build image - command: "docker build -t panda_strict_compiler -f tests/build_strict/Dockerfile ." - - run: - name: Build Panda with strict compiler rules - command: | - docker run panda_strict_compiler /bin/bash -c "cd /panda/tests/build_strict; ./test_build_strict.sh" - build: machine: docker_layer_caching: true @@ -70,41 +29,25 @@ jobs: command: | docker run panda_build /bin/bash -c "cd /panda/board; make bin" - run: - name: Build Panda STM bootstub image + name: Build Honda Pedal STM image command: | - docker run panda_build /bin/bash -c "cd /panda/board; make obj/bootstub.panda.bin" + docker run panda_build /bin/bash -c "cd /panda/board/pedal_honda; make obj/comma.bin" - run: - name: Build Pedal STM image + name: Build Toyota Pedal STM image command: | - docker run panda_build /bin/bash -c "cd /panda/board/pedal; make obj/comma.bin" + docker run panda_build /bin/bash -c "cd /panda/board/pedal_toyota; make obj/comma.bin" - run: - name: Build Pedal STM bootstub image + name: Build NEO STM image command: | - docker run panda_build /bin/bash -c "cd /panda/board/pedal; make obj/bootstub.bin" + docker run panda_build /bin/bash -c "cd /panda/board; make clean; make -f Makefile.legacy obj/comma.bin" - run: name: Build ESP image command: | docker run panda_build /bin/bash -c "cd /panda/boardesp; make user1.bin" - safety_replay: - machine: - docker_layer_caching: true - steps: - - checkout - - run: - name: Build image - command: "docker build -t panda_safety_replay -f tests/safety_replay/Dockerfile ." - - run: - name: Replay drives - command: | - docker run panda_safety_replay /bin/bash -c "cd /openpilot/panda/tests/safety_replay; PYTHONPATH=/openpilot ./test_safety_replay.py" - workflows: version: 2 main: jobs: - safety - - misra-c2012 - - strict-compiler - build - - safety_replay diff --git a/panda/.gitignore b/panda/.gitignore index 70d010fabf3d7f..7b7762ef490663 100644 --- a/panda/.gitignore +++ b/panda/.gitignore @@ -10,6 +10,5 @@ a.out dist/ pandacan.egg-info/ board/obj/ -examples/output.csv -.DS_Store -nosetests.xml +examples/output.csv +.DS_Store diff --git a/panda/README.md b/panda/README.md index 42f432dfc32a38..e323d400e1fdec 100644 --- a/panda/README.md +++ b/panda/README.md @@ -3,7 +3,7 @@ Welcome to panda [panda](http://github.com/commaai/panda) is the nicest universal car interface ever. - + diff --git a/panda/VERSION b/panda/VERSION index ec7b9678297f1e..02e8c95da5fd8a 100644 --- a/panda/VERSION +++ b/panda/VERSION @@ -1 +1 @@ -v1.4.0 \ No newline at end of file +v1.1.8 \ No newline at end of file diff --git a/panda/board/README.md b/panda/board/README.md index 5fe2e4b238c2dd..b3be654be31ebe 100644 --- a/panda/board/README.md +++ b/panda/board/README.md @@ -23,6 +23,12 @@ Programming make ``` +**NEO** + +``` +make -f Makefile.legacy +``` + Troubleshooting ---- diff --git a/panda/board/bootstub.c b/panda/board/bootstub.c index 691d0d02e33cda..4e516110c5d4bb 100644 --- a/panda/board/bootstub.c +++ b/panda/board/bootstub.c @@ -12,14 +12,11 @@ #include "stm32f2xx_hal_gpio_ex.h" #endif -// default since there's no serial -void puts(const char *a) {} -void puth(unsigned int i) {} - #include "libc.h" #include "provision.h" -#include "drivers/clock.h" +#include "drivers/drivers.h" + #include "drivers/llgpio.h" #include "gpio.h" @@ -27,6 +24,15 @@ void puth(unsigned int i) {} #include "drivers/usb.h" //#include "drivers/uart.h" +#ifdef PEDAL +#define CUSTOM_CAN_INTERRUPTS +#include "safety.h" +#include "drivers/can.h" +#endif + +int puts(const char *a) { return 0; } +void puth(unsigned int i) {} + #include "crypto/rsa.h" #include "crypto/sha.h" @@ -45,9 +51,6 @@ void fail() { // know where to sig check extern void *_app_start[]; -// FIXME: sometimes your panda will fail flashing and will quickly blink a single Green LED -// BOUNTY: $200 coupon on shop.comma.ai or $100 check. - int main() { __disable_irq(); clock_init(); diff --git a/panda/board/build.mk b/panda/board/build.mk index af2bffeb0a4616..aee724c19044c1 100644 --- a/panda/board/build.mk +++ b/panda/board/build.mk @@ -1,18 +1,7 @@ -CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -Os +CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -O2 CFLAGS += -Tstm32_flash.ld -DFU_UTIL = "dfu-util" - -# Compile fast charge (DCP) only not on EON -ifeq (,$(wildcard /EON)) - BUILDER = DEV -else - CFLAGS += "-DEON" - BUILDER = EON - DFU_UTIL = "tools/dfu-util-aarch64" -endif - CC = arm-none-eabi-gcc OBJCOPY = arm-none-eabi-objcopy OBJDUMP = arm-none-eabi-objdump @@ -25,11 +14,7 @@ else CFLAGS += "-DALLOW_DEBUG" endif - -DEPDIR = generated_dependencies -$(shell mkdir -p -m 777 $(DEPDIR) >/dev/null) -DEPFLAGS = -MT $@ -MMD -MP -MF $(DEPDIR)/$*.Td -POSTCOMPILE = @mv -f $(DEPDIR)/$*.Td $(DEPDIR)/$*.d && touch $@ +DFU_UTIL = "dfu-util" # this no longer pushes the bootstub flash: obj/$(PROJ_NAME).bin @@ -52,9 +37,8 @@ include ../common/version.mk obj/cert.h: ../crypto/getcertheader.py ../crypto/getcertheader.py ../certs/debug.pub ../certs/release.pub > $@ -obj/%.$(PROJ_NAME).o: %.c obj/gitversion.h obj/cert.h $(DEPDIR)/%.d - $(CC) $(DEPFLAGS) $(CFLAGS) -o $@ -c $< - $(POSTCOMPILE) +obj/%.$(PROJ_NAME).o: %.c obj/cert.h obj/gitversion.h config.h drivers/*.h gpio.h libc.h provision.h safety.h safety/*.h spi_flasher.h + $(CC) $(CFLAGS) -o $@ -c $< obj/%.$(PROJ_NAME).o: ../crypto/%.c $(CC) $(CFLAGS) -o $@ -c $< @@ -67,18 +51,10 @@ obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o $(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^ $(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin SETLEN=1 ../crypto/sign.py obj/code.bin $@ $(CERT) - @BINSIZE=$$(du -b "obj/$(PROJ_NAME).bin" | cut -f 1) ; \ - if [ $$BINSIZE -ge 32768 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi; obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o $(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^ $(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@ -$(DEPDIR)/%.d: ; -.PRECIOUS: $(DEPDIR)/%.d - -include $(wildcard $(patsubst %,$(DEPDIR)/%.d,$(basename $(wildcard *.c)))) - clean: - @$(RM) obj/* - @rm -rf $(DEPDIR) + @rm -f obj/* diff --git a/panda/board/config.h b/panda/board/config.h index 219ce64c24a938..e83429980d4163 100644 --- a/panda/board/config.h +++ b/panda/board/config.h @@ -22,17 +22,17 @@ #include #define NULL ((void*)0) -#define COMPILE_TIME_ASSERT(pred) ((void)sizeof(char[1 - (2 * (!(pred)))])) +#define COMPILE_TIME_ASSERT(pred) switch(0){case 0:case pred:;} -#define MIN(a,b) \ +#define min(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ - (_a < _b) ? _a : _b; }) + _a < _b ? _a : _b; }) -#define MAX(a,b) \ +#define max(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ - (_a > _b) ? _a : _b; }) + _a > _b ? _a : _b; }) #define MAX_RESP_LEN 0x40 diff --git a/panda/board/drivers/adc.h b/panda/board/drivers/adc.h index 3e5f1b32a059c7..cb2aeede03958a 100644 --- a/panda/board/drivers/adc.h +++ b/panda/board/drivers/adc.h @@ -8,7 +8,7 @@ #define ADCCHAN_VOLTAGE 12 #define ADCCHAN_CURRENT 13 -void adc_init(void) { +void adc_init() { // global setup ADC->CCR = ADC_CCR_TSVREFE | ADC_CCR_VBATE; //ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_EOCS | ADC_CR2_DDS; diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h index 7d0e63058a5e98..b837094c9a0d38 100644 --- a/panda/board/drivers/can.h +++ b/panda/board/drivers/can.h @@ -1,34 +1,4 @@ -// IRQs: CAN1_TX, CAN1_RX0, CAN1_SCE -// CAN2_TX, CAN2_RX0, CAN2_SCE -// CAN3_TX, CAN3_RX0, CAN3_SCE - -typedef struct { - volatile uint32_t w_ptr; - volatile uint32_t r_ptr; - uint32_t fifo_size; - CAN_FIFOMailBox_TypeDef *elems; -} can_ring; - -#define CAN_BUS_RET_FLAG 0x80 -#define CAN_BUS_NUM_MASK 0x7F - -#define BUS_MAX 4 - -extern int can_live, pending_can_live; - -// must reinit after changing these -extern int can_loopback, can_silent; -extern uint32_t can_speed[4]; - -void can_set_forwarding(int from, int to); - -void can_init(uint8_t can_number); -void can_init_all(void); -void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number); -bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem); - -// end API - +// IRQs: CAN1_TX, CAN1_RX0, CAN1_SCE, CAN2_TX, CAN2_RX0, CAN2_SCE, CAN3_TX, CAN3_RX0, CAN3_SCE #define ALL_CAN_SILENT 0xFF #define ALL_CAN_BUT_MAIN_SILENT 0xFE #define ALL_CAN_LIVE 0 @@ -44,21 +14,19 @@ int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_S can_buffer(rx_q, 0x1000) can_buffer(tx1_q, 0x100) can_buffer(tx2_q, 0x100) -can_buffer(tx3_q, 0x100) -can_buffer(txgmlan_q, 0x100) -can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q}; -// global CAN stats -int can_rx_cnt = 0; -int can_tx_cnt = 0; -int can_txd_cnt = 0; -int can_err_cnt = 0; -int can_overflow_cnt = 0; +#ifdef PANDA + can_buffer(tx3_q, 0x100) + can_buffer(txgmlan_q, 0x100) + can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q}; +#else + can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q}; +#endif // ********************* interrupt safe queue ********************* -bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { - bool ret = 0; +int can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { + int ret = 0; enter_critical_section(); if (q->w_ptr != q->r_ptr) { @@ -85,12 +53,7 @@ int can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { ret = 1; } exit_critical_section(); - if (ret == 0) { - can_overflow_cnt++; - #ifdef DEBUG - puts("can_push failed!\n"); - #endif - } + if (ret == 0) puts("can_push failed!\n"); return ret; } @@ -110,54 +73,181 @@ void can_clear(can_ring *q) { // can_num_lookup: Translates from 'bus number' to 'can number'. // can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward. +int can_rx_cnt = 0; +int can_tx_cnt = 0; +int can_txd_cnt = 0; +int can_err_cnt = 0; + +// NEO: Bus 1=CAN1 Bus 2=CAN2 // Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3 -CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3}; -uint8_t bus_lookup[] = {0,1,2}; -uint8_t can_num_lookup[] = {0,1,2,-1}; -int8_t can_forwarding[] = {-1,-1,-1,-1}; -uint32_t can_speed[] = {5000, 5000, 5000, 333}; -#define CAN_MAX 3 +#ifdef PANDA + CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3}; + uint8_t bus_lookup[] = {0,1,2}; + uint8_t can_num_lookup[] = {0,1,2,-1}; + int8_t can_forwarding[] = {-1,-1,-1,-1}; + uint32_t can_speed[] = {5000, 5000, 5000, 333}; + bool can_autobaud_enabled[] = {false, false, false, false}; + #define CAN_MAX 3 +#else + CAN_TypeDef *cans[] = {CAN1, CAN2}; + uint8_t bus_lookup[] = {1,0}; + uint8_t can_num_lookup[] = {1,0}; + int8_t can_forwarding[] = {-1,-1}; + uint32_t can_speed[] = {5000, 5000}; + bool can_autobaud_enabled[] = {false, false}; + #define CAN_MAX 2 +#endif + +uint32_t can_autobaud_speeds[] = {5000, 2500, 1250, 1000, 10000}; +#define AUTOBAUD_SPEEDS_LEN (sizeof(can_autobaud_speeds) / sizeof(can_autobaud_speeds[0])) #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")) +#ifdef PANDA +#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")) +#else +#define CAN_NUM_FROM_CANIF(CAN) (CAN==CAN1 ? 0 : 1) +#define CAN_NAME_FROM_CANIF(CAN) (CAN==CAN1 ? "CAN1" : "CAN2") +#endif #define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num]) #define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num]) +// other option +/*#define CAN_QUANTA 16 +#define CAN_SEQ1 13 +#define CAN_SEQ2 2*/ + +// this is needed for 1 mbps support +#define CAN_QUANTA 8 +#define CAN_SEQ1 6 // roundf(quanta * 0.875f) - 1; +#define CAN_SEQ2 1 // roundf(quanta * 0.125f); + +#define CAN_PCLK 24000 +// 333 = 33.3 kbps +// 5000 = 500 kbps +#define can_speed_to_prescaler(x) (CAN_PCLK / CAN_QUANTA * 10 / (x)) + +void can_autobaud_speed_increment(uint8_t can_number) { + uint32_t autobaud_speed = can_autobaud_speeds[0]; + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + for (int i = 0; i < AUTOBAUD_SPEEDS_LEN; i++) { + if (can_speed[bus_number] == can_autobaud_speeds[i]) { + if (i+1 < AUTOBAUD_SPEEDS_LEN) { + autobaud_speed = can_autobaud_speeds[i+1]; + } + break; + } + } + can_speed[bus_number] = autobaud_speed; +#ifdef DEBUG + CAN_TypeDef* CAN = CANIF_FROM_CAN_NUM(can_number); + puts(CAN_NAME_FROM_CANIF(CAN)); + puts(" auto-baud test "); + putui(can_speed[bus_number]); + puts(" cbps\n"); +#endif +} + void process_can(uint8_t can_number); void can_set_speed(uint8_t can_number) { 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, can_silent & (1 << can_number))) { - puts("CAN init FAILED!!!!!\n"); - puth(can_number); puts(" "); - puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); + while (true) { + // initialization mode + CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ; + while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); + + // set time quanta from defines + CAN->BTR = (CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | + (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | + (can_speed_to_prescaler(can_speed[bus_number]) - 1); + + // silent loopback mode for debugging + if (can_loopback) { + CAN->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM; + } + if (can_silent & (1 << can_number)) { + CAN->BTR |= CAN_BTR_SILM; + } + + // reset + CAN->MCR = CAN_MCR_TTCM | CAN_MCR_ABOM; + + #define CAN_TIMEOUT 1000000 + int tmp = 0; + while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++; + if (tmp < CAN_TIMEOUT) { + return; + } + + if (can_autobaud_enabled[bus_number]) { + can_autobaud_speed_increment(can_number); + } else { + puts("CAN init FAILED!!!!!\n"); + puth(can_number); puts(" "); + puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); + return; + } } } void can_init(uint8_t can_number) { - if (can_number != 0xff) { - CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); - set_can_enable(CAN, 1); - can_set_speed(can_number); - - llcan_init(CAN); + if (can_number == 0xff) return; - // in case there are queued up messages - process_can(can_number); + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + set_can_enable(CAN, 1); + can_set_speed(can_number); + + // accept all filter + CAN->FMR |= CAN_FMR_FINIT; + + // no mask + CAN->sFilterRegister[0].FR1 = 0; + CAN->sFilterRegister[0].FR2 = 0; + CAN->sFilterRegister[14].FR1 = 0; + CAN->sFilterRegister[14].FR2 = 0; + CAN->FA1R |= 1 | (1 << 14); + + CAN->FMR &= ~(CAN_FMR_FINIT); + + // enable certain CAN interrupts + CAN->IER |= CAN_IER_TMEIE | CAN_IER_FMPIE0; + + switch (can_number) { + case 0: + NVIC_EnableIRQ(CAN1_TX_IRQn); + NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN1_SCE_IRQn); + break; + case 1: + NVIC_EnableIRQ(CAN2_TX_IRQn); + NVIC_EnableIRQ(CAN2_RX0_IRQn); + NVIC_EnableIRQ(CAN2_SCE_IRQn); + break; +#ifdef CAN3 + case 2: + NVIC_EnableIRQ(CAN3_TX_IRQn); + NVIC_EnableIRQ(CAN3_RX0_IRQn); + NVIC_EnableIRQ(CAN3_SCE_IRQn); + break; +#endif } + + // in case there are queued up messages + process_can(can_number); } -void can_init_all(void) { +void can_init_all() { for (int i=0; i < CAN_MAX; i++) { can_init(i); } } void can_set_gmlan(int bus) { - if ((bus == -1) || (bus != can_num_lookup[3])) { + #ifdef PANDA + if (bus == -1 || bus != can_num_lookup[3]) { // GMLAN OFF switch (can_num_lookup[3]) { case 1: @@ -176,9 +266,6 @@ void can_set_gmlan(int bus) { can_num_lookup[3] = -1; can_init(2); break; - default: - puts("GMLAN bus value invalid\n"); - break; } } @@ -190,7 +277,7 @@ void can_set_gmlan(int bus) { can_num_lookup[1] = -1; can_num_lookup[3] = 1; can_init(1); - } else if (bus == 2) { + } else if (bus == 2 && revision == PANDA_REV_C) { puts("GMLAN on CAN3\n"); // GMLAN on CAN3 set_can_mode(2, 1); @@ -199,12 +286,14 @@ void can_set_gmlan(int bus) { can_num_lookup[3] = 2; can_init(2); } + #endif } // CAN error void can_sce(CAN_TypeDef *CAN) { enter_critical_section(); + can_err_cnt += 1; #ifdef DEBUG if (CAN==CAN1) puts("CAN1: "); if (CAN==CAN2) puts("CAN2: "); @@ -224,66 +313,78 @@ void can_sce(CAN_TypeDef *CAN) { puts("\n"); #endif - can_err_cnt += 1; - llcan_clear_send(CAN); + uint8_t can_number = CAN_NUM_FROM_CANIF(CAN); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + if (can_autobaud_enabled[bus_number] && (CAN->ESR & CAN_ESR_LEC)) { + can_autobaud_speed_increment(can_number); + can_set_speed(can_number); + } + + // clear current send + CAN->TSR |= CAN_TSR_ABRQ0; + CAN->MSR &= ~(CAN_MSR_ERRI); + CAN->MSR = CAN->MSR; + exit_critical_section(); } // ***************************** CAN ***************************** void process_can(uint8_t can_number) { - if (can_number != 0xff) { - - enter_critical_section(); - - CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); - uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - - // check for empty mailbox - CAN_FIFOMailBox_TypeDef to_send; - if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { - // add successfully transmitted message to my fifo - if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) { - can_txd_cnt += 1; - - if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) { - CAN_FIFOMailBox_TypeDef to_push; - to_push.RIR = CAN->sTxMailBox[0].TIR; - to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((CAN_BUS_RET_FLAG | bus_number) << 4); - to_push.RDLR = CAN->sTxMailBox[0].TDLR; - to_push.RDHR = CAN->sTxMailBox[0].TDHR; - can_push(&can_rx_q, &to_push); - } - - if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) { - #ifdef DEBUG - puts("CAN TX ERROR!\n"); - #endif - } - - if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) { - #ifdef DEBUG - puts("CAN TX ARBITRATION LOST!\n"); - #endif - } - - // clear interrupt - // careful, this can also be cleared by requesting a transmission - CAN->TSR |= CAN_TSR_RQCP0; + if (can_number == 0xff) return; + + enter_critical_section(); + + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + #ifdef DEBUG + puts("process CAN TX\n"); + #endif + + // check for empty mailbox + CAN_FIFOMailBox_TypeDef to_send; + if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { + // add successfully transmitted message to my fifo + if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) { + can_txd_cnt += 1; + + if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) { + CAN_FIFOMailBox_TypeDef to_push; + to_push.RIR = CAN->sTxMailBox[0].TIR; + to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((CAN_BUS_RET_FLAG | bus_number) << 4); + to_push.RDLR = CAN->sTxMailBox[0].TDLR; + to_push.RDHR = CAN->sTxMailBox[0].TDHR; + can_push(&can_rx_q, &to_push); } - if (can_pop(can_queues[bus_number], &to_send)) { - can_tx_cnt += 1; - // only send if we have received a packet - CAN->sTxMailBox[0].TDLR = to_send.RDLR; - CAN->sTxMailBox[0].TDHR = to_send.RDHR; - CAN->sTxMailBox[0].TDTR = to_send.RDTR; - CAN->sTxMailBox[0].TIR = to_send.RIR; + if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) { + #ifdef DEBUG + puts("CAN TX ERROR!\n"); + #endif } + + if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) { + #ifdef DEBUG + puts("CAN TX ARBITRATION LOST!\n"); + #endif + } + + // clear interrupt + // careful, this can also be cleared by requesting a transmission + CAN->TSR |= CAN_TSR_RQCP0; } - exit_critical_section(); + if (can_pop(can_queues[bus_number], &to_send)) { + can_tx_cnt += 1; + // only send if we have received a packet + CAN->sTxMailBox[0].TDLR = to_send.RDLR; + CAN->sTxMailBox[0].TDHR = to_send.RDHR; + CAN->sTxMailBox[0].TDTR = to_send.RDTR; + CAN->sTxMailBox[0].TIR = to_send.RIR; + } } + + exit_critical_section(); } // CAN receive handlers @@ -291,7 +392,17 @@ void process_can(uint8_t can_number) { void can_rx(uint8_t can_number) { CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) { + while (CAN->RF0R & CAN_RF0R_FMP0) { + if (can_autobaud_enabled[bus_number]) { + can_autobaud_enabled[bus_number] = false; + puts(CAN_NAME_FROM_CANIF(CAN)); + #ifdef DEBUG + puts(" auto-baud "); + putui(can_speed[bus_number]); + puts(" cbps\n"); + #endif + } + can_rx_cnt += 1; // can is live @@ -308,19 +419,23 @@ void can_rx(uint8_t can_number) { to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4); // forwarding (panda only) - int bus_fwd_num = (can_forwarding[bus_number] != -1) ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push); - if (bus_fwd_num != -1) { - CAN_FIFOMailBox_TypeDef to_send; - to_send.RIR = to_push.RIR | 1; // TXRQ - to_send.RDTR = to_push.RDTR; - to_send.RDLR = to_push.RDLR; - to_send.RDHR = to_push.RDHR; - can_send(&to_send, bus_fwd_num); - } + #ifdef PANDA + int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push); + if (bus_fwd_num != -1) { + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_push.RIR | 1; // TXRQ + to_send.RDTR = to_push.RDTR; + to_send.RDLR = to_push.RDLR; + to_send.RDHR = to_push.RDHR; + can_send(&to_send, bus_fwd_num); + } + #endif safety_rx_hook(&to_push); - set_led(LED_BLUE, 1); + #ifdef PANDA + set_led(LED_BLUE, 1); + #endif can_push(&can_rx_q, &to_push); // next @@ -328,27 +443,35 @@ void can_rx(uint8_t can_number) { } } -void CAN1_TX_IRQHandler(void) { process_can(0); } -void CAN1_RX0_IRQHandler(void) { can_rx(0); } -void CAN1_SCE_IRQHandler(void) { can_sce(CAN1); } +#ifndef CUSTOM_CAN_INTERRUPTS + +void CAN1_TX_IRQHandler() { process_can(0); } +void CAN1_RX0_IRQHandler() { can_rx(0); } +void CAN1_SCE_IRQHandler() { can_sce(CAN1); } -void CAN2_TX_IRQHandler(void) { process_can(1); } -void CAN2_RX0_IRQHandler(void) { can_rx(1); } -void CAN2_SCE_IRQHandler(void) { can_sce(CAN2); } +void CAN2_TX_IRQHandler() { process_can(1); } +void CAN2_RX0_IRQHandler() { can_rx(1); } +void CAN2_SCE_IRQHandler() { can_sce(CAN2); } -void CAN3_TX_IRQHandler(void) { process_can(2); } -void CAN3_RX0_IRQHandler(void) { can_rx(2); } -void CAN3_SCE_IRQHandler(void) { can_sce(CAN3); } +#ifdef CAN3 +void CAN3_TX_IRQHandler() { process_can(2); } +void CAN3_RX0_IRQHandler() { can_rx(2); } +void CAN3_SCE_IRQHandler() { can_sce(CAN3); } +#endif + +#endif void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) { - if (safety_tx_hook(to_push) != 0) { + if (safety_tx_hook(to_push) && !can_autobaud_enabled[bus_number]) { if (bus_number < BUS_MAX) { // add CAN packet to send queue // bus number isn't passed through to_push->RDTR &= 0xF; - if ((bus_number == 3) && (can_num_lookup[3] == 0xFF)) { + if (bus_number == 3 && can_num_lookup[3] == 0xFF) { + #ifdef PANDA // TODO: why uint8 bro? only int8? bitbang_gmlan(to_push); + #endif } else { can_push(can_queues[bus_number], to_push); process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); @@ -360,4 +483,3 @@ void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) { void can_set_forwarding(int from, int to) { can_forwarding[from] = to; } - diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h index 357cffcd9984cb..8521100a88083a 100644 --- a/panda/board/drivers/gmlan_alt.h +++ b/panda/board/drivers/gmlan_alt.h @@ -9,7 +9,7 @@ #define MAX_BITS_CAN_PACKET (200) -int gmlan_alt_mode = DISABLED; +int gmlan_alt_mode = DISABLED; // returns out_len int do_bitstuff(char *out, char *in, int in_len) { @@ -18,8 +18,7 @@ int do_bitstuff(char *out, char *in, int in_len) { int j = 0; for (int i = 0; i < in_len; i++) { char bit = in[i]; - out[j] = bit; - j++; + out[j++] = bit; // do the stuffing if (bit == last_bit) { @@ -27,8 +26,7 @@ int do_bitstuff(char *out, char *in, int in_len) { if (bit_cnt == 5) { // 5 in a row the same, do stuff last_bit = !bit; - out[j] = last_bit; - j++; + out[j++] = last_bit; bit_cnt = 1; } } else { @@ -44,30 +42,27 @@ int append_crc(char *in, int in_len) { int crc = 0; for (int i = 0; i < in_len; i++) { crc <<= 1; - if ((in[i] ^ ((crc >> 15) & 1)) != 0) { + if (in[i] ^ ((crc>>15)&1)) { crc = crc ^ 0x4599; } crc &= 0x7fff; } for (int i = 14; i >= 0; i--) { - in[in_len] = (crc>>i)&1; - in_len++; + in[in_len++] = (crc>>i)&1; } return in_len; } int append_bits(char *in, int in_len, char *app, int app_len) { for (int i = 0; i < app_len; i++) { - in[in_len] = app[i]; - in_len++; + in[in_len++] = app[i]; } return in_len; } int append_int(char *in, int in_len, int val, int val_len) { for (int i = val_len-1; i >= 0; i--) { - in[in_len] = (val&(1<RDTR & 0xF; len = append_int(pkt, len, 0, 1); // Start-of-frame - - if ((to_bang->RIR & 4) != 0) { + + if (to_bang->RIR & 4) { // extended identifier len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier len = append_int(pkt, len, 3, 2); // SRR+IDE @@ -119,7 +114,9 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) { return len; } -void setup_timer4(void) { +#ifdef PANDA + +void setup_timer4() { // setup TIM4->PSC = 48-1; // tick on 1 us TIM4->CR1 = TIM_CR1_CEN; // enable @@ -136,7 +133,7 @@ void setup_timer4(void) { int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms int can_timeout_counter = GMLAN_TICKS_PER_SECOND; //1 second -int inverted_bit_to_send = GMLAN_HIGH; +int inverted_bit_to_send = GMLAN_HIGH; int gmlan_switch_below_timeout = -1; int gmlan_switch_timeout_enable = 0; @@ -145,9 +142,9 @@ void gmlan_switch_init(int timeout_enable) { gmlan_alt_mode = GPIO_SWITCH; gmlan_switch_below_timeout = 1; set_gpio_mode(GPIOB, 13, MODE_OUTPUT); - + setup_timer4(); - + inverted_bit_to_send = GMLAN_LOW; //We got initialized, set the output low } @@ -167,7 +164,7 @@ void reset_gmlan_switch_timeout(void) { } void set_bitbanged_gmlan(int val) { - if (val != 0) { + if (val) { GPIOB->ODR |= (1 << 13); } else { GPIOB->ODR &= ~(1 << 13); @@ -185,7 +182,7 @@ int gmlan_fail_count = 0; void TIM4_IRQHandler(void) { if (gmlan_alt_mode == BITBANG) { - if ((TIM4->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) { + if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) { int read = get_gpio_input(GPIOB, 12); if (gmlan_silent_count < REQUIRED_SILENT_TIME) { if (read == 0) { @@ -194,16 +191,16 @@ void TIM4_IRQHandler(void) { gmlan_silent_count++; } } else if (gmlan_silent_count == REQUIRED_SILENT_TIME) { - bool retry = 0; + int retry = 0; // in send loop - if ((gmlan_sending > 0) && // not first bit - ((read == 0) && (pkt_stuffed[gmlan_sending-1] == 1)) && // bus wrongly dominant - (gmlan_sending != (gmlan_sendmax - 11))) { //not ack bit + if (gmlan_sending > 0 && // not first bit + (read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant + gmlan_sending != (gmlan_sendmax-11)) { //not ack bit puts("GMLAN ERR: bus driven at "); puth(gmlan_sending); puts("\n"); retry = 1; - } else if ((read == 1) && (gmlan_sending == (gmlan_sendmax - 11))) { // recessive during ACK + } else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK puts("GMLAN ERR: didn't recv ACK\n"); retry = 1; } @@ -221,7 +218,7 @@ void TIM4_IRQHandler(void) { gmlan_sending++; } } - if ((gmlan_sending == gmlan_sendmax) || (gmlan_fail_count == MAX_FAIL_COUNT)) { + if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) { set_bitbanged_gmlan(1); // recessive set_gpio_mode(GPIOB, 13, MODE_INPUT); TIM4->DIER = 0; // no update interrupt @@ -233,8 +230,8 @@ void TIM4_IRQHandler(void) { } //bit bang mode else if (gmlan_alt_mode == GPIO_SWITCH) { - if ((TIM4->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) { - if ((can_timeout_counter == 0) && gmlan_switch_timeout_enable) { + if (TIM4->SR & TIM_SR_UIF && gmlan_switch_below_timeout != -1) { + if (can_timeout_counter == 0 && gmlan_switch_timeout_enable) { //it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output set_gpio_output(GPIOB, 13, GMLAN_LOW); gmlan_switch_below_timeout = -1; @@ -261,19 +258,19 @@ void TIM4_IRQHandler(void) { void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) { gmlan_alt_mode = BITBANG; // TODO: make failure less silent - if (gmlan_sendmax == -1) { + if (gmlan_sendmax != -1) return; - int len = get_bit_message(pkt_stuffed, to_bang); - gmlan_fail_count = 0; - gmlan_silent_count = 0; - gmlan_sending = 0; - gmlan_sendmax = len; + int len = get_bit_message(pkt_stuffed, to_bang); + gmlan_fail_count = 0; + gmlan_silent_count = 0; + gmlan_sending = 0; + gmlan_sendmax = len; - // setup for bitbang loop - set_bitbanged_gmlan(1); // recessive - set_gpio_mode(GPIOB, 13, MODE_OUTPUT); + // setup for bitbang loop + set_bitbanged_gmlan(1); // recessive + set_gpio_mode(GPIOB, 13, MODE_OUTPUT); - setup_timer4(); - } + setup_timer4(); } +#endif diff --git a/panda/board/drivers/llgpio.h b/panda/board/drivers/llgpio.h index 172776eb35d3cc..e1835ec0f40c91 100644 --- a/panda/board/drivers/llgpio.h +++ b/panda/board/drivers/llgpio.h @@ -1,12 +1,3 @@ -#define MODE_INPUT 0 -#define MODE_OUTPUT 1 -#define MODE_ALTERNATE 2 -#define MODE_ANALOG 3 - -#define PULL_NONE 0 -#define PULL_UP 1 -#define PULL_DOWN 2 - void set_gpio_mode(GPIO_TypeDef *GPIO, int pin, int mode) { uint32_t tmp = GPIO->MODER; tmp &= ~(3 << (pin*2)); @@ -14,8 +5,8 @@ void set_gpio_mode(GPIO_TypeDef *GPIO, int pin, int mode) { GPIO->MODER = tmp; } -void set_gpio_output(GPIO_TypeDef *GPIO, int pin, bool enabled) { - if (enabled) { +void set_gpio_output(GPIO_TypeDef *GPIO, int pin, int val) { + if (val) { GPIO->ODR |= (1 << pin); } else { GPIO->ODR &= ~(1 << pin); @@ -39,6 +30,6 @@ void set_gpio_pullup(GPIO_TypeDef *GPIO, int pin, int mode) { } int get_gpio_input(GPIO_TypeDef *GPIO, int pin) { - return (GPIO->IDR & (1U << pin)) == (1U << pin); + return (GPIO->IDR & (1 << pin)) == (1 << pin); } diff --git a/panda/board/drivers/spi.h b/panda/board/drivers/spi.h index b61cd719a17514..7a5945d37257cb 100644 --- a/panda/board/drivers/spi.h +++ b/panda/board/drivers/spi.h @@ -1,16 +1,11 @@ // IRQs: DMA2_Stream2, DMA2_Stream3, EXTI4 -void spi_init(void); -int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out); - -// end API - #define SPI_BUF_SIZE 256 uint8_t spi_buf[SPI_BUF_SIZE]; int spi_buf_count = 0; int spi_total_count = 0; -void spi_init(void) { +void spi_init() { //puts("SPI init\n"); SPI1->CR1 = SPI_CR1_SPE; @@ -28,8 +23,8 @@ void spi_init(void) { // setup interrupt on falling edge of SPI enable (on PA4) SYSCFG->EXTICR[2] = SYSCFG_EXTICR2_EXTI4_PA; - EXTI->IMR |= (1 << 4); - EXTI->FTSR |= (1 << 4); + EXTI->IMR = (1 << 4); + EXTI->FTSR = (1 << 4); NVIC_EnableIRQ(EXTI4_IRQn); } @@ -113,12 +108,12 @@ void DMA2_Stream3_IRQHandler(void) { } void EXTI4_IRQHandler(void) { - volatile int pr = EXTI->PR & (1 << 4); + volatile int pr = EXTI->PR; #ifdef DEBUG_SPI puts("exti4\n"); #endif // SPI CS falling - if ((pr & (1 << 4)) != 0) { + if (pr & (1 << 4)) { spi_total_count = 0; spi_rx_dma(spi_buf, 0x14); } diff --git a/panda/board/drivers/uart.h b/panda/board/drivers/uart.h index f86c5f159307e8..5ca82888d4fe7a 100644 --- a/panda/board/drivers/uart.h +++ b/panda/board/drivers/uart.h @@ -1,27 +1,5 @@ // IRQs: USART1, USART2, USART3, UART5 -#define FIFO_SIZE 0x400 -typedef struct uart_ring { - volatile uint16_t w_ptr_tx; - volatile uint16_t r_ptr_tx; - uint8_t elems_tx[FIFO_SIZE]; - volatile uint16_t w_ptr_rx; - volatile uint16_t r_ptr_rx; - uint8_t elems_rx[FIFO_SIZE]; - USART_TypeDef *uart; - void (*callback)(struct uart_ring*); -} uart_ring; - -void uart_init(USART_TypeDef *u, int baud); - -bool getc(uart_ring *q, char *elem); -bool putc(uart_ring *q, char elem); - -void puts(const char *a); -void puth(unsigned int i); -void hexdump(const void *a, int l); - - // ***************************** serial port queues ***************************** // esp = USART1 @@ -50,25 +28,18 @@ uart_ring debug_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0, uart_ring *get_ring_by_number(int a) { - uart_ring *ring = NULL; switch(a) { case 0: - ring = &debug_ring; - break; + return &debug_ring; case 1: - ring = &esp_ring; - break; + return &esp_ring; case 2: - ring = &lin1_ring; - break; + return &lin1_ring; case 3: - ring = &lin2_ring; - break; + return &lin2_ring; default: - ring = NULL; - break; + return NULL; } - return ring; } // ***************************** serial port ***************************** @@ -79,32 +50,31 @@ void uart_ring_process(uart_ring *q) { int sr = q->uart->SR; if (q->w_ptr_tx != q->r_ptr_tx) { - if ((sr & USART_SR_TXE) != 0) { + if (sr & USART_SR_TXE) { q->uart->DR = q->elems_tx[q->r_ptr_tx]; q->r_ptr_tx = (q->r_ptr_tx + 1) % FIFO_SIZE; + } else { + // push on interrupt later + q->uart->CR1 |= USART_CR1_TXEIE; } - // there could be more to send - q->uart->CR1 |= USART_CR1_TXEIE; } else { // nothing to send q->uart->CR1 &= ~USART_CR1_TXEIE; } - if ((sr & USART_SR_RXNE) || (sr & USART_SR_ORE)) { + if (sr & USART_SR_RXNE || sr & USART_SR_ORE) { uint8_t c = q->uart->DR; // TODO: can drop packets if (q != &esp_ring) { uint16_t next_w_ptr = (q->w_ptr_rx + 1) % FIFO_SIZE; if (next_w_ptr != q->r_ptr_rx) { q->elems_rx[q->w_ptr_rx] = c; q->w_ptr_rx = next_w_ptr; - if (q->callback != NULL) { - q->callback(q); - } + if (q->callback) q->callback(q); } } } - if ((sr & USART_SR_ORE) != 0) { + if (sr & USART_SR_ORE) { // set dropped packet flag? } @@ -118,22 +88,22 @@ void USART2_IRQHandler(void) { uart_ring_process(&debug_ring); } void USART3_IRQHandler(void) { uart_ring_process(&lin2_ring); } void UART5_IRQHandler(void) { uart_ring_process(&lin1_ring); } -bool getc(uart_ring *q, char *elem) { - bool ret = false; +int getc(uart_ring *q, char *elem) { + int ret = 0; enter_critical_section(); if (q->w_ptr_rx != q->r_ptr_rx) { - if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx]; + *elem = q->elems_rx[q->r_ptr_rx]; q->r_ptr_rx = (q->r_ptr_rx + 1) % FIFO_SIZE; - ret = true; + ret = 1; } exit_critical_section(); return ret; } -bool injectc(uart_ring *q, char elem) { - int ret = false; +int injectc(uart_ring *q, char elem) { + int ret = 0; uint16_t next_w_ptr; enter_critical_section(); @@ -141,15 +111,15 @@ bool injectc(uart_ring *q, char elem) { if (next_w_ptr != q->r_ptr_rx) { q->elems_rx[q->w_ptr_rx] = elem; q->w_ptr_rx = next_w_ptr; - ret = true; + ret = 1; } exit_critical_section(); return ret; } -bool putc(uart_ring *q, char elem) { - bool ret = false; +int putc(uart_ring *q, char elem) { + int ret = 0; uint16_t next_w_ptr; enter_critical_section(); @@ -157,7 +127,7 @@ bool putc(uart_ring *q, char elem) { if (next_w_ptr != q->r_ptr_tx) { q->elems_tx[q->w_ptr_tx] = elem; q->w_ptr_tx = next_w_ptr; - ret = true; + ret = 1; } exit_critical_section(); @@ -166,24 +136,6 @@ bool putc(uart_ring *q, char elem) { return ret; } -void uart_flush(uart_ring *q) { - while (q->w_ptr_tx != q->r_ptr_tx) { - __WFI(); - } -} - -void uart_flush_sync(uart_ring *q) { - // empty the TX buffer - while (q->w_ptr_tx != q->r_ptr_tx) { - uart_ring_process(q); - } -} - -void uart_send_break(uart_ring *u) { - while ((u->uart->CR1 & USART_CR1_SBK) != 0); - u->uart->CR1 |= USART_CR1_SBK; -} - void clear_uart_buff(uart_ring *q) { enter_critical_section(); q->w_ptr_tx = 0; @@ -195,10 +147,10 @@ void clear_uart_buff(uart_ring *q) { // ***************************** start UART code ***************************** -#define __DIV(_PCLK_, _BAUD_) (((_PCLK_) * 25) / (4 * (_BAUD_))) -#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_)) / 100) -#define __DIVFRAQ(_PCLK_, _BAUD_) ((((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16) + 50) / 100) -#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4) | (__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F)) +#define __DIV(_PCLK_, _BAUD_) (((_PCLK_)*25)/(4*(_BAUD_))) +#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_))/100) +#define __DIVFRAQ(_PCLK_, _BAUD_) (((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16 + 50) / 100) +#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4)|(__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F)) void uart_set_baud(USART_TypeDef *u, int baud) { if (u == USART1) { @@ -212,19 +164,19 @@ void uart_set_baud(USART_TypeDef *u, int baud) { #define USART1_DMA_LEN 0x20 char usart1_dma[USART1_DMA_LEN]; -void uart_dma_drain(void) { +void uart_dma_drain() { uart_ring *q = &esp_ring; enter_critical_section(); - if ((DMA2->HISR & DMA_HISR_TCIF5) || (DMA2->HISR & DMA_HISR_HTIF5) || (DMA2_Stream5->NDTR != USART1_DMA_LEN)) { + if (DMA2->HISR & DMA_HISR_TCIF5 || DMA2->HISR & DMA_HISR_HTIF5 || DMA2_Stream5->NDTR != USART1_DMA_LEN) { // disable DMA q->uart->CR3 &= ~USART_CR3_DMAR; DMA2_Stream5->CR &= ~DMA_SxCR_EN; - while ((DMA2_Stream5->CR & DMA_SxCR_EN) != 0); + while (DMA2_Stream5->CR & DMA_SxCR_EN); - unsigned int i; - for (i = 0; i < (USART1_DMA_LEN - DMA2_Stream5->NDTR); i++) { + int i; + for (i = 0; i < USART1_DMA_LEN - DMA2_Stream5->NDTR; i++) { char c = usart1_dma[i]; uint16_t next_w_ptr = (q->w_ptr_rx + 1) % FIFO_SIZE; if (next_w_ptr != q->r_ptr_rx) { @@ -306,23 +258,22 @@ void putch(const char a) { } } -void puts(const char *a) { +int puts(const char *a) { for (;*a;a++) { if (*a == '\n') putch('\r'); putch(*a); } + return 0; } void putui(uint32_t i) { char str[11]; uint8_t idx = 10; - str[idx] = '\0'; - idx--; + str[idx--] = '\0'; do { - str[idx] = (i % 10) + 0x30; - idx--; + str[idx--] = (i % 10) + 0x30; i /= 10; - } while (i != 0); + } while (i); puts(str + idx + 1); } @@ -345,7 +296,7 @@ void puth2(unsigned int i) { void hexdump(const void *a, int l) { int i; for (i=0;i> 8) & 0xFF) + (num&0xFF), ((num>>8)&0xFF) // take in string length and return the first 2 bytes of a string descriptor #define STRING_DESCRIPTOR_HEADER(size)\ - (((((size) * 2) + 2) & 0xFF) | 0x0300) + (((size * 2 + 2)&0xFF) | 0x0300) uint8_t device_desc[] = { DSCR_DEVICE_LEN, USB_DESC_TYPE_DEVICE, //Length, Type @@ -216,10 +186,17 @@ uint16_t string_manufacturer_desc[] = { 'c', 'o', 'm', 'm', 'a', '.', 'a', 'i' }; +#ifdef PANDA uint16_t string_product_desc[] = { STRING_DESCRIPTOR_HEADER(5), 'p', 'a', 'n', 'd', 'a' }; +#else +uint16_t string_product_desc[] = { + STRING_DESCRIPTOR_HEADER(5), + 'N', 'E', 'O', 'v', '1' +}; +#endif // default serial number when we're not a panda uint16_t string_serial_desc[] = { @@ -233,6 +210,7 @@ uint16_t string_configuration_desc[] = { '0', '1' // "01" }; +#ifdef PANDA // WCID (auto install WinUSB driver) // https://github.com/pbatard/libwdi/wiki/WCID-Devices // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/winusb-installation#automatic-installation-of--winusb-without-an-inf-file @@ -382,6 +360,8 @@ uint8_t winusb_20_desc[WINUSB_PLATFORM_DESCRIPTOR_LENGTH] = { '1', 0x00, 'a', 0x00, 'd', 0x00, 'e', 0x00, '9', 0x00, '}', 0x00, 0x00, 0x00 // 78 bytes }; +#endif + // current packet USB_Setup_TypeDef setup; uint8_t usbdata[0x100]; @@ -397,10 +377,9 @@ void *USB_ReadPacket(void *dest, uint16_t len) { uint32_t i=0; uint32_t count32b = (len + 3) / 4; - for ( i = 0; i < count32b; i++) { + for ( i = 0; i < count32b; i++, dest += 4 ) { // packed? *(__attribute__((__packed__)) uint32_t *)dest = USBx_DFIFO(0); - dest += 4; } return ((void *)dest); } @@ -421,9 +400,8 @@ void USB_WritePacket(const uint8_t *src, uint16_t len, uint32_t ep) { USBx_INEP(ep)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA); // load the FIFO - for (i = 0; i < count32b; i++) { + for (i = 0; i < count32b; i++, src += 4) { USBx_DFIFO(ep) = *((__attribute__((__packed__)) uint32_t *)src); - src += 4; } } @@ -435,7 +413,7 @@ void USB_WritePacket_EP0(uint8_t *src, uint16_t len) { hexdump(src, len); #endif - uint16_t wplen = MIN(len, 0x40); + uint16_t wplen = min(len, 0x40); USB_WritePacket(src, wplen, 0); if (wplen < len) { @@ -447,7 +425,7 @@ void USB_WritePacket_EP0(uint8_t *src, uint16_t len) { } } -void usb_reset(void) { +void usb_reset() { // unmask endpoint interrupts, so many sets USBx_DEVICE->DAINT = 0xFFFFFFFF; USBx_DEVICE->DAINTMSK = 0xFFFFFFFF; @@ -491,16 +469,14 @@ void usb_reset(void) { } char to_hex_char(int a) { - char ret; if (a < 10) { - ret = '0' + a; + return '0' + a; } else { - ret = 'a' + (a - 10); + return 'a' + (a-10); } - return ret; } -void usb_setup(void) { +void usb_setup() { int resp_len; // setup packet is ready switch (setup.b.bRequest) { @@ -549,32 +525,32 @@ void usb_setup(void) { //puts(" writing device descriptor\n"); // setup transfer - USB_WritePacket(device_desc, MIN(sizeof(device_desc), setup.b.wLength.w), 0); + USB_WritePacket(device_desc, min(sizeof(device_desc), setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; //puts("D"); break; case USB_DESC_TYPE_CONFIGURATION: - USB_WritePacket(configuration_desc, MIN(sizeof(configuration_desc), setup.b.wLength.w), 0); + USB_WritePacket(configuration_desc, min(sizeof(configuration_desc), setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_DESC_TYPE_DEVICE_QUALIFIER: - USB_WritePacket(device_qualifier, MIN(sizeof(device_qualifier), setup.b.wLength.w), 0); + USB_WritePacket(device_qualifier, min(sizeof(device_qualifier), setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_DESC_TYPE_STRING: switch (setup.b.wValue.bw.msb) { case STRING_OFFSET_LANGID: - USB_WritePacket((uint8_t*)string_language_desc, MIN(sizeof(string_language_desc), setup.b.wLength.w), 0); + USB_WritePacket((uint8_t*)string_language_desc, min(sizeof(string_language_desc), setup.b.wLength.w), 0); break; case STRING_OFFSET_IMANUFACTURER: - USB_WritePacket((uint8_t*)string_manufacturer_desc, MIN(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0); + USB_WritePacket((uint8_t*)string_manufacturer_desc, min(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0); break; case STRING_OFFSET_IPRODUCT: - USB_WritePacket((uint8_t*)string_product_desc, MIN(sizeof(string_product_desc), setup.b.wLength.w), 0); + USB_WritePacket((uint8_t*)string_product_desc, min(sizeof(string_product_desc), setup.b.wLength.w), 0); break; case STRING_OFFSET_ISERIAL: - #ifdef UID_BASE + #ifdef PANDA resp[0] = 0x02 + 12*4; resp[1] = 0x03; @@ -587,17 +563,19 @@ void usb_setup(void) { resp[2 + i*4 + 3] = '\0'; } - USB_WritePacket(resp, MIN(resp[0], setup.b.wLength.w), 0); + USB_WritePacket(resp, min(resp[0], setup.b.wLength.w), 0); #else - USB_WritePacket((const uint8_t *)string_serial_desc, MIN(sizeof(string_serial_desc), setup.b.wLength.w), 0); + USB_WritePacket((const uint8_t *)string_serial_desc, min(sizeof(string_serial_desc), setup.b.wLength.w), 0); #endif break; + #ifdef PANDA case STRING_OFFSET_ICONFIGURATION: - USB_WritePacket((uint8_t*)string_configuration_desc, MIN(sizeof(string_configuration_desc), setup.b.wLength.w), 0); + USB_WritePacket((uint8_t*)string_configuration_desc, min(sizeof(string_configuration_desc), setup.b.wLength.w), 0); break; case 238: - USB_WritePacket((uint8_t*)string_238_desc, MIN(sizeof(string_238_desc), setup.b.wLength.w), 0); + USB_WritePacket((uint8_t*)string_238_desc, min(sizeof(string_238_desc), setup.b.wLength.w), 0); break; + #endif default: // nothing USB_WritePacket(0, 0, 0); @@ -605,10 +583,12 @@ void usb_setup(void) { } USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; + #ifdef PANDA case USB_DESC_TYPE_BINARY_OBJECT_STORE: - USB_WritePacket(binary_object_store_desc, MIN(sizeof(binary_object_store_desc), setup.b.wLength.w), 0); + USB_WritePacket(binary_object_store_desc, min(sizeof(binary_object_store_desc), setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; + #endif default: // nothing here? USB_WritePacket(0, 0, 0); @@ -629,10 +609,11 @@ void usb_setup(void) { USB_WritePacket(0, 0, 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; + #ifdef PANDA case WEBUSB_VENDOR_CODE: switch (setup.b.wIndex.w) { case WEBUSB_REQ_GET_URL: - USB_WritePacket(webusb_url_descriptor, MIN(sizeof(webusb_url_descriptor), setup.b.wLength.w), 0); + USB_WritePacket(webusb_url_descriptor, min(sizeof(webusb_url_descriptor), setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; default: @@ -646,28 +627,29 @@ void usb_setup(void) { switch (setup.b.wIndex.w) { // winusb 2.0 descriptor from BOS case WINUSB_REQ_GET_DESCRIPTOR: - USB_WritePacket_EP0((uint8_t*)winusb_20_desc, MIN(sizeof(winusb_20_desc), setup.b.wLength.w)); + USB_WritePacket_EP0((uint8_t*)winusb_20_desc, min(sizeof(winusb_20_desc), setup.b.wLength.w)); break; // Extended Compat ID OS Descriptor case WINUSB_REQ_GET_COMPATID_DESCRIPTOR: - USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, MIN(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w)); + USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, min(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w)); break; // Extended Properties OS Descriptor case WINUSB_REQ_GET_EXT_PROPS_OS: - USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, MIN(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w)); + USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, min(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w)); break; default: USB_WritePacket_EP0(0, 0); } break; + #endif default: resp_len = usb_cb_control_msg(&setup, resp, 1); - USB_WritePacket(resp, MIN(resp_len, setup.b.wLength.w), 0); + USB_WritePacket(resp, min(resp_len, setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; } } -void usb_init(void) { +void usb_init() { // full speed PHY, do reset and remove power down /*puth(USBx->GRSTCTL); puts(" resetting PHY\n");*/ @@ -760,27 +742,27 @@ void usb_irqhandler(void) { puts(" USB interrupt!\n"); #endif - if ((gintsts & USB_OTG_GINTSTS_CIDSCHG) != 0) { + if (gintsts & USB_OTG_GINTSTS_CIDSCHG) { puts("connector ID status change\n"); } - if ((gintsts & USB_OTG_GINTSTS_ESUSP) != 0) { + if (gintsts & USB_OTG_GINTSTS_ESUSP) { puts("ESUSP detected\n"); } - if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0) { + if (gintsts & USB_OTG_GINTSTS_USBRST) { puts("USB reset\n"); usb_reset(); } - if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0) { + if (gintsts & USB_OTG_GINTSTS_ENUMDNE) { puts("enumeration done"); // Full speed, ENUMSPD //puth(USBx_DEVICE->DSTS); puts("\n"); } - if ((gintsts & USB_OTG_GINTSTS_OTGINT) != 0) { + if (gintsts & USB_OTG_GINTSTS_OTGINT) { puts("OTG int:"); puth(USBx->GOTGINT); puts("\n"); @@ -790,7 +772,7 @@ void usb_irqhandler(void) { } // RX FIFO first - if ((gintsts & USB_OTG_GINTSTS_RXFLVL) != 0) { + if (gintsts & USB_OTG_GINTSTS_RXFLVL) { // 1. Read the Receive status pop register volatile unsigned int rxst = USBx->GRXSTSP; @@ -852,7 +834,7 @@ void usb_irqhandler(void) { USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGONAK | USB_OTG_DCTL_CGINAK; } - if ((gintsts & USB_OTG_GINTSTS_SRQINT) != 0) { + if (gintsts & USB_OTG_GINTSTS_SRQINT) { // we want to do "A-device host negotiation protocol" since we are the A-device /*puts("start request\n"); puth(USBx->GOTGCTL); @@ -863,7 +845,7 @@ void usb_irqhandler(void) { } // out endpoint hit - if ((gintsts & USB_OTG_GINTSTS_OEPINT) != 0) { + if (gintsts & USB_OTG_GINTSTS_OEPINT) { #ifdef DEBUG_USB puts(" 0:"); puth(USBx_OUTEP(0)->DOEPINT); @@ -878,7 +860,7 @@ void usb_irqhandler(void) { puts(" OUT ENDPOINT\n"); #endif - if ((USBx_OUTEP(2)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0) { + if (USBx_OUTEP(2)->DOEPINT & USB_OTG_DOEPINT_XFRC) { #ifdef DEBUG_USB puts(" OUT2 PACKET XFRC\n"); #endif @@ -886,32 +868,32 @@ void usb_irqhandler(void) { USBx_OUTEP(2)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; } - if ((USBx_OUTEP(3)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0) { + if (USBx_OUTEP(3)->DOEPINT & USB_OTG_DOEPINT_XFRC) { #ifdef DEBUG_USB puts(" OUT3 PACKET XFRC\n"); #endif USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40; USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; - } else if ((USBx_OUTEP(3)->DOEPINT & 0x2000) != 0) { + } else if (USBx_OUTEP(3)->DOEPINT & 0x2000) { #ifdef DEBUG_USB puts(" OUT3 PACKET WTF\n"); #endif // if NAK was set trigger this, unknown interrupt USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40; USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - } else if ((USBx_OUTEP(3)->DOEPINT) != 0) { + } else if (USBx_OUTEP(3)->DOEPINT) { puts("OUTEP3 error "); puth(USBx_OUTEP(3)->DOEPINT); puts("\n"); } - if ((USBx_OUTEP(0)->DOEPINT & USB_OTG_DIEPINT_XFRC) != 0) { + if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DIEPINT_XFRC) { // ready for next packet USBx_OUTEP(0)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) | (1 * 8); } // respond to setup packets - if ((USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) != 0) { + if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) { usb_setup(); } @@ -921,7 +903,7 @@ void usb_irqhandler(void) { } // interrupt endpoint hit (Page 1221) - if ((gintsts & USB_OTG_GINTSTS_IEPINT) != 0) { + if (gintsts & USB_OTG_GINTSTS_IEPINT) { #ifdef DEBUG_USB puts(" "); puth(USBx_INEP(0)->DIEPINT); @@ -936,8 +918,8 @@ void usb_irqhandler(void) { // No need to set NAK in OTG_DIEPCTL0 when nothing to send, // Appears USB core automatically sets NAK. WritePacket clears it. - // Handle the two interface alternate settings. Setting 0 has EP1 - // as bulk. Setting 1 has EP1 as interrupt. The code to handle + // Handle the two interface alternate settings. Setting 0 is has + // EP1 as bulk. Setting 1 has EP1 as interrupt. The code to handle // these two EP variations are very similar and can be // restructured for smaller code footprint. Keeping split out for // now for clarity. @@ -946,7 +928,7 @@ void usb_irqhandler(void) { switch (current_int0_alt_setting) { case 0: ////// Bulk config // *** IN token received when TxFIFO is empty - if ((USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) { + if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) { #ifdef DEBUG_USB puts(" IN PACKET QUEUE\n"); #endif @@ -957,7 +939,7 @@ void usb_irqhandler(void) { case 1: ////// Interrupt config // *** IN token received when TxFIFO is empty - if ((USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) { + if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) { #ifdef DEBUG_USB puts(" IN PACKET QUEUE\n"); #endif @@ -968,18 +950,15 @@ void usb_irqhandler(void) { } } break; - default: - puts("current_int0_alt_setting value invalid\n"); - break; } - if ((USBx_INEP(0)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) { + if (USBx_INEP(0)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) { #ifdef DEBUG_USB puts(" IN PACKET QUEUE\n"); #endif - if ((ep0_txlen != 0) && ((USBx_INEP(0)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40)) { - uint16_t len = MIN(ep0_txlen, 0x40); + if (ep0_txlen != 0 && (USBx_INEP(0)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40) { + uint16_t len = min(ep0_txlen, 0x40); USB_WritePacket(ep0_txdata, len, 0); ep0_txdata += len; ep0_txlen -= len; diff --git a/panda/board/gpio.h b/panda/board/gpio.h index 26a82d6275e05f..6112f6f887c703 100644 --- a/panda/board/gpio.h +++ b/panda/board/gpio.h @@ -1,5 +1,3 @@ -// this is last place with ifdef PANDA - #ifdef STM32F4 #include "stm32f4xx_hal_gpio_ex.h" #else @@ -13,25 +11,23 @@ #define PULL_EFFECTIVE_DELAY 10 -void puts(const char *a); - -bool has_external_debug_serial = 0; -bool is_giant_panda = 0; -bool is_entering_bootmode = 0; +int has_external_debug_serial = 0; +int is_giant_panda = 0; +int is_entering_bootmode = 0; int revision = PANDA_REV_AB; -bool is_grey_panda = 0; +int is_grey_panda = 0; -bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { +int detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { set_gpio_mode(GPIO, pin, MODE_INPUT); set_gpio_pullup(GPIO, pin, mode); for (volatile int i=0; iCR |= RCC_CR_HSEON; + while ((RCC->CR & RCC_CR_HSERDY) == 0); + + // divide shit + RCC->CFGR = RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4; + #ifdef PANDA + RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 | + RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE; + #else + #ifdef PEDAL + // comma pedal has a 16mhz crystal + RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 | + RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE; + #else + // NEO board has a 8mhz crystal + RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 | + RCC_PLLCFGR_PLLN_7 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLSRC_HSE; + #endif + #endif + + // start PLL + RCC->CR |= RCC_CR_PLLON; + while ((RCC->CR & RCC_CR_PLLRDY) == 0); + + // Configure Flash prefetch, Instruction cache, Data cache and wait state + // *** without this, it breaks *** + FLASH->ACR = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_5WS; + + // switch to PLL + RCC->CFGR |= RCC_CFGR_SW_PLL; + while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL); + + // *** running on PLL *** +} + +void periph_init() { // enable GPIOB, UART2, CAN, USB clock RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; @@ -84,22 +117,22 @@ void periph_init(void) { RCC->APB1ENR |= RCC_APB1ENR_CAN3EN; #endif RCC->APB1ENR |= RCC_APB1ENR_DACEN; - RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // main counter - RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // slow loop and pedal - RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // gmlan_alt - //RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; - //RCC->APB1ENR |= RCC_APB1ENR_TIM6EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; RCC->APB2ENR |= RCC_APB2ENR_USART1EN; RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN; - //RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; + RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; + + // needed? RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; } // ********************* setters ********************* -void set_can_enable(CAN_TypeDef *CAN, bool enabled) { +void set_can_enable(CAN_TypeDef *CAN, int enabled) { // enable CAN busses if (CAN == CAN1) { #ifdef PANDA @@ -141,16 +174,16 @@ void set_can_enable(CAN_TypeDef *CAN, bool enabled) { #endif void set_led(int led_num, int on) { - if (led_num != -1) { + if (led_num == -1) return; + #ifdef PANDA set_gpio_output(GPIOC, led_num, !on); #else set_gpio_output(GPIOB, led_num, !on); #endif - } } -void set_can_mode(int can, bool use_gmlan) { +void set_can_mode(int can, int use_gmlan) { // connects to CAN2 xcvr or GMLAN xcvr if (use_gmlan) { if (can == 1) { @@ -162,7 +195,7 @@ void set_can_mode(int can, bool use_gmlan) { set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2); set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2); #ifdef CAN3 - } else if (can == 2) { + } else if (revision == PANDA_REV_C && can == 2) { // A8,A15: disable normal mode set_gpio_mode(GPIOA, 8, MODE_INPUT); set_gpio_mode(GPIOA, 15, MODE_INPUT); @@ -183,9 +216,11 @@ void set_can_mode(int can, bool use_gmlan) { set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2); #ifdef CAN3 } else if (can == 2) { - // B3,B4: disable gmlan mode - set_gpio_mode(GPIOB, 3, MODE_INPUT); - set_gpio_mode(GPIOB, 4, MODE_INPUT); + if(revision == PANDA_REV_C){ + // B3,B4: disable gmlan mode + set_gpio_mode(GPIOB, 3, MODE_INPUT); + set_gpio_mode(GPIOB, 4, MODE_INPUT); + } // A8,A15: normal mode set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3); set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3); @@ -202,7 +237,6 @@ void set_can_mode(int can, bool use_gmlan) { int usb_power_mode = USB_POWER_NONE; void set_usb_power_mode(int mode) { - bool valid_mode = true; switch (mode) { case USB_POWER_CLIENT: // B2,A13: set client mode @@ -219,15 +253,8 @@ void set_usb_power_mode(int mode) { set_gpio_output(GPIOB, 2, 0); set_gpio_output(GPIOA, 13, 0); break; - default: - valid_mode = false; - puts("Invalid usb power mode\n"); - break; - } - - if (valid_mode) { - usb_power_mode = mode; } + usb_power_mode = mode; } #define ESP_DISABLED 0 @@ -250,16 +277,13 @@ void set_esp_mode(int mode) { set_gpio_output(GPIOC, 14, 1); set_gpio_output(GPIOC, 5, 0); break; - default: - puts("Invalid esp mode\n"); - break; } } // ********************* big init function ********************* // board specific -void gpio_init(void) { +void gpio_init() { // pull low to hold ESP in reset?? // enable OTG out tied to ground GPIOA->ODR = 0; @@ -351,7 +375,11 @@ void gpio_init(void) { #ifdef PANDA // K-line enable moved from B4->B7 to make room for GMLAN on CAN3 - set_gpio_output(GPIOB, 7, 1); // REV C + if (revision == PANDA_REV_C) { + set_gpio_output(GPIOB, 7, 1); // REV C + } else { + set_gpio_output(GPIOB, 4, 1); // REV AB + } // C12,D2: K-Line setup on UART 5 set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5); @@ -367,7 +395,9 @@ void gpio_init(void) { set_gpio_pullup(GPIOC, 11, PULL_UP); #endif - set_usb_power_mode(USB_POWER_CLIENT); + if (revision == PANDA_REV_C) { + set_usb_power_mode(USB_POWER_CLIENT); + } } // ********************* early bringup ********************* @@ -379,7 +409,7 @@ void gpio_init(void) { extern void *g_pfnVectors; extern uint32_t enter_bootloader_mode; -void jump_to_bootloader(void) { +void jump_to_bootloader() { // do enter bootloader enter_bootloader_mode = 0; void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004)); @@ -392,11 +422,11 @@ void jump_to_bootloader(void) { NVIC_SystemReset(); } -void early(void) { +void early() { // after it's been in the bootloader, things are initted differently, so we reset - if ((enter_bootloader_mode != BOOT_NORMAL) && - (enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) && - (enter_bootloader_mode != ENTER_SOFTLOADER_MAGIC)) { + if (enter_bootloader_mode != BOOT_NORMAL && + enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC && + enter_bootloader_mode != ENTER_SOFTLOADER_MAGIC) { enter_bootloader_mode = BOOT_NORMAL; NVIC_SystemReset(); } @@ -424,7 +454,6 @@ void early(void) { #ifdef PANDA // enable the ESP, disable ESP boot mode // unless we are on a giant panda, then there's no ESP - // dont disable on grey panda if (is_giant_panda) { set_esp_mode(ESP_DISABLED); } else { @@ -445,3 +474,4 @@ void early(void) { enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; } } + diff --git a/panda/board/libc.h b/panda/board/libc.h index 564d29cd893ca4..8d80a3aad855a9 100644 --- a/panda/board/libc.h +++ b/panda/board/libc.h @@ -1,12 +1,12 @@ -// **** libc **** +// **** shitty libc **** void delay(int a) { volatile int i; - for (i = 0; i < a; i++); + for (i=0;iCCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1; + TIM3->CCER = TIM_CCER_CC3E; + timer_init(TIM3, 10); +} + +void fan_set_speed(int fan_speed) { + TIM3->CCR3 = fan_speed; +} // ********************* serial debugging ********************* @@ -62,40 +68,10 @@ void debug_ring_callback(uart_ring *ring) { } } -// ***************************** started logic ***************************** - -bool is_gpio_started(void) { - // ignition is on PA1 - return (GPIOA->IDR & (1U << 1)) == 0; -} - -void EXTI1_IRQHandler(void) { - volatile int pr = EXTI->PR & (1U << 1); - if ((pr & (1U << 1)) != 0) { - #ifdef DEBUG - puts("got started interrupt\n"); - #endif - - // jenky debounce - delay(100000); - - // set power savings mode here - int power_save_state = is_gpio_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED; - set_power_save_state(power_save_state); - EXTI->PR = (1U << 1); - } -} - -void started_interrupt_init(void) { - SYSCFG->EXTICR[1] = SYSCFG_EXTICR1_EXTI1_PA; - EXTI->IMR |= (1U << 1); - EXTI->RTSR |= (1U << 1); - EXTI->FTSR |= (1U << 1); - NVIC_EnableIRQ(EXTI1_IRQn); -} - // ***************************** USB port ***************************** +int usb_live = 0; + int get_health_pkt(void *dat) { struct __attribute__((packed)) { uint32_t voltage; @@ -109,25 +85,40 @@ int get_health_pkt(void *dat) { //Voltage will be measured in mv. 5000 = 5V uint32_t voltage = adc_get(ADCCHAN_VOLTAGE); + if (revision == PANDA_REV_AB) { + //REVB has a 100, 27 (27/127) voltage divider + //Here is the calculation for the scale + //ADCV = VIN_S * (27/127) * (4095/3.3) + //RETVAL = ADCV * s = VIN_S*1000 + //s = 1000/((4095/3.3)*(27/127)) = 3.79053046 + + //Avoid needing floating point math + health->voltage = (voltage * 3791) / 1000; + } else { + //REVC has a 10, 1 (1/11) voltage divider + //Here is the calculation for the scale (s) + //ADCV = VIN_S * (1/11) * (4095/3.3) + //RETVAL = ADCV * s = VIN_S*1000 + //s = 1000/((4095/3.3)*(1/11)) = 8.8623046875 + + //Avoid needing floating point math + health->voltage = (voltage * 8862) / 1000; + } - // REVC has a 10, 1 (1/11) voltage divider - // Here is the calculation for the scale (s) - // ADCV = VIN_S * (1/11) * (4095/3.3) - // RETVAL = ADCV * s = VIN_S*1000 - // s = 1000/((4095/3.3)*(1/11)) = 8.8623046875 - - // Avoid needing floating point math - health->voltage = (voltage * 8862) / 1000; - +#ifdef PANDA health->current = adc_get(ADCCHAN_CURRENT); int safety_ignition = safety_ignition_hook(); if (safety_ignition < 0) { //Use the GPIO pin to determine ignition - health->started = is_gpio_started(); + health->started = (GPIOA->IDR & (1 << 1)) == 0; } else { //Current safety hooks want to determine ignition (ex: GM) health->started = safety_ignition; } +#else + health->current = 0; + health->started = (GPIOC->IDR & (1 << 13)) != 0; +#endif health->controls_allowed = controls_allowed; health->gas_interceptor_detected = gas_interceptor_detected; @@ -139,34 +130,28 @@ int get_health_pkt(void *dat) { return sizeof(*health); } -int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { - UNUSED(hardwired); +int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { + usb_live = 1; CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata; int ilen = 0; - while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) { - ilen++; - } + while (ilen < min(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) ilen++; return ilen*0x10; } // send on serial, first byte to select the ring -void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) { - UNUSED(hardwired); +void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) { + usb_live = 1; + if (len == 0) return; uart_ring *ur = get_ring_by_number(usbdata[0]); - if ((len != 0) && (ur != NULL)) { - if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) { - for (int i = 1; i < len; i++) { - while (!putc(ur, usbdata[i])) { - // wait - } - } - } + if (!ur) return; + if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) { + for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i])); } } // send on CAN -void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) { - UNUSED(hardwired); +void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) { + usb_live = 1; int dpkt = 0; for (dpkt = 0; dpkt < len; dpkt += 0x10) { uint32_t *tf = (uint32_t*)(&usbdata[dpkt]); @@ -183,14 +168,16 @@ void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) { } } -bool is_enumerated = 0; +int is_enumerated = 0; void usb_cb_enumeration_complete() { + usb_live = 1; puts("USB enumeration complete\n"); is_enumerated = 1; } -int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) { +int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { + usb_live = 1; int resp_len = 0; uart_ring *ur = NULL; int i; @@ -210,14 +197,16 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) break; // **** 0xd0: fetch serial number case 0xd0: - // addresses are OTP - if (setup->b.wValue.w == 1) { - memcpy(resp, (void *)0x1fff79c0, 0x10); - resp_len = 0x10; - } else { - get_provision_chunk(resp); - resp_len = PROVISION_CHUNK_LEN; - } + #ifdef PANDA + // addresses are OTP + if (setup->b.wValue.w == 1) { + memcpy(resp, (void *)0x1fff79c0, 0x10); + resp_len = 0x10; + } else { + get_provision_chunk(resp); + resp_len = PROVISION_CHUNK_LEN; + } + #endif break; // **** 0xd1: enter bootloader mode case 0xd1: @@ -236,18 +225,19 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; NVIC_SystemReset(); break; - default: - puts("Bootloader mode invalid\n"); - break; } break; // **** 0xd2: get health packet case 0xd2: resp_len = get_health_pkt(resp); break; + // **** 0xd3: set fan speed + case 0xd3: + fan_set_speed(setup->b.wValue.w); + break; // **** 0xd6: get version case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN); + COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN) memcpy(resp, gitversion, sizeof(gitversion)); resp_len = sizeof(gitversion)-1; break; @@ -279,43 +269,44 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) break; // **** 0xdb: set GMLAN multiplexing mode case 0xdb: - if (setup->b.wValue.w == 1) { - // GMLAN ON - if (setup->b.wIndex.w == 1) { - can_set_gmlan(1); - } else if (setup->b.wIndex.w == 2) { - can_set_gmlan(2); + #ifdef PANDA + if (setup->b.wValue.w == 1) { + // GMLAN ON + if (setup->b.wIndex.w == 1) { + can_set_gmlan(1); + } else if (setup->b.wIndex.w == 2) { + // might be ignored on rev b panda + can_set_gmlan(2); + } + } else { + can_set_gmlan(-1); } - } else { - can_set_gmlan(-1); - } + #endif break; // **** 0xdc: set safety mode case 0xdc: // this is the only way to leave silent mode // and it's blocked over WiFi // Allow ELM security mode to be set over wifi. - if (hardwired || (setup->b.wValue.w == SAFETY_NOOUTPUT) || (setup->b.wValue.w == SAFETY_ELM327)) { + if (hardwired || setup->b.wValue.w == SAFETY_NOOUTPUT || setup->b.wValue.w == SAFETY_ELM327) { safety_set_mode(setup->b.wValue.w, (int16_t)setup->b.wIndex.w); - if (safety_ignition_hook() != -1) { - // if the ignition hook depends on something other than the started GPIO - // we have to disable power savings (fix for GM and Tesla) - set_power_save_state(POWER_SAVE_STATUS_DISABLED); + switch (setup->b.wValue.w) { + case SAFETY_NOOUTPUT: + can_silent = ALL_CAN_SILENT; + break; + case SAFETY_ELM327: + can_silent = ALL_CAN_BUT_MAIN_SILENT; + can_autobaud_enabled[0] = false; + break; + default: + can_silent = ALL_CAN_LIVE; + can_autobaud_enabled[0] = false; + can_autobaud_enabled[1] = false; + #ifdef PANDA + can_autobaud_enabled[2] = false; + #endif + break; } - #ifndef EON - // always LIVE on EON - switch (setup->b.wValue.w) { - case SAFETY_NOOUTPUT: - can_silent = ALL_CAN_SILENT; - break; - case SAFETY_ELM327: - can_silent = ALL_CAN_BUT_MAIN_SILENT; - break; - default: - can_silent = ALL_CAN_LIVE; - break; - } - #endif can_init_all(); } break; @@ -323,37 +314,28 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) case 0xdd: // wValue = Can Bus Num to forward from // wIndex = Can Bus Num to forward to - if ((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w < BUS_MAX) && - (setup->b.wValue.w != setup->b.wIndex.w)) { // set forwarding + if (setup->b.wValue.w < BUS_MAX && setup->b.wIndex.w < BUS_MAX && + setup->b.wValue.w != setup->b.wIndex.w) { // set forwarding can_set_forwarding(setup->b.wValue.w, setup->b.wIndex.w & CAN_BUS_NUM_MASK); - } else if((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w == 0xFF)){ //Clear Forwarding + } else if(setup->b.wValue.w < BUS_MAX && setup->b.wIndex.w == 0xFF){ //Clear Forwarding can_set_forwarding(setup->b.wValue.w, -1); } break; // **** 0xde: set can bitrate case 0xde: if (setup->b.wValue.w < BUS_MAX) { + can_autobaud_enabled[setup->b.wValue.w] = false; can_speed[setup->b.wValue.w] = setup->b.wIndex.w; can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); } break; - // **** 0xdf: set long controls allowed - case 0xdf: - if (hardwired) { - long_controls_allowed = setup->b.wValue.w & 1; - } - break; // **** 0xe0: uart read case 0xe0: ur = get_ring_by_number(setup->b.wValue.w); - if (!ur) { - break; - } - if (ur == &esp_ring) { - uart_dma_drain(); - } + if (!ur) break; + if (ur == &esp_ring) uart_dma_drain(); // read - while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) && + while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) && getc(ur, (char*)&resp[resp_len])) { ++resp_len; } @@ -361,17 +343,13 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) // **** 0xe1: uart set baud rate case 0xe1: ur = get_ring_by_number(setup->b.wValue.w); - if (!ur) { - break; - } + if (!ur) break; uart_set_baud(ur->uart, setup->b.wIndex.w); break; // **** 0xe2: uart set parity case 0xe2: ur = get_ring_by_number(setup->b.wValue.w); - if (!ur) { - break; - } + if (!ur) break; switch (setup->b.wIndex.w) { case 0: // disable parity, 8-bit @@ -394,9 +372,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) // **** 0xe4: uart set baud rate extended case 0xe4: ur = get_ring_by_number(setup->b.wValue.w); - if (!ur) { - break; - } + if (!ur) break; uart_set_baud(ur->uart, (int)setup->b.wIndex.w*300); break; // **** 0xe5: set CAN loopback (for testing) @@ -406,25 +382,27 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) break; // **** 0xe6: set USB power case 0xe6: - if (setup->b.wValue.w == 1) { - puts("user setting CDP mode\n"); - set_usb_power_mode(USB_POWER_CDP); - } else if (setup->b.wValue.w == 2) { - puts("user setting DCP mode\n"); - set_usb_power_mode(USB_POWER_DCP); - } else { - puts("user setting CLIENT mode\n"); - set_usb_power_mode(USB_POWER_CLIENT); + if (revision == PANDA_REV_C) { + if (setup->b.wValue.w == 1) { + puts("user setting CDP mode\n"); + set_usb_power_mode(USB_POWER_CDP); + } else if (setup->b.wValue.w == 2) { + puts("user setting DCP mode\n"); + set_usb_power_mode(USB_POWER_DCP); + } else { + puts("user setting CLIENT mode\n"); + set_usb_power_mode(USB_POWER_CLIENT); + } } break; // **** 0xf0: do k-line wValue pulse on uart2 for Acura case 0xf0: if (setup->b.wValue.w == 1) { - GPIOC->ODR &= ~(1U << 10); + GPIOC->ODR &= ~(1 << 10); GPIOC->MODER &= ~GPIO_MODER_MODER10_1; GPIOC->MODER |= GPIO_MODER_MODER10_0; } else { - GPIOC->ODR &= ~(1U << 12); + GPIOC->ODR &= ~(1 << 12); GPIOC->MODER &= ~GPIO_MODER_MODER12_1; GPIOC->MODER |= GPIO_MODER_MODER12_0; } @@ -432,11 +410,11 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) for (i = 0; i < 80; i++) { delay(8000); if (setup->b.wValue.w == 1) { - GPIOC->ODR |= (1U << 10); - GPIOC->ODR &= ~(1U << 10); + GPIOC->ODR |= (1 << 10); + GPIOC->ODR &= ~(1 << 10); } else { - GPIOC->ODR |= (1U << 12); - GPIOC->ODR &= ~(1U << 12); + GPIOC->ODR |= (1 << 12); + GPIOC->ODR &= ~(1 << 12); } } @@ -464,7 +442,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) case 0xf2: { uart_ring * rb = get_ring_by_number(setup->b.wValue.w); - if (rb != NULL) { + if (rb) { puts("Clearing UART queue.\n"); clear_uart_buff(rb); } @@ -479,11 +457,12 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) return resp_len; } +#ifdef PANDA int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) { // data[0] = endpoint // data[2] = length // data[4:] = data - UNUSED(len); + int resp_len = 0; switch (data[0]) { case 0: @@ -502,122 +481,41 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) { // ep 3, send CAN usb_cb_ep3_out(data+4, data[2], 0); break; - default: - puts("SPI data invalid"); - break; } return resp_len; } +#else -// ***************************** main code ***************************** +int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) { return 0; }; -void __initialize_hardware_early(void) { - early(); -} +#endif -void __attribute__ ((noinline)) enable_fpu(void) { - // enable the FPU - SCB->CPACR |= ((3UL << (10U * 2)) | (3UL << (11U * 2))); +// allow safety_forward to enable sending can messages +void safety_cb_enable_all() { + // allow sending can messages + can_silent = ALL_CAN_LIVE; + can_autobaud_enabled[0] = false; + can_autobaud_enabled[1] = false; + #ifdef PANDA + can_autobaud_enabled[2] = false; + #endif + can_init_all(); } -uint64_t tcnt = 0; -uint64_t marker = 0; - -// called once per second -void TIM3_IRQHandler(void) { - #define CURRENT_THRESHOLD 0xF00 - #define CLICKS 5 // 5 seconds to switch modes - - if (TIM3->SR != 0) { - can_live = pending_can_live; - - //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n"); - - uint32_t current = adc_get(ADCCHAN_CURRENT); - - switch (usb_power_mode) { - case USB_POWER_CLIENT: - if ((tcnt-marker) >= CLICKS) { - if (!is_enumerated) { - puts("USBP: didn't enumerate, switching to CDP mode\n"); - // switch to CDP - set_usb_power_mode(USB_POWER_CDP); - marker = tcnt; - } - } - // keep resetting the timer if it's enumerated - if (is_enumerated) { - marker = tcnt; - } - break; - case USB_POWER_CDP: - // On the EON, if we get into CDP mode we stay here. No need to go to DCP. - #ifndef EON - // been CLICKS clicks since we switched to CDP - if ((tcnt-marker) >= CLICKS) { - // measure current draw, if positive and no enumeration, switch to DCP - if (!is_enumerated && (current < CURRENT_THRESHOLD)) { - puts("USBP: no enumeration with current draw, switching to DCP mode\n"); - set_usb_power_mode(USB_POWER_DCP); - marker = tcnt; - } - } - // keep resetting the timer if there's no current draw in CDP - if (current >= CURRENT_THRESHOLD) { - marker = tcnt; - } - #endif - break; - case USB_POWER_DCP: - // been at least CLICKS clicks since we switched to DCP - if ((tcnt-marker) >= CLICKS) { - // if no current draw, switch back to CDP - if (current >= CURRENT_THRESHOLD) { - puts("USBP: no current draw, switching back to CDP mode\n"); - set_usb_power_mode(USB_POWER_CDP); - marker = tcnt; - } - } - // keep resetting the timer if there's current draw in DCP - if (current < CURRENT_THRESHOLD) { - marker = tcnt; - } - break; - default: - puts("USB power mode invalid\n"); // set_usb_power_mode prevents assigning invalid values - break; - } - - // ~0x9a = 500 ma - /*puth(current); - puts("\n");*/ - - // reset this every 16th pass - if ((tcnt&0xF) == 0) { - pending_can_live = 0; - } - #ifdef DEBUG - puts("** blink "); - puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" "); - puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" "); - puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n"); - #endif - // set green LED to be controls allowed - set_led(LED_GREEN, controls_allowed); +// ***************************** main code ***************************** - // turn off the blue LED, turned on by CAN - // unless we are in power saving mode - set_led(LED_BLUE, (tcnt & 1) && (power_save_status == POWER_SAVE_STATUS_ENABLED)); +void __initialize_hardware_early() { + early(); +} - // on to the next one - tcnt += 1; - } - TIM3->SR = 0; +void __attribute__ ((noinline)) enable_fpu() { + // enable the FPU + SCB->CPACR |= ((3UL << 10*2) | (3UL << 11*2)); } -int main(void) { +int main() { // shouldn't have interrupts here, but just in case __disable_irq(); @@ -631,21 +529,21 @@ int main(void) { // detect the revision and init the GPIOs puts("config:\n"); - puts((revision == PANDA_REV_C) ? " panda rev c\n" : " panda rev a or b\n"); + #ifdef PANDA + puts(revision == PANDA_REV_C ? " panda rev c\n" : " panda rev a or b\n"); + #else + puts(" legacy\n"); + #endif puts(has_external_debug_serial ? " real serial\n" : " USB serial\n"); puts(is_giant_panda ? " GIANTpanda detected\n" : " not GIANTpanda\n"); puts(is_grey_panda ? " gray panda detected!\n" : " white panda\n"); puts(is_entering_bootmode ? " ESP wants bootmode\n" : " no bootmode\n"); - - // non rev c panda are no longer supported - while (revision != PANDA_REV_C) { - // hang - } - gpio_init(); +#ifdef PANDA // panda has an FPU, let's use it! enable_fpu(); +#endif // enable main uart if it's connected if (has_external_debug_serial) { @@ -654,18 +552,19 @@ int main(void) { uart_init(USART2, 115200); } +#ifdef PANDA if (is_grey_panda) { uart_init(USART1, 9600); } else { // enable ESP uart uart_init(USART1, 115200); } - // enable LIN uart_init(UART5, 10400); UART5->CR2 |= USART_CR2_LINEN; uart_init(USART3, 10400); USART3->CR2 |= USART_CR2_LINEN; +#endif // init microsecond system timer // increments 1000000 times per second @@ -679,69 +578,143 @@ int main(void) { usb_init(); // default to silent mode to prevent issues with Ford - // hardcode a specific safety mode if you want to force the panda to be in a specific mode safety_set_mode(SAFETY_NOOUTPUT, 0); -#ifdef EON - // if we're on an EON, it's fine for CAN to be live for fingerprinting - can_silent = ALL_CAN_LIVE; -#else can_silent = ALL_CAN_SILENT; -#endif can_init_all(); adc_init(); -#ifndef EON +#ifdef PANDA spi_init(); #endif -#ifdef EON - // have to save power - if (!is_grey_panda) { - set_esp_mode(ESP_DISABLED); - } - // only enter power save after the first cycle - /*if (is_gpio_started()) { - set_power_save_state(POWER_SAVE_STATUS_ENABLED); - }*/ - // interrupt on started line - started_interrupt_init(); -#endif + // set PWM + fan_init(); + fan_set_speed(0); - // 48mhz / 65536 ~= 732 / 732 = 1 - timer_init(TIM3, 732); - NVIC_EnableIRQ(TIM3_IRQn); + puts("**** INTERRUPTS ON ****\n"); -#ifdef DEBUG - puts("DEBUG ENABLED\n"); -#endif + __enable_irq(); - puts("**** INTERRUPTS ON ****\n"); - enable_interrupts(); + // if the error interrupt is enabled to quickly when the CAN bus is active + // something bad happens and you can't connect to the device over USB + delay(10000000); + CAN1->IER |= CAN_IER_ERRIE | CAN_IER_LECIE; // LED should keep on blinking all the time uint64_t cnt = 0; + #ifdef PANDA + uint64_t marker = 0; + #define CURRENT_THRESHOLD 0xF00 + #define CLICKS 8 + // Enough clicks to ensure that enumeration happened. Should be longer than bootup time of the device connected to EON + #define CLICKS_BOOTUP 30 + #endif + for (cnt=0;;cnt++) { - if (power_save_status == POWER_SAVE_STATUS_DISABLED) { - int div_mode = ((usb_power_mode == USB_POWER_DCP) ? 4 : 1); - - // useful for debugging, fade breaks = panda is overloaded - for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) { - for (int fade = 0; fade < 1024; fade += 8) { - for (int i = 0; i < (128/div_mode); i++) { - set_led(LED_RED, 1); - if (fade < 512) { delay(fade); } else { delay(1024-fade); } - set_led(LED_RED, 0); - if (fade < 512) { delay(512-fade); } else { delay(fade-512); } + can_live = pending_can_live; + + //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n"); + + #ifdef PANDA + int current = adc_get(ADCCHAN_CURRENT); + + switch (usb_power_mode) { + case USB_POWER_CLIENT: + if ((cnt-marker) >= CLICKS) { + if (!is_enumerated) { + puts("USBP: didn't enumerate, switching to CDP mode\n"); + // switch to CDP + set_usb_power_mode(USB_POWER_CDP); + marker = cnt; + } + } + // keep resetting the timer if it's enumerated + if (is_enumerated) { + marker = cnt; } + break; + case USB_POWER_CDP: + // been CLICKS_BOOTUP clicks since we switched to CDP + if ((cnt-marker) >= CLICKS_BOOTUP ) { + // measure current draw, if positive and no enumeration, switch to DCP + if (!is_enumerated && current < CURRENT_THRESHOLD) { + puts("USBP: no enumeration with current draw, switching to DCP mode\n"); + set_usb_power_mode(USB_POWER_DCP); + marker = cnt; + } + } + // keep resetting the timer if there's no current draw in CDP + if (current >= CURRENT_THRESHOLD) { + marker = cnt; + } + break; + case USB_POWER_DCP: + // been at least CLICKS clicks since we switched to DCP + if ((cnt-marker) >= CLICKS) { + // if no current draw, switch back to CDP + if (current >= CURRENT_THRESHOLD) { + puts("USBP: no current draw, switching back to CDP mode\n"); + set_usb_power_mode(USB_POWER_CDP); + marker = cnt; + } + } + // keep resetting the timer if there's current draw in DCP + if (current < CURRENT_THRESHOLD) { + marker = cnt; + } + break; + } + + // ~0x9a = 500 ma + /*puth(current); + puts("\n");*/ + #endif + + // reset this every 16th pass + if ((cnt&0xF) == 0) pending_can_live = 0; + + // reset this every 2nd pass + if ((cnt&0x2) == 0) { + // check if usb connection is active, attempt forwarding if not + if (usb_live == 0 && current_safety_mode != SAFETY_FORWARD) { + safety_set_mode(SAFETY_FORWARD, 0); + // leave can_silent in it's current state. Fingerprinting will work with ALL_CAN_SILENT + can_init_all(); + } + usb_live = 0; + } + + #ifdef DEBUG + puts("** blink "); + puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" "); + puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" "); + puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n"); + #endif + + // set green LED to be controls allowed + set_led(LED_GREEN, controls_allowed); + + // blink the red LED + int div_mode = ((usb_power_mode == USB_POWER_DCP) ? 4 : 1); + + for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) { + for (int fade = 0; fade < 1024; fade += 8) { + for (int i = 0; i < 128/div_mode; i++) { + set_led(LED_RED, 0); + if (fade < 512) { delay(512-fade); } else { delay(fade-512); } + set_led(LED_RED, 1); + if (fade < 512) { delay(fade); } else { delay(1024-fade); } } } - } else { - __WFI(); } + + // turn off the blue LED, turned on by CAN + #ifdef PANDA + set_led(LED_BLUE, 0); + #endif } return 0; } - diff --git a/panda/board/pedal/Makefile b/panda/board/pedal/Makefile index 37b95f90fb1880..9917e381503b03 100644 --- a/panda/board/pedal/Makefile +++ b/panda/board/pedal/Makefile @@ -30,20 +30,12 @@ recover: obj/bootstub.bin obj/$(PROJ_NAME).bin $(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.bin -include ../../common/version.mk - -obj/cert.h: ../../crypto/getcertheader.py - ../../crypto/getcertheader.py ../../certs/debug.pub ../../certs/release.pub > $@ - obj/main.o: main.c ../*.h mkdir -p obj $(CC) $(CFLAGS) -o $@ -c $< -obj/bootstub.o: ../bootstub.c ../*.h obj/gitversion.h obj/cert.h +obj/bootstub.o: ../bootstub.c ../*.h mkdir -p obj - mkdir -p ../obj - cp obj/gitversion.h ../obj/gitversion.h - cp obj/cert.h ../obj/cert.h $(CC) $(CFLAGS) -o $@ -c $< obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 4656db283160b4..5d5264791ccf54 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -1,26 +1,29 @@ +//#define DEBUG +//#define CAN_LOOPBACK_MODE +//#define USE_INTERNAL_OSC + #include "../config.h" -#include "drivers/llcan.h" +#include "drivers/drivers.h" #include "drivers/llgpio.h" -#include "drivers/clock.h" +#include "gpio.h" + +#define CUSTOM_CAN_INTERRUPTS + +#include "libc.h" +#include "safety.h" #include "drivers/adc.h" +#include "drivers/uart.h" #include "drivers/dac.h" +#include "drivers/can.h" #include "drivers/timer.h" -#include "gpio.h" -#include "libc.h" - #define CAN CAN1 //#define PEDAL_USB #ifdef PEDAL_USB - #include "drivers/uart.h" #include "drivers/usb.h" -#else - // no serial either - void puts(const char *a) {} - void puth(unsigned int i) {} #endif #define ENTER_BOOTLOADER_MAGIC 0xdeadbeef @@ -32,8 +35,6 @@ void __initialize_hardware_early() { // ********************* serial debugging ********************* -#ifdef PEDAL_USB - void debug_ring_callback(uart_ring *ring) { char rcv; while (getc(ring, &rcv)) { @@ -41,12 +42,14 @@ void debug_ring_callback(uart_ring *ring) { } } -int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { return 0; } -void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {} -void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {} +#ifdef PEDAL_USB + +int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; } +void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {} +void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {} void usb_cb_enumeration_complete() {} -int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) { +int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { int resp_len = 0; uart_ring *ur = NULL; switch (setup->b.bRequest) { @@ -56,7 +59,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) if (!ur) break; if (ur == &esp_ring) uart_dma_drain(); // read - while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) && + while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) && getc(ur, (char*)&resp[resp_len])) { ++resp_len; } @@ -67,24 +70,21 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) #endif -// ***************************** pedal can checksum ***************************** - -uint8_t pedal_checksum(uint8_t *dat, int len) { - uint8_t crc = 0xFF; - uint8_t poly = 0xD5; // standard crc8 - int i, j; - for (i = len - 1; i >= 0; i--) { - crc ^= dat[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80) != 0) { - crc = (uint8_t)((crc << 1) ^ poly); - } - else { - crc <<= 1; - } - } +// ***************************** honda can checksum ***************************** + +int can_cksum(uint8_t *dat, int len, int addr, int idx) { + int i; + int s = 0; + for (i = 0; i < len; i++) { + s += (dat[i] >> 4); + s += dat[i] & 0xF; } - return crc; + s += (addr>>0)&0xF; + s += (addr>>4)&0xF; + s += (addr>>8)&0xF; + s += idx; + s = 8-s; + return s&0xF; } // ***************************** can port ***************************** @@ -92,8 +92,6 @@ uint8_t pedal_checksum(uint8_t *dat, int len) { // addresses to be used on CAN #define CAN_GAS_INPUT 0x200 #define CAN_GAS_OUTPUT 0x201 -#define CAN_GAS_SIZE 6 -#define COUNTER_CYCLE 0xF void CAN1_TX_IRQHandler() { // clear interrupt @@ -136,19 +134,14 @@ void CAN1_RX0_IRQHandler() { } // normal packet - uint8_t dat[8]; - uint8_t *rdlr = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR; - uint8_t *rdhr = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR; - for (int i=0; i<4; i++) { - dat[i] = rdlr[i]; - dat[i+4] = rdhr[i]; - } + uint8_t *dat = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR; + uint8_t *dat2 = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR; uint16_t value_0 = (dat[0] << 8) | dat[1]; uint16_t value_1 = (dat[2] << 8) | dat[3]; - uint8_t enable = (dat[4] >> 7) & 1; - uint8_t index = dat[4] & COUNTER_CYCLE; - if (pedal_checksum(dat, CAN_GAS_SIZE - 1) == dat[5]) { - if (((current_index + 1) & COUNTER_CYCLE) == index) { + uint8_t enable = (dat2[0] >> 7) & 1; + uint8_t index = (dat2[1] >> 4) & 3; + if (can_cksum(dat, 5, CAN_GAS_INPUT, index) == (dat2[1] & 0xF)) { + if (((current_index+1)&3) == index) { #ifdef DEBUG puts("setting gas "); puth(value); @@ -182,7 +175,7 @@ void CAN1_RX0_IRQHandler() { void CAN1_SCE_IRQHandler() { state = FAULT_SCE; - llcan_clear_send(CAN); + can_sce(CAN); } int pdl0 = 0, pdl1 = 0; @@ -203,18 +196,18 @@ void TIM3_IRQHandler() { // check timer for sending the user pedal and clearing the CAN if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { uint8_t dat[8]; - dat[0] = (pdl0>>8) & 0xFF; - dat[1] = (pdl0>>0) & 0xFF; - dat[2] = (pdl1>>8) & 0xFF; - dat[3] = (pdl1>>0) & 0xFF; - dat[4] = (state & 0xF) << 4 | pkt_idx; - dat[5] = pedal_checksum(dat, CAN_GAS_SIZE - 1); + dat[0] = (pdl0>>8)&0xFF; + dat[1] = (pdl0>>0)&0xFF; + dat[2] = (pdl1>>8)&0xFF; + dat[3] = (pdl1>>0)&0xFF; + dat[4] = state; + dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT, pkt_idx) | (pkt_idx<<4); CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24); CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5]<<8); CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5 CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1; ++pkt_idx; - pkt_idx &= COUNTER_CYCLE; + pkt_idx &= 3; } else { // old can packet hasn't sent! state = FAULT_SEND; @@ -246,14 +239,15 @@ void pedal() { // write the pedal to the DAC if (state == NO_FAULT) { - dac_set(0, MAX(gas_set_0, pdl0)); - dac_set(1, MAX(gas_set_1, pdl1)); + dac_set(0, max(gas_set_0, pdl0)); + dac_set(1, max(gas_set_1, pdl1)); } else { dac_set(0, pdl0); dac_set(1, pdl1); } - watchdog_feed(); + // feed the watchdog + IWDG->KR = 0xAAAA; } int main() { @@ -274,14 +268,19 @@ int main() { adc_init(); // init can - llcan_set_speed(CAN1, 5000, false, false); - llcan_init(CAN1); + can_silent = ALL_CAN_LIVE; + can_init(0); // 48mhz / 65536 ~= 732 timer_init(TIM3, 15); NVIC_EnableIRQ(TIM3_IRQn); - watchdog_init(); + // setup watchdog + IWDG->KR = 0x5555; + IWDG->PR = 0; // divider /4 + // 0 = 0.125 ms, let's have a 50ms watchdog + IWDG->RLR = 400 - 1; + IWDG->KR = 0xCCCC; puts("**** INTERRUPTS ON ****\n"); __enable_irq(); @@ -293,3 +292,4 @@ int main() { return 0; } + diff --git a/panda/board/safety.h b/panda/board/safety.h index dd99e6db2ee6dd..b899662bd456d7 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -1,19 +1,70 @@ -// include first, needed by safety policies -#include "safety_declarations.h" +// sample struct that keeps 3 samples in memory +struct sample_t { + int values[6]; + int min; + int max; +} sample_t_default = {{0}, 0, 0}; + +// no float support in STM32F2 micros (cortex-m3) +#ifdef PANDA +struct lookup_t { + float x[3]; + float y[3]; +}; +#endif + +void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +int safety_tx_lin_hook(int lin_num, uint8_t *data, int len); +int safety_ignition_hook(); +uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last); +int to_signed(int d, int bits); +void update_sample(struct sample_t *sample, int sample_new); +int max_limit_check(int val, const int MAX, const int MIN); +int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, + const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR); +int driver_limit_check(int val, int val_last, struct sample_t *val_driver, + const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, + const int MAX_ALLOWANCE, const int DRIVER_FACTOR); +int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); +#ifdef PANDA +float interpolate(struct lookup_t xy, float x); +#endif + +typedef void (*safety_hook_init)(int16_t param); +typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); +typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send); +typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len); +typedef int (*ign_hook)(); +typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); + +typedef struct { + safety_hook_init init; + ign_hook ignition; + rx_hook rx; + tx_hook tx; + tx_lin_hook tx_lin; + fwd_hook fwd; +} safety_hooks; + +// This can be set by the safety hooks. +int controls_allowed = 0; + +void safety_cb_enable_all(); + // Include the actual safety policies. #include "safety/safety_defaults.h" #include "safety/safety_honda.h" #include "safety/safety_toyota.h" +#ifdef PANDA #include "safety/safety_toyota_ipas.h" -#include "safety/safety_tesla.h" -#include "safety/safety_gm_ascm.h" +#endif #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" #include "safety/safety_elm327.h" +#include "safety/safety_forward.h" const safety_hooks *current_hooks = &nooutput_hooks; @@ -52,13 +103,11 @@ typedef struct { #define SAFETY_FORD 5 #define SAFETY_CADILLAC 6 #define SAFETY_HYUNDAI 7 -#define SAFETY_TESLA 8 -#define SAFETY_CHRYSLER 9 -#define SAFETY_SUBARU 10 -#define SAFETY_GM_ASCM 0x1334 #define SAFETY_TOYOTA_IPAS 0x1335 +#define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_ALLOUTPUT 0x1337 #define SAFETY_ELM327 0xE327 +#define SAFETY_FORWARD 0xFA0D const safety_hook_config safety_hook_registry[] = { {SAFETY_NOOUTPUT, &nooutput_hooks}, @@ -69,103 +118,95 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_FORD, &ford_hooks}, {SAFETY_CADILLAC, &cadillac_hooks}, {SAFETY_HYUNDAI, &hyundai_hooks}, - {SAFETY_CHRYSLER, &chrysler_hooks}, - {SAFETY_SUBARU, &subaru_hooks}, + {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, +#ifdef PANDA {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, - {SAFETY_GM_ASCM, &gm_ascm_hooks}, - {SAFETY_TESLA, &tesla_hooks}, +#endif {SAFETY_ALLOUTPUT, &alloutput_hooks}, {SAFETY_ELM327, &elm327_hooks}, + {SAFETY_FORWARD, &forward_hooks}, }; +#define HOOK_CONFIG_COUNT (sizeof(safety_hook_registry)/sizeof(safety_hook_config)) + +extern uint16_t current_safety_mode = SAFETY_NOOUTPUT; + int safety_set_mode(uint16_t mode, int16_t param) { - int set_status = -1; // not set - int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config); - for (int i = 0; i < hook_config_count; i++) { + for (int i = 0; i < HOOK_CONFIG_COUNT; i++) { if (safety_hook_registry[i].id == mode) { current_hooks = safety_hook_registry[i].hooks; - set_status = 0; // set - break; + if (current_hooks->init) current_hooks->init(param); + current_safety_mode = mode; + return 0; } } - if ((set_status == 0) && (current_hooks->init != NULL)) { - current_hooks->init(param); - } - return set_status; + return -1; } // compute the time elapsed (in microseconds) from 2 counter samples -// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) { - return ts - ts_last; + return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts; } // convert a trimmed integer to signed 32 bit int int to_signed(int d, int bits) { - int d_signed = d; - if (d >= (1 << MAX((bits - 1), 0))) { - d_signed = d - (1 << MAX(bits, 0)); + if (d >= (1 << (bits - 1))) { + d -= (1 << bits); } - return d_signed; + return d; } // given a new sample, update the smaple_t struct void update_sample(struct sample_t *sample, int sample_new) { - int sample_size = sizeof(sample->values) / sizeof(sample->values[0]); - for (int i = sample_size - 1; i > 0; i--) { + for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) { sample->values[i] = sample->values[i-1]; } sample->values[0] = sample_new; // get the minimum and maximum measured samples - sample->min = sample->values[0]; - sample->max = sample->values[0]; - for (int i = 1; i < sample_size; i++) { - if (sample->values[i] < sample->min) { - sample->min = sample->values[i]; - } - if (sample->values[i] > sample->max) { - sample->max = sample->values[i]; - } + sample->min = sample->max = sample->values[0]; + for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) { + if (sample->values[i] < sample->min) sample->min = sample->values[i]; + if (sample->values[i] > sample->max) sample->max = sample->values[i]; } } -bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) { - return (val > MAX_VAL) || (val < MIN_VAL); +int max_limit_check(int val, const int MAX, const int MIN) { + return (val > MAX) || (val < MIN); } // check that commanded value isn't too far from measured -bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, +int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) { // *** val rate limit check *** - int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP; - int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP; + int highest_allowed_val = max(val_last, 0) + MAX_RATE_UP; + int lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP; // if we've exceeded the meas val, we must start moving toward 0 - int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, MAX(val_meas->max, 0) + MAX_ERROR)); - int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR)); + highest_allowed_val = min(highest_allowed_val, max(val_last - MAX_RATE_DOWN, max(val_meas->max, 0) + MAX_ERROR)); + lowest_allowed_val = max(lowest_allowed_val, min(val_last + MAX_RATE_DOWN, min(val_meas->min, 0) - MAX_ERROR)); // check for violation - return (val < lowest_allowed) || (val > highest_allowed); + return (val < lowest_allowed_val) || (val > highest_allowed_val); } // check that commanded value isn't fighting against driver -bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, - const int MAX_VAL, const int MAX_RATE_UP, const int MAX_RATE_DOWN, +int driver_limit_check(int val, int val_last, struct sample_t *val_driver, + const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ALLOWANCE, const int DRIVER_FACTOR) { - int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP; - int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP; + int highest_allowed = max(val_last, 0) + MAX_RATE_UP; + int lowest_allowed = min(val_last, 0) - MAX_RATE_UP; - int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR; - int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR; + int driver_max_limit = MAX + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR; + int driver_min_limit = -MAX + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR; // if we've exceeded the applied torque, we must start moving toward 0 - int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, - MAX(driver_max_limit, 0))); - int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, - MIN(driver_min_limit, 0))); + highest_allowed = min(highest_allowed, max(val_last - MAX_RATE_DOWN, + max(driver_max_limit, 0))); + lowest_allowed = max(lowest_allowed, min(val_last + MAX_RATE_DOWN, + min(driver_min_limit, 0))); // check for violation return (val < lowest_allowed) || (val > highest_allowed); @@ -173,43 +214,40 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, // real time check, mainly used for steer torque rate limiter -bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { +int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { // *** torque real time rate limit check *** - int highest_val = MAX(val_last, 0) + MAX_RT_DELTA; - int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA; + int highest_val = max(val_last, 0) + MAX_RT_DELTA; + int lowest_val = min(val_last, 0) - MAX_RT_DELTA; // check for violation return (val < lowest_val) || (val > highest_val); } +#ifdef PANDA // interp function that holds extreme values float interpolate(struct lookup_t xy, float x) { - int size = sizeof(xy.x) / sizeof(xy.x[0]); - float ret = xy.y[size - 1]; // default output is last point - // x is lower than the first point in the x array. Return the first point if (x <= xy.x[0]) { - ret = xy.y[0]; + return xy.y[0]; } else { // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp - for (int i=0; i < (size - 1); i++) { + for (int i=0; i < size-1; i++) { if (x < xy.x[i+1]) { float x0 = xy.x[i]; float y0 = xy.y[i]; float dx = xy.x[i+1] - x0; float dy = xy.y[i+1] - y0; // dx should not be zero as xy.x is supposed ot be monotonic - if (dx <= 0.) { - dx = 0.0001; - } - ret = (dy * (x - x0) / dx) + y0; - break; + if (dx <= 0.) dx = 0.0001; + return dy * (x - x0) / dx + y0; } } + // if no such point is found, then x > xy.x[size-1]. Return last point + return xy.y[size - 1]; } - return ret; } +#endif diff --git a/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h index ef114abfe9143e..2a2d8b9857ff91 100644 --- a/panda/board/safety/safety_cadillac.h +++ b/panda/board/safety/safety_cadillac.h @@ -1,10 +1,8 @@ -#define CADILLAC_TORQUE_MSG_N 4 // 4 torque messages: 0x151, 0x152, 0x153, 0x154 - 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 int32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks const int CADILLAC_MAX_RATE_UP = 2; const int CADILLAC_MAX_RATE_DOWN = 5; const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50; @@ -13,19 +11,21 @@ const int CADILLAC_DRIVER_TORQUE_FACTOR = 4; int cadillac_ign = 0; 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}; +int cadillac_desired_torque_last[4] = {0}; // 4 torque messages uint32_t cadillac_ts_last = 0; int cadillac_supercruise_on = 0; struct sample_t cadillac_torque_driver; // last few driver torques measured -int cadillac_get_torque_idx(int addr, int array_size) { - return MIN(MAX(addr - 0x151, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on... +int cadillac_get_torque_idx(uint32_t addr) { + if (addr==0x151) return 0; + else if (addr==0x152) return 1; + else if (addr==0x153) return 2; + else return 3; } static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); + int bus_number = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr = to_push->RIR >> 21; if (addr == 356) { int torque_driver_new = ((to_push->RDLR & 0x7) << 8) | ((to_push->RDLR >> 8) & 0xFF); @@ -35,17 +35,16 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // this message isn't all zeros when ignition is on - if ((addr == 0x160) && (bus == 0)) { + if (addr == 0x160 && bus_number == 0) { cadillac_ign = to_push->RDLR > 0; } // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == 0x370) && (bus == 0)) { + if ((addr == 0x370) && (bus_number == 0)) { int cruise_engaged = to_push->RDLR & 0x800000; // bit 23 if (cruise_engaged && !cadillac_cruise_engaged_last) { controls_allowed = 1; - } - if (!cruise_engaged) { + } else if (!cruise_engaged) { controls_allowed = 0; } cadillac_cruise_engaged_last = cruise_engaged; @@ -58,15 +57,14 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); + uint32_t addr = to_send->RIR >> 21; // steer cmd checks - if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) { + if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) { int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8); int violation = 0; uint32_t ts = TIM2->CNT; - int idx = cadillac_get_torque_idx(addr, CADILLAC_TORQUE_MSG_N); + int idx = cadillac_get_torque_idx(addr); desired_torque = to_signed(desired_torque, 14); if (controls_allowed) { @@ -107,20 +105,19 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } if (violation || cadillac_supercruise_on) { - tx = 0; + return false; } } - return tx; + return true; } static void cadillac_init(int16_t param) { - UNUSED(param); controls_allowed = 0; cadillac_ign = 0; } -static int cadillac_ign_hook(void) { +static int cadillac_ign_hook() { return cadillac_ign; } @@ -130,5 +127,5 @@ const safety_hooks cadillac_hooks = { .tx = cadillac_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = cadillac_ign_hook, - .fwd = default_fwd_hook, + .fwd = alloutput_fwd_hook, }; diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index 19149b6b7a7ae9..b1c6a743f10258 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -1,11 +1,11 @@ const int CHRYSLER_MAX_STEER = 261; const int CHRYSLER_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks -const uint32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks +const int32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks const int CHRYSLER_MAX_RATE_UP = 3; const int CHRYSLER_MAX_RATE_DOWN = 3; const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor -bool chrysler_camera_detected = 0; // is giraffe switch 2 high? +int chrysler_camera_detected = 0; int chrysler_rt_torque_last = 0; int chrysler_desired_torque_last = 0; int chrysler_cruise_engaged_last = 0; @@ -13,32 +13,40 @@ uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); + int bus = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + if (to_push->RIR & 4) { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } else { + // Normal + addr = to_push->RIR >> 21; + } // Measured eps torque if (addr == 544) { - uint32_t rdhr = to_push->RDHR; - int torque_meas_new = ((rdhr & 0x7U) << 8) + ((rdhr & 0xFF00U) >> 8) - 1024U; + int rdhr = to_push->RDHR; + int torque_meas_new = ((rdhr & 0x7) << 8) + ((rdhr & 0xFF00) >> 8) - 1024; // update array of samples update_sample(&chrysler_torque_meas, torque_meas_new); } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1F4) { + if (addr == 0x1f4) { int cruise_engaged = ((to_push->RDLR & 0x380000) >> 19) == 7; if (cruise_engaged && !chrysler_cruise_engaged_last) { controls_allowed = 1; - } - if (!cruise_engaged) { + } else if (!cruise_engaged) { controls_allowed = 0; } chrysler_cruise_engaged_last = cruise_engaged; } // check if stock camera ECU is still online - if ((bus == 0) && (addr == 0x292)) { + if (bus == 0 && addr == 0x292) { chrysler_camera_detected = 1; controls_allowed = 0; } @@ -46,21 +54,27 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - - // If camera is on bus 0, then nothing can be sent + // There can be only one! (camera) if (chrysler_camera_detected) { - tx = 0; + return 0; + } + + uint32_t addr; + if (to_send->RIR & 4) { + // Extended + addr = to_send->RIR >> 3; + } else { + // Normal + addr = to_send->RIR >> 21; } - int addr = GET_ADDR(to_send); // LKA STEER if (addr == 0x292) { - uint32_t rdlr = to_send->RDLR; - int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8) - 1024U; + int rdlr = to_send->RDLR; + int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8) - 1024; uint32_t ts = TIM2->CNT; - bool violation = 0; + int violation = 0; if (controls_allowed) { @@ -98,7 +112,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } if (violation) { - tx = 0; + return false; } } @@ -108,36 +122,15 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // TODO: fix bug preventing the button msg to be fwd'd on bus 2 // 1 allows the message through - return tx; -} - -static void chrysler_init(int16_t param) { - UNUSED(param); - controls_allowed = 0; - chrysler_camera_detected = 0; -} - -static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - - int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); - // forward CAN 0 -> 2 so stock LKAS camera sees messages - if ((bus_num == 0) && !chrysler_camera_detected) { - bus_fwd = 2; - } - // forward all messages from camera except LKAS_COMMAND and LKAS_HUD - if ((bus_num == 2) && !chrysler_camera_detected && (addr != 658) && (addr != 678)) { - bus_fwd = 0; - } - return bus_fwd; + return true; } const safety_hooks chrysler_hooks = { - .init = chrysler_init, + .init = nooutput_init, .rx = chrysler_rx_hook, .tx = chrysler_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = chrysler_fwd_hook, + .fwd = nooutput_fwd_hook, }; diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h index 9dc23b54b79906..196df65d2f8eb6 100644 --- a/panda/board/safety/safety_defaults.h +++ b/panda/board/safety/safety_defaults.h @@ -1,33 +1,24 @@ -void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - UNUSED(to_push); -} +void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} -int default_ign_hook(void) { +int default_ign_hook() { return -1; // use GPIO to determine ignition } // *** no output safety mode *** static void nooutput_init(int16_t param) { - UNUSED(param); controls_allowed = 0; } static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - UNUSED(to_send); return false; } static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { - UNUSED(lin_num); - UNUSED(data); - UNUSED(len); return false; } -static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - UNUSED(bus_num); - UNUSED(to_fwd); +static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } @@ -37,33 +28,32 @@ const safety_hooks nooutput_hooks = { .tx = nooutput_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = default_fwd_hook, + .fwd = nooutput_fwd_hook, }; // *** all output safety mode *** static void alloutput_init(int16_t param) { - UNUSED(param); controls_allowed = 1; } static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - UNUSED(to_send); return true; } static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { - UNUSED(lin_num); - UNUSED(data); - UNUSED(len); return true; } +static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + return -1; +} + const safety_hooks alloutput_hooks = { .init = alloutput_init, .rx = default_rx_hook, .tx = alloutput_tx_hook, .tx_lin = alloutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = default_fwd_hook, + .fwd = alloutput_fwd_hook, }; diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h index 1f44e992a0d4e9..98dce6532ac8ba 100644 --- a/panda/board/safety/safety_elm327.h +++ b/panda/board/safety/safety_elm327.h @@ -1,49 +1,45 @@ -static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - - int tx = 1; - int bus = GET_BUS(to_send); - int addr = GET_ADDR(to_send); - int len = GET_LEN(to_send); +static void elm327_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} +static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { //All ELM traffic must appear on CAN0 - if (bus != 0) { - tx = 0; - } - + if(((to_send->RDTR >> 4) & 0xf) != 0) return 0; //All ISO 15765-4 messages must be 8 bytes long - if (len != 8) { - tx = 0; - } + if((to_send->RDTR & 0xf) != 8) return 0; - //Check valid 29 bit send addresses for ISO 15765-4 - //Check valid 11 bit send addresses for ISO 15765-4 - if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) && - ((addr != 0x7DF) && ((addr & 0x7F8) != 0x7E0))) { - tx = 0; + if(to_send->RIR & 4){ + uint32_t addr = to_send->RIR >> 3; + //Check valid 29 bit send addresses for ISO 15765-4 + if(!(addr == 0x18DB33F1 || (addr & 0x1FFF00FF) == 0x18DA00F1)) return 0; + } else { + uint32_t addr = to_send->RIR >> 21; + //Check valid 11 bit send addresses for ISO 15765-4 + if(!(addr == 0x7DF || (addr & 0x7F8) == 0x7E0)) return 0; } - return tx; + + return true; } static int elm327_tx_lin_hook(int lin_num, uint8_t *data, int len) { - int tx = 1; - if (lin_num != 0) { - tx = 0; //Only operate on LIN 0, aka serial 2 - } - if ((len < 5) || (len > 11)) { - tx = 0; //Valid KWP size - } - if (!(((data[0] & 0xF8U) == 0xC0U) && ((data[0] & 0x07U) != 0U) && - (data[1] == 0x33U) && (data[2] == 0xF1U))) { - tx = 0; //Bad msg - } - return tx; + if(lin_num != 0) return false; //Only operate on LIN 0, aka serial 2 + if(len < 5 || len > 11) return false; //Valid KWP size + if(!((data[0] & 0xF8) == 0xC0 && (data[0] & 0x07) > 0 && + data[1] == 0x33 && data[2] == 0xF1)) return false; //Bad msg + return true; +} + +static void elm327_init(int16_t param) { + controls_allowed = 1; +} + +static int elm327_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + return -1; } const safety_hooks elm327_hooks = { - .init = nooutput_init, - .rx = default_rx_hook, + .init = elm327_init, + .rx = elm327_rx_hook, .tx = elm327_tx_hook, .tx_lin = elm327_tx_lin_hook, .ignition = default_ign_hook, - .fwd = default_fwd_hook, + .fwd = elm327_fwd_hook, }; diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 21c9c54db1300c..075029fb623e0f 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -13,29 +13,26 @@ int ford_is_moving = 0; static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int addr = GET_ADDR(to_push); - - if (addr == 0x217) { + if ((to_push->RIR>>21) == 0x217) { // wheel speeds are 14 bits every 16 ford_is_moving = 0xFCFF & (to_push->RDLR | (to_push->RDLR >> 16) | to_push->RDHR | (to_push->RDHR >> 16)); } // state machine to enter and exit controls - if (addr == 0x83) { - bool cancel = (to_push->RDLR >> 8) & 0x1; - bool set_or_resume = (to_push->RDLR >> 28) & 0x3; + if ((to_push->RIR>>21) == 0x83) { + int cancel = ((to_push->RDLR >> 8) & 0x1); + int set_or_resume = (to_push->RDLR >> 28) & 0x3; if (cancel) { controls_allowed = 0; - } - if (set_or_resume) { + } else if (set_or_resume) { controls_allowed = 1; } } // exit controls on rising edge of brake press or on brake press when // speed > 0 - if (addr == 0x165) { + if ((to_push->RIR>>21) == 0x165) { int brake = to_push->RDLR & 0x20; if (brake && (!(ford_brake_prev) || ford_is_moving)) { controls_allowed = 0; @@ -44,7 +41,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // exit controls on rising edge of gas press - if (addr == 0x204) { + if ((to_push->RIR>>21) == 0x204) { int gas = to_push->RDLR & 0xFF03; if (gas && !(ford_gas_prev)) { controls_allowed = 0; @@ -61,33 +58,29 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_is_moving); - bool current_controls_allowed = controls_allowed && !(pedal_pressed); - int addr = GET_ADDR(to_send); + int current_controls_allowed = controls_allowed && !(pedal_pressed); // STEER: safety check - if (addr == 0x3CA) { - if (!current_controls_allowed) { + if ((to_send->RIR>>21) == 0x3CA) { + if (current_controls_allowed) { + // all messages are fine here + } else { // bits 7-4 need to be 0xF to disallow lkas commands - if (((to_send->RDLR >> 4) & 0xF) != 0xF) { - tx = 0; - } + if (((to_send->RDLR >> 4) & 0xF) != 0xF) return 0; } } // FORCE CANCEL: safety check only relevant when spamming the cancel button // ensuring that set and resume aren't sent - if (addr == 0x83) { - if (((to_send->RDLR >> 28) & 0x3) != 0) { - tx = 0; - } + if ((to_send->RIR>>21) == 0x83) { + if ((to_send->RDLR >> 28) & 0x3) return 0; } // 1 allows the message through - return tx; + return true; } const safety_hooks ford_hooks = { @@ -96,5 +89,5 @@ const safety_hooks ford_hooks = { .tx = ford_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = default_fwd_hook, + .fwd = nooutput_fwd_hook, }; diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 949f5c7e73f09f..f35b26b4ef84d3 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -10,7 +10,7 @@ const int GM_MAX_STEER = 300; const int GM_MAX_RT_DELTA = 128; // max delta torque allowed for real time checks -const uint32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks +const int32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks const int GM_MAX_RATE_UP = 7; const int GM_MAX_RATE_DOWN = 17; const int GM_DRIVER_TORQUE_ALLOWANCE = 50; @@ -23,16 +23,25 @@ int gm_brake_prev = 0; int gm_gas_prev = 0; int gm_speed = 0; // silence everything if stock car control ECUs are still online -bool gm_ascm_detected = 0; -bool gm_ignition_started = 0; +int gm_ascm_detected = 0; +int gm_ignition_started = 0; int gm_rt_torque_last = 0; int gm_desired_torque_last = 0; uint32_t gm_ts_last = 0; struct sample_t gm_torque_driver; // last few driver torques measured static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus_number = GET_BUS(to_push); - int addr = GET_ADDR(to_push); + int bus_number = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + if (to_push->RIR & 4) { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } else { + // Normal + addr = to_push->RIR >> 21; + } if (addr == 388) { int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF); @@ -41,11 +50,11 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { update_sample(&gm_torque_driver, torque_driver_new); } - if ((addr == 0x1F1) && (bus_number == 0)) { + if (addr == 0x1f1 && bus_number == 0) { //Bit 5 should be ignition "on" //Backup plan is Bit 2 (accessory power) - bool ign = ((to_push->RDLR) & 0x20) != 0; - gm_ignition_started = ign; + uint32_t ign = (to_push->RDLR) & 0x20; + gm_ignition_started = ign > 0; } // sample speed, really only care if car is moving or not @@ -58,24 +67,19 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // on powertrain bus. // 384 = ASCMLKASteeringCmd // 715 = ASCMGasRegenCmd - if ((bus_number == 0) && ((addr == 384) || (addr == 715))) { + if (bus_number == 0 && (addr == 384 || addr == 715)) { gm_ascm_detected = 1; controls_allowed = 0; } // ACC steering wheel buttons if (addr == 481) { - int button = (to_push->RDHR >> 12) & 0x7; - switch (button) { - case 2: // resume - case 3: // set - controls_allowed = 1; - break; - case 6: // cancel - controls_allowed = 0; - break; - default: - break; // any other button is irrelevant + int buttons = (to_push->RDHR >> 12) & 0x7; + // res/set - enable, cancel button - disable + if (buttons == 2 || buttons == 3) { + controls_allowed = 1; + } else if (buttons == 6) { + controls_allowed = 0; } } @@ -97,7 +101,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 417) { int gas = to_push->RDHR & 0xFF0000; - if (gas && !gm_gas_prev && long_controls_allowed) { + if (gas && !gm_gas_prev) { controls_allowed = 0; } gm_gas_prev = gas; @@ -105,7 +109,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on regen paddle if (addr == 189) { - bool regen = to_push->RDLR & 0x20; + int regen = to_push->RDLR & 0x20; if (regen) { controls_allowed = 0; } @@ -120,41 +124,43 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - // There can be only one! (ASCM) if (gm_ascm_detected) { - tx = 0; + return 0; } // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_speed); - bool current_controls_allowed = controls_allowed && !pedal_pressed; - - int addr = GET_ADDR(to_send); + int current_controls_allowed = controls_allowed && !pedal_pressed; + + uint32_t addr; + if (to_send->RIR & 4) { + // Extended + addr = to_send->RIR >> 3; + } else { + // Normal + addr = to_send->RIR >> 21; + } // BRAKE: safety check if (addr == 789) { - uint32_t rdlr = to_send->RDLR; - int brake = ((rdlr & 0xFU) << 8) + ((rdlr & 0xFF00U) >> 8); + int rdlr = to_send->RDLR; + int brake = ((rdlr & 0xF) << 8) + ((rdlr & 0xFF00) >> 8); brake = (0x1000 - brake) & 0xFFF; - if (!current_controls_allowed || !long_controls_allowed) { - if (brake != 0) { - tx = 0; - } - } - if (brake > GM_MAX_BRAKE) { - tx = 0; + if (current_controls_allowed) { + if (brake > GM_MAX_BRAKE) return 0; + } else { + if (brake != 0) return 0; } } // LKA STEER: safety check if (addr == 384) { - uint32_t rdlr = to_send->RDLR; - int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8); + int rdlr = to_send->RDLR; + int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8); uint32_t ts = TIM2->CNT; - bool violation = 0; + int violation = 0; desired_torque = to_signed(desired_torque, 11); if (current_controls_allowed) { @@ -194,43 +200,37 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } if (violation) { - tx = 0; + return false; } } // PARK ASSIST STEER: unlimited torque, no thanks - if (addr == 823) { - tx = 0; - } + if (addr == 823) return 0; // GAS/REGEN: safety check if (addr == 715) { - uint32_t rdlr = to_send->RDLR; - int gas_regen = ((rdlr & 0x7F0000U) >> 11) + ((rdlr & 0xF8000000U) >> 27); - // Disabled message is !engaed with gas - // value that corresponds to max regen. - if (!current_controls_allowed || !long_controls_allowed) { - bool apply = (rdlr & 1U) != 0U; - if (apply || (gas_regen != GM_MAX_REGEN)) { - tx = 0; - } - } - if (gas_regen > GM_MAX_GAS) { - tx = 0; + int rdlr = to_send->RDLR; + int gas_regen = ((rdlr & 0x7F0000) >> 11) + ((rdlr & 0xF8000000) >> 27); + int apply = rdlr & 1; + if (current_controls_allowed) { + if (gas_regen > GM_MAX_GAS) return 0; + } else { + // Disabled message is !engaed with gas + // value that corresponds to max regen. + if (apply || gas_regen != GM_MAX_REGEN) return 0; } } // 1 allows the message through - return tx; + return true; } static void gm_init(int16_t param) { - UNUSED(param); controls_allowed = 0; gm_ignition_started = 0; } -static int gm_ign_hook(void) { +static int gm_ign_hook() { return gm_ignition_started; } @@ -240,5 +240,6 @@ const safety_hooks gm_hooks = { .tx = gm_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = gm_ign_hook, - .fwd = default_fwd_hook, + .fwd = nooutput_fwd_hook, }; + diff --git a/panda/board/safety/safety_gm_ascm.h b/panda/board/safety/safety_gm_ascm.h index d452818d69b59d..ceec2b62d3867a 100644 --- a/panda/board/safety/safety_gm_ascm.h +++ b/panda/board/safety/safety_gm_ascm.h @@ -3,35 +3,42 @@ static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - int bus_fwd = -1; + uint32_t addr = to_fwd->RIR>>21; if (bus_num == 0) { - int addr = GET_ADDR(to_fwd); - bus_fwd = 2; - // do not propagate lkas messages from ascm to actuators, unless supercruise is on + + // do not propagate lkas messages from ascm to actuators // block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2 // block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1 //if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) { if ((addr == 0x152) || (addr == 0x154)) { - int supercruise_on = (to_fwd->RDHR >> 4) & 0x1; // bit 36 - if (!supercruise_on) { - bus_fwd = -1; - } + int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36 + if (!supercruise_on) return -1; + } + + // on the chassis bus, the OBDII port is on the module side, so we need to read + // the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to + // the actuator as 0x152 and 0x154 + if (addr == 0x151) { + to_fwd->RIR = (0x152 << 21) | (to_fwd->RIR & 0x1fffff); } - if ((addr == 0x151) || (addr == 0x153) || (addr == 0x314)) { - // on the chassis bus, the OBDII port is on the module side, so we need to read - // the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to - // the actuator as 0x152 and 0x154 - uint32_t fwd_addr = addr + 1; - to_fwd->RIR = (fwd_addr << 21) | (to_fwd->RIR & 0x1fffff); + if (addr == 0x153) { + to_fwd->RIR = (0x154 << 21) | (to_fwd->RIR & 0x1fffff); } + + // brake + //if (addr == 0x314) { + // to_fwd->RIR = (0x315 << 21) | (to_fwd->RIR & 0x1fffff); + //} + + return 2; } if (bus_num == 2) { - bus_fwd = 0; + return 0; } - return bus_fwd; + return -1; } const safety_hooks gm_ascm_hooks = { diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 44a57ec978ff36..82e6eadda24fa1 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -7,38 +7,33 @@ // brake rising edge // brake > 0mph -const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file -int honda_brake_prev = 0; -int honda_gas_prev = 0; -int honda_ego_speed = 0; -bool honda_bosch_hardware = false; +// these are set in the Honda safety hooks...this is the wrong place +const int gas_interceptor_threshold = 328; +int gas_interceptor_detected = 0; +int brake_prev = 0; +int gas_prev = 0; +int gas_interceptor_prev = 0; +int ego_speed = 0; +// TODO: auto-detect bosch hardware based on CAN messages? +bool bosch_hardware = false; bool honda_alt_brake_msg = false; static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int addr = GET_ADDR(to_push); - int len = GET_LEN(to_push); - // sample speed - if (addr == 0x158) { + if ((to_push->RIR>>21) == 0x158) { // first 2 bytes - honda_ego_speed = to_push->RDLR & 0xFFFF; + ego_speed = to_push->RDLR & 0xFFFF; } // state machine to enter and exit controls // 0x1A6 for the ILX, 0x296 for the Civic Touring - if ((addr == 0x1A6) || (addr == 0x296)) { - int button = (to_push->RDLR & 0xE0) >> 5; - switch (button) { - case 2: // cancel - controls_allowed = 0; - break; - case 3: // set - case 4: // resume - controls_allowed = 1; - break; - default: - break; // any other button is irrelevant + if ((to_push->RIR>>21) == 0x1A6 || (to_push->RIR>>21) == 0x296) { + int buttons = (to_push->RDLR & 0xE0) >> 5; + if (buttons == 4 || buttons == 3) { + controls_allowed = 1; + } else if (buttons == 2) { + controls_allowed = 0; } } @@ -48,27 +43,25 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // in these cases, this is used instead. // most hondas: 0x17C bit 53 // accord, crv: 0x1BE bit 4 - #define IS_USER_BRAKE_MSG(addr) (!honda_alt_brake_msg ? ((addr) == 0x17C) : ((addr) == 0x1BE)) - #define USER_BRAKE_VALUE(to_push) (!honda_alt_brake_msg ? ((to_push)->RDHR & 0x200000) : ((to_push)->RDLR & 0x10)) + #define IS_USER_BRAKE_MSG(to_push) (!honda_alt_brake_msg ? to_push->RIR>>21 == 0x17C : to_push->RIR>>21 == 0x1BE) + #define USER_BRAKE_VALUE(to_push) (!honda_alt_brake_msg ? to_push->RDHR & 0x200000 : to_push->RDLR & 0x10) // exit controls on rising edge of brake press or on brake press when // speed > 0 - bool is_user_brake_msg = IS_USER_BRAKE_MSG(addr); // needed to enforce type - if (is_user_brake_msg) { + if (IS_USER_BRAKE_MSG(to_push)) { int brake = USER_BRAKE_VALUE(to_push); - if (brake && (!(honda_brake_prev) || honda_ego_speed)) { + if (brake && (!(brake_prev) || ego_speed)) { controls_allowed = 0; } - honda_brake_prev = brake; + brake_prev = brake; } // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) // length check because bosch hardware also uses this id (0x201 w/ len = 8) - if ((addr == 0x201) && (len == 6)) { + if ((to_push->RIR>>21) == 0x201 && (to_push->RDTR & 0xf) == 6) { gas_interceptor_detected = 1; int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8); - if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD) && - long_controls_allowed) { + if ((gas_interceptor > gas_interceptor_threshold) && + (gas_interceptor_prev <= gas_interceptor_threshold)) { controls_allowed = 0; } gas_interceptor_prev = gas_interceptor; @@ -76,12 +69,12 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if no interceptor if (!gas_interceptor_detected) { - if (addr == 0x17C) { + if ((to_push->RIR>>21) == 0x17C) { int gas = to_push->RDLR & 0xFF; - if (gas && !(honda_gas_prev) && long_controls_allowed) { + if (gas && !(gas_prev)) { controls_allowed = 0; } - honda_gas_prev = gas; + gas_prev = gas; } } } @@ -94,122 +87,81 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); - int bus = GET_BUS(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 = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || - (honda_brake_prev && honda_ego_speed); - bool current_controls_allowed = controls_allowed && !(pedal_pressed); + int pedal_pressed = gas_prev || (gas_interceptor_prev > gas_interceptor_threshold) || + (brake_prev && ego_speed); + int current_controls_allowed = controls_allowed && !(pedal_pressed); // BRAKE: safety check - if (addr == 0x1FA) { - if (!current_controls_allowed || !long_controls_allowed) { - if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) { - tx = 0; - } - } - if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) { - tx = 0; + if ((to_send->RIR>>21) == 0x1FA) { + if (current_controls_allowed) { + if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) return 0; + } else { + if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0; } } // STEER: safety check - if ((addr == 0xE4) || (addr == 0x194)) { - if (!current_controls_allowed) { - if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) { - tx = 0; - } + if ((to_send->RIR>>21) == 0xE4 || (to_send->RIR>>21) == 0x194) { + if (current_controls_allowed) { + // all messages are fine here + } else { + if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0; } } // GAS: safety check - if (addr == 0x200) { - if (!current_controls_allowed || !long_controls_allowed) { - if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) { - tx = 0; - } + if ((to_send->RIR>>21) == 0x200) { + if (current_controls_allowed) { + // all messages are fine here + } else { + if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0; } } // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam - if ((addr == 0x296) && honda_bosch_hardware && - !current_controls_allowed && (bus == 0)) { - if (((to_send->RDLR >> 5) & 0x7) != 2) { - tx = 0; - } + if (((to_send->RIR>>21) == 0x296) && bosch_hardware && + !current_controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) { + if (((to_send->RDLR >> 5) & 0x7) != 2) return 0; } // 1 allows the message through - return tx; + return true; } static void honda_init(int16_t param) { - UNUSED(param); controls_allowed = 0; - honda_bosch_hardware = false; + bosch_hardware = false; honda_alt_brake_msg = false; } +const safety_hooks honda_hooks = { + .init = honda_init, + .rx = honda_rx_hook, + .tx = honda_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = nooutput_fwd_hook, +}; + static void honda_bosch_init(int16_t param) { controls_allowed = 0; - honda_bosch_hardware = true; + bosch_hardware = true; // Checking for alternate brake override from safety parameter - honda_alt_brake_msg = (param == 1) ? true : false; -} - -static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - // fwd from car to camera. also fwd certain msgs from camera to car - // 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX, - // 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud, - // 0x39f is radar hud - int bus_fwd = -1; - - if (bus_num == 0) { - bus_fwd = 2; - } - if (bus_num == 2) { - // block stock lkas messages and stock acc messages (if OP is doing ACC) - int addr = GET_ADDR(to_fwd); - int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D); - int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F); - int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed); - if (!block_fwd) { - bus_fwd = 0; - } - } - return bus_fwd; + honda_alt_brake_msg = param == 1 ? true : false; } static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - int bus_fwd = -1; - - if (bus_num == 2) { - bus_fwd = 1; - } - if (bus_num == 1) { - int addr = GET_ADDR(to_fwd); - int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D); - if (!is_lkas_msg) { - bus_fwd = 2; - } + if (bus_num == 1 || bus_num == 2) { + int addr = to_fwd->RIR>>21; + return addr != 0xE4 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1; } - return bus_fwd; + return -1; } -const safety_hooks honda_hooks = { - .init = honda_init, - .rx = honda_rx_hook, - .tx = honda_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .ignition = default_ign_hook, - .fwd = honda_fwd_hook, -}; - const safety_hooks honda_bosch_hooks = { .init = honda_bosch_init, .rx = honda_rx_hook, diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 3c5eef4ef90810..55a9a421279128 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -1,24 +1,29 @@ -const int HYUNDAI_MAX_STEER = 255; // like stock +const int HYUNDAI_MAX_STEER = 300; const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks -const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks -const int HYUNDAI_MAX_RATE_UP = 3; -const int HYUNDAI_MAX_RATE_DOWN = 7; -const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; -const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; - -bool hyundai_camera_detected = 0; -bool hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high? +const int32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks +const int HYUNDAI_MAX_RATE_UP = 6; +const int HYUNDAI_MAX_RATE_DOWN = 8; + +int hyundai_camera_detected = 0; int hyundai_camera_bus = 0; +int hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high? int hyundai_rt_torque_last = 0; -int hyundai_desired_torque_last = 0; int hyundai_cruise_engaged_last = 0; uint32_t hyundai_ts_last = 0; struct sample_t hyundai_torque_driver; // last few driver torques measured static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - controls_allowed = 1; + int bus = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + if (to_push->RIR & 4) { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } else { + // Normal + addr = to_push->RIR >> 21; + } if (addr == 897) { int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048; @@ -27,46 +32,65 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // check if stock camera ECU is still online - if ((bus == 0) && (addr == 832)) { + if (bus == 0 && addr == 832) { hyundai_camera_detected = 1; controls_allowed = 0; } // Find out which bus the camera is on - if (addr == 832) { - hyundai_camera_bus = bus; + if (bus == 1 && addr == 832) { + hyundai_camera_bus = 1; + } + if (bus == 2 && addr == 832) { + hyundai_camera_bus = 2; } + /* enter controls on rising edge of ACC, exit controls on ACC off + if ((to_push->RIR>>21) == 1057) { + // 2 bits: 13-14 + int cruise_engaged = (to_push->RDLR >> 13) & 0x3; + if (cruise_engaged && !hyundai_cruise_engaged_last) { + controls_allowed = 1; + } else if (!cruise_engaged) { + controls_allowed = 0; + } + hyundai_cruise_engaged_last = cruise_engaged; + } */ + controls_allowed = 1; + // 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high - if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) { + if ((to_push->RIR>>21) == 832 && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) { hyundai_giraffe_switch_2 = 1; } } static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); - // There can be only one! (camera) if (hyundai_camera_detected) { - tx = 0; + return 0; + } + + uint32_t addr; + if (to_send->RIR & 4) { + // Extended + addr = to_send->RIR >> 3; + } else { + // Normal + addr = to_send->RIR >> 21; } // LKA STEER: safety check if (addr == 832) { int desired_torque = ((to_send->RDLR >> 16) & 0x7ff) - 1024; uint32_t ts = TIM2->CNT; - bool violation = 0; + int violation = 0; if (controls_allowed) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER); - // used next time - hyundai_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); @@ -85,50 +109,41 @@ 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; } if (violation) { - tx = 0; + return false; } } // FORCE CANCEL: safety check only relevant when spamming the cancel button. // ensuring that only the cancel button press is sent (VAL 4) when controls are off. // This avoids unintended engagements while still allowing resume spam - // TODO: fix bug preventing the button msg to be fwd'd on bus 2 - //if ((addr == 1265) && !controls_allowed && (bus == 0) { - // if ((to_send->RDLR & 0x7) != 4) { - // tx = 0; - // } - //} + if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) { + if ((to_send->RDLR & 0x7) != 4) return 0; + } // 1 allows the message through - return tx; + return true; } static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - - int bus_fwd = -1; - // forward cam to ccan and viceversa, except lkas cmd - if (hyundai_giraffe_switch_2) { - if (bus_num == 0) { - bus_fwd = hyundai_camera_bus; - } - if (bus_num == hyundai_camera_bus) { - int addr = GET_ADDR(to_fwd); - if (addr != 832) { - bus_fwd = 0; - } - } + // forward cam to ccan and viceversa, except lkas cmd and mdps message + if ((bus_num == 0 || bus_num == hyundai_camera_bus) && hyundai_giraffe_switch_2) { + int addr = to_fwd->RIR>>21; + + if (addr == 832 && bus_num == hyundai_camera_bus) return -1; + // if (addr == 593 && bus_num == 0) return -1; + if (bus_num == 0) return (uint8_t)(hyundai_camera_bus); + if (bus_num == hyundai_camera_bus) return (uint8_t)(0); } - return bus_fwd; + + return -1; } static void hyundai_init(int16_t param) { - UNUSED(param); controls_allowed = 0; hyundai_giraffe_switch_2 = 0; } diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index c7a8c20e52c963..3d21d2269a733e 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -1,127 +1,49 @@ -const int SUBARU_MAX_STEER = 2047; // 1s -// real time torque limit to prevent controls spamming -// the real time limit is 1500/sec -const int SUBARU_MAX_RT_DELTA = 940; // max delta torque allowed for real time checks -const uint32_t SUBARU_RT_INTERVAL = 250000; // 250ms between real time checks -const int SUBARU_MAX_RATE_UP = 50; -const int SUBARU_MAX_RATE_DOWN = 70; -const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60; -const int SUBARU_DRIVER_TORQUE_FACTOR = 10; +void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} -int subaru_cruise_engaged_last = 0; -int subaru_rt_torque_last = 0; -int subaru_desired_torque_last = 0; -uint32_t subaru_ts_last = 0; -struct sample_t subaru_torque_driver; // last few driver torques measured +// FIXME +// *** all output safety mode *** - -static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - - if ((addr == 0x119) && (bus == 0)){ - int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF); - torque_driver_new = to_signed(torque_driver_new, 11); - // update array of samples - update_sample(&subaru_torque_driver, torque_driver_new); - } - - // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == 0x240) && (bus == 0)) { - int cruise_engaged = (to_push->RDHR >> 9) & 1; - if (cruise_engaged && !subaru_cruise_engaged_last) { - controls_allowed = 1; - } - if (!cruise_engaged) { - controls_allowed = 0; - } - subaru_cruise_engaged_last = cruise_engaged; - } +static void subaru_init(int16_t param) { + controls_allowed = 1; } static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); - - // steer cmd checks - if (addr == 0x122) { - int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF); - bool violation = 0; - uint32_t ts = TIM2->CNT; - desired_torque = to_signed(desired_torque, 13); - - if (controls_allowed) { - - // *** global torque limit check *** - 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, - 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; - - // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, subaru_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); - if (ts_elapsed > SUBARU_RT_INTERVAL) { - subaru_rt_torque_last = desired_torque; - subaru_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) { - subaru_desired_torque_last = 0; - subaru_rt_torque_last = 0; - subaru_ts_last = ts; - } - - if (violation) { - tx = 0; - } - - } - return tx; + return true; } static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + + // shifts bits 29 > 11 + int32_t addr = to_fwd->RIR >> 21; - int bus_fwd = -1; + // forward CAN 0 > 1 if (bus_num == 0) { - bus_fwd = 2; // Camera CAN + return 1; // ES CAN } - if (bus_num == 2) { - // 356 is LKAS for outback 2015 - // 356 is LKAS for Global Platform - // 545 is ES_Distance - // 802 is ES_LKAS - int addr = GET_ADDR(to_fwd); - int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802); - if (!block_msg) { - bus_fwd = 0; // Main CAN + // forward CAN 1 > 0, except ES_LKAS + else if (bus_num == 1) { + + // outback 2015 + if (addr == 0x164) { + return -1; + } + // global platform + if (addr == 0x122) { + return -1; } + + return 0; // Main CAN } // fallback to do not forward - return bus_fwd; + return -1; } const safety_hooks subaru_hooks = { - .init = nooutput_init, + .init = subaru_init, .rx = subaru_rx_hook, .tx = subaru_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = subaru_fwd_hook, -}; +}; \ No newline at end of file diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index b58e6b2bbf7ad4..5a5ad875a9a003 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -1,229 +1,564 @@ -// board enforces -// in-state -// accel set/resume -// out-state -// cancel button -// regen paddle -// accel rising edge -// brake rising edge -// brake > 0mph -// -bool fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) { - return (val > MAX_VAL) || (val < MIN_VAL); -} - -// 2m/s are added to be less restrictive -const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { - {2., 7., 17.}, - {5., .8, .25}}; - -const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { - {2., 7., 17.}, - {5., 3.5, .8}}; - -const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = { - {2., 29., 38.}, - {410., 92., 36.}}; - -const uint32_t TESLA_RT_INTERVAL = 250000; // 250ms between real time checks - -// state of angle limits -float tesla_desired_angle_last = 0; // last desired steer angle -float tesla_rt_angle_last = 0.; // last real time angle -float tesla_ts_angle_last = 0; - -int tesla_controls_allowed_last = 0; - -int tesla_brake_prev = 0; -int tesla_gas_prev = 0; -int tesla_speed = 0; -int eac_status = 0; - -int tesla_ignition_started = 0; - - -void set_gmlan_digital_output(int to_set); -void reset_gmlan_switch_timeout(void); -void gmlan_switch_init(int timeout_enable); - - -static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - set_gmlan_digital_output(0); // #define GMLAN_HIGH 0 - reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled - - int addr = GET_ADDR(to_push); - - if (addr == 0x45) { - // 6 bits starting at position 0 - int lever_position = (to_push->RDLR & 0x3F); - if (lever_position == 2) { // pull forward - // activate openpilot - controls_allowed = 1; - } - if (lever_position == 1) { // push towards the back - // deactivate openpilot - controls_allowed = 0; - } - } - - // Detect drive rail on (ignition) (start recording) - if (addr == 0x348) { - // GTW_status - int drive_rail_on = (to_push->RDLR & 0x0001); - tesla_ignition_started = drive_rail_on == 1; - } - - // exit controls on brake press - // DI_torque2::DI_brakePedal 0x118 - if (addr == 0x118) { - // 1 bit at position 16 - if ((((to_push->RDLR & 0x8000)) >> 15) == 1) { - // disable break cancel by commenting line below - controls_allowed = 0; - } - //get vehicle speed in m/s. Tesla gives MPH - tesla_speed = ((((((to_push->RDLR >> 24) & 0xF) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05) - 25) * 1.609 / 3.6; - if (tesla_speed < 0) { - tesla_speed = 0; - } - } - - // exit controls on EPAS error - // EPAS_sysStatus::EPAS_eacStatus 0x370 - if (addr == 0x370) { - // if EPAS_eacStatus is not 1 or 2, disable control - eac_status = ((to_push->RDHR >> 21)) & 0x7; - // For human steering override we must not disable controls when eac_status == 0 - // Additional safety: we could only allow eac_status == 0 when we have human steering allowed - if (controls_allowed && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) { - controls_allowed = 0; - //puts("EPAS error! \n"); - } - } - //get latest steering wheel angle - if (addr == 0x00E) { - float angle_meas_now = (int)(((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1) - 819.2); - uint32_t ts = TIM2->CNT; - uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last); - - // *** angle real time check - // add 1 to not false trigger the violation and multiply by 25 since the check is done every 250 ms and steer angle is updated at 100Hz - float rt_delta_angle_up = (interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25.) + 1.; - float rt_delta_angle_down = (interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25.) + 1.; - float highest_rt_angle = tesla_rt_angle_last + ((tesla_rt_angle_last > 0.) ? rt_delta_angle_up : rt_delta_angle_down); - float lowest_rt_angle = tesla_rt_angle_last - ((tesla_rt_angle_last > 0.) ? rt_delta_angle_down : rt_delta_angle_up); - - if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) { - tesla_rt_angle_last = angle_meas_now; - tesla_ts_angle_last = ts; - } - - // check for violation; - if (fmax_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) { - // We should not be able to STEER under these conditions - // Other sending is fine (to allow human override) - controls_allowed = 0; - //puts("WARN: RT Angle - No steer allowed! \n"); - } else { - controls_allowed = 1; - } - - tesla_controls_allowed_last = controls_allowed; - } -} - -// all commands: gas/regen, friction brake and steering -// if controls_allowed and no pedals pressed -// allow all commands up to limit -// else -// block all commands that produce actuation - -static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - - int tx = 1; - int addr = GET_ADDR(to_send); - - // do not transmit CAN message if steering angle too high - // DAS_steeringControl::DAS_steeringAngleRequest - if (addr == 0x488) { - float angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8); - float desired_angle = (angle_raw * 0.1) - 1638.35; - bool violation = 0; - int st_enabled = (to_send->RDLR & 0x400000) >> 22; - - if (st_enabled == 0) { - //steering is not enabled, do not check angles and do send - tesla_desired_angle_last = desired_angle; - } else if (controls_allowed) { - // add 1 to not false trigger the violation - float delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) + 1.; - float delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) + 1.; - float highest_desired_angle = tesla_desired_angle_last + ((tesla_desired_angle_last > 0.) ? delta_angle_up : delta_angle_down); - float lowest_desired_angle = tesla_desired_angle_last - ((tesla_desired_angle_last > 0.) ? delta_angle_down : delta_angle_up); - float TESLA_MAX_ANGLE = interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.; - - //check for max angles - violation |= fmax_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE); - - //check for angle delta changes - violation |= fmax_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); - - if (violation) { - controls_allowed = 0; - tx = 0; - } - tesla_desired_angle_last = desired_angle; - } else { - tx = 0; - } - } - return tx; -} - -static void tesla_init(int16_t param) { - UNUSED(param); - controls_allowed = 0; - tesla_ignition_started = 0; - gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled -} - -static int tesla_ign_hook(void) { - return tesla_ignition_started; -} - -static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - - int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); - - if (bus_num == 0) { - // change inhibit of GTW_epasControl - - if (addr != 0x214) { - // remove EPB_epasControl - bus_fwd = 2; // Custom EPAS bus - } - if (addr == 0x101) { - to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque) - uint32_t checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF; - to_fwd->RDLR = to_fwd->RDLR & 0xFFFF; - to_fwd->RDLR = to_fwd->RDLR + (checksum << 16); - } - } - if (bus_num == 2) { - // remove GTW_epasControl in forwards - if (addr != 0x101) { - bus_fwd = 0; // Chassis CAN - } - } - return bus_fwd; -} - -const safety_hooks tesla_hooks = { - .init = tesla_init, - .rx = tesla_rx_hook, - .tx = tesla_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .ignition = tesla_ign_hook, - .fwd = tesla_fwd_hook, -}; +// board enforces +// in-state +// accel set/resume +// out-state +// cancel button +// regen paddle +// accel rising edge +// brake rising edge +// brake > 0mph + +// 2m/s are added to be less restrictive +const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { + {2., 7., 17.}, + {15., 15.8, 15.25}}; + +const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { + {2., 7., 17.}, + {15., 13.5, 15.8}}; + +const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = { + {2., 29., 38.}, + {410., 492., 436.}}; + +const int TESLA_RT_INTERVAL = 250000; // 250ms between real time checks + +struct sample_t tesla_angle_meas; // last 3 steer angles + +// state of angle limits +int tesla_desired_angle_last = 0; // last desired steer angle +int16_t tesla_rt_angle_last = 0.; // last real time angle +uint32_t tesla_ts_angle_last = 0; + +int tesla_controls_allowed_last = 0; +int steer_allowed = 1; + +int tesla_brake_prev = 0; +int tesla_gas_prev = 0; +int tesla_speed = 0; +int current_car_time = -1; +int time_at_last_stalk_pull = -1; +int eac_status = 0; + +int tesla_ignition_started = 0; + +static int add_tesla_crc(CAN_FIFOMailBox_TypeDef *msg , int msg_len) { + //"""Calculate CRC8 using 1D poly, FF start, FF end""" + int crc_lookup[256] = { 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, + 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, + 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, + 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, + 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, + 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, + 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, + 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, + 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, + 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, + 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, + 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, + 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, + 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, + 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, + 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4 }; + int crc = 0xFF; + for (int x = 0; x < msg_len; x++) { + int v = 0; + if (x <= 3) { + v = (msg->RDLR >> (x * 8)) & 0xFF; + } else { + v = (msg->RDHR >> ( (x-4) * 8)) & 0xFF; + } + crc = crc_lookup[crc ^ v]; + } + crc = crc ^ 0xFF; + return crc; +} + +static int add_tesla_cksm(CAN_FIFOMailBox_TypeDef *msg , int msg_id, int msg_len) { + int cksm = (0xFF & msg_id) + (0xFF & (msg_id >> 8)); + for (int x = 0; x < msg_len; x++) { + int v = 0; + if (x <= 3) { + v = (msg->RDLR >> (x * 8)) & 0xFF; + } else { + v = (msg->RDHR >> ( (x-4) * 8)) & 0xFF; + } + cksm = (cksm + v) & 0xFF; + } + return cksm; +} + +// interp function that holds extreme values +float tesla_interpolate(struct lookup_t xy, float x) +{ + int size = sizeof(xy.x) / sizeof(xy.x[0]); + // x is lower than the first point in the x array. Return the first point + if (x <= xy.x[0]) + { + return xy.y[0]; + } + else + { + // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp + for (int i = 0; i < size - 1; i++) + { + if (x < xy.x[i + 1]) + { + float x0 = xy.x[i]; + float y0 = xy.y[i]; + float dx = xy.x[i + 1] - x0; + float dy = xy.y[i + 1] - y0; + // dx should not be zero as xy.x is supposed ot be monotonic + if (dx <= 0.) + dx = 0.0001; + return dy * (x - x0) / dx + y0; + } + } + // if no such point is found, then x > xy.x[size-1]. Return last point + return xy.y[size - 1]; + } +} + +static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) +{ + set_gmlan_digital_output(GMLAN_HIGH); + reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled + + //int bus_number = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + if (to_push->RIR & 4) + { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } + else + { + // Normal + addr = to_push->RIR >> 21; + } + + // Record the current car time in current_car_time (for use with double-pulling cruise stalk) + if (addr == 0x318) + { + int hour = (to_push->RDLR & 0x1F000000) >> 24; + int minute = (to_push->RDHR & 0x3F00) >> 8; + int second = (to_push->RDLR & 0x3F0000) >> 16; + current_car_time = (hour * 3600) + (minute * 60) + second; + } + + if (addr == 0x45) + { + // 6 bits starting at position 0 + int lever_position = (to_push->RDLR & 0x3F); + if (lever_position == 2) + { // pull forward + // activate openpilot + // TODO: uncomment the if to use double pull to activate + //if (current_car_time <= time_at_last_stalk_pull + 1 && current_car_time != -1 && time_at_last_stalk_pull != -1) { + controls_allowed = 1; + //} + time_at_last_stalk_pull = current_car_time; + } + else if (lever_position == 1) + { // push towards the back + // deactivate openpilot + controls_allowed = 0; + } + } + + // Detect drive rail on (ignition) (start recording) + if (addr == 0x348) + { + // GTW_status + int drive_rail_on = (to_push->RDLR & 0x0001); + tesla_ignition_started = drive_rail_on == 1; + } + + // exit controls on brake press + // DI_torque2::DI_brakePedal 0x118 + if (addr == 0x118) + { + // 1 bit at position 16 + if (((to_push->RDLR & 0x8000)) >> 15 == 1) + { + //disable break cancel by commenting line below + //controls_allowed = 0; + } + //get vehicle speed in m/2. Tesla gives MPH + tesla_speed = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05 - 25) * 1.609 / 3.6); + if (tesla_speed < 0) + { + tesla_speed = 0; + } + } + + // exit controls on EPAS error + // EPAS_sysStatus::EPAS_eacStatus 0x370 + if (addr == 0x370) + { + // if EPAS_eacStatus is not 1 or 2, disable control + eac_status = ((to_push->RDHR >> 21)) & 0x7; + // For human steering override we must not disable controls when eac_status == 0 + // Additional safety: we could only allow eac_status == 0 when we have human steerign allowed + if ((controls_allowed == 1) && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) + { + controls_allowed = 0; + puts("EPAS error! \n"); + } + } + //get latest steering wheel angle + if (addr == 0x00E) + { + int angle_meas_now = (int)((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1 - 819.2); + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last); + + // *** angle real time check + // add 1 to not false trigger the violation and multiply by 25 since the check is done every 250 ms and steer angle is updated at 100Hz + int rt_delta_angle_up = ((int)((tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.))); + int rt_delta_angle_down = ((int)((tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.))); + int highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up : rt_delta_angle_down); + int lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down : rt_delta_angle_up); + + if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) + { + tesla_rt_angle_last = angle_meas_now; + tesla_ts_angle_last = ts; + } + + // update array of samples + update_sample(&tesla_angle_meas, angle_meas_now); + + // check for violation; + if (max_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) + { + // We should not be able to STEER under these conditions + // Other sending is fine (to allow human override) + steer_allowed = 0; + puts("WARN: RT Angle - No steer allowed! \n"); + } + else + { + steer_allowed = 1; + } + + tesla_controls_allowed_last = controls_allowed; + } +} + +// all commands: gas/regen, friction brake and steering +// if controls_allowed and no pedals pressed +// allow all commands up to limit +// else +// block all commands that produce actuation + +static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) +{ + + uint32_t addr; + int angle_raw; + int desired_angle; + + addr = to_send->RIR >> 21; + + // do not transmit CAN message if steering angle too high + // DAS_steeringControl::DAS_steeringAngleRequest + if (addr == 0x488) + { + angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8); + desired_angle = angle_raw * 0.1 - 1638.35; + int16_t violation = 0; + int st_enabled = (to_send->RDLR & 0x400000) >> 22; + + if (st_enabled == 0) { + //steering is not enabled, do not check angles and do send + tesla_desired_angle_last = desired_angle; + return true; + } + + if (controls_allowed) + { + if (steer_allowed) + { + + // add 1 to not false trigger the violation + int delta_angle_up = (int)(tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.); + int delta_angle_down = (int)(tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.); + int highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up : delta_angle_down); + int lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down : delta_angle_up); + int TESLA_MAX_ANGLE = (int)(tesla_interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.); + + if (max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle)) + { + violation = 1; + controls_allowed = 0; + puts("Angle limit - delta! \n"); + } + if (max_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE)) + { + violation = 1; + controls_allowed = 0; + puts("Angle limit - max! \n"); + } + } + else + { + violation = 1; + controls_allowed = 0; + puts("Steering commads disallowed"); + } + } + + // makes no sense to have angle limits when not engaged + // if ((!controls_allowed) && max_limit_check(desired_angle, tesla_angle_meas.max + 1, tesla_angle_meas.min -1)) { + // violation = 1; + // puts("Angle limit when not engaged! \n"); + // } + + tesla_desired_angle_last = desired_angle; + + if (violation) + { + return false; + } + return true; + } + return true; +} + +static int tesla_tx_lin_hook(int lin_num, uint8_t *data, int len) +{ + // LIN is not used on the Tesla + return false; +} + +static void tesla_init(int16_t param) +{ + controls_allowed = 0; + tesla_ignition_started = 0; + gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled +} + +static int tesla_ign_hook() +{ + return tesla_ignition_started; +} + +static void tesla_fwd_to_radar_as_is(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_fwd->RIR | 1; // TXRQ + to_send.RDTR = to_fwd->RDTR; + to_send.RDLR = to_fwd->RDLR; + to_send.RDHR = to_fwd->RDHR; + can_send(&to_send, bus_num); +} + + + +static void tesla_fwd_to_radar_modded(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + int32_t addr = to_fwd->RIR >> 21; + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_fwd->RIR | 1; // TXRQ + to_send.RDTR = to_fwd->RDTR; + to_send.RDLR = to_fwd->RDLR; + to_send.RDHR = to_fwd->RDHR; + uint32_t addr_mask = 0x001FFFFF; + //now modd + if (addr == 0x405 ) + { + to_send.RIR = (0x2B9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + if ((to_send.RDLR & 0x10) == 0x10) + { + int rec = to_send.RDLR & 0xFF; + if (rec == 0x12) { + to_send.RDHR = 0x36333537; + to_send.RDLR = 0x38304600 | rec; + } + if (rec == 0x10) { + to_send.RDLR = 0x00000000 | rec; + to_send.RDHR = 0x4a593500; + } + if (rec == 0x11) { + to_send.RDLR = 0x31415300 | rec; + to_send.RDHR = 0x46373248; + } + } + } + if (addr == 0x398 ) + { + //change frontradarHW = 1 and dashw = 1 + //SG_ GTW_dasHw : 7|2@0+ (1,0) [0|0] "" NEO + //SG_ GTW_parkAssistInstalled : 11|2@0+ (1,0) [0|0] "" NEO + to_send.RDLR = to_send.RDLR & 0xFFFFF33F; + to_send.RDLR = to_send.RDLR | 0x440; + to_send.RDHR = to_send.RDHR & 0xCFFFFFFF; + to_send.RDHR = to_send.RDHR | 0x10000000; + to_send.RIR = (0x2A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + if (addr == 0x00E ) + { + to_send.RIR = (0x199 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + if (addr == 0x20A ) + { + to_send.RIR = (0x159 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + if (addr == 0x115 ) + { + + int counter = ((to_fwd->RDLR & 0x000F0000) >> 16 ) & 0x0F; + + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x06; + to_send.RIR = (0x129 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDLR = 0x00000000 ; + int cksm = (0x16 + (counter << 4)) & 0xFF; + to_send.RDHR = (cksm << 8 ) + (counter << 4); + can_send(&to_send, bus_num); + + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x08; + to_send.RIR = (0x149 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDLR = 0x6A022600; + cksm = (0x95 + (counter << 4)) & 0xFF; + to_send.RDHR = 0x000F04AA | (counter << 20) | (cksm << 24); + can_send(&to_send, bus_num); + + + + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x05; + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDLR = 0x000C0000 | (counter << 28); + cksm = (0x49 + 0x0C + (counter << 4)) & 0xFF; + to_send.RDHR = cksm; + } + + if (addr == 0x118 ) + { + to_send.RIR = (0x119 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + int counter = to_fwd->RDHR & 0x0F; + to_send.RIR = (0x169 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x08; + int32_t speed_kph = (((0xFFF0000 & to_send.RDLR) >> 16) * 0.05 -25) * 1.609; + if (speed_kph < 0) { + speed_kph = 0; + } + speed_kph = 20; //force it at 20 kph for debug + speed_kph = (int)(speed_kph/0.04) & 0x1FFF; + to_send.RDLR = (speed_kph | (speed_kph << 13) | (speed_kph << 26)) & 0xFFFFFFFF; + to_send.RDHR = ((speed_kph >> 6) | (speed_kph << 7) | (counter << 20)) & 0x00FFFFFF; + int cksm = 0x76; + cksm = (cksm + (to_send.RDLR & 0xFF) + ((to_send.RDLR >> 8) & 0xFF) + ((to_send.RDLR >> 16) & 0xFF) + ((to_send.RDLR >> 24) & 0xFF)) & 0xFF; + cksm = (cksm + (to_send.RDHR & 0xFF) + ((to_send.RDHR >> 8) & 0xFF) + ((to_send.RDHR >> 16) & 0xFF) + ((to_send.RDHR >> 24) & 0xFF)) & 0xFF; + to_send.RDHR = to_send.RDHR | (cksm << 24); + } + if (addr == 0x108 ) + { + to_send.RIR = (0x109 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + if (addr == 0x308 ) + { + to_send.RIR = (0x209 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + if (addr == 0x45 ) + { + to_send.RIR = (0x219 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDLR = to_send.RDLR & 0xFFFF00FF; + to_send.RDHR = to_send.RDHR & 0x00FFFFFF; + int crc = add_tesla_crc(&to_send,7); + to_send.RDHR = to_send.RDHR | (crc << 24); + } + if (addr == 0x148 ) + { + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + if (addr == 0x30A) + { + to_send.RIR = (0x2D9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + } + can_send(&to_send, bus_num); +} + +static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) +{ + + int32_t addr = to_fwd->RIR >> 21; + + if (bus_num == 0) + { + + //check all messages we need to also send to radar unmoddified + /*if ((addr == 0x649 ) || (addr == 0x730 ) || (addr == 0x647 ) || (addr == 0x644 ) || (addr == 0x645 ) || (addr == 0x7DF ) || (addr == 0x64D ) || (addr == 0x64C ) || (addr == 0x643 ) || + (addr == 0x64E ) || (addr == 0x671 ) || (addr == 0x674 ) || (addr == 0x675 ) || (addr == 0x672 ) || (addr == 0x673 ) || (addr == 0x7F1 ) || (addr == 0x641 ) || (addr == 0x790 ) || (addr == 0x64B ) || + (addr == 0x64F ) || (addr == 0x71800 ) || (addr == 0x72B)) */ + /*if ((addr == 0x641) || (addr == 0x537)) + { + //these messages are just forwarded with the same IDs + tesla_fwd_to_radar_as_is(1, to_fwd); + }*/ + + //check all messages we need to also send to radar moddified + //145 does not exist, we use 115 at the same frequency to trigger + //175 does not exist, we use 118 at the same frequency to trigger and pass vehicle speed + if ((addr == 0x405 ) || (addr == 0x398 ) || (addr == 0xE ) || (addr == 0x20A ) || (addr == 0x118 ) || (addr == 0x108 ) || (addr == 0x308 ) || + (addr == 0x115 ) || (addr == 0x45 ) || (addr == 0x148 ) || (addr == 0x30A)) + { + tesla_fwd_to_radar_modded(1, to_fwd); + } + + // change inhibit of GTW_epasControl + if (addr == 0x101) + { + to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque) + int checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF; + to_fwd->RDLR = to_fwd->RDLR & 0xFFFF; + to_fwd->RDLR = to_fwd->RDLR + (checksum << 16); + return 2; + } + + // remove EPB_epasControl + if (addr == 0x214) + { + return -1; + } + + return 2; // Custom EPAS bus + } + + if ((bus_num != 0) && (bus_num != 2)) { + //everything but the radar data 0x300-0x3FF will be forwarded to can 0 + if ((addr > 0x300) && (addr <= 0x3FF)){ + return -1; + } + return 0; + } + + if (bus_num == 2) + { + + // remove GTW_epasControl in forwards + if (addr == 0x101) + { + return -1; + } + + // remove Pedal in forwards + if ((addr == 0x520) || (addr == 0x521)) { + return -1; + } + + return 0; // Chassis CAN + } + return -1; +} + +const safety_hooks tesla_hooks = { + .init = tesla_init, + .rx = tesla_rx_hook, + .tx = tesla_tx_hook, + .tx_lin = tesla_tx_lin_hook, + .ignition = tesla_ign_hook, + .fwd = tesla_fwd_hook, +}; diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index c4d57956314384..6ddf2952d10f11 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -1,3 +1,6 @@ +int toyota_no_dsu_car = 0; // ch-r and camry don't have the DSU +int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high? + // global torque limit const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever @@ -10,141 +13,95 @@ const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque // real time torque limit to prevent controls spamming // the real time limit is 1500/sec const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real time checks -const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks +const int 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_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file - -// global actuation limit states +// global actuation limit state +int toyota_actuation_limits = 1; // by default steer limits are imposed int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file -// states -int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high? -int toyota_camera_forwarded = 0; // should we forward the camera bus? +// state of torque limits 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 -int toyota_gas_prev = 0; struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - // get eps motor torque (0.66 factor in dbc) - if (addr == 0x260) { + if ((to_push->RIR>>21) == 0x260) { int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF)); torque_meas_new = to_signed(torque_meas_new, 16); // scale by dbc_factor torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; + // increase torque_meas by 1 to be conservative on rounding + torque_meas_new += (torque_meas_new > 0 ? 1 : -1); + // update array of sample update_sample(&toyota_torque_meas, torque_meas_new); - - // increase torque_meas by 1 to be conservative on rounding - toyota_torque_meas.min--; - toyota_torque_meas.max++; } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1D2) { - // 5th bit is CRUISE_ACTIVE - int cruise_engaged = to_push->RDLR & 0x20; - if (!cruise_engaged) { - controls_allowed = 0; - } + if ((to_push->RIR>>21) == 0x1D2) { + // 4 bits: 55-52 + int cruise_engaged = to_push->RDHR & 0xF00000; if (cruise_engaged && !toyota_cruise_engaged_last) { controls_allowed = 1; - } - toyota_cruise_engaged_last = cruise_engaged; - } - - // exit controls on rising edge of interceptor gas press - if (addr == 0x201) { - gas_interceptor_detected = 1; - int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && - long_controls_allowed) { + } else if (!cruise_engaged) { controls_allowed = 0; } - gas_interceptor_prev = gas_interceptor; - } - - // exit controls on rising edge of gas press - if (addr == 0x2C1) { - int gas = (to_push->RDHR >> 16) & 0xFF; - if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected && long_controls_allowed) { - controls_allowed = 0; - } - toyota_gas_prev = gas; + toyota_cruise_engaged_last = cruise_engaged; } - // msgs are only on bus 2 if panda is connected to frc - if (bus == 2) { - toyota_camera_forwarded = 1; + int bus = (to_push->RDTR >> 4) & 0xF; + // 0x680 is a radar msg only found in dsu-less cars + if ((to_push->RIR>>21) == 0x680 && (bus == 1)) { + toyota_no_dsu_car = 1; } // 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high - if ((addr == 0x2E4) && (bus == 0)) { + if ((to_push->RIR>>21) == 0x2E4 && (bus == 0)) { toyota_giraffe_switch_1 = 1; } + } static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); - int bus = GET_BUS(to_send); - // Check if msg is sent on BUS 0 - if (bus == 0) { + if (((to_send->RDTR >> 4) & 0xF) == 0) { // no IPAS in non IPAS mode - if ((addr == 0x266) || (addr == 0x167)) { - tx = 0; - } - - // GAS PEDAL: safety check - if (addr == 0x200) { - if (!controls_allowed || !long_controls_allowed) { - if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) { - tx = 0; - } - } - } + if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) return false; // ACCEL: safety check on byte 1-2 - if (addr == 0x343) { + if ((to_send->RIR>>21) == 0x343) { int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF); desired_accel = to_signed(desired_accel, 16); - if (!controls_allowed || !long_controls_allowed) { - if (desired_accel != 0) { - tx = 0; - } - } - bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); - if (violation) { - tx = 0; + if (controls_allowed && toyota_actuation_limits) { + int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); + if (violation) return 0; + } else if (!controls_allowed && (desired_accel != 0)) { + return 0; } } // STEER: safety check on bytes 2-3 - if (addr == 0x2E4) { + if ((to_send->RIR>>21) == 0x2E4) { int desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF); desired_torque = to_signed(desired_torque, 16); - bool violation = 0; + int violation = 0; uint32_t ts = TIM2->CNT; - if (controls_allowed) { + // only check if controls are allowed and actuation_limits are imposed + if (controls_allowed && toyota_actuation_limits) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); @@ -180,43 +137,31 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } if (violation) { - tx = 0; + return false; } } } // 1 allows the message through - return tx; + return true; } static void toyota_init(int16_t param) { controls_allowed = 0; + toyota_actuation_limits = 1; toyota_giraffe_switch_1 = 0; - toyota_camera_forwarded = 0; toyota_dbc_eps_torque_factor = param; } static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - int bus_fwd = -1; - if (toyota_camera_forwarded && !toyota_giraffe_switch_1) { - if (bus_num == 0) { - bus_fwd = 2; - } - if (bus_num == 2) { - int addr = GET_ADDR(to_fwd); - // block stock lkas messages and stock acc messages (if OP is doing ACC) - // in TSS2, 0.191 is LTA which we need to block to avoid controls collision - int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191)); - // in TSS2 the camera does ACC as well, so filter 0x343 - int is_acc_msg = (addr == 0x343); - int block_msg = is_lkas_msg || (is_acc_msg && long_controls_allowed); - if (!block_msg) { - bus_fwd = 0; - } - } + // forward cam to radar and viceversa if car is dsu-less, except lkas cmd and hud + if ((bus_num == 0 || bus_num == 2) && toyota_no_dsu_car && !toyota_giraffe_switch_1) { + int addr = to_fwd->RIR>>21; + bool is_lkas_msg = (addr == 0x2E4 || addr == 0x412) && bus_num == 2; + return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2); } - return bus_fwd; + return -1; } const safety_hooks toyota_hooks = { @@ -227,3 +172,19 @@ const safety_hooks toyota_hooks = { .ignition = default_ign_hook, .fwd = toyota_fwd_hook, }; + +static void toyota_nolimits_init(int16_t param) { + controls_allowed = 0; + toyota_actuation_limits = 0; + toyota_giraffe_switch_1 = 0; + toyota_dbc_eps_torque_factor = param; +} + +const safety_hooks toyota_nolimits_hooks = { + .init = toyota_nolimits_init, + .rx = toyota_rx_hook, + .tx = toyota_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = toyota_fwd_hook, +}; diff --git a/panda/board/safety/safety_toyota_ipas.h b/panda/board/safety/safety_toyota_ipas.h index 99e6dae0587a9c..ff4158e3c77c3b 100644 --- a/panda/board/safety/safety_toyota_ipas.h +++ b/panda/board/safety/safety_toyota_ipas.h @@ -35,9 +35,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check standard toyota stuff as well toyota_rx_hook(to_push); - int addr = GET_ADDR(to_push); - - if (addr == 0x260) { + if ((to_push->RIR>>21) == 0x260) { // get driver steering torque int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF)); @@ -46,7 +44,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // get steer angle - if (addr == 0x25) { + if ((to_push->RIR>>21) == 0x25) { int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8); uint32_t ts = TIM2->CNT; @@ -57,10 +55,10 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // *** angle real time check // add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz - int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG) + 1.))); - int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG) + 1.))); - int highest_rt_angle = rt_angle_last + ((rt_angle_last > 0) ? rt_delta_angle_up : rt_delta_angle_down); - int lowest_rt_angle = rt_angle_last - ((rt_angle_last > 0) ? rt_delta_angle_down : rt_delta_angle_up); + int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG + 1.))); + int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG + 1.))); + int highest_rt_angle = rt_angle_last + (rt_angle_last > 0? rt_delta_angle_up:rt_delta_angle_down); + int lowest_rt_angle = rt_angle_last - (rt_angle_last > 0? rt_delta_angle_down:rt_delta_angle_up); // every RT_INTERVAL or when controls are turned on, set the new limits uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); @@ -80,12 +78,12 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // get speed - if (addr == 0xb4) { + if ((to_push->RIR>>21) == 0xb4) { speed = ((float) (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF))) * 0.01 / 3.6; } // get ipas state - if (addr == 0x262) { + if ((to_push->RIR>>21) == 0x262) { ipas_state = (to_push->RDLR & 0xf); } @@ -99,34 +97,25 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int bypass_standard_tx_hook = 0; - int bus = GET_BUS(to_send); - int addr = GET_ADDR(to_send); - // Check if msg is sent on BUS 0 - if (bus == 0) { + if (((to_send->RDTR >> 4) & 0xF) == 0) { // STEER ANGLE - if ((addr == 0x266) || (addr == 0x167)) { + if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) { angle_control = 1; // we are in angle control mode int desired_angle = ((to_send->RDLR & 0xf) << 8) + ((to_send->RDLR & 0xff00) >> 8); int ipas_state_cmd = ((to_send->RDLR & 0xff) >> 4); - bool violation = 0; + int16_t violation = 0; desired_angle = to_signed(desired_angle, 12); if (controls_allowed) { // add 1 to not false trigger the violation - float delta_angle_float; - delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG) + 1.; - int delta_angle_up = (int) (delta_angle_float); - delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG) + 1.; - int delta_angle_down = (int) (delta_angle_float); - - int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); - int lowest_desired_angle = desired_angle_last - ((desired_angle_last > 0) ? delta_angle_down : delta_angle_up); + int delta_angle_up = (int) (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG + 1.); + int delta_angle_down = (int) (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG + 1.); + int highest_desired_angle = desired_angle_last + (desired_angle_last > 0? delta_angle_up:delta_angle_down); + int lowest_desired_angle = desired_angle_last - (desired_angle_last > 0? delta_angle_down:delta_angle_up); if ((desired_angle > highest_desired_angle) || (desired_angle < lowest_desired_angle)){ violation = 1; @@ -145,18 +134,15 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { desired_angle_last = desired_angle; if (violation) { - tx = 0; + return false; } - bypass_standard_tx_hook = 1; - } - } - // check standard toyota stuff as well if addr isn't IPAS related - if (!bypass_standard_tx_hook) { - tx &= toyota_tx_hook(to_send); + return true; + } } - return tx; + // check standard toyota stuff as well + return toyota_tx_hook(to_send); } const safety_hooks toyota_ipas_hooks = { @@ -167,3 +153,4 @@ const safety_hooks toyota_ipas_hooks = { .ignition = default_ign_hook, .fwd = toyota_fwd_hook, }; + diff --git a/panda/board/spi_flasher.h b/panda/board/spi_flasher.h index 94d41c7e326c64..179a095b970ef9 100644 --- a/panda/board/spi_flasher.h +++ b/panda/board/spi_flasher.h @@ -2,11 +2,9 @@ uint32_t *prog_ptr = NULL; int unlocked = 0; -#ifdef uart_ring void debug_ring_callback(uart_ring *ring) {} -#endif -int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) { +int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { int resp_len = 0; // flasher machine @@ -48,7 +46,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) break; // **** 0xd0: fetch serial number case 0xd0: - #ifdef STM32F4 + #ifdef PANDA // addresses are OTP if (setup->b.wValue.w == 1) { memcpy(resp, (void *)0x1fff79c0, 0x10); @@ -80,7 +78,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) break; // **** 0xd6: get version case 0xd6: - COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN); + COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN) memcpy(resp, gitversion, sizeof(gitversion)); resp_len = sizeof(gitversion); break; @@ -92,8 +90,8 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) return resp_len; } -int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { return 0; } -void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) { } +int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; } +void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) { } int is_enumerated = 0; void usb_cb_enumeration_complete() { @@ -101,7 +99,7 @@ void usb_cb_enumeration_complete() { is_enumerated = 1; } -void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) { +void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) { set_led(LED_RED, 0); for (int i = 0; i < len/4; i++) { // program byte 1 @@ -134,7 +132,6 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) { #ifdef PEDAL -#include "drivers/llcan.h" #define CAN CAN1 #define CAN_BL_INPUT 0x1 @@ -242,7 +239,7 @@ void CAN1_RX0_IRQHandler() { } void CAN1_SCE_IRQHandler() { - llcan_clear_send(CAN); + can_sce(CAN); } #endif @@ -267,8 +264,8 @@ void soft_flasher_start() { set_can_enable(CAN1, 1); // init can - llcan_set_speed(CAN1, 5000, false, false); - llcan_init(CAN1); + can_silent = ALL_CAN_LIVE; + can_init(0); #endif // A4,A5,A6,A7: setup SPI diff --git a/panda/boardesp/elm327.c b/panda/boardesp/elm327.c index 58ac4c86386aa1..6c18c4463b484d 100644 --- a/panda/boardesp/elm327.c +++ b/panda/boardesp/elm327.c @@ -10,8 +10,8 @@ //#define ELM_DEBUG -#define MIN(a,b) ((a) < (b) ? (a) : (b)) -#define MAX(a,b) ((a) > (b) ? (a) : (b)) +#define min(a,b) ((a) < (b) ? (a) : (b)) +#define max(a,b) ((a) > (b) ? (a) : (b)) int ICACHE_FLASH_ATTR spi_comm(char *dat, int len, uint32_t *recvData, int recvDataLen); #define ELM_PORT 35000 @@ -855,7 +855,7 @@ static void ICACHE_FLASH_ATTR elm_process_obd_cmd_LINFast(const elm_protocol_t* panda_kline_wakeup_pulse(); } else { - bytelen = MIN(bytelen, 7); + bytelen = min(bytelen, 7); for(int i = 0; i < bytelen; i++){ msg.dat[i] = elm_decode_hex_byte(&cmd[i*2]); msg.dat[bytelen] += msg.dat[i]; @@ -1059,7 +1059,7 @@ static void ICACHE_FLASH_ATTR elm_process_obd_cmd_ISO15765(const elm_protocol_t* return; } - msg.len = MIN(msg.len, 7); + msg.len = min(msg.len, 7); for(int i = 0; i < msg.len; i++) msg.dat[i] = elm_decode_hex_byte(&cmd[i*2]); @@ -1398,7 +1398,7 @@ static void ICACHE_FLASH_ATTR elm_process_at_cmd(char *cmd, uint16_t len) { } tmp = elm_decode_hex_byte(&cmd[2]); - elm_mode_keepalive_period = tmp ? MAX(tmp, 0x20) * 20 : 0; + elm_mode_keepalive_period = tmp ? max(tmp, 0x20) * 20 : 0; if(lin_bus_initialized){ os_timer_disarm(&elm_proto_aux_timeout); diff --git a/panda/boardesp/proxy.c b/panda/boardesp/proxy.c index 65586d6096c2bb..bb89743c198ec9 100644 --- a/panda/boardesp/proxy.c +++ b/panda/boardesp/proxy.c @@ -9,12 +9,12 @@ #include "driver/uart.h" #include "crypto/sha.h" -#define MIN(a,b) \ +#define min(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ _a < _b ? _a : _b; }) -#define MAX(a,b) \ +#define max(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ _a > _b ? _a : _b; }) diff --git a/panda/boardesp/webserver.c b/panda/boardesp/webserver.c index f855f88c919b64..9266b08f70f605 100644 --- a/panda/boardesp/webserver.c +++ b/panda/boardesp/webserver.c @@ -14,8 +14,8 @@ #include "obj/gitversion.h" #include "obj/cert.h" -#define MAX(a,b) ((a) > (b) ? (a) : (b)) -#define MIN(a,b) ((a) < (b) ? (a) : (b)) +#define max(a,b) ((a) > (b) ? (a) : (b)) +#define min(a,b) ((a) < (b) ? (a) : (b)) #define espconn_send_string(conn, x) espconn_send(conn, x, strlen(x)) #define MAX_RESP 0x800 @@ -116,7 +116,7 @@ void ICACHE_FLASH_ATTR st_flash() { // real content length will always be 0x10 aligned os_printf("st_flash: flashing\n"); for (int i = 0; i < real_content_length; i += 0x10) { - int rl = MIN(0x10, real_content_length-i); + int rl = min(0x10, real_content_length-i); usb_cmd(2, rl, 0, 0, 0, &st_firmware[i]); system_soft_wdt_feed(); } @@ -288,7 +288,7 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) { } } else if (state == RECEIVING_ST_FIRMWARE) { os_printf("receiving st firmware: %d/%d\n", len, content_length); - memcpy(st_firmware_ptr, data, MIN(content_length, len)); + memcpy(st_firmware_ptr, data, min(content_length, len)); st_firmware_ptr += len; content_length -= len; @@ -333,7 +333,7 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) { SHA_init(&ctx); for (ll = start_address; ll < esp_address-RSANUMBYTES; ll += 0x80) { spi_flash_read(ll, dat, 0x80); - SHA_update(&ctx, dat, MIN((esp_address-RSANUMBYTES)-ll, 0x80)); + SHA_update(&ctx, dat, min((esp_address-RSANUMBYTES)-ll, 0x80)); } memcpy(digest, SHA_final(&ctx), SHA_DIGEST_SIZE); diff --git a/panda/common/version.mk b/panda/common/version.mk index cc66efaa6dbc4b..cc42223da3d03d 100644 --- a/panda/common/version.mk +++ b/panda/common/version.mk @@ -1,20 +1,18 @@ ifeq ($(RELEASE),1) - BUILD_TYPE = "RELEASE" + BUILD_TYPE = "RELEASE" else - BUILD_TYPE = "DEBUG" + BUILD_TYPE = "DEBUG" endif -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) - -ifneq ($(wildcard $(SELF_DIR)/../.git/HEAD),) -obj/gitversion.h: $(SELF_DIR)/../VERSION $(SELF_DIR)/../.git/HEAD $(SELF_DIR)/../.git/index - echo "const uint8_t gitversion[] = \"$(shell cat $(SELF_DIR)/../VERSION)-$(BUILDER)-$(shell git rev-parse --short=8 HEAD)-$(BUILD_TYPE)\";" > $@ +ifneq ($(wildcard ../.git/HEAD),) +obj/gitversion.h: ../VERSION ../.git/HEAD ../.git/index + echo "const uint8_t gitversion[] = \"$(shell cat ../VERSION)-$(shell git rev-parse --short=8 HEAD)-$(BUILD_TYPE)\";" > $@ else -ifneq ($(wildcard $(SELF_DIR)/../../.git/modules/panda/HEAD),) -obj/gitversion.h: $(SELF_DIR)/../VERSION $(SELF_DIR)/../../.git/modules/panda/HEAD $(SELF_DIR)/../../.git/modules/panda/index - echo "const uint8_t gitversion[] = \"$(shell cat $(SELF_DIR)/../VERSION)-$(BUILDER)-$(shell git rev-parse --short=8 HEAD)-$(BUILD_TYPE)\";" > $@ +ifneq ($(wildcard ../../.git/modules/panda/HEAD),) +obj/gitversion.h: ../VERSION ../../.git/modules/panda/HEAD ../../.git/modules/panda/index + echo "const uint8_t gitversion[] = \"$(shell cat ../VERSION)-$(shell git rev-parse --short=8 HEAD)-$(BUILD_TYPE)\";" > $@ else -obj/gitversion.h: $(SELF_DIR)/../VERSION - echo "const uint8_t gitversion[] = \"$(shell cat $(SELF_DIR)/../VERSION)-$(BUILDER)-unknown-$(BUILD_TYPE)\";" > $@ +obj/gitversion.h: ../VERSION + echo "const uint8_t gitversion[] = \"$(shell cat ../VERSION)-unknown-$(BUILD_TYPE)\";" > $@ endif endif diff --git a/panda/crypto/sha.c b/panda/crypto/sha.c index 8e1715525c68a2..47676df4b6d8a6 100644 --- a/panda/crypto/sha.c +++ b/panda/crypto/sha.c @@ -127,36 +127,10 @@ const uint8_t* SHA_final(SHA_CTX* ctx) { while ((ctx->count & 63) != 56) { SHA_update(ctx, (uint8_t*)"\0", 1); } - - /* Hack - right shift operator with non const argument requires - * libgcc.a which is missing in EON - * thus expanding for loop from - - for (i = 0; i < 8; ++i) { - uint8_t tmp = (uint8_t) (cnt >> ((7 - i) * 8)); - SHA_update(ctx, &tmp, 1); - } - - to - */ - - uint8_t tmp = 0; - tmp = (uint8_t) (cnt >> ((7 - 0) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 1) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 2) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 3) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 4) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 5) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 6) * 8)); - SHA_update(ctx, &tmp, 1); - tmp = (uint8_t) (cnt >> ((7 - 7) * 8)); - SHA_update(ctx, &tmp, 1); + for (i = 0; i < 8; ++i) { + uint8_t tmp = (uint8_t) (cnt >> ((7 - i) * 8)); + SHA_update(ctx, &tmp, 1); + } for (i = 0; i < 5; i++) { uint32_t tmp = ctx->state[i]; diff --git a/panda/drivers/windows/pandaJ2534DLL/MessageRx.h b/panda/drivers/windows/pandaJ2534DLL/MessageRx.h index 2af24364ff3a36..e22278d4d0a934 100644 --- a/panda/drivers/windows/pandaJ2534DLL/MessageRx.h +++ b/panda/drivers/windows/pandaJ2534DLL/MessageRx.h @@ -21,7 +21,7 @@ class MessageRx } this->next_part = (this->next_part + 1) % 0x10; - unsigned int payload_len = MIN(expected_size - msg.size(), max_packet_size); + unsigned int payload_len = min(expected_size - msg.size(), max_packet_size); if (piece.size() < payload_len) { //A frame was received that could have held more data. //No examples of this protocol show that happening, so diff --git a/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp b/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp index 4cda1fa2e27fb3..1b961579e079ab 100644 --- a/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp +++ b/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp @@ -170,7 +170,7 @@ DWORD PandaJ2534Device::msg_tx_thread() { } else { //Ran out of things that need to be sent now. Sleep! auto time_diff = std::chrono::duration_cast (this->task_queue.front()->expire - std::chrono::steady_clock::now()); - sleepDuration = MAX(1, time_diff.count()); + sleepDuration = max(1, time_diff.count()); goto break_flow_ctrl_loop; } } diff --git a/panda/examples/query_vin_and_stats.py b/panda/examples/query_vin_and_stats.py index f3d6c198aff9f1..cd2185b4a6f11f 100755 --- a/panda/examples/query_vin_and_stats.py +++ b/panda/examples/query_vin_and_stats.py @@ -3,7 +3,7 @@ import struct from panda import Panda from hexdump import hexdump -from panda.python.isotp import isotp_send, isotp_recv +from isotp import isotp_send, isotp_recv # 0x7e0 = Toyota # 0x18DB33F1 for Honda? @@ -33,7 +33,7 @@ def get_supported_pids(): panda.can_clear(0) # 09 02 = Get VIN - isotp_send(panda, "\x09\x02", 0x7df) + isotp_send(panda, "\x09\x02", 0x7e0) ret = isotp_recv(panda, 0x7e8) hexdump(ret) print "VIN: %s" % ret[2:] diff --git a/panda/python/__init__.py b/panda/python/__init__.py index bfca642e8256f7..86b76285ee0ddb 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -8,7 +8,6 @@ import os import time import traceback -import subprocess from dfu import PandaDFU from esptool import ESPROM, CesantaFlasher from flash_release import flash_release @@ -26,13 +25,7 @@ def build_st(target, mkfile="Makefile"): from panda import BASEDIR - cmd = 'cd %s && make -f %s clean && make -f %s %s >/dev/null' % (os.path.join(BASEDIR, "board"), mkfile, mkfile, target) - try: - output = subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True) - except subprocess.CalledProcessError as exception: - output = exception.output - returncode = exception.returncode - raise + assert(os.system('cd %s && make -f %s clean && make -f %s %s >/dev/null' % (os.path.join(BASEDIR, "board"), mkfile, mkfile, target)) == 0) def parse_can_buffer(dat): ret = [] @@ -112,14 +105,7 @@ class Panda(object): SAFETY_NOOUTPUT = 0 SAFETY_HONDA = 1 SAFETY_TOYOTA = 2 - SAFETY_GM = 3 SAFETY_HONDA_BOSCH = 4 - SAFETY_FORD = 5 - SAFETY_CADILLAC = 6 - SAFETY_HYUNDAI = 7 - SAFETY_TESLA = 8 - SAFETY_CHRYSLER = 9 - SAFETY_TOYOTA_IPAS = 0x1335 SAFETY_TOYOTA_NOLIMITS = 0x1336 SAFETY_ALLOUTPUT = 0x1337 SAFETY_ELM327 = 0xE327 @@ -182,7 +168,6 @@ def connect(self, claim=True, wait=False): traceback.print_exc() if wait == False or self._handle != None: break - context = usb1.USBContext() #New context needed so new devices show up assert(self._handle != None) print("connected") @@ -251,7 +236,6 @@ def flash_static(handle, code): pass def flash(self, fn=None, code=None, reconnect=True): - print("flash: main version is " + self.get_version()) if not self.bootstub: self.reset(enter_bootstub=True) assert(self.bootstub) @@ -272,7 +256,7 @@ def flash(self, fn=None, code=None, reconnect=True): code = f.read() # get version - print("flash: bootstub version is " + self.get_version()) + print("flash: version is "+self.get_version()) # do flash Panda.flash_static(self._handle, code) @@ -281,14 +265,11 @@ def flash(self, fn=None, code=None, reconnect=True): if reconnect: self.reconnect() - def recover(self, timeout=None): + def recover(self): self.reset(enter_bootloader=True) - t_start = time.time() while len(PandaDFU.list()) == 0: print("waiting for DFU...") time.sleep(0.1) - if timeout is not None and (time.time() - t_start) > timeout: - return False dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(self._serial)) dfu.recover() @@ -296,7 +277,6 @@ def recover(self, timeout=None): # reflash after recover self.connect(True, True) self.flash() - return True @staticmethod def flash_ota_st(): @@ -305,9 +285,8 @@ def flash_ota_st(): return ret==0 @staticmethod - def flash_ota_wifi(release=False): - release_str = "RELEASE=1" if release else "" - ret = os.system("cd {} && make clean && {} make ota".format(os.path.join(BASEDIR, "boardesp"),release_str)) + def flash_ota_wifi(): + ret = os.system("cd %s && make clean && make ota" % (os.path.join(BASEDIR, "boardesp"))) time.sleep(1) return ret==0 @@ -396,10 +375,6 @@ def set_can_loopback(self, enable): # set can loopback mode for all buses self._handle.controlWrite(Panda.REQUEST_OUT, 0xe5, int(enable), 0, b'') - def set_can_enable(self, bus_num, enable): - # sets the can transciever enable pin - self._handle.controlWrite(Panda.REQUEST_OUT, 0xf4, int(bus_num), int(enable), b'') - def set_can_speed_kbps(self, bus, speed): self._handle.controlWrite(Panda.REQUEST_OUT, 0xde, bus, int(speed*10), b'') @@ -559,3 +534,4 @@ def kline_recv(self, bus=2): msg = self.kline_ll_recv(2, bus=bus) msg += self.kline_ll_recv(ord(msg[1])-2, bus=bus) return msg + diff --git a/panda/python/dfu.py b/panda/python/dfu.py index 02deed47bbe2ec..782b4dca429906 100644 --- a/panda/python/dfu.py +++ b/panda/python/dfu.py @@ -18,7 +18,7 @@ def __init__(self, dfu_serial): for device in context.getDeviceList(skip_on_error=True): if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11: try: - this_dfu_serial = device.open().getASCIIStringDescriptor(3) + this_dfu_serial = device._getASCIIStringDescriptor(3) except Exception: continue if this_dfu_serial == dfu_serial or dfu_serial is None: @@ -35,7 +35,7 @@ def list(): for device in context.getDeviceList(skip_on_error=True): if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11: try: - dfu_serials.append(device.open().getASCIIStringDescriptor(3)) + dfu_serials.append(device._getASCIIStringDescriptor(3)) except Exception: pass except Exception: @@ -73,7 +73,7 @@ def erase(self, address): def program(self, address, dat, block_size=None): if block_size == None: block_size = len(dat) - + # Set Address Pointer self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, "\x21" + struct.pack("I", address)) self.status() @@ -119,3 +119,4 @@ def reset(self): stat = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)) except Exception: pass + diff --git a/panda/python/esptool.py b/panda/python/esptool.py index e68e6cd6e06779..970aa3d4d83fd5 100755 --- a/panda/python/esptool.py +++ b/panda/python/esptool.py @@ -1216,7 +1216,7 @@ def add_spi_flash_subparsers(parent, auto_detect=False): operation_func = globals()[args.operation] operation_args,_,_,_ = inspect.getargspec(operation_func) if operation_args[0] == 'esp': # operation function takes an ESPROM connection object - initial_baud = MIN(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate + initial_baud = min(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate esp = ESPROM(args.port, initial_baud) esp.connect() operation_func(esp, args) diff --git a/panda/python/update.py b/panda/python/update.py index ce730e4919119e..d393df9fb44152 100755 --- a/panda/python/update.py +++ b/panda/python/update.py @@ -8,8 +8,6 @@ def ensure_st_up_to_date(): with open(os.path.join(BASEDIR, "VERSION")) as f: repo_version = f.read() - repo_version += "-EON" if os.path.isfile('/EON') else "-DEV" - panda = None panda_dfu = None should_flash_recover = False @@ -26,7 +24,7 @@ def ensure_st_up_to_date(): if len(panda_dfu) > 0: panda_dfu = PandaDFU(panda_dfu[0]) panda_dfu.recover() - + print "waiting for board..." time.sleep(1) diff --git a/panda/requirements.txt b/panda/requirements.txt index ad6b4c76e054f3..e968bfb6bad43e 100644 --- a/panda/requirements.txt +++ b/panda/requirements.txt @@ -1,7 +1,4 @@ -libusb1 == 1.6.6 +libusb1 hexdump pycrypto tqdm -nose -parameterized -requests diff --git a/panda/run_automated_tests.sh b/panda/run_automated_tests.sh index 4e07d329cace20..d7b7541902d9eb 100755 --- a/panda/run_automated_tests.sh +++ b/panda/run_automated_tests.sh @@ -1,14 +1,3 @@ #!/bin/bash -TEST_FILENAME=${TEST_FILENAME:-nosetests.xml} -if [ ! -f "/EON" ]; then - TESTSUITE_NAME="Panda_Test-EON" -else - TESTSUITE_NAME="Panda_Test-DEV" -fi +PYTHONPATH="." nosetests -x -s tests/automated/$1*.py -cd boardesp -make flashall -cd .. - - -PYTHONPATH="." python $(which nosetests) -v --with-xunit --xunit-file=./$TEST_FILENAME --xunit-testsuite-name=$TESTSUITE_NAME -s tests/automated/$1*.py diff --git a/panda/setup.py b/panda/setup.py index 2acd9b9e1654df..312fdca42a0848 100644 --- a/panda/setup.py +++ b/panda/setup.py @@ -46,7 +46,7 @@ def find_version(*file_paths): platforms='any', license='MIT', install_requires=[ - 'libusb1 == 1.6.6', + 'libusb1 >= 1.6.4', 'hexdump >= 3.3', 'pycrypto >= 2.6.1', 'tqdm >= 4.14.0', diff --git a/panda/tests/automated/0_builds.py b/panda/tests/automated/0_builds.py index df4186cce4e6f6..d5e434a2c22876 100644 --- a/panda/tests/automated/0_builds.py +++ b/panda/tests/automated/0_builds.py @@ -1,6 +1,12 @@ import os from panda import build_st +def test_build_legacy(): + build_st("obj/comma.bin", "Makefile.legacy") + +def test_build_bootstub_legacy(): + build_st("obj/bootstub.comma.bin", "Makefile.legacy") + def test_build_panda(): build_st("obj/panda.bin") diff --git a/panda/tests/automated/1_program.py b/panda/tests/automated/1_program.py index 1e0beb8ae52425..7da801bcd54e7b 100644 --- a/panda/tests/automated/1_program.py +++ b/panda/tests/automated/1_program.py @@ -1,15 +1,11 @@ import os from panda import Panda -from helpers import panda_color_to_serial, test_white_and_grey -@test_white_and_grey -@panda_color_to_serial -def test_recover(serial=None): - p = Panda(serial=serial) - assert p.recover(timeout=30) +def test_recover(): + p = Panda() + p.recover() -@test_white_and_grey -@panda_color_to_serial -def test_flash(serial=None): - p = Panda(serial=serial) +def test_flash(): + p = Panda() p.flash() + diff --git a/panda/tests/automated/2_usb_to_can.py b/panda/tests/automated/2_usb_to_can.py index 7860d3290fb0e5..481564240752a2 100644 --- a/panda/tests/automated/2_usb_to_can.py +++ b/panda/tests/automated/2_usb_to_can.py @@ -3,16 +3,14 @@ import sys import time from panda import Panda -from nose.tools import assert_equal, assert_less, assert_greater -from helpers import time_many_sends, connect_wo_esp, test_white_and_grey, panda_color_to_serial +from nose.tools import timed, assert_equal, assert_less, assert_greater +from helpers import time_many_sends, connect_wo_esp SPEED_NORMAL = 500 SPEED_GMLAN = 33.3 -@test_white_and_grey -@panda_color_to_serial -def test_can_loopback(serial=None): - p = connect_wo_esp(serial) +def test_can_loopback(): + p = connect_wo_esp() # enable output mode p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) @@ -44,10 +42,8 @@ def test_can_loopback(serial=None): assert 0x1aa == sr[0][0] == lb[0][0] assert "message" == sr[0][2] == lb[0][2] -@test_white_and_grey -@panda_color_to_serial -def test_safety_nooutput(serial=None): - p = connect_wo_esp(serial) +def test_safety_nooutput(): + p = connect_wo_esp() # enable output mode p.set_safety_mode(Panda.SAFETY_NOOUTPUT) @@ -63,10 +59,8 @@ def test_safety_nooutput(serial=None): r = p.can_recv() assert len(r) == 0 -@test_white_and_grey -@panda_color_to_serial -def test_reliability(serial=None): - p = connect_wo_esp(serial) +def test_reliability(): + p = connect_wo_esp() LOOP_COUNT = 100 MSG_COUNT = 100 @@ -103,10 +97,8 @@ def test_reliability(serial=None): sys.stdout.write("P") sys.stdout.flush() -@test_white_and_grey -@panda_color_to_serial -def test_throughput(serial=None): - p = connect_wo_esp(serial) +def test_throughput(): + p = connect_wo_esp() # enable output mode p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) @@ -128,10 +120,8 @@ def test_throughput(serial=None): print("loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct)) -@test_white_and_grey -@panda_color_to_serial -def test_gmlan(serial=None): - p = connect_wo_esp(serial) +def test_gmlan(): + p = connect_wo_esp() if p.legacy: return @@ -145,7 +135,7 @@ def test_gmlan(serial=None): p.set_can_speed_kbps(1, SPEED_NORMAL) p.set_can_speed_kbps(2, SPEED_NORMAL) p.set_can_speed_kbps(3, SPEED_GMLAN) - + # set gmlan on CAN2 for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]: p.set_gmlan(bus) @@ -160,10 +150,8 @@ def test_gmlan(serial=None): print("%d: %.2f kbps vs %.2f kbps" % (bus, comp_kbps_gmlan, comp_kbps_normal)) -@test_white_and_grey -@panda_color_to_serial -def test_gmlan_bad_toggle(serial=None): - p = connect_wo_esp(serial) +def test_gmlan_bad_toggle(): + p = connect_wo_esp() if p.legacy: return @@ -190,10 +178,9 @@ def test_gmlan_bad_toggle(serial=None): # this will fail if you have hardware serial connected -@test_white_and_grey -@panda_color_to_serial -def test_serial_debug(serial=None): - p = connect_wo_esp(serial) +def test_serial_debug(): + p = connect_wo_esp() junk = p.serial_read(Panda.SERIAL_DEBUG) p.call_control_api(0xc0) assert(p.serial_read(Panda.SERIAL_DEBUG).startswith("can ")) + diff --git a/panda/tests/automated/3_wifi.py b/panda/tests/automated/3_wifi.py index 1251663ba5838d..c6f4154ba8eaec 100644 --- a/panda/tests/automated/3_wifi.py +++ b/panda/tests/automated/3_wifi.py @@ -1,60 +1,33 @@ from __future__ import print_function import os -import time from panda import Panda -from helpers import connect_wifi, test_white, test_white_and_grey, panda_color_to_serial +from helpers import connect_wifi import requests -@test_white_and_grey -@panda_color_to_serial -def test_get_serial(serial=None): - p = Panda(serial) +def test_get_serial(): + p = Panda() print(p.get_serial()) -@test_white_and_grey -@panda_color_to_serial -def test_get_serial_in_flash_mode(serial=None): - p = Panda(serial) +def test_get_serial_in_flash_mode(): + p = Panda() p.reset(enter_bootstub=True) assert(p.bootstub) print(p.get_serial()) p.reset() -@test_white -@panda_color_to_serial -def test_connect_wifi(serial=None): - connect_wifi(serial) - -@test_white -@panda_color_to_serial -def test_flash_wifi(serial=None): - connect_wifi(serial) - assert Panda.flash_ota_wifi(release=False), "OTA Wifi Flash Failed" - connect_wifi(serial) - -@test_white -@panda_color_to_serial -def test_wifi_flash_st(serial=None): - connect_wifi(serial) - assert Panda.flash_ota_st(), "OTA ST Flash Failed" - connected = False - st = time.time() - while not connected and (time.time() - st) < 20: - try: - p = Panda(serial=serial) - p.get_serial() - connected = True - except: - time.sleep(1) - - if not connected: - assert False, "Panda failed to connect on USB after flashing" - -@test_white -@panda_color_to_serial -def test_webpage_fetch(serial=None): - connect_wifi(serial) +def test_connect_wifi(): + connect_wifi() + +def test_flash_wifi(): + Panda.flash_ota_wifi() + connect_wifi() + +def test_wifi_flash_st(): + Panda.flash_ota_st() + +def test_webpage_fetch(): r = requests.get("http://192.168.0.10/") print(r.text) assert "This is your comma.ai panda" in r.text + diff --git a/panda/tests/automated/4_wifi_functionality.py b/panda/tests/automated/4_wifi_functionality.py index 0cf42d1f3fba17..68686008e3373f 100644 --- a/panda/tests/automated/4_wifi_functionality.py +++ b/panda/tests/automated/4_wifi_functionality.py @@ -1,22 +1,17 @@ from __future__ import print_function import time from panda import Panda -from helpers import time_many_sends, connect_wifi, test_white, panda_color_to_serial +from helpers import time_many_sends, connect_wifi from nose.tools import timed, assert_equal, assert_less, assert_greater -@test_white -@panda_color_to_serial -def test_get_serial_wifi(serial=None): - connect_wifi(serial) +def test_get_serial_wifi(): + connect_wifi() p = Panda("WIFI") print(p.get_serial()) -@test_white -@panda_color_to_serial -def test_throughput(serial=None): - connect_wifi(serial) - p = Panda(serial) +def test_throughput(): + p = Panda() # enable output mode p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) @@ -29,7 +24,7 @@ def test_throughput(serial=None): for speed in [100,250,500,750,1000]: # set bus 0 speed to speed p.set_can_speed_kbps(0, speed) - time.sleep(0.1) + time.sleep(0.05) comp_kbps = time_many_sends(p, 0) @@ -40,11 +35,8 @@ def test_throughput(serial=None): print("WIFI loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct)) -@test_white -@panda_color_to_serial -def test_recv_only(serial=None): - connect_wifi(serial) - p = Panda(serial) +def test_recv_only(): + p = Panda() p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) p.set_can_loopback(True) pwifi = Panda("WIFI") @@ -57,3 +49,4 @@ def test_recv_only(serial=None): saturation_pct = (comp_kbps/speed) * 100.0 print("HT WIFI loopback %d messages at speed %d, comp speed is %.2f, percent %.2f" % (msg_count, speed, comp_kbps, saturation_pct)) + diff --git a/panda/tests/automated/5_wifi_udp.py b/panda/tests/automated/5_wifi_udp.py index 873f78bdb39bf8..7d6ccba660b645 100644 --- a/panda/tests/automated/5_wifi_udp.py +++ b/panda/tests/automated/5_wifi_udp.py @@ -1,16 +1,14 @@ from __future__ import print_function import sys import time -from helpers import time_many_sends, connect_wifi, test_white, panda_color_to_serial +from helpers import time_many_sends, connect_wifi from panda import Panda, PandaWifiStreaming from nose.tools import timed, assert_equal, assert_less, assert_greater -@test_white -@panda_color_to_serial -def test_udp_doesnt_drop(serial=None): - connect_wifi(serial) +def test_udp_doesnt_drop(): + connect_wifi() - p = Panda(serial) + p = Panda() p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) p.set_can_loopback(True) @@ -20,7 +18,6 @@ def test_udp_doesnt_drop(serial=None): break for msg_count in [1, 100]: - saturation_pcts = [] for i in range({1: 0x80, 100: 0x20}[msg_count]): pwifi.kick() @@ -34,33 +31,9 @@ def test_udp_doesnt_drop(serial=None): sys.stdout.flush() else: print("UDP WIFI loopback %d messages at speed %d, comp speed is %.2f, percent %.2f" % (msg_count, speed, comp_kbps, saturation_pct)) - assert_greater(saturation_pct, 15) #sometimes the wifi can be slow... + assert_greater(saturation_pct, 40) assert_less(saturation_pct, 100) - saturation_pcts.append(saturation_pct) - if len(saturation_pcts) > 0: - assert_greater(sum(saturation_pcts)/len(saturation_pcts), 60) + print("") + + - time.sleep(5) - usb_ok_cnt = 0 - REQ_USB_OK_CNT = 500 - st = time.time() - msg_id = 0x1bb - bus = 0 - last_missing_msg = 0 - while usb_ok_cnt < REQ_USB_OK_CNT and (time.time() - st) < 40: - p.can_send(msg_id, "message", bus) - time.sleep(0.01) - r = [1] - missing = True - while len(r) > 0: - r = p.can_recv() - r = filter(lambda x: x[3] == bus and x[0] == msg_id, r) - if len(r) > 0: - missing = False - usb_ok_cnt += len(r) - if missing: - last_missing_msg = time.time() - et = time.time() - st - last_missing_msg = last_missing_msg - st - print("waited {} for panda to recv can on usb, {} msgs, last missing at {}".format(et, usb_ok_cnt, last_missing_msg)) - assert usb_ok_cnt >= REQ_USB_OK_CNT, "Unable to recv can on USB after UDP" diff --git a/panda/tests/automated/helpers.py b/panda/tests/automated/helpers.py index 9e92f56bfe4bcd..1ddced117980ee 100644 --- a/panda/tests/automated/helpers.py +++ b/panda/tests/automated/helpers.py @@ -4,34 +4,12 @@ import random import subprocess import requests -from functools import wraps from panda import Panda from nose.tools import timed, assert_equal, assert_less, assert_greater -from parameterized import parameterized, param - -test_white_and_grey = parameterized([param(panda_color="White"), - param(panda_color="Grey")]) -test_white = parameterized([param(panda_color="White")]) -test_grey = parameterized([param(panda_color="Grey")]) -test_two_panda = parameterized([param(panda_color=["Grey", "White"]), - param(panda_color=["White", "Grey"])]) - -_serials = {} -def get_panda_serial(is_grey=None): - global _serials - if is_grey not in _serials: - for serial in Panda.list(): - p = Panda(serial=serial) - if is_grey is None or p.is_grey() == is_grey: - _serials[is_grey] = serial - return serial - raise IOError("Panda not found. is_grey: {}".format(is_grey)) - else: - return _serials[is_grey] - -def connect_wo_esp(serial=None): + +def connect_wo_esp(): # connect to the panda - p = Panda(serial=serial) + p = Panda() # power down the ESP p.set_esp_power(False) @@ -42,28 +20,15 @@ def connect_wo_esp(serial=None): return p -def connect_wifi(serial=None): - p = Panda(serial=serial) - p.set_esp_power(True) +def connect_wifi(): + p = Panda() dongle_id, pw = p.get_serial() assert(dongle_id.isalnum()) _connect_wifi(dongle_id, pw) -FNULL = open(os.devnull, 'w') def _connect_wifi(dongle_id, pw, insecure_okay=False): ssid = str("panda-" + dongle_id) - r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT) - if not r: - #Can already ping, try connecting on wifi - try: - p = Panda("WIFI") - p.get_serial() - print("Already connected") - return - except: - pass - print("WIFI: connecting to %s" % ssid) while 1: @@ -74,8 +39,8 @@ def _connect_wifi(dongle_id, pw, insecure_okay=False): cnt = 0 MAX_TRIES = 10 while cnt < MAX_TRIES: - print("WIFI: scanning %d" % cnt) - os.system("iwlist %s scanning > /dev/null" % wlan_interface) + print "WIFI: scanning %d" % cnt + os.system("sudo iwlist %s scanning > /dev/null" % wlan_interface) os.system("nmcli device wifi rescan") wifi_scan = filter(lambda x: ssid in x, subprocess.check_output(["nmcli","dev", "wifi", "list"]).split("\n")) if len(wifi_scan) != 0: @@ -86,107 +51,45 @@ def _connect_wifi(dongle_id, pw, insecure_okay=False): assert cnt < MAX_TRIES if "-pair" in wifi_scan[0]: os.system("nmcli d wifi connect %s-pair" % (ssid)) - connect_cnt = 0 - MAX_TRIES = 20 - while connect_cnt < MAX_TRIES: - connect_cnt += 1 - r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT) - if r: - print("Waiting for panda to ping...") - time.sleep(0.1) - else: - break if insecure_okay: break # fetch webpage - print("connecting to insecure network to secure") - try: - r = requests.get("http://192.168.0.10/") - except requests.ConnectionError: - r = requests.get("http://192.168.0.10/") + print "connecting to insecure network to secure" + r = requests.get("http://192.168.0.10/") assert r.status_code==200 - print("securing") + print "securing" try: r = requests.get("http://192.168.0.10/secure", timeout=0.01) except requests.exceptions.Timeout: - print("timeout http request to secure") pass else: - ret = os.system("nmcli d wifi connect %s password %s" % (ssid, pw)) - if os.WEXITSTATUS(ret) == 0: - #check ping too - ping_ok = False - connect_cnt = 0 - MAX_TRIES = 10 - while connect_cnt < MAX_TRIES: - connect_cnt += 1 - r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT) - if r: - print("Waiting for panda to ping...") - time.sleep(0.1) - else: - ping_ok = True - break - if ping_ok: - break - + os.system("nmcli d wifi connect %s password %s" % (ssid, pw)) + break + # TODO: confirm that it's connected to the right panda -def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None, two_pandas=False): +def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None): if precv == None: precv = p if msg_id == None: msg_id = random.randint(0x100, 0x200) - if p == precv and two_pandas: - raise ValueError("Cannot have two pandas that are the same panda") st = time.time() p.can_send_many([(msg_id, 0, "\xaa"*8, bus)]*msg_count) r = [] - r_echo = [] - r_len_expected = msg_count if two_pandas else msg_count*2 - r_echo_len_exected = msg_count if two_pandas else 0 - while len(r) < r_len_expected and (time.time() - st) < 5: + while len(r) < (msg_count*2) and (time.time() - st) < 3: r.extend(precv.can_recv()) - et = time.time() - if two_pandas: - while len(r_echo) < r_echo_len_exected and (time.time() - st) < 10: - r_echo.extend(p.can_recv()) sent_echo = filter(lambda x: x[3] == 0x80 | bus and x[0] == msg_id, r) - sent_echo.extend(filter(lambda x: x[3] == 0x80 | bus and x[0] == msg_id, r_echo)) - resp = filter(lambda x: x[3] == bus and x[0] == msg_id, r) + loopback_resp = filter(lambda x: x[3] == bus and x[0] == msg_id, r) - leftovers = filter(lambda x: (x[3] != 0x80 | bus and x[3] != bus) or x[0] != msg_id, r) - assert_equal(len(leftovers), 0) - - assert_equal(len(resp), msg_count) assert_equal(len(sent_echo), msg_count) + assert_equal(len(loopback_resp), msg_count) - et = (et-st)*1000.0 + et = (time.time()-st)*1000.0 comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7)*msg_count / et return comp_kbps - -def panda_color_to_serial(fn): - @wraps(fn) - def wrapper(panda_color=None, **kwargs): - pandas_is_grey = [] - if panda_color is not None: - if not isinstance(panda_color, list): - panda_color = [panda_color] - panda_color = [s.lower() for s in panda_color] - for p in panda_color: - if p is None: - pandas_is_grey.append(None) - elif p in ["grey", "gray"]: - pandas_is_grey.append(True) - elif p in ["white"]: - pandas_is_grey.append(False) - else: - raise ValueError("Invalid Panda Color {}".format(p)) - return fn(*[get_panda_serial(is_grey) for is_grey in pandas_is_grey], **kwargs) - return wrapper diff --git a/panda/tests/debug_console.py b/panda/tests/debug_console.py index 0238ed789783e4..f4db77f091e0da 100755 --- a/panda/tests/debug_console.py +++ b/panda/tests/debug_console.py @@ -21,9 +21,6 @@ pandas = list(map(lambda x: Panda(x, claim=claim), serials)) - if not len(pandas): - sys.exit("no pandas found") - if os.getenv("BAUD") is not None: for panda in pandas: panda.set_uart_baud(port_number, int(os.getenv("BAUD"))) diff --git a/panda/tests/elm_car_simulator.py b/panda/tests/elm_car_simulator.py index bcee821cd04d44..f931e66ff40999 100755 --- a/panda/tests/elm_car_simulator.py +++ b/panda/tests/elm_car_simulator.py @@ -152,7 +152,7 @@ def __lin_process_msg(self, priority, toaddr, fromaddr, data): if len(outmsg) <= 5: self._lin_send(0x10, obd_header + outmsg) else: - first_msg_len = MIN(4, len(outmsg)%4) or 4 + first_msg_len = min(4, len(outmsg)%4) or 4 self._lin_send(0x10, obd_header + b'\x01' + b'\x00'*(4-first_msg_len) + outmsg[:first_msg_len]) @@ -229,7 +229,7 @@ def __can_process_msg(self, mode, pid, address, ts, data, src): outaddr = 0x7E8 if address == 0x7DF or address == 0x7E0 else 0x18DAF110 msgnum = 1 while(self.__can_multipart_data): - datalen = MIN(7, len(self.__can_multipart_data)) + datalen = min(7, len(self.__can_multipart_data)) msgpiece = struct.pack("B", 0x20 | msgnum) + self.__can_multipart_data[:datalen] self._can_send(outaddr, msgpiece) self.__can_multipart_data = self.__can_multipart_data[7:] @@ -246,7 +246,7 @@ def __can_process_msg(self, mode, pid, address, ts, data, src): self._can_send(outaddr, struct.pack("BBB", len(outmsg)+2, 0x40|data[1], pid) + outmsg) else: - first_msg_len = MIN(3, len(outmsg)%7) + first_msg_len = min(3, len(outmsg)%7) payload_len = len(outmsg)+3 msgpiece = struct.pack("BBBBB", 0x10 | ((payload_len>>8)&0xF), payload_len&0xFF, diff --git a/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py index dc5e5be5afb29e..dfd396045bf316 100644 --- a/panda/tests/safety/libpandasafety_py.py +++ b/panda/tests/safety/libpandasafety_py.py @@ -30,67 +30,56 @@ uint32_t CNT; } TIM_TypeDef; -void set_controls_allowed(bool c); -bool get_controls_allowed(void); -void set_long_controls_allowed(bool c); -bool get_long_controls_allowed(void); -void set_gas_interceptor_detected(bool c); -bool get_gas_interceptor_detetcted(void); -int get_gas_interceptor_prev(void); -void set_timer(uint32_t t); +void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +void toyota_init(int16_t param); +void set_controls_allowed(int c); void reset_angle_control(void); - -void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send); -int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push); -int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); -int safety_set_mode(uint16_t mode, int16_t param); - +int get_controls_allowed(void); void init_tests_toyota(void); -int get_toyota_torque_meas_min(void); -int get_toyota_torque_meas_max(void); -int get_toyota_gas_prev(void); +void set_timer(int t); void set_toyota_torque_meas(int min, int max); -void set_toyota_desired_torque_last(int t); -void set_toyota_camera_forwarded(int t); +void set_cadillac_torque_driver(int min, int max); +void set_gm_torque_driver(int min, int max); +void set_hyundai_torque_driver(int min, int max); void set_toyota_rt_torque_last(int t); +void set_toyota_desired_torque_last(int t); +int get_toyota_torque_meas_min(void); +int get_toyota_torque_meas_max(void); void init_tests_honda(void); -int get_honda_ego_speed(void); -int get_honda_brake_prev(void); -int get_honda_gas_prev(void); +int get_ego_speed(void); +void honda_init(int16_t param); +void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +int get_brake_prev(void); +int get_gas_prev(void); void set_honda_alt_brake_msg(bool); -void set_honda_bosch_hardware(bool); +void set_bosch_hardware(bool); void init_tests_cadillac(void); +void cadillac_init(int16_t param); +void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); void set_cadillac_desired_torque_last(int t); void set_cadillac_rt_torque_last(int t); -void set_cadillac_torque_driver(int min, int max); void init_tests_gm(void); +void gm_init(int16_t param); +void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); void set_gm_desired_torque_last(int t); void set_gm_rt_torque_last(int t); -void set_gm_torque_driver(int min, int max); void init_tests_hyundai(void); +void nooutput_init(int16_t param); +void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); 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 set_hyundai_giraffe_switch_2(int t); -void set_hyundai_camera_bus(int t); - -void init_tests_chrysler(void); -void set_chrysler_desired_torque_last(int t); -void set_chrysler_rt_torque_last(int t); -void set_chrysler_camera_detected(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); -void set_subaru_torque_driver(int min, int max); +void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); """) diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c index be13d346afcf6b..c3f280644b06f9 100644 --- a/panda/tests/safety/test.c +++ b/panda/tests/safety/test.c @@ -26,69 +26,41 @@ 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; TIM_TypeDef timer; TIM_TypeDef *TIM2 = &timer; -#define MIN(a,b) \ +#define min(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ _a < _b ? _a : _b; }) -#define MAX(a,b) \ +#define max(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ _a > _b ? _a : _b; }) -#define UNUSED(x) (void)(x) #define PANDA -#define NULL ((void*)0) #define static #include "safety.h" -void set_controls_allowed(bool c){ +void set_controls_allowed(int c){ controls_allowed = c; } -void set_long_controls_allowed(bool c){ - long_controls_allowed = c; -} - -void set_gas_interceptor_detected(bool c){ - gas_interceptor_detected = c; -} - void reset_angle_control(void){ angle_control = 0; } -bool get_controls_allowed(void){ +int get_controls_allowed(void){ return controls_allowed; } -bool get_long_controls_allowed(void){ - return long_controls_allowed; -} - -bool get_gas_interceptor_detected(void){ - return gas_interceptor_detected; -} - -int get_gas_interceptor_prev(void){ - return gas_interceptor_prev; -} - -void set_timer(uint32_t t){ +void set_timer(int t){ timer.CNT = t; } -void set_toyota_camera_forwarded(int t){ - toyota_camera_forwarded = t; -} - void set_toyota_torque_meas(int min, int max){ toyota_torque_meas.min = min; toyota_torque_meas.max = max; @@ -109,40 +81,6 @@ void set_hyundai_torque_driver(int min, int max){ hyundai_torque_driver.max = max; } -void set_hyundai_camera_bus(int t){ - hyundai_camera_bus = t; -} - -void set_hyundai_giraffe_switch_2(int t){ - hyundai_giraffe_switch_2 = t; -} - -void set_chrysler_camera_detected(int t){ - chrysler_camera_detected = t; -} - -void set_chrysler_torque_meas(int min, int max){ - chrysler_torque_meas.min = min; - chrysler_torque_meas.max = max; -} - -void set_subaru_torque_driver(int min, int max){ - subaru_torque_driver.min = min; - subaru_torque_driver.max = 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_gas_prev(void){ - return toyota_gas_prev; -} - int get_toyota_torque_meas_min(void){ return toyota_torque_meas.min; } @@ -167,14 +105,6 @@ 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_toyota_desired_torque_last(int t){ toyota_desired_torque_last = t; } @@ -191,32 +121,24 @@ 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; +int get_ego_speed(void){ + return ego_speed; } -int get_honda_ego_speed(void){ - return honda_ego_speed; +int get_brake_prev(void){ + return brake_prev; } -int get_honda_brake_prev(void){ - return honda_brake_prev; -} - -int get_honda_gas_prev(void){ - return honda_gas_prev; +int get_gas_prev(void){ + return gas_prev; } void set_honda_alt_brake_msg(bool c){ honda_alt_brake_msg = c; } -void set_honda_bosch_hardware(bool c){ - honda_bosch_hardware = c; +void set_bosch_hardware(bool c){ + bosch_hardware = c; } void init_tests_toyota(void){ @@ -255,36 +177,9 @@ void init_tests_hyundai(void){ set_timer(0); } -void init_tests_chrysler(void){ - 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){ - 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_honda(void){ - honda_ego_speed = 0; - honda_brake_prev = 0; - honda_gas_prev = 0; -} - -void set_gmlan_digital_output(int to_set){ -} - -void reset_gmlan_switch_timeout(void){ + ego_speed = 0; + gas_interceptor_detected = 0; + brake_prev = 0; + gas_prev = 0; } - -void gmlan_switch_init(int timeout_enable){ -} - diff --git a/panda/tests/safety/test_cadillac.py b/panda/tests/safety/test_cadillac.py index af89e69cc7710e..53c943e966d6fc 100644 --- a/panda/tests/safety/test_cadillac.py +++ b/panda/tests/safety/test_cadillac.py @@ -31,16 +31,9 @@ class TestCadillacSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(6, 0) + cls.safety.cadillac_init(0) cls.safety.init_tests_cadillac() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - return to_send - def _set_prev_torque(self, t): self.safety.set_cadillac_desired_torque_last(t) self.safety.set_cadillac_rt_torque_last(t) @@ -53,6 +46,10 @@ def _torque_driver_msg(self, torque): to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8) return to_send + def _torque_driver_msg_array(self, torque): + for i in range(3): + self.safety.cadillac_ipas_rx_hook(self._torque_driver_msg(torque)) + def _torque_msg(self, torque): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 0x151 << 21 @@ -75,7 +72,7 @@ def test_enable_control_allowed_from_cruise(self): to_push[0].RDLR = 0x800000 to_push[0].RDTR = 0 - self.safety.safety_rx_hook(to_push) + self.safety.cadillac_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): @@ -85,7 +82,7 @@ def test_disable_control_allowed_from_cruise(self): to_push[0].RDTR = 0 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) + self.safety.cadillac_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) def test_torque_absolute_limits(self): @@ -101,22 +98,22 @@ def test_torque_absolute_limits(self): else: send = torque == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque))) + self.assertEqual(send, self.safety.cadillac_tx_hook(self._torque_msg(torque))) def test_non_realtime_limit_up(self): self.safety.set_cadillac_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertTrue(self.safety.cadillac_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.assertTrue(self.safety.cadillac_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.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self.safety.set_cadillac_torque_driver(0, 0) @@ -130,10 +127,10 @@ def test_exceed_torque_sensor(self): 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.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(MAX_TORQUE * sign))) self.safety.set_cadillac_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_TORQUE))) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(-MAX_TORQUE))) # spot check some individual cases for sign in [-1, 1]: @@ -142,20 +139,20 @@ def test_exceed_torque_sensor(self): 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.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_TORQUE * sign) self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign))) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_TORQUE * sign) self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(0))) self._set_prev_torque(MAX_TORQUE * sign) self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN + 1) * sign))) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN + 1) * sign))) def test_realtime_limits(self): @@ -167,29 +164,18 @@ def test_realtime_limits(self): 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.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) self._set_prev_torque(0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.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 = range(0x0, 0x3) - msgs = range(0x1, 0x800) - - for b in buss: - for m in msgs: - # assume len 8 - self.assertEqual(-1, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.cadillac_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) if __name__ == "__main__": diff --git a/panda/tests/safety/test_chrysler.py b/panda/tests/safety/test_chrysler.py index 09aa424dd99b28..568c92aaaadbf7 100755 --- a/panda/tests/safety/test_chrysler.py +++ b/panda/tests/safety/test_chrysler.py @@ -1,6 +1,4 @@ #!/usr/bin/env python2 -import csv -import glob import unittest import numpy as np import libpandasafety_py @@ -26,25 +24,13 @@ def sign(a): else: return -1 -def swap_bytes(data_str): - """Accepts string with hex, returns integer with order swapped for CAN.""" - a = int(data_str, 16) - return ((a & 0xff) << 24) + ((a & 0xff00) << 8) + ((a & 0x00ff0000) >> 8) + ((a & 0xff000000) >> 24) - class TestChryslerSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(9, 0) + cls.safety.nooutput_init(0) cls.safety.init_tests_chrysler() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - return to_send - def _button_msg(self, buttons): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 1265 << 21 @@ -77,9 +63,9 @@ def test_steer_safety_check(self): self.safety.set_controls_allowed(enabled) self._set_prev_torque(t) if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): self.safety.set_controls_allowed(1) @@ -92,7 +78,7 @@ def test_enable_control_allowed_from_cruise(self): to_push[0].RIR = 0x1f4 << 21 to_push[0].RDLR = 0x380000 - self.safety.safety_rx_hook(to_push) + self.safety.chrysler_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): @@ -101,17 +87,17 @@ def test_disable_control_allowed_from_cruise(self): to_push[0].RDLR = 0 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) + self.safety.chrysler_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) 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.assertTrue(self.safety.chrysler_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.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) def test_non_realtime_limit_down(self): self.safety.set_controls_allowed(True) @@ -120,12 +106,12 @@ def test_non_realtime_limit_down(self): 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.assertTrue(self.safety.chrysler_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))) + self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1))) def test_exceed_torque_sensor(self): self.safety.set_controls_allowed(True) @@ -134,9 +120,9 @@ def test_exceed_torque_sensor(self): 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.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2)))) + self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2)))) def test_realtime_limit_up(self): self.safety.set_controls_allowed(True) @@ -147,62 +133,39 @@ def test_realtime_limit_up(self): 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.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.chrysler_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))) + self.assertTrue(self.safety.chrysler_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)))) + self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(sign * MAX_RT_DELTA))) + self.assertTrue(self.safety.chrysler_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.safety.chrysler_rx_hook(self._torque_meas_msg(50)) + self.safety.chrysler_rx_hook(self._torque_meas_msg(-50)) + self.safety.chrysler_rx_hook(self._torque_meas_msg(0)) + self.safety.chrysler_rx_hook(self._torque_meas_msg(0)) + self.safety.chrysler_rx_hook(self._torque_meas_msg(0)) + self.safety.chrysler_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.safety.chrysler_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.safety.chrysler_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_fwd_hook(self): - buss = range(0x0, 0x3) - msgs = range(0x1, 0x800) - chrysler_camera_detected = [0, 1] - - for ccd in chrysler_camera_detected: - self.safety.set_chrysler_camera_detected(ccd) - blocked_msgs = [658, 678] - for b in buss: - for m in msgs: - if not ccd: - 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 - else: - fwd_bus = -1 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) - if __name__ == "__main__": unittest.main() diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py index d9a1d39ec76dd3..299de5a7e9ff51 100644 --- a/panda/tests/safety/test_gm.py +++ b/panda/tests/safety/test_gm.py @@ -32,16 +32,9 @@ class TestGmSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(3, 0) + cls.safety.gm_init(0) cls.safety.init_tests_gm() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - return to_send - def _speed_msg(self, speed): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 842 << 21 @@ -105,86 +98,75 @@ def test_default_controls_not_allowed(self): 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.safety.gm_rx_hook(self._button_msg(RESUME_BTN)) self.assertTrue(self.safety.get_controls_allowed()) def test_set_button(self): SET_BTN = 3 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(SET_BTN)) + self.safety.gm_rx_hook(self._button_msg(SET_BTN)) self.assertTrue(self.safety.get_controls_allowed()) def test_cancel_button(self): CANCEL_BTN = 6 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN)) + self.safety.gm_rx_hook(self._button_msg(CANCEL_BTN)) self.assertFalse(self.safety.get_controls_allowed()) def test_disengage_on_brake(self): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.gm_rx_hook(self._brake_msg(True)) self.assertFalse(self.safety.get_controls_allowed()) def test_allow_brake_at_zero_speed(self): # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.gm_rx_hook(self._brake_msg(True)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.gm_rx_hook(self._brake_msg(True)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.gm_rx_hook(self._brake_msg(False)) def test_not_allow_brake_when_moving(self): # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) - self.safety.safety_rx_hook(self._speed_msg(100)) + self.safety.gm_rx_hook(self._brake_msg(True)) + self.safety.gm_rx_hook(self._speed_msg(100)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.gm_rx_hook(self._brake_msg(True)) self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.gm_rx_hook(self._brake_msg(False)) def test_disengage_on_gas(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(True)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(False)) + self.safety.set_controls_allowed(1) + self.safety.gm_rx_hook(self._gas_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.gm_rx_hook(self._gas_msg(False)) def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._gas_msg(True)) + self.safety.gm_rx_hook(self._gas_msg(True)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(True)) + self.safety.gm_rx_hook(self._gas_msg(True)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(False)) + self.safety.gm_rx_hook(self._gas_msg(False)) def test_brake_safety_check(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - 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 or not long_controls_allowed) and b != 0): - self.assertFalse(self.safety.safety_tx_hook(self._send_brake_msg(b))) - else: - self.assertTrue(self.safety.safety_tx_hook(self._send_brake_msg(b))) - self.safety.set_long_controls_allowed(True) + for enabled in [0, 1]: + for b in range(0, 500): + self.safety.set_controls_allowed(enabled) + if abs(b) > MAX_BRAKE or (not enabled and b != 0): + self.assertFalse(self.safety.gm_tx_hook(self._send_brake_msg(b))) + else: + self.assertTrue(self.safety.gm_tx_hook(self._send_brake_msg(b))) def test_gas_safety_check(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - 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 or not long_controls_allowed) and g != MAX_REGEN): - self.assertFalse(self.safety.safety_tx_hook(self._send_gas_msg(g))) - else: - self.assertTrue(self.safety.safety_tx_hook(self._send_gas_msg(g))) - self.safety.set_long_controls_allowed(True) + for enabled in [0, 1]: + for g in range(0, 2**12-1): + self.safety.set_controls_allowed(enabled) + if abs(g) > MAX_GAS or (not enabled and g != MAX_REGEN): + self.assertFalse(self.safety.gm_tx_hook(self._send_gas_msg(g))) + else: + self.assertTrue(self.safety.gm_tx_hook(self._send_gas_msg(g))) def test_steer_safety_check(self): for enabled in [0, 1]: @@ -192,9 +174,9 @@ def test_steer_safety_check(self): self.safety.set_controls_allowed(enabled) self._set_prev_torque(t) if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): self.safety.set_controls_allowed(1) @@ -207,15 +189,15 @@ 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.assertTrue(self.safety.gm_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.assertTrue(self.safety.gm_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.assertFalse(self.safety.gm_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self.safety.set_gm_torque_driver(0, 0) @@ -229,10 +211,10 @@ def test_against_torque_driver(self): t *= -sign self.safety.set_gm_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.safety.gm_tx_hook(self._torque_msg(MAX_STEER * sign))) self.safety.set_gm_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -241,20 +223,20 @@ def test_against_torque_driver(self): 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.assertTrue(self.safety.gm_tx_hook(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) self.safety.set_gm_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(0))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) def test_realtime_limits(self): @@ -266,29 +248,18 @@ def test_realtime_limits(self): self.safety.set_gm_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.safety.gm_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) self._set_prev_torque(0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.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 = range(0x0, 0x3) - msgs = range(0x1, 0x800) - - for b in buss: - for m in msgs: - # assume len 8 - self.assertEqual(-1, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.gm_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) if __name__ == "__main__": diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py index bc5e8d192f1712..48b678eaaec43e 100755 --- a/panda/tests/safety/test_honda.py +++ b/panda/tests/safety/test_honda.py @@ -3,23 +3,14 @@ import numpy as np import libpandasafety_py -MAX_BRAKE = 255 class TestHondaSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(1, 0) + cls.safety.honda_init(0) cls.safety.init_tests_honda() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - - return to_send - def _speed_msg(self, speed): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 0x158 << 21 @@ -58,15 +49,14 @@ def _gas_msg(self, gas): def _send_brake_msg(self, brake): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 0x1FA << 21 - to_send[0].RDLR = ((brake & 0x3) << 8) | ((brake & 0x3FF) >> 2) + to_send[0].RDLR = brake return to_send - def _send_interceptor_msg(self, gas, addr): + def _send_gas_msg(self, gas): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = 6 - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) + to_send[0].RIR = 0x200 << 21 + to_send[0].RDLR = gas return to_send @@ -83,190 +73,112 @@ def test_default_controls_not_allowed(self): def test_resume_button(self): RESUME_BTN = 4 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x1A6)) + self.safety.honda_rx_hook(self._button_msg(RESUME_BTN, 0x1A6)) self.assertTrue(self.safety.get_controls_allowed()) def test_set_button(self): SET_BTN = 3 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.honda_rx_hook(self._button_msg(SET_BTN, 0x1A6)) self.assertTrue(self.safety.get_controls_allowed()) def test_cancel_button(self): CANCEL_BTN = 2 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6)) + self.safety.honda_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6)) self.assertFalse(self.safety.get_controls_allowed()) def test_sample_speed(self): - self.assertEqual(0, self.safety.get_honda_ego_speed()) - self.safety.safety_rx_hook(self._speed_msg(100)) - self.assertEqual(100, self.safety.get_honda_ego_speed()) + self.assertEqual(0, self.safety.get_ego_speed()) + self.safety.honda_rx_hook(self._speed_msg(100)) + self.assertEqual(100, self.safety.get_ego_speed()) def test_prev_brake(self): - self.assertFalse(self.safety.get_honda_brake_prev()) - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_honda_brake_prev()) + self.assertFalse(self.safety.get_brake_prev()) + self.safety.honda_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_brake_prev()) def test_disengage_on_brake(self): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(1)) + self.safety.honda_rx_hook(self._brake_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) def test_alt_disengage_on_brake(self): self.safety.set_honda_alt_brake_msg(1) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._alt_brake_msg(1)) + self.safety.honda_rx_hook(self._alt_brake_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) self.safety.set_honda_alt_brake_msg(0) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._alt_brake_msg(1)) + self.safety.honda_rx_hook(self._alt_brake_msg(1)) self.assertTrue(self.safety.get_controls_allowed()) def test_allow_brake_at_zero_speed(self): # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.honda_rx_hook(self._brake_msg(True)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.honda_rx_hook(self._brake_msg(True)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes + self.safety.honda_rx_hook(self._brake_msg(False)) # reset no brakes def test_not_allow_brake_when_moving(self): # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) - self.safety.safety_rx_hook(self._speed_msg(100)) + self.safety.honda_rx_hook(self._brake_msg(True)) + self.safety.honda_rx_hook(self._speed_msg(100)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.honda_rx_hook(self._brake_msg(True)) self.assertFalse(self.safety.get_controls_allowed()) def test_prev_gas(self): - self.safety.safety_rx_hook(self._gas_msg(False)) - self.assertFalse(self.safety.get_honda_gas_prev()) - self.safety.safety_rx_hook(self._gas_msg(True)) - self.assertTrue(self.safety.get_honda_gas_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) + self.assertFalse(self.safety.get_gas_prev()) + self.safety.honda_rx_hook(self._gas_msg(True)) + self.assertTrue(self.safety.get_gas_prev()) def test_disengage_on_gas(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(1)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.set_long_controls_allowed(True) + self.safety.set_controls_allowed(1) + self.safety.honda_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.honda_rx_hook(self._gas_msg(1)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(1)) + self.safety.honda_rx_hook(self._gas_msg(1)) self.assertTrue(self.safety.get_controls_allowed()) - def test_disengage_on_gas_interceptor(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - 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) - self.safety.set_long_controls_allowed(True) - - def test_allow_engage_with_gas_interceptor_pressed(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + def test_brake_safety_check(self): + self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) + self.safety.set_controls_allowed(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) + self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0))) - def test_brake_safety_check(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - for brake in np.arange(0, MAX_BRAKE + 10, 1): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed and long_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_long_controls_allowed(True) - - def test_gas_interceptor_safety_check(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - for gas in np.arange(0, 4000, 100): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed and long_controls_allowed: - send = True - else: - send = gas == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200))) - self.safety.set_long_controls_allowed(True) + def test_gas_safety_check(self): + self.safety.set_controls_allowed(0) + self.assertTrue(self.safety.honda_tx_hook(self._send_gas_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_gas_msg(0x1000))) def test_steer_safety_check(self): self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._send_steer_msg(0x0000))) - self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000))) + self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000))) def test_spam_cancel_safety_check(self): RESUME_BTN = 4 SET_BTN = 3 CANCEL_BTN = 2 BUTTON_MSG = 0x296 - self.safety.set_honda_bosch_hardware(1) + self.safety.set_bosch_hardware(1) 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))) + self.assertTrue(self.safety.honda_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG))) + self.assertFalse(self.safety.honda_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) + self.assertFalse(self.safety.honda_tx_hook(self._button_msg(SET_BTN, BUTTON_MSG))) # do not block resume if we are engaged already self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) - - def test_fwd_hook(self): - buss = range(0x0, 0x3) - msgs = range(0x1, 0x800) - long_controls_allowed = [0, 1] - - self.safety.set_honda_bosch_hardware(0) - - for l in long_controls_allowed: - self.safety.set_long_controls_allowed(l) - blocked_msgs = [0xE4, 0x194, 0x33D] - if l: - blocked_msgs += [0x1FA ,0x30C, 0x39F] - 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, self._send_msg(b, m, 8))) - - self.safety.set_long_controls_allowed(True) - + self.assertTrue(self.safety.honda_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) if __name__ == "__main__": diff --git a/panda/tests/safety/test_hyundai.py b/panda/tests/safety/test_hyundai.py index 539982fab92a26..0a6ce0f91f3d03 100644 --- a/panda/tests/safety/test_hyundai.py +++ b/panda/tests/safety/test_hyundai.py @@ -5,7 +5,7 @@ MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 -MAX_STEER = 255 +MAX_STEER = 250 MAX_RT_DELTA = 112 RT_INTERVAL = 250000 @@ -29,16 +29,9 @@ class TestHyundaiSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(7, 0) + cls.safety.nooutput_init(0) cls.safety.init_tests_hyundai() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - return to_send - def _button_msg(self, buttons): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 1265 << 21 @@ -70,9 +63,9 @@ def test_steer_safety_check(self): self.safety.set_controls_allowed(enabled) self._set_prev_torque(t) if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): self.safety.set_controls_allowed(1) @@ -85,7 +78,7 @@ def test_enable_control_allowed_from_cruise(self): to_push[0].RIR = 1057 << 21 to_push[0].RDLR = 1 << 13 - self.safety.safety_rx_hook(to_push) + self.safety.hyundai_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): @@ -94,7 +87,7 @@ def test_disable_control_allowed_from_cruise(self): to_push[0].RDLR = 0 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) + self.safety.hyundai_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) def test_non_realtime_limit_up(self): @@ -102,15 +95,15 @@ 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.assertTrue(self.safety.hyundai_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.assertTrue(self.safety.hyundai_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.assertFalse(self.safety.hyundai_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))) + self.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self.safety.set_hyundai_torque_driver(0, 0) @@ -124,10 +117,10 @@ def test_against_torque_driver(self): t *= -sign self.safety.set_hyundai_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.safety.hyundai_tx_hook(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.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -136,20 +129,20 @@ def test_against_torque_driver(self): 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.assertTrue(self.safety.hyundai_tx_hook(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.assertFalse(self.safety.hyundai_tx_hook(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.assertTrue(self.safety.hyundai_tx_hook(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.assertTrue(self.safety.hyundai_tx_hook(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.assertFalse(self.safety.hyundai_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) def test_realtime_limits(self): @@ -161,18 +154,18 @@ def test_realtime_limits(self): self.safety.set_hyundai_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.safety.hyundai_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.hyundai_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))) + self.assertTrue(self.safety.hyundai_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)))) + self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) #def test_spam_cancel_safety_check(self): @@ -181,37 +174,12 @@ def test_realtime_limits(self): # CANCEL_BTN = 4 # BUTTON_MSG = 1265 # 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.safety.hyundai_tx_hook(self._button_msg(CANCEL_BTN))) + # self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN))) + # self.assertFalse(self.safety.hyundai_tx_hook(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 = range(0x0, 0x3) - msgs = range(0x1, 0x800) - hyundai_giraffe_switch_2 = [0, 1] - - self.safety.set_hyundai_camera_bus(2) - for hgs in hyundai_giraffe_switch_2: - self.safety.set_hyundai_giraffe_switch_2(hgs) - blocked_msgs = [832] - for b in buss: - for m in msgs: - if hgs: - 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 - else: - fwd_bus = -1 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) + # self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN))) if __name__ == "__main__": diff --git a/panda/tests/safety/test_subaru.py b/panda/tests/safety/test_subaru.py index 13fe1fb14f2b91..a1e03fea15cc85 100644 --- a/panda/tests/safety/test_subaru.py +++ b/panda/tests/safety/test_subaru.py @@ -29,16 +29,9 @@ class TestSubaruSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(10, 0) + cls.safety.nooutput_init(0) cls.safety.init_tests_subaru() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - return to_send - def _set_prev_torque(self, t): self.safety.set_subaru_desired_torque_last(t) self.safety.set_subaru_rt_torque_last(t) @@ -67,7 +60,7 @@ def test_enable_control_allowed_from_cruise(self): to_push[0].RIR = 0x240 << 21 to_push[0].RDHR = 1 << 9 - self.safety.safety_rx_hook(to_push) + self.safety.subaru_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): @@ -76,7 +69,7 @@ def test_disable_control_allowed_from_cruise(self): to_push[0].RDHR = 0 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) + self.safety.subaru_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) def test_steer_safety_check(self): @@ -85,9 +78,9 @@ def test_steer_safety_check(self): self.safety.set_controls_allowed(enabled) self._set_prev_torque(t) if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): self.safety.set_controls_allowed(1) @@ -100,15 +93,15 @@ 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.assertTrue(self.safety.subaru_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.assertTrue(self.safety.subaru_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.assertFalse(self.safety.subaru_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))) + self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self.safety.set_subaru_torque_driver(0, 0) @@ -122,10 +115,10 @@ def test_against_torque_driver(self): t *= -sign self.safety.set_subaru_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.safety.subaru_tx_hook(self._torque_msg(MAX_STEER * sign))) self.safety.set_subaru_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.safety.subaru_tx_hook(self._torque_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -134,20 +127,20 @@ def test_against_torque_driver(self): delta = 1 * sign self._set_prev_torque(torque_desired) self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) + self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) + self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_subaru_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.assertTrue(self.safety.subaru_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) + self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(0))) self._set_prev_torque(MAX_STEER * sign) self.safety.set_subaru_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.assertFalse(self.safety.subaru_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) def test_realtime_limits(self): @@ -159,35 +152,18 @@ def test_realtime_limits(self): self.safety.set_subaru_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.safety.subaru_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.subaru_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))) + self.assertTrue(self.safety.subaru_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): - buss = range(0x0, 0x3) - msgs = range(0x1, 0x800) - blocked_msgs = [290, 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, self._send_msg(b, m, 8))) + self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) if __name__ == "__main__": diff --git a/panda/tests/safety/test_toyota.py b/panda/tests/safety/test_toyota.py index 7dd1601d7712c3..b4d23af65df321 100644 --- a/panda/tests/safety/test_toyota.py +++ b/panda/tests/safety/test_toyota.py @@ -15,6 +15,12 @@ MAX_TORQUE_ERROR = 350 +IPAS_OVERRIDE_THRESHOLD = 200 + +ANGLE_DELTA_BP = [0., 5., 15.] +ANGLE_DELTA_V = [5., .8, .15] # windup limit +ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + def twos_comp(val, bits): if val >= 0: return val @@ -31,16 +37,9 @@ class TestToyotaSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.safety_set_mode(2, 100) + cls.safety.toyota_init(100) cls.safety.init_tests_toyota() - def _send_msg(self, bus, addr, length): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = length - to_send[0].RDTR = bus << 4 - return to_send - def _set_prev_torque(self, t): self.safety.set_toyota_desired_torque_last(t) self.safety.set_toyota_rt_torque_last(t) @@ -54,6 +53,30 @@ def _torque_meas_msg(self, torque): to_send[0].RDHR = t | ((t & 0xFF) << 16) return to_send + def _torque_driver_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x260 << 21 + + t = twos_comp(torque, 16) + to_send[0].RDLR = t | ((t & 0xFF) << 16) + return to_send + + def _torque_driver_msg_array(self, torque): + for i in range(6): + self.safety.toyota_ipas_rx_hook(self._torque_driver_msg(torque)) + + def _angle_meas_msg(self, angle): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x25 << 21 + + t = twos_comp(angle, 12) + to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) + return to_send + + def _angle_meas_msg_array(self, angle): + for i in range(6): + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(angle)) + def _torque_msg(self, torque): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_send[0].RIR = 0x2E4 << 21 @@ -62,34 +85,38 @@ def _torque_msg(self, torque): to_send[0].RDLR = t | ((t & 0xFF) << 16) return to_send - def _accel_msg(self, accel): + def _ipas_state_msg(self, state): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = 0x343 << 21 + to_send[0].RIR = 0x262 << 21 - a = twos_comp(accel, 16) - to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) + to_send[0].RDLR = state & 0xF return to_send - def _send_gas_msg(self, gas): + def _ipas_control_msg(self, angle, state): + # note: we command 2/3 of the angle due to CAN conversion to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = 0x2C1 << 21 - to_send[0].RDHR = (gas & 0xFF) << 16 + to_send[0].RIR = 0x266 << 21 + + t = twos_comp(angle, 12) + to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) + to_send[0].RDLR |= ((state & 0xf) << 4) return to_send - def _send_interceptor_msg(self, gas, addr): + def _speed_msg(self, speed): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = addr << 21 - to_send[0].RDTR = 6 - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) + to_send[0].RIR = 0xb4 << 21 + speed = int(speed * 100 * 3.6) + to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00) return to_send - def _pcm_cruise_msg(self, cruise_on): + def _accel_msg(self, accel): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') - to_send[0].RIR = 0x1D2 << 21 - to_send[0].RDLR = cruise_on << 5 + to_send[0].RIR = 0x343 << 21 + a = twos_comp(accel, 16) + to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) return to_send def test_default_controls_not_allowed(self): @@ -100,83 +127,32 @@ def test_manually_enable_controls_allowed(self): self.assertTrue(self.safety.get_controls_allowed()) 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()) + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x1D2 << 21 + to_push[0].RDHR = 0xF00000 - 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(g, self.safety.get_toyota_gas_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): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - 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)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.set_long_controls_allowed(True) - - 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.safety.toyota_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) - def test_disengage_on_gas_interceptor(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - 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(0x1000, 0x201)) - if long_controls_allowed: - self.assertFalse(self.safety.get_controls_allowed()) - else: - 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) - self.safety.set_long_controls_allowed(True) + def test_disable_control_allowed_from_cruise(self): + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x1D2 << 21 + to_push[0].RDHR = 0 - 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) + self.safety.toyota_rx_hook(to_push) + self.assertFalse(self.safety.get_controls_allowed()) def test_accel_actuation_limits(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - 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 and long_controls_allowed: - send = MIN_ACCEL <= accel <= MAX_ACCEL - else: - send = accel == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._accel_msg(accel))) - self.safety.set_long_controls_allowed(True) + 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.toyota_tx_hook(self._accel_msg(accel))) def test_torque_absolute_limits(self): for controls_allowed in [True, False]: @@ -191,16 +167,16 @@ def test_torque_absolute_limits(self): else: send = torque == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque))) + self.assertEqual(send, self.safety.toyota_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.assertTrue(self.safety.toyota_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.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) def test_non_realtime_limit_down(self): self.safety.set_controls_allowed(True) @@ -208,12 +184,12 @@ def test_non_realtime_limit_down(self): 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.assertTrue(self.safety.toyota_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))) + self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) def test_exceed_torque_sensor(self): self.safety.set_controls_allowed(True) @@ -222,9 +198,9 @@ def test_exceed_torque_sensor(self): 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.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) + self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) def test_realtime_limit_up(self): self.safety.set_controls_allowed(True) @@ -235,77 +211,203 @@ def test_realtime_limit_up(self): 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.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.toyota_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))) + self.assertTrue(self.safety.toyota_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))) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 370))) + self.assertTrue(self.safety.toyota_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.safety.toyota_rx_hook(self._torque_meas_msg(50)) + self.safety.toyota_rx_hook(self._torque_meas_msg(-50)) + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) self.assertEqual(-51, self.safety.get_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.safety.toyota_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.safety.toyota_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): + def test_ipas_override(self): + + ## angle control is not active + self.safety.set_controls_allowed(1) + + # 3 consecutive msgs where driver exceeds threshold but angle_control isn't active + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) + self.assertTrue(self.safety.get_controls_allowed()) + + self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) + self.assertTrue(self.safety.get_controls_allowed()) + + # ipas state is override + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(5)) + self.assertTrue(self.safety.get_controls_allowed()) + + ## now angle control is active + self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 0)) + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(0)) + + # 3 consecutive msgs where driver does exceed threshold + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) + self.assertFalse(self.safety.get_controls_allowed()) + + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) + self.assertFalse(self.safety.get_controls_allowed()) + + # ipas state is override and torque isn't overriding any more + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(0) + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(5)) + self.assertFalse(self.safety.get_controls_allowed()) + + # 3 consecutive msgs where driver does not exceed threshold and + # ipas state is not override + self.safety.set_controls_allowed(1) + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + + self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD) + self.assertTrue(self.safety.get_controls_allowed()) + + self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD) + self.assertTrue(self.safety.get_controls_allowed()) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_cmd_when_disabled(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))) + + # test angle cmd too far from actual + angle_refs = [-10, 10] + deltas = range(-2, 3) + expected_results = [False, True, True, True, False] + + for a in angle_refs: + self._angle_meas_msg_array(a) + for i, d in enumerate(deltas): + self.assertEqual(expected_results[i], self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + d, 1))) + + # test ipas state cmd enabled + self._angle_meas_msg_array(0) + self.assertEqual(0, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 3))) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_cmd_when_enabled(self): + + # ipas angle cmd should pass through when controls are enabled + self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200))) - - def test_fwd_hook(self): - - buss = range(0x0, 0x3) - msgs = range(0x1, 0x800) - long_controls_allowed = [0, 1] - toyota_camera_forwarded = [0, 1] - - for tcf in toyota_camera_forwarded: - self.safety.set_toyota_camera_forwarded(tcf) - for lca in long_controls_allowed: - self.safety.set_long_controls_allowed(lca) - blocked_msgs = [0x2E4, 0x412, 0x191] - if lca: - blocked_msgs += [0x343] - for b in buss: - for m in msgs: - if tcf: - 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 - else: - fwd_bus = -1 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) - - self.safety.set_long_controls_allowed(True) + self._angle_meas_msg_array(0) + self.safety.toyota_ipas_rx_hook(self._speed_msg(0.1)) + + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 1))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(4, 1))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 3))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-4, 3))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-8, 3))) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + + def test_angle_cmd_rate_when_disabled(self): + + # as long as the command is close to the measured, no rate limit is enforced when + # controls are disabled + self.safety.set_controls_allowed(0) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(0)) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 1))) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(100)) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(100, 1))) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(-100)) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-100, 1))) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_cmd_rate_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.] + angles = [-300, -100, -10, 0, 10, 100, 300] + for a in angles: + for s in speeds: + + # first test against false positives + self._angle_meas_msg_array(a) + self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)) + self.safety.set_controls_allowed(1) + self.safety.toyota_ipas_rx_hook(self._speed_msg(s)) + max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) + max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + + # now inject too high rates + self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * + (max_delta_up + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * + (max_delta_down + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_measured_rate(self): + + speeds = [0., 1., 5., 10., 15., 100.] + angles = [-300, -100, -10, 0, 10, 100, 300] + angles = [10] + for a in angles: + for s in speeds: + self._angle_meas_msg_array(a) + self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)) + self.safety.set_controls_allowed(1) + self.safety.toyota_ipas_rx_hook(self._speed_msg(s)) + max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) + max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(a)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(a + 150)) + self.assertFalse(self.safety.get_controls_allowed()) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() if __name__ == "__main__": diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 54d8d95e8718fc..1672a7a2a45f0f 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -11,7 +11,7 @@ def update_panda(): with open(os.path.join(BASEDIR, "VERSION")) as f: repo_version = f.read() #repo_version += "-EON" if os.path.isfile('/EON') else "-DEV" - repo_version += "-EON-unknown-DEBUG" + repo_version += "-unknown-DEBUG" panda = None panda_dfu = None From 4f8a07ae41b0bc5c61d0b24e40fde3f7937221e8 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Mon, 8 Jul 2019 11:55:15 +1000 Subject: [PATCH 02/24] 0.6.0.2 Untested --- launch_chffrplus.sh | 5 +- selfdrive/car/car_helpers.py | 4 +- selfdrive/car/hyundai/carcontroller.py | 50 ++++++++++++--- selfdrive/car/hyundai/carstate.py | 2 +- selfdrive/car/hyundai/hyundaican.py | 4 +- selfdrive/car/hyundai/interface.py | 89 +++++--------------------- selfdrive/car/hyundai/values.py | 60 ++--------------- selfdrive/common/version.h | 2 +- selfdrive/crash.py | 2 +- selfdrive/tombstoned.py | 2 +- 10 files changed, 69 insertions(+), 151 deletions(-) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 570e7913dfa672..b3d6ee8d3290c9 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -13,14 +13,13 @@ fi function launch { # apply update if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then + rm /data/data/com.termux/files/continue.sh ; + cp /data/openpilot/continue.sh /data/data/com.termux/files/continue.sh ; git reset --hard @{u} && git clean -xdf && exec "${BASH_SOURCE[0]}" fi - rm /data/data/com.termux/files/continue.sh ; - cp /data/openpilot/continue.sh /data/data/com.termux/files/continue.sh ; - # no cpu rationing for now echo 0-3 > /dev/cpuset/background/cpus echo 0-3 > /dev/cpuset/system-background/cpus diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 765cdb7a9f3f9b..1fd99f76e31454 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -117,8 +117,8 @@ def fingerprint(logcan, sendcan): # bail if no cars left or we've been waiting for more than 2s since can_seen elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200): #return None, finger, "" - print "Fingerprinting Failed: Returning Generic Style Car" - return "UNKNOWN CAR ON EMMERTEX FORK", finger, "" + print "WARNING: This fork is ONLY compatible with Hyundai, Kia and Genesis Vehicles" + return "HYUNDAI OR KIA", finger, "" # keep sending VIN qury if ECU isn't responsing. # sendcan is probably not ready due to the zmq slow joiner syndrome diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c5360b098b1d73..1a7130d1bad2aa 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -21,30 +21,40 @@ class CarController(object): def __init__(self, dbc_name, car_fingerprint): self.apply_steer_last = 0 self.car_fingerprint = car_fingerprint + self.lkas11_cnt = 0 + self.clu11_cnt = 0 self.cnt = 0 self.last_resume_cnt = 0 + self.checksum = "NONE" self.checksum_learn_cnt = 0 - # True when giraffe switch 2 is low and we need to replace all the camera messages - # otherwise we forward the camera msgs and we just replace the lkas cmd signals + + self.turning_signal_timer = 0 + self.min_steer_speed = 0. self.camera_disconnected = False self.packer = CANPacker(dbc_name) def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): + ### Error State Resets ### + disable_steer = False + ### Learn Checksum ### - + # Learn Checksum from the Camera if self.checksum == "NONE": self.checksum = learn_checksum(self.packer, CS.lkas11) print ("Discovered Checksum", self.checksum) - if self.checksum == "NONE": + if self.checksum == "NONE" and self.checksum_learn_cnt < 50: + self.checksum_learn_cnt += 1 return + # If MDPS is faulted from bad checksum, then cycle through all Checksums until 1 works if CS.steer_error == 1: - if self.checksum_learn_cnt > 200: - self.checksum_learn_cnt = 0 + self.camera_disconnected = True + if self.checksum_learn_cnt > 250: + self.checksum_learn_cnt = 50 if self.checksum == "NONE": print ("Testing 6B Checksum") self.checksum == "6B" @@ -59,18 +69,40 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): else: self.checksum_learn_cnt += 1 - ### Steering Torque - apply_steer = actuators.steer * SteerLimitParams.STEER_MAX + ### Minimum Steer Speed ### + + # Learn Minimum Steer Speed + if CS.mdps12_flt != 0 and CS.v_ego_raw > 0.: + if CS.v_ego_raw > self.min_steer_speed: + self.min_steer_speed = CS.v_ego_raw + 0.1 + print ("Discovered new Min Speed as", self.min_steer_speed) + # Apply Usage of Minimum Steer Speed + if CS.v_ego_raw < self.min_steer_speed: + disable_steer = True + + + ### Turning Indicators ### + if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1): + self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off + + if self.turning_signal_timer > 0: + disable_steer = True + self.turning_signal_timer -= 1 + + ### Steering Torque ### + apply_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) - if not enabled: + if not enabled or disable_steer: apply_steer = 0 steer_req = 1 if enabled else 0 self.apply_steer_last = apply_steer + + ### Generate CAN Messages ### can_sends = [] self.lkas11_cnt = self.cnt % 0x10 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index d199490d7e229d..e982d343c80f54 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -96,7 +96,6 @@ def get_can_parser(CP): ("TCS15", 10), ("CLU11", 50), ("ESP12", 100), - ("EMS12", 100), ("CGW1", 10), ("WHL_SPD11", 50), ("SAS11", 100) @@ -223,6 +222,7 @@ def update(self, cp, cp_cam): self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. if self.has_scc else False self.mdps11_strang = cp.vl["MDPS11"]["CR_Mdps_StrAng"] self.mdps11_stat = cp.vl["MDPS11"]["CF_Mdps_Stat"] + self.mdps12_flt = cp.vl["MDPS12"]['CF_Mdps_ToiFlt'] self.user_brake = 0 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index a61a775c491bdd..f1999bdece50a6 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -1,5 +1,4 @@ import crcmod -from selfdrive.car.hyundai.values import CHECKSUM hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) @@ -8,7 +7,7 @@ def make_can_msg(addr, dat, alt): def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock, checksum): values = { - "CF_Lkas_Icon": 3 if enabled else 1, + "CF_Lkas_Icon": 2, "CF_Lkas_LdwsSysState": 3 if steer_req else 1, "CF_Lkas_SysWarning": hud_alert, "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0, @@ -101,7 +100,6 @@ def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, camcan, checksum return packer.make_can_msg("MDPS12", camcan, values) def learn_checksum(packer, lkas11): - # Learn checksum used values = { "CF_Lkas_Icon": lkas11["CF_Lkas_Icon"], "CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"], diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 2ab8efdf5b76ea..cd1bc033542e5e 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -5,7 +5,7 @@ from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser -from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES +from selfdrive.car.hyundai.values import CAR, get_hud_alerts from selfdrive.car import STD_CARGO_KG @@ -62,72 +62,22 @@ def get_params(candidate, fingerprint, vin=""): tireStiffnessRear_civic = 202500 tire_stiffness_factor = 1. - ret.steerActuatorDelay = 0.1 # Default delay + + # Hyundai Kia and Genesis Starting Values + # Auto Tune corrects anything from here on out. + + ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 - if candidate == CAR.SANTA_FE: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG - ret.wheelbase = 2.766 - - # Values from optimizer - ret.steerRatio = 16.55 # 13.8 is spec end-to-end - tire_stiffness_factor = 0.82 - - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] - ret.minSteerSpeed = 0. - elif candidate == CAR.KIA_SORENTO: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1985. + STD_CARGO_KG - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - ret.minSteerSpeed = 0. - elif candidate == CAR.ELANTRA: - ret.lateralTuning.pid.kf = 0.00006 - ret.mass = 1275. + STD_CARGO_KG - ret.wheelbase = 2.7 - ret.steerRatio = 13.73 #Spec - tire_stiffness_factor = 0.385 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate == CAR.GENESIS: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 2060. + STD_CARGO_KG - ret.wheelbase = 3.01 - ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - ret.minSteerSpeed = 35 * CV.MPH_TO_MS - elif candidate == CAR.KIA_OPTIMA: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 3558. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 - tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate == CAR.KIA_STINGER: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1825. + STD_CARGO_KG - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - ret.minSteerSpeed = 0. - else: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1985. + STD_CARGO_KG - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - ret.minSteerSpeed = 0. - - ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1985. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 15.0 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.minSteerSpeed = 0. + + ret.minEnableSpeed = -1. ret.longitudinalTuning.kpBP = [0.] ret.longitudinalTuning.kpV = [0.] ret.longitudinalTuning.kiBP = [0.] @@ -166,7 +116,6 @@ def get_params(candidate, fingerprint, vin=""): ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.openpilotLongitudinalControl = False ret.steerLimitAlert = False @@ -199,14 +148,6 @@ def update(self, c): ret.wheelSpeeds.rl = self.CS.v_wheel_rl ret.wheelSpeeds.rr = self.CS.v_wheel_rr - # gear shifter - if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: - ret.gearShifter = self.CS.gear_shifter_cluster - elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: - ret.gearShifter = self.CS.gear_tcu - else: - ret.gearShifter = self.CS.gear_shifter - # gas pedal ret.gas = self.CS.car_gas ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 60d5b81f9cc8a6..37ec9dadbd4e0e 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -11,13 +11,7 @@ def get_hud_alerts(visual_alert, audible_alert): return 0 class CAR: - ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" # First User @TK211X <-- Ported by @ku7 (Second, First Hyundai in World) - GENESIS = "HYUNDAI GENESIS 2018" # First User @xx979xx <-- Ported by @xx979xx - KIA_OPTIMA = "KIA OPTIMA SX 2019" # First User @blender0 - KIA_SORENTO = "KIA SORENTO GT LINE 2018" # First User @ku7 <-- Ported by @ku7 (First Kia in World) - KIA_STINGER = "KIA STINGER GT2 2018" # First User @killian <-- Ported by @ku7 (Third) - SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" # First User @rickbias <-- Ported by @rickbias - UNKNOWN = "UNKNOWN CAR ON EMMERTEX FORK" # Unsupported Car + HKG = "HYUNDAI OR KIA" class Buttons: NONE = 0 @@ -25,60 +19,14 @@ class Buttons: SET_DECEL = 2 CANCEL = 4 - FINGERPRINTS = { - CAR.ELANTRA: [{ - 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], - CAR.GENESIS: [{ - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 - }], - CAR.KIA_OPTIMA: [{ - 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], - CAR.KIA_SORENTO: [{ - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 - }], - CAR.KIA_STINGER: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8 - }], - CAR.SANTA_FE: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 - }, - { - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 - }], - CAR.UNKNOWN: [{ + CAR.HKG: [{ 832: 8 }], } -CAMERA_MSGS = [832, 1156, 1191, 1342] - -CHECKSUM = { - "crc8": [CAR.SANTA_FE], - "6B": [CAR.KIA_SORENTO, CAR.GENESIS, CAR.UNKNOWN], - "7B": [CAR.KIA_STINGER, CAR.ELANTRA, CAR.KIA_OPTIMA], -} - -FEATURES = { - "icon_basic": [CAR.GENESIS], # Anything but 2 for LKAS_Icon causes MDPS Fault - "soft_disable": [CAR.GENESIS], # Any steer message below 16.5m/s faults MDPS - "use_cluster_gears": [CAR.ELANTRA], # Use Cluster for Gear Selection, rather than Transmission - "use_tcu_gears": [CAR.KIA_OPTIMA], # Use TCU Message for Gear Selection -} - DBC = { - CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None), - CAR.UNKNOWN: dbc_dict('hyundai_kia_generic', None), + CAR.HKG: dbc_dict('hyundai_kia_generic', None), } -STEER_THRESHOLD = 1.0 +STEER_THRESHOLD = 1.5 diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 9fe812ee27980e..3710c7d39598f6 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.0.1-ku7" +#define COMMA_VERSION "0.6.0.2-ku7" diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 6ceb56b627756c..fe7e328e679f14 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -19,7 +19,7 @@ def install(): else: from raven import Client from raven.transport.http import HTTPTransport - client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', + client = Client('https://84d713b5bd674bcbb7030d1b86115dcb:80109516f2dd4ee0b9dbb72331930189@sentry.io/1405628', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) def capture_exception(*args, **kwargs): diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 3af76818d484cd..35d2af8def5b33 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -104,7 +104,7 @@ def report_tombstone(fn, client): def main(gctx=None): initial_tombstones = set(get_tombstones()) - client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', + client = Client('https://84d713b5bd674bcbb7030d1b86115dcb:80109516f2dd4ee0b9dbb72331930189@sentry.io/1405628', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}, string_max_length=10000) client.user_context({'id': os.environ.get('DONGLE_ID')}) From c54abb7a50ff5a218575e3f570ea9d846b697185 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Mon, 8 Jul 2019 12:07:16 +1000 Subject: [PATCH 03/24] update releases --- RELEASES.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/RELEASES.md b/RELEASES.md index f73b8daf83ebb8..32d7e142d0637b 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,26 @@ +Version 0.6.0.2-ku7 (2019-07-08) +======================== +* HKG - Auto Discover Checksum if LKAS11 Message not present +* HKG - Disable Torque on Lane Change +* HKG - Auto Learn Low Speed Cutoff +* HKG - Disable during Low Speed, where low speed is learnt +* HKG - Improve Support, maybe even Genesis +* HKG - Clean up redundant code, remove last "Fingerprint" stuff +* HKG - Add Sentry Crash Reporting +* HKG - PUF is back, Auto Forward CAM/CAR when EON is not active +* HKG - Hopefully a better attempt at "Remove remnants of CP UI" + + +Version 0.6.0.1-ku7 (2019-07-05) +======================== +* HKG - Auto Support Cars with all 3 Gear Selector Types +* HKG - Auto Support Cars with no SCC +* HKG - Auto Learn Checksum from LKAS11 +* HKG - Do not diable on Accelerator or Brake +* HKG - Removed Fingerprint Requiement +* HKG - Removed Noisy Console/Logs +* HKG - Remove remnants of CP UI + Version 0.6 (2019-07-01) ======================== * New model, with double the pixels and ten times the temporal context! From 89140147ea2092389d5ff2373f5f39829196e0d1 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Mon, 8 Jul 2019 16:47:50 +1000 Subject: [PATCH 04/24] bug --- selfdrive/car/hyundai/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index cd1bc033542e5e..6b7619656cfba7 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -117,7 +117,7 @@ def get_params(candidate, fingerprint, vin=""): ret.brakeMaxV = [1.] ret.openpilotLongitudinalControl = False - + ret.enableCamera = True ret.steerLimitAlert = False ret.stoppingControl = False ret.startAccel = 0.0 From 97ea1f5b235353563b5a6958b844a2467fdd3461 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Mon, 8 Jul 2019 22:33:41 +1000 Subject: [PATCH 05/24] Send fingerprints to Sentry --- selfdrive/car/car_helpers.py | 7 ++++++ selfdrive/crash.py | 41 ++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 1fd99f76e31454..c88abd3f2caef3 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,6 +5,7 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging +import selfdrive.crash as crash def get_startup_alert(car_recognized, controller_available): @@ -146,6 +147,12 @@ def get_car(logcan, sendcan): if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" + else: + cloudlog.warning("car does match fingerprint: %r", fingerprints) + try: + crash.capture_warning("fingerprinted %s" % candidate) + except: # fixes occasional travis errors + pass CarInterface, CarController = interfaces[candidate] params = CarInterface.get_params(candidate, fingerprints, vin) diff --git a/selfdrive/crash.py b/selfdrive/crash.py index fe7e328e679f14..89cd420570755a 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -19,6 +19,44 @@ def install(): else: from raven import Client from raven.transport.http import HTTPTransport + import json + + error_tags = {'dirty': dirty, 'username': 'char_error'} + + try: + with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f: + auth = json.loads(f.read()) + auth = json.loads(auth['commaUser']) + tags = ['username', 'email'] + for tag in tags: + try: + error_tags[tag] = ''.join(char for char in auth[tag].decode('utf-8', 'ignore') if char.isalnum()) + except: + pass + except: + pass + + try: + with open("/data/params/d/CommunityPilotUser", "r") as f: + auth = json.loads(f.read()) + tags = ['username', 'email'] + for tag in tags: + try: + error_tags[tag] = ''.join(char for char in auth[tag].decode('utf-8', 'ignore') if char.isalnum()) + except: + pass + except: + pass + + logging_data = {"branch": "/data/params/d/GitBranch", "commit": "/data/params/d/GitCommit", "remote": "/data/params/d/GitRemote"} + + for key in logging_data: + try: + with open(logging_data[key], "r") as f: + error_tags[key] = str(f.read()) + except: + error_tags[key] = "unknown" + client = Client('https://84d713b5bd674bcbb7030d1b86115dcb:80109516f2dd4ee0b9dbb72331930189@sentry.io/1405628', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) @@ -28,6 +66,9 @@ def capture_exception(*args, **kwargs): client.captureException(*args, **kwargs) cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) + def capture_warning(warning_string): + client.captureMessage(warning_string, level='warning') + def bind_user(**kwargs): client.user_context(kwargs) From 4a48ef8dbc51ea2df02b1cf1b54fb514d8120c92 Mon Sep 17 00:00:00 2001 From: dekerr Date: Mon, 8 Jul 2019 19:59:32 -0400 Subject: [PATCH 06/24] Refactor default Civic params (#720) * move civic params out * fix variable name * simplify ford scaling * cleanup * remove import dependency * requested changes * keep hyundai --- selfdrive/car/__init__.py | 28 ++++++++++++++++++++++++ selfdrive/car/chrysler/interface.py | 23 +++---------------- selfdrive/car/ford/interface.py | 31 +++++--------------------- selfdrive/car/gm/interface.py | 25 +++++---------------- selfdrive/car/honda/interface.py | 30 ++++++------------------- selfdrive/car/hyundai/interface.py | 27 +++++------------------ selfdrive/car/subaru/interface.py | 24 +++----------------- selfdrive/car/toyota/interface.py | 34 ++++++++--------------------- 8 files changed, 65 insertions(+), 157 deletions(-) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index bb996e5885e37a..3b259c622289fd 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -3,6 +3,34 @@ # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. + +# FIXME: hardcoding honda civic 2016 touring params so they can be used to +# scale unknown params for other cars +class CivicParams: + MASS = 1326. + STD_CARGO_KG + WHEELBASE = 2.70 + CENTER_TO_FRONT = WHEELBASE * 0.4 + CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT + ROTATIONAL_INERTIA = 2500 + TIRE_STIFFNESS_FRONT = 192150 + TIRE_STIFFNESS_REAR = 202500 + +# TODO: get actual value, for now starting with reasonable value for +# civic and scaling by mass and wheelbase +def scale_rot_inertia(mass, wheelbase): + return CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.WHEELBASE ** 2) + +# TODO: start from empirically derived lateral slip stiffness for the civic and scale by +# mass and CG position, so all cars will have approximately similar dyn behaviors +def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor=1.0): + center_to_rear = wheelbase - center_to_front + tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \ + (center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE) + + tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \ + (center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE) + + return tire_stiffness_front, tire_stiffness_rear def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 6621c7e20ffcdf..793f7320844148 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -51,16 +51,6 @@ def get_params(candidate, fingerprint, vin=""): # pedal ret.enableCruise = True - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 * 2.0 - tireStiffnessRear_civic = 90000 * 2.0 - # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 @@ -85,20 +75,13 @@ def get_params(candidate, fingerprint, vin=""): ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. # TODO allow 2019 cars to steer down to 13 m/s if already engaged. - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 7664d99640eaab..0f3bbc33cc2b84 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -7,7 +7,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.ford.carstate import CarState, get_can_parser from selfdrive.car.ford.values import MAX_ANGLE -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -51,16 +51,6 @@ def get_params(candidate, fingerprint, vin=""): # pedal ret.enableCruise = True - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 - tireStiffnessRear_civic = 90000 - ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG @@ -69,32 +59,21 @@ def get_params(candidate, fingerprint, vin=""): ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 - - f = 1.2 - tireStiffnessFront_civic *= f - tireStiffnessRear_civic *= f - ret.centerToFront = ret.wheelbase * 0.44 - + tire_stiffness_factor = 0.5328 # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. ret.minEnableSpeed = -1. - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 4a909bc84403ff..c066a6523f2213 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -7,7 +7,7 @@ from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \ SUPERCRUISE_CARS, AccState from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CanBus(object): @@ -60,6 +60,7 @@ def get_params(candidate, fingerprint, vin=""): # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) ret.openpilotLongitudinalControl = ret.enableCamera + tire_stiffness_factor = 0.444 # not optimized yet if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) @@ -129,30 +130,14 @@ def get_params(candidate, fingerprint, vin=""): ret.centerToFront = ret.wheelbase * 0.465 - # hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 - tireStiffnessRear_civic = 90000 - - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # same tuning for Volt and CT6 for now ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 72c85c862cea57..93aaf0182b7db6 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -10,7 +10,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD, CAMERA_MSGS -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -152,16 +152,6 @@ def get_params(candidate, fingerprint, vin=""): ret.enableCruise = not ret.enableGasInterceptor - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle # model optimization process. Certain Hondas have an extra steering sensor at the bottom # of the steering rack, which improves controls quality as it removes the steering column @@ -174,9 +164,9 @@ def get_params(candidate, fingerprint, vin=""): if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: stop_and_go = True - ret.mass = mass_civic - ret.wheelbase = wheelbase_civic - ret.centerToFront = centerToFront_civic + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 14.63 # 10.93 is end-to-end spec tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car @@ -334,20 +324,14 @@ def get_params(candidate, fingerprint, vin=""): # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index bd99eaa5ac7b91..67ca2261ff7f17 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -51,19 +51,9 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyModel = car.CarParams.SafetyModel.hyundai ret.enableCruise = True # stock acc - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - tire_stiffness_factor = 1. - ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 + tire_stiffness_factor = 1. if candidate == CAR.SANTA_FE: ret.lateralTuning.pid.kf = 0.00005 @@ -129,21 +119,14 @@ def get_params(candidate, fingerprint, vin=""): ret.centerToFront = ret.wheelbase * 0.4 - centerToRear = ret.wheelbase - ret.centerToFront - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index a609ee9092c115..e9d9c117fc714c 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.subaru.values import CAR from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -58,7 +58,6 @@ def get_params(candidate, fingerprint, vin=""): ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 15 - tire_stiffness_factor = 1.0 ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] @@ -84,30 +83,13 @@ def get_params(candidate, fingerprint, vin=""): # end from gm - # hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - centerToRear = ret.wheelbase - ret.centerToFront - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) return ret diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 83bf921d95d6dd..a1cffae9594942 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness from selfdrive.swaglog import cloudlog @@ -54,16 +54,6 @@ def get_params(candidate, fingerprint, vin=""): # pedal ret.enableCruise = not ret.enableGasInterceptor - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay if candidate != CAR.PRIUS: ret.lateralTuning.init('pid') @@ -100,7 +90,7 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyParam = 100 ret.wheelbase = 2.70 ret.steerRatio = 17.8 - tire_stiffness_factor = 0.444 + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 @@ -140,7 +130,7 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyParam = 73 ret.wheelbase = 2.78 ret.steerRatio = 16.0 - tire_stiffness_factor = 0.444 # not optimized yet + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] ret.lateralTuning.pid.kf = 0.00006 @@ -170,7 +160,7 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 - tire_stiffness_factor = 0.444 + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 @@ -179,8 +169,8 @@ def get_params(candidate, fingerprint, vin=""): stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.8702 - ret.steerRatio = 16.0 #not optimized - tire_stiffness_factor = 0.444 + ret.steerRatio = 16.0 # not optimized + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 @@ -195,20 +185,14 @@ def get_params(candidate, fingerprint, vin=""): # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. From 16eb74250c41d68869a7ab4c52716e58191311b6 Mon Sep 17 00:00:00 2001 From: Nick Brown Date: Mon, 8 Jul 2019 20:02:59 -0400 Subject: [PATCH 07/24] 2019 Rav4 Limited AWD (#732) * Fingerprint * Merge Limited and XLE fingerprint because they're the same --- selfdrive/car/toyota/values.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f487888424942d..da8b02dcd7212c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -165,9 +165,9 @@ def check_ecu_msgs(fingerprint, ecu): { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, - # XLE and AWD + # XLE, Limited, and AWD { - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 }], CAR.COROLLA_TSS2: [ # hatch 2019+ and sedan 2020+ From c2d4cc8abb4037b00b32366ffa7dcc49f1191ec7 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 11:30:03 +1000 Subject: [PATCH 08/24] 0.6.0.3 - Untested --- RELEASES.md | 15 +- selfdrive/car/hyundai/carcontroller.py | 72 +++- selfdrive/car/hyundai/carstate.py | 7 +- selfdrive/car/hyundai/hyundaican.py | 10 +- selfdrive/car/hyundai/interface.py | 4 +- selfdrive/common/version.h | 2 +- selfdrive/manager.py | 2 + selfdrive/mapd/__init__.py | 0 selfdrive/mapd/default_speeds.json | 106 +++++ selfdrive/mapd/default_speeds_generator.py | 240 ++++++++++++ selfdrive/mapd/mapd.py | 341 ++++++++++++++++ selfdrive/mapd/mapd_helpers.py | 435 +++++++++++++++++++++ 12 files changed, 1217 insertions(+), 17 deletions(-) create mode 100644 selfdrive/mapd/__init__.py create mode 100644 selfdrive/mapd/default_speeds.json create mode 100755 selfdrive/mapd/default_speeds_generator.py create mode 100755 selfdrive/mapd/mapd.py create mode 100644 selfdrive/mapd/mapd_helpers.py diff --git a/RELEASES.md b/RELEASES.md index 32d7e142d0637b..cacdf51aaccbc8 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,9 +1,18 @@ -Version 0.6.0.2-ku7 (2019-07-08) +Version 0.6.0.3-ku7 (2019-07-**) ======================== +* HKG - Send fingerprints to Sentry +* HKG - Bring back mapd from 0.5.12 +* HKG - Auto Set Cruise to Speed Limit +* HKG - Don't Learn Min Steer Speed when turning corners +* HKG - Create MDPS12 Message (to reduce/stop Stock cam faults) +* HKG - Implement Min Speed Speed Warning + +Version 0.6.0.2-ku7 (2019-07-08) +========================low * HKG - Auto Discover Checksum if LKAS11 Message not present * HKG - Disable Torque on Lane Change -* HKG - Auto Learn Low Speed Cutoff -* HKG - Disable during Low Speed, where low speed is learnt +* HKG - Auto Learn Min Steer Speed Cutoff +* HKG - Disable during Min Steer Speed, where min speed is learnt * HKG - Improve Support, maybe even Genesis * HKG - Clean up redundant code, remove last "Fingerprint" stuff * HKG - Add Sentry Crash Reporting diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1a7130d1bad2aa..000d6d7f3dace7 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,9 +1,14 @@ from selfdrive.car import limit_steer_rate from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \ create_1191, create_1156, \ - create_clu11, learn_checksum + create_clu11, learn_checksum, create_mdps12 from selfdrive.car.hyundai.values import Buttons from selfdrive.can.packer import CANPacker +import zmq +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.config import Conversions as CV +from common.params import Params # Steer torque limits @@ -24,9 +29,19 @@ def __init__(self, dbc_name, car_fingerprint): self.lkas11_cnt = 0 self.clu11_cnt = 0 + self.mdps12_cnt = 0 self.cnt = 0 self.last_resume_cnt = 0 + self.map_speed = 0 + context = zmq.Context() + self.map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True) + self.params = Params() + self.speed_conv = 3.6 + self.speed_offset = 1.03 # Multiplier for cruise speed vs speed limit TODO: Add to UI + self.speed_enable = True # Enable Auto Speed Set TODO: Add to UI + self.speed_adjusted = False + self.checksum = "NONE" self.checksum_learn_cnt = 0 @@ -39,6 +54,7 @@ def __init__(self, dbc_name, car_fingerprint): def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Error State Resets ### disable_steer = False + can_sends = [] ### Learn Checksum ### @@ -53,6 +69,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # If MDPS is faulted from bad checksum, then cycle through all Checksums until 1 works if CS.steer_error == 1: self.camera_disconnected = True + print ("Camera Not Detected: Brute Forcing Checksums") if self.checksum_learn_cnt > 250: self.checksum_learn_cnt = 50 if self.checksum == "NONE": @@ -72,7 +89,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Minimum Steer Speed ### # Learn Minimum Steer Speed - if CS.mdps12_flt != 0 and CS.v_ego_raw > 0.: + if CS.mdps12_flt != 0 and CS.v_ego_raw > 0. and abs(CS.angle_steers) < 10.0 : if CS.v_ego_raw > self.min_steer_speed: self.min_steer_speed = CS.v_ego_raw + 0.1 print ("Discovered new Min Speed as", self.min_steer_speed) @@ -102,11 +119,52 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.apply_steer_last = apply_steer + ### Auto Speed Limit ### + + # Read Speed Limit and define if adjustment needed + if (self.cnt % 50) == 0 and self.speed_enable: + if not (enabled and CS.acc_active): + self.speed_adjusted = False + map_data = messaging.recv_one_or_none(self.map_data_sock) + if map_data is not None: + if bool(self.params.get("IsMetric")): + self.speed_conv = CV.MS_TO_KPH + else: + self.speed_conv = CV.MS_TO_MPH + + if map_data.liveMapData.speedLimitValid: + last_speed = self.map_speed + v_speed = int(map_data.liveMapData.speedLimit * self.speed_offset) + self.map_speed = v_speed * self.speed_conv + if last_speed != self.map_speed: + self.speed_adjusted = False + else: + self.map_speed = 0 + self.speed_adjusted = True + else: + self.map_speed = 0 + self.speed_adjusted = True + + # Spam buttons for Speed Adjustment + if CS.acc_active and not self.speed_adjusted and self.map_speed > (8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1): + if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005): + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0))) + elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005): + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0))) + else: + self.speed_adjusted = True + + # Cancel Adjustment on Pedal + if CS.pedal_gas: + self.speed_adjusted = True + + + ### Generate CAN Messages ### - can_sends = [] self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 + self.mdps12_cnt = self.cnt % 0x100 if self.camera_disconnected: if (self.cnt % 10) == 0: @@ -119,11 +177,15 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, enabled, CS.lkas11, hud_alert, (not self.camera_disconnected), self.checksum)) + if not self.camera_disconnected: + can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \ + self.checksum)) + if pcm_cancel_cmd: - can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0)) elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt - can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0)) self.cnt += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e982d343c80f54..684ef84ec116b7 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -159,6 +159,10 @@ def __init__(self, CP): self.right_blinker_on = 0 self.right_blinker_flash = 0 self.has_scc = False + self.min_steer_speed = 0 + + def update_min_speed(speed): + self.min_steer_speed = speed def update(self, cp, cp_cam): if (cp.vl["SCC11"]['TauGapSet'] > 0): @@ -270,6 +274,7 @@ def update(self, cp, cp_cam): else: self.gear_tcu = "unknown" - # save the entire LKAS11 and CLU11 + # save the entire LKAS11, CLU11 and MDPS12 messages self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] + self.mdps12 = cp.vl["MDPS12"] diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index f1999bdece50a6..7590e5601a452b 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -58,7 +58,7 @@ def create_1191(): def create_1156(): return make_can_msg(1156, "\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) -def create_clu11(packer, clu11, button): +def create_clu11(packer, clu11, button, cnt): values = { "CF_Clu_CruiseSwState": button, "CF_Clu_CruiseSwMain": clu11["CF_Clu_CruiseSwMain"], @@ -71,12 +71,12 @@ def create_clu11(packer, clu11, button): "CF_Clu_RheostatLevel": clu11["CF_Clu_RheostatLevel"], "CF_Clu_CluInfo": clu11["CF_Clu_CluInfo"], "CF_Clu_AmpInfo": clu11["CF_Clu_AmpInfo"], - "CF_Clu_AliveCnt1": 0, + "CF_Clu_AliveCnt1": cnt, } return packer.make_can_msg("CLU11", 0, values) -def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, camcan, checksum): +def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, checksum): values = { "CR_Mdps_StrColTq": mdps12["CR_Mdps_StrColTq"], "CF_Mdps_Def": mdps12["CF_Mdps_Def"], @@ -92,12 +92,12 @@ def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, camcan, checksum } if not (checksum == "crc8"): - dat = packer.make_can_msg("MDPS12", camcan, values)[2] + dat = packer.make_can_msg("MDPS12", 2, values)[2] dat = [ord(i) for i in dat] checksum = (dat[0] + dat[1] + dat[2] + dat[4] + dat[5] + dat[6] + dat[7]) % 256 values["CF_Mdps_Chksum2"] = checksum - return packer.make_can_msg("MDPS12", camcan, values) + return packer.make_can_msg("MDPS12", 2, values) def learn_checksum(packer, lkas11): values = { diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6b7619656cfba7..dbacd2f1bdd779 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -196,9 +196,9 @@ def update(self, c): ret.seatbeltUnlatched = not self.CS.seatbelt # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: + if ret.vEgo < (self.CS.min_steer_speed + 2.) and self.CS.min_steer_speed > 10.: self.low_speed_alert = True - if ret.vEgo > (self.CP.minSteerSpeed + 4.): + if ret.vEgo > (self.CS.min_steer_speed + 4.): self.low_speed_alert = False events = [] diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 3710c7d39598f6..37e984188c0258 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.0.2-ku7" +#define COMMA_VERSION "0.6.0.3-ku7" diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 8c28a9e2e97f99..d9905a041cde11 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -100,6 +100,7 @@ def unblock_stdout(): "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), + "mapd": "selfdrive.mapd.mapd", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", @@ -151,6 +152,7 @@ def get_running(): 'proclogd', 'ubloxd', 'gpsd', + 'mapd', 'deleter', ] diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json new file mode 100644 index 00000000000000..7126c27fcdef40 --- /dev/null +++ b/selfdrive/mapd/default_speeds.json @@ -0,0 +1,106 @@ +{ + "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", + "AR:urban": "40", + "AR:urban:primary": "60", + "AR:urban:secondary": "60", + "AR:rural": "110", + "AT:urban": "50", + "AT:rural": "100", + "AT:trunk": "100", + "AT:motorway": "130", + "BE:urban": "50", + "BE-VLG:rural": "70", + "BE-WAL:rural": "90", + "BE:trunk": "120", + "BE:motorway": "120", + "CH:urban[1]": "50", + "CH:rural": "80", + "CH:trunk": "100", + "CH:motorway": "120", + "CZ:pedestrian_zone": "20", + "CZ:living_street": "20", + "CZ:urban": "50", + "CZ:urban_trunk": "80", + "CZ:urban_motorway": "80", + "CZ:rural": "90", + "CZ:trunk": "110", + "CZ:motorway": "130", + "DK:urban": "50", + "DK:rural": "80", + "DK:motorway": "130", + "DE:living_street": "7", + "DE:residential": "30", + "DE:urban": "50", + "DE:rural": "100", + "DE:trunk": "none", + "DE:motorway": "none", + "FI:urban": "50", + "FI:rural": "80", + "FI:trunk": "100", + "FI:motorway": "120", + "FR:urban": "50", + "FR:rural": "80", + "FR:trunk": "110", + "FR:motorway": "130", + "GR:urban": "50", + "GR:rural": "90", + "GR:trunk": "110", + "GR:motorway": "130", + "HU:urban": "50", + "HU:rural": "90", + "HU:trunk": "110", + "HU:motorway": "130", + "IT:urban": "50", + "IT:rural": "90", + "IT:trunk": "110", + "IT:motorway": "130", + "JP:national": "60", + "JP:motorway": "100", + "LT:living_street": "20", + "LT:urban": "50", + "LT:rural": "90", + "LT:trunk": "120", + "LT:motorway": "130", + "PL:living_street": "20", + "PL:urban": "50", + "PL:rural": "90", + "PL:trunk": "100", + "PL:motorway": "140", + "RO:urban": "50", + "RO:rural": "90", + "RO:trunk": "100", + "RO:motorway": "130", + "RU:living_street": "20", + "RU:urban": "60", + "RU:rural": "90", + "RU:motorway": "110", + "SK:urban": "50", + "SK:rural": "90", + "SK:trunk": "90", + "SK:motorway": "90", + "SI:urban": "50", + "SI:rural": "90", + "SI:trunk": "110", + "SI:motorway": "130", + "ES:living_street": "20", + "ES:urban": "50", + "ES:rural": "50", + "ES:trunk": "90", + "ES:motorway": "120", + "SE:urban": "50", + "SE:rural": "70", + "SE:trunk": "90", + "SE:motorway": "110", + "GB:nsl_restricted": "30 mph", + "GB:nsl_single": "60 mph", + "GB:nsl_dual": "70 mph", + "GB:motorway": "70 mph", + "UA:urban": "50", + "UA:rural": "90", + "UA:trunk": "110", + "UA:motorway": "130", + "UZ:living_street": "30", + "UZ:urban": "70", + "UZ:rural": "100", + "UZ:motorway": "110" +} diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py new file mode 100755 index 00000000000000..888c9e1df9e933 --- /dev/null +++ b/selfdrive/mapd/default_speeds_generator.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python +import json + +DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" + +def main(filename = DEFAULT_OUTPUT_FILENAME): + countries = [] + + """ + -------------------------------------------------- + US - United State of America + -------------------------------------------------- + """ + US = Country("US") # First step, create the country using the ISO 3166 two letter code + countries.append(US) # Second step, add the country to countries list + + """ Default rules """ + # Third step, add some default rules for the country + # Speed limit rules are based on OpenStreetMaps (OSM) tags. + # The dictionary {...} defines the tag_name: value + # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied. + # The text at the end is the speed limit (use no unit for km/h) + # Rules apply in the order in which they are written for each country + # Rules for specific regions (states) take priority over country rules + # If you modify existing country rules, you must update all existing states without that rule to use the old rule + US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph + US.add_rule({"highway": "trunk"}, "55 mph") + US.add_rule({"highway": "primary"}, "55 mph") + US.add_rule({"highway": "secondary"}, "45 mph") + US.add_rule({"highway": "tertiary"}, "35 mph") + US.add_rule({"highway": "unclassified"}, "55 mph") + US.add_rule({"highway": "residential"}, "25 mph") + US.add_rule({"highway": "service"}, "25 mph") + US.add_rule({"highway": "motorway_link"}, "55 mph") + US.add_rule({"highway": "trunk_link"}, "55 mph") + US.add_rule({"highway": "primary_link"}, "55 mph") + US.add_rule({"highway": "secondary_link"}, "45 mph") + US.add_rule({"highway": "tertiary_link"}, "35 mph") + US.add_rule({"highway": "living_street"}, "15 mph") + + """ States """ + new_york = US.add_region("New York") # Fourth step, add a state/region to country + new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules + new_york.add_rule({"highway": "secondary"}, "55 mph") + new_york.add_rule({"highway": "tertiary"}, "55 mph") + new_york.add_rule({"highway": "residential"}, "30 mph") + new_york.add_rule({"highway": "primary_link"}, "45 mph") + new_york.add_rule({"highway": "secondary_link"}, "55 mph") + new_york.add_rule({"highway": "tertiary_link"}, "55 mph") + # All if not written by the state, the rules will default to the country rules + + #california = US.add_region("California") + # California uses only the default US rules + + michigan = US.add_region("Michigan") + michigan.add_rule({"highway": "motorway"}, "70 mph") + + oregon = US.add_region("Oregon") + oregon.add_rule({"highway": "motorway"}, "55 mph") + oregon.add_rule({"highway": "secondary"}, "35 mph") + oregon.add_rule({"highway": "tertiary"}, "30 mph") + oregon.add_rule({"highway": "service"}, "15 mph") + oregon.add_rule({"highway": "secondary_link"}, "35 mph") + oregon.add_rule({"highway": "tertiary_link"}, "30 mph") + + south_dakota = US.add_region("South Dakota") + south_dakota.add_rule({"highway": "motorway"}, "80 mph") + south_dakota.add_rule({"highway": "trunk"}, "70 mph") + south_dakota.add_rule({"highway": "primary"}, "65 mph") + south_dakota.add_rule({"highway": "trunk_link"}, "70 mph") + south_dakota.add_rule({"highway": "primary_link"}, "65 mph") + + wisconsin = US.add_region("Wisconsin") + wisconsin.add_rule({"highway": "trunk"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary"}, "45 mph") + wisconsin.add_rule({"highway": "unclassified"}, "35 mph") + wisconsin.add_rule({"highway": "trunk_link"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph") + + """ + -------------------------------------------------- + AU - Australia + -------------------------------------------------- + """ + AU = Country("AU") + countries.append(AU) + + """ Default rules """ + AU.add_rule({"highway": "motorway"}, "100") + AU.add_rule({"highway": "trunk"}, "80") + AU.add_rule({"highway": "primary"}, "80") + AU.add_rule({"highway": "secondary"}, "50") + AU.add_rule({"highway": "tertiary"}, "50") + AU.add_rule({"highway": "unclassified"}, "80") + AU.add_rule({"highway": "residential"}, "50") + AU.add_rule({"highway": "service"}, "40") + AU.add_rule({"highway": "motorway_link"}, "90") + AU.add_rule({"highway": "trunk_link"}, "80") + AU.add_rule({"highway": "primary_link"}, "80") + AU.add_rule({"highway": "secondary_link"}, "50") + AU.add_rule({"highway": "tertiary_link"}, "50") + AU.add_rule({"highway": "living_street"}, "30") + + """ + -------------------------------------------------- + CA - Canada + -------------------------------------------------- + """ + CA = Country("CA") + countries.append(CA) + + """ Default rules """ + CA.add_rule({"highway": "motorway"}, "100") + CA.add_rule({"highway": "trunk"}, "80") + CA.add_rule({"highway": "primary"}, "80") + CA.add_rule({"highway": "secondary"}, "50") + CA.add_rule({"highway": "tertiary"}, "50") + CA.add_rule({"highway": "unclassified"}, "80") + CA.add_rule({"highway": "residential"}, "40") + CA.add_rule({"highway": "service"}, "40") + CA.add_rule({"highway": "motorway_link"}, "90") + CA.add_rule({"highway": "trunk_link"}, "80") + CA.add_rule({"highway": "primary_link"}, "80") + CA.add_rule({"highway": "secondary_link"}, "50") + CA.add_rule({"highway": "tertiary_link"}, "50") + CA.add_rule({"highway": "living_street"}, "20") + + + """ + -------------------------------------------------- + DE - Germany + -------------------------------------------------- + """ + DE = Country("DE") + countries.append(DE) + + """ Default rules """ + DE.add_rule({"highway": "motorway"}, "none") + DE.add_rule({"highway": "living_street"}, "10") + DE.add_rule({"highway": "residential"}, "30") + DE.add_rule({"zone:traffic": "DE:rural"}, "100") + DE.add_rule({"zone:traffic": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:30"}, "30") + DE.add_rule({"zone:maxspeed": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") + DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") + DE.add_rule({"bicycle_road": "yes"}, "30") + + + """ + -------------------------------------------------- + EE - Estonia + -------------------------------------------------- + """ + EE = Country("EE") + countries.append(EE) + + """ Default rules """ + EE.add_rule({"highway": "motorway"}, "90") + EE.add_rule({"highway": "trunk"}, "90") + EE.add_rule({"highway": "primary"}, "90") + EE.add_rule({"highway": "secondary"}, "50") + EE.add_rule({"highway": "tertiary"}, "50") + EE.add_rule({"highway": "unclassified"}, "90") + EE.add_rule({"highway": "residential"}, "40") + EE.add_rule({"highway": "service"}, "40") + EE.add_rule({"highway": "motorway_link"}, "90") + EE.add_rule({"highway": "trunk_link"}, "70") + EE.add_rule({"highway": "primary_link"}, "70") + EE.add_rule({"highway": "secondary_link"}, "50") + EE.add_rule({"highway": "tertiary_link"}, "50") + EE.add_rule({"highway": "living_street"}, "20") + + + """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ + """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ + + # Final step + write_json(countries, filename) + +def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME): + out_dict = {} + for country in countries: + out_dict.update(country.jsonify()) + json_string = json.dumps(out_dict, indent=2) + with open(filename, "wb") as f: + f.write(json_string) + + +class Region(object): + ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"] + ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"] + def __init__(self, name): + self.name = name + self.rules = [] + + def add_rule(self, tag_conditions, speed): + new_rule = {} + if not isinstance(tag_conditions, dict): + raise TypeError("Rule tag conditions must be dictionary") + if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions): + raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS + if 'highway' in tag_conditions: + if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES: + raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"])) + new_rule['tags'] = tag_conditions + try: + new_rule['speed'] = str(speed) + except ValueError: + raise ValueError("Rule speed must be string") + self.rules.append(new_rule) + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = self.rules + return ret_dict + +class Country(Region): + ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"] + def __init__(self, ISO_3166_alpha_2): + Region.__init__(self, ISO_3166_alpha_2) + if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES: + raise ValueError("Not valid IOS 3166 country code") + self.regions = {} + + def add_region(self, name): + self.regions[name] = Region(name) + return self.regions[name] + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = {} + for r_name, region in self.regions.items(): + ret_dict[self.name].update(region.jsonify()) + ret_dict[self.name]['Default'] = self.rules + return ret_dict + + +if __name__ == '__main__': + main() diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100755 index 00000000000000..f20c8ba3da12bf --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,341 @@ +#!/usr/bin/env python + +# Add phonelibs openblas to LD_LIBRARY_PATH if import fails +from common.basedir import BASEDIR +try: + from scipy import spatial +except ImportError as e: + import os + import sys + + + openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/") + os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path + + args = [sys.executable] + args.extend(sys.argv) + os.execv(sys.executable, args) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +from selfdrive.mapd import default_speeds_generator +default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) + +import os +import sys +import time +import zmq +import threading +import numpy as np +import overpy +from collections import defaultdict +from math import sin,cos +from common.params import Params +from common.transformations.coordinates import geodetic2ecef +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points +import selfdrive.crash as crash +from selfdrive.version import version, dirty + + +OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter" +OVERPASS_HEADERS = { + 'User-Agent': 'NEOS (comma.ai)', + 'Accept-Encoding': 'gzip' +} + +last_gps = None +query_lock = threading.Lock() +last_query_result = None +last_query_pos = None +cache_valid = False + +def build_way_query(lat, lon, radius=50): + """Builds a query to find all highways within a given radius around a point""" + pos = " (around:%f,%f,%f)" % (radius, lat, lon) + lat_lon = "(%f,%f)" % (lat, lon) + q = """( + way + """ + pos + """ + [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; + >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; + convert area ::id = id(), admin_level = t['admin_level'], + name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; + """ + return q + + +def query_thread(): + global last_query_result, last_query_pos, cache_valid + api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.) + + while True: + time.sleep(1) + if last_gps is not None: + fix_ok = last_gps.flags & 1 + if not fix_ok: + continue + + if last_query_pos is not None: + cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) + dist = np.linalg.norm(cur_ecef - prev_ecef) + if dist < 3000: #updated when we are 1km from the edge of the downloaded circle + continue + + if dist > 4000: + cache_valid = False + + q = build_way_query(last_gps.latitude, last_gps.longitude, radius=4000) + try: + new_result = api.query(q) + + # Build kd-tree + nodes = [] + real_nodes = [] + node_to_way = defaultdict(list) + location_info = {} + + for n in new_result.nodes: + nodes.append((float(n.lat), float(n.lon), 0)) + real_nodes.append(n) + + for way in new_result.ways: + for n in way.nodes: + node_to_way[n.id].append(way) + + for area in new_result.areas: + if area.tags.get('admin_level', '') == "2": + location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') + if area.tags.get('admin_level', '') == "4": + location_info['region'] = area.tags.get('name', '') + + nodes = np.asarray(nodes) + nodes = geodetic2ecef(nodes) + tree = spatial.cKDTree(nodes) + + query_lock.acquire() + last_query_result = new_result, tree, real_nodes, node_to_way, location_info + last_query_pos = last_gps + cache_valid = True + query_lock.release() + + except Exception as e: + print(e) + query_lock.acquire() + last_query_result = None + query_lock.release() + +def save_gps_data(gps): + try: + location = [gps.speed, gps.bearing, gps.latitude, gps.longitude, gps.altitude, gps.accuracy, time.time()] + with open("/data/openpilot/selfdrive/data_collection/gps-data", "a") as f: + f.write("{}\n".format(location)) + except: + pass + +def mapsd_thread(): + global last_gps + + context = zmq.Context() + poller = zmq.Poller() + gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) + gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller) + map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port) + + cur_way = None + curvature_valid = False + curvature = None + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + max_speed = None + max_speed_ahead = None + max_speed_ahead_dist = None + + max_speed_prev = 0 + + while True: + gps = messaging.recv_one(gps_sock) + gps_ext = None + for socket, event in poller.poll(0): + if socket is gps_external_sock: + gps_ext = messaging.recv_one(socket) + if gps_ext is not None: + gps = gps_ext.gpsLocationExternal + else: + gps = gps.gpsLocation + + save_gps_data(gps) + + last_gps = gps + + fix_ok = gps.flags & 1 + if not fix_ok or last_query_result is None or not cache_valid: + cur_way = None + curvature = None + max_speed_ahead = None + max_speed_ahead_dist = None + curvature_valid = False + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + map_valid = False + else: + map_valid = True + lat = gps.latitude + lon = gps.longitude + heading = gps.bearing + speed = gps.speed + + query_lock.acquire() + cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) + if cur_way is not None: + pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + + xs = pnts[:, 0] + ys = pnts[:, 1] + road_points = [float(x) for x in xs], [float(y) for y in ys] + + if speed < 10: + curvature_valid = False + if curvature_valid and pnts.shape[0] <= 3: + curvature_valid = False + + # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found + if curvature_valid: + # Compute the curvature for each point + with np.errstate(divide='ignore'): + circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])] + circles = np.asarray(circles) + radii = np.nan_to_num(circles[:, 2]) + radii[radii < 10] = np.inf + curvature = 1. / radii + + # Index of closest point + closest = np.argmin(np.linalg.norm(pnts, axis=1)) + dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close + + # Compute distance along path + dists = list() + dists.append(0) + for p, p_prev in zip(pnts, pnts[1:, :]): + dists.append(dists[-1] + np.linalg.norm(p - p_prev)) + dists = np.asarray(dists) + dists = dists - dists[closest] + dist_to_closest + dists = dists[1:-1] + + close_idx = np.logical_and(dists > 0, dists < 500) + dists = dists[close_idx] + curvature = curvature[close_idx] + + if len(curvature): + # TODO: Determine left or right turn + curvature = np.nan_to_num(curvature) + + # Outlier rejection + new_curvature = np.percentile(curvature, 90, interpolation='lower') + + k = 0.6 + upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature + in_turn_indices = curvature > 0.8 * new_curvature + + if np.any(in_turn_indices): + dist_to_turn = np.min(dists[in_turn_indices]) + else: + dist_to_turn = 999 + else: + upcoming_curvature = 0. + dist_to_turn = 999 + + query_lock.release() + + dat = messaging.new_message() + dat.init('liveMapData') + + if last_gps is not None: + dat.liveMapData.lastGps = last_gps + + if cur_way is not None: + dat.liveMapData.wayId = cur_way.id + + # Speed limit + max_speed = cur_way.max_speed() + if max_speed is not None: + #new_latitude = gps.latitude + (MAPS_LOOKAHEAD_DISTANCE * cos(heading/180*3.14159265358979) / (6371010 + gps.altitude)) * (180 / 3.14159265358979) + #new_longitude = gps.longitude + (MAPS_LOOKAHEAD_DISTANCE * sin(heading/180*3.14159265358979) / (6371010 + gps.altitude)) * (180 / 3.14159265358979) / cos(gps.latitude * 3.14159265358979/180) + ahead_speed = None + max_speed_ahead = None + max_speed_ahead_dist = None + #ahead_speed = Way.closest(last_query_result, new_latitude, new_longitude, heading, ahead_speed) + #if ahead_speed is not None and ahead_speed < max_speed: + # max_speed_ahead = ahead_speed.max_speed() + # print "speed ahead found" + # print max_speed_ahead + # max_speed_ahead_dist = cur_way.distance_to_closest_node(lat, lon, heading, pnts) + # print "distance" + # print max_speed_ahead_dist + + if abs(max_speed - max_speed_prev) > 0.1: + max_speed_prev = max_speed + + + + # TODO: use the function below to anticipate upcoming speed limits + max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + if max_speed_ahead is not None and max_speed_ahead_dist is not None: + dat.liveMapData.speedLimitAheadValid = True + dat.liveMapData.speedLimitAhead = float(max_speed_ahead) + dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) + + + advisory_max_speed = cur_way.advisory_max_speed() + if advisory_max_speed is not None: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = advisory_max_speed + + # Curvature + dat.liveMapData.curvatureValid = curvature_valid + dat.liveMapData.curvature = float(upcoming_curvature) + dat.liveMapData.distToTurn = float(dist_to_turn) + if road_points is not None: + dat.liveMapData.roadX, dat.liveMapData.roadY = road_points + if curvature is not None: + dat.liveMapData.roadCurvatureX = [float(x) for x in dists] + dat.liveMapData.roadCurvature = [float(x) for x in curvature] + + + if max_speed is not None: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = max_speed + + #print "max_speed_prev" + #print max_speed_prev + #print "max_speed" + #print max_speed + dat.liveMapData.mapValid = map_valid + + map_data_sock.send(dat.to_bytes()) + + +def main(gctx=None): + params = Params() + dongle_id = params.get("DongleId") + crash.bind_user(id=dongle_id) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.install() + + main_thread = threading.Thread(target=mapsd_thread) + main_thread.daemon = True + main_thread.start() + + q_thread = threading.Thread(target=query_thread) + q_thread.daemon = True + q_thread.start() + + while True: + time.sleep(0.1) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py new file mode 100644 index 00000000000000..de24ea58882c03 --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -0,0 +1,435 @@ +import math +import json +import numpy as np +from datetime import datetime +from common.basedir import BASEDIR +from selfdrive.config import Conversions as CV +from common.transformations.coordinates import LocalCoord, geodetic2ecef + +LOOKAHEAD_TIME = 10. +MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME + +DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json" +DEFAULT_SPEEDS = {} +with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS = json.loads(f.read()) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +DEFAULT_SPEEDS_BY_REGION = {} +with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) + +def circle_through_points(p1, p2, p3): + """Fits a circle through three points + Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm""" + x1, y1, _ = p1 + x2, y2, _ = p2 + x3, y3, _ = p3 + + A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2 + B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1) + C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2) + D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2) + + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + +def parse_speed_unit(max_speed): + """Converts a maxspeed string to m/s based on the unit present in the input. + OpenStreetMap defaults to kph if no unit is present. """ + + if not max_speed: + return None + + conversion = CV.KPH_TO_MS + if 'mph' in max_speed: + max_speed = max_speed.replace(' mph', '') + conversion = CV.MPH_TO_MS + try: + return float(max_speed) * conversion + except ValueError: + return None + +def parse_speed_tags(tags): + """Parses tags on a way to find the maxspeed string""" + max_speed = None + + if 'maxspeed' in tags: + max_speed = tags['maxspeed'] + + if 'maxspeed:conditional' in tags: + try: + max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') + cond = cond[1:-1] + + start, end = cond.split('-') + now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays + start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + + if start <= now <= end: + max_speed = max_speed_cond + except ValueError: + pass + + if not max_speed and 'source:maxspeed' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None) + if not max_speed and 'maxspeed:type' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None) + + max_speed = parse_speed_unit(max_speed) + return max_speed + +def geocode_maxspeed(tags, location_info): + max_speed = None + try: + geocode_country = location_info.get('country', '') + geocode_region = location_info.get('region', '') + + country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {}) + country_defaults = country_rules.get('Default', []) + for rule in country_defaults: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching country + + region_rules = country_rules.get(geocode_region, []) + for rule in region_rules: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching region + except KeyError: + pass + max_speed = parse_speed_unit(max_speed) + return max_speed + +class Way: + def __init__(self, way, query_results): + self.id = way.id + self.way = way + self.query_results = query_results + + points = list() + + for node in self.way.get_nodes(resolve_missing=False): + points.append((float(node.lat), float(node.lon), 0.)) + + self.points = np.asarray(points) + + @classmethod + def closest(cls, query_results, lat, lon, heading, prev_way=None): + results, tree, real_nodes, node_to_way, location_info = query_results + + cur_pos = geodetic2ecef((lat, lon, 0)) + nodes = tree.query_ball_point(cur_pos, 500) + + # If no nodes within 500m, choose closest one + if not nodes: + nodes = [tree.query(cur_pos)[1]] + + ways = [] + for n in nodes: + real_node = real_nodes[n] + ways += node_to_way[real_node.id] + ways = set(ways) + + closest_way = None + best_score = None + for way in ways: + way = Way(way, query_results) + points = way.points_in_car_frame(lat, lon, heading) + + on_way = way.on_way(lat, lon, heading, points) + if not on_way: + continue + + # Create mask of points in front and behind + x = points[:, 0] + y = points[:, 1] + angles = np.arctan2(y, x) + front = np.logical_and((-np.pi / 2) < angles, + angles < (np.pi / 2)) + behind = np.logical_not(front) + + dists = np.linalg.norm(points, axis=1) + + # Get closest point behind the car + dists_behind = np.copy(dists) + dists_behind[front] = np.NaN + closest_behind = points[np.nanargmin(dists_behind)] + + # Get closest point in front of the car + dists_front = np.copy(dists) + dists_front[behind] = np.NaN + closest_front = points[np.nanargmin(dists_front)] + + # fit line: y = a*x + b + x1, y1, _ = closest_behind + x2, y2, _ = closest_front + a = (y2 - y1) / max((x2 - x1), 1e-5) + b = y1 - a * x1 + + # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error + # (A 20 degree heading offset results in an a of about 1/3) + score = abs(a) * 60. + abs(b) + + # Prefer same type of road + if prev_way is not None: + if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): + score *= 0.5 + + if closest_way is None or score < best_score: + closest_way = way + best_score = score + + # Normal score is < 5 + if best_score > 50: + return None + + return closest_way + + def __str__(self): + return "%s %s" % (self.id, self.way.tags) + + def max_speed(self): + """Extracts the (conditional) speed limit from a way""" + if not self.way: + return None + + max_speed = parse_speed_tags(self.way.tags) + if not max_speed: + location_info = self.query_results[4] + max_speed = geocode_maxspeed(self.way.tags, location_info) + + return max_speed + + def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): + """Look ahead for a max speed""" + if not self.way: + return None + + speed_ahead = None + speed_ahead_dist = None + lookahead_ways = 3 + way = self + for i in range(lookahead_ways): + way_pts = way.points_in_car_frame(lat, lon, heading) + #print way_pts + # Check current lookahead distance + if way_pts[0,0] < 0: + max_dist = np.linalg.norm(way_pts[-1, :]) + elif way_pts[-1,0] < 0: + max_dist = np.linalg.norm(way_pts[0, :]) + else: + max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + + + if max_dist > 2 * lookahead: + #print "max_dist break" + break + try: + if way.way.tags['junction']=='roundabout': + latmin = 181 + lonmin = 181 + latmax = -181 + lonmax = -181 + for n in way.way.nodes: + lonmax = max(n.lon,lonmax) + lonmin = min(n.lon,lonmin) + latmax = max(n.lat,latmax) + latmin = min(n.lat,latmin) + a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin) + speed_ahead = np.sqrt(1.6075*a) + min_dist = 999.9 + for w in way_pts: + min_dist = min(min_dist, float(np.linalg.norm(w))) + speed_ahead_dist = min_dist + break + except KeyError: + pass + if 'maxspeed' in way.way.tags: + spd = parse_speed_tags(way.way.tags) + #print "spd found" + #print spd + if not spd: + location_info = self.query_results[4] + spd = geocode_maxspeed(way.way.tags, location_info) + #print "spd is actually" + #print spd + if spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + #print "slower speed found" + #print min_dist + + break + # Find next way + way = way.next_way(heading) + if not way: + #print "no way break" + break + + return speed_ahead, speed_ahead_dist + + def advisory_max_speed(self): + if not self.way: + return None + + tags = self.way.tags + adv_speed = None + + if 'maxspeed:advisory' in tags: + adv_speed = tags['maxspeed:advisory'] + adv_speed = parse_speed_unit(adv_speed) + return adv_speed + + def on_way(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + x = points[:, 0] + return np.min(x) < 0. and np.max(x) > 0. + + def closest_point(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + i = np.argmin(np.linalg.norm(points, axis=1)) + return points[i] + + def distance_to_closest_node(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + return np.min(np.linalg.norm(points, axis=1)) + + def points_in_car_frame(self, lat, lon, heading): + lc = LocalCoord.from_geodetic([lat, lon, 0.]) + + # Build rotation matrix + heading = math.radians(-heading + 90) + c, s = np.cos(heading), np.sin(heading) + rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]]) + + # Convert to local coordinates + points_carframe = lc.geodetic2ned(self.points).T + + # Rotate with heading of car + points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T + + return points_carframe + + def next_way(self, heading): + results, tree, real_nodes, node_to_way, location_info = self.query_results + #print "way.id" + #print self.id + #print "node0.id" + #print self.way.nodes[0].id + #print "node-1.id" + #print self.way.nodes[-1].id + #print "heading" + #print heading + angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180 + #print "angle before" + #print angle + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + #print "angle" + #print angle + backwards = abs(angle) > 90 + #print "backwards" + #print backwards + if backwards: + node = self.way.nodes[0] + else: + node = self.way.nodes[-1] + + ways = node_to_way[node.id] + + way = None + try: + # Simple heuristic to find next way + ways = [w for w in ways if w.id != self.id] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + ways = [w for w in ways if (w.nodes[0] == node or w.nodes[-1] == node)] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + if len(ways) == 2: + try: + if ways[0].tags['junction']=='roundabout': + #print ("roundabout found") + way = Way(ways[0], self.query_results) + return way + except KeyError: + pass + # Filter on highway tag + acceptable_tags = list() + cur_tag = self.way.tags['highway'] + acceptable_tags.append(cur_tag) + if cur_tag == 'motorway_link': + acceptable_tags.append('motorway') + acceptable_tags.append('trunk') + acceptable_tags.append('primary') + ways = [w for w in ways if w.tags['highway'] in acceptable_tags] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + # Filter on number of lanes + cur_num_lanes = int(self.way.tags['lanes']) + if len(ways) > 1: + ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] + if len(ways_same_lanes) == 1: + ways = ways_same_lanes + if len(ways) > 1: + ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + + except (KeyError, ValueError): + pass + + return way + + def get_lookahead(self, lat, lon, heading, lookahead): + pnts = None + way = self + valid = False + + for i in range(5): + # Get new points and append to list + new_pnts = way.points_in_car_frame(lat, lon, heading) + + if pnts is None: + pnts = new_pnts + else: + pnts = np.vstack([pnts, new_pnts]) + + # Check current lookahead distance + max_dist = np.linalg.norm(pnts[-1, :]) + if max_dist > lookahead: + valid = True + + if max_dist > 2 * lookahead: + break + + # Find next way + way = way.next_way(heading) + if not way: + break + + return pnts, valid From e0d6c32c8b3aca9fa22ca671089247561480c2ad Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 12:33:23 +1000 Subject: [PATCH 09/24] no mapd just yet --- RELEASES.md | 2 +- selfdrive/manager.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index cacdf51aaccbc8..99bb0a0c5dc344 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,7 +1,7 @@ Version 0.6.0.3-ku7 (2019-07-**) ======================== * HKG - Send fingerprints to Sentry -* HKG - Bring back mapd from 0.5.12 +* HKG - Start to bring back mapd from 0.5.12 * HKG - Auto Set Cruise to Speed Limit * HKG - Don't Learn Min Steer Speed when turning corners * HKG - Create MDPS12 Message (to reduce/stop Stock cam faults) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index d9905a041cde11..bfa38dc72b491b 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -100,7 +100,7 @@ def unblock_stdout(): "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), - "mapd": "selfdrive.mapd.mapd", + # "mapd": "selfdrive.mapd.mapd", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", @@ -152,7 +152,7 @@ def get_running(): 'proclogd', 'ubloxd', 'gpsd', - 'mapd', + # 'mapd', 'deleter', ] From 3a43f17191205e2599e94462214088d674d28973 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 12:41:30 +1000 Subject: [PATCH 10/24] Remote ASL --- RELEASES.md | 4 ++-- selfdrive/car/hyundai/carcontroller.py | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 99bb0a0c5dc344..8601c2e585f121 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,8 +1,8 @@ Version 0.6.0.3-ku7 (2019-07-**) ======================== * HKG - Send fingerprints to Sentry -* HKG - Start to bring back mapd from 0.5.12 -* HKG - Auto Set Cruise to Speed Limit +* HKG - WIP: bring back mapd from 0.5.12 +* HKG - WIP: Auto Set Cruise to Speed Limit * HKG - Don't Learn Min Steer Speed when turning corners * HKG - Create MDPS12 Message (to reduce/stop Stock cam faults) * HKG - Implement Min Speed Speed Warning diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 000d6d7f3dace7..e70bd0fcbf412d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -34,9 +34,9 @@ def __init__(self, dbc_name, car_fingerprint): self.last_resume_cnt = 0 self.map_speed = 0 - context = zmq.Context() - self.map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True) - self.params = Params() + #context = zmq.Context() + #self.map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True) + #self.params = Params() self.speed_conv = 3.6 self.speed_offset = 1.03 # Multiplier for cruise speed vs speed limit TODO: Add to UI self.speed_enable = True # Enable Auto Speed Set TODO: Add to UI @@ -118,7 +118,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.apply_steer_last = apply_steer - + ''' ### Auto Speed Limit ### # Read Speed Limit and define if adjustment needed @@ -158,7 +158,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if CS.pedal_gas: self.speed_adjusted = True - + ''' ### Generate CAN Messages ### From 12e265da05b5b4c99e45b16875adfb6ce3566382 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 13:19:27 +1000 Subject: [PATCH 11/24] Done --- RELEASES.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/RELEASES.md b/RELEASES.md index 8601c2e585f121..da84c9789aed3d 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,4 +1,4 @@ -Version 0.6.0.3-ku7 (2019-07-**) +Version 0.6.0.3-ku7 (2019-07-09) ======================== * HKG - Send fingerprints to Sentry * HKG - WIP: bring back mapd from 0.5.12 From c8225257d7053e33f4c2e51d34230dc9b84cfa8e Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 13:20:49 +1000 Subject: [PATCH 12/24] panda too --- panda/board/safety/safety_hyundai.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 55a9a421279128..c9e1f88c076276 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -135,7 +135,7 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = to_fwd->RIR>>21; if (addr == 832 && bus_num == hyundai_camera_bus) return -1; - // if (addr == 593 && bus_num == 0) return -1; + if (addr == 593 && bus_num == 0) return -1; if (bus_num == 0) return (uint8_t)(hyundai_camera_bus); if (bus_num == hyundai_camera_bus) return (uint8_t)(0); } From f06d5c40de280817a94e78120aa48e38f532d3d6 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 17:35:21 +1000 Subject: [PATCH 13/24] mapd, auto speed and cloudlog --- RELEASES.md | 7 +++++++ selfdrive/car/hyundai/carcontroller.py | 23 ++++++++++++----------- selfdrive/common/version.h | 2 +- selfdrive/mapd/mapd.py | 7 +++---- 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 49479b568a1082..664d8401572578 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,10 @@ +Version 0.6.0.4-ku7 (2019-07-**) +======================== +* HKG - Log to cloudlog rather than print to stdout +* HKG - Finish bringing back mapd +* HKG - Finish Auto Cruise Speed Limit + + Version 0.6.0.3-ku7 (2019-07-09) ======================== * HKG - Send fingerprints to Sentry diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b90be1525cfe49..644c0129d9725f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -9,6 +9,8 @@ import selfdrive.messaging as messaging from selfdrive.config import Conversions as CV from common.params import Params +from selfdrive.swaglog import cloudlog + # Steer torque limits @@ -34,9 +36,8 @@ def __init__(self, dbc_name, car_fingerprint): self.last_resume_cnt = 0 self.map_speed = 0 - #context = zmq.Context() - #self.map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True) - #self.params = Params() + self.map_data_sock = messaging.sub_sock(service_list['liveMapData'].port) + self.params = Params() self.speed_conv = 3.6 self.speed_offset = 1.03 # Multiplier for cruise speed vs speed limit TODO: Add to UI self.speed_enable = True # Enable Auto Speed Set TODO: Add to UI @@ -61,7 +62,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # Learn Checksum from the Camera if self.checksum == "NONE": self.checksum = learn_checksum(self.packer, CS.lkas11) - print ("Discovered Checksum", self.checksum) + cloudlog.info("Discovered Checksum", self.checksum) if self.checksum == "NONE" and self.checksum_learn_cnt < 50: self.checksum_learn_cnt += 1 return @@ -69,17 +70,17 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # If MDPS is faulted from bad checksum, then cycle through all Checksums until 1 works if CS.steer_error == 1: self.camera_disconnected = True - print ("Camera Not Detected: Brute Forcing Checksums") + cloudlog.warning("Camera Not Detected: Brute Forcing Checksums") if self.checksum_learn_cnt > 250: self.checksum_learn_cnt = 50 if self.checksum == "NONE": - print ("Testing 6B Checksum") + cloudlog.info("Testing 6B Checksum") self.checksum == "6B" elif self.checksum == "6B": - print ("Testing 7B Checksum") + cloudlog.info("Testing 7B Checksum") self.checksum == "7B" elif self.checksum == "7B": - print ("Testing CRC8 Checksum") + cloudlog.info("Testing CRC8 Checksum") self.checksum == "crc8" else: self.checksum == "NONE" @@ -92,7 +93,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if CS.mdps12_flt != 0 and CS.v_ego_raw > 0. and abs(CS.angle_steers) < 10.0 : if CS.v_ego_raw > self.min_steer_speed: self.min_steer_speed = CS.v_ego_raw + 0.1 - print ("Discovered new Min Speed as", self.min_steer_speed) + cloudlog.info("Discovered new Min Speed as", self.min_steer_speed) # Apply Usage of Minimum Steer Speed if CS.v_ego_raw < self.min_steer_speed: @@ -117,7 +118,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.apply_steer_last = apply_steer - ''' + ### Auto Speed Limit ### # Read Speed Limit and define if adjustment needed @@ -157,7 +158,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if CS.pedal_gas: self.speed_adjusted = True - ''' + ### Generate CAN Messages ### diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 219f916614ba41..1c1c76c06619f9 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.0.3-ku7" \ No newline at end of file +#define COMMA_VERSION "0.6.0.4-ku7" \ No newline at end of file diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index f20c8ba3da12bf..6dea07e06a6609 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -137,11 +137,10 @@ def save_gps_data(gps): def mapsd_thread(): global last_gps - context = zmq.Context() poller = zmq.Poller() - gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) - gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller) - map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port) + gps_sock = messaging.sub_sock(service_list['gpsLocation'].port) + gps_external_sock = messaging.sub_sock(service_list['gpsLocationExternal'].port) + map_data_sock = messaging.pub_sock(service_list['liveMapData'].port) cur_way = None curvature_valid = False From 80ee25f1538b4182240c789aade66490c4a78879 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 17:36:25 +1000 Subject: [PATCH 14/24] start mapd --- selfdrive/manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index bfa38dc72b491b..d9905a041cde11 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -100,7 +100,7 @@ def unblock_stdout(): "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), - # "mapd": "selfdrive.mapd.mapd", + "mapd": "selfdrive.mapd.mapd", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", @@ -152,7 +152,7 @@ def get_running(): 'proclogd', 'ubloxd', 'gpsd', - # 'mapd', + 'mapd', 'deleter', ] From df2b9d6472c98c8721191f6cd18be77a5f6e865b Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Thu, 11 Jul 2019 09:37:57 +1000 Subject: [PATCH 15/24] Test --- RELEASES.md | 8 +++++-- selfdrive/car/hyundai/carcontroller.py | 33 +++++++++++++++----------- selfdrive/car/hyundai/carstate.py | 3 ++- selfdrive/car/hyundai/hyundaican.py | 2 +- selfdrive/common/version.h | 2 +- selfdrive/manager.py | 4 ++-- 6 files changed, 31 insertions(+), 21 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 664d8401572578..6f73dd76dfb2f7 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,8 +1,12 @@ Version 0.6.0.4-ku7 (2019-07-**) ======================== * HKG - Log to cloudlog rather than print to stdout -* HKG - Finish bringing back mapd -* HKG - Finish Auto Cruise Speed Limit +* HKG - WIP: mapd is going to need a lot of reworking +* HKG - Forward LKAS_Icon (not really an icon) +* HKG - Disengage Properly +* HKG - Fixed Brute Force Checksum Learner +* HKG - Detect Genesis and default min speed +* HLG - Fix Min Speed Code Version 0.6.0.3-ku7 (2019-07-09) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 644c0129d9725f..95f0a4b5b0ad71 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -3,6 +3,7 @@ create_1191, create_1156, \ create_clu11, learn_checksum, create_mdps12 from selfdrive.car.hyundai.values import Buttons +from selfdrive.car.hyundai.carstate import update_min_speed from selfdrive.can.packer import CANPacker import zmq from selfdrive.services import service_list @@ -47,7 +48,6 @@ def __init__(self, dbc_name, car_fingerprint): self.checksum_learn_cnt = 0 self.turning_signal_timer = 0 - self.min_steer_speed = 0. self.camera_disconnected = False self.packer = CANPacker(dbc_name) @@ -75,28 +75,32 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.checksum_learn_cnt = 50 if self.checksum == "NONE": cloudlog.info("Testing 6B Checksum") - self.checksum == "6B" + self.checksum = "6B" elif self.checksum == "6B": cloudlog.info("Testing 7B Checksum") - self.checksum == "7B" + self.checksum = "7B" elif self.checksum == "7B": cloudlog.info("Testing CRC8 Checksum") - self.checksum == "crc8" + self.checksum = "crc8" else: - self.checksum == "NONE" + self.checksum = "NONE" else: self.checksum_learn_cnt += 1 ### Minimum Steer Speed ### # Learn Minimum Steer Speed - if CS.mdps12_flt != 0 and CS.v_ego_raw > 0. and abs(CS.angle_steers) < 10.0 : - if CS.v_ego_raw > self.min_steer_speed: - self.min_steer_speed = CS.v_ego_raw + 0.1 - cloudlog.info("Discovered new Min Speed as", self.min_steer_speed) + if CS.mdps12_flt != 0 and CS.v_ego_raw > 0. and abs(CS.angle_steers) < 10.0 and CS.lkas11_icon != 2: + if CS.v_ego_raw > CS.min_steer_speed: + update_min_speed(CS.v_ego_raw + 0.1) + cloudlog.info("Discovered new Min Speed as", CS.min_steer_speed) + # If we have LKAS_Icon == 2, then we know its 16.7m/s + elif CS.lkas11_icon == 2 and CS.min_steer_speed < 16.7: + update_min_speed(16.7) + cloudlog.info("Suspected Genesis, new Min Speed as 16.7") # Apply Usage of Minimum Steer Speed - if CS.v_ego_raw < self.min_steer_speed: + if CS.v_ego_raw < CS.min_steer_speed: disable_steer = True ### Turning Indicators ### @@ -113,12 +117,13 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if not enabled or disable_steer: apply_steer = 0 - - steer_req = 1 if enabled else 0 + steer_req = 0 + else: + steer_req = 1 self.apply_steer_last = apply_steer - + ''' ### Auto Speed Limit ### # Read Speed Limit and define if adjustment needed @@ -158,7 +163,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if CS.pedal_gas: self.speed_adjusted = True - + ''' ### Generate CAN Messages ### diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 684ef84ec116b7..a6125cba3315f3 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -161,7 +161,7 @@ def __init__(self, CP): self.has_scc = False self.min_steer_speed = 0 - def update_min_speed(speed): + def update_min_speed(self, speed): self.min_steer_speed = speed def update(self, cp, cp_cam): @@ -226,6 +226,7 @@ def update(self, cp, cp_cam): self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. if self.has_scc else False self.mdps11_strang = cp.vl["MDPS11"]["CR_Mdps_StrAng"] self.mdps11_stat = cp.vl["MDPS11"]["CF_Mdps_Stat"] + self.lkas11_icon = cp.vl["LKAS11"]["CF_Lkas_Icon"] self.mdps12_flt = cp.vl["MDPS12"]['CF_Mdps_ToiFlt'] self.user_brake = 0 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 7590e5601a452b..bdddc44f26e635 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -7,7 +7,7 @@ def make_can_msg(addr, dat, alt): def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock, checksum): values = { - "CF_Lkas_Icon": 2, + "CF_Lkas_Icon": lkas11["CF_Lkas_Icon"] if keep_stock else 3, "CF_Lkas_LdwsSysState": 3 if steer_req else 1, "CF_Lkas_SysWarning": hud_alert, "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0, diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 1c1c76c06619f9..99ba8fb5f4dad0 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.0.4-ku7" \ No newline at end of file +#define COMMA_VERSION "0.6.0.4-dev" \ No newline at end of file diff --git a/selfdrive/manager.py b/selfdrive/manager.py index d9905a041cde11..b6e697d3ae3a39 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -100,7 +100,7 @@ def unblock_stdout(): "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), - "mapd": "selfdrive.mapd.mapd", + #"mapd": "selfdrive.mapd.mapd", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", @@ -152,7 +152,7 @@ def get_running(): 'proclogd', 'ubloxd', 'gpsd', - 'mapd', + #'mapd', 'deleter', ] From 91bf0e581de944cc39ff72daf323ce51487514d1 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Thu, 11 Jul 2019 10:02:46 +1000 Subject: [PATCH 16/24] bug --- selfdrive/car/hyundai/carstate.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index a6125cba3315f3..f75daa78c9535e 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -136,7 +136,10 @@ def get_camera_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100) +min_steer_speed = 0. +def update_min_speed(self, speed): + min_steer_speed = speed class CarState(object): def __init__(self, CP): @@ -159,10 +162,6 @@ def __init__(self, CP): self.right_blinker_on = 0 self.right_blinker_flash = 0 self.has_scc = False - self.min_steer_speed = 0 - - def update_min_speed(self, speed): - self.min_steer_speed = speed def update(self, cp, cp_cam): if (cp.vl["SCC11"]['TauGapSet'] > 0): @@ -239,6 +238,8 @@ def update(self, cp, cp_cam): self.pedal_gas = cp.vl["EMS12"]['TPS'] self.car_gas = cp.vl["EMS12"]['TPS'] + self.min_steer_speed = min_steer_speed + # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with gear = cp.vl["LVR12"]["CF_Lvr_Gear"] if gear == 5: From e5bf426ef3679c293ff7a0fc8996d5383f766884 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 14:44:35 +1000 Subject: [PATCH 17/24] better code --- selfdrive/car/hyundai/carcontroller.py | 10 ---------- selfdrive/car/hyundai/carstate.py | 13 ++++++++----- 2 files changed, 8 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 95f0a4b5b0ad71..15249c43a99a0f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -89,16 +89,6 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Minimum Steer Speed ### - # Learn Minimum Steer Speed - if CS.mdps12_flt != 0 and CS.v_ego_raw > 0. and abs(CS.angle_steers) < 10.0 and CS.lkas11_icon != 2: - if CS.v_ego_raw > CS.min_steer_speed: - update_min_speed(CS.v_ego_raw + 0.1) - cloudlog.info("Discovered new Min Speed as", CS.min_steer_speed) - # If we have LKAS_Icon == 2, then we know its 16.7m/s - elif CS.lkas11_icon == 2 and CS.min_steer_speed < 16.7: - update_min_speed(16.7) - cloudlog.info("Suspected Genesis, new Min Speed as 16.7") - # Apply Usage of Minimum Steer Speed if CS.v_ego_raw < CS.min_steer_speed: disable_steer = True diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index f75daa78c9535e..95e569e444e69f 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -136,10 +136,6 @@ def get_camera_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100) -min_steer_speed = 0. - -def update_min_speed(self, speed): - min_steer_speed = speed class CarState(object): def __init__(self, CP): @@ -162,6 +158,7 @@ def __init__(self, CP): self.right_blinker_on = 0 self.right_blinker_flash = 0 self.has_scc = False + self.min_steer_speed = 0 def update(self, cp, cp_cam): if (cp.vl["SCC11"]['TauGapSet'] > 0): @@ -238,7 +235,13 @@ def update(self, cp, cp_cam): self.pedal_gas = cp.vl["EMS12"]['TPS'] self.car_gas = cp.vl["EMS12"]['TPS'] - self.min_steer_speed = min_steer_speed + # Learn Minimum Steer Speed + if self.mdps12_flt != 0 and self.v_ego_raw > 0. and abs(self.angle_steers) < 10.0 and self.lkas11_icon != 2: + if self.v_ego_raw > self.min_steer_speed: + self.min_steer_speed = self.v_ego_raw + 0.1 + # If we have LKAS_Icon == 2, then we know its 16.7m/s + elif self.lkas11_icon == 2 and self.min_steer_speed < 16.7: + self.min_steer_speed = 16.7 # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with gear = cp.vl["LVR12"]["CF_Lvr_Gear"] From 6d854c111c01bcd503794b918c9a09afdd33a734 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 14:46:21 +1000 Subject: [PATCH 18/24] bug --- selfdrive/car/hyundai/carcontroller.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 15249c43a99a0f..f1f02f59e7ea27 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -3,7 +3,6 @@ create_1191, create_1156, \ create_clu11, learn_checksum, create_mdps12 from selfdrive.car.hyundai.values import Buttons -from selfdrive.car.hyundai.carstate import update_min_speed from selfdrive.can.packer import CANPacker import zmq from selfdrive.services import service_list From 4e938e8fca9ae7e42725560c4bb487561b3a90e8 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 14:51:53 +1000 Subject: [PATCH 19/24] bug --- selfdrive/car/hyundai/carstate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 95e569e444e69f..1d74dd6f2d5575 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -220,9 +220,9 @@ def update(self, cp, cp_cam): self.steer_torque_driver = cp.vl["MDPS12"]['CR_Mdps_StrColTq'] self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq'] self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. if self.has_scc else False - self.mdps11_strang = cp.vl["MDPS11"]["CR_Mdps_StrAng"] - self.mdps11_stat = cp.vl["MDPS11"]["CF_Mdps_Stat"] - self.lkas11_icon = cp.vl["LKAS11"]["CF_Lkas_Icon"] + self.mdps11_strang = cp.vl["MDPS11"]['CR_Mdps_StrAng'] + self.mdps11_stat = cp.vl["MDPS11"]['CF_Mdps_Stat'] + self.lkas11_icon = cp_cam.vl["LKAS11"]['CF_Lkas_Icon'] self.mdps12_flt = cp.vl["MDPS12"]['CF_Mdps_ToiFlt'] self.user_brake = 0 From f99885882107385e1c3a5293f622c8e3f9e157c8 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 14:57:30 +1000 Subject: [PATCH 20/24] bug --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index f1f02f59e7ea27..6ce0930c45668c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -61,7 +61,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # Learn Checksum from the Camera if self.checksum == "NONE": self.checksum = learn_checksum(self.packer, CS.lkas11) - cloudlog.info("Discovered Checksum", self.checksum) + cloudlog.info("Discovered Checksum") if self.checksum == "NONE" and self.checksum_learn_cnt < 50: self.checksum_learn_cnt += 1 return From 2e1d9e120632f1969ff85fc5f2fef3b2de70036c Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 15:18:21 +1000 Subject: [PATCH 21/24] bug --- selfdrive/car/hyundai/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 05da0bf6aa9230..ef0dda9177c07e 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -178,9 +178,9 @@ def update(self, c): ret.seatbeltUnlatched = not self.CS.seatbelt # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if ret.vEgo < (self.CS.min_steer_speed + 2.) and self.CS.min_steer_speed > 10.: + if self.CS.v_ego_raw < (self.CS.min_steer_speed + 0.1) and self.CS.min_steer_speed > 2.: self.low_speed_alert = True - if ret.vEgo > (self.CS.min_steer_speed + 4.): + if self.CS.v_ego_raw > (self.CS.min_steer_speed + 0.2): self.low_speed_alert = False events = [] From 46551baabd95cc12cefb1555d3a64927a26e9121 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 15:24:48 +1000 Subject: [PATCH 22/24] bug --- selfdrive/car/hyundai/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ef0dda9177c07e..354af9adec3c66 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -178,7 +178,7 @@ def update(self, c): ret.seatbeltUnlatched = not self.CS.seatbelt # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if self.CS.v_ego_raw < (self.CS.min_steer_speed + 0.1) and self.CS.min_steer_speed > 2.: + if self.CS.v_ego_raw < (self.CS.min_steer_speed) and self.CS.min_steer_speed > 2.: self.low_speed_alert = True if self.CS.v_ego_raw > (self.CS.min_steer_speed + 0.2): self.low_speed_alert = False From 43b8b4a204ac8cf7cf897d5a9431439155a712a8 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 12 Jul 2019 15:31:57 +1000 Subject: [PATCH 23/24] tweak --- selfdrive/car/hyundai/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 1d74dd6f2d5575..575dbfa43c94bf 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -236,7 +236,7 @@ def update(self, cp, cp_cam): self.car_gas = cp.vl["EMS12"]['TPS'] # Learn Minimum Steer Speed - if self.mdps12_flt != 0 and self.v_ego_raw > 0. and abs(self.angle_steers) < 10.0 and self.lkas11_icon != 2: + if self.mdps12_flt != 0 and self.v_ego_raw > 0. and abs(self.angle_steers) < 5.0 and self.lkas11_icon != 2: if self.v_ego_raw > self.min_steer_speed: self.min_steer_speed = self.v_ego_raw + 0.1 # If we have LKAS_Icon == 2, then we know its 16.7m/s From 9823b5d3f610464dfa9b75670ffa98a52f175ff5 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Fri, 2 Aug 2019 08:14:17 +1000 Subject: [PATCH 24/24] Remove Min Speed Steer Disengage --- selfdrive/car/hyundai/interface.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 354af9adec3c66..89d29ae0c8d4c5 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -178,10 +178,10 @@ def update(self, c): ret.seatbeltUnlatched = not self.CS.seatbelt # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if self.CS.v_ego_raw < (self.CS.min_steer_speed) and self.CS.min_steer_speed > 2.: - self.low_speed_alert = True - if self.CS.v_ego_raw > (self.CS.min_steer_speed + 0.2): - self.low_speed_alert = False + #if self.CS.v_ego_raw < (self.CS.min_steer_speed) and self.CS.min_steer_speed > 2.: + # self.low_speed_alert = True + #if self.CS.v_ego_raw > (self.CS.min_steer_speed + 0.2): + self.low_speed_alert = False events = [] if (self.CS.gear_shifter != 'drive') and (self.CS.gear_tcu != 'drive') and (self.CS.gear_shifter_cluster != 'drive'):