small corrections

1 files changed, 38 insertions, 96 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 4ff47b56c..9eb9d9822 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -68,21 +68,6 @@ typedef struct MillisecMessage{
     uint32_t tick;
     uint8_t messageData[20];
 }MillisecMessageStruc;
-typedef enum
-{
-    ScrewMessage,
-    //ScrewCalculateTemperature,
-}ScrewMessages;
-
-typedef struct ScrewMessage{
-    uint16_t messageId;
-    uint16_t msglen;
-    uint32_t tick;
-  //  uint32_t SensorId;
-  //  uint32_t Data;
-}ScrewMessageStruc;
-Task_Handle Screw_Task_Handle;
-Mailbox_Handle          ScrewMsgQ = NULL;
 
 //uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
 //uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
@@ -115,9 +100,10 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 //**********************************************************************
 /******************** CODE  ********************************************/
 //**********************************************************************
-uint32_t MSBacklog[200]={0};
-uint8_t Motor_Id[200]={0};
-uint32_t MSTick[200]={0};
+#define LOG_SIZE 200
+uint32_t MSBacklog[LOG_SIZE+1]={0};
+uint8_t Motor_Id[LOG_SIZE+1]={0};
+uint32_t MSTick[LOG_SIZE+1]={0};
 uint16_t MsecLogindex = 0;
 
 void MillisecInit(void)
@@ -127,9 +113,7 @@ void MillisecInit(void)
 
     Error_init(&eb);
 
-    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 5, NULL,&eb);
-    ScrewMsgQ = Mailbox_create(sizeof(ScrewMessageStruc), 5, NULL,&eb);
-
+    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
     for (i=0;i<NUM_OF_MOTORS;i++)
     {
         MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb);
@@ -170,16 +154,6 @@ void MillisecStart(void)
     ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
     ADCAcquireStart(0,1);
 }
-void ScrewFreeInterrupt(void)
-{
-    MillisecMessageStruc Message;
-    Message.messageId = ScrewMessage;
-    if (ScrewMsgQ != NULL)
-        Mailbox_post(ScrewMsgQ , &Message, BIOS_NO_WAIT);
-
-    ROM_IntMasterEnable();
-    return ;
-}
 
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
@@ -225,13 +199,12 @@ uint32_t MotorActivity = 0;
 int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
 {
     //==========================
-    MSBacklog[MsecLogindex]=Data;
+    /*MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
     MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
-    if (MsecLogindex>=199)
-        MsecLogindex = 0;
-    LOG_ERROR(Data, "MillisecMoveScrew");
+    if (MsecLogindex>=LOG_SIZE)
+        MsecLogindex = 0;*/
 
     //==========================
 
@@ -251,14 +224,12 @@ int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
 int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
 {
     //==========================
-    MSBacklog[MsecLogindex]=Data;
+/*    MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
-    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
-    if (MsecLogindex>=199)
+    if (MsecLogindex>=LOG_SIZE)
         MsecLogindex = 0;
-    //==========================
-    LOG_ERROR(Data, "MillisecSetScrewSpeed");
+  */  //==========================
 
     ScrewSetMaxSpeedPending.Callback = Callback;
     ScrewSetMaxSpeedPending.Data = Data;
@@ -276,12 +247,18 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
 {
     if (MotorId >= NUM_OF_MOTORS) return -1;
     //==========================
+    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
+        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
+        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
+		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
+    {
     MSBacklog[MsecLogindex]=Data;
     MSTick[MsecLogindex]=msec_millisecondCounter;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
-    if (MsecLogindex>=199)
+    if (MsecLogindex>=LOG_SIZE)
         MsecLogindex = 0;
+    }
     //==========================
 
     SpeedSetPending[MotorId].Callback = Callback;
@@ -312,12 +289,18 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng
 {
     MillisecMotorDataStruc MotorData = {0};
     //==========================
+    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
+        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
+        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
+		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
+    {
     MSBacklog[MsecLogindex]=Data;
     MSTick[MsecLogindex]=msec_millisecondCounter;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
-    if (MsecLogindex>=199)
+    if (MsecLogindex>=LOG_SIZE)
         MsecLogindex = 0;
+    }
     //==========================
     if (MotorId >= NUM_OF_MOTORS) return -1;
     MotorActivity++;
@@ -334,12 +317,18 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
     MillisecMotorDataStruc MotorData = {0};
     if (MotorId >= NUM_OF_MOTORS) return -1;
 //==========================
+    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
+        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
+        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
+		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
+    {
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=MotorId;
     MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
-    if (MsecLogindex>=199)
+    if (MsecLogindex>=LOG_SIZE)
         MsecLogindex = 0;
+    }
     //==========================
     MotorActivity++;
     MotorActivity++;
@@ -398,7 +387,6 @@ uint32_t MillisecLoop(uint32_t tick)
                 }
                 MotorActivity--;
             }
-
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
             {
                 if (ScrewSetMaxSpeedPending.Active == true)
@@ -414,8 +402,6 @@ uint32_t MillisecLoop(uint32_t tick)
                 {
                     MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
                     MotorActivity--;
-                    /*if (Screwindex>=50)
-                        Screwindex = 0;*/
                     ScrewMovePending.Active = false;
                     if (ScrewMovePending.Callback)
                         ScrewMovePending.Callback(Motor_i,0);
@@ -449,12 +435,16 @@ uint32_t MillisecLoop(uint32_t tick)
         }
    }
     //FPGA_GetTempSensorBusy();
+   //int PT100Busy[MAX_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
    if (PT100Activity)
    {
        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
-           //if (TempDriverDriverResponse[Sensor_i].Busy == true)
-           //    continue;
+           /*if (TempSensorResponse[Sensor_i].Busy == true)
+           {
+               PT100Busy[Sensor_i]++;
+               continue;
+           }*/
            if (PT100Data[Sensor_i].Active == true)
            {
                TemperatureSendSensorDummyClk(Sensor_i);
@@ -535,27 +525,6 @@ uint32_t MillisecLoop(uint32_t tick)
 
     return OK;
 }
-int SendNextScrewMessage()
-{
-    volatile uint32_t temp; // read the busy value
-
-    if (Mailbox_pend(MotorsMsgQ[HARDWARE_MOTOR_TYPE__MOTO_SCREW] , &MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW], BIOS_NO_WAIT)==true)
-    {
-        if (MotorSendFPGARequest((HardwareMotorType)HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Data,MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Length) != OK) //sent the data to the FPGA
-        {
-            LOG_ERROR(MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Data,"SCREW Send message failed");
-        }
-        else
-        {
-            LOG_ERROR(MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Data,"SCREW Send message OK");
-            MotorActivity--;
-        }
-    }
-    ROM_SysCtlDelay(SYS_CLK_FREQ / 50000);
-
-    temp = F1_Moto_Driver_NBUSY2_L; // read the latch in order to clear the latch value (read it after moving to prevent interrupt before movement)
-
-}
 /******************************************************************************
  *  ======== messageTsk ========
  *  Task for this function is created statically. See the project's .cfg file.
@@ -616,30 +585,3 @@ uint32_t getDrawerFansStatus(void)
 {
     return DrawerFansStatus;
 }
-
-/******************************************************************************
- *  ======== messageTsk ========
- *  Task for this function is created statically. See the project's .cfg file.
- *  this message task is created statically in system initialization,
- ******************************************************************************/
-void ScrewTask(UArg arg0, UArg arg1)
-{
-    ScrewMessageStruc Message;
-    //char str[60];
-    //uint16_t length;
-    //Clock_setTimeout(HostKAClock, 1000);
-    //Clock_start(HostKAClock);
-    Screw_Task_Handle = Task_self();
-    while(1)
-    {
-        Mailbox_pend(ScrewMsgQ , &Message, BIOS_WAIT_FOREVER);
-        switch (Message.messageId)
-        {
-        case ScrewMessage:
-            SendNextScrewMessage();
-            break;
-            default:
-                break;
-        }
-    }
-}