Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

14 files changed, 162 insertions, 103 deletions

diff --git a/Software/Embedded_SW/Embedded/Common/protobuf-c/protobuf-c.c b/Software/Embedded_SW/Embedded/Common/protobuf-c/protobuf-c.c
index 5debac820..917022de5 100644
--- a/Software/Embedded_SW/Embedded/Common/protobuf-c/protobuf-c.c
+++ b/Software/Embedded_SW/Embedded/Common/protobuf-c/protobuf-c.c
@@ -778,6 +778,8 @@ uint32_pack(uint32_t value, uint8_t *out)
 {
 	unsigned rv = 0;
 
+    assert(out > 0);
+
 	if (value >= 0x80) {
 		out[rv++] = value | 0x80;
 		value >>= 7;
diff --git a/Software/Embedded_SW/Embedded/Common/report/distributor.c b/Software/Embedded_SW/Embedded/Common/report/distributor.c
index e587e0575..46a3affd6 100644
--- a/Software/Embedded_SW/Embedded/Common/report/distributor.c
+++ b/Software/Embedded_SW/Embedded/Common/report/distributor.c
@@ -700,6 +700,7 @@ STATUS ReportMessage2Dist(DistributorHandle distributor, const char *message)
  *		None
  *
  */
+char reportmsg[254];
 
 static void messageDistribute(char *msg,
                               int DistTableEntry,
@@ -722,11 +723,12 @@ static void messageDistribute(char *msg,
             Dist_Table[DistTableEntry].dest[index].function(msg,FileName,LineNumber,errorCode,parameter1,parameter2);
     }
     */
+    strcpy (reportmsg,msg);
 
     if (ReportFunc1 != NULL)
-        ReportFunc1(msg,  FileName,LineNumber,errorCode,Severity, parameter);
+        ReportFunc1(reportmsg,  FileName,LineNumber,errorCode,Severity, parameter);
     if (ReportFunc2 != NULL)
-        ReportFunc2(msg,  FileName,LineNumber,errorCode,Severity, parameter);
+        ReportFunc2(reportmsg,  FileName,LineNumber,errorCode,Severity, parameter);
 
 }
 
diff --git a/Software/Embedded_SW/Embedded/Communication/CommunicationTask.c b/Software/Embedded_SW/Embedded/Communication/CommunicationTask.c
index 91f1a78aa..b0b5115a2 100644
--- a/Software/Embedded_SW/Embedded/Communication/CommunicationTask.c
+++ b/Software/Embedded_SW/Embedded/Communication/CommunicationTask.c
@@ -16,6 +16,7 @@ Mailbox_Handle          CommunicationTxMsgQ = NULL;
 typedef struct CommRxMessage{
     uint16_t messageId;
     uint16_t msgSize;
+    uint32_t BuffId;
 }CommRxMessageStruc;
 typedef struct CommTxMessage{
     uint16_t messageId;
@@ -30,28 +31,65 @@ struct serialBuffer {
   size_t used;
   size_t size;
 } typedef SerialBuffer;
-
+#define SHORT_BUFFER_SIZE 100
 char CommRxBuffer[COMM_MAX_BUFFER_SIZE];
-SerialBuffer inBuffer;
-uint32_t initArray(size_t initialSize) {
-  SerialBuffer *a = &inBuffer;
+char CommShortRxBuffer[10][SHORT_BUFFER_SIZE];
+SerialBuffer inBuffer[11];
+bool SerialBufferUsed[11] = {false,false,false,false,false,false,false,false,false,false,false,false};
+uint32_t initArray(size_t initialSize)
+{
   if (initialSize >= COMM_MAX_BUFFER_SIZE) return 0;
+  if (initialSize > SHORT_BUFFER_SIZE)
+  {
+      if (SerialBufferUsed[10] == true)
+      {
+          return 0xFF;
+      }
+      else
+      {
+          SerialBufferUsed[10] = true;
+          inBuffer[10].buffer = CommRxBuffer;
+          inBuffer[10].used = 0;
+          inBuffer[10].size = initialSize;
+          return 10;
+      }
+  }
+  else
+  {
+      int i;
+      for (i = 0; i < 10; i++)
+      {
+          if (SerialBufferUsed[i] == false)
+              break;
+      }
+      if (i == 10) return 0xFF;
+      else
+      {
+          SerialBufferUsed[i] = true;
+          inBuffer[i].buffer = CommRxBuffer;
+          inBuffer[i].used = 0;
+          inBuffer[i].size = initialSize;
+          return i;
+      }
+  }
+  /*SerialBuffer *a = &inBuffer;
   a->buffer = CommRxBuffer;
   a->used = 0;
   a->size = initialSize;
-  return initialSize;
+  return initialSize;*/
 }
 
-void insertArray(char element) {
-    SerialBuffer *a = &inBuffer;
+void insertArray(uint32_t buffer,char element)
+{
+    SerialBuffer *a = &inBuffer[buffer];
   // a->used is the number of used entries, because a->array[a->used++] updates a->used only *after* the array has been accessed.
   // Therefore a->used can go up to a->size
   a->buffer[a->used++] = element;
 }
 
-void freeArray(void) {
-    SerialBuffer *a = &inBuffer;
-  a->used = a->size = 0;
+void freeArray(uint32_t buffer)
+{
+     SerialBufferUsed[buffer] = false;
 }
 
 uint32_t CommunicationTaskInit(void)
@@ -62,13 +100,14 @@ uint32_t CommunicationTaskInit(void)
 }
 uint32_t CommTxMsgCounter = 0;
 uint32_t CommRxMsgCounter = 0;
-uint32_t CommunicationTaskMessageReceived(uint16_t msgSize)
+uint32_t CommunicationTaskMessageReceived(uint32_t buffer,uint16_t msgSize)
 {
     CommRxMessageStruc Message;
     CommRxMsgCounter++;
 
     Message.messageId = 1;
     Message.msgSize = msgSize;
+    Message.BuffId = buffer;
     if (CommunicationRxMsgQ != NULL)
         /*retcode =*/ Mailbox_post(CommunicationRxMsgQ , &Message, BIOS_NO_WAIT);
 
@@ -86,13 +125,13 @@ void RegisterReceiveCallback(void (*callback_ptr)(char* buffer, size_t length))
  *  this communication task is created statically in system initialization, in blocking mode
  *  over one of the chosen ommunication methods (USB or Blutooth).
  ******************************************************************************/
+uint32_t cLength[50] = {0};
+byte cindex = 0;
 void communicationTask(UArg arg0, UArg arg1)
 {
 
-    //uint32_t ui32RxCount;
     CommRxMessageStruc Message;
 
-    initArray(1);
 
     //ui32RxCount = 0;
     CommunicationRxMsgQ = Mailbox_create(sizeof(CommRxMessageStruc), 4, NULL,NULL);
@@ -109,11 +148,13 @@ void communicationTask(UArg arg0, UArg arg1)
                 //ui32RxCount += Message.msgSize;
                 if (callback != NULL)
                 {
-                    callback(inBuffer.buffer,inBuffer.used);
+                    cLength[cindex] = Message.msgSize;
+                    if (cindex++>=50)
+                        cindex = 0;
+                    callback(inBuffer[Message.BuffId].buffer,inBuffer[Message.BuffId].used);
                 }
 
-                freeArray();
-                initArray( 1);
+                freeArray(Message.BuffId);
                 break;
             default:
                 break;
@@ -150,27 +191,10 @@ int32_t SetCommunicationPath(bool UARTorUSB)
 void communicationTxTask(UArg arg0, UArg arg1)
 {
 
-    //uint32_t ui32RxCount;
     CommTxMessageStruc Message;
-/*    typedef struct CommRxMessage{
-        uint16_t messageId;
-        uint16_t msgSize;
-        char *Buff;
-    }CommTxMessageStruc;
-*/
-    initArray(1);
-
-    /*#ifdef USE_USB
-         SetCommunicationPath(isUSB);
-    #else
-         SetCommunicationPath(isUART);
-    #endif*/
-    //ui32RxCount = 0;
+    //initArray(1);
     CommunicationTxMsgQ = Mailbox_create(sizeof(CommTxMessageStruc), 20, NULL,NULL);
 
-    /* Block while the device is NOT connected to the USB */
-//    Semaphore_pend(initConnectionSem, BIOS_WAIT_FOREVER);
-
     while(1)
     {
         Mailbox_pend(CommunicationTxMsgQ , &Message, BIOS_WAIT_FOREVER);
diff --git a/Software/Embedded_SW/Embedded/Communication/CommunicationTask.h b/Software/Embedded_SW/Embedded/Communication/CommunicationTask.h
index 50309c899..27907ee07 100644
--- a/Software/Embedded_SW/Embedded/Communication/CommunicationTask.h
+++ b/Software/Embedded_SW/Embedded/Communication/CommunicationTask.h
@@ -13,13 +13,13 @@ void RegisterReceiveCallback(void (*callback_ptr)(char* buffer, size_t length));
 
 uint32_t initArray( size_t initialSize);
 
-void insertArray( char element);
+void insertArray(uint32_t buffer, char element);
 
-void freeArray(void);
+void freeArray(uint32_t buffer);
 
 extern uint32_t CommunicationTaskInit(void);
 
-extern uint32_t CommunicationTaskMessageReceived(uint16_t msgSize);
+extern uint32_t CommunicationTaskMessageReceived(uint32_t buffer,uint16_t msgSize);
 
 extern bool CommunicationTaskSendMessage(char* buffer,size_t length);
 
diff --git a/Software/Embedded_SW/Embedded/Communication/Container.c b/Software/Embedded_SW/Embedded/Communication/Container.c
index 96f5b6a3e..bf27c9a56 100644
--- a/Software/Embedded_SW/Embedded/Communication/Container.c
+++ b/Software/Embedded_SW/Embedded/Communication/Container.c
@@ -61,9 +61,14 @@ MessageContainer createContainer(MessageType type, char* token, protobuf_c_boole
 
 
     uint8_t* response_buffer = malloc((*sizePtr)(response));
-    size_t response_size = (*packPtr)(response, response_buffer);
-    container.data.data = response_buffer;
-    container.data.len = response_size;
+    size_t response_size = 0;
+    if (response_buffer)
+    {
+        response_size = (*packPtr)(response, response_buffer);
+    }
+        container.data.data = response_buffer;
+        container.data.len = response_size;
+
     TxmsgId[Txindex] = type;
     TxLength[Txindex] = response_size;
     if (Txindex++>=50)
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
index ef3933d30..d2392323f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
@@ -179,7 +179,7 @@ bool FPGA_Read_limit_Switches(FPGA_GPI_ENUM Limit_Switch)
             Ls_Right_Screw_Spool.bits.F1_LS_SCREW_LEFT = NO_LIMIT;
             break;
 
-
+/**/
         case GPI_LS_DISPENSER_50_1:
             LM_Status = LS_Dispenser_1_2.bits.F2_LS_DISPENSER_50_1;
             LS_Dispenser_1_2.bits.F2_LS_DISPENSER_50_1 = NO_LIMIT;
@@ -307,11 +307,13 @@ bool FPGA_Read_limit_Switches(FPGA_GPI_ENUM Limit_Switch)
         case GPI_LS_DISPENSER_UP_8:
             LM_Status = LS_Dispenser_7_8.bits.F2_LS_DISPENSER_UP_8;
             LS_Dispenser_7_8.bits.F2_LS_DISPENSER_UP_8 = NO_LIMIT;
-                break;
+                break;/**/
         default :
-            LM_Status = NO_LIMIT;
-                break;
+//            #warning temporary !!!!
+            return NO_LIMIT;
+            break;
     }
+
     return LM_Status;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
index 62df18294..a5c122574 100644
--- a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.c
@@ -74,7 +74,6 @@ uint32_t Read_Speed_Sensor () // //The speed sensor, RMB14SC12BC59N , is 12bit S
     FPGA_SSI_Speed_Sensor_Receive();
     return SpeedSensorResponseS.Speed;
 }
-
 float Calculate_Speed_Sensor_Velocity( void ) // Call the function every 10mSec (read at least once every
 {
 
@@ -101,18 +100,17 @@ float Calculate_Speed_Sensor_Velocity( void ) // Call the function every 10mSec
     }
     else if(Prev_Enc_Position < Enc_Position)
     {
-        if((Enc_Position - Prev_Enc_Position)<10)//noise
+        /*if((Enc_Position - Prev_Enc_Position)<10)//noise
         {
             number_of_pulses = Enc_Position - Prev_Enc_Position;
         }
-        else
+        else*/
             number_of_pulses = Prev_Enc_Position + (0xFFF - Enc_Position);
     }
     else
     {
         number_of_pulses = Prev_Enc_Position - Enc_Position;
     }
-
     //uint32_t sampling_period_mSec = 10;//10mSec
 
     uint32_t pulses_per_rotation = 4096; //  12 bit
@@ -133,7 +131,7 @@ float Calculate_Speed_Sensor_Velocity( void ) // Call the function every 10mSec
     velocity *= 9.5493;//1 Rad/Sec = 9.5493 RPM
  */
     //------------------
-    temp = number_of_pulses * 8.9;//8.9 cm per round
+    temp = number_of_pulses * 8.3;//8.9 cm per round  CHANGED TO 8.3 - Yoav
     //temp*=100; // to send [(cm/sec)*100]
     temp /=pulses_per_rotation;//bits for round
     temp /=0.01;//10mSec - sampling period
diff --git a/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c b/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c
index 04561139e..b4da9afa9 100644
--- a/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c
+++ b/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c
@@ -530,6 +530,8 @@ uint32_t ControlHandler(void *pvCBData, uint32_t ui32Event, uint32_t ui32MsgValu
 
     return(0);
 }
+uint32_t buffId = 0xFF;
+
 void handleRx(void)
 {
     uint32_t ui32Read;
@@ -551,7 +553,9 @@ if (expected_message_size == 0)
         } while(size_bar < 4);
 
        expected_message_size = *(int *)size;
-       initArray( expected_message_size);
+       buffId = initArray( expected_message_size);
+       if (buffId == 0xFF)
+           expected_message_size = 0;
     }
 
     do
@@ -561,14 +565,14 @@ if (expected_message_size == 0)
         // Did we get a character?
         if(ui32Read)
         {
-            insertArray(ui8Char);
+            insertArray(buffId,ui8Char);
             current_message_size++;
         }
 
         if (current_message_size == expected_message_size)
         {
             g_RxCount += current_message_size;
-            CommunicationTaskMessageReceived(g_RxCount);
+            CommunicationTaskMessageReceived(buffId,current_message_size);
             expected_message_size = 0;
             current_message_size = 0;
             break;
diff --git a/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c b/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c
index ec61f414c..b5c61eb46 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c
@@ -199,6 +199,8 @@ void UART_ResetBuffers(void)
 // The UART interrupt handler.
 //
 //*****************************************************************************
+uint32_t U0buffId = 0xFF;
+
 void UARTIntHandler(UArg arg0)
 {
     uint32_t ui32Status;
@@ -235,7 +237,8 @@ void UARTIntHandler(UArg arg0)
                  //uDataLength[uindex] = U0_expected_message_size;
                  //if (uindex++>=50)
                  //    uindex = 0;
-                 if (initArray( U0_expected_message_size) == 0)
+                 U0buffId = initArray( U0_expected_message_size);
+                 if (U0buffId == 0xFF)
                      U0_expected_message_size = 0;
                  U0_size_bar = 0;
                  memcpy (oldsize,size,4);
@@ -246,7 +249,7 @@ void UARTIntHandler(UArg arg0)
 
                  ui8Char = (char)ROM_UARTCharGetNonBlocking(UART_BASE);
                  Uart_rx_TotalCounter++;
-                 insertArray(ui8Char);
+                 insertArray(U0buffId,ui8Char);
                  U0_current_message_size++;
 
              if (U0_current_message_size == U0_expected_message_size)
@@ -254,7 +257,7 @@ void UARTIntHandler(UArg arg0)
                  Clock_setTimeout(UartClock, UartClockTimeout);
                  Clock_start(UartClock);
                  Uart_rx_Counter+=(4+U0_current_message_size);
-                 CommunicationTaskMessageReceived(U0_current_message_size);
+                 CommunicationTaskMessageReceived(U0buffId, U0_current_message_size);
                  U0_expected_message_size = 0;
                  U0_current_message_size = 0;
                  break;
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index f9f075477..7ffba4d69 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -516,15 +516,19 @@ void SendEventNotifications(void)
     responseContainer.continuous = true;
 
     uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
-    size_t container_size = message_container__pack(&responseContainer, container_buffer);
-
-    if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full
+    if (container_buffer)
     {
-        AlarmHandlingToken[0] = 0;
+        size_t container_size = message_container__pack(&responseContainer, container_buffer);
+
+        if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full
+        {
+            AlarmHandlingToken[0] = 0;
+        }
     }
     if (response.events)
         free (response.events);
-    free(responseContainer.data.data);
+    if (responseContainer.data.data)
+        free(responseContainer.data.data);
 }
 
 uint32_t StartEventsNotificationRequestFunc(MessageContainer* requestContainer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index a85e1cfde..495b7bded 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -476,23 +476,26 @@ void SendDiagnostics(void)
     diagnosticsresponseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
     diagnosticsresponseContainer.continuous = true;
 
-    diagnosticscontainer_buffer = 0;
-    diagnosticscontainer_buffer = malloc(message_container__get_packed_size(&diagnosticsresponseContainer));
-    if (diagnosticscontainer_buffer)
+    if (diagnosticsresponseContainer.data.data)
     {
-        size_t container_size = message_container__pack(&diagnosticsresponseContainer, diagnosticscontainer_buffer);
-        //memmap();
-        free(diagnosticsresponseContainer.data.data);
-        //free(DiagnosticsMonitor.dispenserspressure);
-        //free(DiagnosticsMonitor.dispensersmotorsfrequency);
-        if (SendChars(diagnosticscontainer_buffer, container_size) == false) //comm tx mailbox full
+        diagnosticscontainer_buffer = 0;
+        diagnosticscontainer_buffer = malloc(message_container__get_packed_size(&diagnosticsresponseContainer));
+        if (diagnosticscontainer_buffer)
         {
-            DiagnosticsStop();
+            size_t container_size = message_container__pack(&diagnosticsresponseContainer, diagnosticscontainer_buffer);
+            //memmap();
+            //free(DiagnosticsMonitor.dispenserspressure);
+            //free(DiagnosticsMonitor.dispensersmotorsfrequency);
+            if (SendChars(diagnosticscontainer_buffer, container_size) == false) //comm tx mailbox full
+            {
+                DiagnosticsStop();
+            }
         }
-    }
-    else
-    {
-        LOG_ERROR(Task_self(),"malloc failed");
+        else
+        {
+            LOG_ERROR(Task_self(),"malloc failed");
+        }
+        free(diagnosticsresponseContainer.data.data);
     }
     DiagnosticsReset();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index af8932872..70ede0916 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -183,6 +183,7 @@ uint32_t DispenserHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
 
     //close dry air valve in the dispenser
     Valve_Set((Valves_t) (DispenserId+VALVE_2W_MID_AIR_LF), Atm_MidTank_OFF);
+    MotorSetMicroStep(deviceID, MotorsCfg[deviceID].microstep);
 
     responseContainer = createContainer(MESSAGE_TYPE__DispenserHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
     responseContainer.continuous = true;
@@ -230,6 +231,7 @@ uint32_t DispenserHomingRequestFunc(MessageContainer* requestContainer)
                 //open dry air valve in the dispenser
                 Valve_Set((Valves_t) (request->index+VALVE_2W_MID_AIR_LF), Atm_MidTank_ON);
 
+                MotorSetMicroStep(MotorId, 1);
 
                 MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_Id[MotorId], DispenserHomingRequestCallback);
                 return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index f9eb69de7..54a47e927 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -28,6 +28,7 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 #include "modules/General/process.h"
+#include "Control/MillisecTask.h"
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
@@ -209,34 +210,41 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
 return OK;
 }
+float SpeedSamples[MAX_CONTROL_SAMPLES] = {0};
+
 uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
     int index=MAX_THREAD_MOTORS_NUM;
+    int32_t i, avreageSampleValue = 0;
+    //double tempcalcspeed = 0;
+    uint32_t calculated_speed;
+    float speed = getSensorSpeedData();
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
         return 0xFFFFFFFF;
     }
     index = IfIndex&0xFF;
-/*    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ControlIdtoMotorId[i] == deviceID)
-        {
-            index = i;
-            break;
-        }
-    if (index==MAX_THREAD_MOTORS_NUM)
-    {
-        LOG_ERROR (deviceID, "No motor  for device");
-        return 0xFFFFFFFF;
-    }
-    */
+    SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue);
+    MotorSamplePointer[index]++;
+    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
+        MotorSamplePointer[index] = 0;
+    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+        avreageSampleValue += SpeedSamples[i];
+    avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
     if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
     {
         MotorControlConfig[index].m_mesuredParam = ReadValue;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
+        calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
+        {
+            CurrentControlledSpeed[index] = calculated_speed;
+            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+        }
     }
  return OK;
 }
@@ -442,6 +450,11 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     return OK;
 }
 bool InitialProcess = false;
+uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
+
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
@@ -515,17 +528,10 @@ bool InitialProcess = false;
                 }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             }
-            /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
-    //            continue;
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
-            else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
+//            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+//                AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,ThreadEmptyCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 continue;
-                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
-//            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-//                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
     }
     //testDancersControl();
     PrepareReady(Module_Thread,ModuleDone);
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index 79eacf9b3..7a3771de9 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -399,20 +399,24 @@ void JobRequestFunc(MessageContainer* requestContainer)
     //free(requestContainer);
 }
 //********************************************************************************************************************
-//char logmsg[150];
+char logmsg[254];
+char infomsg[254];
 void SendJobProgress(double ProcessedLength, int SegmentId, bool done, char *Message)
 {
     MessageContainer responseContainer;
     uint8_t* container_buffer;
 
 //
-//    int len;
-//    static msdid = 0;
-//    int length = (int)ProcessedLength;
-//    len = usnprintf(logmsg, 150, "MSG: Job Progress Length %d, Seg %d Done %d %s",length,  SegmentId,  done, Message);
+    strcpy (infomsg,Message);
+
+    int len;
+    static msdid = 0;
+    int length = (int)ProcessedLength;
+    len = usnprintf(logmsg, 254, "MSG: Job Progress Length %f, Seg %d Done %d ",ProcessedLength,  SegmentId,  done);
 
     //REPORT_MSG(msdid++,logmsg);
-   // Report(logmsg,__FILE__,__LINE__,msdid,RpWarning,SegmentId,  done);
+    Report(logmsg,__FILE__,__LINE__,SegmentId,RpWarning,SegmentId,  done);
+    Report(infomsg,__FILE__,__LINE__,55,RpWarning,33,  44);
 
     if (JobToken[0] == 0)
         return;