Encoder handling has been updated

OZE_Sensor/Core/Inc/meas_tasks.h

@@ -16,35 +16,31 @@
 
 #define GAIN_AUTO_CORRECTION
 
-struct _LimiterSwitchData {
-    uint16_t gpioPin;
-    GPIO_PinState pinState;
+struct _EncoderTaskArg
+{
+	uint16_t dbgLed;
+	float* currentPosition;
+	float* pvEncoder;
+	uint32_t initPinStates;
+	osMessageQueueId_t dataQueue;
 };
 
-typedef struct _LimiterSwitchData LimiterSwitchData;
-
-enum _EncoderDirection { encoderCW, encoderCCW };
-typedef enum _EncoderDirection EncoderDirection;
-
-enum _EncoderAxe { encoderAxeX, encoderAxeY };
-typedef enum _EncoderAxe EncoderAxe;
-
-struct _EncoderData {
-    EncoderAxe axe;
-    EncoderDirection direction;
-};
-
-typedef struct _EncoderData EncoderData;
+typedef struct _EncoderTaskArg EncoderTaskArg;
 
 extern osMessageQueueId_t adc1MeasDataQueue;
 extern osMessageQueueId_t adc2MeasDataQueue;
 extern osMessageQueueId_t adc3MeasDataQueue;
 extern osMessageQueueId_t limiterSwitchDataQueue;
-extern osMessageQueueId_t encoderDataQueue;
 
 extern osMutexId_t resMeasurementsMutex;
 extern osMutexId_t sensorsInfoMutex;
 
+extern osThreadId_t encoderXTaskHandle;
+extern osThreadId_t encoderYTaskHandle;
+
+extern EncoderTaskArg encoderXTaskArg;
+extern EncoderTaskArg encoderYTaskArg;
+
 void MeasTasksInit (void);
 void ADC1MeasTask (void* arg);
 void ADC2MeasTask (void* arg);

OZE_Sensor/Core/Inc/node-red-config.h

@@ -15,8 +15,8 @@
 
 #define UART_TASK_LOGS FEATURE_ON
 
-#define ENCODER_X_IMP_PER_TURN 2000
-#define ENCODER_Y_IMP_PER_TURN 2000
+#define ENCODER_X_IMP_PER_TURN (4 * 200) // (4 interrupts) * (Encoder positions count for 360 deggres)
+#define ENCODER_Y_IMP_PER_TURN (4 * 200) // (4 interrupts) * (Encoder positions count for 360 degress)
 
 #define MAX_X_AXE_ANGLE 360	// Max angle value for 100% range
 #define MAX_Y_AXE_ANGLE 360 // Max angle value for 100% range
@@ -26,8 +26,8 @@
 #define ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT 5 // percentage value for motor slow down/speed up boundary
 #define AXE_X_MIDDLE_VALUE 50 // percentage value of position for C limiter
 #define AXE_Y_MIDDLE_VALUE 50 // percentage value of position for C limiter
-#define TIME_MS_FOR_MOTOR_SPEED_LIMIT 1000
-#define NO_MOVE_TIMEOUT_MS 1000 // timeout for no movement detection
+#define TIME_MS_FOR_MOTOR_SPEED_LIMIT 5000
+#define NO_MOVE_TIMEOUT_MS 5000 // timeout for no movement detection
 
 #define WATCHDOG_ENABLED
 //#define USER_MOCKS

OZE_Sensor/Core/Inc/stm32h7xx_it.h

@@ -56,9 +56,6 @@ void RCC_IRQHandler(void);
 void DMA1_Stream0_IRQHandler(void);
 void DMA1_Stream1_IRQHandler(void);
 void DMA1_Stream2_IRQHandler(void);
-void EXTI9_5_IRQHandler(void);
-void TIM2_IRQHandler(void);
-void TIM4_IRQHandler(void);
 void USART1_IRQHandler(void);
 void EXTI15_10_IRQHandler(void);
 void TIM6_DAC_IRQHandler(void);

OZE_Sensor/Core/Src/main.c

@@ -23,6 +23,7 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "string.h"
+#include <stdio.h>
 
 #include "uart_tasks.h"
 #include "mock_tasks.h"
@@ -69,9 +70,7 @@ IWDG_HandleTypeDef hiwdg1;
 RNG_HandleTypeDef hrng;
 
 TIM_HandleTypeDef htim1;
-TIM_HandleTypeDef htim2;
 TIM_HandleTypeDef htim3;
-TIM_HandleTypeDef htim4;
 TIM_HandleTypeDef htim8;
 
 UART_HandleTypeDef huart8;
@@ -141,12 +140,10 @@ static void MX_UART8_Init(void);
 static void MX_CRC_Init(void);
 static void MX_ADC2_Init(void);
 static void MX_ADC3_Init(void);
-static void MX_TIM2_Init(void);
 static void MX_TIM1_Init(void);
 static void MX_TIM3_Init(void);
 static void MX_DAC1_Init(void);
 static void MX_COMP1_Init(void);
-static void MX_TIM4_Init(void);
 static void MX_TIM8_Init(void);
 static void MX_IWDG1_Init(void);
 void StartDefaultTask(void *argument);
@@ -172,10 +169,16 @@ int __io_putchar(int ch)
 
 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
 {
-	LimiterSwitchData limiterSwitchData = { 0 };
-	limiterSwitchData.gpioPin = GPIO_Pin;
-	limiterSwitchData.pinState = HAL_GPIO_ReadPin(GPIOD, GPIO_Pin);
-	osMessageQueuePut(limiterSwitchDataQueue, &limiterSwitchData, 0, 0);
+	if((GPIO_Pin == GPIO_PIN_14) || (GPIO_Pin == GPIO_PIN_15))
+	{
+		uint32_t pinStates = ((HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_15) << 1) | HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_14)) & 0x3;
+		osMessageQueuePut(encoderXTaskArg.dataQueue, &pinStates, 0, 0);
+	}
+	else if ((GPIO_Pin == GPIO_PIN_10) || (GPIO_Pin == GPIO_PIN_11))
+	{
+		uint32_t pinStates = ((HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) << 1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_10)) & 0x3;
+		osMessageQueuePut(encoderYTaskArg.dataQueue, &pinStates, 0, 0);
+	}
 }
 /* USER CODE END 0 */
 
@@ -230,12 +233,10 @@ int main(void)
   MX_CRC_Init();
   MX_ADC2_Init();
   MX_ADC3_Init();
-  MX_TIM2_Init();
   MX_TIM1_Init();
   MX_TIM3_Init();
   MX_DAC1_Init();
   MX_COMP1_Init();
-  MX_TIM4_Init();
   MX_TIM8_Init();
 #ifdef WATCHDOG_ENABLED
   MX_IWDG1_Init();
@@ -949,68 +950,6 @@ static void MX_TIM1_Init(void)
 }
 
 /**
-  * @brief TIM2 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM2_Init(void)
-{
-
-  /* USER CODE BEGIN TIM2_Init 0 */
-
-  /* USER CODE END TIM2_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-  TIM_IC_InitTypeDef sConfigIC = {0};
-
-  /* USER CODE BEGIN TIM2_Init 1 */
-
-  /* USER CODE END TIM2_Init 1 */
-  htim2.Instance = TIM2;
-  htim2.Init.Prescaler = 9999;
-  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim2.Init.Period = 2999;
-  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV2;
-  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
-  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_TIM_IC_Init(&htim2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_ENABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
-  sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
-  sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
-  sConfigIC.ICFilter = 0;
-  if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_TIM_IC_ConfigChannel(&htim2, &sConfigIC, TIM_CHANNEL_4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM2_Init 2 */
-
-  /* USER CODE END TIM2_Init 2 */
-
-}
-
-/**
   * @brief TIM3 Initialization Function
   * @param None
   * @retval None
@@ -1077,68 +1016,6 @@ static void MX_TIM3_Init(void)
 }
 
 /**
-  * @brief TIM4 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM4_Init(void)
-{
-
-  /* USER CODE BEGIN TIM4_Init 0 */
-
-  /* USER CODE END TIM4_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-  TIM_IC_InitTypeDef sConfigIC = {0};
-
-  /* USER CODE BEGIN TIM4_Init 1 */
-
-  /* USER CODE END TIM4_Init 1 */
-  htim4.Instance = TIM4;
-  htim4.Init.Prescaler = 9999;
-  htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim4.Init.Period = 2999;
-  htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV2;
-  htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
-  if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_TIM_IC_Init(&htim4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
-  sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
-  sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
-  sConfigIC.ICFilter = 0;
-  if (HAL_TIM_IC_ConfigChannel(&htim4, &sConfigIC, TIM_CHANNEL_3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_TIM_IC_ConfigChannel(&htim4, &sConfigIC, TIM_CHANNEL_4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM4_Init 2 */
-
-  /* USER CODE END TIM4_Init 2 */
-
-}
-
-/**
   * @brief TIM8 Initialization Function
   * @param None
   * @retval None
@@ -1339,17 +1216,23 @@ static void MX_GPIO_Init(void)
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
 
+  /*Configure GPIO pins : PB10 PB11 */
+  GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
   /*Configure GPIO pins : PD8 PD9 PD10 PD11
-                           PD12 PD13 */
+                           PD12 PD13 PD3 */
   GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11
-                          |GPIO_PIN_12|GPIO_PIN_13;
-  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+                          |GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_3;
+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PD3 */
-  GPIO_InitStruct.Pin = GPIO_PIN_3;
-  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+  /*Configure GPIO pins : PD14 PD15 */
+  GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
@@ -1361,9 +1244,6 @@ static void MX_GPIO_Init(void)
   HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
   /* EXTI interrupt init*/
-  HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
-  HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
-
   HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
 
@@ -1413,91 +1293,6 @@ void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc)
 	}osTimerStop (debugLedTimerHandle);
 }
 
-void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
-{
-	if (htim->Instance == TIM4)
-	{
-		if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3)
-		{
-			if(encoderXChannelB > 0)
-			{
-				encoderXChannelA = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_3);
-			}
-			else
-			{
-				encoderXChannelA = 1;
-				__HAL_TIM_SET_COUNTER(htim,0);
-			}
-		} else if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
-		{
-			if(encoderXChannelA > 0)
-			{
-				encoderXChannelB = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4);
-			}
-			else
-			{
-				encoderXChannelB = 1;
-				__HAL_TIM_SET_COUNTER(htim,0);
-			}
-		}
-		if((encoderXChannelA != 0) && (encoderXChannelB != 0))
-		{
-			EncoderData encoderData = { 0 };
-			encoderData.axe = encoderAxeX;
-			encoderData.direction = encoderXChannelA - encoderXChannelB < 0 ? encoderCW : encoderCCW;
-			if (encoderData.direction == encoderCCW)
-			{
-				asm("nop;");
-			}
-			osMessageQueuePut(encoderDataQueue, &encoderData, 0, 0);
-			encoderXChannelA = 0;
-			encoderXChannelB = 0;
-		}
-	} else if (htim->Instance == TIM2)
-	{
-			if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3)
-			{
-				if(encoderYChannelB > 0)
-				{
-					encoderYChannelA = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_3);
-				}
-				else
-				{
-					encoderYChannelA = 1;
-					__HAL_TIM_SET_COUNTER(htim,0);
-				}
-			} else if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
-			{
-				if(encoderYChannelA > 0)
-				{
-					encoderYChannelB = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4);
-				}
-				else
-				{
-					encoderYChannelB = 1;
-					__HAL_TIM_SET_COUNTER(htim,0);
-				}
-			}
-			if((encoderYChannelA != 0) && (encoderYChannelB != 0))
-			{
-				EncoderData encoderData = { 0 };
-				encoderData.axe = encoderAxeY;
-				encoderData.direction = encoderYChannelA - encoderYChannelB < 0 ? encoderCW : encoderCCW;
-				if (encoderData.direction == encoderCCW)
-				{
-					asm("nop;");
-				}
-				if (encoderData.direction == encoderCW)
-				{
-					asm("nop;");
-				}
-				osMessageQueuePut(encoderDataQueue, &encoderData, 0, 0);
-				encoderYChannelA = 0;
-				encoderYChannelB = 0;
-			}
-		}
-}
-
 /* USER CODE END 4 */
 
 /* USER CODE BEGIN Header_StartDefaultTask */
@@ -1525,30 +1320,6 @@ void StartDefaultTask(void *argument)
   {
 	  Error_Handler();
   }
-  if(HAL_TIM_Base_Start_IT(&htim2) != HAL_OK)
-  {
-	  Error_Handler();
-  }
-  if(HAL_TIM_Base_Start_IT(&htim4) != HAL_OK)
-  {
-	  Error_Handler();
-  }
-  if(HAL_TIM_IC_Start_IT(&htim4, TIM_CHANNEL_3) != HAL_OK)
-  {
-	  Error_Handler();
-  }
-  if(HAL_TIM_IC_Start_IT(&htim4, TIM_CHANNEL_4) != HAL_OK)
-  {
-	  Error_Handler();
-  }
-  if(HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_3) != HAL_OK)
-  {
-	  Error_Handler();
-  }
-  if(HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_4) != HAL_OK)
-  {
-	  Error_Handler();
-  }
   if(HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Data.adcDataBuffer, ADC1LastData) != HAL_OK)
   {
 	  Error_Handler();

OZE_Sensor/Core/Src/meas_tasks.c

@@ -6,9 +6,10 @@
  */
 
 #include <math.h>
+#include <stdio.h>
 
-#include "meas_tasks.h"
 #include "adc_buffers.h"
+#include "meas_tasks.h"
 #include "measurements.h"
 #include "node-red-config.h"
 #include "peripherial.h"
@@ -30,13 +31,12 @@ osThreadId_t adc1MeasTaskHandle      = NULL;
 osThreadId_t adc2MeasTaskHandle      = NULL;
 osThreadId_t adc3MeasTaskHandle      = NULL;
 osThreadId_t limiterSwitchTaskHandle = NULL;
-osThreadId_t encoderTaskHandle      = NULL;
+osThreadId_t encoderXTaskHandle      = NULL;
+osThreadId_t encoderYTaskHandle      = NULL;
 
-osMessageQueueId_t adc1MeasDataQueue      = NULL;
-osMessageQueueId_t adc2MeasDataQueue      = NULL;
-osMessageQueueId_t adc3MeasDataQueue      = NULL;
-osMessageQueueId_t limiterSwitchDataQueue = NULL;
-osMessageQueueId_t encoderDataQueue      = NULL;
+osMessageQueueId_t adc1MeasDataQueue = NULL;
+osMessageQueueId_t adc2MeasDataQueue = NULL;
+osMessageQueueId_t adc3MeasDataQueue = NULL;
 
 osMutexId_t vRefmVMutex;
 osMutexId_t resMeasurementsMutex;
@@ -45,16 +45,17 @@ osMutexId_t ILxRefMutex;
 
 volatile uint32_t vRefmV = 3000;
 
-RESMeasurements resMeasurements = { 0 };
-SesnorsInfo sensorsInfo         = { 0 };
+RESMeasurements resMeasurements __attribute__ ((aligned (32))) = { 0 };
+SesnorsInfo sensorsInfo __attribute__ ((aligned (32)))         = { 0 };
 
-uint16_t ILxRef[CURRENTS_COUNT] = { 0 };
+uint16_t ILxRef[CURRENTS_COUNT] __attribute__ ((aligned (32))) = { 0 };
+EncoderTaskArg encoderXTaskArg __attribute__ ((aligned (32)))  = { 0 };
+EncoderTaskArg encoderYTaskArg __attribute__ ((aligned (32)))  = { 0 };
 
 extern TIM_HandleTypeDef htim3;
 extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
 extern osTimerId_t motorXTimerHandle;
 extern osTimerId_t motorYTimerHandle;
-//extern osMutexId_t positionSettingMutex;
 
 
 void MeasTasksInit (void) {
@@ -83,19 +84,29 @@ void MeasTasksInit (void) {
     adc2MeasTaskHandle = osThreadNew (ADC2MeasTask, NULL, &osThreadAttradc2MeasTask);
     adc3MeasTaskHandle = osThreadNew (ADC3MeasTask, NULL, &osThreadAttradc3MeasTask);
 
-    limiterSwitchDataQueue = osMessageQueueNew (8, sizeof (LimiterSwitchData), NULL);
-
     osThreadAttr_t osThreadAttradc1LimiterSwitchTask = { 0 };
     osThreadAttradc1LimiterSwitchTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
     osThreadAttradc1LimiterSwitchTask.priority       = (osPriority_t)osPriorityNormal;
     limiterSwitchTaskHandle                          = osThreadNew (LimiterSwitchTask, NULL, &osThreadAttradc1LimiterSwitchTask);
 
-    encoderDataQueue = osMessageQueueNew (16, sizeof (EncoderData), NULL);
+    encoderXTaskArg.dbgLed                   = DBG_LED2;
+    encoderXTaskArg.pvEncoder                = &(sensorsInfo.pvEncoderX);
+    encoderXTaskArg.currentPosition          = &(sensorsInfo.currentXPosition);
+    osMessageQueueAttr_t encoderMsgQueueAttr = { 0 };
+    encoderXTaskArg.dataQueue                = osMessageQueueNew (16, sizeof (uint32_t), &encoderMsgQueueAttr);
+    encoderXTaskArg.initPinStates = ((HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_15) << 1) | HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_14)) & 0x3;
+
+    encoderYTaskArg.dbgLed          = DBG_LED3;
+    encoderYTaskArg.pvEncoder       = &(sensorsInfo.pvEncoderY);
+    encoderYTaskArg.currentPosition = &(sensorsInfo.currentYPosition);
+    encoderYTaskArg.dataQueue       = osMessageQueueNew (16, sizeof (uint32_t), &encoderMsgQueueAttr);
+    encoderYTaskArg.initPinStates = ((HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) << 1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_10)) & 0x3;
 
     osThreadAttr_t osThreadAttrEncoderTask = { 0 };
     osThreadAttrEncoderTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
-    osThreadAttrEncoderTask.priority       = (osPriority_t)osPriorityNormal;
-    encoderTaskHandle                      = osThreadNew (EncoderTask, encoderDataQueue, &osThreadAttrEncoderTask);
+    osThreadAttrEncoderTask.priority       = (osPriority_t)osPriorityRealtime;
+    encoderXTaskHandle                     = osThreadNew (EncoderTask, &encoderXTaskArg, &osThreadAttrEncoderTask);
+    encoderYTaskHandle                     = osThreadNew (EncoderTask, &encoderYTaskArg, &osThreadAttrEncoderTask);
 }
 
 void ADC1MeasTask (void* arg) {
@@ -251,121 +262,122 @@ void ADC3MeasTask (void* arg) {
 }
 
 void LimiterSwitchTask (void* arg) {
-    LimiterSwitchData limiterSwitchData = { 0 };
-    limiterSwitchData.gpioPin           = GPIO_PIN_8;
-    for (uint8_t i = 0; i < 6; i++) {
-        limiterSwitchData.pinState = HAL_GPIO_ReadPin (GPIOD, limiterSwitchData.gpioPin);
-        osMessageQueuePut (limiterSwitchDataQueue, &limiterSwitchData, 0, 0);
-        limiterSwitchData.gpioPin = limiterSwitchData.gpioPin << 1;
-        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-        	sensorsInfo.positionXWeak = 1;
-        	sensorsInfo.positionYWeak = 1;
-        	osMutexRelease (sensorsInfoMutex);
-        }
+    uint8_t limitXSwitchDownPrevState   = 0;
+    uint8_t limitXSwitchCenterPrevState = 0;
+    uint8_t limitXSwitchUpPrevState     = 0;
+    uint8_t limitYSwitchDownPrevState   = 0;
+    uint8_t limitYSwitchCenterPrevState = 0;
+    uint8_t limitYSwitchUpPrevState     = 0;
+    uint8_t pinStates                   = 0;
+    uint8_t limiterXTriggered           = 0;
+    uint8_t limiterYTriggered           = 0;
+    if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+        sensorsInfo.positionXWeak = 1;
+        sensorsInfo.positionYWeak = 1;
+        osMutexRelease (sensorsInfoMutex);
     }
     while (pdTRUE) {
-        osMessageQueueGet (limiterSwitchDataQueue, &limiterSwitchData, 0, osWaitForever);
+        osDelay (pdMS_TO_TICKS (100));
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-            switch (limiterSwitchData.gpioPin) {
-            case GPIO_PIN_8:
-            	sensorsInfo.limitYSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
-            	if (sensorsInfo.limitYSwitchCenter == 1)
-            	{
-            		sensorsInfo.currentYPosition = AXE_Y_MIDDLE_VALUE;
-            		sensorsInfo.positionYWeak = 0;
-            	}
-            	break;
-            case GPIO_PIN_9:
-            	sensorsInfo.limitYSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
-            	if (sensorsInfo.limitYSwitchDown == 1)
-            	{
-            		sensorsInfo.currentYPosition = 0;
-            		sensorsInfo.positionYWeak = 0;
-            	}
-            	break;
-            case GPIO_PIN_10:
-            	sensorsInfo.limitXSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
-            	if (sensorsInfo.limitXSwitchCenter == 1)
-				{
-					sensorsInfo.currentXPosition = AXE_X_MIDDLE_VALUE;
-					sensorsInfo.positionXWeak = 0;
-				}
-            	break;
-            case GPIO_PIN_11:
-            	sensorsInfo.limitYSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
-            	if (sensorsInfo.limitYSwitchUp == 1)
-				{
-					sensorsInfo.currentYPosition = 100;
-					sensorsInfo.positionYWeak = 0;
-				}
-            	break;
-            case GPIO_PIN_12:
-            	sensorsInfo.limitXSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
-            	if (sensorsInfo.limitXSwitchUp == 1)
-				{
-					sensorsInfo.currentXPosition = 100;
-					sensorsInfo.positionXWeak = 0;
-				}
-            	break;
-            case GPIO_PIN_13:
-            	sensorsInfo.limitXSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
-            	if (sensorsInfo.limitXSwitchDown == 1)
-				{
-					sensorsInfo.currentXPosition = 0;
-					sensorsInfo.positionXWeak = 0;
-				}
-            	break;
-            default: break;
+            sensorsInfo.limitXSwitchDown = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_13);
+            pinStates                    = (limitXSwitchDownPrevState << 1) | sensorsInfo.limitXSwitchDown;
+            if ((pinStates & 0x3) == 0x1) {
+                limiterXTriggered            = 1;
+                sensorsInfo.currentXPosition = 0;
+                sensorsInfo.positionXWeak    = 0;
+            }
+            limitXSwitchDownPrevState = sensorsInfo.limitXSwitchDown;
+
+            sensorsInfo.limitXSwitchUp = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_12);
+            pinStates                  = (limitXSwitchUpPrevState << 1) | sensorsInfo.limitXSwitchUp;
+            if ((pinStates & 0x3) == 0x1) {
+                limiterXTriggered            = 1;
+                sensorsInfo.currentXPosition = 100;
+                sensorsInfo.positionXWeak    = 0;
             }
-            if ((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) {
+            limitXSwitchUpPrevState = sensorsInfo.limitXSwitchUp;
+
+            sensorsInfo.limitXSwitchCenter = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_10);
+            pinStates                      = (limitXSwitchCenterPrevState << 1) | sensorsInfo.limitXSwitchCenter;
+            if ((pinStates & 0x3) == 0x1) {
+                sensorsInfo.currentXPosition = AXE_X_MIDDLE_VALUE;
+                sensorsInfo.positionXWeak    = 0;
+            }
+            limitXSwitchCenterPrevState = sensorsInfo.limitXSwitchCenter;
+
+            sensorsInfo.limitYSwitchDown = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_9);
+            pinStates                    = (limitYSwitchDownPrevState << 1) | sensorsInfo.limitYSwitchDown;
+            if ((pinStates & 0x3) == 0x1) {
+                limiterYTriggered            = 1;
+                sensorsInfo.currentYPosition = 0;
+                sensorsInfo.positionYWeak    = 0;
+            }
+            limitYSwitchDownPrevState = sensorsInfo.limitYSwitchDown;
+
+            sensorsInfo.limitYSwitchUp = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_11);
+            pinStates                  = (limitYSwitchUpPrevState << 1) | sensorsInfo.limitYSwitchUp;
+            if ((pinStates & 0x3) == 0x1) {
+                limiterYTriggered            = 1;
+                sensorsInfo.currentYPosition = 100;
+                sensorsInfo.positionYWeak    = 0;
+            }
+            limitYSwitchUpPrevState = sensorsInfo.limitYSwitchUp;
+
+            sensorsInfo.limitYSwitchCenter = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_8);
+            pinStates                      = (limitYSwitchCenterPrevState << 1) | sensorsInfo.limitYSwitchCenter;
+            if ((pinStates & 0x3) == 0x1) {
+                sensorsInfo.currentYPosition = AXE_Y_MIDDLE_VALUE;
+                sensorsInfo.positionYWeak    = 0;
+            }
+            limitYSwitchCenterPrevState = sensorsInfo.limitYSwitchCenter;
+            if (((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) && (limiterXTriggered == 1)) {
                 sensorsInfo.motorXStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, 0, 0, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
             }
-            if ((sensorsInfo.limitYSwitchDown == 1) || (sensorsInfo.limitYSwitchUp == 1)) {
+            if (((sensorsInfo.limitYSwitchDown == 1) || (sensorsInfo.limitYSwitchUp == 1)) && (limiterYTriggered == 1)) {
                 sensorsInfo.motorYStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, 0, 0, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
             }
+            limiterXTriggered = 0;
+            limiterYTriggered = 0;
             osMutexRelease (sensorsInfoMutex);
         }
     }
 }
 
 void EncoderTask (void* arg) {
-    EncoderData encoderData         = { 0 };
-    osMessageQueueId_t encoderQueue = (osMessageQueueId_t)arg;
-    while (pdTRUE) {
-        osMessageQueueGet (encoderQueue, &encoderData, 0, osWaitForever);
-        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-            if (encoderData.axe == encoderAxeX) {
-                if (encoderData.direction == encoderCW) {
-                    sensorsInfo.pvEncoderX += 360.0 / ENCODER_X_IMP_PER_TURN;
-                } else {
-                    sensorsInfo.pvEncoderX -= 360.0 / ENCODER_X_IMP_PER_TURN;
-                    if(sensorsInfo.pvEncoderX < 0)
-                    {
-                    	sensorsInfo.pvEncoderX = 360.0 + sensorsInfo.pvEncoderX;
-                    }
-                }
-                sensorsInfo.pvEncoderX = fmodf(sensorsInfo.pvEncoderX, 360.0);
-                float currentPercentPos = 100 * sensorsInfo.pvEncoderX / MAX_X_AXE_ANGLE;
-                currentPercentPos = currentPercentPos < 0 ? 0 : currentPercentPos;
-                sensorsInfo.currentXPosition = currentPercentPos > 100 ? 100 : currentPercentPos;
-                DbgLEDToggle(DBG_LED2);
-            } else {
-                if (encoderData.direction == encoderCW) {
-                    sensorsInfo.pvEncoderY += 360.0 / ENCODER_Y_IMP_PER_TURN;
-                } else {
-                    sensorsInfo.pvEncoderY -= 360.0 / ENCODER_Y_IMP_PER_TURN;
-                    if(sensorsInfo.pvEncoderY < 0)
-                    {
-                    	sensorsInfo.pvEncoderY = 360.0 + sensorsInfo.pvEncoderY;
-                    }
-                }
-                sensorsInfo.pvEncoderY = fmodf(sensorsInfo.pvEncoderY, 360.0);
-                float currentPercentPos = 100 * sensorsInfo.pvEncoderY / MAX_X_AXE_ANGLE;
-				currentPercentPos = currentPercentPos < 0 ? 0 : currentPercentPos;
-				sensorsInfo.currentXPosition = currentPercentPos > 100 ? 100 : currentPercentPos;
-                DbgLEDToggle(DBG_LED3);
-            }
-            osMutexRelease (sensorsInfoMutex);
+	// 01 11 10 00
+    const uint32_t encoderStates[4] = { 0x00, 0x01, 0x03, 0x02 };
+    uint8_t step                    = 0;
+    EncoderTaskArg* encoderTaskArg  = (EncoderTaskArg*)arg;
+    uint32_t pinStates              = encoderTaskArg->initPinStates;
+    for (uint8_t i = 0; i < 4; i++) {
+        if (pinStates == encoderStates[i]) {
+            step = i;
+            break;
         }
     }
+    while (pdTRUE) {
+        float encoderValue = *encoderTaskArg->pvEncoder;
+        osMessageQueueGet (encoderTaskArg->dataQueue, &pinStates, 0, osWaitForever);
+		if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+			if (encoderStates[(step + 1) % 4] == pinStates) {
+				step++;
+				encoderValue += 360.0 / ENCODER_X_IMP_PER_TURN;
+//				printf ("Forward\n");
+			} else if (encoderStates[(step - 1) % 4] == pinStates) {
+				encoderValue -= 360.0 / ENCODER_X_IMP_PER_TURN;
+				if (encoderValue < 0) {
+					encoderValue = 360.0 + encoderValue;
+				}
+//				printf ("Reverse\n");
+				step--;
+			} else {
+				printf ("Forbidden\n");
+			}
+			step                             = step % 4;
+			*encoderTaskArg->pvEncoder       = fmodf (encoderValue, 360.0);
+			*encoderTaskArg->currentPosition = 100 * (*encoderTaskArg->pvEncoder) / MAX_X_AXE_ANGLE;
+			osMutexRelease (sensorsInfoMutex);
+		}
+        DbgLEDToggle (encoderTaskArg->dbgLed);
+    }
 }

OZE_Sensor/Core/Src/position_task.c

@@ -15,16 +15,16 @@
 #include <stdlib.h>
 #include <string.h>
 
-float positionXSetting = 0.0;
-float positionYSetting = 0.0;
+float positionXSetting __attribute__ ((aligned (32))) = 0.0;
+float positionYSetting __attribute__ ((aligned (32))) = 0.0;
 
-osMutexId_t positionSettingMutex;
+//osMutexId_t positionSettingMutex;
 
 osThreadId_t positionXControlTaskHandle = NULL;
 osThreadId_t positionYControlTaskHandle = NULL;
 
-PositionControlTaskInitArg positionXControlTaskInitArg = { 0 };
-PositionControlTaskInitArg positionYControlTaskInitArg = { 0 };
+PositionControlTaskInitArg positionXControlTaskInitArg __attribute__ ((aligned (32))) = { 0 };
+PositionControlTaskInitArg positionYControlTaskInitArg __attribute__ ((aligned (32))) = { 0 };
 
 extern osTimerId_t motorXTimerHandle;
 extern osTimerId_t motorYTimerHandle;
@@ -32,7 +32,7 @@ extern TIM_HandleTypeDef htim3;
 extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
 
 void PositionControlTaskInit (void) {
-    positionSettingMutex                           = osMutexNew (NULL);
+//    positionSettingMutex                           = osMutexNew (NULL);
     osThreadAttr_t osThreadAttrPositionControlTask = { 0 };
     osThreadAttrPositionControlTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
     osThreadAttrPositionControlTask.priority       = (osPriority_t)osPriorityNormal;
@@ -74,7 +74,7 @@ void PositionControlTaskInit (void) {
 void PositionControlTask (void* argument) {
     const int32_t PositionControlTaskTimeOut   = 100;
     PositionControlTaskInitArg* posCtrlTaskArg = (PositionControlTaskInitArg*)argument;
-    PositionControlTaskData posCtrlData        = { 0 };
+    PositionControlTaskData posCtrlData  __attribute__ ((aligned (32))) = { 0 };
     uint32_t motorStatus                       = 0;
     osStatus_t queueSatus;
     int32_t pwmValue              = MOTOR_START_STOP_PWM_VALUE;
@@ -100,10 +100,10 @@ void PositionControlTask (void* argument) {
 #endif
                 }
                 osMutexRelease (sensorsInfoMutex);
-                if (osMutexAcquire (positionSettingMutex, osWaitForever) == osOK) {
+//                if (osMutexAcquire (positionSettingMutex, osWaitForever) == osOK) {
                     *positionXControlTaskInitArg.positionSetting = posCtrlData.positionSettingValue;
-                    osMutexRelease (positionSettingMutex);
-                }
+//                    osMutexRelease (positionSettingMutex);
+//                }
             }
         } else if (queueSatus == osErrorTimeout) {
             if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {

OZE_Sensor/Core/Src/stm32h7xx_hal_msp.c

@@ -667,62 +667,7 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* htim_pwm)
 */
 void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 {
-  GPIO_InitTypeDef GPIO_InitStruct = {0};
-  if(htim_base->Instance==TIM2)
-  {
-  /* USER CODE BEGIN TIM2_MspInit 0 */
-
-  /* USER CODE END TIM2_MspInit 0 */
-    /* Peripheral clock enable */
-    __HAL_RCC_TIM2_CLK_ENABLE();
-
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-    /**TIM2 GPIO Configuration
-    PB10     ------> TIM2_CH3
-    PB11     ------> TIM2_CH4
-    */
-    GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
-    /* TIM2 interrupt Init */
-    HAL_NVIC_SetPriority(TIM2_IRQn, 5, 0);
-    HAL_NVIC_EnableIRQ(TIM2_IRQn);
-  /* USER CODE BEGIN TIM2_MspInit 1 */
-
-  /* USER CODE END TIM2_MspInit 1 */
-  }
-  else if(htim_base->Instance==TIM4)
-  {
-  /* USER CODE BEGIN TIM4_MspInit 0 */
-
-  /* USER CODE END TIM4_MspInit 0 */
-    /* Peripheral clock enable */
-    __HAL_RCC_TIM4_CLK_ENABLE();
-
-    __HAL_RCC_GPIOD_CLK_ENABLE();
-    /**TIM4 GPIO Configuration
-    PD14     ------> TIM4_CH3
-    PD15     ------> TIM4_CH4
-    */
-    GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15;
-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
-    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
-
-    /* TIM4 interrupt Init */
-    HAL_NVIC_SetPriority(TIM4_IRQn, 5, 0);
-    HAL_NVIC_EnableIRQ(TIM4_IRQn);
-  /* USER CODE BEGIN TIM4_MspInit 1 */
-
-  /* USER CODE END TIM4_MspInit 1 */
-  }
-  else if(htim_base->Instance==TIM8)
+  if(htim_base->Instance==TIM8)
   {
   /* USER CODE BEGIN TIM8_MspInit 0 */
 
@@ -826,47 +771,7 @@ void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* htim_pwm)
 */
 void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
 {
-  if(htim_base->Instance==TIM2)
-  {
-  /* USER CODE BEGIN TIM2_MspDeInit 0 */
-
-  /* USER CODE END TIM2_MspDeInit 0 */
-    /* Peripheral clock disable */
-    __HAL_RCC_TIM2_CLK_DISABLE();
-
-    /**TIM2 GPIO Configuration
-    PB10     ------> TIM2_CH3
-    PB11     ------> TIM2_CH4
-    */
-    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_10|GPIO_PIN_11);
-
-    /* TIM2 interrupt DeInit */
-    HAL_NVIC_DisableIRQ(TIM2_IRQn);
-  /* USER CODE BEGIN TIM2_MspDeInit 1 */
-
-  /* USER CODE END TIM2_MspDeInit 1 */
-  }
-  else if(htim_base->Instance==TIM4)
-  {
-  /* USER CODE BEGIN TIM4_MspDeInit 0 */
-
-  /* USER CODE END TIM4_MspDeInit 0 */
-    /* Peripheral clock disable */
-    __HAL_RCC_TIM4_CLK_DISABLE();
-
-    /**TIM4 GPIO Configuration
-    PD14     ------> TIM4_CH3
-    PD15     ------> TIM4_CH4
-    */
-    HAL_GPIO_DeInit(GPIOD, GPIO_PIN_14|GPIO_PIN_15);
-
-    /* TIM4 interrupt DeInit */
-    HAL_NVIC_DisableIRQ(TIM4_IRQn);
-  /* USER CODE BEGIN TIM4_MspDeInit 1 */
-
-  /* USER CODE END TIM4_MspDeInit 1 */
-  }
-  else if(htim_base->Instance==TIM8)
+  if(htim_base->Instance==TIM8)
   {
   /* USER CODE BEGIN TIM8_MspDeInit 0 */
 

OZE_Sensor/Core/Src/stm32h7xx_it.c

@@ -59,8 +59,6 @@ extern DMA_HandleTypeDef hdma_adc1;
 extern DMA_HandleTypeDef hdma_adc2;
 extern DMA_HandleTypeDef hdma_adc3;
 extern DAC_HandleTypeDef hdac1;
-extern TIM_HandleTypeDef htim2;
-extern TIM_HandleTypeDef htim4;
 extern UART_HandleTypeDef huart8;
 extern UART_HandleTypeDef huart1;
 extern TIM_HandleTypeDef htim6;
@@ -223,49 +221,6 @@ void DMA1_Stream2_IRQHandler(void)
 }
 
 /**
-  * @brief This function handles EXTI line[9:5] interrupts.
-  */
-void EXTI9_5_IRQHandler(void)
-{
-  /* USER CODE BEGIN EXTI9_5_IRQn 0 */
-
-  /* USER CODE END EXTI9_5_IRQn 0 */
-  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_8);
-  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_9);
-  /* USER CODE BEGIN EXTI9_5_IRQn 1 */
-
-  /* USER CODE END EXTI9_5_IRQn 1 */
-}
-
-/**
-  * @brief This function handles TIM2 global interrupt.
-  */
-void TIM2_IRQHandler(void)
-{
-  /* USER CODE BEGIN TIM2_IRQn 0 */
-
-  /* USER CODE END TIM2_IRQn 0 */
-  HAL_TIM_IRQHandler(&htim2);
-  /* USER CODE BEGIN TIM2_IRQn 1 */
-
-  /* USER CODE END TIM2_IRQn 1 */
-}
-
-/**
-  * @brief This function handles TIM4 global interrupt.
-  */
-void TIM4_IRQHandler(void)
-{
-  /* USER CODE BEGIN TIM4_IRQn 0 */
-
-  /* USER CODE END TIM4_IRQn 0 */
-  HAL_TIM_IRQHandler(&htim4);
-  /* USER CODE BEGIN TIM4_IRQn 1 */
-
-  /* USER CODE END TIM4_IRQn 1 */
-}
-
-/**
   * @brief This function handles USART1 global interrupt.
   */
 void USART1_IRQHandler(void)
@@ -289,8 +244,8 @@ void EXTI15_10_IRQHandler(void)
   /* USER CODE END EXTI15_10_IRQn 0 */
   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_10);
   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_11);
-  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12);
-  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
+  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_14);
+  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
   /* USER CODE BEGIN EXTI15_10_IRQn 1 */
 
   /* USER CODE END EXTI15_10_IRQn 1 */

OZE_Sensor/Core/Src/uart_tasks.c

@@ -512,42 +512,26 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
     	NVIC_SystemReset();
     	break;
     case spSetPositonX:
-    	PositionControlTaskData posXData = { 0 };
+    	PositionControlTaskData posXData  __attribute__ ((aligned (32))) = { 0 };
+        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+        	sensorsInfo.positionXWeak = 1;
+            osMutexRelease (sensorsInfoMutex);
+        }
     	if (positionXControlTaskInitArg.positionSettingQueue != NULL)
     	{
-			float posXPercent = 0;
-			ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posXPercent);
-			float posXDegress = MAX_X_AXE_ANGLE * posXPercent * 0.01;
-			float angleDelta = 360 / ENCODER_X_IMP_PER_TURN;
-			float rest = fmodf(posXDegress, angleDelta);
-			if ( rest > (angleDelta/2))
-			{
-				posXData.positionSettingValue = 100 * (posXDegress - rest + angleDelta) / MAX_X_AXE_ANGLE;
-			}
-			else
-			{
-				posXData.positionSettingValue = 100 * (posXDegress - rest) / MAX_X_AXE_ANGLE;
-			}
+    		ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posXData.positionSettingValue);
 			osMessageQueuePut(positionXControlTaskInitArg.positionSettingQueue, &posXData, 0, 0);
     	}
     	break;
     case spSetPositonY:
-    	PositionControlTaskData posYData = { 0 };
+        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+        	sensorsInfo.positionYWeak = 1;
+            osMutexRelease (sensorsInfoMutex);
+        }
+    	PositionControlTaskData posYData __attribute__ ((aligned (32))) = { 0 };
     	if (positionYControlTaskInitArg.positionSettingQueue != NULL)
     	{
-			float posYPercent = 0;
-			ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posYPercent);
-			float posYDegress = MAX_Y_AXE_ANGLE * posYPercent * 0.01;
-			float angleDelta = 360 / ENCODER_Y_IMP_PER_TURN;
-			float rest = fmodf(posYDegress, angleDelta);
-			if ( rest > (angleDelta/2))
-			{
-				posYData.positionSettingValue = 100 * (posYDegress - rest + angleDelta) / MAX_Y_AXE_ANGLE;
-			}
-			else
-			{
-				posYData.positionSettingValue = 100 * (posYDegress - rest) / MAX_Y_AXE_ANGLE;
-			}
+    		ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posYData.positionSettingValue);
 			osMessageQueuePut(positionYControlTaskInitArg.positionSettingQueue, &posYData, 0, 0);
     	}
     	break;