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

3 files changed, 27 insertions, 6 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 3fcd988bd..1bd37d15c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -129,6 +129,11 @@ typedef struct DiagnosticsMessage{
     uint8_t messageData[20];
 }DiagnosticsMessageStruc;
 
+#define MAX_DIAG_LOG 100
+int diag_index=0;
+uint32_t diag_size[MAX_DIAG_LOG];
+uint32_t diag_time[MAX_DIAG_LOG];
+
 /******************** CODE  ********************************************/
 //**********************************************************************
 
@@ -587,7 +592,17 @@ void SendDiagnostics(void)
             {
                 LOG_ERROR(Task_self(),"my_malloc failed");
                 DiagnosticsStop();
+                CommunicationMailboxFlush();
+            }
+            else
+            {
+                //Report("Diagnostic Messgage",__FILE__,__LINE__,container_size,RpWarning,msec_millisecondCounter,  msec_millisecondCounter);
             }
+            diag_size[diag_index] = container_size;
+            diag_time[diag_index] = msec_millisecondCounter;
+            if (diag_index++>=MAX_DIAG_LOG)
+                diag_index = 0;
+
         }
         else
         {
@@ -612,8 +627,14 @@ uint32_t DiagnosticsStart(void)
         if (DiagnosticRequestAccepted == true)
         {
             DiagnosticsActive = true;
-            DiagnosticsControlId = AddControlCallback(Diagnostics_ControlTrigger,DiagnosticLimit,DiagnosticsEmptyCBFunction,0,0,0);
-            Diagnostics10MSControlId = AddControlCallback(Diagnostics_TenMiliControlTrigger,DiagnosticFastLimit,DiagnosticsEmptyCBFunction,0,0,0);
+            if (DiagnosticsControlId == 0xFF)
+                DiagnosticsControlId = AddControlCallback(Diagnostics_ControlTrigger,DiagnosticLimit,DiagnosticsEmptyCBFunction,0,0,0);
+            else
+                LOG_ERROR(DiagnosticsControlId,"Diagnostics restarted");
+            if (Diagnostics10MSControlId == 0xFF)
+                Diagnostics10MSControlId = AddControlCallback(Diagnostics_TenMiliControlTrigger,DiagnosticFastLimit,DiagnosticsEmptyCBFunction,0,0,0);
+            else
+                LOG_ERROR(DiagnosticsControlId,"Diagnostics restarted");
        }
     }
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index b3ed6fde0..58d05f383 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -163,7 +163,7 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
             {
                 if ( Motor_Id_to_LS_IdUp[MotorId] != MAX_GPI)
                 {
-                    MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdUp[MotorId], MotorHomingRequestCallback);
+                    MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdDown[MotorId], MotorHomingRequestCallback);
                     ustrncpy (HomingToken[MotorId], requestContainer->token,36);
                     return OK;
                 }
@@ -172,7 +172,7 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
             {
                 if ( Motor_Id_to_LS_IdDown[MotorId] != MAX_GPI)
                 {
-                    MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdDown[MotorId], MotorHomingRequestCallback);
+                    MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdUp[MotorId], MotorHomingRequestCallback);
                     ustrncpy (HomingToken[MotorId], requestContainer->token,36);
                     return OK;
                 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsJogging.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsJogging.c
index 5a709e4a4..bc3630698 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsJogging.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsJogging.c
@@ -65,10 +65,10 @@ uint32_t MotorJoggingRequestFunc(MessageContainer* requestContainer)
                 switch (request->direction)
                 {
                     case MOTOR_DIRECTION__Forward:
-                        direction = MotorsCfg[MotorId].directionthreadwize;
+                        direction = 1-MotorsCfg[MotorId].directionthreadwize;
                         break;
                     case MOTOR_DIRECTION__Backward:
-                        direction = 1-MotorsCfg[MotorId].directionthreadwize;
+                        direction = MotorsCfg[MotorId].directionthreadwize;
                         break;
                 }
                 MotorSetDirection(MotorId,direction);