From fc23da7f3510cce58308841eefb96d59868317ff Mon Sep 17 00:00:00 2001
From: Shlomo Hecht <shlomo@twine-s.com>
Date: Mon, 17 Dec 2018 09:41:17 +0200
Subject: Version 1.3.0.2 Reset Cause in log, remove some logs, improve memory
 on alarm handling. stop heaters and dispensers on alarm without  job. split
 microsecond and control tasks to high and low priorities. Heaters PID with
 multiplications - use with new CFG.

---
 .../Embedded/Modules/Control/MillisecTask.c        | 82 ++++++++++++++++---
 .../Embedded_SW/Embedded/Modules/Control/control.c | 91 +++++++++++++++++++++-
 2 files changed, 160 insertions(+), 13 deletions(-)

(limited to 'Software/Embedded_SW/Embedded/Modules/Control')

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 68bd680a8..64b6ac4e0 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -90,6 +90,7 @@ MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
+Mailbox_Handle          TenMillisecMsgQ = NULL;
 Mailbox_Handle          MotorsMsgQ[NUM_OF_MOTORS] = {NULL};
 bool                    MillisecRestart;
 static GateMutex_Handle gateMillisecDB;
@@ -114,6 +115,7 @@ void MillisecInit(void)
     Error_init(&eb);
 
     MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
+    TenMillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
     for (i=0;i<NUM_OF_MOTORS;i++)
     {
         MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb);
@@ -158,6 +160,7 @@ void MillisecStart(void)
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
+    uint32_t Tenmsec_millisecondCounter = 0;
     ROM_IntMasterDisable();
     ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     if (MillisecRestart == true)
@@ -170,13 +173,26 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 	    ROM_IntMasterEnable();
 		return;
 	}
+    if (msec_millisecondCounter%10 == 0)
+    {
+        Tenmsec_millisecondCounter = msec_millisecondCounter;
+    }
     //send message to the Millisec task
     Message.messageId = OneMillisec;
-    Message.tick = msec_millisecondCounter++;;
+    Message.tick = msec_millisecondCounter++;
     Message.msglen = sizeof(MillisecMessageStruc);
     if (MillisecMsgQ != NULL)
         Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
 
+    if (Tenmsec_millisecondCounter)
+    {
+        Message.messageId = OneMillisec;
+        Message.tick = Tenmsec_millisecondCounter;
+        Message.msglen = sizeof(MillisecMessageStruc);
+        if (TenMillisecMsgQ != NULL)
+            Mailbox_post(TenMillisecMsgQ , &Message, BIOS_NO_WAIT);
+    }
+
     //
     // Enable all interrupts.
     //
@@ -341,17 +357,16 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
     else return false;
 }
 
-//TEMPERATURE_SENSOR_ID_ENUM Sensor_Read = 0;
 uint32_t MillisecLoop(uint32_t tick)
 {
-    uint8_t Motor_i,Disp_i;
+    uint8_t Motor_i;
     TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
     unsigned int MotorInfo = 0;
     static int temp=0;
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick;
+/*    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
@@ -360,7 +375,7 @@ uint32_t MillisecLoop(uint32_t tick)
     Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
     //gather Motor data from FPGA
     //ROM_IntMasterDisable();
-
+*/
 #ifndef EVALUATION_BOARD
         FPGA_GetBusy();  //load the busy motor information to all motors
         FPGA_Read_limit_Switches_Registers();
@@ -473,6 +488,26 @@ uint32_t MillisecLoop(uint32_t tick)
     Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
     Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
     Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+    return OK;
+}
+uint32_t MillisecLowLoop(uint32_t tick)
+{
+    uint8_t Disp_i;
+    TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
+
+    static int temp=0;
+    //call all modules Millisec functions
+    //test dancers and speed encoders
+    //check all callback units (state machine waiting for completion of a change)
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick;
+    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
+    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
+    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
+    OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
+    //gather Motor data from FPGA
+    //ROM_IntMasterDisable();
+
     if (Ten_msTick)
     {
         //Speed_Data = Calculate_Speed_Sensor_Velocity();
@@ -501,7 +536,7 @@ uint32_t MillisecLoop(uint32_t tick)
         for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
         {
             CalculateDispenserPressure(Disp_i);
-            Read_MidTank_Pressure_Sensor(Disp_i);
+            //Read_MidTank_Pressure_Sensor(Disp_i);
         }
         FPGA_GetAllDispensersValveBusyOCD();
         DrawerFansStatus = Read_Fans_Tacho();
@@ -509,10 +544,10 @@ uint32_t MillisecLoop(uint32_t tick)
     }
     if (OneMinute_Tick)
     {
-        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
-        {
-            Read_MidTank_Pressure_Sensor(Disp_i);
-        }
+//        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+//        {
+//            Read_MidTank_Pressure_Sensor(Disp_i);
+//        }
 /*        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
@@ -553,6 +588,33 @@ void MillisecTask(UArg arg0, UArg arg1)
         }
     }
 }
+/******************************************************************************
+ *  ======== messageTsk ========
+ *  Task for this function is created statically. See the project's .cfg file.
+ *  this message task is created statically in system initialization,
+ ******************************************************************************/
+void MillisecLowTask(UArg arg0, UArg arg1)
+{
+    MillisecMessageStruc Message;
+    //char str[60];
+    //uint16_t length;
+    //Clock_setTimeout(HostKAClock, 1000);
+    //Clock_start(HostKAClock);
+    //MillisecInit();
+    //Millisecond_Task_Handle = Task_self();
+    while(1)
+    {
+        Mailbox_pend(TenMillisecMsgQ , &Message, BIOS_WAIT_FOREVER);
+        switch (Message.messageId)
+        {
+            case OneMillisec:
+                MillisecLowLoop(Message.tick);
+                break;
+            default:
+                break;
+        }
+    }
+}
 
 
 /*uint32_t getMotorStatusData(int MotorId)
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 2110029e4..ed1e73bef 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -87,6 +87,7 @@ int ControlPhaseDelay = 300; //the control task enters only after data gathering
 //this parameters defines how many microseconds in the delay. it is used only on starting the control loop on the first time
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          ControlMsgQ = NULL;
+Mailbox_Handle          TenControlMsgQ = NULL;
 bool                    ControlRestart;
 static GateMutex_Handle gateControlDB;
 Task_Handle Control_Task_Handle;
@@ -115,6 +116,7 @@ void ControlInit(void)
 
     //Mailbox_Params_init(&ControlMsgQ);
     ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
+    TenControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
 
     ControlRestart = false;
 
@@ -258,6 +260,8 @@ uint32_t millisecondCounter = 0;
 void OneMilliSecondControlInterrupt(UArg arg0)
 {
     ControlMessageStruc Message;
+    uint32_t TenmillisecondCounter = 0;
+
     ROM_IntMasterDisable();
     ROM_TimerIntClear(Control_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
 
@@ -272,6 +276,10 @@ void OneMilliSecondControlInterrupt(UArg arg0)
         //ROM_TimerDisable(Control_timerBase, TIMER_A);
         return;
     }
+    if (millisecondCounter%10 == 0)
+    {
+        TenmillisecondCounter = millisecondCounter;
+    }
 
     //send message to the control task
     Message.messageId = OneMillisec;
@@ -279,6 +287,12 @@ void OneMilliSecondControlInterrupt(UArg arg0)
     Message.msglen = sizeof(ControlMessageStruc);
     if (ControlMsgQ != NULL)
         Mailbox_post(ControlMsgQ , &Message, BIOS_NO_WAIT);
+    //if (TenmillisecondCounter)
+    {
+        //Message.tick = TenmillisecondCounter;
+        if (TenControlMsgQ != NULL)
+            Mailbox_post(TenControlMsgQ , &Message, BIOS_NO_WAIT);
+    }
     if (millisecondCounter == 1000000000)
         millisecondCounter = 0;
     //
@@ -294,7 +308,7 @@ uint32_t ControlLoop(uint32_t tick)
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
     uint32_t Device_i;
-    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
+    /*bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
 //    Hundred_msTick  =   (tick%200 == 0)  ?true:false;
@@ -302,7 +316,7 @@ uint32_t ControlLoop(uint32_t tick)
     Tick98          =   (tick%eHundredMillisecond == 98) ?true:false;
 //    Tick98          =   (tick%200 == 199) ?true:false;
     Tick998         =   (tick%eOneSecond == 996)         ?true:false;
-
+*/
     //ROM_IntMasterDisable();
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
@@ -323,6 +337,40 @@ uint32_t ControlLoop(uint32_t tick)
                     else
                         LOG_ERROR (Device_i, "Invalid callback ptr");
                     break;
+                default:
+                    break;
+            } //switch
+        } //if control active
+    }  //for
+    //ROM_IntMasterEnable();
+
+    return OK;
+}
+uint32_t ControlLowLoop(uint32_t tick)
+{
+    //call all modules control functions
+    //test dancers and speed encoders
+    //check all callback units (state machine waiting for completion of a change)
+    uint32_t Device_i;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
+    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
+    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
+    Tick98          =   (tick%eHundredMillisecond == 98) ?true:false;
+    Tick998         =   (tick%eOneSecond == 996)         ?true:false;
+
+    //ROM_IntMasterDisable();
+    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
+    {
+        if (ControlArray[Device_i].ControlActive)
+        {
+            ControlBacklog[backlogindex]=Device_i;
+            if ( ++backlogindex >= 999)
+                backlogindex = 0;
+            switch (ControlArray[Device_i].ControlTiming)
+            {
+                case eOneMillisecond:
+                    break;
                 case eTenMillisecond:
                     if (Ten_msTick)
                     {
@@ -374,7 +422,17 @@ uint32_t ControlLoop(uint32_t tick)
                     memset (&ControlArray[Device_i],0,sizeof(ControlDeviceStruc) );
                     break;
                 default:
-                    LOG_ERROR (ControlArray[Device_i].ControlTiming, "Invalid control timing value");
+                    if (tick%ControlArray[Device_i].ControlTiming == 0)
+                    {
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
+                    }
                     break;
             } //switch
         } //if control active
@@ -410,3 +468,30 @@ void controlTask(UArg arg0, UArg arg1)
         }
     }
 }
+/******************************************************************************
+ *  ======== messageTsk ========
+ *  Task for this function is created statically. See the project's .cfg file.
+ *  this message task is created statically in system initialization,
+ ******************************************************************************/
+void controlLowTask(UArg arg0, UArg arg1)
+{
+    ControlMessageStruc Message;
+    //char str[60];
+    //uint16_t length;
+    //Clock_setTimeout(HostKAClock, 1000);
+    //Clock_start(HostKAClock);
+    Control_Task_Handle = Task_self();
+
+    while(1)
+    {
+        Mailbox_pend(TenControlMsgQ , &Message, BIOS_WAIT_FOREVER);
+        switch (Message.messageId)
+        {
+            case OneMillisec:
+                ControlLowLoop(Message.tick);
+                break;
+            default:
+                break;
+        }
+    }
+}
-- 
cgit v1.3.1