Adjusted mutex lock and delay for modbus

lib/GPIO/BUZZER_Driver.cpp

@@ -10,18 +10,18 @@ namespace drivers
     Buzzer::Buzzer()
     {
         LOG_INFO("Initializing Beeper");
-        pinMode(buzzerPin, OUTPUT);
+        pinMode(c_buzzerPin, OUTPUT);
-        ledcAttach(buzzerPin, 1000, 8);
+        ledcAttach(c_buzzerPin, 1000, 8);
-        m_bp.pin = buzzerPin;
+        m_bp.pin = c_buzzerPin;
         m_bp.beeperTask = NULL;
-        //beep(50, NOTE_G);
+        beep(50, NOTE_G);
     }
 
     Buzzer::~Buzzer()
     {
         beepStop();
-        ledcDetach(buzzerPin);
+        ledcDetach(c_buzzerPin);
-        pinMode(buzzerPin, INPUT);
+        pinMode(c_buzzerPin, INPUT);
     }
 
     void Buzzer::beep(const uint16_t tBeep, const note_t note)
@@ -57,11 +57,11 @@ namespace drivers
         while (true)
         {
             ledcWriteNote(bPar->pin, bPar->note, OCTAVE); // on with selected note
-            vTaskDelay(pdMS_TO_TICKS(bPar->tOn));
+            delay(bPar->tOn);
             ledcWriteTone(bPar->pin, 0); // off
             if (bPar->tOff == 0)
                 break;
-            vTaskDelay(pdMS_TO_TICKS(bPar->tOff));
+            delay(bPar->tOff);
         }
         LOG_DEBUG("Beeper Task Ended");
         bPar->beeperTask = NULL;

lib/GPIO/BUZZER_Driver.h

@@ -10,7 +10,8 @@ namespace drivers
 
     class Buzzer
     {
-        const uint8_t buzzerPin = 46; // hardware assigned
+        const uint8_t c_buzzerPin = 46; // hardware assigned
+        
         typedef struct
         {
             note_t note;

lib/GPIO/LED_Driver.cpp

@@ -9,21 +9,21 @@ namespace drivers
     Led::Led()
     {
         LOG_INFO("Inizializing RGB Led");
-        pinMode(ledPin, OUTPUT);
+        pinMode(c_ledPin, OUTPUT);
-        m_lp.pin = ledPin;
+        m_lp.pin = c_ledPin;
         m_lp.blinkTask = NULL;
     }
 
     Led::~Led()
     {
         setColor({0, 0, 0});
-        pinMode(ledPin, INPUT);
+        pinMode(c_ledPin, INPUT);
     }
 
     void Led::setColor(const color_t color)
     {
         blinkStop();
-        rgbLedWrite(ledPin, color.r, color.g, color.b);
+        rgbLedWrite(c_ledPin, color.r, color.g, color.b);
     }
 
     void Led::blinkColor(const uint16_t tOn, const uint16_t tOff, const color_t color)
@@ -62,11 +62,11 @@ namespace drivers
         while (true)
         {
             rgbLedWrite(lPar->pin, lPar->color1.g, lPar->color1.r, lPar->color1.b);
-            vTaskDelay(pdMS_TO_TICKS(lPar->tOn));
+            delay(lPar->tOn);
             rgbLedWrite(lPar->pin, lPar->color2.g, lPar->color2.r, lPar->color2.b); // off
             if (lPar->tOff == 0)
                 break;
-            vTaskDelay(pdMS_TO_TICKS(lPar->tOff));
+            delay(lPar->tOff);
         }
         LOG_DEBUG("Blinker Task Ended");
         lPar->blinkTask = NULL;

lib/GPIO/LED_Driver.h

@@ -10,7 +10,7 @@ namespace drivers
 
     class Led
     {
-        const uint8_t ledPin = 38;
+        const uint8_t c_ledPin = 38;
 
     public:
         typedef struct

lib/GPIO/TCA9554PWR_Driver.cpp

@@ -53,6 +53,12 @@ namespace drivers
     return setPort(newState);
   }
 
+  const bool TCA9554PWR::toggleOut(const uint8_t channel)
+  {
+    bool value;
+    return readOut(channel, value) && setOut(channel, value);
+  }
+
   const bool TCA9554PWR::setPort(const uint8_t state)
   {
     if (writeRegister(TCA9554_OUTPUT_REG, state))
@@ -61,7 +67,7 @@ namespace drivers
     return false;
   }
 
-  const bool TCA9554PWR::readOut(const uint8_t ch)
+  const bool TCA9554PWR::readOut(const uint8_t ch, bool &state)
   {
     uint8_t currState(0);
     if (ch < DO1 || ch > DO8)
@@ -71,12 +77,13 @@ namespace drivers
     }
     if (!readPort(currState))
       return false;
-    return (currState && (High >> ch));
+    state = (currState && (High << ch));
+    return true;
   }
 
   const bool TCA9554PWR::readPort(uint8_t &state)
   {
-    if (readRegister(TCA9554_INPUT_REG, state))
+    if (readRegister(TCA9554_OUTPUT_REG, state))
       return true;
     LOG_ERROR("Unable to read IO port: state[%02x]", state);
     return false;

lib/GPIO/TCA9554PWR_Driver.h

@@ -42,9 +42,10 @@ namespace drivers
         ~TCA9554PWR();
 
         const bool setOut(const uint8_t channel, const bool state);
+        const bool toggleOut(const uint8_t channel);
         const bool setPort(const uint8_t state);
 
-        const bool readOut(const uint8_t channel);
+        const bool readOut(const uint8_t channel, bool &state);
         const bool readPort(uint8_t &state);
 
     private:

lib/RS485/RS485_Driver.cpp

@@ -65,12 +65,40 @@ namespace drivers
     readAll(garbage);
     LOG_INFO("Init MODBUS Master Mode");
     m_crc.reset(CRC16_MODBUS_POLYNOME, CRC16_MODBUS_INITIAL, CRC16_MODBUS_XOR_OUT, CRC16_MODBUS_REV_IN, CRC16_MAXIM_REV_OUT);
+    m_lastAccess = millis();
+    m_lastDevice = 0;
+  }
+
+  // Get transaction lock
+  std::unique_lock<std::mutex> MODBUS::getLock()
+  {
+    return std::unique_lock<std::mutex>(m_mutex);
+  }
+
+  std::mutex &MODBUS::getMutex()
+  {
+    return m_mutex;
+  }
+
+  void MODBUS::delayAccess(const uint8_t device)
+  {
+    if (device == m_lastDevice)
+      return;
+    auto now = millis();
+    if ((now - m_lastAccess) < c_minDelay) // fixed milliseconds delay between commands to different devices
+    {                                      
+      LOG_WARN("MODBUS access delay", (now - m_lastAccess), "device", device);
+      delay(now - m_lastAccess);
+    }
+    m_lastDevice = device;
+    m_lastAccess = millis();
   }
 
   // Func 0x01
   const bool MODBUS::readCoils(const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<bool> &coils)
   {
     constexpr uint8_t func = 0x01;
+    delayAccess(device);
     LOG_DEBUG("Read coils: dev[", device, "], reg[", reg, "], num[", num, "]");
     return readBinary(device, func, reg, num, coils);
   }
@@ -79,6 +107,7 @@ namespace drivers
   const bool MODBUS::readInputs(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<bool> &inputs)
   {
     constexpr uint8_t func = 0x02;
+    delayAccess(device);
     LOG_DEBUG("Read multi inputs: dev[", device, "], reg[", reg, "], num[", num, "]");
     return readBinary(device, func, reg, num, inputs);
   }
@@ -87,6 +116,7 @@ namespace drivers
   const bool MODBUS::readHoldingRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
   {
     constexpr uint8_t func = 0x03;
+    delayAccess(device);
     LOG_DEBUG("Read multi holding registers: dev[", device, "], reg[", reg, "], num[", num, "]");
     return readInteger(device, func, reg, num, values);
   }
@@ -95,6 +125,7 @@ namespace drivers
   const bool MODBUS::readInputRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
   {
     constexpr uint8_t func = 0x04;
+    delayAccess(device);
     LOG_DEBUG("Read multi input registers: dev[", device, "], reg[", reg, "], num[", num, "]");
     return readInteger(device, func, reg, num, values);
   }
@@ -103,6 +134,7 @@ namespace drivers
   const bool MODBUS::writeCoil(const uint8_t device, const uint16_t coil, const bool value)
   {
     constexpr uint8_t func = 0x05;
+    delayAccess(device);
     LOG_DEBUG("Write single coil: dev[", device, "], coil[", coil, "], value[", value ? "true" : "false", "]");
     return writeBinary(device, func, coil, {value});
   }
@@ -111,6 +143,7 @@ namespace drivers
   const bool MODBUS::writeRegister(const uint8_t device, const uint16_t reg, const uint16_t value)
   {
     constexpr uint8_t func = 0x06;
+    delayAccess(device);
     LOG_DEBUG("Write single register: dev[", device, "], reg[", reg, "], value[", value, "]");
     return writeInteger(device, func, reg, {value}, false);
   }
@@ -119,6 +152,7 @@ namespace drivers
   const bool MODBUS::writeCoils(const uint8_t device, const uint16_t coils, const std::vector<bool> &values)
   {
     constexpr uint8_t func = 0x0F;
+    delayAccess(device);
     LOG_DEBUG("Write multi coils: dev[", device, "], start[", coils, "], num[", values.size(), "]");
     return writeBinary(device, func, coils, values);
   }
@@ -127,6 +161,7 @@ namespace drivers
   const bool MODBUS::writeRegisters(const uint8_t device, const uint16_t reg, const std::vector<uint16_t> &values)
   {
     constexpr uint8_t func = 0x10;
+    delayAccess(device);
     LOG_DEBUG("Write multi registers: dev[", device, "], start[", reg, "], num[", values.size(), "]");
     return writeInteger(device, func, reg, values, true);
   }
@@ -143,7 +178,7 @@ namespace drivers
       return false;
     }
     const uint16_t nRespDataBytes = (uint16_t)ceil(bits / 8.0f);                          // 1 bit for every coil, if not 8 mutiple padded with zeroes
-    const uint16_t expectedRespLen = (RESP_HEADER_SIZE + RESP_CRC_SIZE) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
+    const uint16_t expectedRespLen = (c_respHeaderSize + c_respCrcSize) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
     std::vector<uint8_t> response;
     if (!readN(expectedRespLen, response))
     {
@@ -172,7 +207,7 @@ namespace drivers
     uint16_t bitNum(0);
 
     // get response data bytes excluding header and crc
-    const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - sizeof(crc_t));
+    const std::vector<uint8_t> respData(response.begin() + c_respHeaderSize, response.end() - sizeof(crc_t));
     for (auto it = respData.begin(); it < respData.end(); it++)
     {
       for (uint8_t j(0); j < 8 && bitNum < bits; j++)
@@ -193,7 +228,7 @@ namespace drivers
       return false;
     }
     const uint16_t nRespDataBytes = num * sizeof(uint16_t);
-    const uint16_t expectedRespLen = (RESP_HEADER_SIZE + sizeof(crc_t)) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
+    const uint16_t expectedRespLen = (c_respHeaderSize + sizeof(crc_t)) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
     std::vector<uint8_t> response;
     if (!readN(expectedRespLen, response))
     {
@@ -220,7 +255,7 @@ namespace drivers
     out.clear();
     out.reserve(nRespDataBytes / sizeof(uint16_t));
     // get response data bytes excluding header and crc
-    const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
+    const std::vector<uint8_t> respData(response.begin() + c_respHeaderSize, response.end() - c_respCrcSize);
     for (auto it = respData.begin(); it < respData.end(); it++)
     {
       const uint8_t lo(*it++);

lib/RS485/RS485_Driver.h

@@ -13,7 +13,7 @@ namespace drivers
 {
     class RS485
     {
-        static const uint8_t PORT = 1;
+        const uint8_t c_port = 1;
 
     public:
         RS485(const uint32_t baud, const SerialConfig conf);
@@ -32,8 +32,9 @@ namespace drivers
     class MODBUS : private RS485
     {
 
-        static const uint8_t RESP_HEADER_SIZE = 3;
+        const uint8_t c_respHeaderSize = 3;
-        static const uint8_t RESP_CRC_SIZE = 2;
+        const uint8_t c_respCrcSize = 2;
+        const uint32_t c_minDelay = 500;
 
         typedef struct
         {
@@ -61,9 +62,8 @@ namespace drivers
         MODBUS &operator=(const MODBUS &) = delete;
 
         // Get transaction lock
-        std::unique_lock<std::mutex> getLock() {
+        std::unique_lock<std::mutex> getLock();
-            return std::unique_lock<std::mutex>(m_mutex);
+        std::mutex &getMutex();
-        }
 
         // Func 0x01
         const bool readCoils(const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<bool> &coils);
@@ -92,6 +92,9 @@ namespace drivers
     private:
         CRC16 m_crc;
         std::mutex m_mutex;
+        uint8_t m_lastDevice;
+        uint32_t m_lastAccess;
+        void delayAccess(const uint8_t device);
         const std::vector<uint8_t> singleRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t data);
         const std::vector<uint8_t> multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint8_t> &data);
         const bool readBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, std::vector<bool> &out);

lib/RTC/PCF85063_Driver.h

@@ -63,38 +63,6 @@
 
 #define RTC_TIMER_FLAG (0x08)
 
-typedef struct
-{
-  uint16_t year;
-  uint8_t month;
-  uint8_t day;
-  uint8_t dotw;
-  uint8_t hour;
-  uint8_t minute;
-  uint8_t second;
-} datetime_t;
-
-const unsigned char MonthStr[12][4] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec"};
-const unsigned char Week[7][4] = {"Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"};
-
-extern datetime_t datetime;
-void PCF85063_Init(void);
-void PCF85063_Reset(void);
-void PCF85063Task(void *parameter);
-
-void PCF85063_Set_Time(datetime_t time);
-void PCF85063_Set_Date(datetime_t date);
-void PCF85063_Set_All(datetime_t time);
-
-void PCF85063_Read_Time(datetime_t *time);
-
-void PCF85063_Enable_Alarm(void);
-uint8_t PCF85063_Get_Alarm_Flag(void);
-void PCF85063_Set_Alarm(datetime_t time);
-void PCF85063_Read_Alarm(datetime_t *time);
-
-void datetime_to_str(char *datetime_str, datetime_t time);
-
 namespace drivers
 {
 

lib/SENECA/S50140_Driver.cpp

@@ -13,6 +13,7 @@ namespace drivers
     const S50140::powerinfo_t S50140::getAll()
     {
         powerinfo_t info{MAXFLOAT};
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
         info.v = getV();
         info.a = getA();
         info.pAct = getPact();
@@ -65,18 +66,19 @@ namespace drivers
     void S50140::delayRequest()
     {
         auto now = millis();
-        if ((now - m_lastRequest) < minDelay)
+        if ((now - m_lastRequest) < c_minDelay)
         { // minimum m_lastRequest between requests
-            vTaskDelay(pdMS_TO_TICKS(now - m_lastRequest));
+            LOG_DEBUG("S50140 delay request", (now-m_lastRequest));
+            delay(now - m_lastRequest);
         }
-        m_lastRequest = now;
+        m_lastRequest = millis();
     }
 
     const uint8_t S50140::getRegset()
     {
         std::vector<uint16_t> value;
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
         delayRequest();
-        auto lock = m_bus.getLock();
         m_bus.readHoldingRegisters(m_address, REG_Regset, 2, value);
         if (value.empty())
             return UINT8_MAX;
@@ -86,8 +88,8 @@ namespace drivers
     const uint16_t S50140::getCounterStatus()
     {
         std::vector<uint16_t> value;
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
         delayRequest();
-        auto lock = m_bus.getLock();
         m_bus.readHoldingRegisters(m_address, REG_PartCount, 2, value);
         if (value.empty())
             return UINT16_MAX;
@@ -101,11 +103,11 @@ namespace drivers
         constexpr uint16_t resetAll = 0x0A03;
         constexpr uint16_t stopAll = 0x0A02;
         constexpr uint16_t startAll = 0x0A01;
-        while (retries++ < maxRetries)
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
+        while (retries++ < c_maxRetries)
         {
             bool ok(true);
             delayRequest();
-            auto lock = m_bus.getLock();
             LOG_WARN("Powermeter Counter STOP");
             ok &= m_bus.writeRegisters(m_address, REG_PartCount, {nullVal, stopAll});
             delayRequest();
@@ -126,11 +128,10 @@ namespace drivers
         uint8_t retries(0);
         std::vector<uint16_t> values;
 
-        while (retries++ < maxRetries)
+        while (retries++ < c_maxRetries)
         {
             delayRequest();
-            auto lock = m_bus.getLock();
+            if (m_bus.readHoldingRegisters(m_address, reg, c_dataWords, values) && values.size() == c_dataWords)
-            if (m_bus.readHoldingRegisters(m_address, reg, dataWords, values) && values.size() == dataWords)
             {
                 floatval_t fv;           // potrebbe essere il contrario, vedremo
                 fv.words.lo = values[0]; // magari va invertita ancora l'endianness

lib/SENECA/S50140_Driver.h

@@ -11,9 +11,9 @@ namespace drivers
     class S50140
     {
     private:
-        const uint8_t maxRetries = 5;
+        const uint8_t c_maxRetries = 5;
-        const uint8_t dataWords = 2;
+        const uint8_t c_dataWords = 2;
-        const uint16_t minDelay = 500;
+        const uint32_t c_minDelay = 500;
 
         const uint16_t REG_V = 0x100C;
         const uint16_t REG_A = 0x1016;
@@ -80,6 +80,6 @@ namespace drivers
     private:
         const uint8_t m_address;
         drivers::MODBUS &m_bus;
-        uint64_t m_lastRequest;
+        uint32_t m_lastRequest;
     };
 }

lib/TEMP/R4DCB08_Driver.cpp

@@ -5,12 +5,24 @@ namespace drivers
     R4DCB08::R4DCB08(drivers::MODBUS &bus, const uint8_t address) : m_address(address), m_bus(bus), m_sensors(0)
     {
         m_sensors = getNum();
+        m_lastRequest = millis();
     }
 
     R4DCB08::~R4DCB08()
     {
     }
 
+    void R4DCB08::delayRequest()
+    {
+        auto now = millis();
+        if ((now - m_lastRequest) < c_minDelay)
+        { // minimum m_lastRequest between requests
+            LOG_DEBUG("R4DCB08 delay request", (now-m_lastRequest));
+            delay(now - m_lastRequest);
+        }
+        m_lastRequest = millis();
+    }
+
     const float R4DCB08::getTemp(const uint8_t ch)
     {
         uint8_t retries(0);
@@ -20,16 +32,16 @@ namespace drivers
             LOG_ERROR("Invalid Temperature Channel number", ch);
             return MAXFLOAT;
         }
-        while (retries++ < maxRetries)
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
+        while (retries++ < c_maxRetries)
         {
-            auto lock = m_bus.getLock();
+            delayRequest();
             if (m_bus.readHoldingRegisters(m_address, REG_TEMP + ch, 1, rawT) && !rawT.empty())
             {
                 return rawT.front() / 10.0f;
             }
             LOG_ERROR("Failed to Read Temperature, device", m_address, "channel", ch);
             rawT.clear();
-            delay(50);
         }
         return MAXFLOAT;
     }
@@ -39,9 +51,10 @@ namespace drivers
         uint8_t retries(0);
         std::vector<uint16_t> rawT;
         std::vector<float> out;
-        while (retries++ < maxRetries)
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
+        while (retries++ < c_maxRetries)
         {
-            auto lock = m_bus.getLock();
+            delayRequest();
             if (m_bus.readHoldingRegisters(m_address, REG_TEMP, getNum(), rawT) && !rawT.empty())
             {
                 out.reserve(rawT.size());
@@ -53,7 +66,6 @@ namespace drivers
             }
             LOG_ERROR("Failed to Read All Temperature, device", m_address);
             rawT.clear();
-            delay(50);
         }
         out.clear();
         return out;
@@ -64,20 +76,18 @@ namespace drivers
         uint8_t retries(0);
         uint8_t channel(0);
         corr.resize(getNum()); // max number of temperature correction values is equal to number of sensors
-
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
         for (auto v : corr)
-        { // convert to decimal degreees to register value
-            while (retries++ < maxRetries)
         {
-                auto lock = m_bus.getLock();
+            while (retries++ < c_maxRetries)
-                if (m_bus.writeRegister(m_address, REG_TEMPCORR + channel, v*10))
+            {
+                delayRequest();
+                if (m_bus.writeRegister(m_address, REG_TEMPCORR + channel, v * 10)) // convert to decimal degreees to register value
                 {
                     channel++;
-                    delay(50);
                     break;
                 }
                 LOG_ERROR("Failed to Set Temperature Correction, device", m_address);
-                delay(50);
             }
         }
     }
@@ -89,9 +99,10 @@ namespace drivers
         std::vector<float> out;
         rawV.reserve(getNum());
 
-        while (retries++ < maxRetries)
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
+        while (retries++ < c_maxRetries)
         {
-            auto lock = m_bus.getLock();
+            delayRequest();
             if (m_bus.readHoldingRegisters(m_address, REG_TEMPCORR, getNum(), rawV))
             {
                 out.reserve(rawV.size());
@@ -103,7 +114,6 @@ namespace drivers
             }
             LOG_ERROR("Failed to Get Temperature Correction, device", m_address);
             rawV.clear();
-            delay(50);
         }
         out.clear();
         return out;
@@ -116,9 +126,10 @@ namespace drivers
         uint8_t retries(0);
         uint8_t sensors(0);
         std::vector<uint16_t> rawT;
-        while (retries++ < maxRetries)
+        std::lock_guard<std::mutex> lock(m_bus.getMutex());
+        while (retries++ < c_maxRetries)
         {
-            auto lock = m_bus.getLock();
+            delayRequest();
             if (m_bus.readHoldingRegisters(m_address, REG_TEMP, T_MAX, rawT))
             {
                 for (auto v : rawT)
@@ -130,7 +141,6 @@ namespace drivers
                 return m_sensors;
             }
             LOG_ERROR("Failed to Get Sensor Number, device", m_address);
-            delay(50);
         }
         LOG_ERROR("No Temperature Sensors Detected, device", m_address);
         return 0;

lib/TEMP/R4DCB08_Driver.h

@@ -25,7 +25,9 @@ namespace drivers
             T_MAX
         };
 
-        const uint8_t maxRetries = 5;
+    private:
+        const uint8_t c_maxRetries = 5;
+        const uint32_t c_minDelay = 500;
         const uint16_t REG_TEMP = 0x0000;
         const uint16_t REG_TEMPCORR = 0x0008;
 
@@ -41,9 +43,13 @@ namespace drivers
 
         const uint8_t getNum();
 
+    private:
+        void delayRequest();
+
     private:
         const uint8_t m_address;
         uint8_t m_sensors;
         MODBUS &m_bus;
+        uint32_t m_lastRequest;
     };
 }

lib/utils/utils.cpp

@@ -1,5 +1,6 @@
 #include "utils.h"
 
+
 void printBytes(const char title[], const std::vector<uint8_t> &b)
 {
   Serial0.flush();
@@ -35,3 +36,15 @@ void printBool(const char title[], const std::vector<bool> &vals)
   printf("\n");
   Serial0.flush();
 }
+
+const std::string printBoolVec(const std::vector<bool> &vals)
+{
+  std::string buf;
+  buf.reserve(vals.size()+1);
+  buf.append("b");
+  for (const auto v : vals)
+  {
+    buf.append(v ? "1" : "0");
+  }
+  return buf;
+}

lib/utils/utils.h

@@ -4,6 +4,7 @@
 
 #include <Arduino.h>
 #include <DebugLog.h>
+#include <string>
 #include <vector>
 
 ///////////// UTIL Functions /////////////////
@@ -13,3 +14,5 @@ void printBytes(const char title[], const std::vector<uint8_t> &b);
 void printBytes(const char title[], const std::vector<uint16_t> &b);
 
 void printBool(const char title[], const std::vector<bool> &vals);
+
+const std::string printBoolVec(const std::vector<bool> &vals);

src/digitalIO.cpp

@@ -1,4 +1,5 @@
 #include <digitalIO.h>
+#include <utils.h>
 
 digitalIO::digitalIO(drivers::I2C &i2c, drivers::MODBUS &bus, std::vector<uint8_t> remotes) : m_localOuts(drivers::TCA9554PWR(i2c, TCA9554_ADDRESS)), m_remoteAddrs(remotes)
 {
@@ -9,31 +10,76 @@ digitalIO::digitalIO(drivers::I2C &i2c, drivers::MODBUS &bus, std::vector<uint8_
 
     for (auto a : remotes)
     {
-        m_remotes.emplace_back(remoteIO(a, bus));
+        m_remotes.emplace_back(a, bus);
     }
+    LOG_INFO("Initialized digitalIO -> inputs", getInNum(), "outputs", getOutNum());
 }
 
 digitalIO::~digitalIO()
 {
 }
 
-void digitalIO::digitalIOWrite(const uint8_t ch, const bool value)
+void digitalIO::digitalOutWrite(const uint8_t ch, const bool value)
 {
     if (ch < 0 || ch > getOutNum())
     {
-        LOG_ERROR("Invalid digitalIOWrite channel number", ch);
+        LOG_ERROR("Invalid digitalOutWrite channel number", ch);
+        return;
     }
 
     if (ch < drivers::TCA9554PWR::DO_MAX) // write to i2c device for local outputs
     {
-        digitalWriteLocal(ch, value);
+        writeLocal(ch, value);
     }
     else
     {
-        digitalWriteRemote(ch - drivers::TCA9554PWR::DO_MAX, value);
+        writeRemote(ch - drivers::TCA9554PWR::DO_MAX, value);
     }
 }
-const bool digitalIO::digitalIORead(const uint8_t ch)
+
+void digitalIO::digitalOutWritePort(const std::vector<bool> &values)
+{
+    if (values.size() != getOutNum())
+    {
+        LOG_ERROR("Invalid digitalOutWrite channel number", values.size());
+        return;
+    }
+    const std::vector<bool> locals(values.begin(), values.begin() + drivers::TCA9554PWR::DO_MAX);
+    const std::vector<bool> remotes(values.begin() + drivers::TCA9554PWR::DO_MAX, values.end());
+    writeLocalPort(locals);
+    writeRemotePort(remotes);
+}
+
+const bool digitalIO::digitalOutRead(const uint8_t ch)
+{
+    if (ch < 0 || ch > getOutNum())
+    {
+        LOG_ERROR("Invalid digitalOutRead channel number", ch);
+        return false;
+    }
+
+    if (ch < drivers::TCA9554PWR::DO_MAX) // write to i2c device for local outputs
+    {
+        return readLocalIn(ch);
+    }
+    else
+    {
+        return readRemoteIn(ch - drivers::TCA9554PWR::DO_MAX);
+    }
+}
+
+const std::vector<bool> digitalIO::digitalOutReadPort()
+{
+    const std::vector<bool> locals(readLocalOutPort());
+    const std::vector<bool> remotes(readRemoteOutPort());
+    std::vector<bool> rv;
+    rv.reserve(getOutNum());
+    rv.insert(rv.begin(), locals.begin(), locals.end());
+    rv.insert(rv.end(), remotes.begin(), remotes.end());
+    return std::move(rv);
+}
+
+const bool digitalIO::digitalInRead(const uint8_t ch)
 {
     if (ch < 0 || ch > getInNum())
     {
@@ -42,14 +88,25 @@ const bool digitalIO::digitalIORead(const uint8_t ch)
 
     if (ch < (DI_MAX - DI1)) // read from local inputs not as gpio numbers
     {
-        return digitalReadLocal(ch);
+        return readLocalIn(ch);
     }
     else
     {
-        return digitalReadRemote(ch - (DI_MAX - DI1));
+        return readRemoteIn(ch - (DI_MAX - DI1));
     }
 }
 
+const std::vector<bool> digitalIO::digitalInReadPort()
+{
+    const std::vector<bool> locals(readLocalInPort());
+    const std::vector<bool> remotes(readRemoteInPort());
+    std::vector<bool> rv;
+    rv.reserve(getInNum());
+    rv.insert(rv.begin(), locals.begin(), locals.end());
+    rv.insert(rv.end(), remotes.begin(), remotes.end());
+    return std::move(rv);
+}
+
 void digitalIO::reset()
 {
     // set all local and remote outputs to 0
@@ -58,67 +115,254 @@ void digitalIO::reset()
         r.resetAll(false);
 }
 
+const uint8_t digitalIO::getLocalInNum()
+{
+    return (DI_MAX - DI1);
+}
+const uint8_t digitalIO::getLocalOutNum()
+{
+    return drivers::TCA9554PWR::DO_MAX;
+}
+const uint8_t digitalIO::getRemoteInNum()
+{
+    return m_remotes.size() * remoteIO::CH_MAX;
+}
+const uint8_t digitalIO::getRemoteOutNum()
+{
+
+    return m_remotes.size() * remoteIO::CH_MAX;
+}
+
 const uint8_t digitalIO::getOutNum()
 {
-    return drivers::TCA9554PWR::DO_MAX + m_remotes.size() * remoteIO::CH_MAX;
+    return getLocalOutNum() + getRemoteOutNum();
 }
 
 const uint8_t digitalIO::getInNum()
 {
-    return DI_MAX + m_remotes.size() * remoteIO::CH_MAX;
+    return getLocalInNum() + getRemoteInNum();
 }
 
-void digitalIO::digitalWriteLocal(const uint8_t ch, const bool value)
+void digitalIO::writeLocal(const uint8_t ch, const bool value)
 {
     uint8_t retries(0);
-    while (retries++ < maxRetries)
+    while (retries++ < c_maxRetries)
     {
         if (m_localOuts.setOut(ch, value))
         {
-            LOG_DEBUG("digitalWriteLocal channel", ch, " status", value ? "True" : "False");
+            LOG_DEBUG("writeLocal channel", ch, " status", value ? "True" : "False");
-            break;
+            return;
         }
-        LOG_ERROR("Failed digitalWriteLocal channel ", ch, " status", value ? "True" : "False");
+        LOG_ERROR("Failed writeLocal channel ", ch, " status", value ? "True" : "False");
     }
 }
 
-void digitalIO::digitalWriteRemote(const uint8_t ch, const bool value)
+void digitalIO::writeLocalPort(const std::vector<bool> &values)
 {
     uint8_t retries(0);
-    const uint8_t selectedRemote(floor(ch / 8.0f));
+    uint8_t decValue(0);
+    for (uint8_t i(0); i < 8; i++) // convert from bits to byte value
+    {
+        if (values[i])
+            decValue |= High << i;
+    }
+    while (retries++ < c_maxRetries)
+    {
+        if (m_localOuts.setPort(decValue))
+        {
+            LOG_DEBUG("writeLocalPort value", printBoolVec(values).c_str());
+            return;
+        }
+        LOG_ERROR("Failed writeLocalPort value", printBoolVec(values).c_str());
+    }
+}
+
+void digitalIO::writeRemote(const uint8_t ch, const bool value)
+{
+    uint8_t retries(0);
+    const uint8_t selectedRemote(floor(ch / (float)remoteIO::CH_MAX));
     const uint8_t selectedChannel(ch % remoteIO::CH_MAX);
-    while (retries++ < maxRetries)
+    while (retries++ < c_maxRetries)
     {
         if (m_remotes[selectedRemote].setOut((remoteIO::channel_t)selectedChannel, value))
         {
-            LOG_DEBUG("digitalWriteRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
+            LOG_DEBUG("writeRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
-            break;
+            return;
         }
-        LOG_ERROR("Failed digitalWriteRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
+        LOG_ERROR("Failed writeRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
     }
 }
 
-const bool digitalIO::digitalReadLocal(const uint8_t ch)
+void digitalIO::writeRemotePort(const std::vector<bool> &values)
+{
+    uint8_t retries(0);
+    while (retries++ < c_maxRetries)
+    {
+        bool ok(true);
+        for (uint8_t i(0); i < values.size(); i += remoteIO::CH_MAX)
+        {
+            const uint8_t selectedRemote(floor(i / (float)remoteIO::CH_MAX));
+            const std::vector<bool> currValues(values.begin() + i, values.begin() + i + remoteIO::CH_MAX);
+            ok &= m_remotes[selectedRemote].setOutPort(currValues);
+            if (ok)
+            {
+                LOG_DEBUG("writeRemotePort remote", selectedRemote, "values", printBoolVec(values).c_str());
+                continue;
+            }
+            LOG_ERROR("Failed writeRemotePort remote", selectedRemote, "values", printBoolVec(values).c_str());
+            break;
+        }
+        if (ok)
+            break;
+    }
+}
+
+const bool digitalIO::readLocalIn(const uint8_t ch)
 {
     bool value = !digitalRead(ch + DI1); // base pin number in enum, inverted input
-    LOG_DEBUG("digitalReadLocal pin", (ch + DI1), " status", value ? "True" : "False");
+    LOG_DEBUG("readLocalIn pin", (ch + DI1), " status", value ? "True" : "False");
     return value;
 }
 
-const bool digitalIO::digitalReadRemote(const uint8_t ch)
+const bool digitalIO::readLocalOut(const uint8_t ch)
+{
+    bool value(false);
+    uint8_t retries(0);
+    while (retries++ < c_maxRetries)
+    {
+        if (m_localOuts.readOut(ch, value))
+        {
+            LOG_DEBUG("readLocalOut pin", (ch), " status", value ? "True" : "False");
+            return value;
+        }
+        LOG_ERROR("Failed readLocalOut channel", ch);
+    }
+    return false;
+}
+
+const std::vector<bool> digitalIO::readLocalInPort()
+{
+    std::vector<bool> values(getLocalInNum());
+    for (uint8_t i(0); i < values.size(); i++)
+    {
+        values[i] = readLocalIn(i);
+    }
+    LOG_DEBUG("readLocalInPort values", printBoolVec(values).c_str());
+    return values;
+}
+
+const std::vector<bool> digitalIO::readLocalOutPort()
+{
+    uint8_t retries(0);
+    uint8_t state(0);
+    std::vector<bool> values(getLocalOutNum());
+    while (retries++ < c_maxRetries)
+    {
+        if (m_localOuts.readPort(state))
+        {
+            for (uint8_t i(0); i < values.size(); i++)
+            {
+                values[i] = (state >> i) & High;
+            }
+            LOG_DEBUG("readLocalOutPort values", printBoolVec(values).c_str());
+            return values;
+        }
+        LOG_ERROR("Failed readLocalOutPort");
+    }
+    values.clear();
+    return values;
+}
+
+const bool digitalIO::readRemoteIn(const uint8_t ch)
 {
     uint8_t retries(0);
     const uint8_t selectedRemote(floor(ch / 8.0f));
     const uint8_t selectedChannel(ch % remoteIO::CH_MAX);
     bool value;
-    while (retries++ < maxRetries)
+    while (retries++ < c_maxRetries)
     {
         if (m_remotes[selectedRemote].getIn((remoteIO::channel_t)selectedChannel, value))
         {
-            LOG_DEBUG("digitalReadRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
+            LOG_DEBUG("readRemoteIn remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
             return value;
         }
-        LOG_ERROR("Failed digitalReadRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
+        LOG_ERROR("Failed readRemoteIn remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
     }
     return false;
 }
+
+const bool digitalIO::readRemoteOut(const uint8_t ch)
+{
+    uint8_t retries(0);
+    const uint8_t selectedRemote(floor(ch / (float)remoteIO::CH_MAX));
+    const uint8_t selectedChannel(ch % remoteIO::CH_MAX);
+    bool value;
+    while (retries++ < c_maxRetries)
+    {
+        if (m_remotes[selectedRemote].getOut((remoteIO::channel_t)selectedChannel, value))
+        {
+            LOG_DEBUG("readRemoteOut remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
+            return value;
+        }
+        LOG_ERROR("Failed readRemoteOut remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
+    }
+    return false;
+}
+
+const std::vector<bool> digitalIO::readRemoteInPort()
+{
+    uint8_t retries(0);
+    std::vector<bool> values;
+    values.reserve(getRemoteInNum());
+    while (retries++ < c_maxRetries)
+    {
+        bool ok(true);
+        for (uint8_t i(0); i < getRemoteInNum(); i += remoteIO::CH_MAX)
+        {
+            const uint8_t selectedRemote(floor(i / (float)remoteIO::CH_MAX));
+            std::vector<bool> remVals(remoteIO::CH_MAX);
+            ok &= m_remotes[selectedRemote].getInPort(remVals);
+            if (ok)
+            {
+                values.insert(values.begin() + values.size(), remVals.begin(), remVals.end());
+                LOG_DEBUG("readRemoteInPort remote", selectedRemote, "values", printBoolVec(remVals).c_str());
+                continue;
+            }
+            LOG_ERROR("Failed readRemoteInPort remote", selectedRemote);
+            break;
+        }
+        if (ok)
+            return values;
+    }
+    values.clear();
+    return values;
+}
+
+const std::vector<bool> digitalIO::readRemoteOutPort()
+{
+    uint8_t retries(0);
+    std::vector<bool> values;
+    values.reserve(getRemoteOutNum());
+    while (retries++ < c_maxRetries)
+    {
+        bool ok(true);
+        for (uint8_t i(0); i < getRemoteOutNum(); i += remoteIO::CH_MAX)
+        {
+            const uint8_t selectedRemote(floor(i / (float)remoteIO::CH_MAX));
+            std::vector<bool> remVals(remoteIO::CH_MAX);
+            ok &= m_remotes[selectedRemote].getOutPort(remVals);
+            if (ok)
+            {
+                values.insert(values.begin() + values.size(), remVals.begin(), remVals.end());
+                LOG_DEBUG("readRemoteOutPort remote", selectedRemote, "values", printBoolVec(remVals).c_str());
+                continue;
+            }
+            LOG_ERROR("Failed readRemoteOutPort remote", selectedRemote);
+            break;
+        }
+        if (ok)
+            return values;
+    }
+    values.clear();
+    return values;
+}

src/digitalIO.h

@@ -28,25 +28,44 @@ private:
         DI_MAX
     };
 
-    const uint8_t maxRetries = 5;
+    const uint8_t c_maxRetries = 5;
 
 public:
     digitalIO(drivers::I2C &i2c, drivers::MODBUS &bus, std::vector<uint8_t> remotes);
     ~digitalIO();
 
-    void digitalIOWrite(const uint8_t ch, const bool value);
+    void digitalOutWrite(const uint8_t ch, const bool value);
-    const bool digitalIORead(const uint8_t ch);
+    void digitalOutWritePort(const std::vector<bool> &values);
+    const bool digitalOutRead(const uint8_t ch);
+    const std::vector<bool> digitalOutReadPort();
+
+    const bool digitalInRead(const uint8_t ch);
+    const std::vector<bool> digitalInReadPort();
+
     void reset();
 
     const uint8_t getOutNum();
     const uint8_t getInNum();
 
     private:
-    void digitalWriteLocal(const uint8_t ch, const bool value);
+    const uint8_t getLocalInNum();
-    void digitalWriteRemote(const uint8_t ch, const bool value);
+    const uint8_t getLocalOutNum();
+    const uint8_t getRemoteInNum();
+    const uint8_t getRemoteOutNum();
 
-    const bool digitalReadLocal(const uint8_t ch);
+    void writeLocal(const uint8_t ch, const bool value);
-    const bool digitalReadRemote(const uint8_t ch);
+    void writeLocalPort(const std::vector<bool> &values);
+    void writeRemote(const uint8_t ch, const bool value);
+    void writeRemotePort(const std::vector<bool> &values);
+
+    const bool readLocalIn(const uint8_t ch);
+    const bool readLocalOut(const uint8_t ch);
+    const std::vector<bool> readLocalInPort();
+    const std::vector<bool> readLocalOutPort();
+    const bool readRemoteIn(const uint8_t ch);
+    const bool readRemoteOut(const uint8_t ch);
+    const std::vector<bool> readRemoteInPort();
+    const std::vector<bool> readRemoteOutPort();
 
 private:
     std::vector<uint8_t> m_remoteAddrs;

src/main.cpp

@@ -1,10 +1,9 @@
 #define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG
 
+#include <Arduino.h>
 #include <DebugLog.h>
 #include <DebugLogEnable.h>
 
-#include <Arduino.h>
-
 #include <config.h>
 #include <PCF85063_Driver.h>
 #include <R4DCB08_Driver.h>
@@ -14,8 +13,6 @@
 #include <ETH_Driver.h>
 
 #include <digitalIO.h>
-
-#define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG
 #include <mqtt.h>
 #include "utils.h"
 
@@ -50,27 +47,42 @@ void loop()
   auto rtc = drivers::PCF85063(i2c, PCF85063_ADDRESS);
   auto eth = drivers::Ethernet(conf.m_ethHostname);
   auto tmp = drivers::R4DCB08(bus, conf.m_modbusTemperatureAddr);
-  delay(100);
-  auto io = digitalIO(i2c, bus, {conf.m_modbusRelayAddr});
-  delay(100);
   auto seneca = drivers::S50140(bus, conf.m_modbusSenecaAddr);
   auto buzzer = drivers::Buzzer();
   auto led = drivers::Led();
-  //////////////// DEVICES ////////////////
+  delay(500);
+  auto io = digitalIO(i2c, bus, {conf.m_modbusRelayAddr});
   // Initialize temperature sensors
   sensors = tmp.getNum();
   LOG_INFO("Temperature sensors connected ->", sensors);
+  //////////////// DEVICES ////////////////
 
   //////////////// NETWORK ////////////////
   auto mqtt = MQTTwrapper();
   //////////////// NETWORK ////////////////
 
   std::function<void(const ArduinoJson::JsonDocument &)> mycallback =
-    [&io](const ArduinoJson::JsonDocument &doc) {
+      [&io](const ArduinoJson::JsonDocument &doc)
-        io.digitalIOWrite(0, doc["stat"].as<bool>());
+  {
-        io.digitalIOWrite(15, doc["stat"].as<bool>());
+    std::vector<bool> v1 = {1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
-    };
+    std::vector<bool> v2 = {0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1};
+    std::vector<bool> v0(io.getOutNum(), 0);
 
+    LOG_INFO("SET Digital Outputs V1: ", printBoolVec(v1).c_str());
+    io.digitalOutWritePort(v1);
+    delay(100);
+    LOG_INFO("GET Digital Outputs V1: ", printBoolVec(io.digitalOutReadPort()).c_str());
+    delay(2000);
+
+    LOG_INFO("SET Digital Outputs V2: ", printBoolVec(v2).c_str());
+    io.digitalOutWritePort(v2);
+    delay(100);
+    LOG_INFO("GET Digital Outputs V2: ", printBoolVec(io.digitalOutReadPort()).c_str());
+    delay(2000);
+
+    LOG_INFO("GET Digital Inputs: ", printBoolVec(io.digitalInReadPort()).c_str());
+    io.digitalOutWritePort(v0);
+  };
 
   //////////////// NETWORK ////////////////
   /////////////// CALLBACK ////////////////
@@ -128,27 +140,33 @@ void loop()
     {
       LOG_INFO("Temperature channel", i++, "->", v);
     }
-    i = 0;
+    
-    delay(10);
+    LOG_INFO("Read Red");
-    for (auto v : tmp.getCorrection())
+    if (io.digitalInRead(0)) // rosso
     {
-      LOG_INFO("Temperature correction channel", i++, "tc", v);
+      std::vector<bool> v1 = {1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+      std::vector<bool> v2 = {0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1};
+      std::vector<bool> v0(16, 0);
+
+      LOG_INFO("SET Digital Outputs V1: ", printBoolVec(v1).c_str());
+      io.digitalOutWritePort(v1);
+      LOG_INFO("GET Digital Outputs V1: ", printBoolVec(io.digitalOutReadPort()).c_str());
+      delay(2000);
+
+      LOG_INFO("SET Digital Outputs V2: ", printBoolVec(v2).c_str());
+      io.digitalOutWritePort(v2);
+      LOG_INFO("GET Digital Outputs V2: ", printBoolVec(io.digitalOutReadPort()).c_str());
+      delay(2000);
+
+      LOG_INFO("GET Digital Inputs: ", printBoolVec(io.digitalInReadPort()).c_str());
+      delay(2000);
+
+      io.digitalOutWritePort(v0);
+      delay(2000);
     }
 
-    delay(100);
+    LOG_INFO("Read Blue");
-    drivers::S50140::powerinfo_t pinfo = seneca.getAll();
+    if (io.digitalInRead(8)) // blu
-    LOG_INFO("Power Info ==> V:", pinfo.v, "- A:", pinfo.a, "- W:", pinfo.pAct, "- F:", pinfo.f, "- Wh_t:", pinfo.whTot, "- Wh_p:", pinfo.whPar);
-
-    if (io.digitalIORead(0)) // rosso
-    {
-      uint8_t regset(seneca.getRegset());
-      uint16_t countStat(seneca.getCounterStatus());
-      LOG_INFO("Register Set: ", regset);
-      LOG_INFO("Counter Status: ", countStat);
-      seneca.resetPartialCounters();
-    }
-    delay(100);
-    if (io.digitalIORead(8)) // blu
     {
       if (!buzzing)
       {
@@ -165,16 +183,21 @@ void loop()
       LOG_INFO("Buzzing -> ", buzzing ? "True" : "False");
     }
     
-    if (io.digitalIORead(9))
+    LOG_INFO("Read Green");
+    if (io.digitalInRead(9))
     { // verde
       conf.resetConfig();
     }
     
-    if (io.digitalIORead(10))
+    LOG_INFO("Read Yellow");
+    if (io.digitalInRead(10))
     { // giallo
       esp_restart();
     }
 
+    drivers::S50140::powerinfo_t pinfo = seneca.getAll();
+    LOG_INFO("Power Info ==> V:", pinfo.v, "- A:", pinfo.a, "- W:", pinfo.pAct, "- F:", pinfo.f, "- Wh_t:", pinfo.whTot, "- Wh_p:", pinfo.whPar);
+
     delay(conf.m_globalLoopDelay);
   }
 

src/mqtt.cpp

@@ -1,7 +1,7 @@
 #include <mqtt.h>
 
 #define STACK_DEPTH 4096
-#define PRIOTITY 0
+#define PRIOTITY 2
 
 MQTTwrapper::MQTTwrapper() : m_config(Config::getInstance()), m_tcp(NetworkClient()), m_client(PubSubClient(m_tcp)), m_loopHandle(NULL)
 {
@@ -76,7 +76,8 @@ const bool MQTTwrapper::unsubscribe(topic_t topic)
     return false;
 }
 
-const bool MQTTwrapper::connected() {
+const bool MQTTwrapper::connected()
+{
     return m_loopHandle != NULL;
 }
 
@@ -146,12 +147,12 @@ void MQTTwrapper::clientLoop(void *params)
         while (client.connected())
         {
             client.loop();
-            vTaskDelay(pdMS_TO_TICKS(loopTime));
+            delay(loopTime);
         }
         if (client.state() != MQTT_CONNECTED)
         {
-            LOG_ERROR("MQTT disconnect reason ", stateMap.at(client.state()));
+            LOG_ERROR("MQTT disconnect reason ", stateMap.at(client.state()).c_str());
-            vTaskDelay(pdMS_TO_TICKS(loopTime * 50));
+            delay(loopTime * 50);
             const bool ok = client.connect(clientName.c_str());
             LOG_WARN("MQTT reconnected", ok ? "True" : "False");
             if (ok)
@@ -160,7 +161,8 @@ void MQTTwrapper::clientLoop(void *params)
                 {
                     const std::string &topic(v.first);
                     LOG_WARN("MQTT resubscribing to", topic.c_str());
-                    if(!wrapper->m_client.subscribe(topic.c_str())){
+                    if (!wrapper->m_client.subscribe(topic.c_str()))
+                    {
                         LOG_ERROR("Unable to resubscribe to", topic.c_str());
                     }
                 }

src/remoteIO.cpp

@@ -4,13 +4,14 @@ remoteIO::remoteIO(const uint8_t address, drivers::MODBUS &bus) : m_address(addr
 {
     LOG_INFO("Initializing relay module");
     std::vector<uint16_t> response;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
     if (!m_bus.readHoldingRegisters(m_address, REG_VERSION, 1, response))
     {
         LOG_ERROR("Unable to inizialize relay module");
     };
     LOG_INFO("Software version", std::to_string(response.at(0) / 100.0f).c_str());
     m_initialized = true;
+    m_lastRequest = millis();
     resetAll(false);
 }
 
@@ -20,11 +21,23 @@ remoteIO::~remoteIO()
     resetAll(false);
 }
 
+void remoteIO::delayRequest()
+{
+    auto now = millis();
+    if ((now - m_lastRequest) < c_minDelay)
+    { // minimum m_lastRequest between requests
+        LOG_DEBUG("remoteIO delay request", (now - m_lastRequest));
+        delay(now - m_lastRequest);
+    }
+    m_lastRequest = millis();
+}
+
 const bool remoteIO::setOut(const channel_t ch, const bool value)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     LOG_DEBUG("Write Channel", ch, "->", value ? "True" : "False");
     return m_bus.writeCoil(m_address, REG_COILS + ch, value);
 }
@@ -33,7 +46,8 @@ const bool remoteIO::toggleOut(const channel_t ch)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     std::vector<bool> value;
     if (!m_bus.readCoils(m_address, REG_COILS + ch, 1, value))
         return false;
@@ -45,16 +59,18 @@ const bool remoteIO::setOutPort(const std::vector<bool> values)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     LOG_DEBUG("Write Port", CH_MAX);
-    return m_bus.writeCoils(m_address, CH_MAX, values);
+    return m_bus.writeCoils(m_address, REG_COILS, values);
 }
 
 const bool remoteIO::getOut(const channel_t ch, bool &value)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     std::vector<bool> values;
     if (!m_bus.readCoils(m_address, REG_COILS + ch, 1, values))
         return false;
@@ -67,16 +83,18 @@ const bool remoteIO::getOutPort(std::vector<bool> &values)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     LOG_DEBUG("Read Port", CH_MAX);
-    return m_bus.readCoils(m_address, REG_COILS, 8, values);
+    return m_bus.readCoils(m_address, REG_COILS, CH_MAX, values);
 }
 
 const bool remoteIO::getIn(const channel_t input, bool &value)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     std::vector<bool> values;
     if (!m_bus.readInputs(m_address, REG_INPUT + input, 1, values))
         return false;
@@ -89,7 +107,8 @@ const bool remoteIO::getInPort(std::vector<bool> &values)
 {
     if (!m_initialized)
         return false;
-    auto lock = m_bus.getLock();
+    std::lock_guard<std::mutex> lock(m_bus.getMutex());
+    delayRequest();
     LOG_DEBUG("Read Inputs", CH_MAX);
     return m_bus.readInputs(m_address, REG_INPUT, CH_MAX, values);
 }

src/remoteIO.h

@@ -8,9 +8,21 @@
 class remoteIO
 {
 public:
-    typedef enum {CH1, CH2, CH3, CH4, CH5, CH6, CH7, CH8, CH_MAX} channel_t;
+    typedef enum
+    {
+        CH1,
+        CH2,
+        CH3,
+        CH4,
+        CH5,
+        CH6,
+        CH7,
+        CH8,
+        CH_MAX
+    } channel_t;
 
 private:
+    const uint32_t c_minDelay = 100;
     const uint16_t REG_VERSION = 0x8000;
     const uint16_t REG_COILS = 0x0000;
     const uint16_t REG_INPUT = 0x0000;
@@ -32,8 +44,12 @@ public:
 
     void resetAll(const bool value);
 
+private:
+    void delayRequest();
+
 private:
     bool m_initialized;
     drivers::MODBUS &m_bus;
     const uint8_t m_address;
+    uint32_t m_lastRequest;
 };