merge

1 files changed, 38 insertions, 21 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index f9ba6f46b..9eb9d9822 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -100,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)
@@ -112,7 +113,7 @@ void MillisecInit(void)
 
     Error_init(&eb);
 
-    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 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);
@@ -198,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;*/
 
     //==========================
 
@@ -224,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;
@@ -249,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;
@@ -285,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++;
@@ -307,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++;
@@ -371,7 +387,6 @@ uint32_t MillisecLoop(uint32_t tick)
                 }
                 MotorActivity--;
             }
-
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
             {
                 if (ScrewSetMaxSpeedPending.Active == true)
@@ -387,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);
@@ -422,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);