From 75d7bcf3fa8fdfc7bb14b3366a6a5e6fbd7f1250 Mon Sep 17 00:00:00 2001
From: Moreno Gasperini <m.gasperini@digiteco.it>
Date: Fri, 15 Nov 2024 09:29:31 +0100
Subject: [PATCH] DSDL Leaf

Add DSDL Leaf (starting module development)
Fix debug_h for mistake including trace_level for cycloneTCP modules
---
 .../data_types/rmap/measures/Leaf.1.0.dsdl    |   3 +
 .../data_types/rmap/module/Leaf.1.0.dsdl      |   2 +
 .../data_types/rmap/sensors/Leaf.1.0.dsdl     |   3 +
 .../rmap/service/module/Leaf.1.0.dsdl         |  22 +++
 .../data_types/rmap/tableb/B13212.1.0.dsdl    |   6 +
 platformio/stima_v4/master/include/debug.h    |   9 +-
 platformio/stima_v4/master/include/debug_F.h  |  61 ++++----
 .../stima_v4/master/include/tasks/mqtt_task.h | 133 ++++++++++++------
 .../master/include/tasks/supervisor_task.h    |   1 +
 .../stima_v4/master/src/tasks/can_task.cpp    |  42 ++----
 .../stima_v4/master/src/tasks/mqtt_task.cpp   |  32 ++---
 .../master/src/tasks/supervisor_task.cpp      |  36 +++--
 .../stima_v4/slave-level/include/debug.h      |  77 +++++-----
 .../stima_v4/slave-level/include/debug_F.h    |  84 ++++++-----
 .../stima_v4/slave-mppt/include/debug.h       |  77 +++++-----
 .../stima_v4/slave-mppt/include/debug_F.h     |  84 ++++++-----
 .../stima_v4/slave-radiation/include/debug.h  |  77 +++++-----
 .../slave-radiation/include/debug_F.h         |  84 ++++++-----
 .../stima_v4/slave-rain/include/debug.h       |  77 +++++-----
 .../stima_v4/slave-rain/include/debug_F.h     |  84 ++++++-----
 platformio/stima_v4/slave-th/include/debug.h  |  77 +++++-----
 .../stima_v4/slave-th/include/debug_F.h       |  84 ++++++-----
 platformio/stima_v4/slave-vwc/include/debug.h |  77 +++++-----
 .../stima_v4/slave-vwc/include/debug_F.h      |  84 ++++++-----
 .../stima_v4/slave-wind/include/debug.h       |  79 ++++++-----
 .../stima_v4/slave-wind/include/debug_F.h     |  84 ++++++-----
 26 files changed, 893 insertions(+), 586 deletions(-)
 create mode 100644 platformio/stima_v4/cyphal/data_types/rmap/measures/Leaf.1.0.dsdl
 create mode 100644 platformio/stima_v4/cyphal/data_types/rmap/module/Leaf.1.0.dsdl
 create mode 100644 platformio/stima_v4/cyphal/data_types/rmap/sensors/Leaf.1.0.dsdl
 create mode 100644 platformio/stima_v4/cyphal/data_types/rmap/service/module/Leaf.1.0.dsdl
 create mode 100644 platformio/stima_v4/cyphal/data_types/rmap/tableb/B13212.1.0.dsdl

diff --git a/platformio/stima_v4/cyphal/data_types/rmap/measures/Leaf.1.0.dsdl b/platformio/stima_v4/cyphal/data_types/rmap/measures/Leaf.1.0.dsdl
new file mode 100644
index 000000000..c66ab2e86
--- /dev/null
+++ b/platformio/stima_v4/cyphal/data_types/rmap/measures/Leaf.1.0.dsdl
@@ -0,0 +1,3 @@
+rmap.tableb.B13212.1.0 val
+rmap.tableb.B33199.1.0 confidence
+@sealed
\ No newline at end of file
diff --git a/platformio/stima_v4/cyphal/data_types/rmap/module/Leaf.1.0.dsdl b/platformio/stima_v4/cyphal/data_types/rmap/module/Leaf.1.0.dsdl
new file mode 100644
index 000000000..8ddf3fd24
--- /dev/null
+++ b/platformio/stima_v4/cyphal/data_types/rmap/module/Leaf.1.0.dsdl
@@ -0,0 +1,2 @@
+rmap.sensors.Leaf.1.0 BFT
+@sealed
\ No newline at end of file
diff --git a/platformio/stima_v4/cyphal/data_types/rmap/sensors/Leaf.1.0.dsdl b/platformio/stima_v4/cyphal/data_types/rmap/sensors/Leaf.1.0.dsdl
new file mode 100644
index 000000000..a98babdb1
--- /dev/null
+++ b/platformio/stima_v4/cyphal/data_types/rmap/sensors/Leaf.1.0.dsdl
@@ -0,0 +1,3 @@
+rmap.metadata.Metadata.1.0 metadata
+rmap.measures.Leaf.1.0 leaf
+@sealed
\ No newline at end of file
diff --git a/platformio/stima_v4/cyphal/data_types/rmap/service/module/Leaf.1.0.dsdl b/platformio/stima_v4/cyphal/data_types/rmap/service/module/Leaf.1.0.dsdl
new file mode 100644
index 000000000..acf27d730
--- /dev/null
+++ b/platformio/stima_v4/cyphal/data_types/rmap/service/module/Leaf.1.0.dsdl
@@ -0,0 +1,22 @@
+rmap.service.setmode.1.0 parameter
+
+@sealed
+
+---
+
+#RMAP Response state
+uint4 state
+
+#RMAP version and revision
+uint8 version
+uint8 revision
+
+#MODULE Flags state
+bool is_adc_unit_error
+bool is_adc_unit_overflow
+uint8 rbt_event
+uint8 wdt_event
+
+rmap.sensors.Leaf.1.0 BFT
+
+@sealed
diff --git a/platformio/stima_v4/cyphal/data_types/rmap/tableb/B13212.1.0.dsdl b/platformio/stima_v4/cyphal/data_types/rmap/tableb/B13212.1.0.dsdl
new file mode 100644
index 000000000..df5a60557
--- /dev/null
+++ b/platformio/stima_v4/cyphal/data_types/rmap/tableb/B13212.1.0.dsdl
@@ -0,0 +1,6 @@
+# Time leaf (sec)
+# fattore scala x1
+# MAX 12h (sec)
+uint16 MAX = 43200
+uint16 value
+@sealed
\ No newline at end of file
diff --git a/platformio/stima_v4/master/include/debug.h b/platformio/stima_v4/master/include/debug.h
index 55a5bb39d..3a62c4de4 100644
--- a/platformio/stima_v4/master/include/debug.h
+++ b/platformio/stima_v4/master/include/debug.h
@@ -2,11 +2,12 @@
  ******************************************************************************
  * @file    debug.h
  * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
  * @brief   Debug constants and functions
  ******************************************************************************
  * @attention
  *
- * <h2><center>&copy; Copyright (C) 2022 Marco Baldinetti <m.baldinetti@digiteco.it></center></h2>
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
  * <h2><center>All rights reserved.</center></h2>
  *
  * This program is free software; you can redistribute it and/or
@@ -25,7 +26,7 @@
  * <http://www.gnu.org/licenses/>.
  *
  ******************************************************************************
- */
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
@@ -69,8 +70,6 @@
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
 // Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
@@ -161,6 +160,8 @@ void print_debug(const char *fmt, ...);
 // Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
 // Deprecated definitions
diff --git a/platformio/stima_v4/master/include/debug_F.h b/platformio/stima_v4/master/include/debug_F.h
index fb81659b0..9fa10dfb6 100644
--- a/platformio/stima_v4/master/include/debug_F.h
+++ b/platformio/stima_v4/master/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <m.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -94,14 +101,4 @@ void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
    #define LOG_VERBOSE_F(...)
 #endif
 
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
-#endif
-
 #endif
diff --git a/platformio/stima_v4/master/include/tasks/mqtt_task.h b/platformio/stima_v4/master/include/tasks/mqtt_task.h
index 46f9f28bb..2118225a3 100644
--- a/platformio/stima_v4/master/include/tasks/mqtt_task.h
+++ b/platformio/stima_v4/master/include/tasks/mqtt_task.h
@@ -118,40 +118,98 @@
 const uint16_t MqttCipherSuites[] =
 {
   // rmap server psk ciphers
-  TLS_PSK_WITH_AES_256_CCM                        // WEAK BUT WORK
-  // TLS_DHE_PSK_WITH_AES_128_GCM_SHA256          // RECOMMENDED BUT NOT WORK
-  // TLS_DHE_PSK_WITH_AES_256_GCM_SHA384          // RECOMMENDED BUT NOT WORK
-  // TLS_ECDHE_PSK_WITH_CHACHA20_POLY1305_SHA256  // RECOMMENDED BUT NOT WORK
-  // TLS_DHE_PSK_WITH_CHACHA20_POLY1305_SHA256    // RECOMMENDED BUT NOT WORK
-
-  // TLS_PSK_WITH_AES_256_CBC_SHA,                // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_PSK_WITH_AES_256_GCM_SHA384,             // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_ECDHE_PSK_WITH_AES_256_CBC_SHA384,       // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_ECDHE_PSK_WITH_AES_256_CBC_SHA,          // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_DHE_PSK_WITH_AES_256_CBC_SHA384,         // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_PSK_WITH_AES_256_CBC_SHA384,             // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_DHE_PSK_WITH_AES_128_GCM_SHA256,         // RECOMMENDED BUT NOT WORK
-  // TLS_PSK_WITH_AES_128_GCM_SHA256,             // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_ECDHE_PSK_WITH_AES_128_CBC_SHA256,       // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_ECDHE_PSK_WITH_AES_128_CBC_SHA,          // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_DHE_PSK_WITH_AES_128_CBC_SHA256,         // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_DHE_PSK_WITH_AES_128_CBC_SHA,            // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_PSK_WITH_AES_128_CBC_SHA256,             // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-  // TLS_PSK_WITH_AES_128_CBC_SHA                 // WEAK BUT NOT WORK (PREVIOUSLY WORK)
-
-  // Recommended psk ciphers
-  // TLS_DHE_PSK_WITH_AES_128_GCM_SHA256,
-  // TLS_DHE_PSK_WITH_AES_256_GCM_SHA384,
-  // TLS_DHE_PSK_WITH_CAMELLIA_128_GCM_SHA256,
-  // TLS_DHE_PSK_WITH_CAMELLIA_256_GCM_SHA384,
-  // TLS_DHE_PSK_WITH_ARIA_128_GCM_SHA256,
-  // TLS_DHE_PSK_WITH_ARIA_256_GCM_SHA384,
-  // TLS_DHE_PSK_WITH_CHACHA20_POLY1305_SHA256,
-  // TLS_ECDHE_PSK_WITH_AES_128_GCM_SHA256,
-  // TLS_ECDHE_PSK_WITH_AES_256_GCM_SHA384,
-  // TLS_ECDHE_PSK_WITH_CHACHA20_POLY1305_SHA256
+  TLS_PSK_WITH_AES_256_CCM
 };
 
+// const uint16_t AllMqttCipherSuitesPsk[] =
+// {  
+//   TLS_PSK_WITH_NULL_SHA                         ,
+//   TLS_PSK_WITH_NULL_SHA256                      ,
+//   TLS_PSK_WITH_NULL_SHA384                      ,
+//   TLS_PSK_WITH_RC4_128_SHA                      ,
+//   TLS_PSK_WITH_3DES_EDE_CBC_SHA                 ,
+//   TLS_PSK_WITH_AES_128_CBC_SHA                  ,
+//   TLS_PSK_WITH_AES_256_CBC_SHA                  ,
+//   TLS_PSK_WITH_AES_128_CBC_SHA256               ,
+//   TLS_PSK_WITH_AES_256_CBC_SHA384               ,
+//   TLS_PSK_WITH_AES_128_GCM_SHA256               ,
+//   TLS_PSK_WITH_AES_128_CCM                      ,
+//   TLS_PSK_WITH_AES_256_CCM                      ,
+//   TLS_PSK_WITH_AES_128_CCM_8                    ,
+//   TLS_PSK_WITH_AES_256_CCM_8                    ,
+//   TLS_PSK_WITH_CAMELLIA_128_CBC_SHA256          ,
+//   TLS_PSK_WITH_CAMELLIA_256_CBC_SHA384          ,
+//   TLS_PSK_WITH_CAMELLIA_128_GCM_SHA256          ,
+//   TLS_PSK_WITH_CAMELLIA_256_GCM_SHA384          ,
+//   TLS_PSK_WITH_ARIA_128_CBC_SHA256              ,
+//   TLS_PSK_WITH_ARIA_256_CBC_SHA384              ,
+//   TLS_PSK_WITH_ARIA_128_GCM_SHA256              ,
+//   TLS_PSK_WITH_ARIA_256_GCM_SHA384              ,
+//   TLS_PSK_WITH_CHACHA20_POLY1305_SHA256         ,
+//   TLS_RSA_PSK_WITH_NULL_SHA                     ,
+//   TLS_RSA_PSK_WITH_NULL_SHA256                  ,
+//   TLS_RSA_PSK_WITH_NULL_SHA384                  ,
+//   TLS_RSA_PSK_WITH_RC4_128_SHA                  ,
+//   TLS_RSA_PSK_WITH_3DES_EDE_CBC_SHA             ,
+//   TLS_RSA_PSK_WITH_AES_128_CBC_SHA              ,
+//   TLS_RSA_PSK_WITH_AES_256_CBC_SHA              ,
+//   TLS_RSA_PSK_WITH_AES_128_CBC_SHA256           ,
+//   TLS_RSA_PSK_WITH_AES_256_CBC_SHA384           ,
+//   TLS_RSA_PSK_WITH_AES_128_GCM_SHA256           ,
+//   TLS_RSA_PSK_WITH_AES_256_GCM_SHA384,           
+//   TLS_RSA_PSK_WITH_CAMELLIA_128_CBC_SHA256      ,
+//   TLS_RSA_PSK_WITH_CAMELLIA_256_CBC_SHA384      ,
+//   TLS_RSA_PSK_WITH_CAMELLIA_128_GCM_SHA256      ,
+//   TLS_RSA_PSK_WITH_CAMELLIA_256_GCM_SHA384      ,
+//   TLS_RSA_PSK_WITH_ARIA_128_CBC_SHA256          ,
+//   TLS_RSA_PSK_WITH_ARIA_256_CBC_SHA384          ,
+//   TLS_RSA_PSK_WITH_ARIA_128_GCM_SHA256          ,
+//   TLS_RSA_PSK_WITH_ARIA_256_GCM_SHA384          ,
+//   TLS_RSA_PSK_WITH_CHACHA20_POLY1305_SHA256     ,
+//   TLS_DHE_PSK_WITH_NULL_SHA                     ,
+//   TLS_DHE_PSK_WITH_NULL_SHA256                  ,
+//   TLS_DHE_PSK_WITH_NULL_SHA384                  ,
+//   TLS_DHE_PSK_WITH_RC4_128_SHA                  ,
+//   TLS_DHE_PSK_WITH_3DES_EDE_CBC_SHA             ,
+//   TLS_DHE_PSK_WITH_AES_128_CBC_SHA              ,
+//   TLS_DHE_PSK_WITH_AES_256_CBC_SHA              ,
+//   TLS_DHE_PSK_WITH_AES_128_CBC_SHA256           ,
+//   TLS_DHE_PSK_WITH_AES_256_CBC_SHA384           ,
+//   TLS_DHE_PSK_WITH_AES_128_GCM_SHA256           ,
+//   TLS_DHE_PSK_WITH_AES_256_GCM_SHA384           ,
+//   TLS_DHE_PSK_WITH_AES_128_CCM                  ,
+//   TLS_DHE_PSK_WITH_AES_256_CCM                  ,
+//   TLS_DHE_PSK_WITH_AES_128_CCM_8                ,
+//   TLS_DHE_PSK_WITH_AES_256_CCM_8                ,
+//   TLS_DHE_PSK_WITH_CAMELLIA_128_CBC_SHA256      ,
+//   TLS_DHE_PSK_WITH_CAMELLIA_256_CBC_SHA384      ,
+//   TLS_DHE_PSK_WITH_CAMELLIA_128_GCM_SHA256      ,
+//   TLS_DHE_PSK_WITH_CAMELLIA_256_GCM_SHA384      ,
+//   TLS_DHE_PSK_WITH_ARIA_128_CBC_SHA256          ,
+//   TLS_DHE_PSK_WITH_ARIA_256_CBC_SHA384          ,
+//   TLS_DHE_PSK_WITH_ARIA_128_GCM_SHA256          ,
+//   TLS_DHE_PSK_WITH_ARIA_256_GCM_SHA384          ,
+//   TLS_DHE_PSK_WITH_CHACHA20_POLY1305_SHA256     ,
+//   TLS_ECDHE_PSK_WITH_NULL_SHA                   ,
+//   TLS_ECDHE_PSK_WITH_NULL_SHA256                ,
+//   TLS_ECDHE_PSK_WITH_NULL_SHA384                ,
+//   TLS_ECDHE_PSK_WITH_RC4_128_SHA                ,
+//   TLS_ECDHE_PSK_WITH_3DES_EDE_CBC_SHA           ,
+//   TLS_ECDHE_PSK_WITH_AES_128_CBC_SHA            ,
+//   TLS_ECDHE_PSK_WITH_AES_256_CBC_SHA            ,
+//   TLS_ECDHE_PSK_WITH_AES_128_CBC_SHA256         ,
+//   TLS_ECDHE_PSK_WITH_AES_256_CBC_SHA384         ,
+//   TLS_ECDHE_PSK_WITH_AES_128_GCM_SHA256         ,
+//   TLS_ECDHE_PSK_WITH_AES_256_GCM_SHA384         ,
+//   TLS_ECDHE_PSK_WITH_AES_128_CCM_SHA256         ,
+//   TLS_ECDHE_PSK_WITH_AES_128_CCM_8_SHA256       ,
+//   TLS_ECDHE_PSK_WITH_CAMELLIA_128_CBC_SHA256    ,
+//   TLS_ECDHE_PSK_WITH_CAMELLIA_256_CBC_SHA384    ,
+//   TLS_ECDHE_PSK_WITH_ARIA_128_CBC_SHA256        ,
+//   TLS_ECDHE_PSK_WITH_ARIA_256_CBC_SHA384        ,
+//   TLS_ECDHE_PSK_WITH_CHACHA20_POLY1305_SHA256
+// };
+
 using namespace cpp_freertos;
 
 /// @brief struct local elaborate data parameter
@@ -277,20 +335,11 @@ class MqttTask : public cpp_freertos::Thread {
   inline static cpp_freertos::BinarySemaphore *localRpcLock;
   inline static JsonRPC *localStreamRpc;
 
-  inline static MqttClientContext *localPtrMqttClientContext;
-
   inline static system_status_t *localSystemStatus;
+  inline static configuration_t *localConfiguration;
 
   inline static YarrowContext *MqttYarrowContext;
 
-  // Client's PSK key
-  inline static uint8_t *MqttClientPSKKey;
-
-  // Client's PSK identity
-  inline static char_t MqttClientPSKIdentity[CLIENT_PSK_IDENTITY_LENGTH];
-
-  inline static char_t *MqttServer;
-
   bool is_event_rpc;
 };
 
diff --git a/platformio/stima_v4/master/include/tasks/supervisor_task.h b/platformio/stima_v4/master/include/tasks/supervisor_task.h
index 8bf72bc4b..4deef6d5d 100644
--- a/platformio/stima_v4/master/include/tasks/supervisor_task.h
+++ b/platformio/stima_v4/master/include/tasks/supervisor_task.h
@@ -53,6 +53,7 @@
 #define SUPERVISOR_TASK_SLEEP_DELAY_MS          (1000)
 
 #define MIN_ATTEMPTED_CONNECTION_VALID          (5)
+#define RANDOM_RUN_CONNECTION_SERVER_SEC        (60)
 
 #if (ENABLE_I2C1 || ENABLE_I2C2)
 #include <Wire.h>
diff --git a/platformio/stima_v4/master/src/tasks/can_task.cpp b/platformio/stima_v4/master/src/tasks/can_task.cpp
index 22b7416f3..3c7f3cd42 100644
--- a/platformio/stima_v4/master/src/tasks/can_task.cpp
+++ b/platformio/stima_v4/master/src/tasks/can_task.cpp
@@ -2057,10 +2057,8 @@ void CanTask::Run() {
                                     TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"),
                                         stimaName, clCanard.slave[queueId].get_node_id(), retTHData->state);
                                     TRACE_VERBOSE_F(F("Value (ITH) TP %d, UH: %d\r\n"), retTHData->ITH.temperature.val.value, retTHData->ITH.humidity.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retTHData->version>4)||((retTHData->version==4)&&(retTHData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retTHData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retTHData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retTHData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
@@ -2136,10 +2134,8 @@ void CanTask::Run() {
                                     TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"),
                                         stimaName, clCanard.slave[queueId].get_node_id(), retRainData->state);
                                     TRACE_VERBOSE_F(F("Value (TBR) Rain %d\r\n"), retRainData->TBR.rain.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retRainData->version>4)||((retRainData->version==4)&&(retRainData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retRainData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retRainData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retRainData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
@@ -2221,10 +2217,8 @@ void CanTask::Run() {
                                     TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"),
                                         stimaName, clCanard.slave[queueId].get_node_id(), retWindData->state);
                                     TRACE_VERBOSE_F(F("Value (DWA) Speed %d, Dir: %d\r\n"), retWindData->DWA.speed.val.value, retWindData->DWA.direction.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retWindData->version>4)||((retWindData->version==4)&&(retWindData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retWindData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retWindData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retWindData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
@@ -2303,10 +2297,8 @@ void CanTask::Run() {
                                     TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"),
                                         stimaName, clCanard.slave[queueId].get_node_id(), retRadiationData->state);
                                     TRACE_VERBOSE_F(F("Value (DSA) Radiation %d\r\n"), retRadiationData->DSA.radiation.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retRadiationData->version>4)||((retRadiationData->version==4)&&(retRadiationData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retRadiationData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retRadiationData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retRadiationData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
@@ -2382,10 +2374,8 @@ void CanTask::Run() {
                                     TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"),
                                         stimaName, clCanard.slave[queueId].get_node_id(), retLevelData->state);
                                     TRACE_VERBOSE_F(F("Value (LVM) Level %d\r\n"), retLevelData->LVM.river_level.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retLevelData->version>4)||((retLevelData->version==4)&&(retLevelData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retLevelData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retLevelData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retLevelData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
@@ -2462,10 +2452,8 @@ void CanTask::Run() {
                                         stimaName, clCanard.slave[queueId].get_node_id(), retPwrData->state);
                                     TRACE_VERBOSE_F(F("Value (MPP) Batt Chg. %d, In V. %d, Batt Curr. %d\r\n"),
                                         retPwrData->MPP.battery_charge.val.value, retPwrData->MPP.input_voltage.val.value, retPwrData->MPP.battery_current.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retPwrData->version>4)||((retPwrData->version==4)&&(retPwrData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retPwrData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retPwrData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retPwrData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
@@ -2550,10 +2538,8 @@ void CanTask::Run() {
                                     TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"),
                                         stimaName, clCanard.slave[queueId].get_node_id(), retVwcData->state);
                                     TRACE_VERBOSE_F(F("Value (VWC) Soil moisture 1,2,3 [ %d, %d, %d ]\r\n"), retVwcData->VWC1.vwc.val.value, retVwcData->VWC2.vwc.val.value, retVwcData->VWC3.vwc.val.value);
-                                    // Get security remote state on maintenance mode from 4.2 Version
-                                    if((retVwcData->version>4)||((retVwcData->version==4)&&(retVwcData->revision>=2))) {
-                                        param.system_status->data_slave[queueId].maintenance_mode = (retVwcData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
-                                    }
+                                    // Get security remote state on maintenance mode from relative state flags
+                                    param.system_status->data_slave[queueId].maintenance_mode = (retVwcData->state & CAN_FLAG_IS_MAINTENANCE_MODE);
                                     retVwcData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE;
                                     if(param.system_status->data_slave[queueId].maintenance_mode) {
                                         TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n"));
diff --git a/platformio/stima_v4/master/src/tasks/mqtt_task.cpp b/platformio/stima_v4/master/src/tasks/mqtt_task.cpp
index d824303c4..b6091bcc9 100644
--- a/platformio/stima_v4/master/src/tasks/mqtt_task.cpp
+++ b/platformio/stima_v4/master/src/tasks/mqtt_task.cpp
@@ -37,12 +37,6 @@
 
 using namespace cpp_freertos;
 
-/* 
-  List of preferred ciphersuites
-  https://ciphersuite.info/cs/?security=recommended&singlepage=true&page=2&tls=all&sort=asc
-  defined in tasks/mqtt_task.h -> const uint16_t cipherSuites[] = TYPE_VALUE
-*/
-
 /// @brief Construct a new Mqtt Task:: Mqtt Task object
 /// @param taskName name of the task
 /// @param stackSize size of the stack
@@ -58,7 +52,7 @@ MqttTask::MqttTask(const char *taskName, uint16_t stackSize, uint8_t priority, M
   localRpcLock = param.rpcLock;
   localStreamRpc = param.streamRpc;
   localSystemStatus = param.system_status;
-  localPtrMqttClientContext = &mqttClientContext;
+  localConfiguration = param.configuration;
 
   state = MQTT_STATE_INIT;
   version = MQTT_VERSION_3_1_1;
@@ -188,10 +182,7 @@ void MqttTask::Run()
         TaskState(state, UNUSED_SUB_POSITION, task_flag::normal);      
         // do mqtt connection
         if (connection_request.do_mqtt_connect)
-        {
-          // Set local pointer MqttServer to config string
-          MqttServer = param.configuration->mqtt_server;
-          
+        {          
           // Start new connection sequence
           mqtt_connection_estabilished = false;
 
@@ -252,11 +243,6 @@ void MqttTask::Run()
       {
         // Shared Pointer
         MqttYarrowContext = param.yarrowContext;
-        MqttClientPSKKey = param.configuration->client_psk_key;
-
-        // Set PSK identity
-        snprintf(MqttClientPSKIdentity, sizeof(MqttClientPSKIdentity), "%s/%s/%s", param.configuration->mqtt_username, param.configuration->stationslug, param.configuration->board_master.boardslug);
-
         // MQTT over TLS
         mqttClientSetTransportProtocol(&mqttClientContext, MQTT_TRANSPORT_PROTOCOL_TLS);
         // Register TLS initialization callback
@@ -920,8 +906,10 @@ void MqttTask::Run()
         if(rmap_data_error) {
           TRACE_ERROR_F(F("MQTT: RMAP Reading Data, exit from upload Data Queue [ %s ]\r\n"), ERROR_STRING);
         }
-        // Trace END Data response
-        TRACE_INFO_F(F("Uploading data RMAP Archive [ %s ]. Updated %d record\r\n"), rmap_eof ? OK_STRING : ERROR_STRING, countData);
+        // TRACE END Data response
+        if(countData) {
+          TRACE_INFO_F(F("Uploading data from RMAP Archive [ %s ]. Updated %d record\r\n"), rmap_eof ? OK_STRING : ERROR_STRING, countData);
+        }
       }
       // *****************************************
       //  END GET RMAP Data Queue and Append MQTT
@@ -3568,6 +3556,7 @@ error_t MqttTask::makeSensorMessageBatteryCharge(rmap_measures_BatteryCharge_1_0
 error_t MqttTask::mqttTlsInitCallback(MqttClientContext *context, TlsContext *tlsContext)
 {
   error_t error;
+  char MqttClientPSKIdentity[MQTT_USERNAME_LENGTH + MQTT_PASSWORD_LENGTH + STATIONSLUG_LENGTH];
 
   //Debug message
   TRACE_INFO_F(F("MQTT TLS initialization callback\r\n"));
@@ -3579,25 +3568,26 @@ error_t MqttTask::mqttTlsInitCallback(MqttClientContext *context, TlsContext *tl
     return error;
 
   //Preferred cipher suite list
-  error = tlsSetCipherSuites(tlsContext, MqttCipherSuites, sizeof(MqttCipherSuites));
+  error = tlsSetCipherSuites(tlsContext, MqttCipherSuites, sizeof(MqttCipherSuites) / 2);
   // Any error to report?
   if(error)
     return error;
 
   //Set the fully qualified domain name of the server
-  error = tlsSetServerName(tlsContext, MqttServer);
+  error = tlsSetServerName(tlsContext, localConfiguration->mqtt_server);
   //Any error to report?
   if(error)
     return error;
 
   //Set the PSK identity to be used by the client
+  snprintf(MqttClientPSKIdentity, sizeof(MqttClientPSKIdentity), "%s/%s/%s", localConfiguration->mqtt_username, localConfiguration->stationslug, localConfiguration->board_master.boardslug);
   error = tlsSetPskIdentity(tlsContext, MqttClientPSKIdentity);
   //Any error to report?
   if(error)
     return error;
 
   //Set the pre-shared key to be used
-  error = tlsSetPsk(tlsContext, MqttClientPSKKey, CLIENT_PSK_KEY_LENGTH);
+  error = tlsSetPsk(tlsContext, localConfiguration->client_psk_key, CLIENT_PSK_KEY_LENGTH);
   //Any error to report?
   if(error)
     return error;
diff --git a/platformio/stima_v4/master/src/tasks/supervisor_task.cpp b/platformio/stima_v4/master/src/tasks/supervisor_task.cpp
index a0958794b..52a38e768 100644
--- a/platformio/stima_v4/master/src/tasks/supervisor_task.cpp
+++ b/platformio/stima_v4/master/src/tasks/supervisor_task.cpp
@@ -241,17 +241,19 @@ void SupervisorTask::Run()
       }
 
       #if (!DISABLE_CONNECTION)
-
-      // Get RND(XX = 60) Second Offset Start connection for avoid overcharging RMAP Server
+      #if (!TEST_CONNECTION)
+      #if (RANDOM_RUN_CONNECTION_SERVER_SEC)
+      // Get RND(XX = RANDOM_RUN_CONNECTION_SERVER_SEC) Second Offset Start connection for avoid overcharging RMAP Server
       if(!bGetNextSecondOffsetStart) {
         bGetNextSecondOffsetStart = true;
-        bSecondOffsetStart = random(60);
+        bSecondOffsetStart = random(RANDOM_RUN_CONNECTION_SERVER_SEC);
       }
       // Waiting coutdown on Timing
       if((param.system_status->flags.new_start_connect)&&(bSecondOffsetStart > 0)) {
         bSecondOffsetStart -= (SUPERVISOR_TASK_SLEEP_DELAY_MS / 1000);
         break;
       }
+      #endif
 
       // Check date RTime for syncro operation (if required)
       rtc.getTime(&hh, &nn, &ss, &ms);
@@ -264,7 +266,6 @@ void SupervisorTask::Run()
         param.systemStatusLock->Give();
       }
 
-      #if (!TEST_CONNECTION)
       // Checking starting Connection inibition next Start... If something Wrong... (Default 10 min)
       // RPC Request Command as (configure or download firmware) will start immediatly
       if(((rtc.getEpoch() - param.system_status->datetime.epoch_connect_run) > MIN_INIBITH_CONNECT_RETRY_S)||
@@ -363,7 +364,6 @@ void SupervisorTask::Run()
       param.systemStatusLock->Give();
 
       #endif
-
       #endif
 
       break;
@@ -453,6 +453,7 @@ void SupervisorTask::Run()
           {
             // No more action. Mqtt is not required (Also connected or dropped)
             param.system_status->connection.is_mqtt_disconnected = true;
+            TRACE_VERBOSE_F(F("SUPERVISOR_STATE_CONNECTION_OPERATION -> SUPERVISOR_CONNECTION_END\r\n"));
             state_check_connection = CONNECTION_END;
             break;
           }
@@ -819,11 +820,7 @@ bool SupervisorTask::loadConfiguration()
     param.configuration->network_regver = CONFIGURATION_GSM_DEFAULT_REGISTRATION;
   }
 
-  // Always FIX configuration eeprom saved paramtere with FIRMWARE fixed parameter
-  param.configuration->module_main_version = MODULE_MAIN_VERSION;
-  param.configuration->module_minor_version = MODULE_MINOR_VERSION;
-  param.configuration->module_type = (Module_Type)MODULE_TYPE;
-
+  // Check VERSION and TYPE for verify EEProm read parameter are good
   #if (INIT_PARAMETER)
   status = saveConfiguration(CONFIGURATION_DEFAULT);
   #else
@@ -833,7 +830,10 @@ bool SupervisorTask::loadConfiguration()
   }
   #endif
 
-  // Set local parameter EXTRA param for specific GSM/GPRS/EDGE/LTE CONNECTION SIM
+  // Force minor versione configuration param with FIRMWARE minor version
+  param.configuration->module_minor_version = MODULE_MINOR_VERSION;
+
+// Set local parameter EXTRA param for specific GSM/GPRS/EDGE/LTE CONNECTION SIM
   // Other local parametre to save...
   #if(SAVE_LOCAL_ASSIGN_CONFIGURATION_PARAMETER)
     // VALID VALUE ->
@@ -890,6 +890,20 @@ bool SupervisorTask::loadConfiguration()
     // Force update configuration
     update_cfg = true;
     #endif
+  #else
+    // Check NETWORK_GSM_BASE Parameter with Default (if some parameter are wrong)
+  if((param.configuration->network_type!=0)||
+     (param.configuration->network_type!=2)||
+     (param.configuration->network_type!=13)||
+     (param.configuration->network_type!=14)||
+     (param.configuration->network_type!=38)||
+     (param.configuration->network_regver>3)) {
+    strSafeCopy(param.configuration->network_order, CONFIGURATION_DEFAULT_GSM_NETWORK_ORDER, GSM_ORDER_NETWORK_LENGTH);
+    param.configuration->monitor_flags = CONFIGURATION_GSM_DEFAULT_MONITOR_FLAGS;
+    param.configuration->network_type = CONFIGURATION_GSM_DEFAULT_NETWORK;
+    param.configuration->network_regver = CONFIGURATION_GSM_DEFAULT_REGISTRATION;
+    update_cfg = true;
+  }
   #endif
 
   // Update configuration required
diff --git a/platformio/stima_v4/slave-level/include/debug.h b/platformio/stima_v4/slave-level/include/debug.h
index b432c0cbf..3a62c4de4 100644
--- a/platformio/stima_v4/slave-level/include/debug.h
+++ b/platformio/stima_v4/slave-level/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,6 +40,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
 #define REDUNDANT_STRING      "REDUNDANT"
 #define MAIN_STRING           "MAIN"
@@ -43,23 +50,27 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -72,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -141,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-level/include/debug_F.h b/platformio/stima_v4/slave-level/include/debug_F.h
index c354940b4..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-level/include/debug_F.h
+++ b/platformio/stima_v4/slave-level/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif
diff --git a/platformio/stima_v4/slave-mppt/include/debug.h b/platformio/stima_v4/slave-mppt/include/debug.h
index b432c0cbf..3a62c4de4 100644
--- a/platformio/stima_v4/slave-mppt/include/debug.h
+++ b/platformio/stima_v4/slave-mppt/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,6 +40,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
 #define REDUNDANT_STRING      "REDUNDANT"
 #define MAIN_STRING           "MAIN"
@@ -43,23 +50,27 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -72,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -141,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-mppt/include/debug_F.h b/platformio/stima_v4/slave-mppt/include/debug_F.h
index c354940b4..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-mppt/include/debug_F.h
+++ b/platformio/stima_v4/slave-mppt/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif
diff --git a/platformio/stima_v4/slave-radiation/include/debug.h b/platformio/stima_v4/slave-radiation/include/debug.h
index b432c0cbf..3a62c4de4 100644
--- a/platformio/stima_v4/slave-radiation/include/debug.h
+++ b/platformio/stima_v4/slave-radiation/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,6 +40,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
 #define REDUNDANT_STRING      "REDUNDANT"
 #define MAIN_STRING           "MAIN"
@@ -43,23 +50,27 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -72,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -141,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-radiation/include/debug_F.h b/platformio/stima_v4/slave-radiation/include/debug_F.h
index c354940b4..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-radiation/include/debug_F.h
+++ b/platformio/stima_v4/slave-radiation/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif
diff --git a/platformio/stima_v4/slave-rain/include/debug.h b/platformio/stima_v4/slave-rain/include/debug.h
index b432c0cbf..3a62c4de4 100644
--- a/platformio/stima_v4/slave-rain/include/debug.h
+++ b/platformio/stima_v4/slave-rain/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,6 +40,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
 #define REDUNDANT_STRING      "REDUNDANT"
 #define MAIN_STRING           "MAIN"
@@ -43,23 +50,27 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -72,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -141,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-rain/include/debug_F.h b/platformio/stima_v4/slave-rain/include/debug_F.h
index c354940b4..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-rain/include/debug_F.h
+++ b/platformio/stima_v4/slave-rain/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif
diff --git a/platformio/stima_v4/slave-th/include/debug.h b/platformio/stima_v4/slave-th/include/debug.h
index 4dfa14273..3a62c4de4 100644
--- a/platformio/stima_v4/slave-th/include/debug.h
+++ b/platformio/stima_v4/slave-th/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <m.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,6 +40,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
 #define REDUNDANT_STRING      "REDUNDANT"
 #define MAIN_STRING           "MAIN"
@@ -43,23 +50,27 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -72,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -141,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-th/include/debug_F.h b/platformio/stima_v4/slave-th/include/debug_F.h
index 27a38a2f6..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-th/include/debug_F.h
+++ b/platformio/stima_v4/slave-th/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <m.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif
diff --git a/platformio/stima_v4/slave-vwc/include/debug.h b/platformio/stima_v4/slave-vwc/include/debug.h
index b432c0cbf..3a62c4de4 100644
--- a/platformio/stima_v4/slave-vwc/include/debug.h
+++ b/platformio/stima_v4/slave-vwc/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,6 +40,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
 #define REDUNDANT_STRING      "REDUNDANT"
 #define MAIN_STRING           "MAIN"
@@ -43,23 +50,27 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -72,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -141,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-vwc/include/debug_F.h b/platformio/stima_v4/slave-vwc/include/debug_F.h
index c354940b4..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-vwc/include/debug_F.h
+++ b/platformio/stima_v4/slave-vwc/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif
diff --git a/platformio/stima_v4/slave-wind/include/debug.h b/platformio/stima_v4/slave-wind/include/debug.h
index 59dbdd29e..3a62c4de4 100644
--- a/platformio/stima_v4/slave-wind/include/debug.h
+++ b/platformio/stima_v4/slave-wind/include/debug.h
@@ -1,30 +1,36 @@
-/**@file debug.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
-//Dependencies
 #include <stdio.h>
 #include <stdint.h>
 #include <Arduino.h>
@@ -34,30 +40,37 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #define NO_STRING             "NO"
 #define YES_STRING            "YES"
 #define FAIL_STRING           "FAIL"
+#define ABORT_STRING          "ABORT"
 #define ERROR_STRING          "ERROR"
+#define REDUNDANT_STRING      "REDUNDANT"
+#define MAIN_STRING           "MAIN"
 #define ON_STRING             "ON"
 #define OFF_STRING            "OFF"
 #define SUSPEND_STRING        "x"
 #define FLAG_STRING           "*"
 #define SPACE_STRING          " "
 
-//Trace level definitions
+/// @brief OFF level of trace
 #define TRACE_LEVEL_OFF       0
+/// @brief Fatal level of trace
 #define TRACE_LEVEL_FATAL     1
+/// @brief Error level of trace
 #define TRACE_LEVEL_ERROR     2
+/// @brief Warning level of trace
 #define TRACE_LEVEL_WARNING   3
+/// @brief Info level of trace
 #define TRACE_LEVEL_INFO      4
+/// @brief Debug level of trace
 #define TRACE_LEVEL_DEBUG     5
+/// @brief Verbose level of trace
 #define TRACE_LEVEL_VERBOSE   6
 
-//Default trace level
+// Default trace level
 #ifndef TRACE_LEVEL
    #define TRACE_LEVEL TRACE_LEVEL_DEBUG
 #endif
 
-void print_debug(const char *fmt, ...);
-
-//Trace output redirection
+// Trace output redirection
 #ifndef TRACE_PRINTF
 #define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks()
 #endif
@@ -70,7 +83,7 @@ void print_debug(const char *fmt, ...);
    #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks()
 #endif
 
-//Debugging macros
+// Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__)
    #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n)
@@ -139,17 +152,19 @@ void print_debug(const char *fmt, ...);
 
 #define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str)
 
-//C++ guard
+// C++ guard
 #ifdef __cplusplus
     extern "C" {
 #endif
 
-//Debug related functions
+// Debug related functions
 void init_debug(uint32_t baudrate);
 
+void print_debug(const char *fmt, ...);
+
 void print_debug_array(const char *prepend, const void *data, size_t length);
 
-//Deprecated definitions
+// Deprecated definitions
 #define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF
 
 // C++ guard
diff --git a/platformio/stima_v4/slave-wind/include/debug_F.h b/platformio/stima_v4/slave-wind/include/debug_F.h
index c354940b4..9fa10dfb6 100644
--- a/platformio/stima_v4/slave-wind/include/debug_F.h
+++ b/platformio/stima_v4/slave-wind/include/debug_F.h
@@ -1,25 +1,32 @@
-/**@file debug_F.h */
-
-/*********************************************************************
-<h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
-authors:
-Marco Baldinetti <marco.baldinetti@digiteco.it>
-
-This program 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 2
-of the License, or (at your option) any later version.
-
-This program 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 this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-<http://www.gnu.org/licenses/>.
-**********************************************************************/
+/**
+ ******************************************************************************
+ * @file    debug_F.h
+ * @author  Marco Baldinetti <m.baldinetti@digiteco.it>
+ * @author  Moreno Gasperini <m.gasperini@digiteco.it>
+ * @brief   Debug constants and functions
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it</center></h2>
+ * <h2><center>All rights reserved.</center></h2>
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * <http://www.gnu.org/licenses/>.
+ *
+ ******************************************************************************
+*/
 
 #ifndef _DEBUG_F_H
 #define _DEBUG_F_H
@@ -27,13 +34,26 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //Dependencies
 #include "debug.h"
 
+#include <STM32FreeRTOS.h>
+#include "../../freertos-cpp/src/thread.hpp"
+#include "../../freertos-cpp/src/semaphore.hpp"
+#include "../../freertos-cpp/src/queue.hpp"
+
+using namespace cpp_freertos;
+
 void print_debug_F(const __FlashStringHelper *fmt, ...);
+void queue_debug_F(Queue *dataLogPutQueue, const __FlashStringHelper *fmt, ...);
 
 //Trace output redirection
 #ifndef TRACE_PRINTF_F
 #define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__)
 #endif
 
+//Trace output redirection
+#ifndef TRACE_LOG_F
+#define TRACE_LOG_F(...) queue_debug_F(__VA_ARGS__)
+#endif
+
 //Debugging macros
 #if (TRACE_LEVEL >= TRACE_LEVEL_FATAL)
    #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__)
@@ -43,42 +63,42 @@ void print_debug_F(const __FlashStringHelper *fmt, ...);
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_ERROR)
    #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_ERROR_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_ERROR_F(...)
+   #define LOG_ERROR_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_WARNING)
    #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_WARNING_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_WARNING_F(...)
+   #define LOG_WARNING_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_INFO)
    #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_INFO_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_INFO_F(...)
+   #define LOG_INFO_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
    #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_DEBUG_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_DEBUG_F(...)
+   #define LOG_DEBUG_F(...)
 #endif
 
 #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE)
    #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__)
+   #define LOG_VERBOSE_F(...) TRACE_LOG_F(__VA_ARGS__)
 #else
    #define TRACE_VERBOSE_F(...)
-#endif
-
-//C++ guard
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// C++ guard
-#ifdef __cplusplus
-}
+   #define LOG_VERBOSE_F(...)
 #endif
 
 #endif