1
0
mirror of https://github.com/DCC-EX/CommandStation-EX.git synced 2024-12-23 21:01:25 +01:00

Merge branch 'EX-RAIL' into EX-RAIL-Ash

This commit is contained in:
Ash-4 2021-09-11 13:32:17 -05:00 committed by GitHub
commit 72528658be
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 291 additions and 34 deletions

View File

@ -757,7 +757,7 @@ void DCC::ackManagerLoop() {
// (typically waiting for a reset counter or ACK waiting, or when all finished.)
switch (opcode) {
case BASELINE:
if (DCCWaveform::progTrack.getPowerMode()==POWERMODE::OVERLOAD) return;
if (DCCWaveform::progTrack.getPowerMode()==POWERMODE::OVERLOAD) return;
if (checkResets(DCCWaveform::progTrack.autoPowerOff || ackManagerRejoin ? 20 : 3)) return;
DCCWaveform::progTrack.setAckBaseline();
callbackState=READY;

View File

@ -114,7 +114,7 @@ void DCCWaveform::setPowerMode(POWERMODE mode) {
powerMode = mode;
bool ison = (mode == POWERMODE::ON);
motorDriver->setPower( ison);
sentResetsSincePacket=0;
sentResetsSincePacket=0;
}

View File

@ -241,6 +241,18 @@ int IODevice::read(VPIN vpin) {
return false;
}
// Read analogue value from virtual pin.
int IODevice::readAnalogue(VPIN vpin) {
for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice) {
if (dev->owns(vpin))
return dev->_readAnalogue(vpin);
}
#ifdef DIAG_IO
//DIAG(F("IODevice::readAnalogue(): Vpin %d not found!"), (int)vpin);
#endif
return false;
}
#else // !defined(IO_NO_HAL)
@ -259,6 +271,13 @@ int IODevice::read(VPIN vpin) {
pinMode(vpin, INPUT_PULLUP);
return !digitalRead(vpin); // Return inverted state (5v=0, 0v=1)
}
int IODevice::readAnalogue(VPIN vpin) {
pinMode(vpin, INPUT);
noInterrupts();
int value = analogRead(vpin);
interrupts();
return value;
}
void IODevice::loop() {}
void IODevice::DumpAll() {
DIAG(F("NO HAL CONFIGURED!"));
@ -330,7 +349,7 @@ void ArduinoPins::_write(VPIN vpin, int value) {
}
}
// Device-specific read function.
// Device-specific read function (digital input).
int ArduinoPins::_read(VPIN vpin) {
int pin = vpin;
uint8_t mask = 1 << ((pin-_firstVpin) % 8);
@ -352,6 +371,39 @@ int ArduinoPins::_read(VPIN vpin) {
return value;
}
// Device-specific readAnalogue function (analogue input)
int ArduinoPins::_readAnalogue(VPIN vpin) {
int pin = vpin;
uint8_t mask = 1 << ((pin-_firstVpin) % 8);
uint8_t index = (pin-_firstVpin) / 8;
if (_pinModes[index] & mask) {
// Currently in write mode, change to read mode
_pinModes[index] &= ~mask;
// Since mode changes should be infrequent, use standard pinMode function
if (_pinPullups[index] & mask)
pinMode(pin, INPUT_PULLUP);
else
pinMode(pin, INPUT);
}
// Since AnalogRead is also called from interrupt code, disable interrupts
// while we're using it. There's only one ADC shared by all analogue inputs
// on the Arduino, so we don't want interruptions.
//******************************************************************************
// NOTE: If the HAL is running on a computer without the DCC signal generator,
// then interrupts needn't be disabled. Also, the DCC signal generator puts
// the ADC into fast mode, so if it isn't present, analogueRead calls will be much
// slower!!
//******************************************************************************
noInterrupts();
int value = analogRead(pin);
interrupts();
#ifdef DIAG_IO
//DIAG(F("Arduino Read Pin:%d Value:%d"), pin, value);
#endif
return value;
}
void ArduinoPins::_display() {
DIAG(F("Arduino Vpins:%d-%d"), (int)_firstVpin, (int)_firstVpin+_nPins-1);
}

View File

@ -141,6 +141,9 @@ public:
// read invokes the IODevice instance's _read method.
static int read(VPIN vpin);
// read invokes the IODevice instance's _readAnalogue method.
static int readAnalogue(VPIN vpin);
// loop invokes the IODevice instance's _loop method.
static void loop();
@ -188,12 +191,18 @@ protected:
return false;
}
// Method to read pin state (optionally implemented within device class)
// Method to read digital pin state (optionally implemented within device class)
virtual int _read(VPIN vpin) {
(void)vpin;
return 0;
};
// Method to read analogue pin state (optionally implemented within device class)
virtual int _readAnalogue(VPIN vpin) {
(void)vpin;
return 0;
};
// _isBusy returns true if the device is currently in an animation of some sort, e.g. is changing
// the output over a period of time. Returns false unless overridden in sub class.
virtual bool _isBusy(VPIN vpin) {
@ -343,8 +352,9 @@ private:
bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) override;
// Device-specific write function.
void _write(VPIN vpin, int value) override;
// Device-specific read function.
// Device-specific read functions.
int _read(VPIN vpin) override;
int _readAnalogue(VPIN vpin) override;
void _display() override;

131
IO_AnalogueInputs.h Normal file
View File

@ -0,0 +1,131 @@
/*
* © 2021, Neil McKechnie. All rights reserved.
*
* This file is part of DCC++EX 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 <https://www.gnu.org/licenses/>.
*/
#ifndef io_analogueinputs_h
#define io_analogueinputs_h
// Uncomment following line to slow the scan cycle down to 1second ADC samples, with
// diagnostic output of scanned values.
//#define IO_ANALOGUE_SLOW
#include "IODevice.h"
#include "I2CManager.h"
#include "DIAG.h"
#include "FSH.h"
/**********************************************************************************************
* ADS111x class for I2C-connected analogue input modules ADS1113, ADS1114 and ADS1115.
*
* ADS1113 and ADS1114 are restricted to 1 input. ADS1115 has a multiplexer which allows
* any of four input pins to be read by its ADC.
*
* The driver polls the device in accordance with the constant 'scanInterval' below. On first loop
* entry, the multiplexer is set to pin A0 and the ADC is triggered. On second and subsequent
* entries, the analogue value is read from the conversion register and then the multiplexer and
* ADC are set up to read the next pin.
*
* The ADS111x is set up as follows:
* Single-shot scan
* Data rate 128 samples/sec (7.8ms/sample)
* Comparator off
* Gain FSR=6.144V
* The gain means that the maximum input voltage of 5V (when Vss=5V) gives a reading
* of 32767*(5.0/6.144) = 26666.
*
* Note: The device is simple and does not need initial configuration, so it should recover from
* temporary loss of communications or power.
**********************************************************************************************/
class ADS111x: public IODevice {
public:
ADS111x(VPIN firstVpin, int nPins, uint8_t i2cAddress) {
_firstVpin = firstVpin;
_nPins = min(nPins,4);
_i2cAddress = i2cAddress;
_currentPin = _nPins; // Suppress read on first loop entry.
addDevice(this);
}
static void create(VPIN firstVpin, int nPins, uint8_t i2cAddress) {
new ADS111x(firstVpin, nPins, i2cAddress);
}
void _begin() {
// Initialise ADS device
if (I2CManager.exists(_i2cAddress)) {
#ifdef DIAG_IO
_display();
#endif
} else {
DIAG(F("ADS111x device not found, I2C:%x"), _i2cAddress);
}
}
void _loop(unsigned long currentMicros) {
if (currentMicros - _lastMicros >= scanInterval) {
// Check that previous non-blocking write has completed, if not then wait
_i2crb.wait();
// If _currentPin is in the valid range, continue reading the pin values
if (_currentPin < _nPins) {
_outBuffer[0] = 0x00; // Conversion register address
uint8_t status = I2CManager.read(_i2cAddress, _inBuffer, 2, 1, _outBuffer); // Read register
if (status == I2C_STATUS_OK) {
_value[_currentPin] = ((uint16_t)_inBuffer[0] << 8) + (uint16_t)_inBuffer[1];
#ifdef IO_ANALOGUE_SLOW
DIAG(F("ADS111x pin:%d value:%d"), _currentPin, _value[_currentPin]);
#endif
}
}
// Move to next pin
if (++_currentPin >= _nPins) _currentPin = 0;
// Configure ADC and multiplexer for next scan. See ADS111x datasheet for details
// of configuration register settings.
_outBuffer[0] = 0x01; // Config register address
_outBuffer[1] = 0xC0 + (_currentPin << 4); // Trigger single-shot, channel n
_outBuffer[2] = 0x83; // 128 samples/sec, comparator off
// Write command, without waiting for completion.
I2CManager.write(_i2cAddress, _outBuffer, 3, &_i2crb);
_lastMicros = currentMicros;
}
}
int _readAnalogue(VPIN vpin) {
int pin = vpin - _firstVpin;
return _value[pin];
}
void _display() {
DIAG(F("ADS111x I2C:x%x Configured on Vpins:%d-%d"), _i2cAddress, _firstVpin, _firstVpin+_nPins-1);
}
protected:
// With ADC set to 128 samples/sec, that's 7.8ms/sample. So set the period between updates to 10ms
#ifndef IO_ANALOGUE_SLOW
const unsigned long scanInterval = 10000UL; // Period between successive ADC scans in microseconds.
#else
const unsigned long scanInterval = 1000000UL; // Period between successive ADC scans in microseconds.
#endif
uint16_t _value[4];
uint8_t _i2cAddress;
uint8_t _outBuffer[3];
uint8_t _inBuffer[2];
uint8_t _currentPin; // ADC pin currently being scanned
unsigned long _lastMicros = 0;
I2CRB _i2crb;
};
#endif // io_analogueinputs_h

View File

@ -275,6 +275,7 @@ RMFT2::RMFT2(int progCtr) {
forward=true;
invert=false;
stackDepth=0;
onTurnoutId=0; // Not handling an ONTHROW/ONCLOSE
// chain into ring of RMFTs
if (loopTask==NULL) {
@ -329,11 +330,16 @@ void RMFT2::driveLoco(byte speed) {
speedo=speed;
}
bool RMFT2::readSensor(int16_t sensorId) {
VPIN vpin=abs(sensorId);
bool RMFT2::readSensor(uint16_t sensorId) {
// Exrail operands are unsigned but we need the signed version as inserted by the macros.
int16_t sId=(int16_t) sensorId;
VPIN vpin=abs(sId);
if (getFlag(vpin,LATCH_FLAG)) return true; // latched on
bool s= IODevice::read(vpin) ^ (sensorId<0);
if (s && diag) DIAG(F("EXRAIL Sensor %d hit"),sensorId);
// negative sensorIds invert the logic (e.g. for a break-beam sensor which goes OFF when detecting)
bool s= IODevice::read(vpin) ^ (sId<0);
if (s && diag) DIAG(F("EXRAIL Sensor %d hit"),sId);
return s;
}
@ -524,11 +530,19 @@ void RMFT2::loop2() {
case OPCODE_FON:
if (loco) DCC::setFn(loco,operand,true);
break;
case OPCODE_FOFF:
if (loco) DCC::setFn(loco,operand,false);
break;
case OPCODE_XFON:
DCC::setFn(operand,GET_OPERAND(1),true);
break;
case OPCODE_XFOFF:
DCC::setFn(operand,GET_OPERAND(1),false);
break;
case OPCODE_FOLLOW:
progCounter=locateRouteStart(operand);
if (progCounter<0) kill(F("FOLLOW unknown"), operand);
@ -625,7 +639,7 @@ void RMFT2::loop2() {
case OPCODE_ROUTE:
case OPCODE_AUTOMATION:
case OPCODE_SEQUENCE:
DIAG(F("EXRAIL begin(%d)"),operand);
if (diag) DIAG(F("EXRAIL begin(%d)"),operand);
break;
case OPCODE_PAD: // Just a padding for previous opcode needing >1 operad byte.
@ -674,26 +688,39 @@ 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(1);
byte redpin=GET_OPERAND(0);
if (redpin!=id)continue;
byte amberpin=GET_OPERAND(2);
byte greenpin=GET_OPERAND(3);
byte amberpin=GET_OPERAND(1);
byte 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);
if (greenpin) IODevice::write(amberpin,green || (amber && (amberpin==0)));
if (greenpin) IODevice::write(greenpin,green || (amber && (amberpin==0)));
return;
}
}
void RMFT2::turnoutEvent(VPIN id, bool closed) {
void RMFT2::turnoutEvent(int16_t turnoutId, bool closed) {
// Check we dont already have a task running this turnout
RMFT2 * task=loopTask;
while(task) {
if (task->onTurnoutId==turnoutId) {
DIAG(F("Recursive ONTHROW/ONCLOSE for Turnout %d"),turnoutId);
return;
}
task=task->next;
if (task==loopTask) break;
}
// Hunt for an ONTHROW/ONCLOSE for this turnout
byte huntFor=closed ? OPCODE_ONCLOSE : OPCODE_ONTHROW ;
// caution hides class progCounter;
for (int progCounter=0;; SKIPOP){
byte opcode=GET_OPCODE;
if (opcode==OPCODE_ENDEXRAIL) return;
if (opcode!=huntFor) continue;
if (id!=GET_OPERAND(0)) continue;
new RMFT2(progCounter); // new task starts at this instruction
if (turnoutId!=(int16_t)GET_OPERAND(0)) continue;
task=new RMFT2(progCounter); // new task starts at this instruction
task->onTurnoutId=turnoutId; // flag for recursion detector
return;
}
}

11
RMFT2.h
View File

@ -34,7 +34,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
OPCODE_LATCH,OPCODE_UNLATCH,OPCODE_SET,OPCODE_RESET,
OPCODE_IF,OPCODE_IFNOT,OPCODE_ENDIF,OPCODE_IFRANDOM,OPCODE_IFRESERVE,
OPCODE_DELAY,OPCODE_DELAYMINS,OPCODE_RANDWAIT,
OPCODE_FON,OPCODE_FOFF,
OPCODE_FON,OPCODE_FOFF,OPCODE_XFON,OPCODE_XFOFF,
OPCODE_RED,OPCODE_GREEN,OPCODE_AMBER,
OPCODE_SERVO,OPCODE_SIGNAL,OPCODE_TURNOUT,OPCODE_WAITFOR,
OPCODE_PAD,OPCODE_FOLLOW,OPCODE_CALL,OPCODE_RETURN,
@ -68,7 +68,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
static void readLocoCallback(int16_t cv);
static void emitWithrottleRouteList(Print* stream);
static void createNewTask(int route, uint16_t cab);
static void turnoutEvent(VPIN id, bool closed);
static void turnoutEvent(int16_t id, bool closed);
private:
static void ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]);
static bool parseSlash(Print * stream, byte & paramCount, int16_t p[]) ;
@ -85,7 +85,7 @@ private:
static RMFT2 * pausingTask;
void delayMe(long millisecs);
void driveLoco(byte speedo);
bool readSensor(int16_t sensorId);
bool readSensor(uint16_t sensorId);
bool skipIfBlock();
bool readLoco();
void loop2();
@ -106,10 +106,11 @@ private:
unsigned long delayTime;
byte taskId;
int loco;
uint16_t loco;
bool forward;
bool invert;
int speedo;
byte speedo;
int16_t onTurnoutId;
byte stackDepth;
int callStack[MAX_STACK_DEPTH];
};

View File

@ -19,8 +19,9 @@
#ifndef RMFTMacros_H
#define RMFTMacros_H
// remove normal code LCD macro (will be restored later)
// remove normal code LCD & SERIAL macros (will be restored later)
#undef LCD
#undef SERIAL
// This file will include and build the EXRAIL script and associated helper tricks.
@ -83,6 +84,7 @@
#define JOIN
#define LATCH(sensor_id)
#define LCD(row,msg)
#define LCN(msg)
#define ONCLOSE(turnout_id)
#define ONTHROW(turnout_id)
#define PAUSE
@ -97,6 +99,10 @@
#define REV(speed)
#define START(route)
#define SENDLOCO(cab,route)
#define SERIAL(msg)
#define SERIAL1(msg)
#define SERIAL2(msg)
#define SERIAL3(msg)
#define SERVO(id,position,profile)
#define SERVO2(id,position,duration)
#define SETLOCO(loco)
@ -113,6 +119,8 @@
#define UNJOIN
#define UNLATCH(sensor_id)
#define WAITFOR(pin)
#define XFOFF(cab,func)
#define XFON(cab,func)
#include "myAutomation.h"
@ -125,14 +133,24 @@
#undef EXRAIL
#undef PRINT
#undef LCN
#undef SERIAL
#undef SERIAL1
#undef SERIAL2
#undef SERIAL3
#undef ENDEXRAIL
#undef LCD
const int StringMacroTracker1=__COUNTER__;
#define ALIAS(name,value)
#define EXRAIL void RMFT2::printMessage(uint16_t id) { switch(id) {
#define ENDEXRAIL default: DIAG(F("printMessage error %d %d"),id,StringMacroTracker1); return ; }}
#define PRINT(msg) case (__COUNTER__ - StringMacroTracker1) : printMessage2(F(msg));break;
#define LCD(id,msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::lcd(id,F(msg));break;
#define PRINT(msg) case (__COUNTER__ - StringMacroTracker1) : printMessage2(F(msg));break;
#define LCN(msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&LCN_SERIAL,F(msg));break;
#define SERIAL(msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial,F(msg));break;
#define SERIAL1(msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial1,F(msg));break;
#define SERIAL2(msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(L&Serial2,F(msg));break;
#define SERIAL3(msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial3,F(msg));break;
#define LCD(id,msg) case (__COUNTER__ - StringMacroTracker1) : StringFormatter::lcd(id,F(msg));break;
#include "myAutomation.h"
// Setup for Pass 3: create main routes table
@ -165,6 +183,7 @@ const int StringMacroTracker1=__COUNTER__;
#undef JOIN
#undef LATCH
#undef LCD
#undef LCN
#undef ONCLOSE
#undef ONTHROW
#undef PAUSE
@ -184,6 +203,10 @@ const int StringMacroTracker1=__COUNTER__;
#undef SERVO2
#undef FADE
#undef SENDLOCO
#undef SERIAL
#undef SERIAL1
#undef SERIAL2
#undef SERIAL3
#undef SETLOCO
#undef SET
#undef SPEED
@ -196,6 +219,8 @@ const int StringMacroTracker1=__COUNTER__;
#undef UNJOIN
#undef UNLATCH
#undef WAITFOR
#undef XFOFF
#undef XFON
// Define macros for route code creation
#define V(val) ((int16_t)(val))&0x00FF,((int16_t)(val)>>8)&0x00FF
@ -234,7 +259,8 @@ const int StringMacroTracker1=__COUNTER__;
#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) OPCODE_PRINT,V(__COUNTER__ - StringMacroTracker2),
#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,NOP,
@ -248,6 +274,10 @@ const int StringMacroTracker1=__COUNTER__;
#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)
#define SERIAL1(msg) PRINT(msg)
#define SERIAL2(msg) PRINT(msg)
#define SERIAL3(msg) PRINT(msg)
#define START(route) OPCODE_START,V(route),
#define SERVO(id,position,profile) OPCODE_SERVO,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(PCA9685::profile),OPCODE_PAD,V(0),
#define SERVO2(id,position,ms) OPCODE_SERVO,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(PCA9685::Instant),OPCODE_PAD,V(ms/100L),
@ -263,13 +293,16 @@ const int StringMacroTracker1=__COUNTER__;
#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),
#define XFON(cab,func) OPCODE_XFON,V(cab),OPCODE_PAD,V(func),
// PASS2 Build RouteCode
const int StringMacroTracker2=__COUNTER__;
#include "myAutomation.h"
// Restore normal code LCD macro
// Restore normal code LCD & SERIAL macro
#undef LCD
#define LCD StringFormatter::lcd
#undef SERIAL
#define SERIAL 0x0
#endif

View File

@ -119,7 +119,7 @@
// Send message to JMRI etc. over Serial USB. This is done here
// to ensure that the message is sent when the turnout operation
// is not initiated by a Serial command.
printState(id, &Serial);
tt->printState(&Serial);
return true;
}
@ -140,6 +140,8 @@
bool ok = tt->setClosedInternal(closeFlag);
if (ok) {
turnoutlistHash++; // let withrottle know something changed
// Write byte containing new closed/thrown state to EEPROM if required. Note that eepromAddress
// is always zero for LCN turnouts.
if (EEStore::eeStore->data.nTurnouts > 0 && tt->_eepromAddress > 0)
@ -152,7 +154,7 @@
// Send message to JMRI etc. over Serial USB. This is done here
// to ensure that the message is sent when the turnout operation
// is not initiated by a Serial command.
printState(id, &Serial);
tt->printState(&Serial);
}
return ok;
}
@ -214,7 +216,7 @@
// Display, on the specified stream, the current state of the turnout (1=thrown or 0=closed).
/* static */ void Turnout::printState(uint16_t id, Print *stream) {
Turnout *tt = get(id);
if (!tt) tt->printState(stream);
if (tt) tt->printState(stream);
}

View File

@ -16,6 +16,7 @@ default_envs =
unowifiR2
nano
src_dir = .
include_dir = .
[env]
build_flags = -Wall -Wextra
@ -41,7 +42,7 @@ lib_deps =
SPI
monitor_speed = 115200
monitor_flags = --echo
build_flags = -DDIAG_IO
build_flags = -DDIAG_IO -DDIAG_LOOPTIMES
[env:mega2560-no-HAL]
platform = atmelavr