thread debug. diagnostics heaters state

1 files changed, 39 insertions, 18 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 05c6c9003..023e0dfc8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -14,6 +14,7 @@
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
 #include "PMR/Printing/JobSegment.pb-c.h"
 #include "PMR/Printing/JobTicket.pb-c.h"
+#include "PMR/common/ErrorCode.pb-c.h"
 #include  <PMR/Diagnostics/EventType.pb-c.h>
 
 #include <utils/ustdlib.h>
@@ -144,10 +145,10 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     double length = 0.0;
     char str[150];
     int index = MAX_THREAD_MOTORS_NUM;
-    if (ThreadControlActive == false)
-        return OK;
-    if (PrepareState == true)
-        return OK;
+ //   if (ThreadControlActive == false)
+ //       return OK;
+ //   if (PrepareState == true)
+ //       return OK;
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
@@ -310,14 +311,16 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
-double calculatedError[200];
+double calculatedError[1000];
 //double eNormalizedError[100];
-int    MotorId[200];
-int    readValue[200];
+int    MotorId[1000];
+int    readValue[1000];
 //int    TranslatedreadValue[100];
-int    AveragereadValue[200];
-int    calculatedspeed[200];
+int    AveragereadValue[1000];
+int    calculatedspeed[1000];
+int    timestamp[1000];
 int controlIndex = 0;
+bool keepdata = true;
 /*int32_t KeepReadValue = 0;
 void testDancersControl()
 {
@@ -343,6 +346,8 @@ void testDancersControl()
     ThreadControlActive = false;
 }*/
 bool dancerinvalid = false;
+int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0};
+char Message[60];
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -375,11 +380,13 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         DancerId = ThreadMotorIdToDancerId[index];
         if (ReadValue < 10)
         {
+            MotorFailedSample[index]++;
             REPORT_MSG(ReadValue, "Dancer value read too small.");
             return OK;
         }
         if (ReadValue == 0x3FFF)
         {
+            MotorFailedSample[index]++;
             if (dancerinvalid == false)
             {
                 dancerinvalid = true;
@@ -429,6 +436,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         //Stop Execution if the dancer moves too much
         if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
         {
+            keepdata = false;
             usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
             //JobAbortedByUser = true;
             ThreadControlActive = false;
@@ -471,18 +479,25 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }*/
         if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
         {
-             calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
-            //double eNormalizedError[100];
-                MotorId[controlIndex] = index;
-                readValue[controlIndex] = ReadValue;
-            //int    TranslatedreadValue[100];
-                AveragereadValue[controlIndex] = avreageSampleValue;
-                calculatedspeed[controlIndex] = calculated_speed;
-             if (controlIndex++>=199)
-                 controlIndex = 0;
+            if (keepdata == true)
+            {
+                 calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
+                //double eNormalizedError[100];
+                    MotorId[controlIndex] = index;
+                    readValue[controlIndex] = ReadValue;
+                //int    TranslatedreadValue[100];
+                    AveragereadValue[controlIndex] = avreageSampleValue;
+                    calculatedspeed[controlIndex] = calculated_speed;
+                    timestamp[controlIndex] = HibernateRTCSSGet();
+                 if (controlIndex++>=999)
+                     controlIndex = 0;
+            }
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
         }
+		else
+           MotorFailedSample[index]++;
+
     }
 
  return OK;
@@ -715,11 +730,17 @@ uint32_t ThreadDistanceToSpoolState(void )
     return OK;
 }
 
+char Endstr[150];
 //********************************************************************************************************************
  uint32_t ThreadEndState(void *JobDetails)
 {
      int Motor_i;
      ThreadControlActive = false;
+
+     usnprintf(Endstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+     SendJobProgress(0.0,0,false, Endstr);
+     Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+
      ThreadUpdateProcessLength (0.0,(void *)NULL);
     SetOriginMotorSpeed(0);