Winder bugs. improved trace, improved control (callback invoked after time requested elabsed)

2 files changed, 30 insertions, 7 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index ed1e73bef..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
@@ -115,8 +116,8 @@ void ControlInit(void)
     Error_Block eb;
 
     //Mailbox_Params_init(&ControlMsgQ);
-    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
-    TenControlMsgQ = 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;
 
@@ -197,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);
@@ -222,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");
 
@@ -364,6 +366,24 @@ uint32_t ControlLowLoop(uint32_t tick)
     {
         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;
@@ -422,8 +442,10 @@ uint32_t ControlLowLoop(uint32_t tick)
                     memset (&ControlArray[Device_i],0,sizeof(ControlDeviceStruc) );
                     break;
                 default:
+                    //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
@@ -434,7 +456,7 @@ uint32_t ControlLowLoop(uint32_t tick)
                             LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
-            } //switch
+            } //switch*/
         } //if control active
     }  //for
     //ROM_IntMasterEnable();
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