1
0
mirror of https://github.com/DCC-EX/CommandStation-EX.git synced 2025-07-30 19:03:44 +02:00

Compare commits

..

11 Commits

Author SHA1 Message Date
Harald Barth
43538d3b32 smaller code 2021-11-26 19:32:45 +01:00
Harald Barth
8a17965cd2 type and correct include 2021-11-25 19:55:48 +01:00
Harald Barth
3bddeeda3e better long/short addr handling under <R>; configurable long/short border 2021-11-25 00:10:11 +01:00
Neil McKechnie
f05b3d1730 Committing a SHA 2021-11-24 13:02:35 +00:00
Neil McKechnie
a2f8a8ec91 Merge branch 'master' of https://github.com/DCC-EX/CommandStation-EX 2021-11-24 13:00:31 +00:00
Neil McKechnie
746350b846 Update version to 3.2.0 rc5 2021-11-24 12:54:02 +00:00
Neil McKechnie
97f3450621 Simplify OLED driver initialisation.
Simplify the initialisation in the SSD1306Ascii driver, by removing some of the complex structures that were inherited from the library on which it is based.  This should also allow it to compile on the ESP32 platform.
2021-11-24 12:53:03 +00:00
Asbelos
2be3e276f9 Committing a SHA 2021-11-24 12:02:40 +00:00
Asbelos
88fa5ad37c VPIN in RMFT2::doSignal 2021-11-24 12:02:16 +00:00
Neil McKechnie
106fb612dc Committing a SHA 2021-11-21 17:56:29 +00:00
Neil McKechnie
53113e981d Update IO_PCF8574.h
Correct handling of input in immediate mode,
2021-11-21 17:56:06 +00:00
34 changed files with 225 additions and 1372 deletions

View File

@@ -58,9 +58,6 @@ void setup()
// Responsibility 1: Start the usb connection for diagnostics
// This is normally Serial but uses SerialUSB on a SAMD processor
Serial.begin(115200);
#ifdef ESP_DEBUG
Serial.setDebugOutput(true);
#endif
DIAG(F("License GPLv3 fsf.org (c) dcc-ex.com"));
@@ -74,12 +71,9 @@ void setup()
// Start the WiFi interface on a MEGA, Uno cannot currently handle WiFi
// Start Ethernet if it exists
#if WIFI_ON
#ifndef ESP_FAMILY
WifiInterface::setup(WIFI_SERIAL_LINK_SPEED, F(WIFI_SSID), F(WIFI_PASSWORD), F(WIFI_HOSTNAME), IP_PORT, WIFI_CHANNEL);
#else
WifiESP::setup(WIFI_SSID, WIFI_PASSWORD, WIFI_HOSTNAME, IP_PORT, WIFI_CHANNEL);
#endif
#endif // WIFI_ON
#if ETHERNET_ON
EthernetInterface::setup();
#endif // ETHERNET_ON
@@ -118,18 +112,14 @@ void loop()
// Responsibility 1: Handle DCC background processes
// (loco reminders and power checks)
DCC::loop();
// Responsibility 2: handle any incoming commands on USB connection
serialParser.loop(Serial);
// Responsibility 3: Optionally handle any incoming WiFi traffic
#if WIFI_ON
#ifndef ESP_FAMILY
WifiInterface::loop();
#endif
#if defined(ARDUINO_ARCH_ESP8266) // on ESP32 own task
WifiESP::loop();
#endif
#endif //WIFI_ON
#if ETHERNET_ON
EthernetInterface::loop();
#endif
@@ -147,9 +137,7 @@ void loop()
// Report any decrease in memory (will automatically trigger on first call)
static int ramLowWatermark = __INT_MAX__; // replaced on first loop
#ifdef ESP_FAMILY
updateMinimumFreeMemory(128);
#endif
int freeNow = minimumFreeMemory();
if (freeNow < ramLowWatermark)
{

38
DCC.cpp
View File

@@ -84,7 +84,7 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode) {
uint8_t nB = 0;
// DIAG(F("setSpeedInternal %d %x"),cab,speedCode);
if (cab > 127)
if (cab > HIGHEST_SHORT_ADDR)
b[nB++] = highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++] = lowByte(cab);
@@ -124,7 +124,7 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) {
byte b[4];
byte nB = 0;
if (cab > 127)
if (cab > HIGHEST_SHORT_ADDR)
b[nB++] = highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++] = lowByte(cab);
if (byte1!=0) b[nB++] = byte1;
@@ -153,7 +153,7 @@ void DCC::setFn( int cab, int16_t functionNumber, bool on) {
//non reminding advanced binary bit set
byte b[5];
byte nB = 0;
if (cab > 127)
if (cab > HIGHEST_SHORT_ADDR)
b[nB++] = highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++] = lowByte(cab);
if (functionNumber <= 127) {
@@ -262,7 +262,7 @@ void DCC::setAccessory(int address, byte number, bool activate) {
void DCC::writeCVByteMain(int cab, int cv, byte bValue) {
byte b[5];
byte nB = 0;
if (cab > 127)
if (cab > HIGHEST_SHORT_ADDR)
b[nB++] = highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++] = lowByte(cab);
@@ -283,7 +283,7 @@ void DCC::writeCVBitMain(int cab, int cv, byte bNum, bool bValue) {
bValue = bValue % 2;
bNum = bNum % 8;
if (cab > 127)
if (cab > HIGHEST_SHORT_ADDR)
b[nB++] = highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++] = lowByte(cab);
@@ -311,14 +311,14 @@ const ackOp FLASH WRITE_BIT0_PROG[] = {
W0,WACK,
V0, WACK, // validate bit is 0
ITC1, // if acked, callback(1)
CALLFAIL // callback (-1)
FAIL // callback (-1)
};
const ackOp FLASH WRITE_BIT1_PROG[] = {
BASELINE,
W1,WACK,
V1, WACK, // validate bit is 1
ITC1, // if acked, callback(1)
CALLFAIL // callback (-1)
FAIL // callback (-1)
};
const ackOp FLASH VERIFY_BIT0_PROG[] = {
@@ -327,7 +327,7 @@ const ackOp FLASH VERIFY_BIT0_PROG[] = {
ITC0, // if acked, callback(0)
V1, WACK, // validate bit is 1
ITC1,
CALLFAIL // callback (-1)
FAIL // callback (-1)
};
const ackOp FLASH VERIFY_BIT1_PROG[] = {
BASELINE,
@@ -335,7 +335,7 @@ const ackOp FLASH VERIFY_BIT1_PROG[] = {
ITC1, // if acked, callback(1)
V0, WACK,
ITC0,
CALLFAIL // callback (-1)
FAIL // callback (-1)
};
const ackOp FLASH READ_BIT_PROG[] = {
@@ -344,7 +344,7 @@ const ackOp FLASH READ_BIT_PROG[] = {
ITC1, // if acked, callback(1)
V0, WACK, // validate bit is zero
ITC0, // if acked callback 0
CALLFAIL // bit not readable
FAIL // bit not readable
};
const ackOp FLASH WRITE_BYTE_PROG[] = {
@@ -352,7 +352,7 @@ const ackOp FLASH WRITE_BYTE_PROG[] = {
WB,WACK,ITC1, // Write and callback(1) if ACK
// handle decoders that dont ack a write
VB,WACK,ITC1, // validate byte and callback(1) if correct
CALLFAIL // callback (-1)
FAIL // callback (-1)
};
const ackOp FLASH VERIFY_BYTE_PROG[] = {
@@ -378,7 +378,7 @@ const ackOp FLASH VERIFY_BYTE_PROG[] = {
V0, WACK, MERGE,
V0, WACK, MERGE,
VB, WACK, ITCBV, // verify merged byte and return it if acked ok - with retry report
CALLFAIL };
FAIL };
const ackOp FLASH READ_CV_PROG[] = {
@@ -401,7 +401,7 @@ const ackOp FLASH READ_CV_PROG[] = {
V0, WACK, MERGE,
V0, WACK, MERGE,
VB, WACK, ITCB, // verify merged byte and return it if acked ok
CALLFAIL }; // verification failed
FAIL }; // verification failed
const ackOp FLASH LOCO_ID_PROG[] = {
@@ -467,7 +467,7 @@ const ackOp FLASH LOCO_ID_PROG[] = {
V0, WACK, MERGE,
V0, WACK, MERGE,
VB, WACK, ITCB, // verify merged byte and callback
CALLFAIL
FAIL
};
const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
@@ -484,7 +484,7 @@ const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
SETBYTEL, // low byte of word
WB,WACK, // some decoders don't ACK writes
VB,WACK,ITCB,
CALLFAIL
FAIL
};
const ackOp FLASH LONG_LOCO_ID_PROG[] = {
@@ -508,7 +508,7 @@ const ackOp FLASH LONG_LOCO_ID_PROG[] = {
SETBYTEL, // low byte of word
WB,WACK,
VB,WACK,ITC1, // callback(1) means Ok
CALLFAIL
FAIL
};
void DCC::writeCVByte(int16_t cv, byte byteValue, ACK_CALLBACK callback) {
@@ -548,7 +548,7 @@ void DCC::setLocoId(int id,ACK_CALLBACK callback) {
callback(-1);
return;
}
if (id<=127)
if (id<=HIGHEST_SHORT_ADDR)
ackManagerSetup(id, SHORT_LOCO_ID_PROG, callback);
else
ackManagerSetup(id | 0xc000,LONG_LOCO_ID_PROG, callback);
@@ -857,7 +857,7 @@ void DCC::ackManagerLoop() {
}
break;
case CALLFAIL: // callback(-1)
case FAIL: // callback(-1)
callback(-1);
return;
@@ -906,7 +906,7 @@ void DCC::ackManagerLoop() {
case COMBINELOCOID:
// ackManagerStash is cv17, ackManagerByte is CV 18
callback( ackManagerByte + ((ackManagerStash - 192) << 8));
callback( LONG_ADDR_MARKER | ( ackManagerByte + ((ackManagerStash - 192) << 8)));
return;
case ITSKIP:

16
DCC.h
View File

@@ -23,6 +23,16 @@
#include "MotorDrivers.h"
#include "FSH.h"
#include "defines.h"
#ifndef HIGHEST_SHORT_ADDR
#define HIGHEST_SHORT_ADDR 127
#else
#if HIGHEST_SHORT_ADDR > 127
#error short addr greater than 127 does not make sense
#endif
#endif
const uint16_t LONG_ADDR_MARKER = 0x4000;
typedef void (*ACK_CALLBACK)(int16_t result);
enum ackOp : byte
@@ -41,7 +51,7 @@ enum ackOp : byte
ITCBV, // If True callback(byte) - end of Verify Byte
ITCB7, // If True callback(byte &0x7F)
NAKFAIL, // if false callback(-1)
CALLFAIL, // callback(-1)
FAIL, // callback(-1)
BIV, // Set ackManagerByte to initial value for Verify retry
STARTMERGE, // Clear bit and byte settings ready for merge pass
MERGE, // Merge previous wack response with byte value and decrement bit number (use for readimng CV bytes)
@@ -207,10 +217,6 @@ private:
#define ARDUINO_TYPE "TEENSY40"
#elif defined(ARDUINO_TEENSY41)
#define ARDUINO_TYPE "TEENSY41"
#elif defined(ARDUINO_ARCH_ESP8266)
#define ARDUINO_TYPE "ESP8266"
#elif defined(ARDUINO_ARCH_ESP32)
#define ARDUINO_TYPE "ESP32"
#else
#error CANNOT COMPILE - DCC++ EX ONLY WORKS WITH AN ARDUINO UNO, NANO 328, OR ARDUINO MEGA 1280/2560
#endif

11
DCCEX.h
View File

@@ -30,13 +30,7 @@
#include "DIAG.h"
#include "DCCEXParser.h"
#include "version.h"
#if defined(ARDUINO_ARCH_ESP8266)
#include "WifiESP8266.h"
#elif defined(ARDUINO_ARCH_ESP32)
#include "WifiESP32.h"
#else
#include "WifiInterface.h"
#endif
#if ETHERNET_ON == true
#include "EthernetInterface.h"
#endif
@@ -49,10 +43,5 @@
#include "Outputs.h"
#include "RMFT.h"
// not yet in this branch
//#if __has_include ( "myAutomation.h")
// #include "RMFT.h"
// #define RMFT_ACTIVE
//#endif
#endif

View File

@@ -17,10 +17,9 @@
* You should have received a copy of the GNU General Public License
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
*/
#include "DCC.h" // includes "Motordriver.h" and <Arduino.h>
#include "defines.h"
#include "StringFormatter.h"
#include "DCCEXParser.h"
#include "DCC.h"
#include "DCCWaveform.h"
#include "Turnouts.h"
#include "Outputs.h"
@@ -32,9 +31,7 @@
#include "EEStore.h"
#include "DIAG.h"
#ifndef ESP_FAMILY
#include <avr/wdt.h>
#endif
////////////////////////////////////////////////////////////////////////////////
//
@@ -863,12 +860,8 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
case HASH_KEYWORD_RESET:
{
#ifndef ESP_FAMILY
wdt_enable( WDTO_15MS); // set Arduino watchdog timer for 15ms
delay(50); // wait for the prescaler time to expire
#else
/* XXX do right thing to reboot */
#endif
wdt_enable( WDTO_15MS); // set Arduino watchdog timer for 15ms
delay(50); // wait for the prescaller time to expire
break; // and <X> if we didnt restart
}
@@ -965,10 +958,21 @@ void DCCEXParser::callback_R(int16_t result)
commitAsyncReplyStream();
}
void DCCEXParser::callback_Rloco(int16_t result)
{
StringFormatter::send(getAsyncReplyStream(), F("<r %d>\n"), result);
commitAsyncReplyStream();
void DCCEXParser::callback_Rloco(int16_t result) {
const FSH * detail;
if (result<=0) {
detail=F("<r ERROR %d>\n");
} else {
bool longAddr=result & LONG_ADDR_MARKER; //long addr
if (longAddr)
result = result^LONG_ADDR_MARKER;
if (longAddr && result <= HIGHEST_SHORT_ADDR)
detail=F("<r LONG %d UNSUPPORTED>\n");
else
detail=F("<r %d>\n");
}
StringFormatter::send(getAsyncReplyStream(), detail, result);
commitAsyncReplyStream();
}
void DCCEXParser::callback_Wloco(int16_t result)

View File

@@ -1,168 +0,0 @@
/*
* © 2021, Harald Barth.
*
* This file is part of DCC-EX
*
* 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 <https://www.gnu.org/licenses/>.
*/
#include "config.h"
#include "defines.h"
#include "DIAG.h"
#include "DCCRMT.h"
#include "DCCWaveform.h" // for MAX_PACKET_SIZE
#define DATA_LEN(X) ((X)*9+1) // Each byte has one bit extra and we have one EOF marker
#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(4,2,0)
#error wrong IDF version
#endif
void setDCCBit1(rmt_item32_t* item) {
item->level0 = 1;
item->duration0 = DCC_1_HALFPERIOD;
item->level1 = 0;
item->duration1 = DCC_1_HALFPERIOD;
}
void setDCCBit0(rmt_item32_t* item) {
item->level0 = 1;
item->duration0 = DCC_0_HALFPERIOD;
item->level1 = 0;
item->duration1 = DCC_0_HALFPERIOD;
}
// special long zero to trigger scope
void setDCCBit0Long(rmt_item32_t* item) {
item->level0 = 1;
item->duration0 = DCC_0_HALFPERIOD + DCC_0_HALFPERIOD/10;
item->level1 = 0;
item->duration1 = DCC_0_HALFPERIOD + DCC_0_HALFPERIOD/10;
}
void setEOT(rmt_item32_t* item) {
item->val = 0;
}
void IRAM_ATTR interrupt(rmt_channel_t channel, void *t) {
RMTPin *tt = (RMTPin *)t;
tt->RMTinterrupt();
}
RMTPin::RMTPin(byte pin, byte ch, byte plen) {
// preamble
preambleLen = plen+2; // plen 1 bits, one 0 bit and one EOF marker
preamble = (rmt_item32_t*)malloc(preambleLen*sizeof(rmt_item32_t));
for (byte n=0; n<plen; n++)
setDCCBit1(preamble + n); // preamble bits
#ifdef SCOPE
setDCCBit0Long(preamble + plen); // start of packet 0 bit long version
#else
setDCCBit0(preamble + plen); // start of packet 0 bit normal version
#endif
setEOT(preamble + plen + 1); // EOT marker
// idle
idleLen = 28;
idle = (rmt_item32_t*)malloc(idleLen*sizeof(rmt_item32_t));
for (byte n=0; n<8; n++) // 0 to 7
setDCCBit1(idle + n);
for (byte n=8; n<18; n++) // 8, 9 to 16, 17
setDCCBit0(idle + n);
for (byte n=18; n<26; n++) // 18 to 25
setDCCBit1(idle + n);
setDCCBit1(idle + 26); // end bit
setEOT(idle + 27); // EOT marker
// data: max packet size today is 5 + checksum
maxDataLen = DATA_LEN(MAX_PACKET_SIZE);
data = (rmt_item32_t*)malloc(maxDataLen*sizeof(rmt_item32_t));
rmt_config_t config;
// Configure the RMT channel for TX
bzero(&config, sizeof(rmt_config_t));
config.rmt_mode = RMT_MODE_TX;
config.channel = channel = (rmt_channel_t)ch;
config.clk_div = RMT_CLOCK_DIVIDER;
config.gpio_num = (gpio_num_t)pin;
config.mem_block_num = 2; // With longest DCC packet 11 inc checksum (future expansion)
// number of bits needed is 22preamble + start +
// 11*9 + extrazero + EOT = 124
// 2 mem block of 64 RMT items should be enough
ESP_ERROR_CHECK(rmt_config(&config));
// NOTE: ESP_INTR_FLAG_IRAM is *NOT* included in this bitmask
ESP_ERROR_CHECK(rmt_driver_install(config.channel, 0, ESP_INTR_FLAG_LOWMED|ESP_INTR_FLAG_SHARED));
DIAG(F("Register interrupt on core %d"), xPortGetCoreID());
ESP_ERROR_CHECK(rmt_set_tx_loop_mode(channel, true));
rmt_register_tx_end_callback(interrupt, this);
rmt_set_tx_intr_en(channel, true);
DIAG(F("Starting channel %d signal generator"), config.channel);
// send one bit to kickstart the signal, remaining data will come from the
// packet queue. We intentionally do not wait for the RMT TX complete here.
//rmt_write_items(channel, preamble, preambleLen, false);
RMTprefill();
preambleNext = true;
dataReady = false;
RMTinterrupt();
}
void RMTPin::RMTprefill() {
rmt_fill_tx_items(channel, preamble, preambleLen, 0);
rmt_fill_tx_items(channel, idle, idleLen, preambleLen-1);
}
const byte transmitMask[] = {0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
bool RMTPin::RMTfillData(const byte buffer[], byte byteCount, byte repeatCount=1) {
if (dataReady == true || dataRepeat > 0) // we have still old work to do
return false;
if (DATA_LEN(byteCount) > maxDataLen) // this would overun our allocated memory for data
return false; // something very broken, can not convert packet
// convert bytes to RMT stream of "bits"
byte bitcounter = 0;
for(byte n=0; n<byteCount; n++) {
for(byte bit=0; bit<8; bit++) {
if (buffer[n] & transmitMask[bit])
setDCCBit1(data + bitcounter++);
else
setDCCBit0(data + bitcounter++);
}
setDCCBit0(data + bitcounter++); // zero at end of each byte
}
setDCCBit1(data + bitcounter-1); // overwrite previous zero bit with one bit
setEOT(data + bitcounter++); // EOT marker
dataLen = bitcounter;
dataReady = true;
dataRepeat = repeatCount;
return true;
}
void IRAM_ATTR RMTPin::RMTinterrupt() {
//no rmt_tx_start(channel,true) as we run in loop mode
//preamble is always loaded at beginning of buffer
if (dataReady) { // if we have new data, fill while preamble is running
rmt_fill_tx_items(channel, data, dataLen, preambleLen-1);
dataReady = false;
}
if (dataRepeat > 0) // if a repeat count was specified, work on that
dataRepeat--;
return;
}

View File

@@ -1,57 +0,0 @@
/*
* © 2021, Harald Barth.
*
* This file is part of DCC-EX
*
* 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 <https://www.gnu.org/licenses/>.
*/
#pragma once
#include <Arduino.h>
#include "driver/rmt.h"
#include "soc/rmt_reg.h"
#include "soc/rmt_struct.h"
// make calculations easy and set up for microseconds
#define RMT_CLOCK_DIVIDER 80
#define DCC_1_HALFPERIOD 58 //4640 // 1 / 80000000 * 4640 = 58us
#define DCC_0_HALFPERIOD 100 //8000
class RMTPin {
public:
RMTPin(byte pin, byte ch, byte plen);
void IRAM_ATTR RMTinterrupt();
void RMTprefill();
bool RMTfillData(const byte buffer[], byte byteCount, byte repeatCount);
static RMTPin mainRMTPin;
static RMTPin progRMTPin;
private:
rmt_channel_t channel;
// 3 types of data to send, preamble and then idle or data
// if this is prog track, idle will contain reset instead
rmt_item32_t *idle;
byte idleLen;
rmt_item32_t *preamble;
byte preambleLen;
rmt_item32_t *data;
byte dataLen;
byte maxDataLen;
// flags
volatile bool preambleNext = true; // alternate between preamble and content
volatile bool dataReady = false; // do we have real data available or send idle
volatile byte dataRepeat = 0;
};

View File

@@ -44,7 +44,7 @@
#include "DCCTimer.h"
const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME);
const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;
INTERRUPT_CALLBACK interruptHandler=0;
@@ -53,11 +53,11 @@ INTERRUPT_CALLBACK interruptHandler=0;
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
interruptHandler=callback;
noInterrupts();
noInterrupts();
ADC0.CTRLC = (ADC0.CTRLC & 0b00110000) | 0b01000011; // speed up analogRead sample time
TCB0.CTRLB = TCB_CNTMODE_INT_gc & ~TCB_CCMPEN_bm; // timer compare mode with output disabled
TCB0.CTRLA = TCB_CLKSEL_CLKDIV2_gc; // 8 MHz ~ 0.125 us
TCB0.CCMP = (CLOCK_CYCLES>>1) -1; // 1 tick less for timer reset
TCB0.CCMP = CLOCK_CYCLES -1; // 1 tick less for timer reset
TCB0.INTFLAGS = TCB_CAPT_bm; // clear interrupt request flag
TCB0.INTCTRL = TCB_CAPT_bm; // Enable the interrupt
TCB0.CNT = 0;
@@ -146,50 +146,6 @@ void DCCTimer::read(uint8_t word, uint8_t *mac, uint8_t offset) {
}
#endif
#elif defined(ARDUINO_ARCH_ESP8266)
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
interruptHandler=callback;
timer1_disable();
// There seem to be differnt ways to attach interrupt handler
// ETS_FRC_TIMER1_INTR_ATTACH(NULL, NULL);
// ETS_FRC_TIMER1_NMI_INTR_ATTACH(interruptHandler);
// Let us choose the one from the API
timer1_attachInterrupt(interruptHandler);
// not exactly sure of order:
timer1_enable(TIM_DIV1, TIM_EDGE, TIM_LOOP);
timer1_write(CLOCK_CYCLES);
}
// We do not support to use PWM to make the Waveform on ESP
bool IRAM_ATTR DCCTimer::isPWMPin(byte pin) {
return false;
}
void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
}
#elif defined(ARDUINO_ARCH_ESP32)
// https://www.visualmicro.com/page/Timer-Interrupts-Explained.aspx
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
interruptHandler = callback;
hw_timer_t *timer = NULL;
timer = timerBegin(0, 2, true); // prescaler can be 2 to 65536 so choose 2
timerAttachInterrupt(timer, interruptHandler, true);
timerAlarmWrite(timer, CLOCK_CYCLES / 6, true); // divide by prescaler*3 (Clockbase is 80Mhz and not F_CPU 240Mhz)
timerAlarmEnable(timer);
}
// We do not support to use PWM to make the Waveform on ESP
bool IRAM_ATTR DCCTimer::isPWMPin(byte pin) {
return false;
}
void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
}
#else
// Arduino nano, uno, mega etc
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
@@ -203,10 +159,10 @@ void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
interruptHandler=callback;
noInterrupts();
noInterrupts();
ADCSRA = (ADCSRA & 0b11111000) | 0b00000100; // speed up analogRead sample time
TCCR1A = 0;
ICR1 = CLOCK_CYCLES>>1;
ICR1 = CLOCK_CYCLES;
TCNT1 = 0;
TCCR1B = _BV(WGM13) | _BV(CS10); // Mode 8, clock select 1
TIMSK1 = _BV(TOIE1); // Enable Software interrupt

View File

@@ -37,14 +37,4 @@ class DCCTimer {
private:
};
#if defined(ARDUINO_ARCH_ESP32)
extern portMUX_TYPE timerMux;
#endif
#if !(defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_ESP8266))
#ifndef IRAM_ATTR
#define IRAM_ATTR
#endif
#endif
#endif //DCCTimer.h

View File

@@ -20,7 +20,6 @@
#include <Arduino.h>
#include "defines.h"
#include "DCCWaveform.h"
#include "DCCTimer.h"
#include "DIAG.h"
@@ -37,9 +36,6 @@ volatile uint8_t DCCWaveform::numAckSamples=0;
uint8_t DCCWaveform::trailingEdgeCounter=0;
void DCCWaveform::begin(MotorDriver * mainDriver, MotorDriver * progDriver) {
mainTrack.rmtPin = new RMTPin(21, 0, PREAMBLE_BITS_MAIN);
mainTrack.motorDriver=mainDriver;
progTrack.motorDriver=progDriver;
progTripValue = progDriver->mA2raw(TRIP_CURRENT_PROG); // need only calculate once hence static
@@ -55,63 +51,33 @@ void DCCWaveform::begin(MotorDriver * mainDriver, MotorDriver * progDriver) {
DCCTimer::begin(DCCWaveform::interruptHandler);
}
#ifdef SLOW_ANALOG_READ
// Flag to hold if we need to run ack checking in loop
volatile bool ackflag = 0;
#endif
void IRAM_ATTR DCCWaveform::loop(bool ackManagerActive) {
if (mainTrack.packetPendingRMT) {
mainTrack.rmtPin->RMTfillData(mainTrack.pendingPacket, mainTrack.pendingLength, mainTrack.pendingRepeats);
mainTrack.packetPendingRMT=false;
// sentResetsSincePacket = 0 // later when progtrack
}
#ifdef SLOW_ANALOG_READ
if (ackflag) {
progTrack.checkAck();
// reset flag AFTER check is done
portENTER_CRITICAL(&timerMux);
ackflag = 0;
portEXIT_CRITICAL(&timerMux);
} else {
progTrack.checkPowerOverload(ackManagerActive);
}
#else
progTrack.checkPowerOverload(ackManagerActive);
#endif
void DCCWaveform::loop(bool ackManagerActive) {
mainTrack.checkPowerOverload(false);
progTrack.checkPowerOverload(ackManagerActive);
}
#pragma GCC push_options
#pragma GCC optimize ("-O3")
void IRAM_ATTR DCCWaveform::interruptHandler() {
void DCCWaveform::interruptHandler() {
// call the timer edge sensitive actions for progtrack and maintrack
// member functions would be cleaner but have more overhead
byte sigMain=signalTransform[mainTrack.state];
byte sigProg=progTrackSyncMain? sigMain : signalTransform[progTrack.state];
// Set the signal state for both tracks
mainTrack.motorDriver->setSignal(sigMain);
progTrack.motorDriver->setSignal(sigProg);
// Move on in the state engine
mainTrack.state=stateTransform[mainTrack.state];
progTrack.state=stateTransform[progTrack.state];
// WAVE_PENDING means we dont yet know what the next bit is
if (mainTrack.state==WAVE_PENDING)
mainTrack.interrupt2();
if (progTrack.state==WAVE_PENDING)
progTrack.interrupt2();
#ifdef SLOW_ANALOG_READ
else if (progTrack.ackPending && ackflag == 0) { // We need AND we are not already checking
portENTER_CRITICAL(&timerMux);
ackflag = 1;
portEXIT_CRITICAL(&timerMux);
}
#else
else if (progTrack.ackPending)
progTrack.checkAck();
#endif
if (mainTrack.state==WAVE_PENDING) mainTrack.interrupt2();
if (progTrack.state==WAVE_PENDING) progTrack.interrupt2();
else if (progTrack.ackPending) progTrack.checkAck();
}
#pragma GCC push_options
@@ -128,7 +94,6 @@ const byte bitMask[] = {0x00, 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
DCCWaveform::DCCWaveform( byte preambleBits, bool isMain) {
isMainTrack = isMain;
packetPending = false;
packetPendingRMT = false;
memcpy(transmitPacket, idlePacket, sizeof(idlePacket));
state = WAVE_START;
// The +1 below is to allow the preamble generator to create the stop bit
@@ -237,7 +202,7 @@ const bool DCCWaveform::signalTransform[]={
#pragma GCC push_options
#pragma GCC optimize ("-O3")
void IRAM_ATTR DCCWaveform::interrupt2() {
void DCCWaveform::interrupt2() {
// calculate the next bit to be sent:
// set state WAVE_MID_1 for a 1=bit
// or WAVE_HIGH_0 for a 0 bit.
@@ -247,9 +212,7 @@ void IRAM_ATTR DCCWaveform::interrupt2() {
remainingPreambles--;
// Update free memory diagnostic as we don't have anything else to do this time.
// Allow for checkAck and its called functions using 22 bytes more.
#ifndef ESP_FAMILY
updateMinimumFreeMemory(22);
#endif
updateMinimumFreeMemory(22);
return;
}
@@ -274,8 +237,7 @@ void IRAM_ATTR DCCWaveform::interrupt2() {
transmitRepeats--;
}
else if (packetPending) {
portENTER_CRITICAL(&timerMux);
// Copy pending packet to transmit packet
// Copy pending packet to transmit packet
// a fixed length memcpy is faster than a variable length loop for these small lengths
// for (int b = 0; b < pendingLength; b++) transmitPacket[b] = pendingPacket[b];
memcpy( transmitPacket, pendingPacket, sizeof(pendingPacket));
@@ -284,7 +246,6 @@ void IRAM_ATTR DCCWaveform::interrupt2() {
transmitRepeats = pendingRepeats;
packetPending = false;
sentResetsSincePacket=0;
portEXIT_CRITICAL(&timerMux);
}
else {
// Fortunately reset and idle packets are the same length
@@ -303,7 +264,7 @@ void IRAM_ATTR DCCWaveform::interrupt2() {
void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repeats) {
if (byteCount > MAX_PACKET_SIZE) return; // allow for chksum
while (packetPending);
portENTER_CRITICAL(&timerMux);
byte checksum = 0;
for (byte b = 0; b < byteCount; b++) {
checksum ^= buffer[b];
@@ -314,9 +275,7 @@ void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repea
pendingLength = byteCount + 1;
pendingRepeats = repeats;
packetPending = true;
packetPendingRMT = true;
sentResetsSincePacket=0;
portEXIT_CRITICAL(&timerMux);
}
// Operations applicable to PROG track ONLY.
@@ -354,14 +313,14 @@ byte DCCWaveform::getAck() {
#pragma GCC push_options
#pragma GCC optimize ("-O3")
void IRAM_ATTR DCCWaveform::checkAck() {
void DCCWaveform::checkAck() {
// This function operates in interrupt() time so must be fast and can't DIAG
if (sentResetsSincePacket > 6) { //ACK timeout
ackCheckDuration=millis()-ackCheckStart;
ackPending = false;
return;
}
int current=motorDriver->getCurrentRaw();
numAckSamples++;
if (current > ackMaxCurrent) ackMaxCurrent=current;

View File

@@ -20,7 +20,6 @@
#ifndef DCCWaveform_h
#define DCCWaveform_h
#include "DCCRMT.h"
#include "MotorDriver.h"
// Wait times for power management. Unit: milliseconds
@@ -83,7 +82,6 @@ class DCCWaveform {
}
void schedulePacket(const byte buffer[], byte byteCount, byte repeats);
volatile bool packetPending;
volatile bool packetPendingRMT;
volatile byte sentResetsSincePacket;
volatile bool autoPowerOff=false;
void setAckBaseline(); //prog track only
@@ -124,7 +122,6 @@ class DCCWaveform {
bool isMainTrack;
MotorDriver* motorDriver;
RMTPin* rmtPin;
// Transmission controller
byte transmitPacket[MAX_PACKET_SIZE+1]; // +1 for checksum
byte transmitLength;

View File

@@ -1 +1 @@
#define GITHUB_SHA "ESP32-2021120-11:30"
#define GITHUB_SHA "a2f8a8e"

View File

@@ -22,12 +22,6 @@
#include "IO_GPIOBase.h"
#if defined(ARDUINO_ARCH_ESP32) // min seems to be missing from that package
#ifndef min
#define min(a,b) ((a)<(b)?(a):(b))
#endif
#endif
class MCP23008 : public GPIOBase<uint8_t> {
public:
static void create(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1) {
@@ -103,4 +97,4 @@ private:
};
#endif
#endif

View File

@@ -99,4 +99,4 @@ private:
uint8_t inputBuffer[1];
};
#endif
#endif

View File

@@ -20,10 +20,11 @@
#include "MotorDriver.h"
#include "DCCTimer.h"
#include "DIAG.h"
#if defined(ARDUINO_ARCH_ESP32)
#include <driver/adc.h>
#define pinToADC1Channel(X) (adc1_channel_t)(((X) > 35) ? (X)-36 : (X)-28)
#endif
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
#define isHIGH(fastpin) (*fastpin.inout & fastpin.maskHIGH)
#define isLOW(fastpin) (!isHIGH(fastpin))
bool MotorDriver::usePWM=false;
bool MotorDriver::commonFaultPin=false;
@@ -58,15 +59,8 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8
currentPin=current_pin;
if (currentPin!=UNUSED_PIN) {
#if defined(ARDUINO_ARCH_ESP32)
pinMode(currentPin, ANALOG);
adc1_config_width(ADC_WIDTH_BIT_12);
adc1_config_channel_atten(pinToADC1Channel(currentPin),ADC_ATTEN_DB_11);
senseOffset = adc1_get_raw(pinToADC1Channel(currentPin));
#else
pinMode(currentPin, INPUT);
senseOffset=analogRead(currentPin); // value of sensor at zero current
#endif
}
faultPin=fault_pin;
@@ -116,7 +110,7 @@ void MotorDriver::setBrake(bool on) {
else setLOW(fastBrakePin);
}
void IRAM_ATTR MotorDriver::setSignal( bool high) {
void MotorDriver::setSignal( bool high) {
if (usePWM) {
DCCTimer::setPWM(signalPin,high);
}
@@ -161,8 +155,6 @@ int MotorDriver::getCurrentRaw() {
current = analogRead(currentPin)-senseOffset;
overflow_count = 0;
SREG = sreg_backup; /* restore interrupt state */
#elif defined(ARDUINO_ARCH_ESP32)
current = adc1_get_raw(pinToADC1Channel(currentPin))-senseOffset;
#else
current = analogRead(currentPin)-senseOffset;
#endif
@@ -184,8 +176,8 @@ int MotorDriver::mA2raw( unsigned int mA) {
void MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & result) {
// DIAG(F("MotorDriver %S Pin=%d,"),type,pin);
(void) type; // avoid compiler warning if diag not used above.
PORTTYPE port = digitalPinToPort(pin);
(void) type; // avoid compiler warning if diag not used above.
uint8_t port = digitalPinToPort(pin);
if (input)
result.inout = portInputRegister(port);
else

View File

@@ -18,7 +18,6 @@
*/
#ifndef MotorDriver_h
#define MotorDriver_h
#include "defines.h"
#include "FSH.h"
// Virtualised Motor shield 1-track hardware Interface
@@ -27,15 +26,13 @@
#define UNUSED_PIN 127 // inside int8_t
#endif
#if defined(__IMXRT1062__) || defined(ESP_FAMILY)
typedef uint32_t PORTTYPE;
#if defined(__IMXRT1062__)
struct FASTPIN {
volatile uint32_t *inout;
uint32_t maskHIGH;
uint32_t maskLOW;
};
#else
typedef uint8_t PORTTYPE;
struct FASTPIN {
volatile uint8_t *inout;
uint8_t maskHIGH;
@@ -43,30 +40,12 @@ struct FASTPIN {
};
#endif
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
#define isHIGH(fastpin) (*fastpin.inout & fastpin.maskHIGH)
#define isLOW(fastpin) (!isHIGH(fastpin))
class MotorDriver {
public:
MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin,
byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
virtual void setPower( bool on);
void setSignal( bool high);/* {
if (usePWM) {
DCCTimer::setPWM(signalPin,high);
}
if (high) {
setHIGH(fastSignalPin);
if (dualSignal) setLOW(fastSignalPin2);
}
else {
setLOW(fastSignalPin);
if (dualSignal) setHIGH(fastSignalPin2);
}
};*/
virtual void setSignal( bool high);
virtual void setBrake( bool on);
virtual int getCurrentRaw();
virtual unsigned int raw2mA( int raw);

View File

@@ -720,10 +720,10 @@ void RMFT2::kill(const FSH * reason, int operand) {
byte opcode=GET_OPCODE;
if (opcode==OPCODE_ENDEXRAIL) return;
if (opcode!=OPCODE_SIGNAL) continue;
byte redpin=GET_OPERAND(0);
VPIN redpin=GET_OPERAND(0);
if (redpin!=id)continue;
byte amberpin=GET_OPERAND(1);
byte greenpin=GET_OPERAND(2);
VPIN amberpin=GET_OPERAND(1);
VPIN greenpin=GET_OPERAND(2);
// If amberpin is zero, synthesise amber from red+green
IODevice::write(redpin,red || (amber && (amberpin==0)));
if (amberpin) IODevice::write(amberpin,amber);

View File

@@ -226,16 +226,16 @@ const int StringMacroTracker1=__COUNTER__;
// Define macros for route code creation
#define V(val) ((int16_t)(val))&0x00FF,((int16_t)(val)>>8)&0x00FF
#define NOOPERAND 0,0
#define NOP 0,0
#define ALIAS(name,value)
#define EXRAIL const FLASH byte RMFT2::RouteCode[] = {
#define AUTOMATION(id, description) OPCODE_AUTOMATION, V(id),
#define ROUTE(id, description) OPCODE_ROUTE, V(id),
#define SEQUENCE(id) OPCODE_SEQUENCE, V(id),
#define ENDTASK OPCODE_ENDTASK,NOOPERAND,
#define DONE OPCODE_ENDTASK,NOOPERAND,
#define ENDEXRAIL OPCODE_ENDTASK,NOOPERAND,OPCODE_ENDEXRAIL,NOOPERAND };
#define ENDTASK OPCODE_ENDTASK,NOP,
#define DONE OPCODE_ENDTASK,NOP,
#define ENDEXRAIL OPCODE_ENDTASK,NOP,OPCODE_ENDEXRAIL,NOP };
#define AFTER(sensor_id) OPCODE_AT,V(sensor_id),OPCODE_AFTER,V(sensor_id),
#define AMBER(signal_id) OPCODE_AMBER,V(signal_id),
@@ -245,7 +245,7 @@ const int StringMacroTracker1=__COUNTER__;
#define DELAY(ms) OPCODE_DELAY,V(ms/100L),
#define DELAYMINS(mindelay) OPCODE_DELAYMINS,V(mindelay),
#define DELAYRANDOM(mindelay,maxdelay) OPCODE_DELAY,V(mindelay/100L),OPCODE_RANDWAIT,V((maxdelay-mindelay)/100L),
#define ENDIF OPCODE_ENDIF,NOOPERAND,
#define ENDIF OPCODE_ENDIF,NOP,
#define ESTOP OPCODE_SPEED,V(1),
#define FADE(pin,value,ms) OPCODE_SERVO,V(pin),OPCODE_PAD,V(value),OPCODE_PAD,V(PCA9685::ProfileType::UseDuration|PCA9685::NoPowerOff),OPCODE_PAD,V(ms/100L),
#define FOFF(func) OPCODE_FOFF,V(func),
@@ -258,23 +258,23 @@ const int StringMacroTracker1=__COUNTER__;
#define IFNOT(sensor_id) OPCODE_IFNOT,V(sensor_id),
#define IFRANDOM(percent) OPCODE_IFRANDOM,V(percent),
#define IFRESERVE(block) OPCODE_IFRESERVE,V(block),
#define INVERT_DIRECTION OPCODE_INVERT_DIRECTION,NOOPERAND,
#define JOIN OPCODE_JOIN,NOOPERAND,
#define INVERT_DIRECTION OPCODE_INVERT_DIRECTION,NOP,
#define JOIN OPCODE_JOIN,NOP,
#define LATCH(sensor_id) OPCODE_LATCH,V(sensor_id),
#define LCD(id,msg) PRINT(msg)
#define LCN(msg) PRINT(msg)
#define ONCLOSE(turnout_id) OPCODE_ONCLOSE,V(turnout_id),
#define ONTHROW(turnout_id) OPCODE_ONTHROW,V(turnout_id),
#define PAUSE OPCODE_PAUSE,NOOPERAND,
#define PAUSE OPCODE_PAUSE,NOP,
#define POM(cv,value) OPCODE_POM,V(cv),OPCODE_PAD,V(value),
#define POWEROFF OPCODE_POWEROFF,NOOPERAND,
#define POWEROFF OPCODE_POWEROFF,NOP,
#define PRINT(msg) OPCODE_PRINT,V(__COUNTER__ - StringMacroTracker2),
#define READ_LOCO OPCODE_READ_LOCO1,NOOPERAND,OPCODE_READ_LOCO2,NOOPERAND,
#define READ_LOCO OPCODE_READ_LOCO1,NOP,OPCODE_READ_LOCO2,NOP,
#define RED(signal_id) OPCODE_RED,V(signal_id),
#define RESERVE(blockid) OPCODE_RESERVE,V(blockid),
#define RESET(pin) OPCODE_RESET,V(pin),
#define RESUME OPCODE_RESUME,NOOPERAND,
#define RETURN OPCODE_RETURN,NOOPERAND,
#define RESUME OPCODE_RESUME,NOP,
#define RETURN OPCODE_RETURN,NOP,
#define REV(speed) OPCODE_REV,V(speed),
#define SENDLOCO(cab,route) OPCODE_SENDLOCO,V(cab),OPCODE_PAD,V(route),
#define SERIAL(msg) PRINT(msg)
@@ -293,7 +293,7 @@ const int StringMacroTracker1=__COUNTER__;
#define PIN_TURNOUT(id,pin) OPCODE_PINTURNOUT,V(id),OPCODE_PAD,V(pin),
#define THROW(id) OPCODE_THROW,V(id),
#define TURNOUT(id,addr,subaddr) OPCODE_TURNOUT,V(id),OPCODE_PAD,V(addr),OPCODE_PAD,V(subaddr),
#define UNJOIN OPCODE_UNJOIN,NOOPERAND,
#define UNJOIN OPCODE_UNJOIN,NOP,
#define UNLATCH(sensor_id) OPCODE_UNLATCH,V(sensor_id),
#define WAITFOR(pin) OPCODE_WAITFOR,V(pin),
#define XFOFF(cab,func) OPCODE_XFOFF,V(cab),OPCODE_PAD,V(func),

View File

@@ -18,7 +18,6 @@
*/
#include "RingStream.h"
#include "defines.h"
#include "DIAG.h"
RingStream::RingStream( const uint16_t len)
@@ -31,36 +30,27 @@ RingStream::RingStream( const uint16_t len)
_overflow=false;
_mark=0;
_count=0;
#if defined(ARDUINO_ARCH_ESP32)
_bufMux = portMUX_INITIALIZER_UNLOCKED;
#endif
}
size_t RingStream::write(uint8_t b) {
if (_overflow) return 0;
portENTER_CRITICAL(&_bufMux);
_buffer[_pos_write] = b;
++_pos_write;
if (_pos_write==_len) _pos_write=0;
if (_pos_write==_pos_read) {
_overflow=true;
portEXIT_CRITICAL(&_bufMux);
return 0;
}
_count++;
portEXIT_CRITICAL(&_bufMux);
return 1;
}
int RingStream::read(byte advance) {
if ((_pos_read==_pos_write) && !_overflow) return -1; // empty
if (_pos_read == _mark) return -1;
portENTER_CRITICAL(&_bufMux);
int RingStream::read() {
if ((_pos_read==_pos_write) && !_overflow) return -1; // empty
byte b=_buffer[_pos_read];
_pos_read += advance;
_pos_read++;
if (_pos_read==_len) _pos_read=0;
_overflow=false;
portEXIT_CRITICAL(&_bufMux);
return b;
}
@@ -78,14 +68,11 @@ int RingStream::freeSpace() {
// mark start of message with client id (0...9)
void RingStream::mark(uint8_t b) {
//DIAG(F("Mark1 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
portENTER_CRITICAL(&_bufMux);
_mark=_pos_write;
write(b); // client id
write((uint8_t)0); // count MSB placemarker
write((uint8_t)0); // count LSB placemarker
_count=0;
portEXIT_CRITICAL(&_bufMux);
}
// peekTargetMark is used by the parser stash routines to know which client
@@ -94,25 +81,17 @@ uint8_t RingStream::peekTargetMark() {
return _buffer[_mark];
}
void RingStream::info() {
DIAG(F("Info len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
}
bool RingStream::commit() {
//DIAG(F("Commit1 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
portENTER_CRITICAL(&_bufMux);
if (_overflow) {
DIAG(F("RingStream(%d) commit(%d) OVERFLOW"),_len, _count);
// just throw it away
_pos_write=_mark;
_overflow=false;
portEXIT_CRITICAL(&_bufMux);
return false; // commit failed
return false; // commit failed
}
if (_count==0) {
// ignore empty response
_pos_write=_mark;
portEXIT_CRITICAL(&_bufMux);
return true; // true=commit ok
}
// Go back to the _mark and inject the count 1 byte later
@@ -122,8 +101,5 @@ bool RingStream::commit() {
_mark++;
if (_mark==_len) _mark=0;
_buffer[_mark]=lowByte(_count);
_mark=_len+1;
//DIAG(F("Commit2 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
portEXIT_CRITICAL(&_bufMux);
return true; // commit worked
}

View File

@@ -28,17 +28,14 @@ class RingStream : public Print {
virtual size_t write(uint8_t b);
using Print::write;
inline int read() { return read(1); };
inline int peek() { return read(0); };
int read();
int count();
int freeSpace();
void mark(uint8_t b);
bool commit();
uint8_t peekTargetMark();
void info();
private:
int read(byte advance);
int _len;
int _pos_write;
int _pos_read;
@@ -46,9 +43,6 @@ class RingStream : public Print {
int _mark;
int _count;
byte * _buffer;
#if defined(ARDUINO_ARCH_ESP32)
portMUX_TYPE _bufMux;
#endif
};
#endif

View File

@@ -89,28 +89,93 @@ const uint8_t FLASH SSD1306AsciiWire::blankPixels[30] =
{0x40, // First byte specifies data mode
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//==============================================================================
// this section is based on https://github.com/adafruit/Adafruit_SSD1306
/** Initialization commands for a 128x32 or 128x64 SSD1306 oled display. */
const uint8_t FLASH SSD1306AsciiWire::Adafruit128xXXinit[] = {
// Init sequence for Adafruit 128x32/64 OLED module
0x00, // Set to command mode
SSD1306_DISPLAYOFF,
SSD1306_SETDISPLAYCLOCKDIV, 0x80, // the suggested ratio 0x80
SSD1306_SETMULTIPLEX, 0x3F, // ratio 64 (initially)
SSD1306_SETDISPLAYOFFSET, 0x0, // no offset
SSD1306_SETSTARTLINE | 0x0, // line #0
SSD1306_CHARGEPUMP, 0x14, // internal vcc
SSD1306_MEMORYMODE, 0x02, // page mode
SSD1306_SEGREMAP | 0x1, // column 127 mapped to SEG0
SSD1306_COMSCANDEC, // column scan direction reversed
SSD1306_SETCOMPINS, 0X12, // set COM pins
SSD1306_SETCONTRAST, 0x7F, // contrast level 127
SSD1306_SETPRECHARGE, 0xF1, // pre-charge period (1, 15)
SSD1306_SETVCOMDETECT, 0x40, // vcomh regulator level
SSD1306_DISPLAYALLON_RESUME,
SSD1306_NORMALDISPLAY,
SSD1306_DISPLAYON
};
//------------------------------------------------------------------------------
// This section is based on https://github.com/stanleyhuangyc/MultiLCD
/** Initialization commands for a 128x64 SH1106 oled display. */
const uint8_t FLASH SSD1306AsciiWire::SH1106_132x64init[] = {
0x00, // Set to command mode
SSD1306_DISPLAYOFF,
SSD1306_SETDISPLAYCLOCKDIV, 0X80, // set osc division
SSD1306_SETMULTIPLEX, 0x3F, // ratio 64
SSD1306_SETDISPLAYOFFSET, 0X00, // set display offset
SSD1306_SETSTARTPAGE | 0X0, // set page address
SSD1306_SETSTARTLINE | 0x0, // set start line
SH1106_SET_PUMP_MODE, SH1106_PUMP_ON, // set charge pump enable
SSD1306_SEGREMAP | 0X1, // set segment remap
SSD1306_COMSCANDEC, // Com scan direction
SSD1306_SETCOMPINS, 0X12, // set COM pins
SSD1306_SETCONTRAST, 0x80, // 128
SSD1306_SETPRECHARGE, 0X1F, // set pre-charge period
SSD1306_SETVCOMDETECT, 0x40, // set vcomh
SH1106_SET_PUMP_VOLTAGE | 0X2, // 8.0 volts
SSD1306_NORMALDISPLAY, // normal / reverse
SSD1306_DISPLAYON
};
//==============================================================================
// SSD1306AsciiWire Method Definitions
//------------------------------------------------------------------------------
// Constructor
SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
m_displayWidth = width;
m_displayHeight = height;
// Set size in characters in base class
lcdRows = height / 8;
lcdCols = width / 6;
m_col = 0;
m_row = 0;
m_colOffset = 0;
I2CManager.begin();
I2CManager.setClock(400000L); // Set max supported I2C speed
for (byte address = 0x3c; address <= 0x3d; address++) {
if (I2CManager.exists(address)) {
m_i2cAddr = address;
if (m_displayWidth==132 && m_displayHeight==64) {
// SH1106 display. This uses 128x64 centered within a 132x64 OLED.
m_colOffset = 2;
I2CManager.write_P(address, SH1106_132x64init, sizeof(SH1106_132x64init));
} else if (m_displayWidth==128 && (m_displayHeight==64 || m_displayHeight==32)) {
// SSD1306 128x64 or 128x32
I2CManager.write_P(address, Adafruit128xXXinit, sizeof(Adafruit128xXXinit));
if (m_displayHeight == 32)
I2CManager.write(address, 5, 0, // Set command mode
SSD1306_SETMULTIPLEX, 0x1F, // ratio 32
SSD1306_SETCOMPINS, 0x02); // sequential COM pins, disable remap
} else {
DIAG(F("OLED configuration option not recognised"));
return;
}
// Device found
DIAG(F("%dx%d OLED display configured on I2C:x%x"), width, height, address);
if (width == 132)
begin(&SH1106_132x64, address);
else if (height == 32)
begin(&Adafruit128x32, address);
else
begin(&Adafruit128x64, address);
// Set singleton address
lcdDisplay = this;
clear();
@@ -132,23 +197,6 @@ void SSD1306AsciiWire::clearNative() {
}
}
// Initialise device
void SSD1306AsciiWire::begin(const DevType* dev, uint8_t i2cAddr) {
m_i2cAddr = i2cAddr;
m_col = 0;
m_row = 0;
const uint8_t* table = (const uint8_t*)GETFLASHW(&dev->initcmds);
uint8_t size = GETFLASH(&dev->initSize);
m_displayWidth = GETFLASH(&dev->lcdWidth);
m_displayHeight = GETFLASH(&dev->lcdHeight);
m_colOffset = GETFLASH(&dev->colOffset);
I2CManager.write_P(m_i2cAddr, table, size);
if (m_displayHeight == 32)
I2CManager.write(m_i2cAddr, 5, 0, // Set command mode
SSD1306_SETMULTIPLEX, 0x1F, // ratio 32
SSD1306_SETCOMPINS, 0x02); // sequential COM pins, disable remap
}
//------------------------------------------------------------------------------
// Set cursor position (by text line)
@@ -209,82 +257,6 @@ size_t SSD1306AsciiWire::writeNative(uint8_t ch) {
return 1;
}
//==============================================================================
// this section is based on https://github.com/adafruit/Adafruit_SSD1306
/** Initialization commands for a 128x32 or 128x64 SSD1306 oled display. */
const uint8_t FLASH SSD1306AsciiWire::Adafruit128xXXinit[] = {
// Init sequence for Adafruit 128x32/64 OLED module
0x00, // Set to command mode
SSD1306_DISPLAYOFF,
SSD1306_SETDISPLAYCLOCKDIV, 0x80, // the suggested ratio 0x80
SSD1306_SETMULTIPLEX, 0x3F, // ratio 64 (initially)
SSD1306_SETDISPLAYOFFSET, 0x0, // no offset
SSD1306_SETSTARTLINE | 0x0, // line #0
SSD1306_CHARGEPUMP, 0x14, // internal vcc
SSD1306_MEMORYMODE, 0x02, // page mode
SSD1306_SEGREMAP | 0x1, // column 127 mapped to SEG0
SSD1306_COMSCANDEC, // column scan direction reversed
SSD1306_SETCOMPINS, 0X12, // set COM pins
SSD1306_SETCONTRAST, 0x7F, // contrast level 127
SSD1306_SETPRECHARGE, 0xF1, // pre-charge period (1, 15)
SSD1306_SETVCOMDETECT, 0x40, // vcomh regulator level
SSD1306_DISPLAYALLON_RESUME,
SSD1306_NORMALDISPLAY,
SSD1306_DISPLAYON
};
/** Initialize a 128x32 SSD1306 oled display. */
const DevType FLASH SSD1306AsciiWire::Adafruit128x32 = {
Adafruit128xXXinit,
sizeof(Adafruit128xXXinit),
128,
32,
0
};
/** Initialize a 128x64 oled display. */
const DevType FLASH SSD1306AsciiWire::Adafruit128x64 = {
Adafruit128xXXinit,
sizeof(Adafruit128xXXinit),
128,
64,
0
};
//------------------------------------------------------------------------------
// This section is based on https://github.com/stanleyhuangyc/MultiLCD
/** Initialization commands for a 128x64 SH1106 oled display. */
const uint8_t FLASH SSD1306AsciiWire::SH1106_132x64init[] = {
0x00, // Set to command mode
SSD1306_DISPLAYOFF,
SSD1306_SETDISPLAYCLOCKDIV, 0X80, // set osc division
SSD1306_SETMULTIPLEX, 0x3F, // ratio 64
SSD1306_SETDISPLAYOFFSET, 0X00, // set display offset
SSD1306_SETSTARTPAGE | 0X0, // set page address
SSD1306_SETSTARTLINE | 0x0, // set start line
SH1106_SET_PUMP_MODE, SH1106_PUMP_ON, // set charge pump enable
SSD1306_SEGREMAP | 0X1, // set segment remap
SSD1306_COMSCANDEC, // Com scan direction
SSD1306_SETCOMPINS, 0X12, // set COM pins
SSD1306_SETCONTRAST, 0x80, // 128
SSD1306_SETPRECHARGE, 0X1F, // set pre-charge period
SSD1306_SETVCOMDETECT, 0x40, // set vcomh
SH1106_SET_PUMP_VOLTAGE | 0X2, // 8.0 volts
SSD1306_NORMALDISPLAY, // normal / reverse
SSD1306_DISPLAYON
};
/** Initialize a 132x64 oled SH1106 display. */
const DevType FLASH SSD1306AsciiWire::SH1106_132x64 = {
SH1106_132x64init,
sizeof(SH1106_132x64init),
128,
64,
2 // SH1106 is a 132x64 controller but most OLEDs are only attached
// to columns 2-129.
};
//------------------------------------------------------------------------------

View File

@@ -32,21 +32,6 @@
//#define NOLOWERCASE
//------------------------------------------------------------------------------
// Device initialization structure.
struct DevType {
/* Pointer to initialization command bytes. */
const uint8_t* initcmds;
/* Number of initialization bytes */
const uint8_t initSize;
/* Width of the display in pixels */
const uint8_t lcdWidth;
/** Height of the display in pixels. */
const uint8_t lcdHeight;
/* Column offset RAM to display. Used to pick start column of SH1106. */
const uint8_t colOffset;
};
// Constructor
class SSD1306AsciiWire : public LCDDisplay {
public:
@@ -55,25 +40,17 @@ class SSD1306AsciiWire : public LCDDisplay {
SSD1306AsciiWire(int width, int height);
// Initialize the display controller.
void begin(const DevType* dev, uint8_t i2cAddr);
void begin(uint8_t i2cAddr);
// Clear the display and set the cursor to (0, 0).
void clearNative() override;
// Set cursor to start of specified text line
void setRowNative(byte line) override;
// Initialize the display controller.
void init(const DevType* dev);
// Write one character to OLED
size_t writeNative(uint8_t c) override;
// Display characteristics / initialisation
static const DevType FLASH Adafruit128x32;
static const DevType FLASH Adafruit128x64;
static const DevType FLASH SH1106_132x64;
bool isBusy() override { return requestBlock.isBusy(); }
private:

View File

@@ -51,6 +51,8 @@
#include "version.h"
#include "RMFT2.h"
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)
#define LOOPLOCOS(THROTTLECHAR, CAB) for (int loco=0;loco<MAX_MY_LOCO;loco++) \
if ((myLocos[loco].throttle==THROTTLECHAR || '*'==THROTTLECHAR) && (CAB<0 || myLocos[loco].cab==CAB))
@@ -409,7 +411,7 @@ void WiThrottle::checkHeartbeat() {
}
char WiThrottle::LorS(int cab) {
return (cab<127)?'S':'L';
return (cab<=HIGHEST_SHORT_ADDR)?'S':'L';
}
// Drive Away feature. Callback handling
@@ -421,13 +423,26 @@ char WiThrottle::stashThrottleChar;
void WiThrottle::getLocoCallback(int16_t locoid) {
stashStream->mark(stashClient);
if (locoid<0) StringFormatter::send(stashStream,F("HMNo loco found on prog track\n"));
if (locoid<=0)
StringFormatter::send(stashStream,F("HMNo loco found on prog track\n"));
else {
char addcmd[20]={'M',stashThrottleChar,'+',LorS(locoid) };
itoa(locoid,addcmd+4,10);
stashInstance->multithrottle(stashStream, (byte *)addcmd);
DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
DCC::setProgTrackSyncMain(true); // <1 JOIN> so we can drive loco away
// short or long
char addrchar;
if (locoid & LONG_ADDR_MARKER) { // long addr
locoid = locoid ^ LONG_ADDR_MARKER;
addrchar = 'L';
} else
addrchar = 'S';
if (addrchar == 'L' && locoid <= HIGHEST_SHORT_ADDR )
StringFormatter::send(stashStream,F("HMLong addr <= " STR(HIGHEST_SHORT_ADDR) " not supported\n"));
else {
char addcmd[20]={'M',stashThrottleChar,'+', addrchar};
itoa(locoid,addcmd+4,10);
stashInstance->multithrottle(stashStream, (byte *)addcmd);
DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
DCC::setProgTrackSyncMain(true); // <1 JOIN> so we can drive loco away
}
}
stashStream->commit();
}

View File

@@ -1,246 +0,0 @@
/*
© 2021, Harald Barth.
This file is part of CommandStation-EX
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 <https://www.gnu.org/licenses/>.
*/
#include <vector>
#include "defines.h"
#if defined(ARDUINO_ARCH_ESP32)
#include <WiFi.h>
#include "WifiESP32.h"
#include "DIAG.h"
#include "RingStream.h"
#include "CommandDistributor.h"
/*
#include "soc/rtc_wdt.h"
#include "esp_task_wdt.h"
*/
#include "soc/timer_group_struct.h"
#include "soc/timer_group_reg.h"
void feedTheDog0(){
// feed dog 0
TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE; // write enable
TIMERG0.wdt_feed=1; // feed dog
TIMERG0.wdt_wprotect=0; // write protect
// feed dog 1
//TIMERG1.wdt_wprotect=TIMG_WDT_WKEY_VALUE; // write enable
//TIMERG1.wdt_feed=1; // feed dog
//TIMERG1.wdt_wprotect=0; // write protect
}
/*
void enableCoreWDT(byte core){
TaskHandle_t idle = xTaskGetIdleTaskHandleForCPU(core);
if(idle == NULL){
DIAG(F("Get idle rask on core %d failed"),core);
} else {
if(esp_task_wdt_add(idle) != ESP_OK){
DIAG(F("Failed to add Core %d IDLE task to WDT"),core);
} else {
DIAG(F("Added Core %d IDLE task to WDT"),core);
}
}
}
void disableCoreWDT(byte core){
TaskHandle_t idle = xTaskGetIdleTaskHandleForCPU(core);
if(idle == NULL || esp_task_wdt_delete(idle) != ESP_OK){
DIAG(F("Failed to remove Core %d IDLE task from WDT"),core);
}
}
*/
static std::vector<WiFiClient> clients; // a list to hold all clients
static WiFiServer *server = NULL;
static RingStream *outboundRing = new RingStream(2048);
static bool APmode = false;
void wifiLoop(void *){
for(;;){
WifiESP::loop();
}
}
bool WifiESP::setup(const char *SSid,
const char *password,
const char *hostname,
int port,
const byte channel) {
bool havePassword = true;
bool haveSSID = true;
bool wifiUp = false;
uint8_t tries = 40;
// tests
// enableCoreWDT(1);
// disableCoreWDT(0);
const char *yourNetwork = "Your network ";
if (strncmp(yourNetwork, SSid, 13) == 0 || strncmp("", SSid, 13) == 0)
haveSSID = false;
if (strncmp(yourNetwork, password, 13) == 0 || strncmp("", password, 13) == 0)
havePassword = false;
if (haveSSID && havePassword) {
WiFi.mode(WIFI_STA);
WiFi.setAutoReconnect(true);
WiFi.begin(SSid, password);
while (WiFi.status() != WL_CONNECTED && tries) {
Serial.print('.');
tries--;
delay(500);
}
if (WiFi.status() == WL_CONNECTED) {
DIAG(F("Wifi STA IP %s"),WiFi.localIP().toString().c_str());
wifiUp = true;
} else {
DIAG(F("Could not connect to Wifi SSID %s"),SSid);
}
}
if (!haveSSID) {
// prepare all strings
String strSSID("DCC_");
String strPass("PASS_");
String strMac = WiFi.macAddress();
strMac.remove(0,9);
strMac.replace(":","");
strMac.replace(":","");
strSSID.concat(strMac);
strPass.concat(strMac);
WiFi.mode(WIFI_AP);
if (WiFi.softAP(strSSID.c_str(),
havePassword ? password : strPass.c_str(),
channel, false, 8)) {
DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
DIAG(F("Wifi AP IP %s"),WiFi.softAPIP().toString().c_str());
wifiUp = true;
APmode = true;
} else {
DIAG(F("Could not set up AP with Wifi SSID %s"),strSSID.c_str());
}
}
if (!wifiUp) {
DIAG(F("Wifi setup all fail (STA and AP mode)"));
// no idea to go on
return false;
}
server = new WiFiServer(port); // start listening on tcp port
server->begin();
// server started here
//start loop task
if (pdPASS != xTaskCreatePinnedToCore(
wifiLoop, /* Task function. */
"wifiLoop",/* name of task. */
10000, /* Stack size of task */
NULL, /* parameter of the task */
1, /* priority of the task */
NULL, /* Task handle to keep track of created task */
0)) { /* pin task to core 0 */
DIAG(F("Could not create wifiLoop task"));
return false;
}
// report server started after wifiLoop creation
// when everything looks good
DIAG(F("Server up port %d"),port);
return true;
}
void WifiESP::loop() {
int clientId; //tmp loop var
// really no good way to check for LISTEN especially in AP mode?
if (APmode || WiFi.status() == WL_CONNECTED) {
if (server->hasClient()) {
// loop over all clients and remove inactive
for (clientId=0; clientId<clients.size(); clientId++){
// check if client is there and alive
if(!clients[clientId].connected()) {
clients[clientId].stop();
clients.erase(clients.begin()+clientId);
}
}
WiFiClient client;
while (client = server->available()) {
clients.push_back(client);
DIAG(F("New client %s"), client.remoteIP().toString().c_str());
}
}
// loop over all connected clients
for (clientId=0; clientId<clients.size(); clientId++){
if(clients[clientId].connected()) {
int len;
if ((len = clients[clientId].available()) > 0) {
// read data from client
byte cmd[len+1];
for(int i=0; i<len; i++) {
cmd[i]=clients[clientId].read();
}
cmd[len]=0;
outboundRing->mark(clientId);
CommandDistributor::parse(clientId,cmd,outboundRing);
outboundRing->commit();
}
}
} // all clients
// something to write out?
clientId=outboundRing->peek();
if (clientId >= 0) {
if ((unsigned int)clientId > clients.size()) {
// something is wrong with the ringbuffer position
outboundRing->info();
} else {
// we have data to send in outboundRing
if(clients[clientId].connected()) {
outboundRing->read(); // read over peek()
int count=outboundRing->count();
{
char buffer[count+1];
for(int i=0;i<count;i++) {
int c = outboundRing->read();
if (c >= 0)
buffer[i] = (char)c;
else {
DIAG(F("Ringread fail at %d"),i);
break;
}
}
buffer[count]=0;
clients[clientId].write(buffer,count);
}
}
}
}
} //connected
// when loop() is running on core0 we must
// feed the core0 wdt ourselves as yield()
// is not necessarily yielding to a low
// prio task. On core1 this is not a problem
// as there the wdt is disabled by the
// arduio IDE startup routines.
if (xPortGetCoreID() == 0)
feedTheDog0();
yield();
}
#endif //ESP32

View File

@@ -1,39 +0,0 @@
/*
* © 2021, Harald Barth.
*
* This file is part of CommandStation-EX
*
* 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 <https://www.gnu.org/licenses/>.
*/
#if defined(ARDUINO_ARCH_ESP32)
#ifndef WifiESP32_h
#define WifiESP32_h
#include "FSH.h"
class WifiESP
{
public:
static bool setup(const char *wifiESSID,
const char *wifiPassword,
const char *hostname,
const int port,
const byte channel);
static void loop();
private:
};
#endif //WifiESP8266_h
#endif //ESP8266

View File

@@ -1,270 +0,0 @@
/*
© 2021, Harald Barth.
This file is part of CommandStation-EX
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 <https://www.gnu.org/licenses/>.
*/
#include "defines.h"
#if defined(ARDUINO_ARCH_ESP8266)
#include <ESP8266WiFi.h>
#include <ESPAsyncTCP.h>
#include <vector>
#include <string>
#include "WifiESP8266.h"
#include "DIAG.h"
#include "RingStream.h"
#include "CommandDistributor.h"
#include <string.h>
static std::vector<AsyncClient*> clients; // a list to hold all clients
static AsyncServer *server;
static RingStream *outboundRing = new RingStream(2048);
static void handleError(void* arg, AsyncClient* client, int8_t error) {
(void)arg;
DIAG(F("connection error %s from client %s"), client->errorToString(error), client->remoteIP().toString().c_str());
}
static void handleData(void* arg, AsyncClient* client, void *data, size_t len) {
(void)arg;
//DIAG(F("data received from client %s"), client->remoteIP().toString().c_str());
uint8_t clientId;
for (clientId=0; clientId<clients.size(); clientId++){
if (clients[clientId] == client) break;
}
if (clientId < clients.size()) {
byte cmd[len+1];
memcpy(cmd,data,len);
cmd[len]=0;
outboundRing->mark(clientId);
CommandDistributor::parse(clientId,cmd,outboundRing);
outboundRing->commit();
}
}
//static AsyncClient *debugclient = NULL;
bool sendData(AsyncClient *client, char* data, size_t count) {
size_t willsend = 0;
// reply to client
if (client->canSend()) {
while (count > 0) {
if (client->connected())
willsend = client->add(data, count); // add checks for space()
else
willsend = 0;
if (willsend < count) {
DIAG(F("Willsend %d of count %d"), willsend, count);
}
if (client->connected() && client->send()) {
count = count - willsend;
data = data + willsend;
} else {
DIAG(F("Could not send promised %d"), count);
return false;
}
}
// Did send all bytes we wanted
return true;
}
DIAG(F("Aborting: Busy or space=0"));
return false;
}
static void deleteClient(AsyncClient* client) {
uint8_t clientId;
for (clientId=0; clientId<clients.size(); clientId++){
if (clients[clientId] == client) break;
}
if (clientId < clients.size()) {
clients[clientId] = NULL;
}
}
static void handleDisconnect(void* arg, AsyncClient* client) {
(void)arg;
DIAG(F("Client disconnected"));
deleteClient(client);
}
static void handleTimeOut(void* arg, AsyncClient* client, uint32_t time) {
(void)arg;
(void)time;
DIAG(F("client ACK timeout ip: %s"), client->remoteIP().toString().c_str());
deleteClient(client);
}
static void handleNewClient(void* arg, AsyncClient* client) {
(void)arg;
DIAG(F("New client %s"), client->remoteIP().toString().c_str());
// add to list
clients.push_back(client);
// register events
client->onData(&handleData, NULL);
client->onError(&handleError, NULL);
client->onDisconnect(&handleDisconnect, NULL);
client->onTimeout(&handleTimeOut, NULL);
}
/* Things one _might_ want to do:
Disable soft watchdog: ESP.wdtDisable()
Enable soft watchdog: ESP.wdtEnable(X) ignores the value of X and enables it for fixed
time at least in version 3.0.2 of the esp8266 package.
Internet says:
I manage to complety disable the hardware watchdog on ESP8266 in order to run the benchmark CoreMark.
void hw_wdt_disable(){
*((volatile uint32_t*) 0x60000900) &= ~(1); // Hardware WDT OFF
}
void hw_wdt_enable(){
*((volatile uint32_t*) 0x60000900) |= 1; // Hardware WDT ON
}
*/
bool WifiESP::setup(const char *SSid,
const char *password,
const char *hostname,
int port,
const byte channel) {
bool havePassword = true;
bool haveSSID = true;
bool wifiUp = false;
// We are server and should not sleep
wifi_set_sleep_type(NONE_SLEEP_T);
// connects to access point
const char *yourNetwork = "Your network ";
if (strncmp(yourNetwork, SSid, 13) == 0 || strncmp("", SSid, 13) == 0)
haveSSID = false;
if (strncmp(yourNetwork, password, 13) == 0 || strncmp("", password, 13) == 0)
havePassword = false;
if (haveSSID && havePassword) {
WiFi.mode(WIFI_STA);
WiFi.setAutoReconnect(true);
WiFi.begin(SSid, password);
while (WiFi.status() != WL_CONNECTED) {
Serial.print('.');
delay(500);
}
if (WiFi.status() == WL_CONNECTED) {
DIAG(F("Wifi STA IP %s"),WiFi.localIP().toString().c_str());
wifiUp = true;
}
}
if (!haveSSID) {
// prepare all strings
String strSSID("DCC_");
String strPass("PASS_");
String strMac = WiFi.macAddress();
strMac.remove(0,9);
strMac.replace(":","");
strMac.replace(":","");
strSSID.concat(strMac);
strPass.concat(strMac);
WiFi.mode(WIFI_AP);
if (WiFi.softAP(strSSID.c_str(),
havePassword ? password : strPass.c_str(),
channel, false, 8)) {
DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
DIAG(F("Wifi AP IP %s"),WiFi.softAPIP().toString().c_str());
wifiUp = true;
}
}
if (!wifiUp) {
DIAG(F("Wifi all fail"));
// no idea to go on
return false;
}
server = new AsyncServer(port); // start listening on tcp port
server->onClient(&handleNewClient, server);
server->begin();
DIAG(F("Server up port %d"),port);
return true;
}
void WifiESP::loop() {
AsyncClient *client = NULL;
// Do something with outboundRing
// call sendData
int clientId=outboundRing->peek();
if (clientId >= 0) {
if ((unsigned int)clientId > clients.size()) {
// something is wrong with the ringbuffer position
outboundRing->info();
client = NULL;
} else {
client = clients[clientId];
}
// if (client != debugclient) {
// DIAG(F("new client pointer = %x from id %d"), client, clientId);
// debugclient = client;
// }
} else {
client = NULL;
}
if (clientId>=0 && client && client->connected() && client->canSend()) {
outboundRing->read();
int count=outboundRing->count();
//DIAG(F("Wifi reply client=%d, count=%d"), clientId,count);
{
char buffer[count+1];
for(int i=0;i<count;i++) {
int c = outboundRing->read();
if (c >= 0)
buffer[i] = (char)c;
else {
DIAG(F("Ringread fail at %d"),i);
break;
}
}
buffer[count]=0;
//DIAG(F("SEND:%s COUNT:%d"),buffer,count);
uint8_t tries = 3;
while (! sendData(client, buffer, count)) {
DIAG(F("senData fail"));
yield();
if (tries == 0) break;
}
}
}
#ifdef ESP_DEBUG
static unsigned long last = 0;
if (millis() - last > 60000) {
last = millis();
DIAG(F("+"));
}
#endif
ESP.wdtFeed();
}
#endif //ESP_FAMILY

View File

@@ -1,39 +0,0 @@
/*
* © 2021, Harald Barth.
*
* This file is part of CommandStation-EX
*
* 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 <https://www.gnu.org/licenses/>.
*/
#if defined(ARDUINO_ARCH_ESP8266)
#ifndef WifiESP8266_h
#define WifiESP8266_h
#include "FSH.h"
class WifiESP
{
public:
static bool setup(const char *wifiESSID,
const char *wifiPassword,
const char *hostname,
const int port,
const byte channel);
static void loop();
private:
};
#endif //WifiESP8266_h
#endif //ESP8266

View File

@@ -17,10 +17,9 @@
You should have received a copy of the GNU General Public License
along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
*/
#include "WifiInterface.h" /* config.h included there */
#ifndef ESP_FAMILY
#ifndef ARDUINO_AVR_UNO_WIFI_REV2
// This code is NOT compiled on a unoWifiRev2 processor which uses a different architecture
#include "WifiInterface.h" /* config.h included there */
#include <avr/pgmspace.h>
#include "DIAG.h"
#include "StringFormatter.h"
@@ -371,5 +370,4 @@ void WifiInterface::loop() {
}
}
#endif //ARDUINO_AVR_UNO_WIFI_REV2
#endif //ESP_FAMILY
#endif

View File

@@ -19,8 +19,6 @@
*/
#ifndef WifiInterface_h
#define WifiInterface_h
#include "defines.h"
#ifndef ESP_FAMILY
#include "FSH.h"
#include "DCCEXParser.h"
#include <Arduino.h>
@@ -52,5 +50,4 @@ private:
static bool checkForOK(const unsigned int timeout, const FSH *waitfor, bool echo, bool escapeEcho = true);
static bool connected;
};
#endif //ESP_FAMILY
#endif
#endif

View File

@@ -41,29 +41,7 @@ The configuration file for DCC-EX Command Station
// |
// +-----------------------v
//
//#define MOTOR_SHIELD_TYPE STANDARD_MOTOR_SHIELD
// https://randomnerdtutorials.com/esp8266-pinout-reference-gpios/
// 4 high at boot
#define ESP8266_MOTOR_SHIELD F("ESP8266"), \
new MotorDriver(D3, D5, UNUSED_PIN, UNUSED_PIN, UNUSED_PIN, 2.99, 2000, UNUSED_PIN),\
new MotorDriver(D2, D6, UNUSED_PIN, UNUSED_PIN, A0 , 2.99, 2000, UNUSED_PIN)
// ESP32 ADC1 only supported GPIO pins 32 to 39, for example
// ADC1 CH4 = GPIO32, ADC1 CH5 = GPIO33, ADC1 CH0 = GPIO36
//
// Adjust pin usage according to info in
// https://randomnerdtutorials.com/esp32-adc-analog-read-arduino-ide/
// https://randomnerdtutorials.com/esp32-pinout-reference-gpios/
//
// Adjust conversion factor according to your voltage divider.
//
#define ESP32_MOTOR_SHIELD F("ESP32"), \
new MotorDriver(16, 17, UNUSED_PIN, UNUSED_PIN, 32, 2.00, 2000, UNUSED_PIN),\
new MotorDriver(18, 19, UNUSED_PIN, UNUSED_PIN, 33, 2.00, 2000, UNUSED_PIN)
#define MOTOR_SHIELD_TYPE ESP32_MOTOR_SHIELD
#define MOTOR_SHIELD_TYPE STANDARD_MOTOR_SHIELD
/////////////////////////////////////////////////////////////////////////////////////
//
// The IP port to talk to a WIFI or Ethernet shield.
@@ -75,8 +53,6 @@ The configuration file for DCC-EX Command Station
// NOTE: Only supported on Arduino Mega
// Set to false if you not even want it on the Arduino Mega
//
// Currently ESP32 single core only works with WIFI ON because of Watchdog code
// and if you have an ESP32 you probably want WIFI anyway.
#define ENABLE_WIFI true
/////////////////////////////////////////////////////////////////////////////////////
@@ -152,6 +128,18 @@ The configuration file for DCC-EX Command Station
// Define scroll mode as 0, 1 or 2
#define SCROLLMODE 1
/////////////////////////////////////////////////////////////////////////////////////
// REDEFINE WHERE SHORT/LONG ADDR break is. According to NMRA the last short address
// is 127 and the first long address is 128. There are manufacturers which have
// another view. Lenz CS for example have considered addresses long from 100. If
// you want to change to that mode, do
//#define HIGHEST_SHORT_ADDR 99
// If you want to run all your locos addressed long format, you could even do a
//#define HIGHEST_SHORT_ADDR 0
// We do not support to use the same address, for example 100(long) and 100(short)
// at the same time, there must be a border.
/////////////////////////////////////////////////////////////////////////////////////
//
// DEFINE TURNOUTS/ACCESSORIES FOLLOW NORM RCN-213

View File

@@ -1,5 +1,5 @@
/*
© 2020,2021 Harald Barth.
© 2020, Harald Barth.
This file is part of CommandStation-EX
@@ -31,34 +31,14 @@
#endif
#endif
////////////////////////////////////////////////////////////////////////////////
//
#if defined(ARDUINO_ARCH_ESP8266)
#define ESP_FAMILY
//#define ESP_DEBUG
#define SLOW_ANALOG_READ
#endif
////////////////////////////////////////////////////////////////////////////////
//
#if defined(ARDUINO_ARCH_ESP32)
#define ESP_FAMILY
#define SLOW_ANALOG_READ
#else
#define portENTER_CRITICAL(A) do {} while (0)
#define portEXIT_CRITICAL(A) do {} while (0)
#endif
////////////////////////////////////////////////////////////////////////////////
//
// WIFI_ON: All prereqs for running with WIFI are met
// Note: WIFI_CHANNEL may not exist in early config.h files so is added here if needed.
#if (defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAMD_ZERO) || defined(TEENSYDUINO) || defined(ESP_FAMILY))
#if (defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAMD_ZERO) || defined(TEENSYDUINO))
#define BIG_RAM
#endif
#if ENABLE_WIFI && defined(BIG_RAM)
#define WIFI_ON true
#ifndef WIFI_CHANNEL
@@ -89,4 +69,4 @@
#define RMFT_ACTIVE
#endif
#endif
#endif

View File

@@ -1,67 +0,0 @@
The ESP-IDF has interrupt watchdogs and task watchdogs. Normally on
each core there is a very low prio idle task (IDLE0, ILDE1) that feeds
the watchdog (an internal timer) and if that timer expires a
reboot/reset happens. This thought to enure that even the lowest prio
tasks get run ever.
Now enter the Arduino IDE generated task loop(). When there are two cores,
loop() is run on core1. If loop runs continiously, IDLE1 is never run and
triggers the watchdog. It is said that this can be previented by one
of the following:
1. Call delay(X) with big enough X
2. Call yield()
While the delay() method works, big enough X to run idle seem to be in
the ms range and there are definitely applications that can not accept
a several ms long pause in loop().
The yield() method does not work because it only seems not to yield to
a low prio task like IDLE1 in all circumstances.
Then the makers of the Arduino IDE did get the brilliant idea to
disable that IDLE1 calls the watchdog. Then loop() can spin on core1
and other tasks (like wifi or interrupts or whatever) can run on core0
and are watched by the IDLE0 watchdog. All swell and well. Almost.
Enter: SINGLE CORE ESP32
As the IDLE0 watchdog is not disabled it will fire when loop() runs on
core0. The next idea is to feed the watchdog from loop() just
alongside the yield. There is a function called esp_task_wdt_feed(),
so can that be used to feed the watchdog? Yes and no. While it
will feed the watchdog, there is as well as check in the ESP-IDF
that the watchdog is fed from ALL tasks that should feed it. So
if the setup is that IDLE0 should feed the watchdog, we can not
get away by calling esp_task_wdt_feed() from loop(). BUMMER!
But there seems to be a way around this. The watchdog is implemented
by low level timers/counters and these are accessible. So we can feed
the dog behind the back of the ESP-IDF:
#include "soc/timer_group_struct.h"
#include "soc/timer_group_reg.h"
void feedTheDog(){
// feed dog 0
TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE; // write enable
TIMERG0.wdt_feed=1; // feed dog
TIMERG0.wdt_wprotect=0; // write protect
// feed dog 1
TIMERG1.wdt_wprotect=TIMG_WDT_WKEY_VALUE; // write enable
TIMERG1.wdt_feed=1; // feed dog
TIMERG1.wdt_wprotect=0; // write protect
}
As I do not have a single core ESP32 I tested this by enabling the
IDLE1 watchdog (which normally is disabled) and checking that I
do get watchdog resets. Then I call feedTheDog() from loop()
and the resets disappear. So I guess the feeding operation is
successful. For a single core ESP32 of course only dog0 has
to be fed.
Feed dog directly behind back of the ESP-IDF routines:
https://forum.arduino.cc/t/esp32-a-better-way-than-vtaskdelay-to-get-around-watchdog-crash/596889/13
Disable/Endable WDT code:
https://github.com/espressif/arduino-esp32/commit/b8f8502f
Get/set taskid on cores:
https://techtutorialsx.com/2017/05/09/esp32-get-task-execution-core/

View File

@@ -1,5 +1,5 @@
/*
* © 2020,2021 Harald Barth
* © 2020, Harald Barth
* © 2021, Neil McKechnie
*
* This file is part of Asbelos DCC-EX
@@ -27,8 +27,6 @@ extern "C" char* sbrk(int);
#elif defined(__AVR__)
extern char *__brkval;
extern char *__malloc_heap_start;
#elif defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)
// supported but nothing needed here
#else
#error Unsupported board type
#endif
@@ -36,7 +34,7 @@ extern char *__malloc_heap_start;
static volatile int minimum_free_memory = __INT_MAX__;
#if !defined(__IMXRT1062__) && !defined(ARDUINO_ARCH_ESP8266) && !defined(ARDUINO_ARCH_ESP32)
#if !defined(__IMXRT1062__)
static inline int freeMemory() {
char top;
#if defined(__arm__)
@@ -57,18 +55,7 @@ int minimumFreeMemory() {
return retval;
}
#elif defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)
// ESP8266 and ESP32
static inline int freeMemory() {
return ESP.getFreeHeap();
}
// Return low memory value.
int minimumFreeMemory() {
int retval = minimum_free_memory;
return retval;
}
#else
// All types of TEENSYs
#if defined(ARDUINO_TEENSY40)
static const unsigned DTCM_START = 0x20000000UL;
static const unsigned OCRAM_START = 0x20200000UL;
@@ -121,3 +108,4 @@ void updateMinimumFreeMemory(unsigned char extraBytes) {
if (spare < 0) spare = 0;
if (spare < minimum_free_memory) minimum_free_memory = spare;
}

View File

@@ -3,7 +3,7 @@
#include "StringFormatter.h"
#define VERSION "3.2.0 ESP32"
#define VERSION "3.2.0 rc5"
// 3.2.0 Major functional and non-functional changes.
// New HAL added for I/O (digital and analogue inputs and outputs, servos etc).
// Support for MCP23008, MCP23017 and PCF9584 I2C GPIO Extender modules.