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

2 files changed, 184 insertions, 6 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 968e52d7d..8e4a36e62 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -40,6 +40,13 @@
 #include "modules/thread/thread_ex.h"
 #include "modules/ids/ids_ex.h"
 
+#include "drivers/Flash_Memory/Flash_Memory.h"
+#include "drivers/Flash_Memory/fatfs/ff.h"
+#include "drivers/Flash_ram/FlashProgram.h"
+
+#include "third_party/fatfs/src/ffconf.h"
+#include "Common/SWUpdate/FileSystem.h"
+
 #include "Communication/Connection.h"
 
 Task_Handle Millisecond_Task_Handle;
@@ -77,6 +84,8 @@ typedef struct MillisecMessage{
 uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 float  Speed_Data = 0;
 uint32_t DrawerFansStatus = 0;
+uint32_t SystemFansStatus = 0;
+
 
 bool watchdogCriticalAlarm = false;
 
@@ -147,7 +156,11 @@ void MillisecStop(void)
     MillisecRestart = false;
     ADCAcquireStop();
 }
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+#define HIGH_TASK_FREQUENCY 12000
+#else
 #define HIGH_TASK_FREQUENCY 120000
+#endif
 void MillisecStart(void)
 {
     MillisecRestart = true;
@@ -164,6 +177,8 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc MillisecMessage;
     uint32_t Tenmsec_millisecondCounter = 0;
+#ifndef HUNDRED_MICROSECONDS_DANCER_READ
+
     ROM_IntMasterDisable();
     ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     if (MillisecRestart == true)
@@ -176,6 +191,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 	    ROM_IntMasterEnable();
 		return;
 	}
+#endif
     if (msec_millisecondCounter%10 == 0)
     {
         Tenmsec_millisecondCounter = msec_millisecondCounter;
@@ -420,15 +436,18 @@ uint32_t MillisecLoop(uint32_t tick)
            }
        }
    }
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+   SaveLogData();
+#else
     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);
-
+#endif
     return OK;
 }
 uint32_t MillisecLowLoop(uint32_t tick)
 {
-    uint8_t Motor_i,Disp_i,Heater_i;
+    uint8_t Motor_i,Disp_i,Heater_i,temp;
     TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
 
     //static int temp=0;
@@ -480,7 +499,9 @@ uint32_t MillisecLowLoop(uint32_t tick)
             //Read_MidTank_Pressure_Sensor(Disp_i);
         }
         FPGA_GetAllDispensersValveBusyOCD();
-        DrawerFansStatus = Read_Fans_Tacho();
+        temp = Read_Fans_Tacho();
+        DrawerFansStatus = temp & 0x1F;
+        SystemFansStatus = temp & 0xE0;
         KeepAliveOneSecondCall();
         for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
@@ -489,10 +510,10 @@ uint32_t MillisecLowLoop(uint32_t tick)
             if (isMotorConfigured(Motor_i))
                 MotorGetStatusFromFPGA(Motor_i);
         }
-       /* for (Heater_i = 0;Heater_i < NUM_OF_HEATERS;Heater_i++)
+        for (Heater_i = 0;Heater_i < NUM_OF_HEATERS;Heater_i++)
         {
-            Read_Head_MixChip_Heaters_Current(Heater_i);
-        }*/
+            Read_Heaters_Current(Heater_i);
+        }
     }
     if (OneMinute_Tick)
     {
@@ -600,3 +621,151 @@ uint32_t getDrawerFansStatus(void)
 {
     return DrawerFansStatus;
 }
+uint32_t getSystemFansStatus(void)
+{
+    return SystemFansStatus;
+}
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+uint32_t DancerData[NUM_OF_DANCERS];
+uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2)
+{
+    return DancerData[DancerId];
+}
+uint32_t dancer1;
+uint32_t dancer2;
+uint32_t dancer3;
+uint32_t dancer1sum;
+uint32_t dancer2sum;
+uint32_t dancer3sum;
+uint32_t dancer_count;
+/*-----------------------*/
+uint32_t StoreBuffer[2][128];
+//char * StoreBuffer[2][512];
+/*-----------------------*/
+int  StoreBufferId = 0;
+int  StoreBufferCounter=0;
+uint32_t  BufferCounter=0xEEEEEEEE;
+bool storeData=false;
+uint8_t len=0;
+void HundredMicroTimerInterrupt(int ARG0)
+{
+    ROM_IntMasterDisable();
+    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
+    if (MillisecRestart == true)
+    {
+        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
+    }
+    else
+    {
+        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
+        ROM_IntMasterEnable();
+        return;
+    }
+    dancer1 = Read_Dancer_Position(WINDER_DANCER);
+    dancer2 = Read_Dancer_Position(POOLER_DANCER);
+    dancer3 = Read_Dancer_Position(FEEDER_DANCER);
+//data store - logging
+//double buffer switch
+    /*-----------------------*/
+    if (StoreBufferCounter>125)
+    //if (StoreBufferCounter>490)
+        /*-----------------------*/
+    {
+        StoreBufferId = 1-StoreBufferId;//switch buffer
+        StoreBufferCounter=0;
+        storeData = true;
+    }
+//double buffer initialize
+    /*-----------------------*/
+    if (StoreBufferCounter==0)
+    {
+        StoreBuffer[StoreBufferId][StoreBufferCounter] = BufferCounter++;
+        StoreBufferCounter++;
+    }
+    /*-----------------------*/
+//store data
+    /*-----------------------*/
+    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer1;
+    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer2;
+    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer3;
+    /*-----------------------*/
+    //len = usprintf(&StoreBuffer[StoreBufferId][StoreBufferCounter], "\r\n%d %d %d", dancer1[dancer_count],dancer2[dancer_count],dancer3[dancer_count]);
+    //StoreBufferCounter+=(len+1);
+    /*-----------------------*/
+    dancer1sum+=dancer1;
+    dancer2sum+=dancer2;
+    dancer3sum+=dancer3;
+    dancer_count++;
+
+
+    if (dancer_count == 10)
+    {
+        DancerData[WINDER_DANCER] = dancer1sum/dancer_count;
+        DancerData[POOLER_DANCER] = dancer2sum/dancer_count;
+        DancerData[FEEDER_DANCER] = dancer3sum/dancer_count;
+        dancer_count = 0;
+        dancer1sum = 0;
+        dancer2sum = 0;
+        dancer3sum = 0;
+        OneMilliSecondMillisecInterrupt(ARG0);
+    }
+    ROM_IntMasterEnable();
+    return ;
+
+}
+char MillisecPath[50] = "0://SysInfo//Millisec.txt";
+FIL *FileHandle;
+void SaveLogData(void)
+{
+    uint32_t WrittenBytes = 0;
+    int BufferID = 1- StoreBufferId;
+
+    if (storeData == true)
+    {
+        if (FileHandle)
+        {
+            f_write(FileHandle,StoreBuffer[BufferID],512,&WrittenBytes );
+            storeData = false;
+        }
+    }
+}
+void MillisecLogInit(void)
+{
+    FRESULT Fresult = FR_OK;
+    ROM_IntMasterDisable();
+    BufferCounter = 0;
+    FileHandle = my_malloc(sizeof(FIL));
+    if (FileHandle == 0)
+        Fresult = FR_DENIED;
+    else
+        Fresult = f_open(FileHandle,MillisecPath,FA_WRITE |  FA_OPEN_ALWAYS);
+    ROM_IntMasterEnable();
+    return ;
+
+}
+void MillisecLogClose(void)
+{
+    FRESULT Fresult = FR_OK;
+    if (FileHandle == 0)
+        Fresult = FR_DENIED;
+    else
+    {
+        storeData = false;
+        ROM_IntMasterDisable();
+        Fresult = f_close(FileHandle);
+        my_free(FileHandle);
+        FileHandle = 0;
+        ROM_IntMasterEnable();
+    }
+    return ;
+
+}
+#endif
+void MillisecInterrupt(UArg arg0)
+{
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+    HundredMicroTimerInterrupt(arg0);
+#else
+    OneMilliSecondMillisecInterrupt(arg0);
+#endif
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 517716c08..7e2af1079 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -31,11 +31,20 @@ uint32_t getADCData(int DeviceId);
 */
 float getSensorSpeedData(void);
 uint32_t getDrawerFansStatus(void);
+uint32_t getSystemFansStatus(void);
 
 void MillisecInit(void);
 void MillisecStop(void);
 void MillisecStart(void);
 
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+void SaveLogData(void);
+void MillisecLogInit(void);
+void MillisecLogClose(void);
+
+#endif
+
+
 extern bool watchdogCriticalAlarm;
 
 extern Task_Handle Millisecond_Task_Handle;