diff --git a/.travis.yml b/.travis.yml
index 29957e7..b0ebc5d 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -48,8 +48,6 @@ jobs:
     - name: platformio
       script: platformio run
       language: python
-      python:
-      - "2.7"
       install:
       - pip install -U platformio
       - platformio update
diff --git a/Inc/comms.h b/Inc/comms.h
new file mode 100644
index 0000000..674e98b
--- /dev/null
+++ b/Inc/comms.h
@@ -0,0 +1,113 @@
+/**
+  * This file is part of the hoverboard-firmware-hack project.
+  *
+  * Copyright (C) 2020-2021 Emanuel FERU <aerdronix@gmail.com>
+  *
+  * 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 3 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, see <http://www.gnu.org/licenses/>.
+*/
+
+// Define to prevent recursive inclusion
+#ifndef COMMS_H
+#define COMMS_H
+
+#include "stm32f1xx_hal.h"
+
+#if defined(DEBUG_SERIAL_PROTOCOL)
+
+enum types {UINT8_T,UINT16_T,UINT32_T,INT8_T,INT16_T,INT32_T,INT,FLOAT};
+#define typename(x) _Generic((x), \
+    uint8_t:    UINT8_T, \
+    uint16_t:   UINT16_T, \
+    uint32_t:   UINT32_T, \
+    int8_t:     INT8_T, \
+    int16_t:    INT16_T, \
+    int32_t:    INT32_T, \
+    int:        INT, \
+    float:      FLOAT)
+
+#define PARAM_SIZE(param) sizeof(param) / sizeof(parameter_entry)
+#define COMMAND_SIZE(command) sizeof(command) / sizeof(command_entry)
+
+#define SIZEP(x) ((char*)(&(x) + 1) - (char*)&(x))
+#define ADD_PARAM(var) typename(var),&var
+
+
+int32_t extToInt(uint8_t index,int32_t value);
+int8_t  setParamValInt(uint8_t index, int32_t newValue);
+int8_t  setParamValExt(uint8_t index, int32_t newValue);
+int32_t intToExt(uint8_t index,int32_t value);
+int32_t getParamValInt(uint8_t index);
+int32_t getParamValExt(uint8_t index);
+
+int8_t initParamVal(uint8_t index);
+int8_t incrParamVal(uint8_t index);
+
+int8_t saveAllParamVal();
+int16_t getParamInitInt(uint8_t index);
+int32_t getParamInitExt(uint8_t index);
+int8_t printCommandHelp(uint8_t index);
+int8_t printParamHelp(uint8_t index);
+int8_t printAllParamHelp();
+int8_t printParamVal();
+int8_t printParamDef(uint8_t index);
+int8_t printAllParamDef();
+void printError(uint8_t errornum );
+int8_t watchParamVal(uint8_t index);
+
+int8_t findCommand(uint8_t *userCommand, uint32_t len);
+int8_t findParam(uint8_t *userCommand, uint32_t len);
+void handle_input(uint8_t *userCommand, uint32_t len);
+void process_debug();
+
+
+typedef struct debug_command_struct debug_command;
+struct debug_command_struct {
+  uint8_t semaphore;
+  uint8_t error;
+  int8_t command_index;
+  int8_t param_index;
+  int32_t param_value;
+};
+
+typedef struct command_entry_struct command_entry;
+struct command_entry_struct {
+  const uint8_t type;
+  const char *name;
+  int8_t (*callback_function0)();
+  int8_t (*callback_function1)(uint8_t index);
+  int8_t (*callback_function2)(uint8_t index,int32_t value);
+  const char *help;
+};
+
+typedef struct parameter_entry_struct parameter_entry;
+struct parameter_entry_struct {
+  const uint8_t type;
+  const char *name;
+  const uint8_t datatype;
+  void *valueL;
+  void *valueR;
+  const uint16_t addr;
+  const int32_t init;
+  const uint8_t initFormat;
+  const int32_t min;
+  const int32_t max;
+  const uint8_t div;
+  const uint8_t mul;
+  const uint8_t fix;
+  void (*callback_function)();
+  const char *help;
+};
+
+#endif  // DEBUG_SERIAL_PROTOCOL
+#endif  // COMMS_H
\ No newline at end of file
diff --git a/Inc/config.h b/Inc/config.h
index 45b2a9d..28775f3 100644
--- a/Inc/config.h
+++ b/Inc/config.h
@@ -172,6 +172,7 @@
 #define ADC_MARGIN                100     // ADC input margin applied on the raw ADC min and max to make sure the MIN and MAX values are reached even in the presence of noise
 #define ADC_PROTECT_TIMEOUT       100     // ADC Protection: number of wrong / missing input commands before safety state is taken
 #define ADC_PROTECT_THRESH        200     // ADC Protection threshold below/above the MIN/MAX ADC values
+#define AUTO_CALIBRATION_ENA              // Enable/Disable input auto-calibration by holding power button pressed. Un-comment this if auto-calibration is not needed.
 
 /* FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16
  * Value of COEFFICIENT is in fixdt(1,16,14)
@@ -227,7 +228,7 @@
  * enable DEBUG_SERIAL_USART3 or DEBUG_SERIAL_USART2
  *
  *
- * DEBUG_SERIAL_ASCII output is:
+ * DEBUG ASCII output is:
  * // "in1:345 in2:1337 cmdL:0 cmdR:0 BatADC:0 BatV:0 TempADC:0 Temp:0\r\n"
  *
  * in1:     (int16_t)input1[inIdx].raw);                                        raw input1: ADC1, UART, PWM, PPM, iBUS
@@ -243,6 +244,7 @@
 
 // #define DEBUG_SERIAL_USART2          // left sensor board cable, disable if ADC or PPM is used!
 // #define DEBUG_SERIAL_USART3          // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
+// #define DEBUG_SERIAL_PROTOCOL        // uncomment this to send user commands to the board, change parameters and print specific signals (see comms.c for the user commands)
 // ########################### END OF DEBUG SERIAL ############################
 
 
diff --git a/Makefile b/Makefile
index 3d4cad7..e8e0f3d 100644
--- a/Makefile
+++ b/Makefile
@@ -38,6 +38,7 @@ Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c \
 Src/system_stm32f1xx.c \
 Src/setup.c \
 Src/control.c \
+Src/comms.c \
 Src/util.c \
 Src/main.c \
 Src/bldc.c \
diff --git a/Src/comms.c b/Src/comms.c
new file mode 100644
index 0000000..c6f8966
--- /dev/null
+++ b/Src/comms.c
@@ -0,0 +1,640 @@
+/**
+  * This file is part of the hoverboard-firmware-hack project.
+  *
+  * Copyright (C) 2020-2021 Emanuel FERU <aerdronix@gmail.com>
+  *
+  * 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 3 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, see <http://www.gnu.org/licenses/>.
+*/
+
+// Includes
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "stm32f1xx_hal.h"
+#include "config.h"
+#include "defines.h"
+#include "eeprom.h"
+#include "BLDC_controller.h"
+#include "util.h"
+#include "comms.h"
+
+#if defined(DEBUG_SERIAL_PROTOCOL)
+#if defined(DEBUG_SERIAL_PROTOCOL) && (defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3))
+
+#ifdef CONTROL_ADC
+  #define RAW_MIN 0
+  #define RAW_MAX 4095
+#else
+  #define RAW_MIN -1000
+  #define RAW_MAX 1000
+#endif
+
+
+#define MAX_PARAM_WATCH 15
+
+extern ExtY rtY_Left;                   /* External outputs */
+extern ExtU rtU_Left;                   /* External inputs */
+extern P    rtP_Left;
+
+extern ExtY rtY_Right;                  /* External outputs */
+extern ExtU rtU_Right;                      /* External inputs */
+extern P    rtP_Right;
+
+
+extern InputStruct input1[];            // input structure
+extern InputStruct input2[];            // input structure
+
+extern uint16_t VirtAddVarTab[NB_OF_VAR];
+extern int16_t  speedAvg;                      // average measured speed
+extern int16_t  speedAvgAbs;                   // average measured speed in absolute
+extern uint8_t  ctrlModReqRaw;
+extern adc_buf_t adc_buffer;
+extern int16_t board_temp_deg_c;
+
+
+enum commandTypes {READ,WRITE};
+// Function0 - Function with 0 parameter
+// Function1 - Function with 1 parameter (e.g. GET PARAM)
+// Function2 - Function with 2 parameter (e.g. SET PARAM XXXX)
+const command_entry commands[] = {
+  // Type   ,Name      ,Function0         ,Function1       ,Function2      ,Help     
+    {READ   ,"GET"     ,printAllParamDef  ,printParamDef   ,NULL           ,"Get Parameter/Variable"},
+    {READ   ,"HELP"    ,printAllParamHelp ,printParamHelp  ,NULL           ,"Command/Parameter/Variable Help"},
+    {READ   ,"WATCH"   ,NULL              ,watchParamVal   ,NULL           ,"Toggle Parameter/Variable Watch"},
+    {WRITE  ,"SET"     ,NULL              ,NULL            ,setParamValExt ,"Set Parameter"},
+    {WRITE  ,"INIT"    ,NULL              ,initParamVal    ,NULL           ,"Init Parameter from EEPROM or CONFIG.H"},
+    {WRITE  ,"SAVE"    ,saveAllParamVal   ,NULL            ,NULL           ,"Save Parameters to EEPROM"},
+};
+
+enum paramTypes {PARAMETER,VARIABLE};
+const parameter_entry params[] = {
+  // CONTROL PARAMETERS
+  // Type       ,Name                 ,Datatype ,ValueL ptr                  ,ValueR                    ,EEPRM Addr ,Init              Int/Ext ,Min    ,Max    ,Div             ,Mul  ,Fix   ,Callback Function  ,Help text
+    {PARAMETER  ,"CTRL_MOD"           ,ADD_PARAM(ctrlModReqRaw)              ,NULL                      ,0          ,CTRL_MOD_REQ      ,0      ,1      ,3      ,0               ,0    ,0     ,NULL               ,"Ctrl mode 1:VLT 2:SPD 3:TRQ"},
+    {PARAMETER  ,"CTRL_TYP"           ,ADD_PARAM(rtP_Left.z_ctrlTypSel)      ,&rtP_Right.z_ctrlTypSel   ,0          ,CTRL_TYP_SEL      ,0      ,0      ,2      ,0               ,0    ,0     ,NULL               ,"Ctrl type 0:COM 1:SIN 2:FOC"},
+    {PARAMETER  ,"I_MOT_MAX"          ,ADD_PARAM(rtP_Left.i_max)             ,&rtP_Right.i_max          ,1          ,I_MOT_MAX         ,1      ,1      ,40     ,A2BIT_CONV      ,0    ,4     ,NULL               ,"Max phase current A"},
+    {PARAMETER  ,"N_MOT_MAX"          ,ADD_PARAM(rtP_Left.n_max)             ,&rtP_Right.n_max          ,2          ,N_MOT_MAX         ,1      ,10     ,2000   ,0               ,0    ,4     ,NULL               ,"Max motor RPM"},
+    {PARAMETER  ,"FI_WEAK_ENA"        ,ADD_PARAM(rtP_Left.b_fieldWeakEna)    ,&rtP_Right.b_fieldWeakEna ,0          ,FIELD_WEAK_ENA    ,0      ,0      ,1      ,0               ,0    ,0     ,NULL               ,"Enable field weak"},
+  	{PARAMETER  ,"FI_WEAK_HI"         ,ADD_PARAM(rtP_Left.r_fieldWeakHi)     ,&rtP_Right.r_fieldWeakHi  ,0          ,FIELD_WEAK_HI     ,1      ,0      ,1500   ,0               ,0    ,4     ,Input_Lim_Init     ,"Field weak high RPM"},
+	  {PARAMETER  ,"FI_WEAK_LO"         ,ADD_PARAM(rtP_Left.r_fieldWeakLo)     ,&rtP_Right.r_fieldWeakLo  ,0          ,FIELD_WEAK_LO     ,1      ,0      ,1000   ,0               ,0    ,4     ,Input_Lim_Init     ,"Field weak low RPM"},
+    {PARAMETER  ,"FI_WEAK_MAX"        ,ADD_PARAM(rtP_Left.id_fieldWeakMax)   ,&rtP_Right.id_fieldWeakMax,0          ,FIELD_WEAK_MAX    ,1      ,0      ,20     ,A2BIT_CONV      ,0    ,4     ,NULL               ,"Field weak max current A(FOC)"},
+    {PARAMETER  ,"PHA_ADV_MAX"        ,ADD_PARAM(rtP_Left.a_phaAdvMax)       ,&rtP_Right.a_phaAdvMax    ,0          ,PHASE_ADV_MAX     ,1      ,0      ,55     ,0               ,0    ,4     ,NULL               ,"Max Phase Adv angle Deg(SIN)"},     
+  // INPUT PARAMETERS
+  // Type       ,Name                 ,ValueL ptr                            ,ValueR                    ,EEPRM Addr ,Init              Int/Ext ,Min    ,Max    ,Div             ,Mul  ,Fix   ,Callback Function  ,Help text
+    {VARIABLE   ,"IN1_RAW"            ,ADD_PARAM(input1[0].raw)              ,NULL                      ,0          ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input1 raw"},        
+    {PARAMETER  ,"IN1_TYP"            ,ADD_PARAM(input1[0].typ)              ,NULL                      ,3          ,0                 ,0      ,0      ,3      ,0               ,0    ,0     ,0                  ,"Input1 type"},        
+    {PARAMETER  ,"IN1_MIN"            ,ADD_PARAM(input1[0].min)              ,NULL                      ,4          ,RAW_MIN           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input1 min"},        
+    {PARAMETER  ,"IN1_MID"            ,ADD_PARAM(input1[0].mid)              ,NULL                      ,5          ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input1 mid"},
+    {PARAMETER  ,"IN1_MAX"            ,ADD_PARAM(input1[0].max)              ,NULL                      ,6          ,RAW_MAX           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input1 max"},        
+    {VARIABLE   ,"IN1_CMD"            ,ADD_PARAM(input1[0].cmd)              ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,0                  ,"Input1 cmd"},        
+    
+    {VARIABLE   ,"IN2_RAW"            ,ADD_PARAM(input2[0].raw)              ,NULL                      ,0          ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input2 raw"},   
+    {PARAMETER  ,"IN2_TYP"            ,ADD_PARAM(input2[0].typ)              ,NULL                      ,7          ,0                 ,0      ,0      ,3      ,0               ,0    ,0     ,0                  ,"Input2 type"},        
+    {PARAMETER  ,"IN2_MIN"            ,ADD_PARAM(input2[0].min)              ,NULL                      ,8          ,RAW_MIN           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input2 min"},        
+    {PARAMETER  ,"IN2_MID"            ,ADD_PARAM(input2[0].mid)              ,NULL                      ,9          ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input2 mid"},
+    {PARAMETER  ,"IN2_MAX"            ,ADD_PARAM(input2[0].max)              ,NULL                      ,10         ,RAW_MAX           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Input2 max"},
+    {VARIABLE   ,"IN2_CMD"            ,ADD_PARAM(input2[0].cmd)              ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,0                  ,"Input2 cmd"},
+#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2)  
+  // Type       ,Name                 ,ValueL ptr                            ,ValueR                    ,EEPRM Addr ,Init              Int/Ext ,Min    ,Max    ,Div             ,Mul  ,Fix   ,Callback Function  ,Help text
+    {VARIABLE   ,"AUX_IN1_RAW"        ,ADD_PARAM(input1[1].raw)              ,NULL                      ,0          ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input1 raw"},        
+    {PARAMETER  ,"AUX_IN1_TYP"        ,ADD_PARAM(input1[1].typ)              ,NULL                      ,11         ,0                 ,0      ,0      ,3      ,0               ,0    ,0     ,0                  ,"Aux. input1 type"},        
+    {PARAMETER  ,"AUX_IN1_MIN"        ,ADD_PARAM(input1[1].min)              ,NULL                      ,12         ,RAW_MIN           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input1 min"},        
+    {PARAMETER  ,"AUX_IN1_MID"        ,ADD_PARAM(input1[1].mid)              ,NULL                      ,13         ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input1 mid"},
+    {PARAMETER  ,"AUX_IN1_MAX"        ,ADD_PARAM(input1[1].max)              ,NULL                      ,14         ,RAW_MAX           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input1 max"},        
+    {VARIABLE   ,"AUX_IN1_CMD"        ,ADD_PARAM(input1[1].cmd)              ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,0                  ,"Aux. input1 cmd"},        
+    
+    {VARIABLE   ,"AUX_IN2_RAW"        ,ADD_PARAM(input2[1].raw)              ,NULL                      ,0          ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input2 raw"},        
+    {PARAMETER  ,"AUX_IN2_TYP"        ,ADD_PARAM(input2[1].typ)              ,NULL                      ,15         ,0                 ,0      ,0      ,3      ,0               ,0    ,0     ,0                  ,"Aux. input2 type"},        
+    {PARAMETER  ,"AUX_IN2_MIN"        ,ADD_PARAM(input2[1].min)              ,NULL                      ,16         ,RAW_MIN           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input2 min"},        
+    {PARAMETER  ,"AUX_IN2_MID"        ,ADD_PARAM(input2[1].mid)              ,NULL                      ,17         ,0                 ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input2 mid"},
+    {PARAMETER  ,"AUX_IN2_MAX"        ,ADD_PARAM(input2[1].max)              ,NULL                      ,18         ,RAW_MAX           ,0      ,RAW_MIN,RAW_MAX,0               ,0    ,0     ,0                  ,"Aux. input2 max"},
+    {VARIABLE   ,"AUX_IN2_CMD"        ,ADD_PARAM(input2[1].cmd)              ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,0                  ,"Aux. input2 cmd"},
+#endif  
+  // FEEDBACK
+  // Type       ,Name                 ,Datatype, ValueL ptr                  ,ValueR                    ,EEPRM Addr ,Init              Int/Ext ,Min    ,Max    ,Div             ,Mul  ,Fix   ,Callback Function  ,Help text
+    {VARIABLE   ,"I_DC_LINK"          ,ADD_PARAM(rtU_Left.i_DCLink)          ,&rtU_Right.i_DCLink       ,0          ,0                 ,0      ,0      ,0      ,A2BIT_CONV      ,100  ,0     ,NULL               ,"DC Link current A *100"},
+    {VARIABLE   ,"SPD_AVG"            ,ADD_PARAM(speedAvg)                   ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Motor Measured Avg RPM"},
+    {VARIABLE   ,"SPDL"               ,ADD_PARAM(rtY_Left.n_mot)             ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Left Motor Measured RPM"},
+    {VARIABLE   ,"SPDR"               ,ADD_PARAM(rtY_Right.n_mot)            ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Right Motor Measured RPM"},
+    {VARIABLE   ,"RATE"               ,0       , NULL                        ,NULL                      ,0          ,RATE              ,0      ,0      ,0      ,0               ,0    ,4     ,NULL               ,"Rate *10"},
+    {VARIABLE   ,"SPD_COEF"           ,0       , NULL                        ,NULL                      ,0          ,SPEED_COEFFICIENT ,0      ,0      ,0      ,0               ,10   ,14    ,NULL               ,"Speed Coefficient *10"},
+    {VARIABLE   ,"STR_COEF"           ,0       , NULL                        ,NULL                      ,0          ,STEER_COEFFICIENT ,0      ,0      ,0      ,0               ,10   ,14    ,NULL               ,"Steer Coefficient *10"},
+    {VARIABLE   ,"BATV"               ,ADD_PARAM(adc_buffer.batt1)           ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Battery voltage *100"},       
+  //{VARIABLE   ,"TEMP"               ,ADD_PARAM(board_temp_deg_c)           ,NULL                      ,0          ,0                 ,0      ,0      ,0      ,0               ,0    ,0     ,NULL               ,"Temperature °C *10"},       
+
+};
+
+
+const char *errors[9] = {
+  "Command not found", // Err1
+  "Parameter not found", // Err2
+  "This command cannot be used with a Variable", // Err3
+  "Value not in range", // Err4
+  "Value expected", // Err5
+  "Start of line expected", // Err6
+  "End of line expected", // Err7
+  "Parameter expected", // Err8
+  "Uncaught error" // Err9
+  "Watch list is full" // Err10
+};
+
+debug_command command;
+int8_t watchParamList[MAX_PARAM_WATCH] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; 
+
+// Set Param with Value from external format
+int8_t setParamValExt(uint8_t index, int32_t value) {   
+  int8_t ret = 0;
+  // check min and max before conversion to internal values
+  if (IN_RANGE(value,params[index].min,params[index].max)){
+    ret = setParamValInt(index,extToInt(index,value));
+    printParamDef(index);
+  }else{
+    printError(4); // Error - Value out of range
+  }
+  return ret;
+}
+
+// Set Param with value from internal format
+int8_t setParamValInt(uint8_t index, int32_t newValue) {
+  int32_t oldValue = getParamValInt(index);
+  if (oldValue != newValue){ 
+    // if value is different, beep, cast and assign new value
+    switch (params[index].datatype){
+      case UINT8_T:
+        if (params[index].valueL != NULL) *(uint8_t*)params[index].valueL = newValue;
+        if (params[index].valueR != NULL) *(uint8_t*)params[index].valueR = newValue;
+        break;
+      case UINT16_T:
+        if (params[index].valueL != NULL) *(uint16_t*)params[index].valueL = newValue; 
+        if (params[index].valueR != NULL) *(uint16_t*)params[index].valueR = newValue;
+        break;
+      case UINT32_T:
+        if (params[index].valueL != NULL) *(uint32_t*)params[index].valueL = newValue; 
+        if (params[index].valueR != NULL) *(uint32_t*)params[index].valueR = newValue;
+        break;
+      case INT8_T:
+        if (params[index].valueL != NULL) *(int8_t*)params[index].valueL = newValue; 
+        if (params[index].valueR != NULL) *(int8_t*)params[index].valueR = newValue;
+        break;
+      case INT16_T:
+        if (params[index].valueL != NULL) *(int16_t*)params[index].valueL = newValue; 
+        if (params[index].valueR != NULL) *(int16_t*)params[index].valueR = newValue;
+        break;
+      case INT32_T:
+        if (params[index].valueL != NULL) *(int32_t*)params[index].valueL = newValue; 
+        if (params[index].valueR != NULL) *(int32_t*)params[index].valueR = newValue;
+        break;
+    }
+
+    // Beep if value was modified
+    beepShort(5);
+  }
+
+  // Run callback function if assigned
+  if (params[index].callback_function) (*params[index].callback_function)();
+  return 1;
+}
+
+// Get Parameter Internal value and translate to external 
+int32_t getParamValExt(uint8_t index) {
+  return intToExt(index,getParamValInt(index));
+}
+
+// Get Parameter Internal Value
+int32_t getParamValInt(uint8_t index) {
+  int32_t value = 0;
+
+  int8_t countVar = 0;
+  if (params[index].valueL != NULL) countVar++;
+  if (params[index].valueR != NULL) countVar++;
+
+  if (countVar > 0){
+    // Read Left and Right values and calculate average 
+    // If left and right have to be summed up, DIV field could be adapted to multiply by 2
+    // Cast to parameter datatype
+    switch (params[index].datatype){
+      case UINT8_T:
+        if (params[index].valueL != NULL) value += *(uint8_t*)params[index].valueL;
+        if (params[index].valueR != NULL) value += *(uint8_t*)params[index].valueR;
+        break;
+      case UINT16_T:
+        if (params[index].valueL != NULL) value += *(uint16_t*)params[index].valueL;
+        if (params[index].valueR != NULL) value += *(uint16_t*)params[index].valueR;
+        break;
+      case UINT32_T:
+        if (params[index].valueL != NULL) value += *(uint32_t*)params[index].valueL;
+        if (params[index].valueR != NULL) value += *(uint32_t*)params[index].valueR;
+        break;
+      case INT8_T:
+        if (params[index].valueL != NULL) value += *(int8_t*)params[index].valueL;
+        if (params[index].valueR != NULL) value += *(int8_t*)params[index].valueR;
+        break;
+      case INT16_T:
+        if (params[index].valueL != NULL) value += *(int16_t*)params[index].valueL;
+        if (params[index].valueR != NULL) value += *(int16_t*)params[index].valueR;
+        break;
+      case INT32_T:
+        if (params[index].valueL != NULL) value += *(int32_t*)params[index].valueL;
+        if (params[index].valueR != NULL) value += *(int32_t*)params[index].valueR;
+        break;
+      default:
+        value = 0;
+    }
+
+    // Divide by number of values provided for the parameter
+    value /= countVar;
+  }else{
+    // No variable was provided, return init value that might contain a macro
+    value = params[index].init;
+  }
+
+  return value;
+}
+
+// Add or remove parameter from watch list
+int8_t watchParamVal(uint8_t index){
+  int8_t i,found = 0;
+  for(i=0;i < MAX_PARAM_WATCH && watchParamList[i]>-1;i++){
+    if (watchParamList[i] == index) found = 1;
+    if (found) watchParamList[i] = (i < MAX_PARAM_WATCH-1)?watchParamList[i+1]:-1;
+  }
+  if (!found){
+    if (watchParamList[i] == -1){
+      watchParamList[i] = index;
+      return 1;
+    }
+    printError(10);
+    return 0;
+  }
+  return 1;
+}
+
+// Print value for all parameters with watch flag
+int8_t printParamVal(){
+  int8_t i = 0; 
+  for(i=0;i < MAX_PARAM_WATCH && watchParamList[i]>-1;i++){
+    printf("%s:%li ",params[watchParamList[i]].name,getParamValExt(watchParamList[i]));
+  }
+  if (i>0) printf("\r\n");
+  return 1;
+}
+
+// Print help for Command
+int8_t printCommandHelp(uint8_t index){
+  printf("? %s:\"%s\"\r\n",commands[index].name,commands[index].help);
+  return 1;
+}
+
+// Print help for parameter
+int8_t printParamHelp(uint8_t index){
+  printf("? %s:\"%s\" ",params[index].name,params[index].help);
+  if (params[index].type == PARAMETER) printf("[min:%li max:%li]",params[index].min,params[index].max);
+  printf("\r\n");
+  return 1;
+}
+
+// Print help for all parameters
+int8_t printAllParamHelp(){
+  printf("? Commands\r\n");
+  for(int i=0;i<COMMAND_SIZE(commands);i++)
+    printCommandHelp(i);
+  printf("?\r\n");
+
+  printf("? Parameters\r\n");
+  for(int i=0;i<PARAM_SIZE(params);i++){
+    if (params[i].type == PARAMETER) printParamHelp(i);
+  }
+  printf("?\r\n");
+
+  printf("? Variables\r\n");
+  for(int i=0;i<PARAM_SIZE(params);i++){
+    if (params[i].type == VARIABLE) printParamHelp(i);
+  }
+  printf("?\r\n");
+
+  return 1;
+}
+
+// Print definition(name,value,initial value, min, max) for parameter
+int8_t printParamDef(uint8_t index){
+  printf("# name:\"%s\" value:%li init:%li min:%li max:%li\r\n",
+         params[index].name,     // Parameter Name
+         getParamValExt(index),  // Parameter Value translated to external format
+         getParamInitExt(index), // Parameter Init Value translated to external format
+         params[index].min,      // Parameter Min Value with External format 
+         params[index].max);     // Parameter Max Value with External format
+  return 1;
+}
+
+// Print definition(name,value,initial value, min, max) for all parameters
+int8_t printAllParamDef(){
+  for(int i=0;i<PARAM_SIZE(params);i++) printParamDef(i);
+  return 1;
+}
+
+void printError(uint8_t errornum ){
+  printf("! Err%i:\"%s\"\r\n",errornum,errors[errornum-1]);
+}
+
+// Function to increment a value
+// Get Parameter in External format, check max value, increment, set Parameter
+// Not used in the protocol yet 
+int8_t incrParamVal(uint8_t index) {
+  // This should be used only if min and max values are known
+  if (params[index].min == params[index].max) return 0;
+  
+  uint32_t value = getParamValExt(index);
+  if (value < params[index].max){
+    return setParamValExt(index,value + 1);
+  }else{
+    return setParamValExt(index,(int32_t) params[index].min);
+  } 
+}
+
+// Get internal Parameter value and save it to EEprom for all paraemeter with an address assigned 
+int8_t saveAllParamVal() {
+  HAL_FLASH_Unlock();
+  EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY);
+  for(int i=0;i<PARAM_SIZE(params);i++){ 
+    // Only Parameters with eeprom address can be saved
+    if (params[i].addr){
+      EE_WriteVariable(VirtAddVarTab[params[i].addr] , (uint16_t)getParamValInt(i));    
+    }
+  }
+  HAL_FLASH_Lock();
+  return 1;
+}
+
+// Translate from Internal to External format
+int32_t intToExt(uint8_t index,int32_t value){
+  // Multiply for small number
+  if(params[index].mul) value *= params[index].mul;
+  // Divide to translate to external format
+  if(params[index].div) value /= params[index].div;
+  // Shift to translate to external format
+  if(params[index].fix) value >>= params[index].fix;
+  return value;
+}
+
+// Translate from External to Internal Format
+int32_t extToInt(uint8_t index,int32_t value){
+  // Multiply to translate to internal format
+  if(params[index].div) value *= params[index].div;
+  // Shift to translate to internal format
+  if (params[index].fix) value <<= params[index].fix;
+  // Divide for small number
+  if(params[index].mul) value /= params[index].mul;
+  return value;
+}
+
+// Get Parameter Init value(EEPROM or init/config.h) and translate to external format
+int32_t getParamInitExt(uint8_t index) {
+  return intToExt(index,getParamInitInt(index));
+}
+
+// Get Parameter value with EEprom data if address is avalaible, init/config.h value otherwise
+int16_t getParamInitInt(uint8_t index){
+  if (params[index].addr){
+    // if EEPROM address is specified, init from EEPROM address
+    uint16_t writeCheck, readVal;
+    
+    HAL_FLASH_Unlock();
+    EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
+    EE_ReadVariable(VirtAddVarTab[params[index].addr] , &readVal);
+    HAL_FLASH_Lock();
+    
+    // EEPROM was written, use stored value
+    if (writeCheck == FLASH_WRITE_KEY){
+      return readVal;
+    }else{
+      // Use init value from array
+      if (params[index].initFormat){
+        // Init Value is in External format (e.g. PHA_ADV_MAX is 25 deg)
+        return extToInt(index,params[index].init);
+      }else{
+        return params[index].init;
+      }
+    }
+  }else{
+    if (params[index].initFormat){
+      // Init Value is in External format (e.g. PHA_ADV_MAX is 25 deg)
+      return extToInt(index,params[index].init);
+    }else{
+      return params[index].init;
+    }
+  }
+}
+
+
+// initialize Parameter value with EEprom data if address is avalaible, init/config.h value otherwise
+int8_t initParamVal(uint8_t index) {
+  int8_t ret = 0;
+  ret = setParamValInt(index,(int32_t) getParamInitInt(index));
+  printParamDef(index);
+  return ret;
+}
+
+// Find command in commands array and return index
+int8_t findCommand(uint8_t *userCommand, uint32_t len){
+  for(int index=0;index<COMMAND_SIZE(commands);index++){
+    uint8_t command_len = strlen(commands[index].name);
+    if (command_len < len){
+      if (memcmp(userCommand,commands[index].name,command_len)==0){
+        return index;
+      }
+    }
+  }
+  return -1; // Not found
+}
+
+// Find parameter in params array and return index
+int8_t findParam(uint8_t *userCommand, uint32_t len){
+  for(int index=0;index<PARAM_SIZE(params);index++){
+    uint8_t param_len = strlen(params[index].name);
+    if (param_len < len){
+      if (memcmp(userCommand,params[index].name,param_len)==0){
+        return index;
+      }
+    }
+  }
+  return -1; // Not found
+}
+
+// Parse and save the command to be executed
+void handle_input(uint8_t *userCommand, uint32_t len)
+{
+
+  // If there is already an unprocessed command, exit
+  if (command.semaphore == 1) return;
+
+  // Check end of line
+  userCommand+=len-1; // Go to last char
+  if (*userCommand != '\n' && *userCommand != '\r'){
+    command.error = 7; // Error - End of line expected
+    return;
+  }
+  userCommand-=len-1; // Come back
+
+  int8_t  cindex = -1;
+  int8_t  pindex = -1;
+  uint8_t size   = 0;
+
+  // Find Command
+  cindex = findCommand(userCommand,len);
+  if (cindex == -1){
+    // Error - Command not found
+    command.error = 1;
+    return;
+  }
+
+  // Skip command characters
+  size = strlen(commands[cindex].name);
+  {len-=size;userCommand+=size;}
+  // Skip if space
+  if (*userCommand == 0x20){len-=1;userCommand+=1;}
+
+  if (*userCommand == '\n' || *userCommand == '\r'){
+    if (commands[cindex].callback_function0 != NULL){
+      // Command without parameter
+      command.semaphore = 1;
+      command.command_index = cindex;
+      command.param_index   = -1;
+      command.param_value   = 0;
+    }else{
+      command.error = 8; // Error - Parameter expected
+    }
+    return;
+  }
+
+  // Find parameter
+  pindex = findParam(userCommand,len);
+  if (pindex == -1){
+    // Error - Parameter not found
+    command.error = 2;
+    return;
+  }
+
+  // Skip parameter characters
+  size = strlen(params[pindex].name);
+  {len-=size;userCommand+=size;}
+  // Skip if space
+  if (*userCommand == 0x20){len-=1;userCommand+=1;}
+   
+  if (commands[cindex].type == WRITE && params[pindex].type == VARIABLE){
+    // Error - This command cannot be used with a Variable
+    command.error = 3;
+    return;
+  }
+  
+  if (commands[cindex].callback_function1 != NULL){
+    if (*userCommand == '\n' || *userCommand == '\r'){
+      // Command with parameter
+      command.semaphore = 1;
+      command.command_index = cindex;
+      command.param_index   = pindex;
+      command.param_value   = 0;
+    }else{
+      command.error = 7; // Error - End of line expected
+    }
+    return;
+  }
+  
+  int32_t value = 0;
+  int8_t  sign  = 1;
+  int8_t  count = 0;
+
+  // Read sign
+  if (*userCommand == '-'){len-=1;userCommand+=1;sign =-1;} 
+  // Read value
+  for (value=0; (unsigned)*userCommand-'0'<10; userCommand++){
+    value = 10*value+(*userCommand-'0');
+    count++;
+    // Error - Value out of range
+    if (value>MAX_int16_T){command.error = 4;return;}
+  }
+
+  if (count == 0){
+    // Error - Value required
+    command.error = 5;
+    return;
+  }
+      
+  // Apply sign
+  value*= sign;
+
+  // Command with parameter and value
+  if (commands[cindex].callback_function2 != NULL){
+    if (*userCommand == '\n' || *userCommand == '\r'){
+      command.semaphore = 1;
+      command.command_index = cindex;
+      command.param_index   = pindex;
+      command.param_value   = value;
+    }else{
+      command.error = 7; // Error - End of line expected
+    }
+    return;
+  }
+
+  // Uncaught error
+  command.error = 9;
+
+}
+
+void process_debug()
+{
+  
+  // Print parameters from watch list
+  printParamVal();
+
+  // Show Error if any
+  if(command.error> 0){
+    printError(command.error);
+    command.error = 0;
+    return;
+  }
+
+  // Nothing to do
+  if (command.semaphore == 0) return;
+
+  int8_t ret = 0;
+  if (commands[command.command_index].callback_function0 != NULL && 
+      command.param_index == -1){
+    // This function needs no parameter
+    ret = (*commands[command.command_index].callback_function0)();
+    if (ret==1){printf("OK\r\n");}
+    command.semaphore = 0;
+    return;
+  }
+
+  if (commands[command.command_index].callback_function1 != NULL &&
+      command.param_index != -1){
+    // This function needs only a parameter
+    ret = (*commands[command.command_index].callback_function1)(command.param_index);
+    if (ret==1){printf("OK\r\n");}
+    command.semaphore = 0;
+    return;
+  }  
+
+  if (commands[command.command_index].callback_function2 != NULL && 
+      command.param_index != -1){
+    // This function needs an additional parameter
+    ret = (*commands[command.command_index].callback_function2)(command.param_index,command.param_value);
+    if (ret==1){printf("OK\r\n");}
+    command.semaphore = 0;
+  }
+}
+
+#endif
+#endif  // DEBUG_SERIAL_PROTOCOL
+
diff --git a/Src/main.c b/Src/main.c
index 3f8c1d4..3d190f8 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -29,6 +29,7 @@
 #include "util.h"
 #include "BLDC_controller.h"      /* BLDC's header file */
 #include "rtwtypes.h"
+#include "comms.h"
 
 #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
 #include "hd44780.h"
@@ -198,6 +199,8 @@ int main(void) {
   int16_t board_temp_adcFilt  = adc_buffer.temp;
   int16_t board_temp_deg_c;
 
+  // Loop until button is released
+  while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
 
   while(1) {
     HAL_Delay(DELAY_IN_MAIN_LOOP);        // delay in ms
@@ -420,16 +423,20 @@ int main(void) {
 
     // ####### DEBUG SERIAL OUT #######
     #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
-      if (main_loop_counter % 25 == 0) {    // Send data periodically every 125 ms
-        printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n",
-          input1[inIdx].raw,        // 1: INPUT1
-          input2[inIdx].raw,        // 2: INPUT2
-          cmdL,                     // 3: output command: [-1000, 1000]
-          cmdR,                     // 4: output command: [-1000, 1000]
-          adc_buffer.batt1,         // 5: for battery voltage calibration
-          batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC, // 6: for verifying battery voltage calibration
-          board_temp_adcFilt,       // 7: for board temperature calibration
-          board_temp_deg_c);        // 8: for verifying board temperature calibration
+      if (main_loop_counter % 25 == 0) {    // Send data periodically every 125 ms      
+        #if defined(DEBUG_SERIAL_PROTOCOL)
+          process_debug();
+        #else
+          printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n",
+            input1[inIdx].raw,        // 1: INPUT1
+            input2[inIdx].raw,        // 2: INPUT2
+            cmdL,                     // 3: output command: [-1000, 1000]
+            cmdR,                     // 4: output command: [-1000, 1000]
+            adc_buffer.batt1,         // 5: for battery voltage calibration
+            batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC, // 6: for verifying battery voltage calibration
+            board_temp_adcFilt,       // 7: for board temperature calibration
+            board_temp_deg_c);        // 8: for verifying board temperature calibration
+        #endif
       }
     #endif
 
diff --git a/Src/util.c b/Src/util.c
index 8f7b2f2..46d5b1e 100644
--- a/Src/util.c
+++ b/Src/util.c
@@ -29,6 +29,7 @@
 #include "util.h"
 #include "BLDC_controller.h"
 #include "rtwtypes.h"
+#include "comms.h"
 
 #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
 #include "hd44780.h"
@@ -231,8 +232,7 @@ static uint8_t standstillAcv = 0;
   #endif
 #endif
 
-
-
+ 
 /* =========================== Initialization Functions =========================== */
 
 void BLDC_Init(void) {
@@ -483,6 +483,7 @@ void calcAvgSpeed(void) {
  * The Values will be saved to flash. Values are persistent if you flash with platformio. To erase them, make a full chip erase.
  */
 void adcCalibLim(void) {
+#ifdef AUTO_CALIBRATION_ENA
   calcAvgSpeed();
   if (speedAvgAbs > 5) {    // do not enter this mode if motors are spinning
     return;
@@ -571,6 +572,7 @@ void adcCalibLim(void) {
   #endif
 
 #endif
+#endif  // AUTO_CALIBRATION_ENA
 }
  /*
  * Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2)
@@ -1063,14 +1065,20 @@ void usart2_rx_check(void)
   #endif
 
   #if defined(DEBUG_SERIAL_USART2)
+  uint8_t *ptr;
   if (pos != old_pos) {                                                 // Check change in received data
     if (pos > old_pos) {                                                // "Linear" buffer mode: check if current position is over previous one
       usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos);        // Process data
     } else {                                                            // "Overflow" buffer mode
-      usart_process_debug(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer
+      ptr = (uint8_t *) malloc(sizeof(uint8_t) * (rx_buffer_L_len - old_pos + pos));
+      memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos);    // First copy data from the end of buffer
       if (pos > 0) {                                                    // Check and continue with beginning of buffer
-        usart_process_debug(&rx_buffer_L[0], pos);                      // Process remaining data
+        ptr += rx_buffer_L_len - old_pos;
+        memcpy(ptr, &rx_buffer_L[0], pos);                              // Copy remaining data
       }
+      ptr -= rx_buffer_L_len - old_pos;
+      usart_process_debug(ptr, rx_buffer_L_len - old_pos + pos);        // Process data
+      free(ptr);
     }
   }
   #endif // DEBUG_SERIAL_USART2
@@ -1133,20 +1141,26 @@ void usart3_rx_check(void)
   #endif
 
   #if defined(DEBUG_SERIAL_USART3)
+  uint8_t *ptr;
   if (pos != old_pos) {                                                 // Check change in received data
     if (pos > old_pos) {                                                // "Linear" buffer mode: check if current position is over previous one
       usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos);        // Process data
     } else {                                                            // "Overflow" buffer mode
-      usart_process_debug(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer
+      ptr = (uint8_t *) malloc(sizeof(uint8_t) * (rx_buffer_R_len - old_pos + pos));
+      memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos);    // First copy data from the end of buffer
       if (pos > 0) {                                                    // Check and continue with beginning of buffer
-        usart_process_debug(&rx_buffer_R[0], pos);                      // Process remaining data
+        ptr += rx_buffer_R_len - old_pos;
+        memcpy(ptr, &rx_buffer_R[0], pos);                              // Copy remaining data
       }
+      ptr -= rx_buffer_R_len - old_pos;
+      usart_process_debug(ptr, rx_buffer_R_len - old_pos + pos);        // Process data
+      free(ptr);
     }
   }
   #endif // DEBUG_SERIAL_USART3
 
   #ifdef CONTROL_SERIAL_USART3
-  uint8_t *ptr;	
+  uint8_t *ptr;
   if (pos != old_pos) {                                                 // Check change in received data
     ptr = (uint8_t *)&commandR_raw;                                     // Initialize the pointer with command_raw address
     if (pos > old_pos && (pos - old_pos) == commandR_len) {             // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
@@ -1195,13 +1209,11 @@ void usart3_rx_check(void)
 #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
 void usart_process_debug(uint8_t *userCommand, uint32_t len)
 {
-  for (; len > 0; len--, userCommand++) {
-    if (*userCommand != '\n' && *userCommand != '\r') {   // Do not accept 'new line' and 'carriage return' commands
-      printf("Command = %c\r\n", *userCommand);
-      // handle_input(*userCommand);                      // -> Create this function to handle the user commands
-    }
-  }
+  #ifdef DEBUG_SERIAL_PROTOCOL
+    handle_input(userCommand, len);
+  #endif
 }
+
 #endif // SERIAL_DEBUG
 
 /*
@@ -1529,9 +1541,11 @@ void poweroffPressCheck(void) {
           updateCurSpdLim();
           beepShort(5);
         } else {                                          // Long press: Calibrate ADC Limits
+          #ifdef AUTO_CALIBRATION_ENA
           beepLong(16); 
           adcCalibLim();
           beepShort(5);
+          #endif
         }
       } else if (cnt_press > 8) {                         // Short press: power off (80 ms debounce)
         poweroff();