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

4 files changed, 55 insertions, 40 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index f9f075477..7ffba4d69 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -516,15 +516,19 @@ void SendEventNotifications(void)
     responseContainer.continuous = true;
 
     uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
-    size_t container_size = message_container__pack(&responseContainer, container_buffer);
-
-    if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full
+    if (container_buffer)
     {
-        AlarmHandlingToken[0] = 0;
+        size_t container_size = message_container__pack(&responseContainer, container_buffer);
+
+        if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full
+        {
+            AlarmHandlingToken[0] = 0;
+        }
     }
     if (response.events)
         free (response.events);
-    free(responseContainer.data.data);
+    if (responseContainer.data.data)
+        free(responseContainer.data.data);
 }
 
 uint32_t StartEventsNotificationRequestFunc(MessageContainer* requestContainer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index a85e1cfde..495b7bded 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -476,23 +476,26 @@ void SendDiagnostics(void)
     diagnosticsresponseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
     diagnosticsresponseContainer.continuous = true;
 
-    diagnosticscontainer_buffer = 0;
-    diagnosticscontainer_buffer = malloc(message_container__get_packed_size(&diagnosticsresponseContainer));
-    if (diagnosticscontainer_buffer)
+    if (diagnosticsresponseContainer.data.data)
     {
-        size_t container_size = message_container__pack(&diagnosticsresponseContainer, diagnosticscontainer_buffer);
-        //memmap();
-        free(diagnosticsresponseContainer.data.data);
-        //free(DiagnosticsMonitor.dispenserspressure);
-        //free(DiagnosticsMonitor.dispensersmotorsfrequency);
-        if (SendChars(diagnosticscontainer_buffer, container_size) == false) //comm tx mailbox full
+        diagnosticscontainer_buffer = 0;
+        diagnosticscontainer_buffer = malloc(message_container__get_packed_size(&diagnosticsresponseContainer));
+        if (diagnosticscontainer_buffer)
         {
-            DiagnosticsStop();
+            size_t container_size = message_container__pack(&diagnosticsresponseContainer, diagnosticscontainer_buffer);
+            //memmap();
+            //free(DiagnosticsMonitor.dispenserspressure);
+            //free(DiagnosticsMonitor.dispensersmotorsfrequency);
+            if (SendChars(diagnosticscontainer_buffer, container_size) == false) //comm tx mailbox full
+            {
+                DiagnosticsStop();
+            }
         }
-    }
-    else
-    {
-        LOG_ERROR(Task_self(),"malloc failed");
+        else
+        {
+            LOG_ERROR(Task_self(),"malloc failed");
+        }
+        free(diagnosticsresponseContainer.data.data);
     }
     DiagnosticsReset();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index af8932872..70ede0916 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -183,6 +183,7 @@ uint32_t DispenserHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
 
     //close dry air valve in the dispenser
     Valve_Set((Valves_t) (DispenserId+VALVE_2W_MID_AIR_LF), Atm_MidTank_OFF);
+    MotorSetMicroStep(deviceID, MotorsCfg[deviceID].microstep);
 
     responseContainer = createContainer(MESSAGE_TYPE__DispenserHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
     responseContainer.continuous = true;
@@ -230,6 +231,7 @@ uint32_t DispenserHomingRequestFunc(MessageContainer* requestContainer)
                 //open dry air valve in the dispenser
                 Valve_Set((Valves_t) (request->index+VALVE_2W_MID_AIR_LF), Atm_MidTank_ON);
 
+                MotorSetMicroStep(MotorId, 1);
 
                 MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_Id[MotorId], DispenserHomingRequestCallback);
                 return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index f9eb69de7..54a47e927 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -28,6 +28,7 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 #include "modules/General/process.h"
+#include "Control/MillisecTask.h"
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
@@ -209,34 +210,41 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
 return OK;
 }
+float SpeedSamples[MAX_CONTROL_SAMPLES] = {0};
+
 uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
     int index=MAX_THREAD_MOTORS_NUM;
+    int32_t i, avreageSampleValue = 0;
+    //double tempcalcspeed = 0;
+    uint32_t calculated_speed;
+    float speed = getSensorSpeedData();
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
         return 0xFFFFFFFF;
     }
     index = IfIndex&0xFF;
-/*    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ControlIdtoMotorId[i] == deviceID)
-        {
-            index = i;
-            break;
-        }
-    if (index==MAX_THREAD_MOTORS_NUM)
-    {
-        LOG_ERROR (deviceID, "No motor  for device");
-        return 0xFFFFFFFF;
-    }
-    */
+    SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue);
+    MotorSamplePointer[index]++;
+    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
+        MotorSamplePointer[index] = 0;
+    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+        avreageSampleValue += SpeedSamples[i];
+    avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
     if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
     {
         MotorControlConfig[index].m_mesuredParam = ReadValue;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
+        calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
+        {
+            CurrentControlledSpeed[index] = calculated_speed;
+            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+        }
     }
  return OK;
 }
@@ -442,6 +450,11 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     return OK;
 }
 bool InitialProcess = false;
+uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
+
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
@@ -515,17 +528,10 @@ bool InitialProcess = false;
                 }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             }
-            /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
-    //            continue;
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
-            else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
+//            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+//                AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,ThreadEmptyCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 continue;
-                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
-//            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-//                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
     }
     //testDancersControl();
     PrepareReady(Module_Thread,ModuleDone);