IDS Handling, Reports, Winder message flushing

8 files changed, 98 insertions, 36 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 67de0271f..3066153ab 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -192,6 +192,18 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
 
     return OK;
 }
+int MillisecFlushMsgQ(TimerMotors_t MotorId)
+{
+    MillisecMotorDataStruc MotorData = {0};
+    int pend = Mailbox_getNumPendingMsgs(MotorsMsgQ[MotorId]);
+    int i;
+    if (pend)
+    {
+        for (i=0;i<pend;i++)
+            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
+    }
+   return pend;
+}
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     MillisecMotorDataStruc MotorData = {0};
@@ -354,6 +366,7 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++)
             CalculateDispenserPressure(Sensor_i);
+        FPGA_GetAllDispensersValveBusyOCD();
     }
 
     /*        OLD CODE
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index cf49e697a..807b81e98 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -19,6 +19,7 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
 int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, MSecFptr Callback);
+int MillisecFlushMsgQ(TimerMotors_t MotorId);
 
 uint32_t getMotorStatusData(int MotorId);
 uint32_t getMotorSpeedData(int MotorId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
index 1878b6e55..757fa0412 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
@@ -70,5 +70,6 @@ extern uint32_t  Heater_timerBase;
 
 uint32_t HeaterRecalculateSharedHeatersParams(uint32_t deviceId, uint32_t new_outputproportionalpowerlimit);
 void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message);
+uint32_t HeaterRecalculateHeaterParams(uint32_t deviceId, uint32_t new_outputproportionalpowerlimit);
 
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index 7d5ec7afd..3fa8cfa40 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -45,10 +45,10 @@ typedef enum {
 }HEATERS_EVENTS_ENUM;
 
 /******************** GLOBAL PARAMETERS  ********************************************/
-HeaterPIDControlConfig HeaterControl[MAX_HEATERS_NUM] = {0};
-HeaterControlConfig_t  HeaterPIDConfig[MAX_HEATERS_NUM] = {0};
+HeaterPIDControlConfig HeaterControl[MAX_HEATERS_NUM] = {0,0,0,0,0,0,0,0,0,0,0,0};
+HeaterControlConfig_t  HeaterPIDConfig[MAX_HEATERS_NUM] = {0,0,0,0,0,0,0,0,0,0,0,0};
 uint32_t HeaterId2PT100Id[MAX_HEATERS_NUM] = {ANALOG_DRYER_TEMP1,ANALOG_DRYER_TEMP2,ANALOG_DRYER_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP1,TEMP_SENSE_ANALOG_DYEINGH_TEMP2,TEMP_SENSE_ANALOG_DYEINGH_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP4,ANALOG_MIXCHIP_TEMP};
-bool AcHeaterConfigured[MAX_AC_HEATERS] = {0};
+bool AcHeaterConfigured[MAX_AC_HEATERS] = {0,0,0};
 
 int      NumberOFSlicesInUse = 0;
 uint32_t MillisecondsPerChange = 0;
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index e7711953f..38ae19f36 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -151,6 +151,11 @@ uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
 {
     //uint32_t status = NOT_SUPPORTED;
     //MessageContainer responseContainer;
+    if (HeaterControl[HeaterId].configured == false)
+    {
+        LOG_ERROR (HeaterId,"HeaterControl not configured");
+        return ERROR;
+    }
     if (HeaterReducedPowerStarted == false)
     {
         HeaterReducedPowerStarted = true;
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index f44f2d790..66cfb3c7d 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -41,9 +41,9 @@ bool    DispenserReady[MAX_SYSTEM_DISPENSERS] = {true};
 /******************** STRUCTURES AND ENUMs  ********************************************/
 /******************** GLOBAL PARAMETERS  ********************************************/
 DispenserControlConfig_t DispenserControlConfig[MAX_SYSTEM_DISPENSERS];
-uint32_t    ControlIdtoDispenserId [MAX_SYSTEM_DISPENSERS] = {0xFF};
-int OriginalDispenserSpd_2PPS[MAX_SYSTEM_DISPENSERS] = {0};
-bool         DispenserPreSegmentReady[MAX_SYSTEM_DISPENSERS] = {false};
+uint32_t    ControlIdtoDispenserId [MAX_SYSTEM_DISPENSERS] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
+int OriginalDispenserSpd_2PPS[MAX_SYSTEM_DISPENSERS] = {0,0,0,0,0,0,0,0};
+bool         DispenserPreSegmentReady[MAX_SYSTEM_DISPENSERS] = {true,true,true,true,true,true,true,true};
 int JobBrushStopId = 0;
 
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
@@ -126,17 +126,13 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
              *
              */
             //ValveCommand (Enable,MixerDirection);
-    #ifdef DEBUG_TEST_FUNCTIONS
-// add control here
-    #else
-    #endif
     }
     //set 3 dancers to the profile positions
     for (i = 0; i < MAX_SYSTEM_DISPENSERS; i++)
     {
         DispenserReady[i] = true;
     }
-
+    DispenserPrepareReady();
     return OK;
 }
  void DispenserPrepareReady(void)
@@ -172,7 +168,7 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-    int Dispenser_i;
+    int Dispenser_i,n_dispensers,DispenserId;
     TimerMotors_t HW_Motor_Id;
 
     JobBrushStopId = 0;
@@ -181,18 +177,35 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
     /* wait for all dispensers  to get to the required pressure
      * move the presegment ready when all dispensers are ready.
      */
-    for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+    if (SegmentId>=JobTicket->n_segments)
+        {
+            LOG_ERROR(SegmentId,"Error Segment");
+            return ERROR;
+        }
+    if (JobBrushStopId>=JobTicket->segments[SegmentId]->n_brushstops)
+    {
+        LOG_ERROR(JobBrushStopId,"Error JobBrushStopId");
+        return ERROR;
+    }
+    if (JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->n_dispensers)
     {
-        HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
-        Control3WayValvesWithCallback (Dispenser_i, MidTank_Dispenser, IDS_Valve_PresegmentReady); //direction: MidTank_Dispenser or Dispenser_Mixer
+        n_dispensers = JobBrushStopId>=JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->n_dispensers;
+        for (Dispenser_i = 0;Dispenser_i <= n_dispensers;Dispenser_i++)
+        {
+            DispenserId = JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->index;
+            HW_Motor_Id = DispenserIdToMotorId[DispenserId];
+            if (MotorsCfg[HW_Motor_Id].hardwaremotortype != DispenserIdToMotorId[DispenserId])//unconfigured dispenser
+                continue;
+            Control3WayValvesWithCallback ((Valves_t)DispenserId, MidTank_Dispenser, IDS_Valve_PresegmentReady); //direction: MidTank_Dispenser or Dispenser_Mixer
 
-        MotorStop(HW_Motor_Id,Hard_Stop);
-        if (JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->has_dispenserstepdivision)
-            MotorSetMicroStep(HW_Motor_Id,JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->dispenserstepdivision);
-        else
-            MotorSetMicroStep(HW_Motor_Id,MotorsCfg[HW_Motor_Id].microstep);
-        DispenserPreSegmentReady[Dispenser_i] = false;
+            MotorStop(HW_Motor_Id,Hard_Stop);
+            if (JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->has_dispenserstepdivision)
+                MotorSetMicroStep(HW_Motor_Id,JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->dispenserstepdivision);
+            else
+                MotorSetMicroStep(HW_Motor_Id,MotorsCfg[HW_Motor_Id].microstep);
+            DispenserPreSegmentReady[DispenserId] = false;
 
+        }
     }
     //call the job state machine when the thread system is ready
     //PreSegmentReady(Module_IDS,ModuleDone);
@@ -204,26 +217,45 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
  uint32_t IDSSegmentState(void *JobDetails, int SegmentId)
 {
      JobTicket* JobTicket = JobDetails;
-     int Dispenser_i;
+     int Dispenser_i,n_dispensers,DispenserId;
      TimerMotors_t HW_Motor_Id;
      double segmentfirst_speed;
-     int CurrentSegment = 0;
+     int CurrentSegment = SegmentId;
 
 
-     for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+     if (CurrentSegment>=JobTicket->n_segments)
+         {
+             LOG_ERROR(CurrentSegment,"Error Segment");
+             return ERROR;
+         }
+     if (JobBrushStopId>=JobTicket->segments[CurrentSegment]->n_brushstops)
      {
-         HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
-         //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius)
-         segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanolitterpersecond/JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanoliterperpulse;
-//         double Dispenser_speed = (segmentfirst_speed *  MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
-         //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed;
-//         OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
-         if ((int)segmentfirst_speed > 0)
-             {
-                 Control3WayValvesWithCallback (Dispenser_i, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
-                 MotorSetSpeed(HW_Motor_Id, segmentfirst_speed);
-             }
+         LOG_ERROR(JobBrushStopId,"Error JobBrushStopId");
+         return ERROR;
      }
+     if (JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->n_dispensers)
+     {
+         n_dispensers = JobBrushStopId>=JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->n_dispensers;
+         for (Dispenser_i = 0;Dispenser_i <= n_dispensers;Dispenser_i++)
+         {
+             DispenserId = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->index;
+             HW_Motor_Id = DispenserIdToMotorId[DispenserId];
+             if (MotorsCfg[HW_Motor_Id].hardwaremotortype != DispenserIdToMotorId[DispenserId])//unconfigured dispenser
+                 continue;
+             //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius)
+             segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanolitterpersecond/
+                     JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanoliterperpulse;
+    //         double Dispenser_speed = (segmentfirst_speed *  MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
+             //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed;
+    //         OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
+             if ((int)segmentfirst_speed > 0)
+                 {
+                     Control3WayValvesWithCallback (DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
+                     MotorSetSpeed(HW_Motor_Id, segmentfirst_speed);
+                 }
+         }
+     }
+
     return OK;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 3505ff3c6..e564e52d0 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -72,6 +72,8 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
     uint32_t status=OK;
     uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 
+    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency);
+
     status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback);
      //set motor location 0 here
         return status;
@@ -180,10 +182,12 @@ uint32_t Winder_Presegment(void *JobDetails)
 }
 uint32_t Winder_End(void)
 {
+    int pend;
     //stop screw
     ScrewNumberOfSteps = 0;
     if (ScrewControlId != 0xFF)
         RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
+    pend = MillisecFlushMsgQ(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index e9cc3ddde..444a9fb5d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -523,12 +523,14 @@ void ThreadDistanceToSpoolEnded(void)
 {
 
 }
+double seglength = 0.0;
 //********************************************************************************************************************
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
 {
     JobTicket* JobTicket = JobDetails;
+    seglength = JobTicket->segments[SegmentId]->length;
     CurrentSegmentId = SegmentId;
-    ThreadUpdateProcessLength (JobTicket->segments[SegmentId]->length,(void *)ThreadSegmentEnded);
+    ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded);
     return OK;
 }
 
@@ -536,9 +538,13 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
  uint32_t ThreadEndState(void *JobDetails)
 {
      int Motor_i;
+     ThreadUpdateProcessLength (0.0,(void *)NULL);
+    SetOriginMotorSpeed(0);
+
      if (SpeedControlId != 0xFF)
      {
          RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+         SpeedControlId == 0xFF;
      }
      for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
      {