From 609a798a4e841f233954bf25cdcb1cc49e443932 Mon Sep 17 00:00:00 2001 From: Eliyahu Gluschove-Koppel Date: Thu, 8 Jun 2023 15:16:10 +0100 Subject: [PATCH 1/6] Switch from register access to ESP SDK so S3 CAN works --- src/CAN.h | 2 +- src/ESP32S3.cpp | 193 +++++++++++++++++++++++++++++++++++++++++++ src/ESP32S3.h | 66 +++++++++++++++ src/ESP32SJA1000.cpp | 2 +- src/ESP32SJA1000.h | 2 +- 5 files changed, 262 insertions(+), 3 deletions(-) create mode 100644 src/ESP32S3.cpp create mode 100644 src/ESP32S3.h diff --git a/src/CAN.h b/src/CAN.h index 11ae0d0..09aee07 100644 --- a/src/CAN.h +++ b/src/CAN.h @@ -7,7 +7,7 @@ #if defined(ADAFRUIT_FEATHER_M4_CAN) #include "CANSAME5x.h" #elif defined(ARDUINO_ARCH_ESP32) -#include "ESP32SJA1000.h" +#include "ESP32S3.h" #else #include "MCP2515.h" #endif diff --git a/src/ESP32S3.cpp b/src/ESP32S3.cpp new file mode 100644 index 0000000..61c69ad --- /dev/null +++ b/src/ESP32S3.cpp @@ -0,0 +1,193 @@ +// Copyright (c) Firefly Racing. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license information. + +#ifdef ARDUINO_ARCH_ESP32 + +#include "ESP32S3.h" +#include +#include +#include +#include +#include +#include + +ESP32S3Class::ESP32S3Class() : + CANControllerClass() +{ +} + +ESP32S3Class::~ESP32S3Class() {} + +int ESP32S3Class::begin(long baudRate) +{ + CANControllerClass::begin(baudRate); + + _loopback = false; + + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(_txPin, _rxPin, TWAI_MODE_NORMAL); + twai_timing_config_t t_config; + switch (baudRate) { + case (long)1000E3: + t_config = TWAI_TIMING_CONFIG_1MBITS(); + break; + case (long)500E3: + t_config = TWAI_TIMING_CONFIG_500KBITS(); + break; + case (long)250E3: + t_config = TWAI_TIMING_CONFIG_250KBITS(); + break; + case (long)125E3: + t_config = TWAI_TIMING_CONFIG_125KBITS(); + break; + case (long)100E3: + t_config = TWAI_TIMING_CONFIG_100KBITS(); + break; + case (long)50E3: + t_config = TWAI_TIMING_CONFIG_50KBITS(); + break; + default: + return 0; + } + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + + //Install TWAI driver + if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { + return 0; + } + + //Start TWAI driver + if (twai_start() != ESP_OK) { + return 0; + } + + xTaskCreate(ESP32S3Class::receive_task, "CANrecv", 4096, nullptr, configMAX_PRIORITIES - 1, nullptr); + + return 1; +} + +void ESP32S3Class::end() +{ + + + CANControllerClass::end(); +} + +int ESP32S3Class::endPacket() +{ + if (!CANControllerClass::endPacket()) { + return -2; + } + + twai_message_t msg; + msg.extd = _txExtended; + msg.rtr = _txRtr; + msg.ss = 0; + msg.self = 0; + msg.dlc_non_comp = 0; + msg.identifier = static_cast(_txId); + msg.data_length_code = static_cast(_txLength); + + memcpy(&msg.data, _txData, _txLength); + + if (twai_transmit(&msg, 0) != ESP_OK) { + return -1; + } + + return 0; +} + +int ESP32S3Class::parsePacket() +{ + return 0; +} + +void ESP32S3Class::onReceive(void(*callback)(int)) +{ + CANControllerClass::onReceive(callback); +} + +int ESP32S3Class::filter(int id, int mask) +{ + return 0; +} + +int ESP32S3Class::filterExtended(long id, long mask) +{ + return 0; +} + +int ESP32S3Class::observe() +{ + + return 0; +} + +int ESP32S3Class::loopback() +{ + return 0; +} + +int ESP32S3Class::sleep() +{ + return 0; +} + +int ESP32S3Class::wakeup() +{ + return 0; +} + +void ESP32S3Class::setPins(int rx, int tx) +{ + _rxPin = (gpio_num_t)rx; + _txPin = (gpio_num_t)tx; +} + +void ESP32S3Class::dumpRegisters(Stream& out) +{ + +} + +void ESP32S3Class::handleInterrupt(twai_message_t message) +{ + _rxId = message.identifier; + _rxExtended = message.extd; + _rxDlc = message.data_length_code; + _rxIndex = 0; + _rxRtr = message.rtr; + _rxLength = message.rtr ? 0 : _rxDlc; + memcpy(_rxData, message.data, _rxLength); + _onReceive(_rxLength); +} + +uint8_t ESP32S3Class::readRegister(uint8_t address) +{ + return 0; +} + +void ESP32S3Class::modifyRegister(uint8_t address, uint8_t mask, uint8_t value) +{ +} + +void ESP32S3Class::writeRegister(uint8_t address, uint8_t value) +{ + +} + +[[noreturn]] void ESP32S3Class::receive_task(void*) +{ + Serial.println("can recv started"); + while (true) { + twai_message_t message; + Serial.println("can recv waiting"); + if (twai_receive(&message, portMAX_DELAY) != ESP_OK) { + continue; + } + Serial.println("can recv recv"); + CAN.handleInterrupt(message); + } +} + +ESP32S3Class CAN; + +#endif diff --git a/src/ESP32S3.h b/src/ESP32S3.h new file mode 100644 index 0000000..46ad718 --- /dev/null +++ b/src/ESP32S3.h @@ -0,0 +1,66 @@ +// Copyright (c) Firefly Racing. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for full license information. + +#ifdef ARDUINO_ARCH_ESP32 + +#ifndef ESP32_S3_H +#define ESP32_S3_H + +#include "CANController.h" +#include + +#define DEFAULT_CAN_RX_PIN GPIO_NUM_4 +#define DEFAULT_CAN_TX_PIN GPIO_NUM_5 + +class ESP32S3Class : public CANControllerClass { + +public: + ESP32S3Class(); + virtual ~ESP32S3Class(); + + virtual int begin(long baudRate); + virtual void end(); + + virtual int endPacket(); + + virtual int parsePacket(); + + virtual void onReceive(void(*callback)(int)); + + using CANControllerClass::filter; + virtual int filter(int id, int mask); + using CANControllerClass::filterExtended; + virtual int filterExtended(long id, long mask); + + virtual int observe(); + virtual int loopback(); + virtual int sleep(); + virtual int wakeup(); + + void setPins(int rx, int tx); + + void dumpRegisters(Stream& out); + +private: + void reset(); + + void handleInterrupt(twai_message_t message); + + uint8_t readRegister(uint8_t address); + void modifyRegister(uint8_t address, uint8_t mask, uint8_t value); + void writeRegister(uint8_t address, uint8_t value); + + [[noreturn]] static void receive_task(void*); + +private: + gpio_num_t _rxPin; + gpio_num_t _txPin; + bool _loopback; + intr_handle_t _intrHandle; +}; + +extern ESP32S3Class CAN; + +#endif + +#endif diff --git a/src/ESP32SJA1000.cpp b/src/ESP32SJA1000.cpp index 3c56fba..ef3df59 100644 --- a/src/ESP32SJA1000.cpp +++ b/src/ESP32SJA1000.cpp @@ -1,7 +1,7 @@ // Copyright (c) Sandeep Mistry. All rights reserved. // Licensed under the MIT license. See LICENSE file in the project root for full license information. -#ifdef ARDUINO_ARCH_ESP32 +#if 0// def ARDUINO_ARCH_ESP32 #include "esp_intr.h" #include "soc/dport_reg.h" diff --git a/src/ESP32SJA1000.h b/src/ESP32SJA1000.h index b83cddc..75bd332 100644 --- a/src/ESP32SJA1000.h +++ b/src/ESP32SJA1000.h @@ -1,7 +1,7 @@ // Copyright (c) Sandeep Mistry. All rights reserved. // Licensed under the MIT license. See LICENSE file in the project root for full license information. -#ifdef ARDUINO_ARCH_ESP32 +#if 0//def ARDUINO_ARCH_ESP32 #ifndef ESP32_SJA1000_H #define ESP32_SJA1000_H From ac38c30ec0b3f746c3b1fcf2b099855249ff0608 Mon Sep 17 00:00:00 2001 From: Eliyahu Gluschove-Koppel Date: Thu, 8 Jun 2023 15:23:07 +0100 Subject: [PATCH 2/6] Fix initialisation of ESP32S3 CAN controller class --- src/ESP32S3.cpp | 17 +++-------------- src/ESP32S3.h | 5 ----- 2 files changed, 3 insertions(+), 19 deletions(-) diff --git a/src/ESP32S3.cpp b/src/ESP32S3.cpp index 61c69ad..384f81e 100644 --- a/src/ESP32S3.cpp +++ b/src/ESP32S3.cpp @@ -14,6 +14,9 @@ ESP32S3Class::ESP32S3Class() : CANControllerClass() { + _txPin = GPIO_NUM_5; + _rxPin = GPIO_NUM_4; + _loopback = false; } ESP32S3Class::~ESP32S3Class() {} @@ -160,20 +163,6 @@ void ESP32S3Class::handleInterrupt(twai_message_t message) _onReceive(_rxLength); } -uint8_t ESP32S3Class::readRegister(uint8_t address) -{ - return 0; -} - -void ESP32S3Class::modifyRegister(uint8_t address, uint8_t mask, uint8_t value) -{ -} - -void ESP32S3Class::writeRegister(uint8_t address, uint8_t value) -{ - -} - [[noreturn]] void ESP32S3Class::receive_task(void*) { Serial.println("can recv started"); diff --git a/src/ESP32S3.h b/src/ESP32S3.h index 46ad718..e9dc985 100644 --- a/src/ESP32S3.h +++ b/src/ESP32S3.h @@ -46,17 +46,12 @@ class ESP32S3Class : public CANControllerClass { void handleInterrupt(twai_message_t message); - uint8_t readRegister(uint8_t address); - void modifyRegister(uint8_t address, uint8_t mask, uint8_t value); - void writeRegister(uint8_t address, uint8_t value); - [[noreturn]] static void receive_task(void*); private: gpio_num_t _rxPin; gpio_num_t _txPin; bool _loopback; - intr_handle_t _intrHandle; }; extern ESP32S3Class CAN; From 6e270d997021842de47bb925ce201476b9b7f9a4 Mon Sep 17 00:00:00 2001 From: Eliyahu Gluschove-Koppel Date: Thu, 8 Jun 2023 15:25:02 +0100 Subject: [PATCH 3/6] Update README with not working features --- README.md | 2 ++ src/ESP32S3.cpp | 5 ----- src/ESP32S3.h | 2 -- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index b612ee4..9a6dffc 100644 --- a/README.md +++ b/README.md @@ -41,6 +41,8 @@ Requires an external 3.3V CAN transceiver, such as a [TI SN65HVD230](http://www. `CTX` and `CRX` pins can be changed by using `CAN.setPins(rx, tx)`. +Loopback mode, sleep, and packet filtering currently do not work. + ## Installation ### Using the Arduino IDE Library Manager diff --git a/src/ESP32S3.cpp b/src/ESP32S3.cpp index 384f81e..7c476d1 100644 --- a/src/ESP32S3.cpp +++ b/src/ESP32S3.cpp @@ -146,11 +146,6 @@ void ESP32S3Class::setPins(int rx, int tx) _txPin = (gpio_num_t)tx; } -void ESP32S3Class::dumpRegisters(Stream& out) -{ - -} - void ESP32S3Class::handleInterrupt(twai_message_t message) { _rxId = message.identifier; diff --git a/src/ESP32S3.h b/src/ESP32S3.h index e9dc985..e7a515f 100644 --- a/src/ESP32S3.h +++ b/src/ESP32S3.h @@ -39,8 +39,6 @@ class ESP32S3Class : public CANControllerClass { void setPins(int rx, int tx); - void dumpRegisters(Stream& out); - private: void reset(); From 3d5343927942e3021360755b23c8429bd2ab45f9 Mon Sep 17 00:00:00 2001 From: Eliyahu Gluschove-Koppel Date: Sat, 17 Jun 2023 20:56:54 +0100 Subject: [PATCH 4/6] Move interrupt handling out of ISR --- src/MCP2515.cpp | 23 ++++++++++++++++------- src/MCP2515.h | 2 ++ 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/MCP2515.cpp b/src/MCP2515.cpp index 14e14be..89d7fbe 100644 --- a/src/MCP2515.cpp +++ b/src/MCP2515.cpp @@ -61,7 +61,8 @@ MCP2515Class::MCP2515Class() : _spiSettings(10E6, MSBFIRST, SPI_MODE0), _csPin(MCP2515_DEFAULT_CS_PIN), _intPin(MCP2515_DEFAULT_INT_PIN), - _clockFrequency(MCP2515_DEFAULT_CLOCK_FREQUENCY) + _clockFrequency(MCP2515_DEFAULT_CLOCK_FREQUENCY), + _pendingInt(false) { } @@ -268,7 +269,7 @@ void MCP2515Class::onReceive(void(*callback)(int)) #ifndef ESP8266 SPI.usingInterrupt(digitalPinToInterrupt(_intPin)); #endif - attachInterrupt(digitalPinToInterrupt(_intPin), MCP2515Class::onInterrupt, LOW); + attachInterrupt(digitalPinToInterrupt(_intPin), MCP2515Class::onInterrupt, FALLING); } else { detachInterrupt(digitalPinToInterrupt(_intPin)); #ifdef SPI_HAS_NOTUSINGINTERRUPT @@ -440,12 +441,20 @@ void MCP2515Class::reset() void MCP2515Class::handleInterrupt() { - if (readRegister(REG_CANINTF) == 0) { - return; - } + _pendingInt = true; +} + +void MCP2515Class::handlePendingInterrupt() +{ + if (_pendingInt) { + _pendingInt = false; + if (readRegister(REG_CANINTF) == 0) { + return; + } - while (parsePacket()) { - _onReceive(available()); + while (parsePacket()) { + _onReceive(available()); + } } } diff --git a/src/MCP2515.h b/src/MCP2515.h index 2f0444f..1c71075 100644 --- a/src/MCP2515.h +++ b/src/MCP2515.h @@ -51,6 +51,7 @@ class MCP2515Class : public CANControllerClass { void setClockFrequency(long clockFrequency); void dumpRegisters(Stream& out); + void handlePendingInterrupt(); private: void reset(); @@ -68,6 +69,7 @@ class MCP2515Class : public CANControllerClass { int _csPin; int _intPin; long _clockFrequency; + volatile bool _pendingInt; }; extern MCP2515Class CAN; From afbbd8514d7b5b1e1bcd9bc61a5bd7434f334d58 Mon Sep 17 00:00:00 2001 From: Eliyahu Gluschove-Koppel Date: Sat, 17 Jun 2023 20:57:22 +0100 Subject: [PATCH 5/6] Remove ESP32 debug prints --- src/ESP32S3.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ESP32S3.cpp b/src/ESP32S3.cpp index 7c476d1..f68c27d 100644 --- a/src/ESP32S3.cpp +++ b/src/ESP32S3.cpp @@ -160,14 +160,11 @@ void ESP32S3Class::handleInterrupt(twai_message_t message) [[noreturn]] void ESP32S3Class::receive_task(void*) { - Serial.println("can recv started"); while (true) { twai_message_t message; - Serial.println("can recv waiting"); if (twai_receive(&message, portMAX_DELAY) != ESP_OK) { continue; } - Serial.println("can recv recv"); CAN.handleInterrupt(message); } } From 94e19976ebe5b5433dcb25c5620d344f4b82cd53 Mon Sep 17 00:00:00 2001 From: Eliyahu Gluschove-Koppel Date: Sun, 18 Jun 2023 18:31:35 +0100 Subject: [PATCH 6/6] Better SPI stuff and use rtos for IRQ handling --- src/MCP2515.cpp | 93 +++++++++++++++++++++++++++++-------------------- src/MCP2515.h | 7 +++- 2 files changed, 61 insertions(+), 39 deletions(-) diff --git a/src/MCP2515.cpp b/src/MCP2515.cpp index 89d7fbe..cc111cf 100644 --- a/src/MCP2515.cpp +++ b/src/MCP2515.cpp @@ -5,6 +5,9 @@ #include "MCP2515.h" +#include +#include + #define REG_BFPCTRL 0x0c #define REG_TXRTSCTRL 0x0d @@ -62,22 +65,31 @@ MCP2515Class::MCP2515Class() : _csPin(MCP2515_DEFAULT_CS_PIN), _intPin(MCP2515_DEFAULT_INT_PIN), _clockFrequency(MCP2515_DEFAULT_CLOCK_FREQUENCY), - _pendingInt(false) + _pendingInt(false), + _theSpi(&SPI) { + } MCP2515Class::~MCP2515Class() { } +void MCP2515Class::setSPI(SPIClass* theSpi) { + _theSpi = theSpi; +} + int MCP2515Class::begin(long baudRate) { CANControllerClass::begin(baudRate); + xTaskCreate(handlePendingInterruptTask, "[LIBRARY] CAN bus", 2048, nullptr, configMAX_PRIORITIES - 5, &_irqTask); + if (!_irqTask) return 0; + pinMode(_csPin, OUTPUT); // start SPI - SPI.begin(); + _theSpi->begin(); reset(); @@ -151,7 +163,7 @@ int MCP2515Class::begin(long baudRate) void MCP2515Class::end() { - SPI.end(); + _theSpi->end(); CANControllerClass::end(); } @@ -267,13 +279,13 @@ void MCP2515Class::onReceive(void(*callback)(int)) if (callback) { #ifndef ESP8266 - SPI.usingInterrupt(digitalPinToInterrupt(_intPin)); + _theSpi->usingInterrupt(digitalPinToInterrupt(_intPin)); #endif attachInterrupt(digitalPinToInterrupt(_intPin), MCP2515Class::onInterrupt, FALLING); } else { detachInterrupt(digitalPinToInterrupt(_intPin)); #ifdef SPI_HAS_NOTUSINGINTERRUPT - SPI.notUsingInterrupt(digitalPinToInterrupt(_intPin)); + _theSpi->notUsingInterrupt(digitalPinToInterrupt(_intPin)); #endif } } @@ -430,11 +442,11 @@ void MCP2515Class::dumpRegisters(Stream& out) void MCP2515Class::reset() { - SPI.beginTransaction(_spiSettings); + _theSpi->beginTransaction(_spiSettings); digitalWrite(_csPin, LOW); - SPI.transfer(0xc0); + _theSpi->transfer(0xc0); digitalWrite(_csPin, HIGH); - SPI.endTransaction(); + _theSpi->endTransaction(); delayMicroseconds(10); } @@ -442,58 +454,47 @@ void MCP2515Class::reset() void MCP2515Class::handleInterrupt() { _pendingInt = true; -} - -void MCP2515Class::handlePendingInterrupt() -{ - if (_pendingInt) { - _pendingInt = false; - if (readRegister(REG_CANINTF) == 0) { - return; - } - - while (parsePacket()) { - _onReceive(available()); - } - } + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + vTaskNotifyGiveFromISR(_irqTask, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); } uint8_t MCP2515Class::readRegister(uint8_t address) { uint8_t value; - SPI.beginTransaction(_spiSettings); + _theSpi->beginTransaction(_spiSettings); digitalWrite(_csPin, LOW); - SPI.transfer(0x03); - SPI.transfer(address); - value = SPI.transfer(0x00); + _theSpi->transfer(0x03); + _theSpi->transfer(address); + value = _theSpi->transfer(0x00); digitalWrite(_csPin, HIGH); - SPI.endTransaction(); + _theSpi->endTransaction(); return value; } void MCP2515Class::modifyRegister(uint8_t address, uint8_t mask, uint8_t value) { - SPI.beginTransaction(_spiSettings); + _theSpi->beginTransaction(_spiSettings); digitalWrite(_csPin, LOW); - SPI.transfer(0x05); - SPI.transfer(address); - SPI.transfer(mask); - SPI.transfer(value); + _theSpi->transfer(0x05); + _theSpi->transfer(address); + _theSpi->transfer(mask); + _theSpi->transfer(value); digitalWrite(_csPin, HIGH); - SPI.endTransaction(); + _theSpi->endTransaction(); } void MCP2515Class::writeRegister(uint8_t address, uint8_t value) { - SPI.beginTransaction(_spiSettings); + _theSpi->beginTransaction(_spiSettings); digitalWrite(_csPin, LOW); - SPI.transfer(0x02); - SPI.transfer(address); - SPI.transfer(value); + _theSpi->transfer(0x02); + _theSpi->transfer(address); + _theSpi->transfer(value); digitalWrite(_csPin, HIGH); - SPI.endTransaction(); + _theSpi->endTransaction(); } void MCP2515Class::onInterrupt() @@ -501,6 +502,22 @@ void MCP2515Class::onInterrupt() CAN.handleInterrupt(); } +void MCP2515Class::handlePendingInterruptTask(void*) { + while (true) { + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); + if (CAN._pendingInt) { + CAN._pendingInt = false; + if (CAN.readRegister(REG_CANINTF) == 0) { + return; + } + + while (CAN.parsePacket()) { + CAN._onReceive(CAN.available()); + } + } + } +} + MCP2515Class CAN; #endif diff --git a/src/MCP2515.h b/src/MCP2515.h index 1c71075..db5b69c 100644 --- a/src/MCP2515.h +++ b/src/MCP2515.h @@ -7,6 +7,8 @@ #define MCP2515_H #include +#include +#include #include "CANController.h" @@ -49,9 +51,9 @@ class MCP2515Class : public CANControllerClass { void setPins(int cs = MCP2515_DEFAULT_CS_PIN, int irq = MCP2515_DEFAULT_INT_PIN); void setSPIFrequency(uint32_t frequency); void setClockFrequency(long clockFrequency); + void setSPI(SPIClass* theSpi); void dumpRegisters(Stream& out); - void handlePendingInterrupt(); private: void reset(); @@ -63,6 +65,7 @@ class MCP2515Class : public CANControllerClass { void writeRegister(uint8_t address, uint8_t value); static void onInterrupt(); + static void handlePendingInterruptTask(void*); private: SPISettings _spiSettings; @@ -70,6 +73,8 @@ class MCP2515Class : public CANControllerClass { int _intPin; long _clockFrequency; volatile bool _pendingInt; + TaskHandle_t _irqTask; + SPIClass *_theSpi; }; extern MCP2515Class CAN;