From 68f02588e2aa4d1d82c3f91e7d2af25340e33021 Mon Sep 17 00:00:00 2001 From: martin chaparro Date: Mon, 21 Jun 2021 19:59:16 -0500 Subject: [PATCH] IBUSM --- ESP32/IBUSM/.gitignore | 5 + ESP32/IBUSM/.vscode/extensions.json | 7 + ESP32/IBUSM/include/IBusBM.h | 96 +++++++++ ESP32/IBUSM/lib/README | 46 +++++ ESP32/IBUSM/platformio.ini | 15 ++ ESP32/IBUSM/src/IBusBM.cpp | 296 ++++++++++++++++++++++++++++ ESP32/IBUSM/src/main.cpp | 33 ++++ ESP32/IBUSM/test/README | 11 ++ ESP8266/MatrixLedsRGB/src/main.cpp | 5 +- ESP8266/MatrixLedsRGB/src/mqtt.hpp | 25 ++- 10 files changed, 527 insertions(+), 12 deletions(-) create mode 100644 ESP32/IBUSM/.gitignore create mode 100644 ESP32/IBUSM/.vscode/extensions.json create mode 100644 ESP32/IBUSM/include/IBusBM.h create mode 100644 ESP32/IBUSM/lib/README create mode 100644 ESP32/IBUSM/platformio.ini create mode 100644 ESP32/IBUSM/src/IBusBM.cpp create mode 100644 ESP32/IBUSM/src/main.cpp create mode 100644 ESP32/IBUSM/test/README diff --git a/ESP32/IBUSM/.gitignore b/ESP32/IBUSM/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/ESP32/IBUSM/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/ESP32/IBUSM/.vscode/extensions.json b/ESP32/IBUSM/.vscode/extensions.json new file mode 100644 index 0000000..0f0d740 --- /dev/null +++ b/ESP32/IBUSM/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/ESP32/IBUSM/include/IBusBM.h b/ESP32/IBUSM/include/IBusBM.h new file mode 100644 index 0000000..9287d43 --- /dev/null +++ b/ESP32/IBUSM/include/IBusBM.h @@ -0,0 +1,96 @@ +/* + * Interface to the RC IBus protocol + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * Created 12 March 2019 Bart Mellink + */ +#ifndef IBusBM_h +#define IBusBM_h + +#include + +#if defined(ARDUINO_ARCH_MBED) +#include "mbed.h" +#include "HardwareSerial.h" +#endif + +// if you have an opentx transciever you can add additional sensor types here. +// see https://github.com/cleanflight/cleanflight/blob/7cd417959b3cb605aa574fc8c0f16759943527ef/src/main/telemetry/ibus_shared.h +// below the values supported by the Turnigy FS-MT6 transceiver +#define IBUSS_INTV 0x00 // Internal voltage (in 0.01) +#define IBUSS_TEMP 0x01 // Temperature (in 0.1 degrees, where 0=-40'C) +#define IBUSS_RPM 0x02 // RPM +#define IBUSS_EXTV 0x03 // External voltage (in 0.01) +#define IBUS_PRESS 0x41 // Pressure (in Pa) +#define IBUS_SERVO 0xfd // Servo value + + +#if defined(ARDUINO_ARCH_MBED) +#define HardwareSerial arduino::HardwareSerial +#else + #if !defined(ARDUINO_ARCH_MEGAAVR) +class HardwareSerial; + #endif +#endif +class Stream; + +class IBusBM { + +public: +#if defined(_VARIANT_ARDUINO_STM32_) + #if !defined(STM32_CORE_VERSION) || (STM32_CORE_VERSION < 0x01090000) + #error "Due to API change, this sketch is compatible with STM32_CORE_VERSION >= 0x01090000" + #endif + #define IBUSBM_NOTIMER NULL // no timer interrupt used + void begin(HardwareSerial &serial, TIM_TypeDef * timerid=TIM1, int8_t rxPin=-1, int8_t txPin=-1); +#else + #define IBUSBM_NOTIMER -1 // no timer interrupt used + void begin(HardwareSerial &serial, int8_t timerid=0, int8_t rxPin=-1, int8_t txPin=-1); +#endif + uint16_t readChannel(uint8_t channelNr); // read servo channel 0..9 + uint8_t addSensor(uint8_t type, uint8_t len=2); // add sensor type and data length (2 or 4), returns address + void setSensorMeasurement(uint8_t adr, int32_t value); + + void loop(void); // used internally for interrupt handline, but needs to be defined as public + + volatile uint8_t cnt_poll; // count received number of sensor poll messages + volatile uint8_t cnt_sensor; // count times a sensor value has been sent back + volatile uint8_t cnt_rec; // count received number of servo messages + +private: + enum State {GET_LENGTH, GET_DATA, GET_CHKSUML, GET_CHKSUMH, DISCARD}; + + static const uint8_t PROTOCOL_LENGTH = 0x20; + static const uint8_t PROTOCOL_OVERHEAD = 3; // packet is , overhead=cmd+chk bytes + static const uint8_t PROTOCOL_TIMEGAP = 3; // Packets are received very ~7ms so use ~half that for the gap + static const uint8_t PROTOCOL_CHANNELS = 14; + static const uint8_t PROTOCOL_COMMAND40 = 0x40; // Command to set servo or motor speed is always 0x40 + static const uint8_t PROTOCOL_COMMAND_DISCOVER = 0x80; // Command discover sensor (lowest 4 bits are sensor) + static const uint8_t PROTOCOL_COMMAND_TYPE = 0x90; // Command discover sensor (lowest 4 bits are sensor) + static const uint8_t PROTOCOL_COMMAND_VALUE = 0xA0; // Command send sensor data (lowest 4 bits are sensor) + static const uint8_t SENSORMAX = 10; // Max number of sensors + + uint8_t state; // state machine state for iBUS protocol + HardwareSerial *stream; // serial port + uint32_t last; // milis() of prior message + uint8_t buffer[PROTOCOL_LENGTH]; // message buffer + uint8_t ptr; // pointer in buffer + uint8_t len; // message length + uint16_t channel[PROTOCOL_CHANNELS]; // servo data received + uint16_t chksum; // checksum calculation + uint8_t lchksum; // checksum lower byte received + typedef struct { + uint8_t sensorType; // sensor type (0,1,2,3, etc) + uint8_t sensorLength; // data length for defined sensor (can be 2 or 4) + int32_t sensorValue; // sensor data for defined sensors (16 or 32 bits) + } sensorinfo; + sensorinfo sensors[SENSORMAX]; + uint8_t NumberSensors = 0; // number of sensors + IBusBM* IBusBMnext = NULL; // pointer to the next class instance to be used to call the loop() method from timer interrupt +}; + +#endif diff --git a/ESP32/IBUSM/lib/README b/ESP32/IBUSM/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/ESP32/IBUSM/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/ESP32/IBUSM/platformio.ini b/ESP32/IBUSM/platformio.ini new file mode 100644 index 0000000..4f4e156 --- /dev/null +++ b/ESP32/IBUSM/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32doit-devkit-v1] +platform = espressif32 +board = esp32doit-devkit-v1 +framework = arduino +monitor_speed = 115200 diff --git a/ESP32/IBUSM/src/IBusBM.cpp b/ESP32/IBUSM/src/IBusBM.cpp new file mode 100644 index 0000000..624c11a --- /dev/null +++ b/ESP32/IBUSM/src/IBusBM.cpp @@ -0,0 +1,296 @@ +/* + * Interface to the RC IBus protocol + * + * Based on original work from: https://gitlab.com/timwilkinson/FlySkyIBus + * Extended to also handle sensors/telemetry data to be sent back to the transmitter, + * interrupts driven and other features. + * + * This lib requires a hardware UART for communication + * Another version using software serial is here https://github.com/Hrastovc/iBUStelemetry + * + * Explaination of sensor/ telemetry prtocol here: + * https://github.com/betaflight/betaflight/wiki/Single-wire-FlySky-(IBus)-telemetry + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * Created 12 March 2019 Bart Mellink + * Updated 4 April 2019 to support ESP32 + * updated 13 jun 2019 to support STM32 (pauluzs) + * Updated 21 Jul 2020 to support MBED (David Peverley) + */ + +#include +#include "IBusBM.h" + +// pointer to the first class instance to be used to call the loop() method from timer interrupt +// will be initiated by class constructor, then daisy channed to other class instances if we have more than one +IBusBM* IBusBMfirst = NULL; + + +// Interrupt on timer0 - called every 1 ms +// we call the IBusSensor.loop() here, so we are certain we respond to sensor requests in a timely matter +#ifdef ARDUINO_ARCH_AVR +SIGNAL(TIMER0_COMPA_vect) { + if (IBusBMfirst) IBusBMfirst->loop(); // gets new servo values if available and process any sensor data +} +#else +void onTimer() { + if (IBusBMfirst) IBusBMfirst->loop(); // gets new servo values if available and process any sensor data +} +#endif + + +#if defined(ARDUINO_ARCH_MBED) +extern "C" { + void TIMER4_IRQHandler_v() { + if (NRF_TIMER4->EVENTS_COMPARE[0] == 1) { + onTimer(); + NRF_TIMER4->EVENTS_COMPARE[0] = 0; + } + } +} +#endif + + +/* + * supports max 14 channels in this lib (with messagelength of 0x20 there is room for 14 channels) + + Example set of bytes coming over the iBUS line for setting servos: + 20 40 DB 5 DC 5 54 5 DC 5 E8 3 D0 7 D2 5 E8 3 DC 5 DC 5 DC 5 DC 5 DC 5 DC 5 DA F3 + Explanation + Protocol length: 20 + Command code: 40 + Channel 0: DB 5 -> value 0x5DB + Channel 1: DC 5 -> value 0x5Dc + Channel 2: 54 5 -> value 0x554 + Channel 3: DC 5 -> value 0x5DC + Channel 4: E8 3 -> value 0x3E8 + Channel 5: D0 7 -> value 0x7D0 + Channel 6: D2 5 -> value 0x5D2 + Channel 7: E8 3 -> value 0x3E8 + Channel 8: DC 5 -> value 0x5DC + Channel 9: DC 5 -> value 0x5DC + Channel 10: DC 5 -> value 0x5DC + Channel 11: DC 5 -> value 0x5DC + Channel 12: DC 5 -> value 0x5DC + Channel 13: DC 5 -> value 0x5DC + Checksum: DA F3 -> calculated by adding up all previous bytes, total must be FFFF + */ + + +#if defined(_VARIANT_ARDUINO_STM32_) +void IBusBM::begin(HardwareSerial &serial, TIM_TypeDef * timerid, int8_t rxPin, int8_t txPin) { +#else +void IBusBM::begin(HardwareSerial &serial, int8_t timerid, int8_t rxPin, int8_t txPin) { +#endif + + #ifdef ARDUINO_ARCH_ESP32 + serial.begin(115200, SERIAL_8N1, rxPin, txPin); + #else + serial.begin(115200, SERIAL_8N1); + #endif + + this->stream = &serial; + this->state = DISCARD; + this->last = millis(); + this->ptr = 0; + this->len = 0; + this->chksum = 0; + this->lchksum = 0; + + // we need to process the iBUS sensor protocol handler frequently enough (at least once each ms) to ensure the response data + // from the sensor is sent on time to the receiver + // if timerid==IBUSBM_NOTIMER the user is responsible for calling the loop function + this->IBusBMnext = IBusBMfirst; + + if (!IBusBMfirst && timerid != IBUSBM_NOTIMER) { + #ifdef ARDUINO_ARCH_AVR + // on AVR architectures Timer0 is already used for millis() - we'll just interrupt somewhere in the middle and call the TIMER0_COMPA_vect interrupt + OCR0A = 0xAF; + TIMSK0 |= _BV(OCIE0A); + #else + // on other architectures we need to use a time + #if defined(ARDUINO_ARCH_ESP32) + hw_timer_t * timer = NULL; + timer = timerBegin(timerid, F_CPU / 1000000L, true); // defaults to timer_id = 0; divider=80 (1 ms); countUp = true; + timerAttachInterrupt(timer, &onTimer, true); // edge = true + timerAlarmWrite(timer, 1000, true); //1 ms + timerAlarmEnable(timer); + #elif defined(_VARIANT_ARDUINO_STM32_) + // see https://github.com/stm32duino/wiki/wiki/HardwareTimer-library + HardwareTimer *stimer_t = new HardwareTimer(timerid); + stimer_t->setOverflow(1000, HERTZ_FORMAT); // 1000 Hz + stimer_t->attachInterrupt(onTimer); + stimer_t->resume(); + #elif defined(ARDUINO_ARCH_MBED) + NRF_TIMER4->TASKS_STOP = 1; // Stop timer + NRF_TIMER4->MODE = TIMER_MODE_MODE_Timer; // Set the timer in Counter Mode + NRF_TIMER4->BITMODE = TIMER_BITMODE_BITMODE_16Bit << TIMER_BITMODE_BITMODE_Pos; + NRF_TIMER2->TASKS_CLEAR = 1; // clear the task first to be usable for later + + // Set prescaler & compare register. + // Prescaler = 0 gives 16MHz timer. + // Prescaler = 4 (2^4) gives 1MHz timer. + NRF_TIMER4->PRESCALER = 4 << TIMER_PRESCALER_PRESCALER_Pos; + NRF_TIMER4->CC[0] = 1000; + + // Enable interrupt on Timer 4 for CC[0] compare match events + NRF_TIMER4->INTENSET = TIMER_INTENSET_COMPARE0_Enabled << TIMER_INTENSET_COMPARE0_Pos; + NRF_TIMER4->SHORTS = TIMER_SHORTS_COMPARE0_CLEAR_Enabled << TIMER_SHORTS_COMPARE0_CLEAR_Pos; + + NVIC_EnableIRQ(TIMER4_IRQn); + + NRF_TIMER4->TASKS_START = 1; // Start TIMER2 + #else + // It should not be too difficult to support additional architectures as most have timer functions, but I only tested AVR and ESP32 + #warning "Timing only supportted for AVR, ESP32 and STM32 architectures. Use timerid IBUSBM_NOTIMER" + #endif + #endif + } + IBusBMfirst = this; +} + +// called from timer interrupt or mannually by user (if IBUSBM_NOTIMER set in begin()) +void IBusBM::loop(void) { + + // if we have multiple instances of IBusBM, we (recursively) call the other instances loop() function + if (IBusBMnext) IBusBMnext->loop(); + + // only process data already in our UART receive buffer + while (stream->available() > 0) { + // only consider a new data package if we have not heard anything for >3ms + uint32_t now = millis(); + if (now - last >= PROTOCOL_TIMEGAP){ + state = GET_LENGTH; + } + last = now; + + uint8_t v = stream->read(); + switch (state) { + case GET_LENGTH: + if (v <= PROTOCOL_LENGTH && v > PROTOCOL_OVERHEAD) { + ptr = 0; + len = v - PROTOCOL_OVERHEAD; + chksum = 0xFFFF - v; + state = GET_DATA; + } else { + state = DISCARD; + } + break; + + case GET_DATA: + buffer[ptr++] = v; + chksum -= v; + if (ptr == len) { + state = GET_CHKSUML; + } + break; + + case GET_CHKSUML: + lchksum = v; + state = GET_CHKSUMH; + break; + + case GET_CHKSUMH: + // Validate checksum + if (chksum == (v << 8) + lchksum) { + // Checksum is all fine Execute command - + uint8_t adr = buffer[0] & 0x0f; + if (buffer[0]==PROTOCOL_COMMAND40) { + // Valid servo command received - extract channel data + for (uint8_t i = 1; i < PROTOCOL_CHANNELS * 2 + 1; i += 2) { + channel[i / 2] = buffer[i] | (buffer[i + 1] << 8); + } + cnt_rec++; + } else if (adr<=NumberSensors && adr>0 && len==1) { + + // all sensor data commands go here + // we only process the len==1 commands (=message length is 4 bytes incl overhead) to prevent the case the + // return messages from the UART TX port loop back to the RX port and are processed again. This is extra + // precaution as it will also be prevented by the PROTOCOL_TIMEGAP required + sensorinfo *s = &sensors[adr-1]; + delayMicroseconds(100); + switch (buffer[0] & 0x0f0) { + case PROTOCOL_COMMAND_DISCOVER: // 0x80, discover sensor + cnt_poll++; + // echo discover command: 0x04, 0x81, 0x7A, 0xFF + stream->write(0x04); + stream->write(PROTOCOL_COMMAND_DISCOVER + adr); + chksum = 0xFFFF - (0x04 + PROTOCOL_COMMAND_DISCOVER + adr); + break; + case PROTOCOL_COMMAND_TYPE: // 0x90, send sensor type + // echo sensortype command: 0x06 0x91 0x00 0x02 0x66 0xFF + stream->write(0x06); + stream->write(PROTOCOL_COMMAND_TYPE + adr); + stream->write(s->sensorType); + stream->write(s->sensorLength); + chksum = 0xFFFF - (0x06 + PROTOCOL_COMMAND_TYPE + adr + s->sensorType + s->sensorLength); + break; + case PROTOCOL_COMMAND_VALUE: // 0xA0, send sensor data + cnt_sensor++; + uint8_t t; + // echo sensor value command: 0x06 0x91 0x00 0x02 0x66 0xFF + stream->write(t = 0x04 + s->sensorLength); + chksum = 0xFFFF - t; + stream->write(t = PROTOCOL_COMMAND_VALUE + adr); + chksum -= t; + stream->write(t = s->sensorValue & 0x0ff); + chksum -= t; + stream->write(t = (s->sensorValue >> 8) & 0x0ff); + chksum -= t; + if (s->sensorLength==4) { + stream->write(t = (s->sensorValue >> 16) & 0x0ff); + chksum -= t; + stream->write(t = (s->sensorValue >> 24) & 0x0ff); + chksum -= t; + } + break; + default: + adr=0; // unknown command, prevent sending chksum + break; + } + if (adr>0) { + stream->write(chksum & 0x0ff); + stream->write(chksum >> 8); + } + } + } + state = DISCARD; + break; + + case DISCARD: + default: + break; + } + } +} + +uint16_t IBusBM::readChannel(uint8_t channelNr) { + if (channelNr < PROTOCOL_CHANNELS) { + return channel[channelNr]; + } else { + return 0; + } +} + +uint8_t IBusBM::addSensor(uint8_t type, uint8_t len) { + // add a sensor, return sensor number + if (len!=2 && len!=4) len = 2; + if (NumberSensors < SENSORMAX) { + sensorinfo *s = &sensors[NumberSensors]; + s->sensorType = type; + s->sensorLength = len; + s->sensorValue = 0; + NumberSensors++; + } + return NumberSensors; +} + +void IBusBM::setSensorMeasurement(uint8_t adr, int32_t value) { + if (adr<=NumberSensors && adr>0) + sensors[adr-1].sensorValue = value; +} + diff --git a/ESP32/IBUSM/src/main.cpp b/ESP32/IBUSM/src/main.cpp new file mode 100644 index 0000000..07b996e --- /dev/null +++ b/ESP32/IBUSM/src/main.cpp @@ -0,0 +1,33 @@ +#include +#include "IBusBM.h" +IBusBM IBus; +int saveval = 0; + +void setup() +{ + Serial.begin(115200); + delay(100); + IBus.begin(Serial2, IBUSBM_NOTIMER); + delay(100); + Serial.println("Start IBus2PWM_ESP32"); +} + +void loop() +{ + uint16_t val; + IBus.loop(); + for (int i=0; i<10; i++) { + Serial.print(IBus.readChannel(i)); + Serial.print(" "); + } + Serial.println(""); + val = IBus.readChannel(2); // get latest value for servo channel 1 + + if (saveval != val) + { + //Serial.println(map(val, 1000, 2000, 0, 500)); + saveval = val; + } + + delay(800); +} \ No newline at end of file diff --git a/ESP32/IBUSM/test/README b/ESP32/IBUSM/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/ESP32/IBUSM/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html diff --git a/ESP8266/MatrixLedsRGB/src/main.cpp b/ESP8266/MatrixLedsRGB/src/main.cpp index f262047..e99f12d 100644 --- a/ESP8266/MatrixLedsRGB/src/main.cpp +++ b/ESP8266/MatrixLedsRGB/src/main.cpp @@ -26,7 +26,7 @@ void setup() delay(5000); ESP.restart(); } - Serial.println(WiFi.localIP()); + //Serial.println(WiFi.localIP()); mensaje += WiFi.localIP().toString(); ota_init(); @@ -39,6 +39,7 @@ void setup() void loop() { ArduinoOTA.handle(); + yield(); if ((millis() - matrix_time) > 150) { matrix_time = millis(); @@ -49,7 +50,7 @@ void loop() { timeAnterior = millis(); if (!client.connected()) - { + { yield(); reconnect(); } timeClient.update(); //sincronizamos con el server NTP diff --git a/ESP8266/MatrixLedsRGB/src/mqtt.hpp b/ESP8266/MatrixLedsRGB/src/mqtt.hpp index a3a5b60..734a71c 100644 --- a/ESP8266/MatrixLedsRGB/src/mqtt.hpp +++ b/ESP8266/MatrixLedsRGB/src/mqtt.hpp @@ -15,6 +15,7 @@ PubSubClient client(wifiClient); void callback(char *p_topic, byte *p_payload, unsigned int p_length) { // concat the payload into a string + String payload; for (uint8_t i = 0; i < p_length; i++) { @@ -24,12 +25,14 @@ void callback(char *p_topic, byte *p_payload, unsigned int p_length) if (String(MQTT_TOPIC_FULL).equals(p_topic)) { + yield(); DeserializationError err = deserializeJson(doc, String(payload)); - if (err) - { - Serial.print(F("deserializeJson() failed with code ")); - Serial.println(err.c_str()); - } + yield(); + // if (err) + // { + // Serial.print(F("deserializeJson() failed with code ")); + // Serial.println(err.c_str()); + // } mensaje = doc["msg"].as(); R = doc["R"]; G = doc["G"]; @@ -47,16 +50,18 @@ void reconnect() String clientId = "matrix_client-" + String(random(0xffff), HEX); if (client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) { - Serial.println("INFO: connected"); + //Serial.println("INFO: connected"); client.subscribe(MQTT_TOPIC_FULL); } else { - Serial.print("ERROR: failed, rc="); - Serial.print(client.state()); - Serial.println("DEBUG: try again in 5 seconds"); - delay(5000); + //Serial.print("ERROR: failed, rc="); + //Serial.print(client.state()); + //Serial.println("DEBUG: try again in 5 seconds"); + yield(); + delay(1000); } + } }