Various HAL enhancements. (#182)

* Add <D SERVO vpin position> command

Allow a PWM servo to be driven to any arbitrary position.

* Enhancements for HAL drivers

Add state change notification for external GPIO module drivers;
Allow drivers to be installed statically by declaration (as an alternative to the 'create' call).

* Create IO_HCSR04.h

HAL driver for HC-SR04 ultrasonic distance sensor (sonar).

* Enable servo commands in NO-HAL mode, but return error.

Avoid compile errors in RMFT.cpp when compiled with basic HAL by including the Turnout::createServo function as a stub that returns NULL.

* Update IO_HCSR04.h

Minor changes

* Change <D SERVO>

Give the <D SERVO> command an optional parameter of the profile.  For example, <D SERVO 100 200 3> will slowly move the servo on pin 100 to PWM position corresponding to 200.  If omitted, the servo will move immediately (no animation).

* IODevice (HAL) changes

1) Put new devices on the end of the chain instead of the beginning.  This will give better performance for devices created first (ArduinoPins and extender GPIO devices, typically).
2) Remove unused functions.

* Update IO_HCSR04.h

Allow thresholds for ON and OFF to be separately configured at creation.

* Update IODevice.cpp

Fix compile error on IO_NO_HAL minimal HAL version.

* Update IO_PCA9685.cpp

Remove unnecessary duplicated call to min() function.

DCCEXParser.cpp

@@ -56,6 +56,7 @@ const int16_t HASH_KEYWORD_LCN = 15137;
 const int16_t HASH_KEYWORD_RESET = 26133;
 const int16_t HASH_KEYWORD_SPEED28 = -17064;
 const int16_t HASH_KEYWORD_SPEED128 = 25816;
+const int16_t HASH_KEYWORD_SERVO = 27709;
 
 int16_t DCCEXParser::stashP[MAX_COMMAND_PARAMS];
 bool DCCEXParser::stashBusy;
@@ -800,6 +801,10 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
 	StringFormatter::send(stream, F("128 Speedsteps"));
         return true;
 
+    case HASH_KEYWORD_SERVO:
+        IODevice::writeAnalogue(p[1], p[2], params>3 ? p[3] : 0);
+        break;
+
     default: // invalid/unknown
         break;
     }

IODevice.cpp

@@ -56,6 +56,7 @@ void IODevice::begin() {
   for (IODevice *dev=_firstDevice; dev!=NULL; dev = dev->_nextDevice) {
     dev->_begin();
   }
+  _initPhase = false;
 }
 
 // Overarching static loop() method for the IODevice subsystem.  Works through the
@@ -114,37 +115,6 @@ bool IODevice::hasCallback(VPIN vpin) {
   return dev->_hasCallback(vpin);
 }
 
-
-// Remove specified device if one exists.  This is necessary if devices are
-// created on-the-fly by Turnouts, Sensors or Outputs since they may have
-// been saved to EEPROM and recreated on start.
-void IODevice::remove(VPIN vpin) {
-  // Only works if the object is exclusive, i.e. only one VPIN.
-  IODevice *previousDev = 0;
-  for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice) {
-    if (dev->owns(vpin)) {
-      // Found object
-      if (dev->_isDeletable()) {
-        // First check it isn't next one to be processed by loop().
-        //   If so, skip to the following one.
-        if (dev == _nextLoopDevice) 
-          _nextLoopDevice = _nextLoopDevice->_nextDevice;
-        // Now unlink
-        if (!previousDev)
-          _firstDevice = dev->_nextDevice;
-        else
-          previousDev->_nextDevice = dev->_nextDevice;
-        delete dev;
-#ifdef DIAG_IO
-        DIAG(F("IODevice deleted Vpin:%d"), vpin);
-#endif
-        return;
-      }
-    }
-    previousDev = dev;
-  }
-}
-
 // Display (to diagnostics) details of the device.
 void IODevice::_display() {
   DIAG(F("Unknown device Vpins:%d-%d"), (int)_firstVpin, (int)_firstVpin+_nPins-1);
@@ -200,22 +170,25 @@ void IODevice::setGPIOInterruptPin(int16_t pinNumber) {
   _gpioInterruptPin = pinNumber;
 }
 
-IONotifyStateChangeCallback *IODevice::registerInputChangeNotification(IONotifyStateChangeCallback *callback) {
-  IONotifyStateChangeCallback *previousHead = _notifyCallbackChain;
-  _notifyCallbackChain = callback;
-  return previousHead;
-}
-
-
 // Private helper function to add a device to the chain of devices.
 void IODevice::addDevice(IODevice *newDevice) {
-  // Link new object to the start of chain.  Thereby,
+  // Link new object to the end of the chain.  Thereby, the first devices to be declared/created
-  // a write or read will act on the first device found.
+  // will be located faster by findDevice than those which are created later.
-  newDevice->_nextDevice = _firstDevice;
+  // Ideally declare/create the digital IO pins first, then servos, then more esoteric devices.
-  _firstDevice = newDevice;
+  IODevice *lastDevice;
+  if (_firstDevice == 0)
+    _firstDevice = newDevice;
+  else {
+    for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice)
+      lastDevice = dev;
+    lastDevice->_nextDevice = newDevice;
+  }
+  newDevice->_nextDevice = 0;
 
-  // Initialise device
+  // If the IODevice::begin() method has already been called, initialise device here.  If not,
-  newDevice->_begin();
+  // the device's _begin() method will be called by IODevice::begin().
+  if (!_initPhase)
+    newDevice->_begin();
 }
 
 // Private helper function to locate a device by VPIN.  Returns NULL if not found
@@ -231,7 +204,17 @@ IODevice *IODevice::findDevice(VPIN vpin) {
 // Static data
 //------------------------------------------------------------------------------------------------------------------
 
-IONotifyStateChangeCallback *IODevice::_notifyCallbackChain = 0;
+// Chain of callback blocks (identifying registered callback functions for state changes)
+IONotifyCallback *IONotifyCallback::first = 0;
+
+// Start of chain of devices.
+IODevice *IODevice::_firstDevice = 0;
+
+// Reference to next device to be called on _loop() method.
+IODevice *IODevice::_nextLoopDevice = 0;
+
+// Flag which is reset when IODevice::begin has been called.
+bool IODevice::_initPhase = true;  
 
 
 //==================================================================================================================
@@ -243,23 +226,6 @@ bool IODevice::owns(VPIN id) {
   return (id >= _firstVpin && id < _firstVpin + _nPins);
 }
 
-// Write to devices which are after the current one in the list; this 
-// function allows a device to have the same input and output VPIN number, and
-// a write to the VPIN from outside the device is passed to the device, but a 
-// call to writeDownstream will pass it to another device with the same
-// VPIN number if one exists.
-// void IODevice::writeDownstream(VPIN vpin, int value) {
-//   for (IODevice *dev = _nextDevice; dev != 0; dev = dev->_nextDevice) {
-//     if (dev->owns(vpin)) {
-//       dev->_write(vpin, value);
-//       return;
-//     }
-//   }
-// #ifdef DIAG_IO
-//   //DIAG(F("IODevice::write(): Vpin ID %d not found!"), (int)vpin);
-// #endif  
-// } 
-
 // Read value from virtual pin.
 int IODevice::read(VPIN vpin) {
   for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice) {
@@ -272,15 +238,6 @@ int IODevice::read(VPIN vpin) {
   return false;
 }
 
-bool IODevice::_isDeletable() {
-  return false;
-}
-
-// Start of chain of devices.
-IODevice *IODevice::_firstDevice = 0;
-
-// Reference to next device to be called on _loop() method.
-IODevice *IODevice::_nextLoopDevice = 0;
 
 #else // !defined(IO_NO_HAL)
 
@@ -298,6 +255,9 @@ void IODevice::write(VPIN vpin, int value) {
   digitalWrite(vpin, value);
   pinMode(vpin, OUTPUT);
 }
+void IODevice::writeAnalogue(VPIN vpin, int value, int profile) {
+  (void)vpin; (void)value; (void)profile; // Avoid compiler warnings
+}
 bool IODevice::hasCallback(VPIN vpin) { 
   (void)vpin;  // Avoid compiler warnings
   return false; 
@@ -311,16 +271,13 @@ void IODevice::DumpAll() {
   DIAG(F("NO HAL CONFIGURED!"));
 }
 bool IODevice::exists(VPIN vpin) { return (vpin > 2 && vpin < 49); }
-void IODevice::remove(VPIN vpin) {
-  (void)vpin;  // Avoid compiler warnings
-}
 void IODevice::setGPIOInterruptPin(int16_t pinNumber) {
   (void) pinNumber; // Avoid compiler warning
 }
-IONotifyStateChangeCallback *IODevice::registerInputChangeNotification(IONotifyStateChangeCallback *callback) {
+
-  (void)callback;  // Avoid compiler warning
+// Chain of callback blocks (identifying registered callback functions for state changes)
-  return NULL;
+// Not used in IO_NO_HAL but must be declared.
-}
+IONotifyCallback *IONotifyCallback::first = 0;
 
 #endif // IO_NO_HAL
 
@@ -373,11 +330,7 @@ void ArduinoPins::_write(VPIN vpin, int value) {
   uint8_t mask = 1 << ((pin-_firstVpin) % 8);
   uint8_t index = (pin-_firstVpin) / 8;
   // First update the output state, then set into write mode if not already.
-  #if defined(USE_FAST_IO)
   fastWriteDigital(pin, value);
-  #else
-  digitalWrite(pin, value);
-  #endif
   if (!(_pinModes[index] & mask)) {
     // Currently in read mode, change to write mode
     _pinModes[index] |= mask;
@@ -400,11 +353,7 @@ int ArduinoPins::_read(VPIN vpin) {
     else
       pinMode(pin, INPUT);
   }
-  #if defined(USE_FAST_IO)
   int value = !fastReadDigital(pin); // Invert (5v=0, 0v=1)
-  #else
-  int value = !digitalRead(pin); // Invert (5v=0, 0v=1)
-  #endif
 
   #ifdef DIAG_IO
   //DIAG(F("Arduino Read Pin:%d Value:%d"), pin, value);
@@ -418,9 +367,9 @@ void ArduinoPins::_display() {
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////
 
-#if defined(USE_FAST_IO)
 
 void ArduinoPins::fastWriteDigital(uint8_t pin, uint8_t value) {
+#if defined(USE_FAST_IO)
   if (pin >= NUM_DIGITAL_PINS) return;
   uint8_t mask = digitalPinToBitMask(pin);
   uint8_t port = digitalPinToPort(pin);
@@ -431,16 +380,22 @@ void ArduinoPins::fastWriteDigital(uint8_t pin, uint8_t value) {
   else
     *outPortAdr &= ~mask;
   interrupts();
+#else
+  digitalWrite(pin, value);
+#endif
 }
 
 bool ArduinoPins::fastReadDigital(uint8_t pin) {
+#if defined(USE_FAST_IO)
   if (pin >= NUM_DIGITAL_PINS) return false;
   uint8_t mask = digitalPinToBitMask(pin);
   uint8_t port = digitalPinToPort(pin);
   volatile uint8_t *inPortAdr = portInputRegister(port);
   // read input
   bool result = (*inPortAdr & mask) != 0;  
+#else
+  bool result = digitalRead(pin);
+#endif
   return result;
 }
 
-#endif

IODevice.h

@@ -49,9 +49,32 @@ typedef uint16_t VPIN;
 #define VPIN_MAX 32767  
 #define VPIN_NONE 65535
 
+/* 
+ * Callback support for state change notification from an IODevice subclass to a 
+ * handler, e.g. Sensor object handling.
+ */
 
-typedef void IONotifyStateChangeCallback(VPIN vpin, int value);
+class IONotifyCallback {
-
+public: 
+  typedef void IONotifyCallbackFunction(VPIN vpin, int value);
+  static void add(IONotifyCallbackFunction *function) {
+    IONotifyCallback *blk = new IONotifyCallback(function);
+    if (first) blk->next = first;
+    first = blk;
+  }
+  static void invokeAll(VPIN vpin, int value) {
+    for (IONotifyCallback *blk = first; blk != NULL; blk = blk->next)
+      blk->invoke(vpin, value);
+  }
+  static bool hasCallback() {
+    return first != NULL;
+  }
+private:
+  IONotifyCallback(IONotifyCallbackFunction *function) { invoke = function; };
+  IONotifyCallback *next = 0;
+  IONotifyCallbackFunction *invoke = 0;
+  static IONotifyCallback *first;
+};
 
 /*
  * IODevice class
@@ -82,7 +105,8 @@ public:
 
   // Static functions to find the device and invoke its member functions
 
-  // begin is invoked to create any standard IODevice subclass instances
+  // begin is invoked to create any standard IODevice subclass instances.
+  // Also, the _begin method of any existing instances is called from here.
   static void begin();
 
   // configure is used invoke an IODevice instance's _configure method
@@ -112,9 +136,6 @@ public:
   // exists checks whether there is a device owning the specified vpin
   static bool exists(VPIN vpin);
 
-  // remove deletes the device associated with the vpin, if it is deletable
-  static void remove(VPIN vpin);
-
   // Enable shared interrupt on specified pin for GPIO extender modules.  The extender module
   // should pull down this pin when requesting a scan.  The pin may be shared by multiple modules.
   // Without the shared interrupt, input states are scanned periodically to detect changes on 
@@ -123,23 +144,6 @@ public:
   // once the GPIO port concerned has been read.
   void setGPIOInterruptPin(int16_t pinNumber);
 
-  // Method to add a notification.  it is the caller's responsibility to save the return value
-  // and invoke the event handler associate with it.  Example:
-  //
-  //    NotifyStateChangeCallback *nextEv = registerInputChangeNotification(myProc);
-  //
-  //    void processChange(VPIN pin, int value) {
-  //      // Do something
-  //      // Pass on to next event handler
-  //      if (nextEv) nextEv(pin, value);
-  //     }
-  //
-  // Note that this implementation is rudimentary and assumes a small number of callbacks (typically one).  If 
-  //  more than one callback is registered, then the calls to successive callback functions are
-  //  nested, and stack usage will be impacted.  If callbacks are extensively used, it is recommended that
-  //  a class or struct be implemented to hold the callback address, which can be chained to avoid
-  //  nested callbacks.
-  static IONotifyStateChangeCallback *registerInputChangeNotification(IONotifyStateChangeCallback *callback);
   
 protected:
   
@@ -200,23 +204,18 @@ protected:
   // Destructor
   virtual ~IODevice() {};
   
-  // isDeletable returns true if object is deletable (i.e. is not a base device driver).
-  virtual bool _isDeletable();
-
   // Common object fields.
   VPIN _firstVpin;
   int _nPins;
 
-  // Pin number of interrupt pin for GPIO extender devices.  The device will pull this
+  // Pin number of interrupt pin for GPIO extender devices.  The extender module will pull this
   //  pin low if an input changes state.
   int16_t _gpioInterruptPin = -1;
 
   // Static support function for subclass creation
   static void addDevice(IODevice *newDevice);
 
-  // Notification of change
+  // Current state of device
-  static IONotifyStateChangeCallback *_notifyCallbackChain;
-
   DeviceStateEnum _deviceState = DEVSTATE_DORMANT;
 
 private:
@@ -229,6 +228,7 @@ private:
   static IODevice *_firstDevice;
 
   static IODevice *_nextLoopDevice;
+  static bool _initPhase;
 };
 
 
@@ -240,6 +240,8 @@ private:
 class PCA9685 : public IODevice {
 public:
   static void create(VPIN vpin, int nPins, uint8_t I2CAddress);
+  // Constructor
+  PCA9685(VPIN vpin, int nPins, uint8_t I2CAddress);
   enum ProfileType {
     Instant = 0,  // Moves immediately between positions
     Fast = 1,     // Takes around 500ms end-to-end
@@ -249,8 +251,6 @@ public:
   };
 
 private:
-  // Constructor
-  PCA9685(VPIN vpin, int nPins, uint8_t I2CAddress);
   // Device-specific initialisation
   void _begin() override;
   bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) override;
@@ -301,11 +301,12 @@ private:
 class DCCAccessoryDecoder: public IODevice {
 public:
   static void create(VPIN firstVpin, int nPins, int DCCAddress, int DCCSubaddress);
-
-private:
   // Constructor
   DCCAccessoryDecoder(VPIN firstVpin, int nPins, int DCCAddress, int DCCSubaddress);
+
+private:
   // Device-specific write function.
+  void _begin() override;
   void _write(VPIN vpin, int value) override;
   void _display() override;
   int _packedAddress;
@@ -326,6 +327,9 @@ public:
   // Constructor
   ArduinoPins(VPIN firstVpin, int nPins);
 
+  static void fastWriteDigital(uint8_t pin, uint8_t value);
+  static bool fastReadDigital(uint8_t pin);
+
 private:
   // Device-specific pin configuration
   bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) override;
@@ -335,8 +339,6 @@ private:
   int _read(VPIN vpin) override;
   void _display() override;
 
-  void fastWriteDigital(uint8_t pin, uint8_t value);
-  bool fastReadDigital(uint8_t pin);
 
   uint8_t *_pinPullups;
   uint8_t *_pinModes; // each bit is 1 for output, 0 for input

IO_DCCAccessory.cpp

@@ -39,7 +39,12 @@ DCCAccessoryDecoder::DCCAccessoryDecoder(VPIN vpin, int nPins, int DCCAddress, i
    _firstVpin = vpin;
   _nPins = nPins;
   _packedAddress = (DCCAddress << 2) + DCCSubaddress;
+}
+
+void DCCAccessoryDecoder::_begin() {
   int endAddress = _packedAddress + _nPins - 1;
+  int DCCAddress = _packedAddress >> 2;
+  int DCCSubaddress = _packedAddress & 3;
   DIAG(F("DCC Accessory Decoder configured Vpins:%d-%d Linear Address:%d-%d (%d/%d-%d/%d)"), _firstVpin, _firstVpin+_nPins-1,
       _packedAddress, _packedAddress+_nPins-1,
      DCCAddress, DCCSubaddress, endAddress >> 2, endAddress % 4);

IO_ExampleSerial.cpp

@@ -25,21 +25,25 @@
 IO_ExampleSerial::IO_ExampleSerial(VPIN firstVpin, int nPins, HardwareSerial *serial, unsigned long baud) {
   _firstVpin = firstVpin;
   _nPins = nPins;
+  _pinValues = (uint16_t *)calloc(_nPins, sizeof(uint16_t));
+  _baud = baud;
   
   // Save reference to serial port driver
   _serial = serial;
-  _serial->begin(baud);
+
-  DIAG(F("ExampleSerial configured Vpins:%d-%d"), _firstVpin, _firstVpin+_nPins-1);
+  addDevice(this);
 }
 
 // Static create method for one module.
 void IO_ExampleSerial::create(VPIN firstVpin, int nPins, HardwareSerial *serial, unsigned long baud) {
-  IO_ExampleSerial *dev = new IO_ExampleSerial(firstVpin, nPins, serial, baud);
+  new IO_ExampleSerial(firstVpin, nPins, serial, baud);
-  addDevice(dev);
 }
 
 // Device-specific initialisation
 void IO_ExampleSerial::_begin() {
+  _serial->begin(_baud);
+  DIAG(F("ExampleSerial configured Vpins:%d-%d"), _firstVpin, _firstVpin+_nPins-1);
+
   // Send a few # characters to the output
   for (uint8_t i=0; i<3; i++)
     _serial->write('#');
@@ -65,9 +69,8 @@ void IO_ExampleSerial::_write(VPIN vpin, int value) {
 // Device-specific read function.
 int IO_ExampleSerial::_read(VPIN vpin) {
 
-  // Return a value for the specified vpin.  For illustration, return 
+  // Return a value for the specified vpin.
-  // a value indicating whether the pin number is odd.
+  int result = _pinValues[vpin-_firstVpin];
-  int result = (vpin & 1);
 
   return result;
 }
@@ -80,35 +83,38 @@ void IO_ExampleSerial::_loop(unsigned long currentMicros) {
   if (_serial->available()) {
     // Input data available to read.  Read a character.
     char c = _serial->read();
-    switch (inputState) {
+    switch (_inputState) {
       case 0: // Waiting for start of command
         if (c == '#')  // Start of command received.
-          inputState = 1;
+          _inputState = 1;
         break;
       case 1: // Expecting command character
         if (c == 'N') { // 'Notify' character received
-          inputState = 2;
+          _inputState = 2;
-          inputValue = inputIndex = 0;
+          _inputValue = _inputIndex = 0;
         } else
-          inputState = 0; // Unexpected char, reset
+          _inputState = 0; // Unexpected char, reset
         break;
       case 2: // reading first parameter (index)
         if (isdigit(c))
-          inputIndex = inputIndex * 10 + (c-'0');
+          _inputIndex = _inputIndex * 10 + (c-'0');
         else if (c==',') 
-          inputState = 3;
+          _inputState = 3;
         else
-          inputState = 0; // Unexpected char, reset
+          _inputState = 0; // Unexpected char, reset
         break;
       case 3: // reading reading second parameter (value)
         if (isdigit(c)) 
-          inputValue = inputValue * 10 - (c-'0');
+          _inputValue = _inputValue * 10 - (c-'0');
         else if (c=='#') { // End of command
           // Complete command received, do something with it.
-          DIAG(F("ExampleSerial Received command, p1=%d, p2=%d"), inputIndex, inputValue);
+          DIAG(F("ExampleSerial Received command, p1=%d, p2=%d"), _inputIndex, _inputValue);
-          inputState = 0; // Done, start again.
+          if (_inputIndex < _nPins) { // Store value
+            _pinValues[_inputIndex] = _inputValue;
+          }
+          _inputState = 0; // Done, start again.
         } else
-          inputState = 0; // Unexpected char, reset
+          _inputState = 0; // Unexpected char, reset
         break;
     }
   }

IO_ExampleSerial.h

@@ -17,6 +17,18 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 
+/*
+ * To declare a device instance, 
+ *    IO_ExampleSerial myDevice(1000, 10, Serial3, 9600);
+ * or to create programmatically,
+ *    IO_ExampleSerial::create(1000, 10, Serial3, 9600);
+ * 
+ * (uses VPINs 1000-1009, talke on Serial 3 at 9600 baud.)
+ * 
+ * See IO_ExampleSerial.cpp for the protocol used over the serial line.
+ * 
+ */
+
 #ifndef IO_EXAMPLESERIAL_H
 #define IO_EXAMPLESERIAL_H
 
@@ -27,6 +39,7 @@ public:
   IO_ExampleSerial(VPIN firstVpin, int nPins, HardwareSerial *serial, unsigned long baud);
   static void create(VPIN firstVpin, int nPins, HardwareSerial *serial, unsigned long baud);  
 
+protected:
   void _begin() override;
   void _loop(unsigned long currentMicros) override;
   void _write(VPIN vpin, int value) override;
@@ -35,9 +48,11 @@ public:
 
 private:
   HardwareSerial *_serial;
-  uint8_t inputState = 0;
+  uint8_t _inputState = 0;
-  int inputIndex = 0;
+  int _inputIndex = 0;
-  int inputValue = 0;
+  int _inputValue = 0;
+  uint16_t *_pinValues; // Pointer to block of memory containing pin values
+  unsigned long _baud;
 };
 
 #endif // IO_EXAMPLESERIAL_H

IO_GPIOBase.h

@@ -45,6 +45,10 @@ protected:
   int _read(VPIN vpin) override;
   void _display() override;
   void _loop(unsigned long currentMicros) override;
+  bool _hasCallback(VPIN vpin) {
+    (void)vpin;   // suppress compiler warning
+    return true;  // Enable callback if caller wants to use it.
+  }
 
   // Data fields
   uint8_t _I2CAddress; 
@@ -82,29 +86,29 @@ GPIOBase<T>::GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, uint8_t I2
   _nPins = nPins;
   _I2CAddress = I2CAddress;
   _gpioInterruptPin = interruptPin;
-  _notifyCallbackChain = 0;
   // Add device to list of devices.
   addDevice(this);
+}
 
+template <class T>
+void GPIOBase<T>::_begin() {
   // Configure pin used for GPIO extender notification of change (if allocated)
   if (_gpioInterruptPin >= 0) 
     pinMode(_gpioInterruptPin, INPUT_PULLUP);
 
   I2CManager.begin();
   I2CManager.setClock(400000);
-  if (I2CManager.exists(I2CAddress)) {
+  if (I2CManager.exists(_I2CAddress)) {
     _display();
     _portMode = 0;  // default to input mode
     _portPullup = -1; // default to pullup enabled
-    _portInputState = 0; 
+    _portInputState = -1; 
   }
+  _setupDevice();
   _deviceState = DEVSTATE_NORMAL;
   _lastLoopEntry = micros();
 }
 
-template <class T>
-void GPIOBase<T>::_begin() {}
-
 // Configuration parameters for inputs: 
 //  params[0]: enable pullup
 //  params[1]: invert input (optional)
@@ -134,9 +138,7 @@ bool GPIOBase<T>::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCoun
 // Periodically read the input port
 template <class T>
 void GPIOBase<T>::_loop(unsigned long currentMicros) {
-  #ifdef DIAG_IO
   T lastPortStates = _portInputState;
-  #endif
   if (_deviceState == DEVSTATE_SCANNING && !requestBlock.isBusy()) {
     uint8_t status = requestBlock.status;
     if (status == I2C_STATUS_OK) {
@@ -146,7 +148,27 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
       DIAG(F("%S I2C:x%x Error:%d"), _deviceName, _I2CAddress, status);
     }
     _processCompletion(status);
+
+    // Scan for changes in input states and invoke callback (if present)
+    T differences = lastPortStates ^ _portInputState;
+    if (differences && IONotifyCallback::hasCallback()) {
+      // Scan for differences bit by bit
+      T mask = 1;
+      for (int pin=0; pin<_nPins; pin++) {
+        if (differences & mask) {
+          // Change detected.
+          IONotifyCallback::invokeAll(_firstVpin+pin, (_portInputState & mask) == 0);
+        }
+        mask <<= 1;
+      }
+    }
+
+    #ifdef DIAG_IO
+    if (differences)
+      DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, _I2CAddress, _portInputState);
+    #endif
   }
+
   // Check if interrupt configured.  If so, and pin is not pulled down, finish.
   if (_gpioInterruptPin >= 0) {
     if (digitalRead(_gpioInterruptPin)) return;
@@ -162,12 +184,6 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
     _readGpioPort(false);  // Initiate non-blocking read
     _deviceState= DEVSTATE_SCANNING;
   }
-
-  #ifdef DIAG_IO
-  T differences = lastPortStates ^ _portInputState;
-  if (differences)
-    DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, _I2CAddress, _portInputState);
-  #endif
 }
 
 template <class T>

IO_HCSR04.h

@@ -0,0 +1,173 @@
+/*
+ *  © 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/>.
+ */
+
+/* 
+ * The HC-SR04 module has an ultrasonic transmitter (40kHz) and a receiver.
+ * It is operated through two signal pins.  When the transmit pin is set to 1 for 
+ * 10us, on the falling edge the transmitter sends a short transmission of 
+ * 8 pulses (like a sonar 'ping').  This is reflected off objects and received
+ * by the receiver.  A pulse is sent on the receive pin whose length is equal
+ * to the delay between the transmission of the pulse and the detection of
+ * its echo.  The distance of the reflecting object is calculated by halving
+ * the time (to allow for the out and back distance), then multiplying by the
+ * speed of sound (assumed to be constant).
+ * 
+ * This driver polls the HC-SR04 by sending the trigger pulse and then measuring
+ * the length of the received pulse.  If the calculated distance is less than the
+ * threshold, the output changes to 1.  If it is greater than the threshold plus
+ * a hysteresis margin, the output changes to 0.
+ * 
+ * The measurement would be more reliable if interrupts were disabled while the
+ * pulse is being timed.  However, this would affect other functions in the CS
+ * so the measurement is being performed with interrupts enabled.  Also, we could
+ * use an interrupt pin in the Arduino for the timing, but the same consideration 
+ * applies.
+ * 
+ * Note: The timing accuracy required by this means that the pins have to be 
+ * direct Arduino pins; GPIO pins on an IO Extender cannot provide the required
+ * accuracy.
+ */
+
+#ifndef IO_HCSR04_H
+#define IO_HCSR04_H
+
+#include "IODevice.h"
+
+class HCSR04 : public IODevice {
+
+private:
+  // pins must be arduino GPIO pins, not extender pins or HAL pins.
+  int _transmitPin = -1;
+  int _receivePin = -1;
+  // Thresholds for setting active state in cm.
+  uint8_t _onThreshold;  // cm
+  uint8_t _offThreshold; // cm
+  // Active=1/inactive=0 state 
+  uint8_t _value = 0;
+  // Time of last loop execution
+  unsigned long _lastExecutionTime;
+  // Factor for calculating the distance (cm) from echo time (ms).
+  //  Based on a speed of sound of 345 metres/second.
+  const uint16_t factor = 58; // ms/cm
+
+public:
+  // Constructor perfroms static initialisation of the device object
+  HCSR04 (VPIN vpin, int transmitPin, int receivePin, uint16_t onThreshold, uint16_t offThreshold) {
+    _firstVpin = vpin;
+    _nPins = 1;
+    _transmitPin = transmitPin;
+    _receivePin = receivePin;
+    _onThreshold = onThreshold;
+    _offThreshold = offThreshold;
+    addDevice(this);
+  }
+
+  // Static create function provides alternative way to create object
+  static void create(VPIN vpin, int transmitPin, int receivePin, uint16_t onThreshold, uint16_t offThreshold) {
+    new HCSR04(vpin, transmitPin, receivePin, onThreshold, offThreshold);
+  }
+
+protected:
+  // _begin function called to perform dynamic initialisation of the device
+  void _begin() override {
+    pinMode(_transmitPin, OUTPUT);
+    pinMode(_receivePin, INPUT);
+    ArduinoPins::fastWriteDigital(_transmitPin, 0);
+    _lastExecutionTime = micros();
+    DIAG(F("HCSR04 configured on VPIN:%d TXpin:%d RXpin:%d On:%dcm Off:%dcm"),
+      _firstVpin, _transmitPin, _receivePin, _onThreshold, _offThreshold);
+  }
+
+  // _read function - just return _value (calculated in _loop).
+  int _read(VPIN vpin) override {
+    (void)vpin;  // avoid compiler warning
+    return _value;
+  }
+
+  // _loop function - read HC-SR04 once every 50 milliseconds.
+  void _loop(unsigned long currentMicros) override {
+    if (currentMicros - _lastExecutionTime > 50000) {
+      _lastExecutionTime = currentMicros;
+
+      _value = read_HCSR04device();
+    }
+  }
+
+private:
+  // This polls the HC-SR04 device by sending a pulse and measuring the duration of
+  //  the pulse observed on the receive pin.  In order to be kind to the rest of the CS
+  //  software, no interrupts are used and interrupts are not disabled.  The pulse duration
+  //  is measured in a loop, using the micros() function.  Therefore, interrupts from other
+  //  sources may affect the result.  However, interrupts response code in CS typically takes
+  //  much less than the 58us frequency for the DCC interrupt, and 58us corresponds to only 1cm
+  //  in the HC-SR04.
+  //  To reduce chatter on the output, hysteresis is applied on reset: the output is set to 1 when the 
+  //  measured distance is less than the onThreshold, and is set to 0 if the measured distance is
+  //  greater than the offThreshold.
+  //
+  uint8_t read_HCSR04device() {
+    // uint16 enough to time up to 65ms
+    uint16_t startTime, waitTime, currentTime, maxTime;  
+
+    // If receive pin is still set on from previous call, abort the read.
+    if (ArduinoPins::fastReadDigital(_receivePin)) return _value;
+
+    // Send 10us pulse to trigger transmitter
+    ArduinoPins::fastWriteDigital(_transmitPin, 1);
+    delayMicroseconds(10);
+    ArduinoPins::fastWriteDigital(_transmitPin, 0);
+
+    // Wait for receive pin to be set
+    startTime = currentTime = micros();
+    maxTime = factor * _offThreshold * 2;
+    while (!ArduinoPins::fastReadDigital(_receivePin)) {
+      // lastTime = currentTime;
+      currentTime = micros();
+      waitTime = currentTime - startTime;
+      if (waitTime > maxTime) {
+        // Timeout waiting for pulse start, abort the read
+        return _value;
+      }
+    }
+
+    // Wait for receive pin to reset, and measure length of pulse
+    startTime = currentTime = micros();
+    maxTime = factor * _offThreshold;
+    while (ArduinoPins::fastReadDigital(_receivePin)) {
+      currentTime = micros();
+      waitTime = currentTime - startTime;
+      // If pulse is too long then set return value to zero,
+      //  and finish without waiting for end of pulse.
+      if (waitTime > maxTime) {
+        // Pulse length longer than maxTime, reset value.
+        return 0;
+      }
+    } 
+    // Check if pulse length is below threshold, if so set value.
+    //DIAG(F("HCSR04: Pulse Len=%l Distance=%d"), waitTime, distance);
+    uint16_t distance = waitTime / factor; // in centimetres
+    if (distance < _onThreshold) 
+      return 1;
+
+    return _value;
+  }
+
+};
+
+#endif //IO_HCSR04_H

IO_MCP23008.h

@@ -28,7 +28,6 @@ public:
     new MCP23008(firstVpin, nPins, I2CAddress, interruptPin);
   }
 
-private:
   // Constructor
   MCP23008(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
     : GPIOBase<uint8_t>((FSH *)F("MCP23008"), firstVpin, min(nPins, 8), I2CAddress, interruptPin) {
@@ -38,6 +37,7 @@ private:
     outputBuffer[0] = REG_GPIO;
   }
   
+private:
   void _writeGpioPort() override {
     I2CManager.write(_I2CAddress, 2, REG_GPIO, _portOutputState);
   }

IO_MCP23017.h

@@ -34,7 +34,6 @@ public:
     new MCP23017(vpin, min(nPins,16), I2CAddress, interruptPin);
   }
   
-private:
   // Constructor
   MCP23017(VPIN vpin, int nPins, uint8_t I2CAddress, int interruptPin=-1) 
     : GPIOBase<uint16_t>((FSH *)F("MCP23017"), vpin, nPins, I2CAddress, interruptPin) 
@@ -42,9 +41,9 @@ private:
     requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
       outputBuffer, sizeof(outputBuffer));
     outputBuffer[0] = REG_GPIOA;
-    _setupDevice();
   }
 
+private:
   void _writeGpioPort() override {
     I2CManager.write(_I2CAddress, 3, REG_GPIOA, _portOutputState, _portOutputState>>8);
   }

IO_PCA9685.cpp

@@ -82,6 +82,10 @@ PCA9685::PCA9685(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
 
   // Initialise structure used for setting pulse rate
   requestBlock.setWriteParams(_I2CAddress, outputBuffer, sizeof(outputBuffer));
+}
+
+// Device-specific initialisation
+void PCA9685::_begin() {
   I2CManager.begin();
   I2CManager.setClock(1000000); // Nominally able to run up to 1MHz on I2C
           // In reality, other devices including the Arduino will limit 
@@ -100,10 +104,6 @@ PCA9685::PCA9685(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
   }
 }
 
-// Device-specific initialisation
-void PCA9685::_begin() {
-}
-
 // Device-specific write function, invoked from IODevice::write().
 void PCA9685::_write(VPIN vpin, int value) {
   #ifdef DIAG_IO
@@ -166,7 +166,7 @@ void PCA9685::_writeAnalogue(VPIN vpin, int value, int profile) {
                 profile==Bounce ? sizeof(_bounceProfile)-1 : 
                 1;
   s->stepNumber = 0;
-  s->toPosition = min(value, 4095);
+  s->toPosition = value;
   s->fromPosition = s->currentPosition;
 }
 

IO_PCF8574.h

@@ -28,13 +28,13 @@ public:
     new PCF8574(firstVpin, nPins, I2CAddress, interruptPin);
   }
 
-private:
   PCF8574(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
     : GPIOBase<uint8_t>((FSH *)F("PCF8574"), firstVpin, min(nPins, 8), I2CAddress, interruptPin) 
   {
     requestBlock.setReadParams(_I2CAddress, inputBuffer, 1);
   }
   
+private:
   // The pin state is '1' if the pin is an input or if it is an output set to 1.  Zero otherwise. 
   void _writeGpioPort() override {
     I2CManager.write(_I2CAddress, 1, _portOutputState | ~_portMode);
@@ -73,7 +73,10 @@ private:
       _portInputState = 0xff;
   }
 
-  void _setupDevice() override { }
+  // Set up device ports
+  void _setupDevice() override { 
+    _writePortModes();
+  }
  
   uint8_t inputBuffer[1];
 };

Sensors.cpp

@@ -91,7 +91,7 @@ void Sensor::checkAll(Print *stream){
 #ifdef USE_NOTIFY
   // Register the event handler ONCE!
   if (!inputChangeCallbackRegistered)
-    nextInputChangeCallback = IODevice::registerInputChangeNotification(inputChangeCallback);
+    IONotifyCallback::add(inputChangeCallback);
   inputChangeCallbackRegistered = true;
 #endif
 
@@ -192,8 +192,6 @@ void Sensor::inputChangeCallback(VPIN vpin, int state) {
   if (tt != NULL) { // Sensor found
     tt->inputState = (state != 0); 
   }
-  // Call next registered callback function
-  if (nextInputChangeCallback) nextInputChangeCallback(vpin, state);
 }
 #endif
 
@@ -345,6 +343,5 @@ unsigned long Sensor::lastReadCycle=0;
 Sensor *Sensor::firstPollSensor = NULL;
 Sensor *Sensor::lastSensor = NULL;
 bool Sensor::pollSignalPhase = false;
-IONotifyStateChangeCallback *Sensor::nextInputChangeCallback = 0;
 bool Sensor::inputChangeCallbackRegistered = false;
 #endif

Sensors.h

@@ -92,7 +92,6 @@ public:
 #ifdef USE_NOTIFY
   static bool pollSignalPhase;
   static void inputChangeCallback(VPIN vpin, int state);
-  static IONotifyStateChangeCallback *nextInputChangeCallback;
   static bool inputChangeCallbackRegistered;
 #endif
   

Turnouts.cpp

@@ -18,7 +18,7 @@
  *  You should have received a copy of the GNU General Public License
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
-#define EESTOREDEBUG 
+//#define EESTOREDEBUG 
 #include "defines.h"
 #include "Turnouts.h"
 #include "EEStore.h"
@@ -72,13 +72,11 @@ void Turnout::print(Print *stream){
       // VPIN Digital output
       StringFormatter::send(stream, F("<H %d VPIN %d %d>\n"), data.id, data.vpinData.vpin, state);
       break;
-#ifndef IO_NO_HAL
     case TURNOUT_SERVO:
       // Servo Turnout
       StringFormatter::send(stream, F("<H %d SERVO %d %d %d %d %d>\n"), data.id, data.servoData.vpin, 
         data.servoData.activePosition, data.servoData.inactivePosition, data.servoData.profile, state);
       break;
-#endif
     default:
       break;
   }
@@ -89,9 +87,6 @@ void Turnout::print(Print *stream){
 //   Returns false if turnout not found.
 
 bool Turnout::activate(int n, bool state){
-#ifdef EESTOREDEBUG
-  DIAG(F("Turnout::activate(%d,%d)"),n,state);
-#endif
   Turnout * tt=get(n);
   if (!tt) return false;
   tt->activate(state);
@@ -136,11 +131,11 @@ void Turnout::activate(bool state) {
         DCC::setAccessory((((data.dccAccessoryData.address-1) >> 2) + 1), 
           ((data.dccAccessoryData.address-1) & 3), state);
         break;
-#ifndef IO_NO_HAL
       case TURNOUT_SERVO:
+#ifndef IO_NO_HAL
         IODevice::write(data.servoData.vpin, state);
-        break;
 #endif
+        break;
       case TURNOUT_VPIN:
         IODevice::write(data.vpinData.vpin, state);
         break;
@@ -205,12 +200,10 @@ void Turnout::load(){
       case TURNOUT_LCN:
         // LCN turnouts are created when the remote device sends a message.
         break;
-#ifndef IO_NO_HAL
       case TURNOUT_SERVO:
         tt=createServo(data.id, data.servoData.vpin, 
           data.servoData.activePosition, data.servoData.inactivePosition, data.servoData.profile, lastKnownState);
         break;
-#endif
       case TURNOUT_VPIN:
         tt=createVpin(data.id, data.vpinData.vpin, lastKnownState);  // VPIN-based turnout
         break;
@@ -294,11 +287,11 @@ Turnout *Turnout::createVpin(int id, VPIN vpin, uint8_t state){
   return(tt);
 }
 
-#ifndef IO_NO_HAL
 ///////////////////////////////////////////////////////////////////////////////
 // Method for creating a Servo Turnout, e.g. connected to PCA9685 PWM device.
 
 Turnout *Turnout::createServo(int id, VPIN vpin, uint16_t activePosition, uint16_t inactivePosition, uint8_t profile, uint8_t state){
+#ifndef IO_NO_HAL
   if (activePosition > 511 || inactivePosition > 511 || profile > 4) return NULL;
 
   Turnout *tt=create(id);
@@ -317,8 +310,11 @@ Turnout *Turnout::createServo(int id, VPIN vpin, uint16_t activePosition, uint16
     return NULL;
   }
   return(tt);
-}
+#else
+  (void)id; (void)vpin; (void)activePosition; (void)inactivePosition; (void)profile; (void)state;  // avoid compiler warnings
+  return NULL;
 #endif
+}
 
 ///////////////////////////////////////////////////////////////////////////////
 // Support for <T id SERVO pin activepos inactive pos profile>
@@ -326,14 +322,12 @@ Turnout *Turnout::createServo(int id, VPIN vpin, uint16_t activePosition, uint16
 // and <T id VPIN pin>
 
 Turnout *Turnout::create(int id, int params, int16_t p[]) {
-#ifndef IO_NO_HAL
   if (p[0] == HASH_KEYWORD_SERVO) { // <T id SERVO n n n n>
     if (params == 5)
       return createServo(id, (VPIN)p[1], (uint16_t)p[2], (uint16_t)p[3], (uint8_t)p[4]);
     else  
       return NULL;
   } else 
-#endif
   if (p[0] == HASH_KEYWORD_VPIN) { // <T id VPIN n>
     if (params==2)
       return createVpin(id, p[1]);
@@ -350,11 +344,9 @@ Turnout *Turnout::create(int id, int params, int16_t p[]) {
   } else if (params==2) { // <T id n n> for DCC or LCN
     return createDCC(id, p[0], p[1]);
   } 
-#ifndef IO_NO_HAL
   else if (params==3) { // legacy <T id n n n> for Servo
     return createServo(id, (VPIN)p[0], (uint16_t)p[1], (uint16_t)p[2]);
   }
-#endif
 
   return NULL;
 }