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

4 files changed, 189 insertions, 21 deletions

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/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index 72774461a..f9aee7929 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -52,8 +52,6 @@ float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_
     }
 
     derivative = (error - *_pre_error)/params->dt;
-    float IntegralErrorMultiplier;
-    float ProportionalErrorMultiplier;
 
     output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 2110029e4..001cafeb3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -65,10 +65,11 @@ typedef struct
   uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
   bool ControlActive;
   uint32_t Parameter1;
-  uint32_t IfIndex;
+  uint16_t IfIndex;
+  uint32_t StartTick;
   DataReadCBFunction ControlDataReadPtr;
   ControlCBFunction ControlCallbackPtr;
-  CTRL_TIMING_ENUM ControlTiming;
+  uint16_t ControlTiming;
 }ControlDeviceStruc;
 
 typedef enum
@@ -87,6 +88,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;
@@ -114,7 +116,8 @@ void ControlInit(void)
     Error_Block eb;
 
     //Mailbox_Params_init(&ControlMsgQ);
-    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
+    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 1, NULL,NULL);
+    TenControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 1, NULL,NULL);
 
     ControlRestart = false;
 
@@ -195,7 +198,7 @@ void ControlStart(void)
  * both these callbacks can be removed. if a new call is arriving, it invalidates the previous one (no dual control or data)
  *
  ***************************************************************************************************************************************************/
-uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
+uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
 {
     assert(Callback);
     assert(DriverfPtr);
@@ -220,6 +223,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     ControlArray[deviceId].ControlDataReadPtr = DriverfPtr;
     ControlArray[deviceId].Parameter1 = Parameter1;
     ControlArray[deviceId].IfIndex = IfIndex;
+    ControlArray[deviceId].StartTick = millisecondCounter;
     GateMutex_leave(gateControlDB, key);
     //LOG_ERROR(deviceId, "Add Callback");
 
@@ -258,6 +262,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 +278,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 +289,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 +310,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 +318,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 +339,58 @@ 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)
+        {
+            if (tick == ControlArray[Device_i].StartTick)
+                continue;
+            if (((tick - ControlArray[Device_i].StartTick)%ControlArray[Device_i].ControlTiming)==0) // run the control on exact intervals
+            {
+                ControlBacklog[backlogindex]=Device_i;
+                if ( ++backlogindex >= 999)
+                    backlogindex = 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");
+            }
+            /*
+            ControlBacklog[backlogindex]=Device_i;
+            if ( ++backlogindex >= 999)
+                backlogindex = 0;
+            switch (ControlArray[Device_i].ControlTiming)
+            {
+                case eOneMillisecond:
+                    break;
                 case eTenMillisecond:
                     if (Ten_msTick)
                     {
@@ -374,9 +442,21 @@ 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");
+                    //LOG_ERROR(tick, "Default Timing checked");
+                    if (tick%ControlArray[Device_i].ControlTiming == 0)
+                    {
+                        Report("Default Timing checked",__FILE__,__LINE__,tick,RpWarning,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
+            } //switch*/
         } //if control active
     }  //for
     //ROM_IntMasterEnable();
@@ -410,3 +490,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;
+        }
+    }
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 0aba4b4ba..b3e574d2a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -37,10 +37,11 @@ typedef enum
 void        ControlInit(void);
 void        ControlStop(void);
 void        ControlStart(void);
-uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
+uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
 int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1);
 
 extern Task_Handle Control_Task_Handle;
+extern uint32_t millisecondCounter;
 
 #endif