Commit e2c7ee41 authored by Lukáš Voborský's avatar Lukáš Voborský Committed by Nathaniel Wesley Filardo
Browse files

NmraDcc upstream version

parent 56c01fa5
...@@ -2,11 +2,21 @@ ...@@ -2,11 +2,21 @@
// //
// Model Railroading with Arduino - NmraDcc.cpp // Model Railroading with Arduino - NmraDcc.cpp
// //
// Copyright (c) 2008 - 2017 Alex Shepherd // Copyright (c) 2008 - 2020 Alex Shepherd
// //
// This source file is subject of the GNU general public license 2, // This library is free software; you can redistribute it and/or
// that is available at the world-wide-web at // modify it under the terms of the GNU Lesser General Public
// http://www.gnu.org/licenses/gpl.txt // License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library 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
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
// //
//------------------------------------------------------------------------ //------------------------------------------------------------------------
// //
...@@ -21,44 +31,25 @@ ...@@ -21,44 +31,25 @@
// and new signature of notifyDccSpeed and notifyDccFunc // and new signature of notifyDccSpeed and notifyDccFunc
// 2015-12-16 Version without use of Timer0 by Franz-Peter Müller // 2015-12-16 Version without use of Timer0 by Franz-Peter Müller
// 2016-07-16 handle glitches on DCC line // 2016-07-16 handle glitches on DCC line
// 2016-08-20 added ESP8266 support by Sven (littleyoda) // 2016-08-20 added ESP8266 support by Sven (littleyoda)
// 2017-01-19 added STM32F1 support by Franz-Peter // 2017-01-19 added STM32F1 support by Franz-Peter
// 2017-11-29 Ken West (kgw4449@gmail.com): // 2017-11-29 Ken West (kgw4449@gmail.com):
// Minor fixes to pass NMRA Baseline Conformance Tests. // Minor fixes to pass NMRA Baseline Conformance Tests.
// 2018-12-17 added ESP32 support by Trusty (thierry@lapajaparis.net) // 2018-12-17 added ESP32 support by Trusty (thierry@lapajaparis.net)
// 2019-02-17 added ESP32 specific changes by Hans Tanner // 2019-02-17 added ESP32 specific changes by Hans Tanner
// // 2020-05-15 changes to pass NMRA Tests ( always search for preamble )
//------------------------------------------------------------------------ //------------------------------------------------------------------------
// //
// purpose: Provide a simplified interface to decode NMRA DCC packets // purpose: Provide a simplified interface to decode NMRA DCC packets
// and build DCC Mobile and Stationary Decoders // and build DCC Mobile and Stationary Decoders
// //
//------------------------------------------------------------------------ //------------------------------------------------------------------------
// NodeMCU Lua port by @voborsky #include "NmraDcc.h"
#include "EEPROM.h"
// #define NODE_DEBUG
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include "platform.h"
#include "user_interface.h"
#include "task/task.h"
#include "driver/NmraDcc.h"
#define BYTE_TO_BINARY_PATTERN "%c%c%c%c%c%c%c%c"
#define BYTE_TO_BINARY(byte) \
(byte & 0x80 ? '1' : '0'), \
(byte & 0x40 ? '1' : '0'), \
(byte & 0x20 ? '1' : '0'), \
(byte & 0x10 ? '1' : '0'), \
(byte & 0x08 ? '1' : '0'), \
(byte & 0x04 ? '1' : '0'), \
(byte & 0x02 ? '1' : '0'), \
(byte & 0x01 ? '1' : '0')
// Uncomment to print DEBUG messages
// #define DEBUG_PRINT
//------------------------------------------------------------------------ //------------------------------------------------------------------------
// DCC Receive Routine // DCC Receive Routine
...@@ -91,50 +82,149 @@ ...@@ -91,50 +82,149 @@
// DCC 1: _________XXXXXXXXX_________XXXXXXXXX_________ // DCC 1: _________XXXXXXXXX_________XXXXXXXXX_________
// |<--------146us------>| // |<--------146us------>|
// ^-INTx ^-INTx // ^-INTx ^-INTx
// less than 138us: its a one-Bit // less than 146us: its a one-Bit
// //
// //
// |<-----------------232us----------->| // |<-----------------232us----------->|
// DCC 0: _________XXXXXXXXXXXXXXXXXX__________________XXXXXXXX__________ // DCC 0: _________XXXXXXXXXXXXXXXXXX__________________XXXXXXXX__________
// |<--------146us------->| // |<--------146us------->|
// ^-INTx ^-INTx // ^-INTx ^-INTx
// greater than 138us: its a zero bit // greater than 146us: its a zero bit
// //
// //
// //
// //
//------------------------------------------------------------------------
#define abs(a) ((a) > 0 ? (a) : (0-a)) //------------------------------------------------------------------------
// if this is commented out, bit synchronisation is only done after a wrong checksum
#define SYNC_ALWAYS
// if this is commented out, Zero-Bit_Stretching is not supported
// ( Bits longer than 2* MAX ONEBIT are treated as error )
#define SUPPORT_ZERO_BIT_STRETCHING
#define MAX_ONEBITFULL 146 #define MAX_ONEBITFULL 146
#define MAX_PRAEAMBEL 146 #define MAX_PRAEAMBEL 146
#define MAX_ONEBITHALF 82 #define MAX_ONEBITHALF 82
#define MIN_ONEBITFULL 82 #define MIN_ONEBITFULL 82
#define MIN_ONEBITHALF 35 #define MIN_ONEBITHALF 35
#define MAX_BITDIFF 18 #define MAX_BITDIFF 24
// Debug-Ports
#ifdef NODE_DEBUG //#define debug // Testpulse for logic analyser
#define PULLUP PLATFORM_GPIO_PULLUP #ifdef debug
#define OUTPUT PLATFORM_GPIO_OUTPUT #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define HIGH PLATFORM_GPIO_HIGH #define MODE_TP1 DDRF |= (1<<2) //pinA2
#define LOW PLATFORM_GPIO_LOW #define SET_TP1 PORTF |= (1<<2)
#define CLR_TP1 PORTF &= ~(1<<2)
#define MODE_TP2 DDRF |= (1<<3) //pinA3
#define SET_TP2 PORTF |= (1<<3)
#define CLR_TP2 PORTF &= ~(1<<3)
#define MODE_TP3 DDRF |= (1<<4) //pinA4
#define SET_TP3 PORTF |= (1<<4)
#define CLR_TP3 PORTF &= ~(1<<4)
#define MODE_TP4 DDRF |= (1<<5) //pinA5
#define SET_TP4 PORTF |= (1<<5)
#define CLR_TP4 PORTF &= ~(1<<5)
#elif defined(__AVR_ATmega32U4__)
#define MODE_TP1 DDRF |= (1<<4) //A3
#define SET_TP1 PORTF |= (1<<4)
#define CLR_TP1 PORTF &= ~(1<<4)
#define MODE_TP2 DDRF |= (1<<5) //A2
#define SET_TP2 PORTF |= (1<<5)
#define CLR_TP2 PORTF &= ~(1<<5)
#define MODE_TP3
#define SET_TP3
#define CLR_TP3
#define MODE_TP4
#define SET_TP4
#define CLR_TP4
#elif defined(__AVR_ATmega328P__)
#define MODE_TP1 DDRC |= (1<<1) //A1
#define SET_TP1 PORTC |= (1<<1)
#define CLR_TP1 PORTC &= ~(1<<1)
#define MODE_TP2 DDRC |= (1<<2) // A2
#define SET_TP2 PORTC |= (1<<2)
#define CLR_TP2 PORTC &= ~(1<<2)
#define MODE_TP3 DDRC |= (1<<3) //A3
#define SET_TP3 PORTC |= (1<<3)
#define CLR_TP3 PORTC &= ~(1<<3)
#define MODE_TP4 DDRC |= (1<<4) //A4
#define SET_TP4 PORTC |= (1<<4)
#define CLR_TP4 PORTC &= ~(1<<4)
#elif defined(__arm__) && (defined(__MK20DX128__) || defined(__MK20DX256__))
// Teensys 3.x
#define MODE_TP1 pinMode( A1,OUTPUT ) // A1= PortC, Bit0
#define SET_TP1 GPIOC_PSOR = 0x01
#define CLR_TP1 GPIOC_PCOR = 0x01
#define MODE_TP2 pinMode( A2,OUTPUT ) // A2= PortB Bit0
#define SET_TP2 GPIOB_PSOR = 0x01
#define CLR_TP2 GPIOB_PCOR = 0x01
#define MODE_TP3 pinMode( A3,OUTPUT ) // A3 = PortB Bit1
#define SET_TP3 GPIOB_PSOR = 0x02
#define CLR_TP3 GPIOB_PCOR = 0x02
#define MODE_TP4 pinMode( A4,OUTPUT ) // A4 = PortB Bit3
#define SET_TP4 GPIOB_PSOR = 0x08
#define CLR_TP4 GPIOB_PCOR = 0x08
#elif defined (__STM32F1__)
// STM32F103...
#define MODE_TP1 pinMode( PB12,OUTPUT ) // TP1= PB12
#define SET_TP1 gpio_write_bit( GPIOB,12, HIGH );
#define CLR_TP1 gpio_write_bit( GPIOB,12, LOW );
#define MODE_TP2 pinMode( PB13,OUTPUT ) // TP2= PB13
#define SET_TP2 gpio_write_bit( GPIOB,13, HIGH );
#define CLR_TP2 gpio_write_bit( GPIOB,13, LOW );
#define MODE_TP3 pinMode( PB14,OUTPUT ) // TP3 = PB14
#define SET_TP3 gpio_write_bit( GPIOB,14, HIGH );
#define CLR_TP3 gpio_write_bit( GPIOB,14, LOW );
#define MODE_TP4 pinMode( PB15,OUTPUT ) // TP4 = PB15
#define SET_TP4 gpio_write_bit( GPIOB,15, HIGH );
#define CLR_TP4 gpio_write_bit( GPIOB,15, LOW );
#elif defined(ESP8266)
#define MODE_TP1 pinMode( D5,OUTPUT ) ; // GPIO 14
#define SET_TP1 GPOS = (1 << D5);
#define CLR_TP1 GPOC = (1 << D5);
#define MODE_TP2 pinMode( D6,OUTPUT ) ; // GPIO 12
#define SET_TP2 GPOS = (1 << D6);
#define CLR_TP2 GPOC = (1 << D6);
#define MODE_TP3 pinMode( D7,OUTPUT ) ; // GPIO 13
#define SET_TP3 GPOS = (1 << D7);
#define CLR_TP3 GPOC = (1 << D7);
#define MODE_TP4 pinMode( D8,OUTPUT ) ; // GPIO 15
#define SET_TP4 GPOS = (1 << D8);
#define CLR_TP4 GPOC = (1 << D8);
#elif defined(ESP32)
#define MODE_TP1 pinMode( 33,OUTPUT ) ; // GPIO 33
#define SET_TP1 GPOS = (1 << 33);
#define CLR_TP1 GPOC = (1 << 33);
#define MODE_TP2 pinMode( 25,OUTPUT ) ; // GPIO 25
#define SET_TP2 GPOS = (1 << 25);
#define CLR_TP2 GPOC = (1 << 25);
#define MODE_TP3 pinMode( 26,OUTPUT ) ; // GPIO 26
#define SET_TP3 GPOS = (1 << 26);
#define CLR_TP3 GPOC = (1 << 26);
#define MODE_TP4 pinMode( 27,OUTPUT ) ; // GPIO 27
#define SET_TP4 GPOS = (1 << 27);
#define CLR_TP4 GPOC = (1 << 27);
//#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
#else
#define MODE_TP1
#define SET_TP1
#define CLR_TP1
#define MODE_TP2
#define SET_TP2
#define CLR_TP2
#define MODE_TP3
#define SET_TP3
#define CLR_TP3
#define MODE_TP4
#define SET_TP4
#define CLR_TP4
#define MODE_TP1 platform_gpio_mode( 5, OUTPUT, PULLUP ); // GPIO 14 #endif
#define SET_TP1 platform_gpio_write(5, HIGH);
#define CLR_TP1 platform_gpio_write(5, LOW);
#define MODE_TP2 platform_gpio_mode( 6, OUTPUT, PULLUP ); // GPIO 12
#define SET_TP2 platform_gpio_write(6, HIGH);
#define CLR_TP2 platform_gpio_write(6, LOW);
#define MODE_TP3 platform_gpio_mode( 7, OUTPUT, PULLUP ); // GPIO 13
#define SET_TP3 platform_gpio_write(7, HIGH);
#define CLR_TP3 platform_gpio_write(7, LOW);
#define MODE_TP4 platform_gpio_mode( 8, OUTPUT, PULLUP ); // GPIO 15
#define SET_TP4 platform_gpio_write(8, HIGH);
#define CLR_TP4 platform_gpio_write(8, LOW);
#else #else
#define MODE_TP1 #define MODE_TP1
#define SET_TP1 #define SET_TP1
...@@ -148,17 +238,40 @@ ...@@ -148,17 +238,40 @@
#define MODE_TP4 #define MODE_TP4
#define SET_TP4 #define SET_TP4
#define CLR_TP4 #define CLR_TP4
#endif
#ifdef DEBUG_PRINT
#define DB_PRINT( x, ... ) { char dbgbuf[80]; sprintf_P( dbgbuf, (const char*) F( x ) , ##__VA_ARGS__ ) ; Serial.println( dbgbuf ); }
#define DB_PRINT_( x, ... ) { char dbgbuf[80]; sprintf_P( dbgbuf, (const char*) F( x ) , ##__VA_ARGS__ ) ; Serial.print( dbgbuf ); }
#else
#define DB_PRINT( x, ... ) ;
#define DB_PRINT_( x, ... ) ;
#endif #endif
static uint8_t ISREdge; // Holder of the Next Edge we're looking for: RISING or FALLING #ifdef DCC_DBGVAR
static int16_t bitMax, bitMin; struct countOf_t countOf;
#endif
DCC_MSG Msg ; #if defined ( __STM32F1__ )
static ExtIntTriggerMode ISREdge;
#elif defined ( ESP32 )
static byte ISREdge; // Holder of the Next Edge we're looking for: RISING or FALLING
static byte ISRWatch; // Interrupt Handler Edge Filter
#else
static byte ISREdge; // Holder of the Next Edge we're looking for: RISING or FALLING
static byte ISRWatch; // Interrupt Handler Edge Filter
#endif
byte ISRLevel; // expected Level at DCC input during ISR ( to detect glitches )
byte ISRChkMask; // Flag if Level must be checked
static word bitMax, bitMin;
typedef enum typedef enum
{ {
WAIT_PREAMBLE = 0, WAIT_PREAMBLE = 0,
WAIT_START_BIT, WAIT_START_BIT,
#ifndef SYNC_ALWAYS
WAIT_START_BIT_FULL,
#endif
WAIT_DATA, WAIT_DATA,
WAIT_END_BIT WAIT_END_BIT
} }
...@@ -176,8 +289,10 @@ OpsInstructionType; ...@@ -176,8 +289,10 @@ OpsInstructionType;
struct DccRx_t struct DccRx_t
{ {
DccRxWaitState State ; DccRxWaitState State ;
uint8_t DataReady ;
uint8_t BitCount ; uint8_t BitCount ;
uint8_t TempByte ; uint8_t TempByte ;
uint8_t chkSum;
DCC_MSG PacketBuf; DCC_MSG PacketBuf;
DCC_MSG PacketCopy; DCC_MSG PacketCopy;
} }
...@@ -192,86 +307,163 @@ typedef struct ...@@ -192,86 +307,163 @@ typedef struct
uint8_t PageRegister ; // Used for Paged Operations in Service Mode Programming uint8_t PageRegister ; // Used for Paged Operations in Service Mode Programming
uint8_t DuplicateCount ; uint8_t DuplicateCount ;
DCC_MSG LastMsg ; DCC_MSG LastMsg ;
uint8_t IntPin; uint8_t ExtIntNum;
uint8_t IntBitmask; uint8_t ExtIntPinNum;
int16_t myDccAddress; // Cached value of DCC Address from CVs volatile uint8_t *ExtIntPort; // use port and bitmask to read input at AVR in ISR
uint8_t ExtIntMask; // digitalRead is too slow on AVR
int16_t myDccAddress; // Cached value of DCC Address from CVs
uint8_t inAccDecDCCAddrNextReceivedMode; uint8_t inAccDecDCCAddrNextReceivedMode;
uint8_t cv29Value;
#ifdef DCC_DEBUG #ifdef DCC_DEBUG
uint8_t IntCount; uint8_t IntCount;
uint8_t TickCount; uint8_t TickCount;
uint8_t NestedIrqCount;
#endif #endif
} }
DCC_PROCESSOR_STATE ; DCC_PROCESSOR_STATE ;
DCC_PROCESSOR_STATE DccProcState ; DCC_PROCESSOR_STATE DccProcState ;
task_handle_t DataReady_taskid; #ifdef ESP32
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status) void IRAM_ATTR ExternalInterruptHandler(void)
#elif defined(ESP8266)
void ICACHE_RAM_ATTR ExternalInterruptHandler(void)
#else
void ExternalInterruptHandler(void)
#endif
{ {
// This function really is running at interrupt level with everything SET_TP3;
// else masked off. It should take as little time as necessary.
#ifdef ESP32
// switch (ISRWatch)
// {
// case RISING: if (digitalRead(DccProcState.ExtIntPinNum)) break;
// case FALLING: if (digitalRead(DccProcState.ExtIntPinNum)) return; break;
// }
// First compare the edge we're looking for to the pin state
switch (ISRWatch)
{
case CHANGE:
break;
case RISING:
if (digitalRead(DccProcState.ExtIntPinNum) != HIGH)
return;
break;
case FALLING:
if (digitalRead(DccProcState.ExtIntPinNum) != LOW)
return;
break;
}
#endif
// Bit evaluation without Timer 0 ------------------------------
uint8_t DccBitVal;
static int8_t bit1, bit2 ;
static unsigned int lastMicros = 0;
static byte halfBit, DCC_IrqRunning, preambleBitCount;
unsigned int actMicros, bitMicros;
#ifdef ALLOW_NESTED_IRQ
if ( DCC_IrqRunning ) {
// nested DCC IRQ - obviously there are glitches
// ignore this interrupt and increment glitchcounter
CLR_TP3;
#ifdef DCC_DEBUG
DccProcState.NestedIrqCount++;
#endif
SET_TP3;
return; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> abort IRQ
}
#endif
actMicros = micros();
bitMicros = actMicros-lastMicros;
uint32 gpio_status = GPIO_REG_READ(GPIO_STATUS_ADDRESS); CLR_TP3; SET_TP3;
if ((gpio_status & DccProcState.IntBitmask) == 0) { #ifdef __AVR_MEGA__
return ret_gpio_status; if ( bitMicros < bitMin || ( DccRx.State != WAIT_START_BIT && (*DccProcState.ExtIntPort & DccProcState.ExtIntMask) != (ISRLevel) ) ) {
} #else
if ( bitMicros < bitMin || ( DccRx.State != WAIT_START_BIT && digitalRead( DccProcState.ExtIntPinNum ) != (ISRLevel) ) ) {
GPIO_REG_WRITE(GPIO_STATUS_W1TC_ADDRESS, gpio_status & DccProcState.IntBitmask); #endif
uint32_t actMicros = system_get_time(); // too short - my be false interrupt due to glitch or false protocol or level does not match RISING / FALLING edge -> ignore this IRQ
ret_gpio_status &= ~(DccProcState.IntBitmask); CLR_TP3;
SET_TP4; /*delayMicroseconds(1); */ CLR_TP4;
// Bit evaluation without Timer 0 ------------------------------ return; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> abort IRQ
uint8_t DccBitVal; }
static int8_t bit1, bit2 ; CLR_TP3; SET_TP3;
static unsigned long lastMicros = 0;
static uint8_t halfBit;
unsigned long bitMicros;
SET_TP3;
bitMicros = actMicros-lastMicros;
if ( bitMicros < bitMin ) {
// too short - my be false interrupt due to glitch or false protocol -> ignore
CLR_TP3;
return ret_gpio_status; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> abort IRQ
}
DccBitVal = ( bitMicros < bitMax );
lastMicros = actMicros;
#ifdef NODE_DEBUG
if(DccBitVal) {SET_TP2;} else {CLR_TP2;};
#endif
#ifdef DCC_DEBUG
DccProcState.TickCount++;
#endif
lastMicros = actMicros;
#ifndef SUPPORT_ZERO_BIT_STRETCHING
//if ( bitMicros > MAX_ZEROBITFULL ) {
if ( bitMicros > (bitMax*2) ) {
// too long - my be false protocol -> start over
DccRx.State = WAIT_PREAMBLE ;
DccRx.BitCount = 0 ;
preambleBitCount = 0;
// SET_TP2; CLR_TP2;
bitMax = MAX_PRAEAMBEL;
bitMin = MIN_ONEBITFULL;
#if defined ( __STM32F1__ )
detachInterrupt( DccProcState.ExtIntNum );
#endif
#ifdef ESP32
ISRWatch = ISREdge;
#else
attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, ISREdge );
#endif
// enable level-checking
ISRChkMask = DccProcState.ExtIntMask;
ISRLevel = (ISREdge==RISING)? DccProcState.ExtIntMask : 0 ;
CLR_TP3;
//CLR_TP3;
return; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> abort IRQ
}
CLR_TP3;
SET_TP3;
#endif
DccBitVal = ( bitMicros < bitMax );
#ifdef ALLOW_NESTED_IRQ
DCC_IrqRunning = true;
interrupts(); // time critical is only the micros() command,so allow nested irq's
#endif
#ifdef DCC_DEBUG
DccProcState.TickCount++;
#endif
switch( DccRx.State ) switch( DccRx.State )
{ {
case WAIT_PREAMBLE: case WAIT_PREAMBLE:
if( DccBitVal ) // We don't have to do anything special - looking for a preamble condition is done always
{ SET_TP2;
SET_TP1; break;
DccRx.BitCount++;
if( DccRx.BitCount > 10 ) { #ifndef SYNC_ALWAYS
DccRx.State = WAIT_START_BIT ; case WAIT_START_BIT_FULL:
// While waiting for the start bit, detect halfbit lengths. We will detect the correct // wait for startbit without level checking
// sync and detect whether we see a false (e.g. motorola) protocol if ( !DccBitVal ) {
// we got the startbit
gpio_pin_intr_state_set(GPIO_ID_PIN(pin_num[DccProcState.IntPin]), GPIO_PIN_INTR_ANYEDGE); CLR_TP2;CLR_TP1;
halfBit = 0; DccRx.State = WAIT_DATA ;
bitMax = MAX_ONEBITHALF;
bitMin = MIN_ONEBITHALF;
CLR_TP1; CLR_TP1;
} // initialize packet buffer
} else { DccRx.PacketBuf.Size = 0;
SET_TP1; /*for(uint8_t i = 0; i< MAX_DCC_MESSAGE_LEN; i++ )
DccRx.BitCount = 0 ; DccRx.PacketBuf.Data[i] = 0;*/
CLR_TP1; DccRx.PacketBuf.PreambleBits = preambleBitCount;
DccRx.BitCount = 0 ;
DccRx.chkSum = 0 ;
DccRx.TempByte = 0 ;
//SET_TP1;
} }
break; break;
#endif
case WAIT_START_BIT: case WAIT_START_BIT:
// we are looking for first half "0" bit after preamble // we are looking for first half "0" bit after preamble
switch ( halfBit ) { switch ( halfBit ) {
case 0: //SET_TP1; case 0:
// check first part // check first part
if ( DccBitVal ) { if ( DccBitVal ) {
// is still 1-bit (Preamble) // is still 1-bit (Preamble)
...@@ -279,99 +471,135 @@ static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status) ...@@ -279,99 +471,135 @@ static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status)
bit1=bitMicros; bit1=bitMicros;
} else { } else {
// was "0" half bit, maybe the startbit // was "0" half bit, maybe the startbit
SET_TP1; halfBit = 4;
halfBit = 4; }
CLR_TP1;
}
break; break;
case 1: //SET_TP1; // previous halfbit was '1' case 1: // previous halfbit was '1'
if ( DccBitVal ) { if ( DccBitVal ) {
// its a '1' halfBit -> we are still in the preamble // its a '1' halfBit -> we are still in the preamble
halfBit = 0; halfBit = 0;
bit2=bitMicros; bit2=bitMicros;
DccRx.BitCount++; preambleBitCount++;
if( abs(bit2-bit1) > MAX_BITDIFF ) { if( abs(bit2-bit1) > MAX_BITDIFF ) {
// the length of the 2 halfbits differ too much -> wrong protokoll // the length of the 2 halfbits differ too much -> wrong protokoll
CLR_TP2;
CLR_TP3;
DccRx.State = WAIT_PREAMBLE; DccRx.State = WAIT_PREAMBLE;
bitMax = MAX_PRAEAMBEL; bitMax = MAX_PRAEAMBEL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
DccRx.BitCount = 0; preambleBitCount = 0;
SET_TP4; // SET_TP2; CLR_TP2;
gpio_pin_intr_state_set(GPIO_ID_PIN(pin_num[DccProcState.IntPin]), ISREdge); #if defined ( __STM32F1__ )
SET_TP3; detachInterrupt( DccProcState.ExtIntNum );
#endif
#ifdef ESP32
ISRWatch = ISREdge;
#else
attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, ISREdge );
// enable level checking ( with direct port reading @ AVR )
ISRChkMask = DccProcState.ExtIntMask;
ISRLevel = (ISREdge==RISING)? DccProcState.ExtIntMask : 0 ;
#endif
SET_TP3;
CLR_TP4; CLR_TP4;
} }
} else { } else {
// first '0' half detected in second halfBit // first '0' half detected in second halfBit
// wrong sync or not a DCC protokoll // wrong sync or not a DCC protokoll
CLR_TP3; CLR_TP3;
halfBit = 3; halfBit = 3;
SET_TP3; SET_TP3;
} }
break; break;
case 3: //SET_TP1; // previous halfbit was '0' in second halfbit case 3: // previous halfbit was '0' in second halfbit
if ( DccBitVal ) { if ( DccBitVal ) {
// its a '1' halfbit -> we got only a half '0' bit -> cannot be DCC // its a '1' halfbit -> we got only a half '0' bit -> cannot be DCC
DccRx.State = WAIT_PREAMBLE; DccRx.State = WAIT_PREAMBLE;
bitMax = MAX_PRAEAMBEL; bitMax = MAX_PRAEAMBEL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
DccRx.BitCount = 0; preambleBitCount = 0;
// SET_TP2; CLR_TP2;
} else { } else {
// we got two '0' halfbits -> it's the startbit // we got two '0' halfbits -> it's the startbit
// but sync is NOT ok, change IRQ edge. // but sync is NOT ok, change IRQ edge.
if ( ISREdge == GPIO_PIN_INTR_POSEDGE ) ISREdge = GPIO_PIN_INTR_NEGEDGE; else ISREdge = GPIO_PIN_INTR_POSEDGE; CLR_TP2;CLR_TP1;
if ( ISREdge == RISING ) ISREdge = FALLING; else ISREdge = RISING;
DccRx.State = WAIT_DATA ; DccRx.State = WAIT_DATA ;
CLR_TP1;
bitMax = MAX_ONEBITFULL; bitMax = MAX_ONEBITFULL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
DccRx.PacketBuf.Size = 0; DccRx.PacketBuf.Size = 0;
DccRx.PacketBuf.PreambleBits = 0; /*for(uint8_t i = 0; i< MAX_DCC_MESSAGE_LEN; i++ )
for(uint8_t i = 0; i< MAX_DCC_MESSAGE_LEN; i++ ) DccRx.PacketBuf.Data[i] = 0;*/
DccRx.PacketBuf.Data[i] = 0; DccRx.PacketBuf.PreambleBits = preambleBitCount;
DccRx.PacketBuf.PreambleBits = DccRx.BitCount;
DccRx.BitCount = 0 ; DccRx.BitCount = 0 ;
DccRx.chkSum = 0 ;
DccRx.TempByte = 0 ; DccRx.TempByte = 0 ;
//SET_TP1;
} }
SET_TP4; //SET_TP4;
gpio_pin_intr_state_set(GPIO_ID_PIN(pin_num[DccProcState.IntPin]), ISREdge);
CLR_TP1; #if defined ( __STM32F1__ )
CLR_TP4; detachInterrupt( DccProcState.ExtIntNum );
#endif
#ifdef ESP32
ISRWatch = ISREdge;
#else
attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, ISREdge );
#endif
// enable level-checking
ISRChkMask = DccProcState.ExtIntMask;
ISRLevel = (ISREdge==RISING)? DccProcState.ExtIntMask : 0 ;
//CLR_TP4;
break; break;
case 4: SET_TP1; // previous (first) halfbit was 0 case 4: // previous (first) halfbit was 0
// if this halfbit is 0 too, we got the startbit // if this halfbit is 0 too, we got the startbit
if ( DccBitVal ) { if ( DccBitVal ) {
// second halfbit is 1 -> unknown protokoll // second halfbit is 1 -> unknown protokoll
DccRx.State = WAIT_PREAMBLE; DccRx.State = WAIT_PREAMBLE;
bitMax = MAX_PRAEAMBEL; bitMax = MAX_PRAEAMBEL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
preambleBitCount = 0;
CLR_TP2;CLR_TP1;
DccRx.BitCount = 0; DccRx.BitCount = 0;
} else { } else {
// we got the startbit // we got the startbit
CLR_TP2;CLR_TP1;
DccRx.State = WAIT_DATA ; DccRx.State = WAIT_DATA ;
CLR_TP1;
bitMax = MAX_ONEBITFULL; bitMax = MAX_ONEBITFULL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
// initialize packet buffer
DccRx.PacketBuf.Size = 0; DccRx.PacketBuf.Size = 0;
DccRx.PacketBuf.PreambleBits = 0; /*for(uint8_t i = 0; i< MAX_DCC_MESSAGE_LEN; i++ )
for(uint8_t i = 0; i< MAX_DCC_MESSAGE_LEN; i++ ) DccRx.PacketBuf.Data[i] = 0;*/
DccRx.PacketBuf.Data[i] = 0; DccRx.PacketBuf.PreambleBits = preambleBitCount;
DccRx.PacketBuf.PreambleBits = DccRx.BitCount;
DccRx.BitCount = 0 ; DccRx.BitCount = 0 ;
DccRx.chkSum = 0 ;
DccRx.TempByte = 0 ; DccRx.TempByte = 0 ;
//SET_TP1;
} }
CLR_TP1; //SET_TP4;
SET_TP4;
gpio_pin_intr_state_set(GPIO_ID_PIN(pin_num[DccProcState.IntPin]), ISREdge); #if defined ( __STM32F1__ )
CLR_TP4; detachInterrupt( DccProcState.ExtIntNum );
#endif
#ifdef ESP32
ISRWatch = ISREdge;
#else
attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, ISREdge );
#endif
// enable level-checking
ISRChkMask = DccProcState.ExtIntMask;
ISRLevel = (ISREdge==RISING)? DccProcState.ExtIntMask : 0 ;
//CLR_TP4;
break; break;
} }
break; break;
case WAIT_DATA: case WAIT_DATA:
CLR_TP2;
DccRx.BitCount++; DccRx.BitCount++;
DccRx.TempByte = ( DccRx.TempByte << 1 ) ; DccRx.TempByte = ( DccRx.TempByte << 1 ) ;
if( DccBitVal ) if( DccBitVal )
...@@ -390,24 +618,44 @@ static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status) ...@@ -390,24 +618,44 @@ static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status)
{ {
DccRx.State = WAIT_END_BIT ; DccRx.State = WAIT_END_BIT ;
DccRx.PacketBuf.Data[ DccRx.PacketBuf.Size++ ] = DccRx.TempByte ; DccRx.PacketBuf.Data[ DccRx.PacketBuf.Size++ ] = DccRx.TempByte ;
DccRx.chkSum ^= DccRx.TempByte;
} }
} }
break; break;
case WAIT_END_BIT: case WAIT_END_BIT:
SET_TP2;CLR_TP2;
DccRx.BitCount++; DccRx.BitCount++;
if( DccBitVal ) // End of packet? if( DccBitVal ) { // End of packet?
{ CLR_TP3; SET_TP4;
CLR_TP3;
DccRx.State = WAIT_PREAMBLE ; DccRx.State = WAIT_PREAMBLE ;
DccRx.BitCount = 0 ;
bitMax = MAX_PRAEAMBEL; bitMax = MAX_PRAEAMBEL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
DccRx.PacketCopy = DccRx.PacketBuf ; SET_TP1;
uint8_t param; if ( DccRx.chkSum == 0 ) {
task_post_high(DataReady_taskid, (os_param_t) &param); // Packet is valid
SET_TP3; #ifdef ESP32
} portENTER_CRITICAL_ISR(&mux);
else // Get next Byte #endif
DccRx.PacketCopy = DccRx.PacketBuf ;
DccRx.DataReady = 1 ;
#ifdef ESP32
portEXIT_CRITICAL_ISR(&mux);
#endif
// SET_TP2; CLR_TP2;
preambleBitCount = 0 ;
} else {
// Wrong checksum
CLR_TP1;
#ifdef DCC_DBGVAR
DB_PRINT("Cerr");
countOf.Err++;
#endif
}
SET_TP3; CLR_TP4;
} else { // Get next Byte
// KGW - Abort immediately if packet is too long. // KGW - Abort immediately if packet is too long.
if( DccRx.PacketBuf.Size == MAX_DCC_MESSAGE_LEN ) // Packet is too long - abort if( DccRx.PacketBuf.Size == MAX_DCC_MESSAGE_LEN ) // Packet is too long - abort
{ {
...@@ -423,28 +671,137 @@ static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status) ...@@ -423,28 +671,137 @@ static uint32_t ICACHE_RAM_ATTR InterruptHandler (uint32_t ret_gpio_status)
DccRx.BitCount = 0 ; DccRx.BitCount = 0 ;
DccRx.TempByte = 0 ; DccRx.TempByte = 0 ;
} }
}
}
// unless we're already looking for the start bit
// we always search for a preamble ( ( 10 or more consecutive 1 bits )
// if we found it within a packet, the packet decoding is aborted because
// that much one bits cannot be valid in a packet.
if ( DccRx.State != WAIT_START_BIT ) {
if( DccBitVal )
{
preambleBitCount++;
//SET_TP2;
if( preambleBitCount > 10 ) {
CLR_TP2;
#ifndef SYNC_ALWAYS
if ( DccRx.chkSum == 0 ) {
// sync must be correct if chksum was ok, no need to check sync
DccRx.State = WAIT_START_BIT_FULL;
} else {
#endif
DccRx.State = WAIT_START_BIT ;
SET_TP2;
// While waiting for the start bit, detect halfbit lengths. We will detect the correct
// sync and detect whether we see a false (e.g. motorola) protocol
#if defined ( __STM32F1__ )
detachInterrupt( DccProcState.ExtIntNum );
#endif
#ifdef ESP32
ISRWatch = CHANGE;
#else
attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, CHANGE);
#endif
ISRChkMask = 0; // AVR level check is always true with this settings
ISRLevel = 0; // ( there cannot be false edge IRQ's with CHANGE )
halfBit = 0;
bitMax = MAX_ONEBITHALF;
bitMin = MIN_ONEBITHALF;
//CLR_TP1;
#ifndef SYNC_ALWAYS
}
#endif
}
} else {
CLR_TP1;
preambleBitCount = 0 ;
// SET_TP2; CLR_TP2;
}
} }
CLR_TP1; #ifdef ALLOW_NESTED_IRQ
DCC_IrqRunning = false;
#endif
//CLR_TP1;
CLR_TP3; CLR_TP3;
return ret_gpio_status; }
void ackCV(void)
{
if( notifyCVAck )
{
DB_PRINT("ackCV: Send Basic ACK");
notifyCVAck() ;
}
}
void ackAdvancedCV(void)
{
if( notifyAdvancedCVAck && (DccProcState.cv29Value & CV29_RAILCOM_ENABLE) )
{
DB_PRINT("ackAdvancedCV: Send RailCom ACK");
notifyAdvancedCVAck() ;
}
}
uint8_t readEEPROM( unsigned int CV )
{
return EEPROM.read(CV) ;
}
void writeEEPROM( unsigned int CV, uint8_t Value )
{
EEPROM.write(CV, Value) ;
#if defined(ESP8266)
EEPROM.commit();
#endif
#if defined(ESP32)
EEPROM.commit();
#endif
}
bool readyEEPROM()
{
#if defined ARDUINO_ARCH_MEGAAVR
return bit_is_clear(NVMCTRL.STATUS,NVMCTRL_EEBUSY_bp);
#elif defined __AVR_MEGA__
return eeprom_is_ready();
#else
return true;
#endif
} }
uint8_t validCV( uint16_t CV, uint8_t Writable ) uint8_t validCV( uint16_t CV, uint8_t Writable )
{ {
if( notifyCVResetFactoryDefault && (CV == CV_MANUFACTURER_ID ) && Writable ) if( notifyCVResetFactoryDefault && (CV == CV_MANUFACTURER_ID ) && Writable )
notifyCVResetFactoryDefault(); notifyCVResetFactoryDefault();
if( notifyCVValid ) if( notifyCVValid )
return notifyCVValid( CV, Writable ) ; return notifyCVValid( CV, Writable ) ;
return 0;
uint8_t Valid = 1 ;
if( CV > MAXCV )
Valid = 0 ;
if( Writable && ( ( CV ==CV_VERSION_ID ) || (CV == CV_MANUFACTURER_ID ) ) )
Valid = 0 ;
return Valid ;
} }
uint8_t readCV( unsigned int CV ) uint8_t readCV( unsigned int CV )
{ {
uint8_t Value ;
if( notifyCVRead ) if( notifyCVRead )
return notifyCVRead( CV ) ; return notifyCVRead( CV ) ;
return 0;
Value = readEEPROM(CV);
return Value ;
} }
uint8_t writeCV( unsigned int CV, uint8_t Value) uint8_t writeCV( unsigned int CV, uint8_t Value)
...@@ -453,49 +810,60 @@ uint8_t writeCV( unsigned int CV, uint8_t Value) ...@@ -453,49 +810,60 @@ uint8_t writeCV( unsigned int CV, uint8_t Value)
{ {
case CV_29_CONFIG: case CV_29_CONFIG:
// copy addressmode Bit to Flags // copy addressmode Bit to Flags
Value = Value & ~CV29_RAILCOM_ENABLE; // Bidi (RailCom) Bit must not be enabled,
// because you cannot build a Bidi decoder with this lib.
DccProcState.cv29Value = Value;
DccProcState.Flags = ( DccProcState.Flags & ~FLAGS_CV29_BITS) | (Value & FLAGS_CV29_BITS); DccProcState.Flags = ( DccProcState.Flags & ~FLAGS_CV29_BITS) | (Value & FLAGS_CV29_BITS);
// no break, because myDccAdress must also be reset // no break, because myDccAdress must also be reset
case CV_ACCESSORY_DECODER_ADDRESS_LSB: // Also same CV for CV_MULTIFUNCTION_PRIMARY_ADDRESS case CV_ACCESSORY_DECODER_ADDRESS_LSB: // Also same CV for CV_MULTIFUNCTION_PRIMARY_ADDRESS
case CV_ACCESSORY_DECODER_ADDRESS_MSB: case CV_ACCESSORY_DECODER_ADDRESS_MSB:
case CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB: case CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB:
case CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB: case CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB:
DccProcState.myDccAddress = -1; // Assume any CV Write Operation might change the Address DccProcState.myDccAddress = -1; // Assume any CV Write Operation might change the Address
} }
if( notifyCVWrite ) if( notifyCVWrite )
return notifyCVWrite( CV, Value ) ; return notifyCVWrite( CV, Value ) ;
return 0;
if( readEEPROM( CV ) != Value )
{
writeEEPROM( CV, Value ) ;
if( notifyCVChange )
notifyCVChange( CV, Value) ;
if( notifyDccCVChange && !(DccProcState.Flags & FLAGS_SETCV_CALLED) )
notifyDccCVChange( CV, Value );
}
return readEEPROM( CV ) ;
} }
uint16_t getMyAddr(void) uint16_t getMyAddr(void)
{ {
uint8_t CV29Value ; if( DccProcState.myDccAddress != -1 ) // See if we can return the cached value
return( DccProcState.myDccAddress );
if( DccProcState.myDccAddress != -1 ) // See if we can return the cached value
return( DccProcState.myDccAddress );
CV29Value = readCV( CV_29_CONFIG ) ;
if( CV29Value & CV29_ACCESSORY_DECODER ) // Accessory Decoder? if( DccProcState.cv29Value & CV29_ACCESSORY_DECODER ) // Accessory Decoder?
{ {
if( CV29Value & CV29_OUTPUT_ADDRESS_MODE ) if( DccProcState.cv29Value & CV29_OUTPUT_ADDRESS_MODE )
DccProcState.myDccAddress = ( readCV( CV_ACCESSORY_DECODER_ADDRESS_MSB ) << 8 ) | readCV( CV_ACCESSORY_DECODER_ADDRESS_LSB ); DccProcState.myDccAddress = ( readCV( CV_ACCESSORY_DECODER_ADDRESS_MSB ) << 8 ) | readCV( CV_ACCESSORY_DECODER_ADDRESS_LSB );
else else
DccProcState.myDccAddress = ( ( readCV( CV_ACCESSORY_DECODER_ADDRESS_MSB ) & 0b00000111) << 6 ) | ( readCV( CV_ACCESSORY_DECODER_ADDRESS_LSB ) & 0b00111111) ; DccProcState.myDccAddress = ( ( readCV( CV_ACCESSORY_DECODER_ADDRESS_MSB ) & 0b00000111) << 6 ) | ( readCV( CV_ACCESSORY_DECODER_ADDRESS_LSB ) & 0b00111111) ;
} }
else // Multi-Function Decoder? else // Multi-Function Decoder?
{ {
if( CV29Value & CV29_EXT_ADDRESSING ) // Two Byte Address? if( DccProcState.cv29Value & CV29_EXT_ADDRESSING ) // Two Byte Address?
DccProcState.myDccAddress = ( ( readCV( CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB ) - 192 ) << 8 ) | readCV( CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB ) ; DccProcState.myDccAddress = ( ( readCV( CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB ) - 192 ) << 8 ) | readCV( CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB ) ;
else else
DccProcState.myDccAddress = readCV( 1 ) ; DccProcState.myDccAddress = readCV( 1 ) ;
} }
return DccProcState.myDccAddress ; return DccProcState.myDccAddress ;
} }
void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value ) void processDirectCVOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value, void (*ackFunction)() )
{ {
// is it a Byte Operation // is it a Byte Operation
if( Cmd & 0x04 ) if( Cmd & 0x04 )
...@@ -505,7 +873,19 @@ void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value ) ...@@ -505,7 +873,19 @@ void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value )
{ {
if( validCV( CVAddr, 1 ) ) if( validCV( CVAddr, 1 ) )
{ {
writeCV( CVAddr, Value ); DB_PRINT("CV: %d Byte Write: %02X", CVAddr, Value)
if( writeCV( CVAddr, Value ) == Value )
ackFunction();
}
}
else // Perform the Verify Operation
{
if( validCV( CVAddr, 0 ) )
{
DB_PRINT("CV: %d Byte Read: %02X", CVAddr, Value)
if( readCV( CVAddr ) == Value )
ackFunction();
} }
} }
} }
...@@ -518,6 +898,8 @@ void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value ) ...@@ -518,6 +898,8 @@ void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value )
uint8_t tempValue = readCV( CVAddr ) ; // Read the Current CV Value uint8_t tempValue = readCV( CVAddr ) ; // Read the Current CV Value
DB_PRINT("CV: %d Current Value: %02X Bit-Wise Mode: %s Mask: %02X Value: %02X", CVAddr, tempValue, BitWrite ? "Write":"Read", BitMask, BitValue);
// Perform the Bit Write Operation // Perform the Bit Write Operation
if( BitWrite ) if( BitWrite )
{ {
...@@ -529,12 +911,32 @@ void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value ) ...@@ -529,12 +911,32 @@ void processDirectOpsOperation( uint8_t Cmd, uint16_t CVAddr, uint8_t Value )
else else
tempValue &= ~BitMask ; // Turn the Bit Off tempValue &= ~BitMask ; // Turn the Bit Off
writeCV( CVAddr, tempValue ); if( writeCV( CVAddr, tempValue ) == tempValue )
} ackFunction() ;
}
}
// Perform the Bit Verify Operation
else
{
if( validCV( CVAddr, 0 ) )
{
if( BitValue )
{
if( tempValue & BitMask )
ackFunction() ;
}
else
{
if( !( tempValue & BitMask) )
ackFunction() ;
}
}
} }
} }
} }
/////////////////////////////////////////////////////////////////////////
#ifdef NMRA_DCC_PROCESS_MULTIFUNCTION #ifdef NMRA_DCC_PROCESS_MULTIFUNCTION
void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t Cmd, uint8_t Data1, uint8_t Data2 ) void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t Cmd, uint8_t Data1, uint8_t Data2 )
{ {
...@@ -545,12 +947,9 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t ...@@ -545,12 +947,9 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t
uint8_t CmdMasked = Cmd & 0b11100000 ; uint8_t CmdMasked = Cmd & 0b11100000 ;
// NODE_DBG("[dcc_processMultiFunctionMessage] Addr: %d, Type: %d, Cmd: %d ("BYTE_TO_BINARY_PATTERN"), Data: %d, %d, CmdMasked="BYTE_TO_BINARY_PATTERN"\n", Addr, AddrType, Cmd, BYTE_TO_BINARY(Cmd), Data1, Data2, BYTE_TO_BINARY(CmdMasked));
// If we are an Accessory Decoder // If we are an Accessory Decoder
if( DccProcState.Flags & FLAGS_DCC_ACCESSORY_DECODER ) if( DccProcState.Flags & FLAGS_DCC_ACCESSORY_DECODER )
{ {
// NODE_DBG("[dcc_processMultiFunctionMessage] DccProcState.Flags & FLAGS_DCC_ACCESSORY_DECODER\n");
// and this isn't an Ops Mode Write or we are NOT faking the Multifunction Ops mode address in CV 33+34 or // and this isn't an Ops Mode Write or we are NOT faking the Multifunction Ops mode address in CV 33+34 or
// it's not our fake address, then return // it's not our fake address, then return
if( ( CmdMasked != 0b11100000 ) || ( DccProcState.OpsModeAddressBaseCV == 0 ) ) if( ( CmdMasked != 0b11100000 ) || ( DccProcState.OpsModeAddressBaseCV == 0 ) )
...@@ -567,16 +966,14 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t ...@@ -567,16 +966,14 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t
else if( ( DccProcState.Flags & FLAGS_MY_ADDRESS_ONLY ) && ( Addr != getMyAddr() ) && ( Addr != 0 ) ) else if( ( DccProcState.Flags & FLAGS_MY_ADDRESS_ONLY ) && ( Addr != getMyAddr() ) && ( Addr != 0 ) )
return ; return ;
NODE_DBG("[dcc_processMultiFunctionMessage] CmdMasked: %x\n", CmdMasked);
switch( CmdMasked ) switch( CmdMasked )
{ {
case 0b00000000: // Decoder Control case 0b00000000: // Decoder Control
switch( Cmd & 0b00001110 ) switch( Cmd & 0b00001110 )
{ {
case 0b00000000: case 0b00000000:
if( notifyDccReset && ( Cmd & 0b00000001 ) ) // Hard Reset if( notifyDccReset)
if( notifyDccReset) notifyDccReset( Cmd & 0b00000001 ) ;
notifyDccReset( 1 ) ;
break ; break ;
case 0b00000010: // Factory Test case 0b00000010: // Factory Test
...@@ -625,7 +1022,7 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t ...@@ -625,7 +1022,7 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t
case 0b01100000: case 0b01100000:
//TODO should we cache this info in DCC_PROCESSOR_STATE.Flags ? //TODO should we cache this info in DCC_PROCESSOR_STATE.Flags ?
#ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE #ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE
speedSteps = (readCV( CV_29_CONFIG ) & CV29_F0_LOCATION) ? SPEED_STEP_28 : SPEED_STEP_14 ; speedSteps = (DccProcState.cv29Value & CV29_F0_LOCATION) ? SPEED_STEP_28 : SPEED_STEP_14 ;
#else #else
speedSteps = SPEED_STEP_28 ; speedSteps = SPEED_STEP_28 ;
#endif #endif
...@@ -661,7 +1058,7 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t ...@@ -661,7 +1058,7 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t
notifyDccSpeed( Addr, AddrType, speed, dir, speedSteps ) ; notifyDccSpeed( Addr, AddrType, speed, dir, speedSteps ) ;
} }
if( notifyDccSpeedRaw ) if( notifyDccSpeedRaw )
notifyDccSpeedRaw(Addr, AddrType, Cmd ); notifyDccSpeedRaw(Addr, AddrType, Cmd );
#ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE #ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE
if( notifyDccFunc && (speedSteps == SPEED_STEP_14) ) if( notifyDccFunc && (speedSteps == SPEED_STEP_14) )
...@@ -692,24 +1089,24 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t ...@@ -692,24 +1089,24 @@ void processMultiFunctionMessage( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t
break; break;
case 0b11000000: // Feature Expansion Instruction case 0b11000000: // Feature Expansion Instruction
switch(Cmd & 0b00011111) switch(Cmd & 0b00011111)
{ {
case 0b00011110: case 0b00011110:
if( notifyDccFunc ) if( notifyDccFunc )
notifyDccFunc( Addr, AddrType, FN_13_20, Data1 ) ; notifyDccFunc( Addr, AddrType, FN_13_20, Data1 ) ;
break; break;
case 0b00011111: case 0b00011111:
if( notifyDccFunc ) if( notifyDccFunc )
notifyDccFunc( Addr, AddrType, FN_21_28, Data1 ) ; notifyDccFunc( Addr, AddrType, FN_21_28, Data1 ) ;
break; break;
} }
break; break;
case 0b11100000: // CV Access case 0b11100000: // CV Access
CVAddr = ( ( ( Cmd & 0x03 ) << 8 ) | Data1 ) + 1 ; CVAddr = ( ( ( Cmd & 0x03 ) << 8 ) | Data1 ) + 1 ;
processDirectOpsOperation( Cmd, CVAddr, Data2 ) ; processDirectCVOperation( Cmd, CVAddr, Data2, ackAdvancedCV) ;
break; break;
} }
} }
...@@ -724,13 +1121,14 @@ void processServiceModeOperation( DCC_MSG * pDccMsg ) ...@@ -724,13 +1121,14 @@ void processServiceModeOperation( DCC_MSG * pDccMsg )
if( pDccMsg->Size == 3) // 3 Byte Packets are for Address Only, Register and Paged Mode if( pDccMsg->Size == 3) // 3 Byte Packets are for Address Only, Register and Paged Mode
{ {
uint8_t RegisterAddr ; uint8_t RegisterAddr ;
NODE_DBG("[dcc_processServiceModeOperation] 3-BytePkt\n"); DB_PRINT("CV Address, Register & Paged Mode Operation");
RegisterAddr = pDccMsg->Data[0] & 0x07 ; RegisterAddr = pDccMsg->Data[0] & 0x07 ;
Value = pDccMsg->Data[1] ; Value = pDccMsg->Data[1] ;
if( RegisterAddr == 5 ) if( RegisterAddr == 5 )
{ {
DccProcState.PageRegister = Value ; DccProcState.PageRegister = Value ;
ackCV();
} }
else else
...@@ -748,7 +1146,17 @@ void processServiceModeOperation( DCC_MSG * pDccMsg ) ...@@ -748,7 +1146,17 @@ void processServiceModeOperation( DCC_MSG * pDccMsg )
{ {
if( validCV( CVAddr, 1 ) ) if( validCV( CVAddr, 1 ) )
{ {
writeCV( CVAddr, Value ); if( writeCV( CVAddr, Value ) == Value )
ackCV();
}
}
else // Perform the Verify Operation
{
if( validCV( CVAddr, 0 ) )
{
if( readCV( CVAddr ) == Value )
ackCV();
} }
} }
} }
...@@ -756,15 +1164,16 @@ void processServiceModeOperation( DCC_MSG * pDccMsg ) ...@@ -756,15 +1164,16 @@ void processServiceModeOperation( DCC_MSG * pDccMsg )
else if( pDccMsg->Size == 4) // 4 Byte Packets are for Direct Byte & Bit Mode else if( pDccMsg->Size == 4) // 4 Byte Packets are for Direct Byte & Bit Mode
{ {
NODE_DBG("[dcc_processServiceModeOperation] BB-Mode\n"); DB_PRINT("CV Direct Byte and Bit Mode Mode Operation");
CVAddr = ( ( ( pDccMsg->Data[0] & 0x03 ) << 8 ) | pDccMsg->Data[1] ) + 1 ; CVAddr = ( ( ( pDccMsg->Data[0] & 0x03 ) << 8 ) | pDccMsg->Data[1] ) + 1 ;
Value = pDccMsg->Data[2] ; Value = pDccMsg->Data[2] ;
processDirectOpsOperation( pDccMsg->Data[0] & 0b00001100, CVAddr, Value ) ; processDirectCVOperation( pDccMsg->Data[0] & 0b00001100, CVAddr, Value, ackCV) ;
} }
} }
#endif #endif
/////////////////////////////////////////////////////////////////////////
void resetServiceModeTimer(uint8_t inServiceMode) void resetServiceModeTimer(uint8_t inServiceMode)
{ {
if (notifyServiceMode && inServiceMode != DccProcState.inServiceMode) if (notifyServiceMode && inServiceMode != DccProcState.inServiceMode)
...@@ -774,13 +1183,14 @@ void resetServiceModeTimer(uint8_t inServiceMode) ...@@ -774,13 +1183,14 @@ void resetServiceModeTimer(uint8_t inServiceMode)
// Set the Service Mode // Set the Service Mode
DccProcState.inServiceMode = inServiceMode ; DccProcState.inServiceMode = inServiceMode ;
DccProcState.LastServiceModeMillis = inServiceMode ? system_get_time() : 0 ; DccProcState.LastServiceModeMillis = inServiceMode ? millis() : 0 ;
if (notifyServiceMode && inServiceMode != DccProcState.inServiceMode) if (notifyServiceMode && inServiceMode != DccProcState.inServiceMode)
{ {
notifyServiceMode(inServiceMode); notifyServiceMode(inServiceMode);
} }
} }
/////////////////////////////////////////////////////////////////////////
void clearDccProcState(uint8_t inServiceMode) void clearDccProcState(uint8_t inServiceMode)
{ {
resetServiceModeTimer( inServiceMode ) ; resetServiceModeTimer( inServiceMode ) ;
...@@ -793,15 +1203,32 @@ void clearDccProcState(uint8_t inServiceMode) ...@@ -793,15 +1203,32 @@ void clearDccProcState(uint8_t inServiceMode)
memset( &DccProcState.LastMsg, 0, sizeof( DCC_MSG ) ) ; memset( &DccProcState.LastMsg, 0, sizeof( DCC_MSG ) ) ;
} }
void execDccProcessor( DCC_MSG * pDccMsg ) /////////////////////////////////////////////////////////////////////////
#ifdef DEBUG_PRINT
void SerialPrintPacketHex(const __FlashStringHelper *strLabel, DCC_MSG * pDccMsg)
{ {
NODE_DBG("[dcc_execDccProcessor]\n"); Serial.print( strLabel );
for( uint8_t i = 0; i < pDccMsg->Size; i++ )
{
if( pDccMsg->Data[i] <= 9)
Serial.print('0');
Serial.print( pDccMsg->Data[i], HEX );
Serial.write( ' ' );
}
Serial.println();
}
#endif
///////////////////////////////////////////////////////////////////////////////
void execDccProcessor( DCC_MSG * pDccMsg )
{
if( ( pDccMsg->Data[0] == 0 ) && ( pDccMsg->Data[1] == 0 ) ) if( ( pDccMsg->Data[0] == 0 ) && ( pDccMsg->Data[1] == 0 ) )
{ {
if( notifyDccReset ) if( notifyDccReset )
notifyDccReset( 0 ) ; notifyDccReset( 0 ) ;
#ifdef NMRA_DCC_PROCESS_SERVICEMODE #ifdef NMRA_DCC_PROCESS_SERVICEMODE
// If this is the first Reset then perform some one-shot actions as we maybe about to enter service mode // If this is the first Reset then perform some one-shot actions as we maybe about to enter service mode
if( DccProcState.inServiceMode ) if( DccProcState.inServiceMode )
...@@ -818,12 +1245,13 @@ void execDccProcessor( DCC_MSG * pDccMsg ) ...@@ -818,12 +1245,13 @@ void execDccProcessor( DCC_MSG * pDccMsg )
{ {
resetServiceModeTimer( 1 ) ; resetServiceModeTimer( 1 ) ;
if( memcmp( pDccMsg, &DccProcState.LastMsg, sizeof( DCC_MSG ) ) ) //Only check the DCC Packet "Size" and "Data" fields and ignore the "PreambleBits" as they can be different to the previous packet
if(pDccMsg->Size != DccProcState.LastMsg.Size || memcmp( pDccMsg->Data, &DccProcState.LastMsg.Data, pDccMsg->Size ) != 0 )
{ {
DccProcState.DuplicateCount = 0 ; DccProcState.DuplicateCount = 0 ;
memcpy( &DccProcState.LastMsg, pDccMsg, sizeof( DCC_MSG ) ) ; memcpy( &DccProcState.LastMsg, pDccMsg, sizeof( DCC_MSG ) ) ;
} }
// Wait until you see 2 identicle packets before acting on a Service Mode Packet // Wait until you see 2 identical packets before acting on a Service Mode Packet
else else
{ {
DccProcState.DuplicateCount++ ; DccProcState.DuplicateCount++ ;
...@@ -834,7 +1262,7 @@ void execDccProcessor( DCC_MSG * pDccMsg ) ...@@ -834,7 +1262,7 @@ void execDccProcessor( DCC_MSG * pDccMsg )
else else
{ {
if( DccProcState.inServiceMode ) if( DccProcState.inServiceMode )
clearDccProcState( 0 ); clearDccProcState( 0 );
#endif #endif
// Idle Packet // Idle Packet
...@@ -848,6 +1276,7 @@ void execDccProcessor( DCC_MSG * pDccMsg ) ...@@ -848,6 +1276,7 @@ void execDccProcessor( DCC_MSG * pDccMsg )
// Multi Function Decoders (7-bit address) // Multi Function Decoders (7-bit address)
else if( pDccMsg->Data[0] < 128 ) else if( pDccMsg->Data[0] < 128 )
processMultiFunctionMessage( pDccMsg->Data[0], DCC_ADDR_SHORT, pDccMsg->Data[1], pDccMsg->Data[2], pDccMsg->Data[3] ) ; processMultiFunctionMessage( pDccMsg->Data[0], DCC_ADDR_SHORT, pDccMsg->Data[1], pDccMsg->Data[2], pDccMsg->Data[3] ) ;
// Basic Accessory Decoders (9-bit) & Extended Accessory Decoders (11-bit) // Basic Accessory Decoders (9-bit) & Extended Accessory Decoders (11-bit)
else if( pDccMsg->Data[0] < 192 ) else if( pDccMsg->Data[0] < 192 )
#else #else
...@@ -860,179 +1289,187 @@ void execDccProcessor( DCC_MSG * pDccMsg ) ...@@ -860,179 +1289,187 @@ void execDccProcessor( DCC_MSG * pDccMsg )
int16_t OutputAddress ; int16_t OutputAddress ;
uint8_t TurnoutPairIndex ; uint8_t TurnoutPairIndex ;
#ifdef NODE_DEBUG #ifdef DEBUG_PRINT
// SerialPrintPacketHex(F( "eDP: AccCmd: "), pDccMsg); SerialPrintPacketHex(F( "eDP: AccCmd: "), pDccMsg);
#endif #endif
BoardAddress = ( ( (~pDccMsg->Data[1]) & 0b01110000 ) << 2 ) | ( pDccMsg->Data[0] & 0b00111111 ) ; BoardAddress = ( ( (~pDccMsg->Data[1]) & 0b01110000 ) << 2 ) | ( pDccMsg->Data[0] & 0b00111111 ) ;
TurnoutPairIndex = (pDccMsg->Data[1] & 0b00000110) >> 1; TurnoutPairIndex = (pDccMsg->Data[1] & 0b00000110) >> 1;
NODE_DBG("[dcc_execDccProcessor] eDP: BAddr:%d, Index:%d\n", BoardAddress, TurnoutPairIndex); DB_PRINT("eDP: BAddr:%d, Index:%d", BoardAddress, TurnoutPairIndex);
// First check for Legacy Accessory Decoder Configuration Variable Access Instruction // First check for Legacy Accessory Decoder Configuration Variable Access Instruction
// as it's got a different format to the others // as it's got a different format to the others
if((pDccMsg->Size == 5) && ((pDccMsg->Data[1] & 0b10001100) == 0b00001100)) if((pDccMsg->Size == 5) && ((pDccMsg->Data[1] & 0b10001100) == 0b00001100))
{ {
NODE_DBG( "eDP: Legacy Accessory Decoder CV Access Command"); DB_PRINT( "eDP: Legacy Accessory Decoder CV Access Command");
// Check if this command is for our address or the broadcast address // Check if this command is for our address or the broadcast address
if((BoardAddress != getMyAddr()) && ( BoardAddress < 511 )) if((BoardAddress != getMyAddr()) && ( BoardAddress < 511 ))
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Board Address Not Matched\n"); DB_PRINT("eDP: Board Address Not Matched");
return; return;
} }
uint16_t cvAddress = ((pDccMsg->Data[1] & 0b00000011) << 8) + pDccMsg->Data[2] + 1; uint16_t cvAddress = ((pDccMsg->Data[1] & 0b00000011) << 8) + pDccMsg->Data[2] + 1;
uint8_t cvValue = pDccMsg->Data[3]; uint8_t cvValue = pDccMsg->Data[3];
NODE_DBG("[dcc_execDccProcessor] eDP: CV:%d Value:%d\n", cvAddress, cvValue ); DB_PRINT("eDP: CV:%d Value:%d", cvAddress, cvValue );
if(validCV( cvAddress, 1 )) if(validCV( cvAddress, 1 ))
writeCV(cvAddress, cvValue); writeCV(cvAddress, cvValue);
return; return;
} }
OutputAddress = (((BoardAddress - 1) << 2 ) | TurnoutPairIndex) + 1 ; //decoder output addresses start with 1, packet address range starts with 0 OutputAddress = (((BoardAddress - 1) << 2 ) | TurnoutPairIndex) + 1 ; //decoder output addresses start with 1, packet address range starts with 0
// ( according to NMRA 9.2.2 ) // ( according to NMRA 9.2.2 )
NODE_DBG("[dcc_execDccProcessor] eDP: OAddr:%d\n", OutputAddress); DB_PRINT("eDP: OAddr:%d", OutputAddress);
if( DccProcState.inAccDecDCCAddrNextReceivedMode) if( DccProcState.inAccDecDCCAddrNextReceivedMode)
{ {
if( DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE ) if( DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE )
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Set OAddr:%d\n", OutputAddress); DB_PRINT("eDP: Set OAddr:%d", OutputAddress);
//uint16_t storedOutputAddress = OutputAddress + 1; // The value stored in CV1 & 9 for Output Addressing Mode is + 1 //uint16_t storedOutputAddress = OutputAddress + 1; // The value stored in CV1 & 9 for Output Addressing Mode is + 1
writeCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, (uint8_t)(OutputAddress % 256)); writeCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, (uint8_t)(OutputAddress % 256));
writeCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, (uint8_t)(OutputAddress / 256)); writeCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, (uint8_t)(OutputAddress / 256));
if( notifyDccAccOutputAddrSet ) if( notifyDccAccOutputAddrSet )
notifyDccAccOutputAddrSet(OutputAddress); notifyDccAccOutputAddrSet(OutputAddress);
} }
else else
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Set BAddr:%d\n", BoardAddress); DB_PRINT("eDP: Set BAddr:%d", BoardAddress);
writeCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, (uint8_t)(BoardAddress % 64)); writeCV(CV_ACCESSORY_DECODER_ADDRESS_LSB, (uint8_t)(BoardAddress % 64));
writeCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, (uint8_t)(BoardAddress / 64)); writeCV(CV_ACCESSORY_DECODER_ADDRESS_MSB, (uint8_t)(BoardAddress / 64));
if( notifyDccAccBoardAddrSet ) if( notifyDccAccBoardAddrSet )
notifyDccAccBoardAddrSet(BoardAddress); notifyDccAccBoardAddrSet(BoardAddress);
} }
DccProcState.inAccDecDCCAddrNextReceivedMode = 0; // Reset the mode now that we have set the address DccProcState.inAccDecDCCAddrNextReceivedMode = 0; // Reset the mode now that we have set the address
} }
// If we're filtering addresses, does the address match our address or is it a broadcast address? If NOT then return // If we're filtering addresses, does the address match our address or is it a broadcast address? If NOT then return
if( DccProcState.Flags & FLAGS_MY_ADDRESS_ONLY ) if( DccProcState.Flags & FLAGS_MY_ADDRESS_ONLY )
{ {
if( DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE ) { if( DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE ) {
NODE_DBG("[dcc_execDccProcessor] AddrChk: OAddr:%d, BAddr:%d, myAddr:%d Chk=%d\n", OutputAddress, BoardAddress, getMyAddr(), OutputAddress != getMyAddr() ); DB_PRINT(" AddrChk: OAddr:%d, BAddr:%d, myAddr:%d Chk=%d", OutputAddress, BoardAddress, getMyAddr(), OutputAddress != getMyAddr() );
if ( OutputAddress != getMyAddr() && OutputAddress < 2045 ) { if ( OutputAddress != getMyAddr() && OutputAddress < 2045 ) {
NODE_DBG("[dcc_execDccProcessor] eDP: OAddr:%d, myAddr:%d - no match\n", OutputAddress, getMyAddr() ); DB_PRINT(" eDP: OAddr:%d, myAddr:%d - no match", OutputAddress, getMyAddr() );
return; return;
} }
} else { } else {
if( ( BoardAddress != getMyAddr() ) && ( BoardAddress < 511 ) ) { if( ( BoardAddress != getMyAddr() ) && ( BoardAddress < 511 ) ) {
NODE_DBG("[dcc_execDccProcessor] eDP: BAddr:%d, myAddr:%d - no match\n", BoardAddress, getMyAddr() ); DB_PRINT(" eDP: BAddr:%d, myAddr:%d - no match", BoardAddress, getMyAddr() );
return; return;
} }
} }
NODE_DBG("[dcc_execDccProcessor] eDP: Address Matched\n"); DB_PRINT("eDP: Address Matched");
} }
if((pDccMsg->Size == 4) && ((pDccMsg->Data[1] & 0b10001001) == 1)) // Extended Accessory Decoder Control Packet Format if((pDccMsg->Size == 4) && ((pDccMsg->Data[1] & 0b10001001) == 1)) // Extended Accessory Decoder Control Packet Format
{ {
// According to the NMRA Dcc Spec the Signal State should only use the lower 5 Bits, // According to the NMRA Dcc Spec the Signal State should only use the lower 5 Bits,
// however some manufacturers seem to allow/use all 8 bits, so we'll relax that constraint for now // however some manufacturers seem to allow/use all 8 bits, so we'll relax that constraint for now
uint8_t state = pDccMsg->Data[2] ; uint8_t state = pDccMsg->Data[2] ;
NODE_DBG("[dcc_execDccProcessor] eDP: OAddr:%d Extended State:%0X\n", OutputAddress, state); DB_PRINT("eDP: OAddr:%d Extended State:%0X", OutputAddress, state);
if( notifyDccSigOutputState ) if( notifyDccSigOutputState )
notifyDccSigOutputState(OutputAddress, state); notifyDccSigOutputState(OutputAddress, state);
}
// old callback ( for compatibility with 1.4.2, not to be used in new designs )
else if(pDccMsg->Size == 3) // Basic Accessory Decoder Packet Format if( notifyDccSigState )
{ notifyDccSigState( OutputAddress, TurnoutPairIndex, pDccMsg->Data[2] ) ;
uint8_t direction = pDccMsg->Data[1] & 0b00000001; }
uint8_t outputPower = (pDccMsg->Data[1] & 0b00001000) >> 3;
else if(pDccMsg->Size == 3) // Basic Accessory Decoder Packet Format
if( DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE ) {
{ uint8_t direction = pDccMsg->Data[1] & 0b00000001;
NODE_DBG("[dcc_execDccProcessor] eDP: OAddr:%d Turnout Dir:%d Output Power:%d\n", OutputAddress, direction, outputPower); uint8_t outputPower = (pDccMsg->Data[1] & 0b00001000) >> 3;
if( notifyDccAccTurnoutOutput )
notifyDccAccTurnoutOutput( OutputAddress, direction, outputPower ); // old callback ( for compatibility with 1.4.2, not to be used in new designs )
} if ( notifyDccAccState )
else notifyDccAccState( OutputAddress, BoardAddress, pDccMsg->Data[1] & 0b00000111, outputPower );
{
NODE_DBG("[dcc_execDccProcessor] eDP: Turnout Pair Index:%d Dir:%d Output Power: %d\n", TurnoutPairIndex, direction, outputPower); if( DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE )
if( notifyDccAccTurnoutBoard )
notifyDccAccTurnoutBoard( BoardAddress, TurnoutPairIndex, direction, outputPower );
}
}
else if(pDccMsg->Size == 6) // Accessory Decoder OPS Mode Programming
{
NODE_DBG("[dcc_execDccProcessor] eDP: OPS Mode CV Programming Command\n");
// Check for unsupported OPS Mode Addressing mode
if(((pDccMsg->Data[1] & 0b10001001) != 1) && ((pDccMsg->Data[1] & 0b10001111) != 0x80))
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Unsupported OPS Mode CV Addressing Mode\n"); DB_PRINT("eDP: OAddr:%d Turnout Dir:%d Output Power:%d", OutputAddress, direction, outputPower);
return; if( notifyDccAccTurnoutOutput )
notifyDccAccTurnoutOutput( OutputAddress, direction, outputPower );
} }
else
// Check if this command is for our address or the broadcast address {
DB_PRINT("eDP: Turnout Pair Index:%d Dir:%d Output Power: ", TurnoutPairIndex, direction, outputPower);
if( notifyDccAccTurnoutBoard )
notifyDccAccTurnoutBoard( BoardAddress, TurnoutPairIndex, direction, outputPower );
}
}
else if(pDccMsg->Size == 6) // Accessory Decoder OPS Mode Programming
{
DB_PRINT("eDP: OPS Mode CV Programming Command");
// Check for unsupported OPS Mode Addressing mode
if(((pDccMsg->Data[1] & 0b10001001) != 1) && ((pDccMsg->Data[1] & 0b10001111) != 0x80))
{
DB_PRINT("eDP: Unsupported OPS Mode CV Addressing Mode");
return;
}
// Check if this command is for our address or the broadcast address
if(DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE) if(DccProcState.Flags & FLAGS_OUTPUT_ADDRESS_MODE)
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Check Output Address:%d\n", OutputAddress); DB_PRINT("eDP: Check Output Address:%d", OutputAddress);
if((OutputAddress != getMyAddr()) && ( OutputAddress < 2045 )) if((OutputAddress != getMyAddr()) && ( OutputAddress < 2045 ))
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Output Address Not Matched\n"); DB_PRINT("eDP: Output Address Not Matched");
return; return;
} }
} }
else else
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Check Board Address:%d\n", BoardAddress); DB_PRINT("eDP: Check Board Address:%d", BoardAddress);
if((BoardAddress != getMyAddr()) && ( BoardAddress < 511 )) if((BoardAddress != getMyAddr()) && ( BoardAddress < 511 ))
{ {
NODE_DBG("[dcc_execDccProcessor] eDP: Board Address Not Matched\n"); DB_PRINT("eDP: Board Address Not Matched");
return; return;
} }
} }
uint16_t cvAddress = ((pDccMsg->Data[2] & 0b00000011) << 8) + pDccMsg->Data[3] + 1; uint16_t cvAddress = ((pDccMsg->Data[2] & 0b00000011) << 8) + pDccMsg->Data[3] + 1;
uint8_t cvValue = pDccMsg->Data[4]; uint8_t cvValue = pDccMsg->Data[4];
OpsInstructionType insType = (OpsInstructionType)((pDccMsg->Data[2] & 0b00001100) >> 2) ; OpsInstructionType insType = (OpsInstructionType)((pDccMsg->Data[2] & 0b00001100) >> 2) ;
NODE_DBG("[dcc_execDccProcessor] eDP: OPS Mode Instruction:%d\n", insType); DB_PRINT("eDP: OPS Mode Instruction:%d", insType);
switch(insType) switch(insType)
{ {
case OPS_INS_RESERVED: case OPS_INS_RESERVED:
case OPS_INS_VERIFY_BYTE: case OPS_INS_VERIFY_BYTE:
NODE_DBG("[dcc_execDccProcessor] eDP: Unsupported OPS Mode Instruction:%d\n", insType); DB_PRINT("eDP: Unsupported OPS Mode Instruction:%d", insType);
break; // We only support Write Byte or Bit Manipulation break; // We only support Write Byte or Bit Manipulation
case OPS_INS_WRITE_BYTE: case OPS_INS_WRITE_BYTE:
NODE_DBG("[dcc_execDccProcessor] eDP: CV:%d Value:%d\n", cvAddress, cvValue); DB_PRINT("eDP: CV:%d Value:%d", cvAddress, cvValue);
if(validCV( cvAddress, 1 )) if(validCV( cvAddress, 1 ))
writeCV(cvAddress, cvValue); writeCV(cvAddress, cvValue);
break; break;
// 111CDBBB // 111CDBBB
// Where BBB represents the bit position within the CV, // Where BBB represents the bit position within the CV,
// D contains the value of the bit to be verified or written, // D contains the value of the bit to be verified or written,
// and C describes whether the operation is a verify bit or a write bit operation. // and C describes whether the operation is a verify bit or a write bit operation.
// C = "1" WRITE BIT // C = "1" WRITE BIT
// C = "0" VERIFY BIT // C = "0" VERIFY BIT
case OPS_INS_BIT_MANIPULATION: case OPS_INS_BIT_MANIPULATION:
// Make sure its a Write Bit Manipulation // Make sure its a Write Bit Manipulation
if((cvValue & 0b00010000) && validCV(cvAddress, 1 )) if((cvValue & 0b00010000) && validCV(cvAddress, 1 ))
{ {
uint8_t currentValue = readCV(cvAddress); uint8_t currentValue = readCV(cvAddress);
uint8_t newValueMask = 1 << (cvValue & 0b00000111); uint8_t newValueMask = 1 << (cvValue & 0b00000111);
if(cvValue & 0b00001000) if(cvValue & 0b00001000)
writeCV(cvAddress, currentValue | newValueMask); writeCV(cvAddress, currentValue | newValueMask);
else else
writeCV(cvAddress, currentValue & ~newValueMask); writeCV(cvAddress, currentValue & ~newValueMask);
} }
break; break;
} }
} }
} }
} }
...@@ -1053,52 +1490,64 @@ void execDccProcessor( DCC_MSG * pDccMsg ) ...@@ -1053,52 +1490,64 @@ void execDccProcessor( DCC_MSG * pDccMsg )
} }
} }
static void process (os_param_t param, uint8_t prio) ////////////////////////////////////////////////////////////////////////
NmraDcc::NmraDcc()
{ {
// !!!!!! - this will not happen as we call process task only when data is ready }
// if( DccProcState.inServiceMode )
// { #ifdef digitalPinToInterrupt
// if( (system_get_time() - DccProcState.LastServiceModeMillis ) > 20L ) void NmraDcc::pin( uint8_t ExtIntPinNum, uint8_t EnablePullup)
// { {
// clearDccProcState( 0 ) ; pin(digitalPinToInterrupt(ExtIntPinNum), ExtIntPinNum, EnablePullup);
// } }
// } #endif
// !!!!!!
void NmraDcc::pin( uint8_t ExtIntNum, uint8_t ExtIntPinNum, uint8_t EnablePullup)
// We need to do this check with interrupts disabled {
//SET_TP4; #if defined ( __STM32F1__ )
Msg = DccRx.PacketCopy ; // with STM32F1 the interuptnumber is equal the pin number
DccProcState.ExtIntNum = ExtIntPinNum;
#ifdef DCC_DBGVAR // because STM32F1 has a NVIC we must set interuptpriorities
countOf.Tel++; const nvic_irq_num irqNum2nvic[] = { NVIC_EXTI0, NVIC_EXTI1, NVIC_EXTI2, NVIC_EXTI3, NVIC_EXTI4,
#endif NVIC_EXTI_9_5, NVIC_EXTI_9_5, NVIC_EXTI_9_5, NVIC_EXTI_9_5, NVIC_EXTI_9_5,
NVIC_EXTI_15_10, NVIC_EXTI_15_10, NVIC_EXTI_15_10, NVIC_EXTI_15_10, NVIC_EXTI_15_10, NVIC_EXTI_15_10 };
uint8_t xorValue = 0 ; exti_num irqNum = (exti_num)(PIN_MAP[ExtIntPinNum].gpio_bit);
for(uint8_t i = 0; i < DccRx.PacketCopy.Size; i++) // DCC-Input IRQ must be able to interrupt other long low priority ( level15 ) IRQ's
xorValue ^= DccRx.PacketCopy.Data[i]; nvic_irq_set_priority ( irqNum2nvic[irqNum], PRIO_DCC_IRQ);
if(xorValue) {
#ifdef DCC_DBGVAR
NODE_DBG("[dcc_process] Cerr\n");
NODE_DBG("[dcc_process] Data dump:");
for(uint8_t i = 0; i < DccRx.PacketCopy.Size; i++)
NODE_DBG(" %x", DccRx.PacketCopy.Data[i]);
NODE_DBG("\n");
countOf.Err++;
#endif
return;// 0 ;
} else {
NODE_DBG("[dcc_process] Size: %d\tPreambleBits: %d\t%d, %d, %d, %d, %d, %d\n",
Msg.Size, Msg.PreambleBits, Msg.Data[0], Msg.Data[1], Msg.Data[2], Msg.Data[3], Msg.Data[4], Msg.Data[5]);
execDccProcessor( &Msg );
}
return;// 1 ; // Systic must be able to interrupt DCC-IRQ to always get correct micros() values
nvic_irq_set_priority(NVIC_SYSTICK, PRIO_SYSTIC);
#else
DccProcState.ExtIntNum = ExtIntNum;
#endif
DccProcState.ExtIntPinNum = ExtIntPinNum;
#ifdef __AVR_MEGA__
// because digitalRead at AVR is slow, we will read the dcc input in the ISR
// by direct port access.
DccProcState.ExtIntPort = portInputRegister( digitalPinToPort(ExtIntPinNum) );
DccProcState.ExtIntMask = digitalPinToBitMask( ExtIntPinNum );
#else
DccProcState.ExtIntMask = 1;
#endif
pinMode( ExtIntPinNum, EnablePullup ? INPUT_PULLUP : INPUT );
}
////////////////////////////////////////////////////////////////////////
void NmraDcc::initAccessoryDecoder( uint8_t ManufacturerId, uint8_t VersionId, uint8_t Flags, uint8_t OpsModeAddressBaseCV )
{
init(ManufacturerId, VersionId, Flags | FLAGS_DCC_ACCESSORY_DECODER, OpsModeAddressBaseCV);
} }
void dcc_setup(uint8_t pin, uint8_t ManufacturerId, uint8_t VersionId, uint8_t Flags, uint8_t OpsModeAddressBaseCV) ////////////////////////////////////////////////////////////////////////
void NmraDcc::init( uint8_t ManufacturerId, uint8_t VersionId, uint8_t Flags, uint8_t OpsModeAddressBaseCV )
{ {
NODE_DBG("[dcc_setup]\n"); #if defined(ESP8266)
EEPROM.begin(MAXCV);
#endif
#if defined(ESP32)
EEPROM.begin(MAXCV);
#endif
// Clear all the static member variables // Clear all the static member variables
memset( &DccRx, 0, sizeof( DccRx) ); memset( &DccRx, 0, sizeof( DccRx) );
...@@ -1106,38 +1555,34 @@ void dcc_setup(uint8_t pin, uint8_t ManufacturerId, uint8_t VersionId, uint8_t F ...@@ -1106,38 +1555,34 @@ void dcc_setup(uint8_t pin, uint8_t ManufacturerId, uint8_t VersionId, uint8_t F
MODE_TP2; MODE_TP2;
MODE_TP3; MODE_TP3;
MODE_TP4; MODE_TP4;
CLR_TP1;
CLR_TP2;
CLR_TP3;
CLR_TP4;
bitMax = MAX_ONEBITFULL; bitMax = MAX_ONEBITFULL;
bitMin = MIN_ONEBITFULL; bitMin = MIN_ONEBITFULL;
DccProcState.Flags = Flags ; DccProcState.Flags = Flags ;
DccProcState.OpsModeAddressBaseCV = OpsModeAddressBaseCV ; DccProcState.OpsModeAddressBaseCV = OpsModeAddressBaseCV ;
DccProcState.myDccAddress = -1; DccProcState.myDccAddress = -1;
DccProcState.inAccDecDCCAddrNextReceivedMode = 0; DccProcState.inAccDecDCCAddrNextReceivedMode = 0;
ISREdge = GPIO_PIN_INTR_POSEDGE; ISREdge = RISING;
// level checking to detect false IRQ's fired by glitches
DccProcState.IntPin = pin; ISRLevel = DccProcState.ExtIntMask;
DccProcState.IntBitmask = 1 << pin_num[pin]; ISRChkMask = DccProcState.ExtIntMask;
platform_gpio_mode(pin, PLATFORM_GPIO_INT, PLATFORM_GPIO_PULLUP); #ifdef ESP32
NODE_DBG("[dcc_setup] platform_gpio_register_intr_hook - pin: %d, mask: %d\n", DccProcState.IntPin, DccProcState.IntBitmask); ISRWatch = ISREdge;
platform_gpio_register_intr_hook(DccProcState.IntBitmask, InterruptHandler); attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, CHANGE);
#else
attachInterrupt( DccProcState.ExtIntNum, ExternalInterruptHandler, RISING);
#endif
gpio_pin_intr_state_set(GPIO_ID_PIN(pin_num[pin]), GPIO_PIN_INTR_POSEDGE);
// Set the Bits that control Multifunction or Accessory behaviour // Set the Bits that control Multifunction or Accessory behaviour
// and if the Accessory decoder optionally handles Output Addressing // and if the Accessory decoder optionally handles Output Addressing
// we need to peal off the top two bits // we need to peal off the top two bits
writeCV( CV_29_CONFIG, ( readCV( CV_29_CONFIG ) & ~FLAGS_CV29_BITS ) | (Flags & FLAGS_CV29_BITS) ) ; //!!!!! DccProcState.cv29Value = writeCV( CV_29_CONFIG, ( readCV( CV_29_CONFIG ) & ~FLAGS_CV29_BITS ) | (Flags & FLAGS_CV29_BITS) ) ;
uint8_t doAutoFactoryDefault = 0; uint8_t doAutoFactoryDefault = 0;
if((Flags & FLAGS_AUTO_FACTORY_DEFAULT) && (readCV(CV_VERSION_ID) == 255) && (readCV(CV_MANUFACTURER_ID) == 255)) if((Flags & FLAGS_AUTO_FACTORY_DEFAULT) && (readCV(CV_VERSION_ID) == 255) && (readCV(CV_MANUFACTURER_ID) == 255))
doAutoFactoryDefault = 1; doAutoFactoryDefault = 1;
writeCV( CV_VERSION_ID, VersionId ) ; writeCV( CV_VERSION_ID, VersionId ) ;
writeCV( CV_MANUFACTURER_ID, ManufacturerId ) ; writeCV( CV_MANUFACTURER_ID, ManufacturerId ) ;
...@@ -1145,17 +1590,118 @@ void dcc_setup(uint8_t pin, uint8_t ManufacturerId, uint8_t VersionId, uint8_t F ...@@ -1145,17 +1590,118 @@ void dcc_setup(uint8_t pin, uint8_t ManufacturerId, uint8_t VersionId, uint8_t F
clearDccProcState( 0 ); clearDccProcState( 0 );
if(notifyCVResetFactoryDefault && doAutoFactoryDefault) if(notifyCVResetFactoryDefault && doAutoFactoryDefault)
notifyCVResetFactoryDefault(); notifyCVResetFactoryDefault();
}
////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::getCV( uint16_t CV )
{
return readCV(CV);
}
////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::setCV( uint16_t CV, uint8_t Value)
{
DccProcState.Flags |= FLAGS_SETCV_CALLED;
uint8_t returnValue = writeCV(CV,Value);
DccProcState.Flags &= ~FLAGS_SETCV_CALLED;
return returnValue;
} }
void dcc_close() ////////////////////////////////////////////////////////////////////////
uint16_t NmraDcc::getAddr(void)
{ {
NODE_DBG("[dcc_close]\n"); return getMyAddr();
platform_gpio_mode(DccProcState.IntPin, PLATFORM_GPIO_INPUT, PLATFORM_GPIO_PULLUP);
} }
void dcc_init() ////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::isSetCVReady(void)
{
if(notifyIsSetCVReady)
return notifyIsSetCVReady();
return readyEEPROM();
}
////////////////////////////////////////////////////////////////////////
#ifdef DCC_DEBUG
uint8_t NmraDcc::getIntCount(void)
{ {
NODE_DBG("[dcc_init]\n"); return DccProcState.IntCount;
DataReady_taskid = task_get_id((task_callback_t) process); }
}
\ No newline at end of file ////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::getTickCount(void)
{
return DccProcState.TickCount;
}
////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::getNestedIrqCount(void)
{
return DccProcState.NestedIrqCount;
}
////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::getState(void)
{
return DccRx.State;
}
////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::getBitCount(void)
{
return DccRx.BitCount;
}
#endif
////////////////////////////////////////////////////////////////////////
void NmraDcc::setAccDecDCCAddrNextReceived(uint8_t enable)
{
DccProcState.inAccDecDCCAddrNextReceivedMode = enable;
}
////////////////////////////////////////////////////////////////////////
uint8_t NmraDcc::process()
{
if( DccProcState.inServiceMode )
{
if( (millis() - DccProcState.LastServiceModeMillis ) > 20L )
{
clearDccProcState( 0 ) ;
}
}
if( DccRx.DataReady )
{
// We need to do this check with interrupts disabled
#ifdef ESP32
portENTER_CRITICAL(&mux);
#else
noInterrupts();
#endif
Msg = DccRx.PacketCopy ;
DccRx.DataReady = 0 ;
#ifdef ESP32
portEXIT_CRITICAL(&mux);
#else
interrupts();
#endif
// Checking of the XOR-byte is now done in the ISR already
#ifdef DCC_DBGVAR
countOf.Tel++;
#endif
// Clear trailing bytes
for ( byte i=Msg.Size; i< MAX_DCC_MESSAGE_LEN; i++ ) Msg.Data[i] = 0;
if( notifyDccMsg ) notifyDccMsg( &Msg );
execDccProcessor( &Msg );
return 1 ;
}
return 0 ;
};
...@@ -2,11 +2,21 @@ ...@@ -2,11 +2,21 @@
// //
// Model Railroading with Arduino - NmraDcc.h // Model Railroading with Arduino - NmraDcc.h
// //
// Copyright (c) 2008 - 2018 Alex Shepherd // Copyright (c) 2008 - 2020 Alex Shepherd
// //
// This source file is subject of the GNU general public license 2, // This library is free software; you can redistribute it and/or
// that is available at the world-wide-web at // modify it under the terms of the GNU Lesser General Public
// http://www.gnu.org/licenses/gpl.txt // License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library 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
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
// //
//------------------------------------------------------------------------ //------------------------------------------------------------------------
// //
...@@ -29,30 +39,35 @@ ...@@ -29,30 +39,35 @@
// //
//------------------------------------------------------------------------ //------------------------------------------------------------------------
// NodeMCU Lua port by @voborsky
// #define NODE_DEBUG
// #define DCC_DEBUG
// #define DCC_DBGVAR
// Uncomment the following Line to Enable Service Mode CV Programming // Uncomment the following Line to Enable Service Mode CV Programming
#define NMRA_DCC_PROCESS_SERVICEMODE #define NMRA_DCC_PROCESS_SERVICEMODE
// Uncomment the following line to Enable MultiFunction Decoder Operations // Uncomment the following line to Enable MultiFunction Decoder Operations
#define NMRA_DCC_PROCESS_MULTIFUNCTION #define NMRA_DCC_PROCESS_MULTIFUNCTION
// #ifndef NMRADCC_IS_IN // Uncomment the following line to Enable 14 Speed Step Support
// #define NMRADCC_IS_IN //#define NMRA_DCC_ENABLE_14_SPEED_STEP_MODE
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#ifndef NMRADCC_IS_IN
#define NMRADCC_IS_IN
#define NMRADCC_VERSION 201 // Version 2.0.1 #define NMRADCC_VERSION 206 // Version 2.0.6
#define MAX_DCC_MESSAGE_LEN 6 // including XOR-Byte #define MAX_DCC_MESSAGE_LEN 6 // including XOR-Byte
//#define ALLOW_NESTED_IRQ // uncomment to enable nested IRQ's ( only for AVR! )
typedef struct typedef struct
{ {
uint8_t Size ; uint8_t Size ;
uint8_t PreambleBits ; uint8_t PreambleBits ;
uint8_t Data[MAX_DCC_MESSAGE_LEN] ; uint8_t Data[MAX_DCC_MESSAGE_LEN] ;
} DCC_MSG ; } DCC_MSG ;
//-------------------------------------------------------------------------- //--------------------------------------------------------------------------
...@@ -91,23 +106,39 @@ typedef struct ...@@ -91,23 +106,39 @@ typedef struct
#define CV_MANUFACTURER_ID 8 #define CV_MANUFACTURER_ID 8
#define CV_29_CONFIG 29 #define CV_29_CONFIG 29
#if defined(ESP32)
#include <esp_spi_flash.h>
#define MAXCV SPI_FLASH_SEC_SIZE
#elif defined(ESP8266)
#include <spi_flash.h>
#define MAXCV SPI_FLASH_SEC_SIZE
#elif defined( __STM32F1__)
#define MAXCV (EEPROM_PAGE_SIZE/4 - 1) // number of storage places (CV address could be larger
// because STM32 uses virtual adresses)
#undef ALLOW_NESTED_IRQ // This is done with NVIC on STM32
#define PRIO_DCC_IRQ 9
#define PRIO_SYSTIC 8 // MUST be higher priority than DCC Irq
#else
#define MAXCV E2END // the upper limit of the CV value currently defined to max memory.
#endif
typedef enum { typedef enum {
CV29_LOCO_DIR = 0b00000001, /** bit 0: Locomotive Direction: "0" = normal, "1" = reversed */ CV29_LOCO_DIR = 0b00000001, /** bit 0: Locomotive Direction: "0" = normal, "1" = reversed */
CV29_F0_LOCATION = 0b00000010, /** bit 1: F0 location: "0" = bit 4 in Speed and Direction instructions, "1" = bit 4 in function group one instruction */ CV29_F0_LOCATION = 0b00000010, /** bit 1: F0 location: "0" = bit 4 in Speed and Direction instructions, "1" = bit 4 in function group one instruction */
CV29_APS = 0b00000100, /** bit 2: Alternate Power Source (APS) "0" = NMRA Digital only, "1" = Alternate power source set by CV12 */ CV29_APS = 0b00000100, /** bit 2: Alternate Power Source (APS) "0" = NMRA Digital only, "1" = Alternate power source set by CV12 */
CV29_ADV_ACK = 0b00001000, /** bit 3: ACK, Advanced Acknowledge mode enabled if 1, disabled if 0 */ CV29_RAILCOM_ENABLE = 0b00001000, /** bit 3: BiDi ( RailCom ) is active */
CV29_SPEED_TABLE_ENABLE = 0b00010000, /** bit 4: STE, Speed Table Enable, "0" = values in CVs 2, 4 and 6, "1" = Custom table selected by CV 25 */ CV29_SPEED_TABLE_ENABLE = 0b00010000, /** bit 4: STE, Speed Table Enable, "0" = values in CVs 2, 4 and 6, "1" = Custom table selected by CV 25 */
CV29_EXT_ADDRESSING = 0b00100000, /** bit 5: "0" = one byte addressing, "1" = two byte addressing */ CV29_EXT_ADDRESSING = 0b00100000, /** bit 5: "0" = one byte addressing, "1" = two byte addressing */
CV29_OUTPUT_ADDRESS_MODE = 0b01000000, /** bit 6: "0" = Decoder Address Mode "1" = Output Address Mode */ CV29_OUTPUT_ADDRESS_MODE = 0b01000000, /** bit 6: "0" = Decoder Address Mode "1" = Output Address Mode */
CV29_ACCESSORY_DECODER = 0b10000000, /** bit 7: "0" = Multi-Function Decoder Mode "1" = Accessory Decoder Mode */ CV29_ACCESSORY_DECODER = 0b10000000, /** bit 7: "0" = Multi-Function Decoder Mode "1" = Accessory Decoder Mode */
} CV_29_BITS; } CV_29_BITS;
typedef enum { typedef enum {
#ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE #ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE
SPEED_STEP_14 = 15, /**< ESTOP=0, 1 to 15 */ SPEED_STEP_14 = 15, /**< ESTOP=0, 1 to 15 */
#endif #endif
SPEED_STEP_28 = 29, /**< ESTOP=0, 1 to 29 */ SPEED_STEP_28 = 29, /**< ESTOP=0, 1 to 29 */
SPEED_STEP_128 = 127 /**< ESTOP=0, 1 to 127 */ SPEED_STEP_128 = 127 /**< ESTOP=0, 1 to 127 */
} DCC_SPEED_STEPS; } DCC_SPEED_STEPS;
typedef enum { typedef enum {
...@@ -122,97 +153,264 @@ typedef enum { ...@@ -122,97 +153,264 @@ typedef enum {
typedef enum typedef enum
{ {
FN_0_4 = 1, FN_0_4 = 1,
FN_5_8, FN_5_8,
FN_9_12, FN_9_12,
FN_13_20, FN_13_20,
FN_21_28, FN_21_28,
#ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE #ifdef NMRA_DCC_ENABLE_14_SPEED_STEP_MODE
FN_0 /** function light is controlled by base line package (14 speed steps) */ FN_0 /** function light is controlled by base line package (14 speed steps) */
#endif #endif
} FN_GROUP; } FN_GROUP;
#define FN_BIT_00 0x10 #define FN_BIT_00 0x10
#define FN_BIT_01 0x01 #define FN_BIT_01 0x01
#define FN_BIT_02 0x02 #define FN_BIT_02 0x02
#define FN_BIT_03 0x04 #define FN_BIT_03 0x04
#define FN_BIT_04 0x08 #define FN_BIT_04 0x08
#define FN_BIT_05 0x01 #define FN_BIT_05 0x01
#define FN_BIT_06 0x02 #define FN_BIT_06 0x02
#define FN_BIT_07 0x04 #define FN_BIT_07 0x04
#define FN_BIT_08 0x08 #define FN_BIT_08 0x08
#define FN_BIT_09 0x01 #define FN_BIT_09 0x01
#define FN_BIT_10 0x02 #define FN_BIT_10 0x02
#define FN_BIT_11 0x04 #define FN_BIT_11 0x04
#define FN_BIT_12 0x08 #define FN_BIT_12 0x08
#define FN_BIT_13 0x01 #define FN_BIT_13 0x01
#define FN_BIT_14 0x02 #define FN_BIT_14 0x02
#define FN_BIT_15 0x04 #define FN_BIT_15 0x04
#define FN_BIT_16 0x08 #define FN_BIT_16 0x08
#define FN_BIT_17 0x10 #define FN_BIT_17 0x10
#define FN_BIT_18 0x20 #define FN_BIT_18 0x20
#define FN_BIT_19 0x40 #define FN_BIT_19 0x40
#define FN_BIT_20 0x80 #define FN_BIT_20 0x80
#define FN_BIT_21 0x01 #define FN_BIT_21 0x01
#define FN_BIT_22 0x02 #define FN_BIT_22 0x02
#define FN_BIT_23 0x04 #define FN_BIT_23 0x04
#define FN_BIT_24 0x08 #define FN_BIT_24 0x08
#define FN_BIT_25 0x10 #define FN_BIT_25 0x10
#define FN_BIT_26 0x20 #define FN_BIT_26 0x20
#define FN_BIT_27 0x40 #define FN_BIT_27 0x40
#define FN_BIT_28 0x80 #define FN_BIT_28 0x80
//#define DCC_DBGVAR
#ifdef DCC_DBGVAR #ifdef DCC_DBGVAR
typedef struct countOf_t { typedef struct countOf_t {
unsigned long Tel; unsigned long Tel;
unsigned long Err; unsigned long Err;
}countOf_t ; }countOf_t ;
countOf_t countOf; extern struct countOf_t countOf;
#endif #endif
class NmraDcc
{
private:
DCC_MSG Msg ;
public:
NmraDcc();
// Flag values to be logically ORed together and passed into the init() method // Flag values to be logically ORed together and passed into the init() method
#define FLAGS_MY_ADDRESS_ONLY 0x01 // Only process DCC Packets with My Address #define FLAGS_MY_ADDRESS_ONLY 0x01 // Only process DCC Packets with My Address
#define FLAGS_AUTO_FACTORY_DEFAULT 0x02 // Call notifyCVResetFactoryDefault() if CV 7 & 8 == 255 #define FLAGS_AUTO_FACTORY_DEFAULT 0x02 // Call notifyCVResetFactoryDefault() if CV 7 & 8 == 255
#define FLAGS_SETCV_CALLED 0x10 // only used internally !! #define FLAGS_SETCV_CALLED 0x10 // only used internally !!
#define FLAGS_OUTPUT_ADDRESS_MODE 0x40 // CV 29/541 bit 6 #define FLAGS_OUTPUT_ADDRESS_MODE 0x40 // CV 29/541 bit 6
#define FLAGS_DCC_ACCESSORY_DECODER 0x80 // CV 29/541 bit 7 #define FLAGS_DCC_ACCESSORY_DECODER 0x80 // CV 29/541 bit 7
// Flag Bits that are cloned from CV29 relating the DCC Accessory Decoder // Flag Bits that are cloned from CV29 relating the DCC Accessory Decoder
#define FLAGS_CV29_BITS (FLAGS_OUTPUT_ADDRESS_MODE | FLAGS_DCC_ACCESSORY_DECODER) #define FLAGS_CV29_BITS (FLAGS_OUTPUT_ADDRESS_MODE | FLAGS_DCC_ACCESSORY_DECODER)
#define DCC_RESET 1
#define DCC_IDLE 2 /*+
#define DCC_SPEED 3 * pin() is called from setup() and sets up the pin used to receive DCC packets.
#define DCC_SPEED_RAW 4 *
#define DCC_FUNC 5 * Inputs:
#define DCC_TURNOUT 6 * ExtIntNum - Interrupt number of the pin. Use digitalPinToInterrupt(ExtIntPinNum).
#define DCC_ACCESSORY 7 * ExtIntPinNum - Input pin number.
#define DCC_RAW 8 * EnablePullup - Set true to enable the pins pullup resistor.
#define DCC_SERVICEMODE 9 *
* Returns:
#define CV_VALID 10 * None.
#define CV_READ 11 */
#define CV_WRITE 12 void pin( uint8_t ExtIntNum, uint8_t ExtIntPinNum, uint8_t EnablePullup);
#define CV_RESET 13
/*+
* pin() is called from setup() and sets up the pin used to receive DCC packets.
void dcc_setup(uint8_t pin, uint8_t ManufacturerId, uint8_t VersionId, uint8_t Flags, uint8_t OpsModeAddressBaseCV ); * This relies on the internal function: digitalPinToInterrupt() to map the input pin number to the right interrupt
*
* Inputs:
void dcc_close(); * ExtIntPinNum - Input pin number.
* EnablePullup - Set true to enable the pins pullup resistor.
*
* Returns:
* None.
*/
#ifdef digitalPinToInterrupt
void pin( uint8_t ExtIntPinNum, uint8_t EnablePullup);
#endif
void dcc_init(); /*+
* init() is called from setup() after the pin() command is called.
* It initializes the NmDcc object and makes it ready to process packets.
*
* Inputs:
* ManufacturerId - Manufacturer ID returned in CV 8.
* Commonly MAN_ID_DIY.
* VersionId - Version ID returned in CV 7.
* Flags - ORed flags beginning with FLAGS_...
* FLAGS_MY_ADDRESS_ONLY - Only process packets with My Address.
* FLAGS_DCC_ACCESSORY_DECODER - Decoder is an accessory decoder.
* FLAGS_OUTPUT_ADDRESS_MODE - This flag applies to accessory decoders only.
* Accessory decoders normally have 4 paired outputs
* and a single address refers to all 4 outputs.
* Setting FLAGS_OUTPUT_ADDRESS_MODE causes each
* address to refer to a single output.
* OpsModeAddressBaseCV - Ops Mode base address. Set it to 0?
*
* Returns:
* None.
*/
void init( uint8_t ManufacturerId, uint8_t VersionId, uint8_t Flags, uint8_t OpsModeAddressBaseCV );
/*+
* initAccessoryDecoder() is called from setup() for accessory decoders.
* It calls init() with FLAGS_DCC_ACCESSORY_DECODER ORed into Flags.
*
* Inputs:
* ManufacturerId - Manufacturer ID returned in CV 8.
* Commonly MAN_ID_DIY.
* VersionId - Version ID returned in CV 7.
* Flags - ORed flags beginning with FLAGS_...
* FLAGS_DCC_ACCESSORY_DECODER will be set for init() call.
* OpsModeAddressBaseCV - Ops Mode base address. Set it to 0?
*
* Returns:
* None.
*/
void initAccessoryDecoder( uint8_t ManufacturerId, uint8_t VersionId, uint8_t Flags, uint8_t OpsModeAddressBaseCV );
/*+
* process() is called from loop() to process DCC packets.
* It must be called very frequently to keep up with the packets.
*
* Inputs:
* None.
*
* Returns:
* 1 - Packet succesfully parsed on this call to process().
* 0 - Packet not ready or received packet had an error.
*/
uint8_t process();
/*+
* getCV() returns the selected CV value.
*
* Inputs:
* CV - CV number. It must point to a valid CV.
*
* Returns:
* Value - CV value. Invalid CV numbers will return an undefined result
* since nothing will have been set in that EEPROM position.
* Calls notifyCVRead() if it is defined.
*/
uint8_t getCV( uint16_t CV );
/*+
* setCV() sets the value of a CV.
*
* Inputs:
* CV - CV number. It must point to a valid CV.
* Value - CV value.
*
* Returns:
* Value - CV value set by this call.
* since nothing will have been set in that EEPROM position.
* Calls notifyCVWrite() if it is defined.
* Calls notifyCVChange() if the value is changed by this call.
*/
uint8_t setCV( uint16_t CV, uint8_t Value);
/*+
* setAccDecDCCAddrNextReceived() enables/disables the setting of the board address from the next received turnout command
*
* Inputs:
* enable- boolean to enable or disable the mode
*
* Returns:
*/
void setAccDecDCCAddrNextReceived(uint8_t enable);
/*+
* isSetCVReady() returns 1 if EEPROM is ready to write.
*
* Inputs:
* CV - CV number. It must point to a valid CV.
* Value - CV value.
*
* Returns:
* ready - 1 if ready to write, 0 otherwise. AVR processor will block
* for several ms. for each write cycle so you should check this to avoid blocks.
* Note: It returns the value returned by notifyIsSetCVReady() if it is defined.
* Calls notifyIsSetCVReady() if it is defined.
*/
uint8_t isSetCVReady( void );
/*+
* getAddr() return the currently active decoder address.
* based on decoder type and current address size.
*
* Inputs:
* None.
*
* Returns:
* Adr - The current decoder address based on decoder type(Multifunction, Accessory)
* and short or long address selection for Multifunction decoders.
*/
uint16_t getAddr(void);
/*+
* getX() return debugging data if DCC_DEBUG is defined.
* You would really need to be modifying the library to need them.
*
* Inputs:
* None.
*
* Returns:
* getIntCount - Init to 0 and apparently never incremented?
* getTickCount - Init to 0 and incremented each time interrupt handler
* completes without an error.
* getBitCount - Bit count of valid packet, 0 otherwise. Only valid until
* start of the next packet.
* getState - Current WAIT_... state as defined by DccRxWaitState in NmraDcc.cpp.
* getNestedIrqCount - Init to 0 and incremented each time the interrupt handler
* is called before the previous interrupt was complete.
* This is an error indication and may indicate the system
* is not handling packets fast enough or some other error is occurring.
*/
// #define DCC_DEBUG
#ifdef DCC_DEBUG
uint8_t getIntCount(void);
uint8_t getTickCount(void);
uint8_t getBitCount(void);
uint8_t getState(void);
uint8_t getNestedIrqCount(void);
#endif
};
/************************************************************************************ /************************************************************************************
Call-back functions Call-back functions
************************************************************************************/ ************************************************************************************/
#if defined (__cplusplus)
extern "C" {
#endif
/*+ /*+
* notifyDccReset(uint8_t hardReset) Callback for a DCC reset command. * notifyDccReset(uint8_t hardReset) Callback for a DCC reset command.
* *
...@@ -445,6 +643,45 @@ extern uint8_t notifyCVRead( uint16_t CV) __attribute__ ((weak)); ...@@ -445,6 +643,45 @@ extern uint8_t notifyCVRead( uint16_t CV) __attribute__ ((weak));
*/ */
extern uint8_t notifyCVWrite( uint16_t CV, uint8_t Value) __attribute__ ((weak)); extern uint8_t notifyCVWrite( uint16_t CV, uint8_t Value) __attribute__ ((weak));
/*+
* notifyIsSetCVReady() Callback to to determine if CVs can be written.
* This is called when the library needs to determine
* is ready to write without blocking or failing.
* Note: If defined, this callback
* MUST determine if a CV write would block or fail
* return the appropriate value.
* If this callback is not defined,
* the library determines if a write to the EEPROM
* would block.
*
* Inputs:
* None
*
* Returns:
* 1 - CV is ready to be written.
* 0 - CV is not ready to be written.
*/
extern uint8_t notifyIsSetCVReady(void) __attribute__ ((weak));
/*+
* notifyCVChange() Called when a CV value is changed.
* This is called whenever a CV's value is changed.
* notifyDccCVChange() Called only when a CV value is changed by a Dcc packet or a internal lib function.
* it is NOT called if the CV is changed by means of the setCV() method.
* Note: It is not called if notifyCVWrite() is defined
* or if the value in the EEPROM is the same as the value
* in the write command.
*
* Inputs:
* CV - CV number.
* Value - Value of the CV.
*
* Returns:
* None
*/
extern void notifyCVChange( uint16_t CV, uint8_t Value) __attribute__ ((weak));
extern void notifyDccCVChange( uint16_t CV, uint8_t Value) __attribute__ ((weak));
/*+ /*+
* notifyCVResetFactoryDefault() Called when CVs must be reset. * notifyCVResetFactoryDefault() Called when CVs must be reset.
* This is called when CVs must be reset * This is called when CVs must be reset
...@@ -462,6 +699,29 @@ extern uint8_t notifyCVWrite( uint16_t CV, uint8_t Value) __attribute__ ((weak)) ...@@ -462,6 +699,29 @@ extern uint8_t notifyCVWrite( uint16_t CV, uint8_t Value) __attribute__ ((weak))
*/ */
extern void notifyCVResetFactoryDefault(void) __attribute__ ((weak)); extern void notifyCVResetFactoryDefault(void) __attribute__ ((weak));
/*+
* notifyCVAck() Called when a CV write must be acknowledged.
* This callback must increase the current drawn by this
* decoder by at least 60mA for 6ms +/- 1ms.
*
* Inputs:
* None
* *
* Returns:
* None
*/
extern void notifyCVAck(void) __attribute__ ((weak));
/*+
* notifyAdvancedCVAck() Called when a CV write must be acknowledged via Advanced Acknowledgement.
* This callback must send the Advanced Acknowledgement via RailComm.
*
* Inputs:
* None
* *
* Returns:
* None
*/
extern void notifyAdvancedCVAck(void) __attribute__ ((weak));
/*+ /*+
* notifyServiceMode(bool) Called when state of 'inServiceMode' changes * notifyServiceMode(bool) Called when state of 'inServiceMode' changes
* *
...@@ -475,5 +735,11 @@ extern void notifyServiceMode(bool) __attribute__ ((weak)); ...@@ -475,5 +735,11 @@ extern void notifyServiceMode(bool) __attribute__ ((weak));
// Deprecated, only for backward compatibility with version 1.4.2. // Deprecated, only for backward compatibility with version 1.4.2.
// Don't use in new designs. These functions may be dropped in future versions // Don't use in new designs. These functions may be dropped in future versions
// extern void notifyDccAccState( uint16_t Addr, uint16_t BoardAddr, uint8_t OutputAddr, uint8_t State ) __attribute__ ((weak)); extern void notifyDccAccState( uint16_t Addr, uint16_t BoardAddr, uint8_t OutputAddr, uint8_t State ) __attribute__ ((weak));
// extern void notifyDccSigState( uint16_t Addr, uint8_t OutputIndex, uint8_t State) __attribute__ ((weak)); extern void notifyDccSigState( uint16_t Addr, uint8_t OutputIndex, uint8_t State) __attribute__ ((weak));
#if defined (__cplusplus)
}
#endif
#endif
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment