diagnostic sends calculated error in the tension info - every 10 msec

3 files changed, 44 insertions, 2 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 1ab4f311a..bda086251 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -55,8 +55,8 @@ uint32_t Diagnostics_TenMiliControlTrigger(uint32_t IfIndex, uint32_t ReadValue)
 
 
 DiagnosticsMonitors DiagnosticsMonitor = DIAGNOSTICS_MONITORS__INIT;
-#define DIAGNOSTICS_LIMIT 10
-#define DIAGNOSTICS_DANCER_LIMIT 100
+#define DIAGNOSTICS_LIMIT 5
+#define DIAGNOSTICS_DANCER_LIMIT 50
 int DiagnosticsIndex = 0;
 int DiagnosticCollectionLimit = 3;  //number of data samples to collect before sending to the host
 //int DiagnosticLimit = eHundredMillisecond; //frequency of data collection
@@ -169,6 +169,8 @@ uint8_t HeaterCounterIndex[MAX_HEATERS_NUM]= {0,0,0,0,0,0,0,0,0,0};
 double HeaterTemperature[MAX_HEATERS_NUM][DIAGNOSTICS_LIMIT];
 uint8_t  DancerCounterIndex[NUM_OF_DANCERS]= {0,0,0};
 double DancerValue[NUM_OF_DANCERS][DIAGNOSTICS_DANCER_LIMIT];
+int8_t  DancerErrorCounterIndex[NUM_OF_DANCERS]= {0,0,0};
+double DancerErrorValue[NUM_OF_DANCERS][DIAGNOSTICS_DANCER_LIMIT];
 uint8_t  MotorCounterIndex[MAX_THREAD_MOTORS_NUM]= {0,0,0};
 double MotorValue[MAX_THREAD_MOTORS_NUM][DIAGNOSTICS_LIMIT];
 uint8_t  SpeedCounterIndex= 0;
@@ -253,6 +255,19 @@ void DiagnosticLoadDancer(int DancerId, uint16_t value)
     if (DancerCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
         DancerCounterIndex[DancerId] = 0;
 }
+void DiagnosticLoadDancerError(int DancerId, double value)
+{
+    if (DancerId >= MAX_HEATERS_NUM)
+    {
+        LOG_ERROR(DancerId,"wrong Dancer Id");
+        return;
+    }
+    DancerErrorValue[DancerId][DancerErrorCounterIndex[DancerId]] = value;
+    DancerErrorCounterIndex[DancerId]++;
+    if (DancerErrorCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
+        DancerErrorCounterIndex[DancerId] = 0;
+
+}
 void DiagnosticTenMsecCollection(void)
 {
     if (DiagnosticsActive == false)
@@ -262,6 +277,11 @@ void DiagnosticTenMsecCollection(void)
     DiagnosticLoadDancer(WINDER_DANCER,Control_Read_Dancer_Position(WINDER_DANCER, 0,0));
     DiagnosticLoadDancer(POOLER_DANCER,Control_Read_Dancer_Position(POOLER_DANCER, 0,0));
     DiagnosticLoadDancer(FEEDER_DANCER,Control_Read_Dancer_Position(FEEDER_DANCER, 0,0));
+
+    DiagnosticLoadDancerError(WINDER_DANCER,ThreadGetMotorCalculatedError(WINDER_DANCER));
+    DiagnosticLoadDancerError(POOLER_DANCER,ThreadGetMotorCalculatedError(POOLER_DANCER));
+    DiagnosticLoadDancerError(FEEDER_DANCER,ThreadGetMotorCalculatedError(FEEDER_DANCER));
+
     //DiagnosticLoadSpeedSensor(getSensorSpeedData());
 
 }
@@ -403,6 +423,12 @@ void SendDiagnostics(void)
         DiagnosticsMonitor.dancer1angle = DancerValue[0];
         DiagnosticsMonitor.dancer2angle = DancerValue[1];
         DiagnosticsMonitor.dancer3angle = DancerValue[2];
+        DiagnosticsMonitor.n_windertension =  DancerErrorCounterIndex[0];
+        DiagnosticsMonitor.n_pullertension =  DancerErrorCounterIndex[1];
+        DiagnosticsMonitor.n_feedertension =  DancerErrorCounterIndex[2];
+        DiagnosticsMonitor.n_windertension = DancerErrorValue[0];
+        DiagnosticsMonitor.pullertension = DancerErrorValue[1];
+        DiagnosticsMonitor.feedertension = DancerErrorValue[2];
         DiagnosticsMonitor.n_threadspeed = SpeedCounterIndex;
         DiagnosticsMonitor.threadspeed = SpeedValue;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 2961bd105..c5e3edc85 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -27,6 +27,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadConfigBreakSensor(void *request);
 
 uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
+double ThreadGetMotorCalculatedError(int DancerId);
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 503ef2c37..0ca0b7159 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -490,6 +490,21 @@ uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId)
 {
     return  CurrentControlledSpeed[MotorId];
 }
+//********************************************************************************************************************
+double ThreadGetMotorCalculatedError(int DancerId)
+{
+    switch (DancerId)
+    {
+        case FEEDER_DANCER:
+            return  (double)MotorControlConfig[FEEDER_MOTOR].m_calculatedError;
+        case POOLER_DANCER:
+            return  (double)MotorControlConfig[POOLER_MOTOR].m_calculatedError;
+        case WINDER_DANCER:
+            return  (double)MotorControlConfig[WINDER_MOTOR].m_calculatedError;
+
+    }
+    return 0;
+}
 
 //********************************************************************************************************************
 uint32_t ThreadInitialTestStub(HardwareMotor * request)