state machine changes, memory allocation bug

3 files changed, 59 insertions, 67 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index b4ceebbc2..7036a79df 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -27,6 +27,7 @@
 #include "drivers/Motors/Motor.h"
 #include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
+#include "drivers/FPGA/FPGA_SPI_Comm.h"
 
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
@@ -55,10 +56,10 @@ typedef struct MillisecMessage{
 
 uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
 uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
-uint32_t  MotorSpeed_Data[MOTOR_SPARE1_1] = {0};
-uint32_t  MotorStatus_Data[MOTOR_SPARE1_1] = {0};
-uint32_t  MotorPosition_Data[MOTOR_SPARE1_1] = {0};
-bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
+uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
+uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
+uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
+bool      MotorBusy_Data[NUM_OF_MOTORS] = {true};
 uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
@@ -129,7 +130,6 @@ void MillisecStart(void)
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
-    bool retcode = false;
     ROM_IntMasterDisable();
     if (MillisecRestart == true)
     {
@@ -143,7 +143,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     Message.tick = UsersysTickGet();
     Message.msglen = sizeof(MillisecMessageStruc);
     if (MillisecMsgQ != NULL)
-        retcode = Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
+        Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
 
     ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     //
@@ -160,6 +160,8 @@ int32_t MillisecWriteToTempSensor(uint32_t TempSensorId, unsigned long Data, int
     PT100Data[TempSensorId].Length = Length;
     PT100Data[TempSensorId].DataRequired = false;
     PT100Data[TempSensorId].Active = true;
+
+    return OK;
 }
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback)
 {
@@ -170,6 +172,7 @@ int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, unsigned long Data, in
     PT100Data[TempSensorId].DataRequired = true;
     PT100Data[TempSensorId].Active = true;
 
+    return OK;
 }
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
@@ -180,6 +183,8 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
     SpeedSetPending[MotorId].Length = Length;
     SpeedSetPending[MotorId].DataRequired = false;
     SpeedSetPending[MotorId].Active = true;
+
+    return OK;
 }
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
@@ -207,14 +212,15 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
 }
 uint32_t MillisecLoop(uint32_t tick)
 {
-    int Motor_i,Sensor_i;
+    TimerMotors_t Motor_i;
+    int Sensor_i;
     unsigned int MotorInfo = 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;
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
-    Hundred_msTick  =   (tick%eHunderdMillisecond == 0)  ?true:false;
+    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
 
     //gather Motor data from FPGA
@@ -254,39 +260,40 @@ uint32_t MillisecLoop(uint32_t tick)
             }
         }
     }
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-    if (Hundred_msTick)
+    //FPGA_GetTempSensorBusy();
+    for (Sensor_i = 0;Sensor_i < NUM_OF_MOTORS;Sensor_i++)
     {
-        //FPGA_GetTempSensorBusy();
-        for (Sensor_i = 0;Sensor_i < NUM_OF_MOTORS;Sensor_i++)
+        //if (TempDriverDriverResponse[Sensor_i].Busy == true)
+        //    continue;
+        if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
         {
-            //if (TempDriverDriverResponse[Sensor_i].Busy == true)
-            //    continue;
-            if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
+            if (SPIGetFPGAResponse(Sensor_i,&MotorInfo) == OK) //got the data from the FPGA
             {
-                if (MotorGetFPGAResponse(Sensor_i,&MotorInfo) == OK) //got the data from the FPGA
-                {
-                    PT100Data[Sensor_i].WaitForData = false;
-                    if (PT100Data[Sensor_i].Callback)
-                        PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
-                }
+                PT100Data[Sensor_i].WaitForData = false;
+                if (PT100Data[Sensor_i].Callback)
+                    PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
             }
-            if (PT100Data[Sensor_i].Active == true)
+        }
+        if (PT100Data[Sensor_i].Active == true)
+        {
+            SPISendFPGARequest(Sensor_i,PT100Data[Sensor_i].Data,PT100Data[Sensor_i].Length);
+            if (PT100Data[Sensor_i].DataRequired == true)
             {
-                MotorSendFPGARequest(Sensor_i,PT100Data[Sensor_i].Data,PT100Data[Sensor_i].Length);
-                if (PT100Data[Sensor_i].DataRequired == true)
-                {
-                    PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
-                }
-                else
-                {
-                    if (PT100Data[Sensor_i].Callback)
-                        PT100Data[Sensor_i].Callback(Sensor_i,0); // call the callback to report execution
-                }
+                PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
+            }
+            else
+            {
+                if (PT100Data[Sensor_i].Callback)
+                    PT100Data[Sensor_i].Callback(Sensor_i,0); // call the callback to report execution
             }
         }
     }
-#ifndef EVALUATION_BOARD
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
+    if (Hundred_msTick)
+    {
+    }
+#ifdef EVALUATION_BOARD
+
     /* this cannot be done within one millisecond, and not needed
      * instead, check if there is a motor waiting with data to send or read request
      * MotorSpeed_Data[MOTOR_DRYER_DRIVING] = MotorGetSpeedFromFPGA(MOTOR_DRYER_DRIVING);
@@ -323,7 +330,7 @@ uint32_t MillisecLoop(uint32_t tick)
             TemperatureSensor_Data[pt100_i] = TemperatureSensorReadFromFPGA(pt100_i);
         MotorGetnBusyFromFPGA(); // get all motors nBusy bit status from the FPGAs
         TimerMotors_t motor_i;
-        for (motor_i = 0; motor_i < MOTOR_SPARE1_1 ; motor_i++)
+        for (motor_i = 0; motor_i < NUM_OF_MOTORS ; motor_i++)
         {
             MotorBusy_Data[motor_i] = MotorGetnBusyState(motor_i);
             if (MotorBusy_Data[motor_i] == false) //can get data
@@ -369,12 +376,12 @@ void MillisecTask(UArg arg0, UArg arg1)
 
 uint32_t getMotorStatusData(int MotorId)
 {
-    assert (MotorId < MOTOR_SPARE1_1);
+    assert (MotorId < NUM_OF_MOTORS);
     return MotorStatus_Data[MotorId];
 }
 uint32_t getMotorSpeedData(int MotorId)
 {
-    assert (MotorId < MOTOR_SPARE1_1);
+    assert (MotorId < NUM_OF_MOTORS);
     return MotorSpeed_Data[MotorId];
 }
 
@@ -390,29 +397,4 @@ uint32_t getADCData(int DeviceId)
     return ADC_Data[DeviceId];
 }
 
-/********************************************************************
-*
-*    Name        : GTIME_Delta_Time_Pass
-*
-*    Parameters  : start_time.
-*
-*    Return      : time pass from start time
-*
-*    Description :
-*
-*********************************************************************/
-
-uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
-{
-    uint32_t Time_Pass;
-  #define   MAX_COUNTER 0x3FFF  //14 bits
-
-
-  if (Current_Read < Previous_Read)
-    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
-  else
-    Time_Pass = Current_Read - Previous_Read;
-
-  return (Time_Pass);
-}
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 0a4a8b791..a56457bdb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -254,10 +254,12 @@ 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;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
-    Hundred_msTick  =   (tick%eHunderdMillisecond == 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 == 998)         ?true:false;
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
@@ -276,17 +278,25 @@ uint32_t ControlLoop(uint32_t tick)
                         ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
                     }
                     break;
-                case eHunderdMillisecond:
-                    if (Hundred_msTick)
+                case eHundredMillisecond:
+                    if (Tick98)
                     {
+                        //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
                         ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                    }
+                    if (Hundred_msTick)
+                    {
+                        //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
                         ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
                     }
                     break;
                 case eOneSecond:
-                    if (Onesecond_Tick)
+                    if (Tick998)
                     {
                         ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                    }
+                    if (Onesecond_Tick)
+                    {
                         ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
                     }
                     break;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 9322f562e..76e90ed2e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -17,7 +17,7 @@ typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
     eTenMilliSecond = 10,
-    eHunderdMillisecond = 100,
+    eHundredMillisecond = 100,
     eOneSecond = 1000
 }CTRL_TIMING_ENUM;
 typedef enum