From 96f8e5158e15ddfa62495dce0b1dfc0bcd3c14d3 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Tue, 30 Jan 2018 12:54:12 -0800 Subject: [PATCH] Squashed 'panda/' changes from 2573d86..b42db6d b42db6d Merge pull request #82 from commaai/uart_dma fd68f86 smallr be99ffc ok that doesn't hurt i think a9f6bf0 this 8b7e849 working now 7fa4808 froze up, maybe thats the fix 1465aa4 ok, it's fixed 915cd84 ugh, ok, need that fd05376 comment out debug 37c5263 big fifo 497f069 dma is all critical, no interrupts 7c34afe minor change 743d244 high baud rate works 5d2a4ba v1.0.6 fbf1390 Toyota Safety: fix in input param 6c01d09 Toyota: less torque error allowance to meet Corolla acceptable behavior 07c01b2 Toyota safety: using input param 4410a59 add safety param support fc81fc1 uart dma in progress 65fb2b2 grey panda query, 1.0.5 f415c9a grey panda detection b68957e add pandadebug support b5e4962 leave msgs around in isotp 0acce2d add recvaddr support 3fc38f4 set bootmode with power d4c052a make that work 21f8195 fix panda serial write af74aa9 from python import git-subtree-dir: panda git-subtree-split: b42db6dc082fb13ef5ac63ed197a63e179651ef6 --- VERSION | 2 +- board/build.mk | 4 +- board/drivers/drivers.h | 10 ++-- board/drivers/uart.h | 85 +++++++++++++++++++++++++++++----- board/gpio.h | 6 +++ board/main.c | 24 ++++++++-- board/safety.h | 6 +-- board/safety/safety_defaults.h | 4 +- board/safety/safety_elm327.h | 2 +- board/safety/safety_honda.h | 2 +- board/safety/safety_toyota.h | 13 ++++-- examples/isotp.py | 40 +++++++++------- python/__init__.py | 14 +++++- tests/location_listener.py | 1 - 14 files changed, 155 insertions(+), 58 deletions(-) diff --git a/VERSION b/VERSION index 3b9e5dbc90..6df8b11430 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.0.4 \ No newline at end of file +v1.0.6 \ No newline at end of file diff --git a/board/build.mk b/board/build.mk index 7b3e271915..069a00e1a0 100644 --- a/board/build.mk +++ b/board/build.mk @@ -17,7 +17,7 @@ DFU_UTIL = "dfu-util" # this no longer pushes the bootstub flash: obj/$(PROJ_NAME).bin - PYTHONPATH=../ python -c "from panda import Panda; Panda().flash('obj/$(PROJ_NAME).bin')" + PYTHONPATH=../ python -c "from python import Panda; Panda().flash('obj/$(PROJ_NAME).bin')" ota: obj/$(PROJ_NAME).bin curl http://192.168.0.10/stupdate --upload-file $< @@ -26,7 +26,7 @@ bin: obj/$(PROJ_NAME).bin # this flashes everything recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin - -PYTHONPATH=../ python -c "from panda import Panda; Panda().reset(enter_bootloader=True)" + -PYTHONPATH=../ python -c "from python import Panda; Panda().reset(enter_bootloader=True)" sleep 1.0 $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin diff --git a/board/drivers/drivers.h b/board/drivers/drivers.h index 871ee8fcf1..ce1e860ceb 100644 --- a/board/drivers/drivers.h +++ b/board/drivers/drivers.h @@ -57,13 +57,13 @@ void usb_cb_enumeration_complete(); // ********************* UART ********************* // IRQs: USART1, USART2, USART3, UART5 -#define FIFO_SIZE 0x100 +#define FIFO_SIZE 0x400 typedef struct uart_ring { - uint8_t w_ptr_tx; - uint8_t r_ptr_tx; + uint16_t w_ptr_tx; + uint16_t r_ptr_tx; uint8_t elems_tx[FIFO_SIZE]; - uint8_t w_ptr_rx; - uint8_t r_ptr_rx; + uint16_t w_ptr_rx; + uint16_t r_ptr_rx; uint8_t elems_rx[FIFO_SIZE]; USART_TypeDef *uart; void (*callback)(struct uart_ring*); diff --git a/board/drivers/uart.h b/board/drivers/uart.h index 2a21ffa23b..9c01416920 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -52,7 +52,7 @@ void uart_ring_process(uart_ring *q) { if (q->w_ptr_tx != q->r_ptr_tx) { if (sr & USART_SR_TXE) { q->uart->DR = q->elems_tx[q->r_ptr_tx]; - q->r_ptr_tx += 1; + q->r_ptr_tx = (q->r_ptr_tx + 1) % FIFO_SIZE; } else { // push on interrupt later q->uart->CR1 |= USART_CR1_TXEIE; @@ -64,11 +64,13 @@ void uart_ring_process(uart_ring *q) { if (sr & USART_SR_RXNE || sr & USART_SR_ORE) { uint8_t c = q->uart->DR; // TODO: can drop packets - uint8_t next_w_ptr = q->w_ptr_rx + 1; - 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) q->callback(q); + 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) q->callback(q); + } } } @@ -92,7 +94,7 @@ int getc(uart_ring *q, char *elem) { enter_critical_section(); if (q->w_ptr_rx != q->r_ptr_rx) { *elem = q->elems_rx[q->r_ptr_rx]; - q->r_ptr_rx += 1; + q->r_ptr_rx = (q->r_ptr_rx + 1) % FIFO_SIZE; ret = 1; } exit_critical_section(); @@ -102,10 +104,10 @@ int getc(uart_ring *q, char *elem) { int injectc(uart_ring *q, char elem) { int ret = 0; - uint8_t next_w_ptr; + uint16_t next_w_ptr; enter_critical_section(); - next_w_ptr = q->w_ptr_rx + 1; + 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] = elem; q->w_ptr_rx = next_w_ptr; @@ -118,10 +120,10 @@ int injectc(uart_ring *q, char elem) { int putc(uart_ring *q, char elem) { int ret = 0; - uint8_t next_w_ptr; + uint16_t next_w_ptr; enter_critical_section(); - next_w_ptr = q->w_ptr_tx + 1; + next_w_ptr = (q->w_ptr_tx + 1) % FIFO_SIZE; if (next_w_ptr != q->r_ptr_tx) { q->elems_tx[q->w_ptr_tx] = elem; q->w_ptr_tx = next_w_ptr; @@ -159,6 +161,51 @@ void uart_set_baud(USART_TypeDef *u, int baud) { } } +#define USART1_DMA_LEN 0x20 +char usart1_dma[USART1_DMA_LEN]; + +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) { + // disable DMA + q->uart->CR3 &= ~USART_CR3_DMAR; + DMA2_Stream5->CR &= ~DMA_SxCR_EN; + while (DMA2_Stream5->CR & DMA_SxCR_EN); + + 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) { + q->elems_rx[q->w_ptr_rx] = c; + q->w_ptr_rx = next_w_ptr; + } + } + + // reset DMA len + DMA2_Stream5->NDTR = USART1_DMA_LEN; + + // clear interrupts + DMA2->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5; + //DMA2->HIFCR = DMA_HIFCR_CTEIF5 | DMA_HIFCR_CDMEIF5 | DMA_HIFCR_CFEIF5; + + // enable DMA + DMA2_Stream5->CR |= DMA_SxCR_EN; + q->uart->CR3 |= USART_CR3_DMAR; + } + + exit_critical_section(); +} + +void DMA2_Stream5_IRQHandler(void) { + //set_led(LED_BLUE, 1); + uart_dma_drain(); + //set_led(LED_BLUE, 0); +} + void uart_init(USART_TypeDef *u, int baud) { // enable uart and tx+rx mode u->CR1 = USART_CR1_UE; @@ -170,9 +217,23 @@ void uart_init(USART_TypeDef *u, int baud) { // ** UART is ready to work ** // enable interrupts - u->CR1 |= USART_CR1_RXNEIE; + if (u != USART1) { + u->CR1 |= USART_CR1_RXNEIE; + } if (u == USART1) { + // DMA2, stream 2, channel 3 + DMA2_Stream5->M0AR = (uint32_t)usart1_dma; + DMA2_Stream5->NDTR = USART1_DMA_LEN; + DMA2_Stream5->PAR = (uint32_t)&(USART1->DR); + + // channel4, increment memory, periph -> memory, enable + DMA2_Stream5->CR = DMA_SxCR_CHSEL_2 | DMA_SxCR_MINC | DMA_SxCR_HTIE | DMA_SxCR_TCIE | DMA_SxCR_EN; + + // this one uses DMA receiver + u->CR3 = USART_CR3_DMAR; + + NVIC_EnableIRQ(DMA2_Stream5_IRQn); NVIC_EnableIRQ(USART1_IRQn); } else if (u == USART2) { NVIC_EnableIRQ(USART2_IRQn); diff --git a/board/gpio.h b/board/gpio.h index 0e15e3ba45..775a88df54 100644 --- a/board/gpio.h +++ b/board/gpio.h @@ -15,6 +15,7 @@ int has_external_debug_serial = 0; int is_giant_panda = 0; int is_entering_bootmode = 0; int revision = PANDA_REV_AB; +int is_grey_panda = 0; int detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) { set_gpio_mode(GPIO, pin, MODE_INPUT); @@ -45,9 +46,14 @@ void detect() { // check if the ESP is trying to put me in boot mode is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP); + + // check if it's a grey panda by seeing if the SPI lines are floating + // TODO: is this reliable? + is_grey_panda = !(detect_with_pull(GPIOA, 4, PULL_DOWN) | detect_with_pull(GPIOA, 5, PULL_DOWN) | detect_with_pull(GPIOA, 6, PULL_DOWN) | detect_with_pull(GPIOA, 7, PULL_DOWN)); #else // need to do this for early detect is_giant_panda = 0; + is_grey_panda = 0; revision = PANDA_REV_AB; is_entering_bootmode = 0; #endif diff --git a/board/main.c b/board/main.c index 402fd73e2b..e46a388563 100644 --- a/board/main.c +++ b/board/main.c @@ -175,6 +175,11 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { puts(" err: "); puth(can_err_cnt); puts("\n"); break; + // **** 0xc1: is grey panda + case 0xc1: + resp[0] = is_grey_panda; + resp_len = 1; + break; // **** 0xd0: fetch serial number case 0xd0: #ifdef PANDA @@ -229,6 +234,8 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { case 0xd9: if (setup->b.wValue.w == 1) { set_esp_mode(ESP_ENABLED); + } else if (setup->b.wValue.w == 2) { + set_esp_mode(ESP_BOOTMODE); } else { set_esp_mode(ESP_DISABLED); } @@ -267,7 +274,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { // 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) { - safety_set_mode(setup->b.wValue.w); + safety_set_mode(setup->b.wValue.w, (int16_t)setup->b.wIndex.w); switch (setup->b.wValue.w) { case SAFETY_NOOUTPUT: can_silent = ALL_CAN_SILENT; @@ -304,6 +311,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { case 0xe0: ur = get_ring_by_number(setup->b.wValue.w); if (!ur) break; + if (ur == &esp_ring) uart_dma_drain(); // read while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) && getc(ur, (char*)&resp[resp_len])) { @@ -489,6 +497,7 @@ int main() { #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"); gpio_init(); @@ -500,9 +509,12 @@ int main() { } #ifdef PANDA - // enable ESP uart - uart_init(USART1, 115200); - + 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; @@ -522,7 +534,7 @@ int main() { usb_init(); // default to silent mode to prevent issues with Ford - safety_set_mode(SAFETY_NOOUTPUT); + safety_set_mode(SAFETY_NOOUTPUT, 0); can_silent = ALL_CAN_SILENT; can_init_all(); @@ -552,6 +564,8 @@ int main() { for (cnt=0;;cnt++) { 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); diff --git a/board/safety.h b/board/safety.h index dc4631f460..8842141544 100644 --- a/board/safety.h +++ b/board/safety.h @@ -2,7 +2,7 @@ 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); -typedef void (*safety_hook_init)(); +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); @@ -60,11 +60,11 @@ const safety_hook_config safety_hook_registry[] = { #define HOOK_CONFIG_COUNT (sizeof(safety_hook_registry)/sizeof(safety_hook_config)) -int safety_set_mode(uint16_t mode) { +int safety_set_mode(uint16_t mode, int16_t param) { for (int i = 0; i < HOOK_CONFIG_COUNT; i++) { if (safety_hook_registry[i].id == mode) { current_hooks = safety_hook_registry[i].hooks; - if (current_hooks->init) current_hooks->init(); + if (current_hooks->init) current_hooks->init(param); return 0; } } diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index 86155f32b9..b7b4d37295 100644 --- a/board/safety/safety_defaults.h +++ b/board/safety/safety_defaults.h @@ -2,7 +2,7 @@ void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} // *** no output safety mode *** -static void nooutput_init() { +static void nooutput_init(int16_t param) { controls_allowed = 0; } @@ -23,7 +23,7 @@ const safety_hooks nooutput_hooks = { // *** all output safety mode *** -static void alloutput_init() { +static void alloutput_init(int16_t param) { controls_allowed = 1; } diff --git a/board/safety/safety_elm327.h b/board/safety/safety_elm327.h index ddacc27be0..b9af35ebdc 100644 --- a/board/safety/safety_elm327.h +++ b/board/safety/safety_elm327.h @@ -27,7 +27,7 @@ static int elm327_tx_lin_hook(int lin_num, uint8_t *data, int len) { return true; } -static void elm327_init() { +static void elm327_init(int16_t param) { controls_allowed = 1; } diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 55ef003e5f..bf30d84707 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -115,7 +115,7 @@ static int honda_tx_lin_hook(int lin_num, uint8_t *data, int len) { return true; } -static void honda_init() { +static void honda_init(int16_t param) { controls_allowed = 0; } diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 0dab4b91cb..9ddaf172bb 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -9,7 +9,7 @@ const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever // packet is sent at 100hz, so this limit is 1000/sec const int32_t MAX_RATE_UP = 10; // ramp up slow const int32_t MAX_RATE_DOWN = 25; // ramp down fast -const int32_t MAX_TORQUE_ERROR = 500; // max torque cmd in excess of torque motor +const int32_t MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor // real time torque limit to prevent controls spamming // the real time limit is 1500/sec @@ -22,6 +22,7 @@ const int16_t MIN_ACCEL = -3000; // 3.0 m/s2 // global actuation limit state int actuation_limits = 1; // by default steer limits are imposed +int16_t dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file // state of torque limits int16_t desired_torque_last = 0; // last desired steer torque @@ -31,10 +32,10 @@ uint32_t ts_last = 0; static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // get eps motor torque (0.66 factor in dbc) if ((to_push->RIR>>21) == 0x260) { - int16_t torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF)); + int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF)); // increase torque_meas by 1 to be conservative on rounding - torque_meas_new = (torque_meas_new / 3 + (torque_meas_new > 0 ? 1 : -1)) * 2; + torque_meas_new = (torque_meas_new * dbc_eps_torque_factor / 100) + (torque_meas_new > 0 ? 1 : -1); // shift the array for (int i = sizeof(torque_meas)/sizeof(torque_meas[0]) - 1; i > 0; i--) { @@ -154,9 +155,10 @@ static int toyota_tx_lin_hook(int lin_num, uint8_t *data, int len) { return true; } -static void toyota_init() { +static void toyota_init(int16_t param) { controls_allowed = 0; actuation_limits = 1; + dbc_eps_torque_factor = param; } const safety_hooks toyota_hooks = { @@ -166,9 +168,10 @@ const safety_hooks toyota_hooks = { .tx_lin = toyota_tx_lin_hook, }; -static void toyota_nolimits_init() { +static void toyota_nolimits_init(int16_t param) { controls_allowed = 0; actuation_limits = 0; + dbc_eps_torque_factor = param; } const safety_hooks toyota_nolimits_hooks = { diff --git a/examples/isotp.py b/examples/isotp.py index 12cef80a96..fd300da751 100644 --- a/examples/isotp.py +++ b/examples/isotp.py @@ -9,7 +9,27 @@ def msg(x): assert False return ret.ljust(8, "\x00") -def isotp_send(panda, x, addr, bus=0): +kmsgs = [] +def recv(panda, cnt, addr, nbus): + global kmsgs + ret = [] + + while len(ret) < cnt: + kmsgs += panda.can_recv() + nmsgs = [] + for ids, ts, dat, bus in kmsgs: + if ids == addr and bus == nbus and len(ret) < cnt: + ret.append(dat) + else: + # leave around + nmsgs.append((ids, ts, dat, bus)) + kmsgs = nmsgs[-256:] + return map(str, ret) + +def isotp_send(panda, x, addr, bus=0, recvaddr=None): + if recvaddr is None: + recvaddr = addr+8 + if len(x) <= 7: panda.can_send(addr, msg(x), bus) else: @@ -24,25 +44,9 @@ def isotp_send(panda, x, addr, bus=0): # actually send panda.can_send(addr, ss, bus) - rr = recv(panda, 1, addr+8, bus)[0] + rr = recv(panda, 1, recvaddr, bus)[0] panda.can_send_many([(addr, None, s, 0) for s in sends]) -kmsgs = [] -def recv(panda, cnt, addr, nbus): - global kmsgs - ret = [] - - while len(ret) < cnt: - kmsgs += panda.can_recv() - nmsgs = [] - for ids, ts, dat, bus in kmsgs: - if ids == addr and bus == nbus and len(ret) < cnt: - ret.append(dat) - else: - pass - kmsgs = nmsgs - return map(str, ret) - def isotp_recv(panda, addr, bus=0, sendaddr=None): msg = recv(panda, 1, addr, bus)[0] diff --git a/python/__init__.py b/python/__init__.py index 48f6bf17a6..011263fff9 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -18,6 +18,8 @@ __version__ = '0.0.6' BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") +DEBUG = os.getenv("PANDADEBUG") is not None + # *** wifi mode *** def build_st(target, mkfile="Makefile"): @@ -34,7 +36,10 @@ def parse_can_buffer(dat): address = f1 >> 3 else: address = f1 >> 21 - ret.append((address, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&0xFF)) + dddat = ddat[8:8+(f2&0xF)] + if DEBUG: + print(" R %x: %s" % (address, str(dddat).encode("hex"))) + ret.append((address, f2>>16, dddat, (f2>>4)&0xFF)) return ret class PandaWifiStreaming(object): @@ -369,6 +374,8 @@ class Panda(object): extended = 4 for addr, _, dat, bus in arr: assert len(dat) <= 8 + if DEBUG: + print(" W %x: %s" % (addr, dat.encode("hex"))) if addr >= 0x800: rir = (addr << 3) | transmit | extended else: @@ -425,7 +432,10 @@ class Panda(object): return b''.join(ret) def serial_write(self, port_number, ln): - return self._handle.bulkWrite(2, struct.pack("B", port_number) + ln) + ret = 0 + for i in range(0, len(ln), 0x20): + ret += self._handle.bulkWrite(2, struct.pack("B", port_number) + ln[i:i+0x20]) + return ret def serial_clear(self, port_number): """Clears all messages (tx and rx) from the specified internal uart diff --git a/tests/location_listener.py b/tests/location_listener.py index 59f83cf59d..cbbb00d794 100755 --- a/tests/location_listener.py +++ b/tests/location_listener.py @@ -27,7 +27,6 @@ if __name__ == "__main__": print ser.read(1024) # upping baud rate - # 460800 has issues baudrate = 460800 print "upping baud rate"