diff --git a/DCCTimer.h b/DCCTimer.h
index fa2b854..34b2d91 100644
--- a/DCCTimer.h
+++ b/DCCTimer.h
@@ -85,7 +85,11 @@ private:
static int freeMemory();
static volatile int minimum_free_memory;
static const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
+#if defined(ARDUINO_ARCH_STM32) // TODO: PMA temporary hack - assumes 100Mhz F_CPU as STM32 can change frequency
+ static const long CLOCK_CYCLES=(100000000L / 1000000 * DCC_SIGNAL_TIME) >>1;
+#else
static const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;
+#endif
};
diff --git a/DCCTimerSTM32.cpp b/DCCTimerSTM32.cpp
new file mode 100644
index 0000000..4ca20c0
--- /dev/null
+++ b/DCCTimerSTM32.cpp
@@ -0,0 +1,118 @@
+/*
+ * © 2022 Paul M Antoine
+ * © 2021 Mike S
+ * © 2021 Harald Barth
+ * © 2021 Fred Decker
+ * © 2021 Chris Harlow
+ * © 2021 David Cutting
+ * All rights reserved.
+ *
+ * This file is part of Asbelos DCC API
+ *
+ * This is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * It is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with CommandStation. If not, see .
+ */
+
+// ATTENTION: this file only compiles on a STM32 based boards
+// Please refer to DCCTimer.h for general comments about how this class works
+// This is to avoid repetition and duplication.
+#ifdef ARDUINO_ARCH_STM32
+
+#include "FSH.h" //PMA temp debug
+#include "DIAG.h" //PMA temp debug
+#include "DCCTimer.h"
+
+// STM32 doesn't have Serial1 defined by default
+HardwareSerial Serial1(PA10, PA15); // Rx=PA10, Tx=PA9
+
+INTERRUPT_CALLBACK interruptHandler=0;
+// Let's use STM32's timer #1 until disabused of this notion
+HardwareTimer timer(TIM1);
+
+// Timer IRQ handler
+void Timer1_Handler() {
+ interruptHandler();
+}
+
+void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
+ interruptHandler=callback;
+ noInterrupts();
+
+ timer.pause();
+ timer.setPrescaleFactor(1);
+// timer.setOverflow(CLOCK_CYCLES * 2);
+ timer.setOverflow(DCC_SIGNAL_TIME * 2, MICROSEC_FORMAT);
+ timer.attachInterrupt(Timer1_Handler);
+ timer.refresh();
+ timer.resume();
+
+ interrupts();
+}
+
+bool DCCTimer::isPWMPin(byte pin) {
+ //TODO: SAMD whilst this call to digitalPinHasPWM will reveal which pins can do PWM,
+ // there's no support yet for High Accuracy, so for now return false
+ // return digitalPinHasPWM(pin);
+ return false;
+}
+
+void DCCTimer::setPWM(byte pin, bool high) {
+ // TODO: High Accuracy mode is not supported as yet, and may never need to be
+ (void) pin;
+ (void) high;
+}
+
+void DCCTimer::clearPWM() {
+ return;
+}
+
+void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
+ volatile uint32_t *serno1 = (volatile uint32_t *)0x0080A00C;
+ volatile uint32_t *serno2 = (volatile uint32_t *)0x0080A040;
+// volatile uint32_t *serno3 = (volatile uint32_t *)0x0080A044;
+// volatile uint32_t *serno4 = (volatile uint32_t *)0x0080A048;
+
+ volatile uint32_t m1 = *serno1;
+ volatile uint32_t m2 = *serno2;
+ mac[0] = m1 >> 8;
+ mac[1] = m1 >> 0;
+ mac[2] = m2 >> 24;
+ mac[3] = m2 >> 16;
+ mac[4] = m2 >> 8;
+ mac[5] = m2 >> 0;
+}
+
+volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
+
+// Return low memory value...
+int DCCTimer::getMinimumFreeMemory() {
+ noInterrupts(); // Disable interrupts to get volatile value
+ int retval = freeMemory();
+ interrupts();
+ return retval;
+}
+
+extern "C" char* sbrk(int incr);
+
+int DCCTimer::freeMemory() {
+ char top;
+ return (int)(&top - reinterpret_cast(sbrk(0)));
+}
+
+void DCCTimer::reset() {
+ __disable_irq();
+ NVIC_SystemReset();
+ while(true) {};
+}
+
+#endif
\ No newline at end of file
diff --git a/DCCTimerTEENSY.cpp b/DCCTimerTEENSY.cpp
index dc682df..a29f000 100644
--- a/DCCTimerTEENSY.cpp
+++ b/DCCTimerTEENSY.cpp
@@ -88,8 +88,20 @@ void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
}
#endif
+volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
+
+// Return low memory value...
+int DCCTimer::getMinimumFreeMemory() {
+ noInterrupts(); // Disable interrupts to get volatile value
+ int retval = freeMemory();
+ interrupts();
+ return retval;
+}
+
+extern "C" char* sbrk(int incr);
+
#if !defined(__IMXRT1062__)
-static inline int freeMemory() {
+int DCCTimer::freeMemory() {
char top;
return &top - reinterpret_cast(sbrk(0));
}
@@ -110,7 +122,7 @@ static inline int freeMemory() {
#endif
#endif
-static inline int freeMemory() {
+int DCCTimer::freeMemory() {
extern unsigned long _ebss;
extern unsigned long _sdata;
extern unsigned long _estack;
diff --git a/FSH.h b/FSH.h
index c901fb9..f13e597 100644
--- a/FSH.h
+++ b/FSH.h
@@ -47,6 +47,14 @@ typedef char FSH;
#define FLASH
#define strlen_P strlen
#define strcpy_P strcpy
+#elif defined(ARDUINO_ARCH_STM32)
+typedef __FlashStringHelper FSH;
+#define GETFLASH(addr) pgm_read_byte_near(addr)
+#define GETFLASHW(addr) pgm_read_word_near(addr)
+#ifdef FLASH
+ #undef FLASH
+#endif
+#define FLASH PROGMEM
#else
typedef __FlashStringHelper FSH;
#define GETFLASH(addr) pgm_read_byte_near(addr)
diff --git a/I2CManager.cpp b/I2CManager.cpp
index a3ea611..a951a87 100644
--- a/I2CManager.cpp
+++ b/I2CManager.cpp
@@ -1,5 +1,7 @@
/*
- * © 2021, Neil McKechnie. All rights reserved.
+ * © 2022 Paul M Antoine
+ * © 2021, Neil McKechnie
+ * All rights reserved.
*
* This file is part of CommandStation-EX
*
@@ -30,6 +32,9 @@
#elif defined(ARDUINO_ARCH_MEGAAVR)
#include "I2CManager_NonBlocking.h"
#include "I2CManager_Mega4809.h" // NanoEvery/UnoWifi
+#elif defined(ARDUINO_ARCH_SAMD)
+#include "I2CManager_NonBlocking.h"
+#include "I2CManager_SAMD.h" // SAMD21 for now... SAMD51 as well later
#else
#define I2C_USE_WIRE
#include "I2CManager_Wire.h" // Other platforms
diff --git a/I2CManager.h b/I2CManager.h
index 49f3e8f..e99b4f3 100644
--- a/I2CManager.h
+++ b/I2CManager.h
@@ -230,7 +230,11 @@ public:
private:
bool _beginCompleted = false;
bool _clockSpeedFixed = false;
+#if defined(__arm__)
+ uint32_t _clockSpeed = 32000000L; // 3.2MHz max on SAMD and STM32
+#else
uint32_t _clockSpeed = 400000L; // 400kHz max on Arduino.
+#endif
// Finish off request block by waiting for completion and posting status.
uint8_t finishRB(I2CRB *rb, uint8_t status);
diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h
index 112e51e..fbcb98a 100644
--- a/I2CManager_NonBlocking.h
+++ b/I2CManager_NonBlocking.h
@@ -1,5 +1,7 @@
/*
- * © 2021, Neil McKechnie. All rights reserved.
+ * © 2022 Paul M Antoine
+ * © 2021, Neil McKechnie
+ * All rights reserved.
*
* This file is part of CommandStation-EX
*
@@ -23,7 +25,46 @@
#include
#include "I2CManager.h"
#if defined(I2C_USE_INTERRUPTS)
+// atomic.h isn't available on SAMD, and likely others too...
+#if defined(__AVR__)
#include
+#elif defined(__arm__)
+// Helper assembly language functions
+static __inline__ uint8_t my_iSeiRetVal(void)
+{
+ __asm__ __volatile__ ("cpsie i" ::);
+ return 1;
+}
+
+static __inline__ uint8_t my_iCliRetVal(void)
+{
+ __asm__ __volatile__ ("cpsid i" ::);
+ return 1;
+}
+
+static __inline__ void my_iRestore(const uint32_t *__s)
+{
+ uint32_t res = *__s;
+ __asm__ __volatile__ ("MSR primask, %0" : : "r" (res) );
+}
+
+static __inline__ uint32_t my_iGetIReg( void )
+{
+ uint32_t reg;
+ __asm__ __volatile__ ("MRS %0, primask" : "=r" (reg) );
+ return reg;
+}
+// Macros for atomic isolation
+#define MY_ATOMIC_RESTORESTATE uint32_t _sa_saved \
+ __attribute__((__cleanup__(my_iRestore))) = my_iGetIReg()
+
+#define ATOMIC() \
+for ( MY_ATOMIC_RESTORESTATE, _done = my_iCliRetVal(); \
+ _done; _done = 0 )
+
+#define ATOMIC_BLOCK(x) ATOMIC()
+#define ATOMIC_RESTORESTATE
+#endif
#else
#define ATOMIC_BLOCK(x)
#define ATOMIC_RESTORESTATE
diff --git a/I2CManager_SAMD.h b/I2CManager_SAMD.h
index aeabd6c..4dd2790 100644
--- a/I2CManager_SAMD.h
+++ b/I2CManager_SAMD.h
@@ -1,5 +1,7 @@
/*
- * © 2021, Neil McKechnie. All rights reserved.
+ * © 2022 Paul M Antoine
+ * © 2021, Neil McKechnie
+ * All rights reserved.
*
* This file is part of CommandStation-EX
*
@@ -25,31 +27,61 @@
//#include
//#include
+#include
+/***************************************************************************
+ * Interrupt handler.
+ * IRQ handler for SERCOM3 which is the default I2C definition for Arduino Zero
+ * compatible variants such as the Sparkfun SAMD21 Dev Breakout etc.
+ * Later we may wish to allow use of an alternate I2C bus, or more than one I2C
+ * bus on the SAMD architecture
+ ***************************************************************************/
#if defined(I2C_USE_INTERRUPTS) && defined(ARDUINO_SAMD_ZERO)
-// PMA - IRQ handler, based on SERCOM3 being used for I2C, as per Arduino Zero & Sparkfun SAMD21
-// TODO: test
void SERCOM3_Handler() {
I2CManagerClass::handleInterrupt();
}
#endif
+// Assume SERCOM3 for now - default I2C bus on Arduino Zero and variants of same
+Sercom *s = SERCOM3;
+
/***************************************************************************
* Set I2C clock speed register.
***************************************************************************/
-void I2CManagerClass::I2C_setClock(unsigned long i2cClockSpeed) {
- unsigned long temp = ((F_CPU / i2cClockSpeed) - 16) / 2;
- for (uint8_t preScaler = 0; preScaler<=3; preScaler++) {
- if (temp <= 255) {
- TWBR = temp;
- TWSR = (TWSR & 0xfc) | preScaler;
- return;
- } else
- temp /= 4;
+void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
+
+ // Calculate a rise time appropriate to the requested bus speed
+ int t_rise;
+ if (i2cClockSpeed < 200000L) {
+ i2cClockSpeed = 100000L;
+ t_rise = 1000;
+ } else if (i2cClockSpeed < 800000L) {
+ i2cClockSpeed = 400000L;
+ t_rise = 300;
+ } else if (i2cClockSpeed < 1200000L) {
+ i2cClockSpeed = 1000000L;
+ t_rise = 120;
+ } else {
+ i2cClockSpeed = 100000L;
+ t_rise = 1000;
}
- // Set slowest speed ~= 500 bits/sec
- TWBR = 255;
- TWSR |= 0x03;
+
+ // Disable the I2C master mode and wait for sync
+ s->I2CM.CTRLA.bit.ENABLE = 0 ;
+ while (s->I2CM.SYNCBUSY.bit.ENABLE != 0);
+
+ // Calculate baudrate - using a rise time appropriate for the speed
+ s->I2CM.BAUD.bit.BAUD = SystemCoreClock / (2 * i2cClockSpeed) - 5 - (((SystemCoreClock / 1000000) * t_rise) / (2 * 1000));
+
+ // Enable the I2C master mode and wait for sync
+ s->I2CM.CTRLA.bit.ENABLE = 1 ;
+ while (s->I2CM.SYNCBUSY.bit.ENABLE != 0);
+
+ // Setting bus idle mode and wait for sync
+ s->I2CM.STATUS.bit.BUSSTATE = 1 ;
+ while (s->I2CM.SYNCBUSY.bit.SYSOP != 0);
+
+ return;
}
/***************************************************************************
@@ -57,16 +89,58 @@ void I2CManagerClass::I2C_setClock(unsigned long i2cClockSpeed) {
***************************************************************************/
void I2CManagerClass::I2C_init()
{
- // PMA - broadly we do the following
- initialise the clock
- initialise the NVIC
- software reset the I2C for the sercom
- set master mode
- do we need smart mode and quick command??
- configure interrupt handlers
- enable interrupts
- set default baud rate
- set SDA/SCL pins as outputs and enable pullups
+ //Setting clock
+ GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID(GCM_SERCOM3_CORE) | // Generic Clock 0 (SERCOM3)
+ GCLK_CLKCTRL_GEN_GCLK0 | // Generic Clock Generator 0 is source
+ GCLK_CLKCTRL_CLKEN ;
+
+ /* Wait for peripheral clock synchronization */
+ while ( GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY );
+
+ // Software reset the SERCOM
+ s->I2CM.CTRLA.bit.SWRST = 1;
+
+ //Wait both bits Software Reset from CTRLA and SYNCBUSY are equal to 0
+ while(s->I2CM.CTRLA.bit.SWRST || s->I2CM.SYNCBUSY.bit.SWRST);
+
+ // Set master mode and enable SCL Clock Stretch mode (stretch after ACK bit)
+ s->I2CM.CTRLA.reg = SERCOM_I2CM_CTRLA_MODE( I2C_MASTER_OPERATION )/* |
+ SERCOM_I2CM_CTRLA_SCLSM*/ ;
+
+ // Enable Smart mode and Quick Command
+ s->I2CM.CTRLB.reg = SERCOM_I2CM_CTRLB_SMEN | SERCOM_I2CM_CTRLB_QCEN;
+
+#if defined(I2C_USE_INTERRUPTS)
+ // Setting NVIC
+ NVIC_EnableIRQ(SERCOM3_IRQn);
+ NVIC_SetPriority (SERCOM3_IRQn, 0); /* set Priority */
+
+ // Enable all interrupts
+ s->I2CM.INTENSET.reg = SERCOM_I2CM_INTENSET_MB | SERCOM_I2CM_INTENSET_SB | SERCOM_I2CM_INTENSET_ERROR;
+#endif
+
+ // Calculate baudrate and set default rate for now
+ s->I2CM.BAUD.bit.BAUD = SystemCoreClock / ( 2 * I2C_FREQ) - 7 / (2 * 1000);
+
+ // Enable the I2C master mode and wait for sync
+ s->I2CM.CTRLA.bit.ENABLE = 1 ;
+ while (s->I2CM.SYNCBUSY.bit.ENABLE != 0);
+
+ // Setting bus idle mode and wait for sync
+ s->I2CM.STATUS.bit.BUSSTATE = 1 ;
+ while (s->I2CM.SYNCBUSY.bit.SYSOP != 0);
+
+ // Set SDA/SCL pins as outputs and enable pullups, at present we assume these are
+ // the default ones for SERCOM3 (see assumption above)
+ pinPeripheral(PIN_WIRE_SDA, g_APinDescription[PIN_WIRE_SDA].ulPinType);
+ pinPeripheral(PIN_WIRE_SCL, g_APinDescription[PIN_WIRE_SCL].ulPinType);
+
+ // Enable the SCL and SDA pins on the sercom: includes increased driver strength,
+ // pull-up resistors and pin multiplexer
+ PORT->Group[g_APinDescription[PIN_WIRE_SCL].ulPort].PINCFG[g_APinDescription[PIN_WIRE_SCL].ulPin].reg =
+ PORT_PINCFG_DRVSTR | PORT_PINCFG_PULLEN | PORT_PINCFG_PMUXEN;
+ PORT->Group[g_APinDescription[PIN_WIRE_SDA].ulPort].PINCFG[g_APinDescription[PIN_WIRE_SDA].ulPin].reg =
+ PORT_PINCFG_DRVSTR | PORT_PINCFG_PULLEN | PORT_PINCFG_PMUXEN;
}
/***************************************************************************
@@ -75,28 +149,36 @@ void I2CManagerClass::I2C_init()
void I2CManagerClass::I2C_sendStart() {
bytesToSend = currentRequest->writeLen;
bytesToReceive = currentRequest->readLen;
+
// We may have initiated a stop bit before this without waiting for it.
// Wait for stop bit to be sent before sending start.
- while (TWCR & (1<I2CM.STATUS.bit.BUSSTATE == 0x2);
+
+ // If anything to send, initiate write. Otherwise initiate read.
+ if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend))
+ {
+ // Send start and address with read/write flag or'd in
+ s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1) | 1;
+ }
+ else {
+ // Wait while the I2C bus is BUSY
+ while (s->I2CM.STATUS.bit.BUSSTATE != 0x1);
+ s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1ul) | 0;
+ }
}
/***************************************************************************
* Initiate a stop bit for transmission (does not interrupt)
***************************************************************************/
void I2CManagerClass::I2C_sendStop() {
- TWDR = 0xff; // Default condition = SDA released
- TWCR = (1<I2CM.CTRLB.bit.CMD = 3; // Stop condition
}
/***************************************************************************
* Close I2C down
***************************************************************************/
void I2CManagerClass::I2C_close() {
- // disable TWI
I2C_sendStop();
- while (TWCR & (1<writeBuffer + (txCount++));
- else
- TWDR = currentRequest->writeBuffer[txCount++];
- bytesToSend--;
- TWCR = (1< 0) {
- currentRequest->readBuffer[rxCount++] = TWDR;
- bytesToReceive--;
- }
- /* fallthrough */
- case TWI_MRX_ADR_ACK: // SLA+R has been sent and ACK received
- if (bytesToReceive <= 1) {
- TWCR = (1< 0) {
- currentRequest->readBuffer[rxCount++] = TWDR;
- bytesToReceive--;
- }
- TWCR = (1<i2cAddress << 1) | 1; // SLA+R
- else
- TWDR = (currentRequest->i2cAddress << 1) | 0; // SLA+W
- TWCR = (1<I2CM.STATUS.bit.ARBLOST) {
+ // Arbitration lost, restart
+ I2C_sendStart(); // Reinitiate request
+ } else if (s->I2CM.STATUS.bit.BUSERR) {
+ // Bus error
+ state = I2C_STATUS_BUS_ERROR;
+ } else if (s->I2CM.INTFLAG.bit.MB) {
+ // Master write completed
+ if (s->I2CM.STATUS.bit.RXNACK) {
+ // Nacked, send stop.
+ I2C_sendStop();
state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
- break;
- case TWI_ARB_LOST: // Arbitration lost
- // Restart transaction from start.
- I2C_sendStart();
- break;
- case TWI_BUS_ERROR: // Bus error due to an illegal START or STOP condition
- default:
- TWDR = 0xff; // Default condition = SDA released
- TWCR = (1<operation == OPERATION_SEND_P)
+ s->I2CM.DATA.bit.DATA = GETFLASH(currentRequest->writeBuffer + (txCount++));
+ else
+ s->I2CM.DATA.bit.DATA = currentRequest->writeBuffer[txCount++];
+ bytesToSend--;
+ } else if (bytesToReceive) {
+ // Last sent byte acked and no more to send. Send repeated start, address and read bit.
+ s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1) | 1;
+ } else {
+ // No more data to send/receive. Initiate a STOP condition.
+ I2C_sendStop();
+ state = I2C_STATUS_OK; // Done
+ }
+ } else if (s->I2CM.INTFLAG.bit.SB) {
+ // Master read completed without errors
+ if (bytesToReceive) {
+ currentRequest->readBuffer[rxCount++] = s->I2CM.DATA.bit.DATA; // Store received byte
+ bytesToReceive--;
+ } else {
+ // Buffer full, issue nack/stop
+ s->I2CM.CTRLB.bit.ACKACT = 1;
+ I2C_sendStop();
+ state = I2C_STATUS_OK;
+ }
+ if (bytesToReceive) {
+ // PMA - I think Smart Mode means we have nothing to do...
+ // More bytes to receive, issue ack and start another read
+ }
+ else
+ {
+ // Transaction finished, issue NACK and STOP.
+ s->I2CM.CTRLB.bit.ACKACT = 1;
+ I2C_sendStop();
+ state = I2C_STATUS_OK;
+ }
}
}
-#if defined(I2C_USE_INTERRUPTS)
-ISR(TWI_vect) {
- I2CManagerClass::handleInterrupt();
-}
-#endif
-
-#endif /* I2CMANAGER_AVR_H */
+#endif /* I2CMANAGER_SAMD_H */
diff --git a/IO_MCP23008.h b/IO_MCP23008.h
index aa4679b..a411117 100644
--- a/IO_MCP23008.h
+++ b/IO_MCP23008.h
@@ -31,7 +31,7 @@ public:
private:
// Constructor
MCP23008(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
- : GPIOBase((FSH *)F("MCP23008"), firstVpin, min(nPins, 8), I2CAddress, interruptPin) {
+ : GPIOBase((FSH *)F("MCP23008"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin) {
requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
outputBuffer, sizeof(outputBuffer));
diff --git a/IO_PCF8574.h b/IO_PCF8574.h
index 44eccad..f862984 100644
--- a/IO_PCF8574.h
+++ b/IO_PCF8574.h
@@ -48,7 +48,7 @@ public:
private:
PCF8574(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
- : GPIOBase((FSH *)F("PCF8574"), firstVpin, min(nPins, 8), I2CAddress, interruptPin)
+ : GPIOBase((FSH *)F("PCF8574"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin)
{
requestBlock.setReadParams(_I2CAddress, inputBuffer, 1);
}
diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index 0239d1c..620d996 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -268,6 +268,8 @@ void MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & res
(void) type; // avoid compiler warning if diag not used above.
#if defined(ARDUINO_ARCH_SAMD)
PortGroup *port = digitalPinToPort(pin);
+#elif defined(ARDUINO_ARCH_STM32)
+ GPIO_TypeDef *port = digitalPinToPort(pin);
#else
uint8_t port = digitalPinToPort(pin);
#endif
diff --git a/MotorDriver.h b/MotorDriver.h
index 9e32ea0..9e2ff4f 100644
--- a/MotorDriver.h
+++ b/MotorDriver.h
@@ -43,6 +43,20 @@
#if defined(ARDUINO_AVR_UNO)
#define HAVE_PORTB(X) X
#endif
+#if defined(ARDUINO_ARCH_SAMD)
+#define PORTA REG_PORT_OUT0
+#define HAVE_PORTA(X) X
+#define PORTB REG_PORT_OUT1
+#define HAVE_PORTB(X) X
+#endif
+#if defined(ARDUINO_ARCH_STM32)
+#define PORTA GPIOA->ODR
+#define HAVE_PORTA(X) X
+#define PORTB GPIOB->ODR
+#define HAVE_PORTB(X) X
+#define PORTC GPIOC->ODR
+#define HAVE_PORTC(X) X
+#endif
// if macros not defined as pass-through we define
// them here as someting that is valid as a
@@ -63,7 +77,7 @@
#define UNUSED_PIN 127 // inside int8_t
#endif
-#if defined(__IMXRT1062__) || defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_SAMD)
+#if defined(__IMXRT1062__) || defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_SAMD) || defined(ARDUINO_ARCH_STM32)
typedef uint32_t portreg_t;
#else
typedef uint8_t portreg_t;
diff --git a/defines.h b/defines.h
index 23a7993..ba96b4e 100644
--- a/defines.h
+++ b/defines.h
@@ -40,6 +40,7 @@
// figure out if we have enough memory for advanced features
// so define HAS_ENOUGH_MEMORY until proved otherwise.
#define HAS_ENOUGH_MEMORY
+#undef USB_SERIAL // Teensy has this defined by default...
#define USB_SERIAL Serial
#if defined(ARDUINO_AVR_UNO)
@@ -55,16 +56,62 @@
#elif defined(ARDUINO_ARCH_MEGAAVR)
#define ARDUINO_TYPE "MEGAAVR"
#undef HAS_ENOUGH_MEMORY
-#elif defined(ARDUINO_TEENSY32)
-#define ARDUINO_TYPE "TEENSY32"
+#elif defined(ARDUINO_TEENSY31)
+#define ARDUINO_TYPE "TEENSY3132"
+#undef USB_SERIAL
+#define USB_SERIAL SerialUSB
+#ifndef DISABLE_EEPROM
+ #define DISABLE_EEPROM
+#endif
+// Teensy support for native I2C is awaiting development
+#ifndef I2C_NO_INTERRUPTS
+ #define I2C_NO_INTERRUPTS
+#endif
#elif defined(ARDUINO_TEENSY35)
#define ARDUINO_TYPE "TEENSY35"
+#undef USB_SERIAL
+#define USB_SERIAL SerialUSB
+// Teensy support for I2C is awaiting development
+#ifndef DISABLE_EEPROM
+ #define DISABLE_EEPROM
+#endif
+// Teensy support for native I2C is awaiting development
+#ifndef I2C_NO_INTERRUPTS
+ #define I2C_NO_INTERRUPTS
+#endif
#elif defined(ARDUINO_TEENSY36)
#define ARDUINO_TYPE "TEENSY36"
+#undef USB_SERIAL
+#define USB_SERIAL SerialUSB
+#ifndef DISABLE_EEPROM
+ #define DISABLE_EEPROM
+#endif
+// Teensy support for native I2C is awaiting development
+#ifndef I2C_NO_INTERRUPTS
+ #define I2C_NO_INTERRUPTS
+#endif
#elif defined(ARDUINO_TEENSY40)
#define ARDUINO_TYPE "TEENSY40"
+#undef USB_SERIAL
+#define USB_SERIAL SerialUSB
+#ifndef DISABLE_EEPROM
+ #define DISABLE_EEPROM
+#endif
+// Teensy support for native I2C is awaiting development
+#ifndef I2C_NO_INTERRUPTS
+ #define I2C_NO_INTERRUPTS
+#endif
#elif defined(ARDUINO_TEENSY41)
#define ARDUINO_TYPE "TEENSY41"
+#undef USB_SERIAL
+#define USB_SERIAL SerialUSB
+#ifndef DISABLE_EEPROM
+ #define DISABLE_EEPROM
+#endif
+// Teensy support for native I2C is awaiting development
+#ifndef I2C_NO_INTERRUPTS
+ #define I2C_NO_INTERRUPTS
+#endif
#elif defined(ARDUINO_ARCH_ESP8266)
#define ARDUINO_TYPE "ESP8266"
#elif defined(ARDUINO_ARCH_ESP32)
@@ -73,10 +120,17 @@
#define ARDUINO_TYPE "SAMD21"
#undef USB_SERIAL
#define USB_SERIAL SerialUSB
-// SAMD support for I2C is awaiting development
+// SAMD no EEPROM by default
#ifndef DISABLE_EEPROM
#define DISABLE_EEPROM
#endif
+#elif defined(ARDUINO_ARCH_STM32)
+#define ARDUINO_TYPE "STM32"
+// STM32 no EEPROM by default
+#ifndef DISABLE_EEPROM
+ #define DISABLE_EEPROM
+#endif
+// STM32 support for native I2C is awaiting development
#ifndef I2C_NO_INTERRUPTS
#define I2C_NO_INTERRUPTS
#endif
diff --git a/platformio.ini b/platformio.ini
index 0a38cf8..43a4092 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -18,6 +18,12 @@ default_envs =
samd21-dev-usb
samd21-zero-usb
ESP32
+ Nucleo-STM32F411RE
+ Teensy3.2
+ Teensy3.5
+ Teensy3.6
+ Teensy4.0
+ Teensy4.1
src_dir = .
include_dir = .
@@ -164,4 +170,53 @@ platform = espressif32
board = esp32dev
framework = arduino
lib_deps = ${env.lib_deps}
-build_flags = -std=c++17
\ No newline at end of file
+build_flags = -std=c++17
+
+[env:Nucleo-STM32F411RE]
+platform = ststm32
+board = nucleo_f411re
+framework = arduino
+lib_deps = ${env.lib_deps}
+build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
+monitor_speed = 115200
+monitor_echo = yes
+
+[env:Teensy3.2]
+platform = teensy
+board = teensy31
+framework = arduino
+build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
+lib_deps = ${env.lib_deps}
+lib_ignore = NativeEthernet
+
+[env:Teensy3.5]
+platform = teensy
+board = teensy35
+framework = arduino
+build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
+lib_deps = ${env.lib_deps}
+lib_ignore = NativeEthernet
+
+[env:Teensy3.6]
+platform = teensy
+board = teensy36
+framework = arduino
+build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
+lib_deps = ${env.lib_deps}
+lib_ignore = NativeEthernet
+
+[env:Teensy4.0]
+platform = teensy
+board = teensy40
+framework = arduino
+build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
+lib_deps = ${env.lib_deps}
+lib_ignore = NativeEthernet
+
+[env:Teensy4.1]
+platform = teensy
+board = teensy41
+framework = arduino
+build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
+lib_deps = ${env.lib_deps}
+lib_ignore =
\ No newline at end of file