From b78368d8374110bf5b45ae737a1fb337a9b5fd4e Mon Sep 17 00:00:00 2001 From: dgey Date: Mon, 14 Nov 2022 07:31:28 +0100 Subject: [PATCH] Updated to Espressif TWAI lib to support newer ESP32 revisions --- CHANGELOG.md | 1 + lib/ESP32-Arduino-CAN/.gitignore | 6 - lib/ESP32-Arduino-CAN/LICENSE | 21 -- lib/ESP32-Arduino-CAN/README.md | 24 -- lib/ESP32-Arduino-CAN/keywords.txt | 20 -- lib/ESP32-Arduino-CAN/library.properties | 9 - lib/ESP32-Arduino-CAN/src/CAN.h | 131 -------- lib/ESP32-Arduino-CAN/src/CAN_config.h | 71 ----- lib/ESP32-Arduino-CAN/src/ESP32CAN.cpp | 20 -- lib/ESP32-Arduino-CAN/src/ESP32CAN.h | 17 -- lib/ESP32-Arduino-CAN/src/VescCanConstants.c | 299 ------------------- lib/ESP32-Arduino-CAN/src/can_regdef.h | 279 ----------------- platformio.ini | 4 +- src/BleCanProxy.cpp | 62 ++-- src/BleCanProxy.h | 2 - src/BleServer.cpp | 2 +- src/BleServer.h | 2 +- src/CanBus.cpp | 151 +++++----- src/CanBus.h | 13 +- src/CanDevice.cpp | 69 +++-- src/CanDevice.h | 14 +- src/config.h | 4 +- 22 files changed, 159 insertions(+), 1062 deletions(-) delete mode 100644 lib/ESP32-Arduino-CAN/.gitignore delete mode 100644 lib/ESP32-Arduino-CAN/LICENSE delete mode 100644 lib/ESP32-Arduino-CAN/README.md delete mode 100644 lib/ESP32-Arduino-CAN/keywords.txt delete mode 100644 lib/ESP32-Arduino-CAN/library.properties delete mode 100644 lib/ESP32-Arduino-CAN/src/CAN.h delete mode 100644 lib/ESP32-Arduino-CAN/src/CAN_config.h delete mode 100644 lib/ESP32-Arduino-CAN/src/ESP32CAN.cpp delete mode 100644 lib/ESP32-Arduino-CAN/src/ESP32CAN.h delete mode 100644 lib/ESP32-Arduino-CAN/src/VescCanConstants.c delete mode 100644 lib/ESP32-Arduino-CAN/src/can_regdef.h diff --git a/CHANGELOG.md b/CHANGELOG.md index 98c2c51..7dd66d5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,5 @@ # Changelog for rESCue firmware +rESCue now uses Github releases, so you can see the changes in Github : https://github.com/thankthemaker/rESCue/releases ## Release 1.3.8 (not yet released) diff --git a/lib/ESP32-Arduino-CAN/.gitignore b/lib/ESP32-Arduino-CAN/.gitignore deleted file mode 100644 index 1e954bb..0000000 --- a/lib/ESP32-Arduino-CAN/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -.pioenvs -.piolibdeps -.vscode/**/* -.travis.yml -platformio.ini -lib/readme.txt \ No newline at end of file diff --git a/lib/ESP32-Arduino-CAN/LICENSE b/lib/ESP32-Arduino-CAN/LICENSE deleted file mode 100644 index 44a1223..0000000 --- a/lib/ESP32-Arduino-CAN/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2018 Michael Wagner - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/lib/ESP32-Arduino-CAN/README.md b/lib/ESP32-Arduino-CAN/README.md deleted file mode 100644 index bcfe3e5..0000000 --- a/lib/ESP32-Arduino-CAN/README.md +++ /dev/null @@ -1,24 +0,0 @@ -# Arduino Library for the ESP32 CAN Bus (ESP32-Arduino-CAN) - -## Features - -* Support the CAN Bus from the ESP32 (SJA1000) -* CAN Messages send and receive -* Various Bus speeds -* Standard and Extended Frames -* CAN Message Filter - - - -## Third Party Components - -- Arduino-ESP32-CAN-Demo - - Arduino CAN Demo from [iotsharing.com - nhatuan84](https://github.com/nhatuan84/arduino-esp32-can-demo) - -- ESPCan Driver - - Base CAN Driver from [Thomas Barth](https://github.com/ThomasBarth/ESP32-CAN-Driver) and [Nayar Systems](https://github.com/nayarsystems/ESP32-CAN-Driver) - - General [Component CAN Driver Pack](https://github.com/ESP32DE/ESP32-CAN-Driver/tree/Component_CAN_Driver_Pack) Work for ESP-IDF with menuconfig from [rudi ;-)](http://esp32.de) - -## Usage - -See the examples in the [/examples](examples) folder. \ No newline at end of file diff --git a/lib/ESP32-Arduino-CAN/keywords.txt b/lib/ESP32-Arduino-CAN/keywords.txt deleted file mode 100644 index 3bd5cb9..0000000 --- a/lib/ESP32-Arduino-CAN/keywords.txt +++ /dev/null @@ -1,20 +0,0 @@ -####################################### -# Syntax Coloring Map ESP32CAN -####################################### - -####################################### -# Datatypes (KEYWORD1) -####################################### - -ESP32CAN KEYWORD1 - -####################################### -# Methods and Functions (KEYWORD2) -####################################### -CANInit KEYWORD2 -CANWriteFrame KEYWORD2 -CANStop KEYWORD2 -CANConfigFilter KEYWORD2 -####################################### -# Constants (LITERAL1) -####################################### diff --git a/lib/ESP32-Arduino-CAN/library.properties b/lib/ESP32-Arduino-CAN/library.properties deleted file mode 100644 index 9e0b21b..0000000 --- a/lib/ESP32-Arduino-CAN/library.properties +++ /dev/null @@ -1,9 +0,0 @@ -name=ESP32CAN -version=0.0.1 -author=Michael Wagner -maintainer=https://github.com/miwagner -sentence=ESP32-Arduino-CAN -paragraph=ESP32-Arduino-CAN -category=Device Control -url=https://github.com/miwagner/ESP32-Arduino-CAN -architectures=esp32 diff --git a/lib/ESP32-Arduino-CAN/src/CAN.h b/lib/ESP32-Arduino-CAN/src/CAN.h deleted file mode 100644 index aebde46..0000000 --- a/lib/ESP32-Arduino-CAN/src/CAN.h +++ /dev/null @@ -1,131 +0,0 @@ -/** - * @section License - * - * The MIT License (MIT) - * - * Copyright (c) 2017, Thomas Barth, barth-dev.de - * 2018, Michael Wagner, mw@iot-make.de - * Permission is hereby granted, free of charge, to any person - * obtaining a copy of this software and associated documentation - * files (the "Software"), to deal in the Software without - * restriction, including without limitation the rights to use, copy, - * modify, merge, publish, distribute, sublicense, and/or sell copies - * of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be - * included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, - * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS - * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN - * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef __DRIVERS_CAN_H__ -#define __DRIVERS_CAN_H__ - -#include -#include "CAN_config.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * \brief CAN frame type (standard/extended) - */ -typedef enum { - CAN_frame_std = 0, /**< Standard frame, using 11 bit identifer. */ - CAN_frame_ext = 1 /**< Extended frame, using 29 bit identifer. */ -} CAN_frame_format_t; - -/** - * \brief CAN RTR - */ -typedef enum { - CAN_no_RTR = 0, /**< No RTR frame. */ - CAN_RTR = 1 /**< RTR frame. */ -} CAN_RTR_t; - -/** \brief Frame information record type */ -typedef union { - uint32_t U; /**< \brief Unsigned access */ - struct { - uint8_t DLC : 4; /**< \brief [3:0] DLC, Data length container */ - unsigned int unknown_2 : 2; /**< \brief \internal unknown */ - CAN_RTR_t RTR : 1; /**< \brief [6:6] RTR, Remote Transmission Request */ - CAN_frame_format_t FF : 1; /**< \brief [7:7] Frame Format, see# CAN_frame_format_t*/ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; -} CAN_FIR_t; - -/** \brief CAN Frame structure */ -typedef struct { - CAN_FIR_t FIR; /**< \brief Frame information record*/ - uint32_t MsgID; /**< \brief Message ID */ - union { - uint8_t u8[8]; /**< \brief Payload byte access*/ - uint32_t u32[2]; /**< \brief Payload u32 access*/ - uint64_t u64; /**< \brief Payload u64 access*/ - } data; -} CAN_frame_t; - -typedef enum { - Dual_Mode=0, /**< \brief The dual acceptance filter option is enabled (two filters, each with the length of 16 bit are active) */ - Single_Mode=1 /**< \brief The single acceptance filter option is enabled (one filter with the length of 32 bit is active) */ -} CAN_filter_mode_t; - -/** \brief CAN Filter structure */ -typedef struct { - CAN_filter_mode_t FM:1; /**< \brief [0:0] Filter Mode */ - uint8_t ACR0; /**< \brief Acceptance Code Register ACR0 */ - uint8_t ACR1; /**< \brief Acceptance Code Register ACR1 */ - uint8_t ACR2; /**< \brief Acceptance Code Register ACR2 */ - uint8_t ACR3; /**< \brief Acceptance Code Register ACR3 */ - uint8_t AMR0; /**< \brief Acceptance Mask Register AMR0 */ - uint8_t AMR1; /**< \brief Acceptance Mask Register AMR1 */ - uint8_t AMR2; /**< \brief Acceptance Mask Register AMR2 */ - uint8_t AMR3; /**< \brief Acceptance Mask Register AMR3 */ -} CAN_filter_t; - -/** - * \brief Initialize the CAN Module - * - * \return 0 CAN Module had been initialized - */ -int CAN_init(void); - -/** - * \brief Send a can frame - * - * \param p_frame Pointer to the frame to be send, see #CAN_frame_t - * \return 0 Frame has been written to the module - */ -int CAN_write_frame(const CAN_frame_t *p_frame); - -/** - * \brief Stops the CAN Module - * - * \return 0 CAN Module was stopped - */ -int CAN_stop(void); - -/** - * \brief Config CAN Filter, must call before CANInit() - * - * \param p_filter Pointer to the filter, see #CAN_filter_t - * \return 0 CAN Filter had been initialized - */ -int CAN_config_filter(const CAN_filter_t* p_filter); - - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/lib/ESP32-Arduino-CAN/src/CAN_config.h b/lib/ESP32-Arduino-CAN/src/CAN_config.h deleted file mode 100644 index 2840a3c..0000000 --- a/lib/ESP32-Arduino-CAN/src/CAN_config.h +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @section License - * - * The MIT License (MIT) - * - * Copyright (c) 2017, Thomas Barth, barth-dev.de - * - * Permission is hereby granted, free of charge, to any person - * obtaining a copy of this software and associated documentation - * files (the "Software"), to deal in the Software without - * restriction, including without limitation the rights to use, copy, - * modify, merge, publish, distribute, sublicense, and/or sell copies - * of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be - * included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, - * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS - * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN - * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef __DRIVERS_CAN_CFG_H__ -#define __DRIVERS_CAN_CFG_H__ - -#include "freertos/FreeRTOS.h" -#include "freertos/queue.h" -#include "freertos/task.h" -#include "driver/gpio.h" -#include "freertos/semphr.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/** \brief CAN Node Bus speed */ -typedef enum { - CAN_SPEED_100KBPS = 100, /**< \brief CAN Node runs at 100kBit/s. */ - CAN_SPEED_125KBPS = 125, /**< \brief CAN Node runs at 125kBit/s. */ - CAN_SPEED_200KBPS = 200, /**< \brief CAN Node runs at 250kBit/s. */ - CAN_SPEED_250KBPS = 250, /**< \brief CAN Node runs at 250kBit/s. */ - CAN_SPEED_500KBPS = 500, /**< \brief CAN Node runs at 500kBit/s. */ - CAN_SPEED_800KBPS = 800, /**< \brief CAN Node runs at 800kBit/s. */ - CAN_SPEED_1000KBPS = 1000 /**< \brief CAN Node runs at 1000kBit/s. */ -} CAN_speed_t; - -/** \brief CAN configuration structure */ -typedef struct { - CAN_speed_t speed; /**< \brief CAN speed. */ - gpio_num_t tx_pin_id; /**< \brief TX pin. */ - gpio_num_t rx_pin_id; /**< \brief RX pin. */ - QueueHandle_t rx_queue; /**< \brief Handler to FreeRTOS RX queue. */ - QueueHandle_t tx_queue; /**< \brief Handler to FreeRTOS TX queue. */ - TaskHandle_t tx_handle; /**< \brief Handler to FreeRTOS TX task. */ - TaskHandle_t rx_handle; /**< \brief Handler to FreeRTOS RX task. */ -} CAN_device_t; - -/** \brief CAN configuration reference */ -extern CAN_device_t CAN_cfg; - -#ifdef __cplusplus -} -#endif - -#endif /* __DRIVERS_CAN_CFG_H__ */ diff --git a/lib/ESP32-Arduino-CAN/src/ESP32CAN.cpp b/lib/ESP32-Arduino-CAN/src/ESP32CAN.cpp deleted file mode 100644 index 1412b4a..0000000 --- a/lib/ESP32-Arduino-CAN/src/ESP32CAN.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "ESP32CAN.h" - -int ESP32CAN::CANInit() -{ - return CAN_init(); -} -int ESP32CAN::CANWriteFrame(const CAN_frame_t* p_frame) -{ - return CAN_write_frame(p_frame); -} -int ESP32CAN::CANStop() -{ - return CAN_stop(); -} -int ESP32CAN::CANConfigFilter(const CAN_filter_t* p_filter) -{ - return CAN_config_filter(p_filter); -} - -ESP32CAN ESP32Can; diff --git a/lib/ESP32-Arduino-CAN/src/ESP32CAN.h b/lib/ESP32-Arduino-CAN/src/ESP32CAN.h deleted file mode 100644 index 0cbe156..0000000 --- a/lib/ESP32-Arduino-CAN/src/ESP32CAN.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef ESP32CAN_H -#define ESP32CAN_H - -#include "CAN_config.h" -#include "CAN.h" - -class ESP32CAN -{ - public: - int CANInit(); - int CANConfigFilter(const CAN_filter_t* p_filter); - int CANWriteFrame(const CAN_frame_t* p_frame); - int CANStop(); -}; - -extern ESP32CAN ESP32Can; -#endif diff --git a/lib/ESP32-Arduino-CAN/src/VescCanConstants.c b/lib/ESP32-Arduino-CAN/src/VescCanConstants.c deleted file mode 100644 index e6f8d77..0000000 --- a/lib/ESP32-Arduino-CAN/src/VescCanConstants.c +++ /dev/null @@ -1,299 +0,0 @@ -/** - * @section License - * - * The MIT License (MIT) - * - * Copyright (c) 2017, Thomas Barth, barth-dev.de - * 2017, Jaime Breva, jbreva@nayarsystems.com - * 2018, Michael Wagner, mw@iot-make.de - * - * Permission is hereby granted, free of charge, to any person - * obtaining a copy of this software and associated documentation - * files (the "Software"), to deal in the Software without - * restriction, including without limitation the rights to use, copy, - * modify, merge, publish, distribute, sublicense, and/or sell copies - * of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be - * included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, - * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS - * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN - * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - * - */ - -#include "CAN.h" - -#include "freertos/FreeRTOS.h" -#include "freertos/queue.h" - -#include "esp_intr.h" -#include "soc/dport_reg.h" -#include - -#include "driver/gpio.h" - -#include "can_regdef.h" -#include "CAN_config.h" - -// CAN Filter - no acceptance filter -static CAN_filter_t __filter = { Dual_Mode, 0, 0, 0, 0, 0Xff, 0Xff, 0Xff, 0Xff }; - -static void CAN_read_frame_phy(); -static void CAN_isr(void *arg_p); -static int CAN_write_frame_phy(const CAN_frame_t *p_frame); -static SemaphoreHandle_t sem_tx_complete; - -static void CAN_isr(void *arg_p) { - - // Interrupt flag buffer - __CAN_IRQ_t interrupt; - BaseType_t higherPriorityTaskWoken = pdFALSE; - - // Read interrupt status and clear flags - interrupt = MODULE_CAN->IR.U; - - // Handle RX frame available interrupt - if ((interrupt & __CAN_IRQ_RX) != 0) - CAN_read_frame_phy(&higherPriorityTaskWoken); - - // Handle TX complete interrupt - // Handle error interrupts. - if ((interrupt & (__CAN_IRQ_TX | __CAN_IRQ_ERR //0x4 - | __CAN_IRQ_DATA_OVERRUN // 0x8 - | __CAN_IRQ_WAKEUP // 0x10 - | __CAN_IRQ_ERR_PASSIVE // 0x20 - | __CAN_IRQ_ARB_LOST // 0x40 - | __CAN_IRQ_BUS_ERR // 0x80 - )) != 0) { - xSemaphoreGiveFromISR(sem_tx_complete, &higherPriorityTaskWoken); - } - - // check if any higher priority task has been woken by any handler - if (higherPriorityTaskWoken) - portYIELD_FROM_ISR(); -} - -static void CAN_read_frame_phy(BaseType_t *higherPriorityTaskWoken) { - - // byte iterator - uint8_t __byte_i; - - // frame read buffer - CAN_frame_t __frame; - - // check if we have a queue. If not, operation is aborted. - if (CAN_cfg.rx_queue == NULL) { - // Let the hardware know the frame has been read. - MODULE_CAN->CMR.B.RRB = 1; - return; - } - - // get FIR - __frame.FIR.U = MODULE_CAN->MBX_CTRL.FCTRL.FIR.U; - - // check if this is a standard or extended CAN frame - // standard frame - if (__frame.FIR.B.FF == CAN_frame_std) { - - // Get Message ID - __frame.MsgID = _CAN_GET_STD_ID; - - // deep copy data bytes - for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++) - __frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i]; - } - // extended frame - else { - - // Get Message ID - __frame.MsgID = _CAN_GET_EXT_ID; - - // deep copy data bytes - for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++) - __frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i]; - } - - // send frame to input queue - xQueueSendToBackFromISR(CAN_cfg.rx_queue, &__frame, higherPriorityTaskWoken); - - // Let the hardware know the frame has been read. - MODULE_CAN->CMR.B.RRB = 1; -} - -static int CAN_write_frame_phy(const CAN_frame_t *p_frame) { - - // byte iterator - uint8_t __byte_i; - - // copy frame information record - MODULE_CAN->MBX_CTRL.FCTRL.FIR.U = p_frame->FIR.U; - - // standard frame - if (p_frame->FIR.B.FF == CAN_frame_std) { - - // Write message ID - _CAN_SET_STD_ID(p_frame->MsgID); - - // Copy the frame data to the hardware - for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++) - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i] = p_frame->data.u8[__byte_i]; - - } - // extended frame - else { - - // Write message ID - _CAN_SET_EXT_ID(p_frame->MsgID); - - // Copy the frame data to the hardware - for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++) - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i] = p_frame->data.u8[__byte_i]; - } - - // Transmit frame - MODULE_CAN->CMR.B.TR = 1; - - return 0; -} - -int CAN_init() { - - // Time quantum - double __tq; - - // enable module - DPORT_SET_PERI_REG_MASK(DPORT_PERIP_CLK_EN_REG, DPORT_CAN_CLK_EN); - DPORT_CLEAR_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST); - - // configure TX pin - gpio_set_level(CAN_cfg.tx_pin_id, 1); - gpio_set_direction(CAN_cfg.tx_pin_id, GPIO_MODE_OUTPUT); - gpio_matrix_out(CAN_cfg.tx_pin_id, CAN_TX_IDX, 0, 0); - gpio_pad_select_gpio(CAN_cfg.tx_pin_id); - - // configure RX pin - gpio_set_direction(CAN_cfg.rx_pin_id, GPIO_MODE_INPUT); - gpio_matrix_in(CAN_cfg.rx_pin_id, CAN_RX_IDX, 0); - gpio_pad_select_gpio(CAN_cfg.rx_pin_id); - - // set to PELICAN mode - MODULE_CAN->CDR.B.CAN_M = 0x1; - - // synchronization jump width is the same for all baud rates - MODULE_CAN->BTR0.B.SJW = 0x1; - - // TSEG2 is the same for all baud rates - MODULE_CAN->BTR1.B.TSEG2 = 0x1; - - // select time quantum and set TSEG1 - switch (CAN_cfg.speed) { - case CAN_SPEED_1000KBPS: - MODULE_CAN->BTR1.B.TSEG1 = 0x4; - __tq = 0.125; - break; - - case CAN_SPEED_800KBPS: - MODULE_CAN->BTR1.B.TSEG1 = 0x6; - __tq = 0.125; - break; - - case CAN_SPEED_200KBPS: - MODULE_CAN->BTR1.B.TSEG1 = 0xc; - MODULE_CAN->BTR1.B.TSEG2 = 0x5; - __tq = 0.25; - break; - - default: - MODULE_CAN->BTR1.B.TSEG1 = 0xc; - __tq = ((float) 1000 / CAN_cfg.speed) / 16; - } - - // set baud rate prescaler - MODULE_CAN->BTR0.B.BRP = (uint8_t) round((((APB_CLK_FREQ * __tq) / 2) - 1) / 1000000) - 1; - - /* Set sampling - * 1 -> triple; the bus is sampled three times; recommended for low/medium speed buses (class A and B) where - * filtering spikes on the bus line is beneficial 0 -> single; the bus is sampled once; recommended for high speed - * buses (SAE class C)*/ - MODULE_CAN->BTR1.B.SAM = 0x1; - - // enable all interrupts - MODULE_CAN->IER.U = 0xff; - - // Set acceptance filter - MODULE_CAN->MOD.B.AFM = __filter.FM; - MODULE_CAN->MBX_CTRL.ACC.CODE[0] = __filter.ACR0; - MODULE_CAN->MBX_CTRL.ACC.CODE[1] = __filter.ACR1; - MODULE_CAN->MBX_CTRL.ACC.CODE[2] = __filter.ACR2; - MODULE_CAN->MBX_CTRL.ACC.CODE[3] = __filter.ACR3; - MODULE_CAN->MBX_CTRL.ACC.MASK[0] = __filter.AMR0; - MODULE_CAN->MBX_CTRL.ACC.MASK[1] = __filter.AMR1; - MODULE_CAN->MBX_CTRL.ACC.MASK[2] = __filter.AMR2; - MODULE_CAN->MBX_CTRL.ACC.MASK[3] = __filter.AMR3; - - // set to normal mode - MODULE_CAN->OCR.B.OCMODE = __CAN_OC_NOM; - - // clear error counters - MODULE_CAN->TXERR.U = 0; - MODULE_CAN->RXERR.U = 0; - (void) MODULE_CAN->ECC; - - // clear interrupt flags - (void) MODULE_CAN->IR.U; - - // install CAN ISR - esp_intr_alloc(ETS_CAN_INTR_SOURCE, 0, CAN_isr, NULL, NULL); - - // allocate the tx complete semaphore - sem_tx_complete = xSemaphoreCreateBinary(); - - // Showtime. Release Reset Mode. - MODULE_CAN->MOD.B.RM = 0; - - return 0; -} - -int CAN_write_frame(const CAN_frame_t *p_frame) { - if (sem_tx_complete == NULL) { - return -1; - } - - // Write the frame to the controller - CAN_write_frame_phy(p_frame); - - // wait for the frame tx to complete - //xSemaphoreTake(sem_tx_complete, portMAX_DELAY); - - return 0; -} - -int CAN_stop() { - // enter reset mode - MODULE_CAN->MOD.B.RM = 1; - - return 0; -} - -int CAN_config_filter(const CAN_filter_t* p_filter) { - - __filter.FM = p_filter->FM; - __filter.ACR0 = p_filter->ACR0; - __filter.ACR1 = p_filter->ACR1; - __filter.ACR2 = p_filter->ACR2; - __filter.ACR3 = p_filter->ACR3; - __filter.AMR0 = p_filter->AMR0; - __filter.AMR1 = p_filter->AMR1; - __filter.AMR2 = p_filter->AMR2; - __filter.AMR3 = p_filter->AMR3; - - return 0; -} \ No newline at end of file diff --git a/lib/ESP32-Arduino-CAN/src/can_regdef.h b/lib/ESP32-Arduino-CAN/src/can_regdef.h deleted file mode 100644 index 967ac81..0000000 --- a/lib/ESP32-Arduino-CAN/src/can_regdef.h +++ /dev/null @@ -1,279 +0,0 @@ -/** - * @section License - * - * The MIT License (MIT) - * - * Copyright (c) 2017, Thomas Barth, barth-dev.de - * - * Permission is hereby granted, free of charge, to any person - * obtaining a copy of this software and associated documentation - * files (the "Software"), to deal in the Software without - * restriction, including without limitation the rights to use, copy, - * modify, merge, publish, distribute, sublicense, and/or sell copies - * of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be - * included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, - * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS - * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN - * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef __DRIVERS_CAN_REGDEF_H_ -#define __DRIVERS_CAN_REGDEF_H_ - -#include "CAN.h" //CAN_FIR_t - -#ifdef __cplusplus -extern "C" { -#endif - -/** \brief Start address of CAN registers */ -#define MODULE_CAN ((volatile CAN_Module_t *) 0x3ff6b000) - -/** \brief Get standard message ID */ -#define _CAN_GET_STD_ID \ - (((uint32_t) MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[0] << 3) | (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[1] >> 5)) - -/** \brief Get extended message ID */ -#define _CAN_GET_EXT_ID \ - (((uint32_t) MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[0] << 21) | \ - (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[1] << 13) | (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[2] << 5) | \ - (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[3] >> 3)) - -/** \brief Set standard message ID */ -#define _CAN_SET_STD_ID(x) \ - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[0] = ((x) >> 3); \ - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[1] = ((x) << 5); - -/** \brief Set extended message ID */ -#define _CAN_SET_EXT_ID(x) \ - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[0] = ((x) >> 21); \ - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[1] = ((x) >> 13); \ - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[2] = ((x) >> 5); \ - MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[3] = ((x) << 3); - -/** \brief Interrupt status register */ -typedef enum { - __CAN_IRQ_RX = BIT(0), /**< \brief RX Interrupt */ - __CAN_IRQ_TX = BIT(1), /**< \brief TX Interrupt */ - __CAN_IRQ_ERR = BIT(2), /**< \brief Error Interrupt */ - __CAN_IRQ_DATA_OVERRUN = BIT(3), /**< \brief Data Overrun Interrupt */ - __CAN_IRQ_WAKEUP = BIT(4), /**< \brief Wakeup Interrupt */ - __CAN_IRQ_ERR_PASSIVE = BIT(5), /**< \brief Passive Error Interrupt */ - __CAN_IRQ_ARB_LOST = BIT(6), /**< \brief Arbitration lost interrupt */ - __CAN_IRQ_BUS_ERR = BIT(7), /**< \brief Bus error Interrupt */ -} __CAN_IRQ_t; - -/** \brief OCMODE options. */ -typedef enum { - __CAN_OC_BOM = 0b00, /**< \brief bi-phase output mode */ - __CAN_OC_TOM = 0b01, /**< \brief test output mode */ - __CAN_OC_NOM = 0b10, /**< \brief normal output mode */ - __CAN_OC_COM = 0b11, /**< \brief clock output mode */ -} __CAN_OCMODE_t; - -/** - * CAN controller (SJA1000). - */ -typedef struct { - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RM : 1; /**< \brief MOD.0 Reset Mode */ - unsigned int LOM : 1; /**< \brief MOD.1 Listen Only Mode */ - unsigned int STM : 1; /**< \brief MOD.2 Self Test Mode */ - unsigned int AFM : 1; /**< \brief MOD.3 Acceptance Filter Mode */ - unsigned int SM : 1; /**< \brief MOD.4 Sleep Mode */ - unsigned int reserved_27 : 27; /**< \brief \internal Reserved */ - } B; - } MOD; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int TR : 1; /**< \brief CMR.0 Transmission Request */ - unsigned int AT : 1; /**< \brief CMR.1 Abort Transmission */ - unsigned int RRB : 1; /**< \brief CMR.2 Release Receive Buffer */ - unsigned int CDO : 1; /**< \brief CMR.3 Clear Data Overrun */ - unsigned int GTS : 1; /**< \brief CMR.4 Go To Sleep */ - unsigned int reserved_27 : 27; /**< \brief \internal Reserved */ - } B; - } CMR; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RBS : 1; /**< \brief SR.0 Receive Buffer Status */ - unsigned int DOS : 1; /**< \brief SR.1 Data Overrun Status */ - unsigned int TBS : 1; /**< \brief SR.2 Transmit Buffer Status */ - unsigned int TCS : 1; /**< \brief SR.3 Transmission Complete Status */ - unsigned int RS : 1; /**< \brief SR.4 Receive Status */ - unsigned int TS : 1; /**< \brief SR.5 Transmit Status */ - unsigned int ES : 1; /**< \brief SR.6 Error Status */ - unsigned int BS : 1; /**< \brief SR.7 Bus Status */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } SR; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RI : 1; /**< \brief IR.0 Receive Interrupt */ - unsigned int TI : 1; /**< \brief IR.1 Transmit Interrupt */ - unsigned int EI : 1; /**< \brief IR.2 Error Interrupt */ - unsigned int DOI : 1; /**< \brief IR.3 Data Overrun Interrupt */ - unsigned int WUI : 1; /**< \brief IR.4 Wake-Up Interrupt */ - unsigned int EPI : 1; /**< \brief IR.5 Error Passive Interrupt */ - unsigned int ALI : 1; /**< \brief IR.6 Arbitration Lost Interrupt */ - unsigned int BEI : 1; /**< \brief IR.7 Bus Error Interrupt */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } IR; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RIE : 1; /**< \brief IER.0 Receive Interrupt Enable */ - unsigned int TIE : 1; /**< \brief IER.1 Transmit Interrupt Enable */ - unsigned int EIE : 1; /**< \brief IER.2 Error Interrupt Enable */ - unsigned int DOIE : 1; /**< \brief IER.3 Data Overrun Interrupt Enable */ - unsigned int WUIE : 1; /**< \brief IER.4 Wake-Up Interrupt Enable */ - unsigned int EPIE : 1; /**< \brief IER.5 Error Passive Interrupt Enable */ - unsigned int ALIE : 1; /**< \brief IER.6 Arbitration Lost Interrupt Enable */ - unsigned int BEIE : 1; /**< \brief IER.7 Bus Error Interrupt Enable */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } IER; - uint32_t RESERVED0; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int BRP : 6; /**< \brief BTR0[5:0] Baud Rate Prescaler */ - unsigned int SJW : 2; /**< \brief BTR0[7:6] Synchronization Jump Width*/ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } BTR0; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int TSEG1 : 4; /**< \brief BTR1[3:0] Timing Segment 1 */ - unsigned int TSEG2 : 3; /**< \brief BTR1[6:4] Timing Segment 2*/ - unsigned int SAM : 1; /**< \brief BTR1.7 Sampling*/ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } BTR1; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int OCMODE : 2; /**< \brief OCR[1:0] Output Control Mode, see # */ - unsigned int OCPOL0 : 1; /**< \brief OCR.2 Output Control Polarity 0 */ - unsigned int OCTN0 : 1; /**< \brief OCR.3 Output Control Transistor N0 */ - unsigned int OCTP0 : 1; /**< \brief OCR.4 Output Control Transistor P0 */ - unsigned int OCPOL1 : 1; /**< \brief OCR.5 Output Control Polarity 1 */ - unsigned int OCTN1 : 1; /**< \brief OCR.6 Output Control Transistor N1 */ - unsigned int OCTP1 : 1; /**< \brief OCR.7 Output Control Transistor P1 */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } OCR; - uint32_t RESERVED1[2]; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int ALC : 8; /**< \brief ALC[7:0] Arbitration Lost Capture */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } ALC; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int ECC : 8; /**< \brief ECC[7:0] Error Code Capture */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } ECC; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int EWLR : 8; /**< \brief EWLR[7:0] Error Warning Limit */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } EWLR; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RXERR : 8; /**< \brief RXERR[7:0] Receive Error Counter */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } RXERR; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int TXERR : 8; /**< \brief TXERR[7:0] Transmit Error Counter */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } TXERR; - - union { - struct { - uint32_t CODE[4]; /**< \brief Acceptance Message ID */ - uint32_t MASK[4]; /**< \brief Acceptance Mask */ - uint32_t RESERVED2[5]; - } ACC; /**< \brief Acceptance filtering */ - struct { - CAN_FIR_t FIR; /**< \brief Frame information record */ - union { - struct { - uint32_t ID[2]; /**< \brief Standard frame message-ID*/ - uint32_t data[8]; /**< \brief Standard frame payload */ - uint32_t reserved[2]; - } STD; /**< \brief Standard frame format */ - struct { - uint32_t ID[4]; /**< \brief Extended frame message-ID*/ - uint32_t data[8]; /**< \brief Extended frame payload */ - } EXT; /**< \brief Extended frame format */ - } TX_RX; /**< \brief RX/TX interface */ - } FCTRL; /**< \brief Function control regs */ - } MBX_CTRL; /**< \brief Mailbox control */ - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RMC : 8; /**< \brief RMC[7:0] RX Message Counter */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved Enable */ - } B; - } RMC; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int RBSA : 8; /**< \brief RBSA[7:0] RX Buffer Start Address */ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved Enable */ - } B; - } RBSA; - union { - uint32_t U; /**< \brief Unsigned access */ - struct { - unsigned int COD : 3; /**< \brief CDR[2:0] CLKOUT frequency selector based of fOSC*/ - unsigned int COFF : 1; /**< \brief CDR.3 CLKOUT off*/ - unsigned int reserved_1 : 1; /**< \brief \internal Reserved */ - unsigned int - RXINTEN : 1; /**< \brief CDR.5 This bit allows the TX1 output to be used as a dedicated receive interrupt - output*/ - unsigned int - CBP : 1; /**< \brief CDR.6 allows to bypass the CAN input comparator and is only possible in reset mode.*/ - unsigned int - CAN_M : 1; /**< \brief CDR.7 If CDR.7 is at logic 0 the CAN controller operates in BasicCAN mode. If set to - logic 1 the CAN controller operates in PeliCAN mode. Write access is only possible in reset - mode*/ - unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ - } B; - } CDR; - uint32_t IRAM[2]; -} CAN_Module_t; - -#ifdef __cplusplus -} -#endif - -#endif /* __DRIVERS_CAN_REGDEF_H_ */ diff --git a/platformio.ini b/platformio.ini index db69406..6daf935 100644 --- a/platformio.ini +++ b/platformio.ini @@ -35,7 +35,8 @@ build_flags= -D LIGHT_BAR_PIN=2 ; DIN of WS28xx for battery indicator -D VESC_RX_PIN=16 ; UART RX to Cheap Focer 2, connent to TX on CF2 -D VESC_TX_PIN=17 ; UART TX to Cheap Focer 2, connent to RX on CF2 -# For CAN-GPIO PINS please see CanDevice.cpp + -D CAN_TX_PIN=_26 + -D CAN_RX_PIN=_27 [env:native] platform = native @@ -46,7 +47,6 @@ board = wemos_d1_mini32 framework = arduino monitor_speed = 115200 upload_Speed = 921600 -upload_port = /dev/tty.wchusbserial54320213941 board_build.partitions = default.csv lib_deps = ${common_env_data.lib_deps_external} diff --git a/src/BleCanProxy.cpp b/src/BleCanProxy.cpp index e8b954d..dd588f5 100644 --- a/src/BleCanProxy.cpp +++ b/src/BleCanProxy.cpp @@ -38,15 +38,15 @@ void BleCanProxy::proxyIn(std::string in) { } if (length <= 6) { - CAN_frame_t tx_frame = {}; - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; - tx_frame.FIR.B.DLC = 0x02 + length; - tx_frame.data.u8[0] = ble_proxy_can_id; - tx_frame.data.u8[1] = 0x00; - tx_frame.data.u8[2] = command; + twai_message_t tx_frame = {}; + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = 0x02 + length; + tx_frame.data[0] = ble_proxy_can_id; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = command; for (int i = 3; i < length + 2; i++) { - tx_frame.data.u8[i] = (uint8_t) in.at(i); + tx_frame.data[i] = (uint8_t) in.at(i); } candevice->sendCanFrame(&tx_frame); } else { @@ -71,16 +71,16 @@ void BleCanProxy::proxyIn(std::string in) { int sendLen = (length >= byteNum+ 7) ? 7 : length - byteNum; //Serial.printf("bufLen %d, byteNum %d, sendlen %d\n", length, byteNum , sendLen); - CAN_frame_t tx_frame = {}; - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_FILL_RX_BUFFER) << 8) + vesc_id; - tx_frame.FIR.B.DLC = sendLen + 1; - tx_frame.data.u8[0] = byteNum; //startbyte counter of frame + twai_message_t tx_frame = {}; + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_FILL_RX_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = sendLen + 1; + tx_frame.data[0] = byteNum; //startbyte counter of frame for (int i = 1; i < sendLen+1; i++) { //Serial.printf("Reading byte %d, length %d, index %d, sendlen %d\n", byteNum + i, bufLen, i, sendLen); - tx_frame.data.u8[i] = (uint8_t) longPackBuffer.at(byteNum-1 + i); + tx_frame.data[i] = (uint8_t) longPackBuffer.at(byteNum-1 + i); } candevice->sendCanFrame(&tx_frame); } @@ -89,31 +89,31 @@ void BleCanProxy::proxyIn(std::string in) { int sendLen = (length >= byteNum + 6) ? 6 : length - byteNum ; //Serial.printf("bufLen %d, byteNum %d, sendlen %d\n", length, byteNum , sendLen); - CAN_frame_t tx_frame = {}; - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_FILL_RX_BUFFER_LONG) << 8) + vesc_id; - tx_frame.FIR.B.DLC = sendLen + 2; - tx_frame.data.u8[0] = byteNum >> 8; - tx_frame.data.u8[1] = byteNum & 0xFF; + twai_message_t tx_frame = {}; + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_FILL_RX_BUFFER_LONG) << 8) + vesc_id; + tx_frame.data_length_code= sendLen + 2; + tx_frame.data[0] = byteNum >> 8; + tx_frame.data[1] = byteNum & 0xFF; for (int i = 2; i < sendLen+2; i++) { //Serial.printf("Reading byte %d, length %d, index %d, sendlen %d\n", byteNum + i, bufLen, i, sendLen); - tx_frame.data.u8[i] = (uint8_t) longPackBuffer.at(byteNum-2 + i); + tx_frame.data[i] = (uint8_t) longPackBuffer.at(byteNum-2 + i); } candevice->sendCanFrame(&tx_frame); } - CAN_frame_t tx_frame = {}; - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_RX_BUFFER) << 8) + vesc_id; - tx_frame.FIR.B.DLC = 6; - tx_frame.data.u8[0] = ble_proxy_can_id; - tx_frame.data.u8[1] = 0; // IS THIS CORRECT????? - tx_frame.data.u8[2] = length >> 8; - tx_frame.data.u8[3] = length & 0xFF; - tx_frame.data.u8[4] = longPackBuffer.at(longPackBuffer.size() - 3); - tx_frame.data.u8[5] = longPackBuffer.at(longPackBuffer.size() - 2); + twai_message_t tx_frame = {}; + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_RX_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = 6; + tx_frame.data[0] = ble_proxy_can_id; + tx_frame.data[1] = 0; // IS THIS CORRECT????? + tx_frame.data[2] = length >> 8; + tx_frame.data[3] = length & 0xFF; + tx_frame.data[4] = longPackBuffer.at(longPackBuffer.size() - 3); + tx_frame.data[5] = longPackBuffer.at(longPackBuffer.size() - 2); candevice->sendCanFrame(&tx_frame); } diff --git a/src/BleCanProxy.h b/src/BleCanProxy.h index c6a7957..0fbe80b 100644 --- a/src/BleCanProxy.h +++ b/src/BleCanProxy.h @@ -5,8 +5,6 @@ #include "config.h" #include #include "VescCanConstants.h" -#include "CAN.h" -#include "CAN_config.h" #include "LoopbackStream.h" #include "CanDevice.h" diff --git a/src/BleServer.cpp b/src/BleServer.cpp index 6625038..c7f1518 100644 --- a/src/BleServer.cpp +++ b/src/BleServer.cpp @@ -150,7 +150,7 @@ void BleServer::init(Stream *vesc) { char buf[128]; snprintf(buf, 128, "Initial MTU size %d", mtu_size); Logger::notice(LOG_TAG_BLESERVER, buf); -#if defined(CANBUD_ENABLED) +#if defined(CANBUS_ENABLED) this->canbus = canbus; #endif diff --git a/src/BleServer.h b/src/BleServer.h index b1d92b1..487e85e 100644 --- a/src/BleServer.h +++ b/src/BleServer.h @@ -53,7 +53,7 @@ class BleServer : private: #if defined(CANBUS_ENABLED) - CanBus *canbus{}; + CanBus *canbus; #endif struct sendConfigValue; void dumpBuffer(std::string header, std::string buffer); diff --git a/src/CanBus.cpp b/src/CanBus.cpp index 8cc41a5..7cac126 100644 --- a/src/CanBus.cpp +++ b/src/CanBus.cpp @@ -1,6 +1,4 @@ #include "CanBus.h" -#include "../lib/ESP32-Arduino-CAN/src/CAN_config.h" -#include "../lib/ESP32-Arduino-CAN/src/CAN.h" #ifdef CANBUS_ENABLED @@ -42,14 +40,14 @@ void CanBus::init() { */ void CanBus::loop() { int frameCount = 0; - + twai_message_t rx_frame; if (initialized) { if (millis() - lastRealtimeData > interval && !proxy->processing) { - //requestRealtimeData(); + requestRealtimeData(); } if (millis() - lastBalanceData > interval && !proxy->processing) { - //requestBalanceData(); + requestBalanceData(); } } else if(initRetryCounter > 0 && millis() - lastRetry > 500) { requestFirmwareVersion(); @@ -61,15 +59,14 @@ void CanBus::loop() { } //receive next CAN frame from queue - CAN_frame_t rx_frame; - while (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE) { + while (twai_receive( &rx_frame, 3 * portTICK_PERIOD_MS) == ESP_OK) { if (!initialized) { Logger::notice(LOG_TAG_CANBUS, "CANBUS is now initialized"); initialized = true; } frameCount++; //VESC only uses ext packages, so skip std packages - if (rx_frame.FIR.B.FF == CAN_frame_ext) { + if (rx_frame.extd) { if (Logger::getLogLevel() == Logger::VERBOSE) { printFrame(rx_frame, frameCount); } @@ -90,64 +87,64 @@ void CanBus::loop() { void CanBus::requestFirmwareVersion() { Logger::notice(LOG_TAG_CANBUS, "requestFirmwareVersion"); - CAN_frame_t tx_frame = {}; - - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; - tx_frame.FIR.B.DLC = 0x03; - tx_frame.data.u8[0] = esp_can_id; - tx_frame.data.u8[1] = 0x00; - tx_frame.data.u8[2] = 0x00; // COMM_FW_VERSION + twai_message_t tx_frame = {}; + + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = 0x03; + tx_frame.data[0] = esp_can_id; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; // COMM_FW_VERSION candevice->sendCanFrame(&tx_frame); } void CanBus::requestRealtimeData() { Logger::notice(LOG_TAG_CANBUS, "requestRealtimeData"); - CAN_frame_t tx_frame = {}; - - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; - tx_frame.FIR.B.DLC = 0x07; - tx_frame.data.u8[0] = esp_can_id; - tx_frame.data.u8[1] = 0x00; - tx_frame.data.u8[2] = 0x32; // COMM_GET_VALUES_SELECTIVE + twai_message_t tx_frame = {}; + + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = 0x07; + tx_frame.data[0] = esp_can_id; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x32; // COMM_GET_VALUES_SELECTIVE // mask - tx_frame.data.u8[3] = 0x00; // Byte1 of mask (Bits 24-31) - tx_frame.data.u8[4] = 0x00; // Byte2 of mask (Bits 16-23) - tx_frame.data.u8[5] = B10000111; // Byte3 of mask (Bits 8-15) - tx_frame.data.u8[6] = B11000011; // Byte4 of mask (Bits 0-7) + tx_frame.data[3] = 0x00; // Byte1 of mask (Bits 24-31) + tx_frame.data[4] = 0x00; // Byte2 of mask (Bits 16-23) + tx_frame.data[5] = B10000111; // Byte3 of mask (Bits 8-15) + tx_frame.data[6] = B11000011; // Byte4 of mask (Bits 0-7) candevice->sendCanFrame(&tx_frame); } void CanBus::requestBalanceData() { Logger::notice(LOG_TAG_CANBUS, "requestBalanceData"); - CAN_frame_t tx_frame = {}; - - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; - tx_frame.FIR.B.DLC = 0x03; - tx_frame.data.u8[0] = esp_can_id; - tx_frame.data.u8[1] = 0x00; - tx_frame.data.u8[2] = 0x4F; // COMM_GET_DECODED_BALANCE + twai_message_t tx_frame = {}; + + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = 0x03; + tx_frame.data[0] = esp_can_id; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x4F; // COMM_GET_DECODED_BALANCE candevice->sendCanFrame(&tx_frame); } void CanBus::ping() { - CAN_frame_t tx_frame = {}; - tx_frame.FIR.B.FF = CAN_frame_ext; - tx_frame.MsgID = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PING) << 8) + vesc_id; - tx_frame.FIR.B.DLC = 0x01; - tx_frame.data.u8[0] = esp_can_id; + twai_message_t tx_frame = {}; + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PING) << 8) + vesc_id; + tx_frame.data_length_code = 0x01; + tx_frame.data[0] = esp_can_id; candevice->sendCanFrame(&tx_frame); } -void CanBus::printFrame(CAN_frame_t rx_frame, int frameCount) { - if (rx_frame.FIR.B.RTR == CAN_RTR) - printf("#%d RTR from 0x%08x, DLC %d\r\n", frameCount, rx_frame.MsgID, rx_frame.FIR.B.DLC); +void CanBus::printFrame(twai_message_t rx_frame, int frameCount) { + if (rx_frame.rtr) + printf("#%d RTR from 0x%08x, DLC %d\r\n", frameCount, rx_frame.identifier, rx_frame.data_length_code); else { - printf("#%d from 0x%08x, DLC %d, data [", frameCount, rx_frame.MsgID, rx_frame.FIR.B.DLC); + printf("#%d from 0x%08x, DLC %d, data [", frameCount, rx_frame.identifier, rx_frame.data_length_code); for (int i = 0; i < 8; i++) { - printf("%d", (uint8_t) rx_frame.data.u8[i]); + printf("%d", (uint8_t) rx_frame.data[i]); if (i != 7) { printf("\t"); } @@ -156,25 +153,22 @@ void CanBus::printFrame(CAN_frame_t rx_frame, int frameCount) { } } -void CanBus::clearFrame(CAN_frame_t rx_frame) { - rx_frame.MsgID=0; - rx_frame.FIR.B.DLC=0; - rx_frame.data.u8[0]=0; - rx_frame.data.u8[1]=0; - rx_frame.data.u8[2]=0; - rx_frame.data.u8[3]=0; - rx_frame.data.u8[4]=0; - rx_frame.data.u8[5]=0; - rx_frame.data.u8[6]=0; - rx_frame.data.u8[7]=0; - rx_frame.data.u32[0]=0; - rx_frame.data.u32[1]=0; - rx_frame.data.u64=0; +void CanBus::clearFrame(twai_message_t rx_frame) { + rx_frame.identifier=0; + rx_frame.data_length_code=0; + rx_frame.data[0]=0; + rx_frame.data[1]=0; + rx_frame.data[2]=0; + rx_frame.data[3]=0; + rx_frame.data[4]=0; + rx_frame.data[5]=0; + rx_frame.data[6]=0; + rx_frame.data[7]=0; } -void CanBus::processFrame(CAN_frame_t rx_frame, int frameCount) { +void CanBus::processFrame(twai_message_t rx_frame, int frameCount) { String frametype = ""; - uint32_t ID = rx_frame.MsgID; + uint32_t ID = rx_frame.identifier; if (RECV_STATUS_1 == ID) { frametype = "status1"; vescData->erpm = readInt32Value(rx_frame, 0); @@ -207,26 +201,25 @@ void CanBus::processFrame(CAN_frame_t rx_frame, int frameCount) { if (RECV_PROCESS_SHORT_BUFFER_PROXY == ID) { frametype = "process short buffer for <>"; - for (int i = 2; i < rx_frame.FIR.B.DLC; i++) { - proxybuffer.push_back(rx_frame.data.u8[i]); + for (int i = 1; i < rx_frame.data_length_code; i++) { + proxybuffer.push_back(rx_frame.data[i]); } - proxybuffer.push_back(1); - proxy->proxyOut(proxybuffer.data(), proxybuffer.size(), 54, 187); + proxy->proxyOut(proxybuffer.data(), proxybuffer.size(), rx_frame.data[4], rx_frame.data[5]); proxybuffer.clear(); } if (RECV_FILL_RX_BUFFER == ID) { frametype = "fill rx buffer"; - for (int i = 1; i < rx_frame.FIR.B.DLC; i++) { - buffer.push_back(rx_frame.data.u8[i]); + for (int i = 1; i < rx_frame.data_length_code; i++) { + buffer.push_back(rx_frame.data[i]); } } if (RECV_FILL_RX_BUFFER_PROXY == ID || RECV_FILL_RX_BUFFER_LONG_PROXY == ID) { boolean longBuffer = RECV_FILL_RX_BUFFER_LONG_PROXY == ID; frametype = longBuffer ? "fill rx long buffer" : "fill rx buffer"; - for (int i = (longBuffer ? 2 : 1); i < rx_frame.FIR.B.DLC; i++) { - proxybuffer.push_back(rx_frame.data.u8[i]); + for (int i = (longBuffer ? 2 : 1); i < rx_frame.data_length_code; i++) { + proxybuffer.push_back(rx_frame.data[i]); } } @@ -386,7 +379,7 @@ void CanBus::processFrame(CAN_frame_t rx_frame, int frameCount) { frametype += command; } if (isProxyRequest) { - proxy->proxyOut(proxybuffer.data(), proxybuffer.size(), rx_frame.data.u8[4], rx_frame.data.u8[5]); + proxy->proxyOut(proxybuffer.data(), proxybuffer.size(), rx_frame.data[4], rx_frame.data[5]); proxybuffer.clear(); } else { buffer.clear(); @@ -480,19 +473,19 @@ void CanBus::dumpVescValues() { lastDump = millis(); } -int32_t CanBus::readInt32Value(CAN_frame_t rx_frame, int startbyte) { +int32_t CanBus::readInt32Value(twai_message_t rx_frame, int startbyte) { int32_t intVal = ( - ((int32_t) rx_frame.data.u8[startbyte] << 24) + - ((int32_t) rx_frame.data.u8[startbyte + 1] << 16) + - ((int32_t) rx_frame.data.u8[startbyte + 2] << 8) + - ((int32_t) rx_frame.data.u8[startbyte + 3])); + ((int32_t) rx_frame.data[startbyte] << 24) + + ((int32_t) rx_frame.data[startbyte + 1] << 16) + + ((int32_t) rx_frame.data[startbyte + 2] << 8) + + ((int32_t) rx_frame.data[startbyte + 3])); return intVal; } -int16_t CanBus::readInt16Value(CAN_frame_t rx_frame, int startbyte) { +int16_t CanBus::readInt16Value(twai_message_t rx_frame, int startbyte) { int16_t intVal = ( - ((int16_t) rx_frame.data.u8[startbyte] << 8) + - ((int16_t) rx_frame.data.u8[startbyte + 1])); + ((int16_t) rx_frame.data[startbyte] << 8) + + ((int16_t) rx_frame.data[startbyte + 1])); return intVal; } diff --git a/src/CanBus.h b/src/CanBus.h index b70737b..91cf28d 100644 --- a/src/CanBus.h +++ b/src/CanBus.h @@ -8,12 +8,9 @@ #ifdef CANBUS_ENABLED -#include #include "AppConfiguration.h" #include #include -#include "CAN.h" -#include "CAN_config.h" #include "VescCanConstants.h" #include "BleCanProxy.h" #include "CanDevice.h" @@ -40,11 +37,11 @@ class CanBus { void requestRealtimeData(); void requestBalanceData(); void ping(); - static void printFrame(CAN_frame_t rx_frame, int frameCount); - void processFrame(CAN_frame_t rx_frame, int frameCount); - void clearFrame(CAN_frame_t rx_frame); - static int32_t readInt32Value(CAN_frame_t rx_frame, int startbyte); - static int16_t readInt16Value(CAN_frame_t rx_frame, int startbyte); + static void clearFrame(twai_message_t rx_frame); + static void printFrame(twai_message_t rx_frame, int frameCount); + void processFrame(twai_message_t rx_frame, int frameCount); + static int32_t readInt32Value(twai_message_t rx_frame, int startbyte); + static int16_t readInt16Value(twai_message_t rx_frame, int startbyte); int32_t readInt32ValueFromBuffer(int startbyte, boolean isProxyRequest); int16_t readInt16ValueFromBuffer(int startbyte, boolean isProxyRequest); int8_t readInt8ValueFromBuffer(int startbyte, boolean isProxyRequest); diff --git a/src/CanDevice.cpp b/src/CanDevice.cpp index 4b78f2b..676080f 100644 --- a/src/CanDevice.cpp +++ b/src/CanDevice.cpp @@ -1,50 +1,49 @@ #include "CanDevice.h" -CAN_device_t CAN_cfg; - void CanDevice::init() { - CAN_cfg.speed = CAN_SPEED_500KBPS; - CAN_cfg.tx_pin_id = GPIO_NUM_26; - CAN_cfg.rx_pin_id = GPIO_NUM_27; - CAN_cfg.rx_queue = xQueueCreate(1000, sizeof(CAN_frame_t)); - - /* - CAN_filter_t p_filter; - p_filter.FM = Single_Mode; - - p_filter.ACR0 = 0x00; - p_filter.ACR1 = 0x00; - p_filter.ACR2 = 0x10; - p_filter.ACR3 = 0x19; + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_26, GPIO_NUM_27, TWAI_MODE_NORMAL); + g_config.rx_queue_len=1000; + g_config.tx_queue_len=10; + twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); - p_filter.AMR0 = 0x1F; - p_filter.AMR1 = 0xFF; - p_filter.AMR2 = 0xFF; - p_filter.AMR3 = 0xFF; - ESP32Can.CANConfigFilter(&p_filter); - */ - //start CAN Module - ESP32Can.CANInit(); + // Install TWAI driver + if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) { + printf("Driver installed\n"); + } else { + printf("Failed to install driver\n"); + return; + } + // Start TWAI driver + if (twai_start() == ESP_OK) { + printf("Driver started\n"); + } else { + printf("Failed to start driver\n"); + return; + } } -void CanDevice::sendCanFrame(const CAN_frame_t *p_frame) { +void CanDevice::sendCanFrame(const twai_message_t *p_frame) { if (Logger::getLogLevel() == Logger::VERBOSE) { char buf[128]; snprintf(buf, 128, "Sending CAN frame %" PRIu32 " DLC %d, [%d, %d, %d, %d, %d, %d, %d, %d]", - p_frame->MsgID, - p_frame->FIR.B.DLC, - p_frame->data.u8[0], - p_frame->data.u8[1], - p_frame->data.u8[2], - p_frame->data.u8[3], - p_frame->data.u8[4], - p_frame->data.u8[5], - p_frame->data.u8[6], - p_frame->data.u8[7]); + p_frame->identifier, + p_frame->data_length_code, + p_frame->data[0], + p_frame->data[1], + p_frame->data[2], + p_frame->data[3], + p_frame->data[4], + p_frame->data[5], + p_frame->data[6], + p_frame->data[7]); Logger::verbose(LOG_TAG_CANDEVICE, buf); } xSemaphoreTake(mutex_v, portMAX_DELAY); - ESP32Can.CANWriteFrame(p_frame); + //Queue message for transmission + if (twai_transmit(p_frame, pdMS_TO_TICKS(1000)) != ESP_OK) { + printf("Failed to queue message for transmission\n"); + } xSemaphoreGive(mutex_v); delay(1); // This is needed, dunno why! } \ No newline at end of file diff --git a/src/CanDevice.h b/src/CanDevice.h index 86e7aed..2bae6c2 100644 --- a/src/CanDevice.h +++ b/src/CanDevice.h @@ -3,18 +3,24 @@ #include "Arduino.h" #include "config.h" -#include "CAN.h" -#include "CAN_config.h" #include -#include +#include "driver/twai.h" #define LOG_TAG_CANDEVICE "CanDevice" +#ifndef CAN_TX_PIN +#define CAN_TX_PIN 26 +#endif //CAN_TX_PIN + +#ifndef CAN_RX_PIN +#define CAN_RX_PIN 27 +#endif //CAN_RX_PIN + class CanDevice { private: SemaphoreHandle_t mutex_v = xSemaphoreCreateMutex(); public: void init(); - void sendCanFrame(const CAN_frame_t *p_frame); + void sendCanFrame(const twai_message_t *p_frame); }; #endif //RESCUE_CANDEVICE_H diff --git a/src/config.h b/src/config.h index 5d30040..0aee09b 100644 --- a/src/config.h +++ b/src/config.h @@ -6,8 +6,8 @@ // ###################################################################### // #define SOFTWARE_VERSION_MAJOR 2 -#define SOFTWARE_VERSION_MINOR 2 -#define SOFTWARE_VERSION_PATCH 1 +#define SOFTWARE_VERSION_MINOR 3 +#define SOFTWARE_VERSION_PATCH 0 #define HARDWARE_VERSION_MAJOR 3 #define HARDWARE_VERSION_MINOR 1