Encoder calcualtions update

OZE_Main/Core/Inc/measurements.h

@@ -24,8 +24,8 @@ struct _RESMeasurements {
 struct _SesnorsInfo {
     float pvTemperature[TEMPERATURE_QTY];
     float fanVoltage;
-    float pvEncoderX;
-    float pvEncoderY;
+    int32_t pvEncoderXraw;
+    int32_t pvEncoderYraw;
     uint8_t motorXStatus;
     uint8_t motorYStatus;
     float motorXAveCurrent;
@@ -43,6 +43,8 @@ struct _SesnorsInfo {
     float currentYPosition;
     uint8_t positionXWeak;
     uint8_t positionYWeak;
+    float positionXOffset;
+    float positionYOffset;
 };
 
 typedef struct _RESMeasurements RESMeasurements;

OZE_Main/Core/Src/mock_tasks.c

@@ -76,8 +76,8 @@ void MockMeasurmetsTask (void* argument) {
 			sensors->pvTemperature[0] = 50 + (0.01 * (counter % 100));
 			sensors->pvTemperature[1] = 51 + (0.01 * (counter % 100));
 			sensors->fanVoltage = 12 + (0.01 * (counter % 100));
-			sensors->pvEncoderX = 15 + (0.01 * (counter % 100));
-			sensors->pvEncoderY = 10 + (0.01 * (counter % 100));
+			sensors->pvEncoderXraw = 15 + (0.01 * (counter % 100));
+			sensors->pvEncoderYraw = 10 + (0.01 * (counter % 100));
 			sensors->motorXStatus = (counter % 100) > 50 ? 1 : 0;
 			sensors->motorYStatus = (counter % 100) > 75 ? 1 : 0;
 			sensors->motorXAveCurrent = 3 + (0.01 * (counter % 100));

OZE_Main/Core/Src/mqtt_client.c

@@ -163,8 +163,10 @@ void MqttClientPubTask (void* argument) {
                         sprintf (topicTextBuffer, "Sensors/%d", boardNumber);
                         bytesInBuffer = sprintf (messageBuffer, "{\"pvTemperature\":[%.1f, %.1f], ", sensors->pvTemperature[0], sensors->pvTemperature[1]);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"fanVoltage\":%.2f, ", sensors->fanVoltage);
-                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"pvEncoderX\":%.2f, ", sensors->pvEncoderX);
-                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"pvEncoderY\":%.2f, ", sensors->pvEncoderY);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"pvEncoderX\":%.2f, ", sensors->currentXPosition);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"pvEncoderY\":%.2f, ", sensors->currentYPosition);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"pvEncoderXraw\":%ld, ", sensors->pvEncoderXraw);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"pvEncoderYraw\":%ld, ", sensors->pvEncoderYraw);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"motorXStatus\":%d, ", sensors->motorXStatus);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"motorYStatus\":%d, ", sensors->motorYStatus);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"motorXAveCurrent\":%.3f, ", sensors->motorXAveCurrent);
@@ -177,8 +179,8 @@ void MqttClientPubTask (void* argument) {
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"limitYSwitchUp\":%d, ", sensors->limitYSwitchUp);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"limitYSwitchDown\":%d, ", sensors->limitYSwitchDown);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"limitYSwitchCenter\":%d, ", sensors->limitYSwitchCenter);
-                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"currentXPosition\":%.2f, ", sensors->currentXPosition);
-                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"currentYPosition\":%.2f, ", sensors->currentYPosition);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"currentXPosition\":%.2f, ", sensors->currentXPosition + sensors->positionXOffset);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"currentYPosition\":%.2f, ", sensors->currentYPosition + sensors->positionYOffset);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"positionXWeak\":%d, ", sensors->positionXWeak);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"positionYWeak\":%d, ", sensors->positionYWeak);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"powerSupplyFailMask\":%d}", sensors->powerSupplyFailMask);

OZE_Main/Core/Src/serial_protocol.c

@@ -67,7 +67,7 @@ void ReadShortWordFromBufer(uint8_t* buff, uint16_t* buffPos, uint16_t* data)
 
 void ReadWordFromBufer(uint8_t* buff, uint16_t* buffPos, uint32_t* data)
 {
-	*data = CONVERT_BYTES_TO_SHORT_WORD(&buff[*buffPos]);
+	*data = CONVERT_BYTES_TO_WORD(&buff[*buffPos]);
 	*buffPos += sizeof(uint32_t);
 }
 

OZE_Main/Core/Src/uart_tasks.c

@@ -348,8 +348,8 @@ void UartTxTask (void* argument) {
     uint32_t rndVal                     = 0;
     uint16_t bytesToSend                = 0;
     SerialProtocolCommands frameCommand = spUnknown;
-    uint16_t inputDataBufferPos         = 0;
     uint8_t boardNumber                 = 0;
+    uint16_t inputDataBufferPos         = 0;
 
     while (pdTRUE) {
         if (uartTaskData->sendCmdToSlaveQueue != NULL) {
@@ -429,8 +429,8 @@ void UartTxTask (void* argument) {
                             ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvTemperature[0]);
                             ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvTemperature[1]);
                             ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->fanVoltage);
-                            ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvEncoderX);
-                            ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->pvEncoderY);
+                            ReadWordFromBufer (frameData.dataBuffer, &inputDataBufferPos, (uint32_t *)&sensors->pvEncoderXraw);
+                            ReadWordFromBufer (frameData.dataBuffer, &inputDataBufferPos, (uint32_t *)&sensors->pvEncoderYraw);
                             ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorXStatus);
                             ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorYStatus);
                             ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->motorXAveCurrent);
@@ -446,6 +446,8 @@ void UartTxTask (void* argument) {
                             ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->powerSupplyFailMask);
                             ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->currentXPosition);
                             ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->currentYPosition);
+                            ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionXOffset);
+                            ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionYOffset);
                             ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionXWeak);
                             ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionYWeak);
                             osMutexRelease (sensorsInfoMutex);

OZE_Sensor/Core/Inc/meas_tasks.h

@@ -20,7 +20,9 @@ struct _EncoderTaskArg
 {
 	uint16_t dbgLed;
 	float* currentPosition;
-	float* pvEncoder;
+	float* positionOffset;
+	int32_t* pvEncoder;
+	float maxAngle;
 	uint32_t initPinStates;
 	osMessageQueueId_t dataQueue;
 };

OZE_Sensor/Core/Inc/measurements.h

@@ -23,8 +23,8 @@ struct _RESMeasurements {
 struct _SesnorsInfo {
     float pvTemperature[TEMPERATURE_QTY];
     float fanVoltage;
-    float pvEncoderX;
-    float pvEncoderY;
+    int32_t pvEncoderXraw;
+    int32_t pvEncoderYraw;
     uint8_t motorXStatus;
     uint8_t motorYStatus;
     float motorXAveCurrent;
@@ -42,6 +42,8 @@ struct _SesnorsInfo {
     float currentYPosition;
     uint8_t positionXWeak;
     uint8_t positionYWeak;
+    float positionXOffset;
+    float positionYOffset;
 };
 
 typedef struct _RESMeasurements RESMeasurements;

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

@@ -37,3 +37,4 @@
 //#define MOCK_VOLTAGES_AND_CURRENS
 
 #endif /* INC_NODE_RED_CONFIG_H_ */
+

OZE_Sensor/Core/Inc/position_task.h

@@ -35,6 +35,7 @@ struct _PositionControlTaskInitArg
 	uint8_t* switchLimiterDownStat;
 	uint8_t* switchLimiterCenterStat;
 	float* currentPosition;
+	float* positionOffset;
 	float* positionSetting;
 	uint8_t* motorStatus;
 	float* motorPeakCurrent;

OZE_Sensor/Core/Src/meas_tasks.c

@@ -110,17 +110,21 @@ void MeasTasksInit (void) {
     limiterSwitchTaskHandle                          = osThreadNew (LimiterSwitchTask, NULL, &osThreadAttradc1LimiterSwitchTask);
 
     encoderXTaskArg.dbgLed                   = DBG_LED2;
-    encoderXTaskArg.pvEncoder                = &(sensorsInfo.pvEncoderX);
+    encoderXTaskArg.pvEncoder                = &(sensorsInfo.pvEncoderXraw);
     encoderXTaskArg.currentPosition          = &(sensorsInfo.currentXPosition);
+    encoderXTaskArg.positionOffset           = &(sensorsInfo.positionXOffset);
+    encoderXTaskArg.maxAngle				 = MAX_X_AXE_ANGLE;
     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;
+    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.pvEncoder       = &(sensorsInfo.pvEncoderYraw);
     encoderYTaskArg.currentPosition = &(sensorsInfo.currentYPosition);
+    encoderYTaskArg.positionOffset  = &(sensorsInfo.positionYOffset);
+    encoderYTaskArg.maxAngle				 = MAX_Y_AXE_ANGLE;
     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;
+    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;
@@ -128,24 +132,24 @@ void MeasTasksInit (void) {
     encoderXTaskHandle                     = osThreadNew (EncoderTask, &encoderXTaskArg, &osThreadAttrEncoderTask);
     encoderYTaskHandle                     = osThreadNew (EncoderTask, &encoderYTaskArg, &osThreadAttrEncoderTask);
 #ifdef MOCK_VOLTAGES_AND_CURRENS
-    GenenarateWaveSamples(325.269, 0, 128, 8, voltageWave[0]);
-    GenenarateWaveSamples(324.269, 0, 128, 8, voltageWave[1]);
-    GenenarateWaveSamples(323.269, 0, 128, 8, voltageWave[2]);
-    GenenarateWaveSamples(1.414213562, 0, 128, 8, currentWave[0]);
-    GenenarateWaveSamples(1.314213562, 0, 128, 8, currentWave[1]);
-    GenenarateWaveSamples(1.214213562, 0, 128, 8, currentWave[2]);
+    GenenarateWaveSamples (325.269, 0, 128, 8, voltageWave[0]);
+    GenenarateWaveSamples (324.269, 0, 128, 8, voltageWave[1]);
+    GenenarateWaveSamples (323.269, 0, 128, 8, voltageWave[2]);
+    GenenarateWaveSamples (1.414213562, 0, 128, 8, currentWave[0]);
+    GenenarateWaveSamples (1.314213562, 0, 128, 8, currentWave[1]);
+    GenenarateWaveSamples (1.214213562, 0, 128, 8, currentWave[2]);
 #endif
-//    arm_hanning_f32
-//    arm_rfft_fast_instance_f32 fft;
-//    arm_rfft_fast_init_f32(&fft, FFT_Length);
-//    arm_rfft_fast_f32(&fft, waveOne, fft_output, ifftFlag);
-//    arm_cmplx_mag_f32(fft_output, fft_power, SAMPLE_BUFFER_LENGTH_HALF);
-//    float32_t scale = 2.0f/FFT_Length;
-//    arm_scale_f32(fft_power, scale, fft_power_scaled, SAMPLE_BUFFER_LENGTH_HALF);
-//    float32_t   maxValue;
-//    uint32_t    maxIndex;
-//    arm_max_f32(fft_power_scaled, SAMPLE_BUFFER_LENGTH_HALF, &maxValue, &maxIndex);
-//    printf("maxValue %f, index %ld\n", maxValue, maxIndex);
+    //    arm_hanning_f32
+    //    arm_rfft_fast_instance_f32 fft;
+    //    arm_rfft_fast_init_f32(&fft, FFT_Length);
+    //    arm_rfft_fast_f32(&fft, waveOne, fft_output, ifftFlag);
+    //    arm_cmplx_mag_f32(fft_output, fft_power, SAMPLE_BUFFER_LENGTH_HALF);
+    //    float32_t scale = 2.0f/FFT_Length;
+    //    arm_scale_f32(fft_power, scale, fft_power_scaled, SAMPLE_BUFFER_LENGTH_HALF);
+    //    float32_t   maxValue;
+    //    uint32_t    maxIndex;
+    //    arm_max_f32(fft_power_scaled, SAMPLE_BUFFER_LENGTH_HALF, &maxValue, &maxIndex);
+    //    printf("maxValue %f, index %ld\n", maxValue, maxIndex);
 }
 
 void ADC1MeasTask (void* arg) {
@@ -365,7 +369,7 @@ void EncoderTask (void* arg) {
     }
     while (pdTRUE) {
         osMessageQueueGet (encoderTaskArg->dataQueue, &pinStates, 0, osWaitForever);
-        float encoderValue = *encoderTaskArg->pvEncoder;
+        int32_t encoderValue = *encoderTaskArg->pvEncoder;
 		if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
 			if (encoderStates[(step + 1) % 4] == pinStates) {
 				step++;
@@ -386,7 +390,7 @@ void EncoderTask (void* arg) {
 			step                             = step % 4;
 //			*encoderTaskArg->pvEncoder       = fmodf (encoderValue, 360.0);
 			*encoderTaskArg->pvEncoder       = encoderValue;
-			*encoderTaskArg->currentPosition = 100 * (*encoderTaskArg->pvEncoder) / MAX_X_AXE_ANGLE;
+			*encoderTaskArg->currentPosition = 100 * (*encoderTaskArg->pvEncoder) / encoderTaskArg->maxAngle;
 			osMutexRelease (sensorsInfoMutex);
 		}
         DbgLEDToggle (encoderTaskArg->dbgLed);

OZE_Sensor/Core/Src/mock_tasks.c

@@ -70,8 +70,8 @@ void MockMeasurmetsTask (void* argument) {
 		sensorsInfo.pvTemperature[0] = 50 + (0.01 * (counter % 100));
 		sensorsInfo.pvTemperature[1] = 51 + (0.01 * (counter % 100));
 		sensorsInfo.fanVoltage = 12 + (0.01 * (counter % 100));
-		sensorsInfo.pvEncoderX = 15 + (0.01 * (counter % 100));
-		sensorsInfo.pvEncoderY = 10 + (0.01 * (counter % 100));
+		sensorsInfo.pvEncoderXraw = 15 + (0.01 * (counter % 100));
+		sensorsInfo.pvEncoderYraw = 10 + (0.01 * (counter % 100));
 		sensorsInfo.motorXStatus = (counter % 100) > 50 ? 1 : 0;
 		sensorsInfo.motorYStatus = (counter % 100) > 75 ? 1 : 0;
 		sensorsInfo.motorXAveCurrent = 3 + (0.01 * (counter % 100));

OZE_Sensor/Core/Src/position_task.c

@@ -47,6 +47,7 @@ void PositionControlTaskInit (void) {
     positionXControlTaskInitArg.switchLimiterUpStat     = &(sensorsInfo.limitXSwitchUp);
     positionXControlTaskInitArg.switchLimiterDownStat   = &(sensorsInfo.limitXSwitchDown);
     positionXControlTaskInitArg.currentPosition         = &(sensorsInfo.currentXPosition);
+    positionXControlTaskInitArg.positionOffset			= &(sensorsInfo.positionXOffset);
     positionXControlTaskInitArg.motorStatus             = &(sensorsInfo.motorXStatus);
     positionXControlTaskInitArg.motorPeakCurrent        = &(sensorsInfo.motorXPeakCurrent);
     positionXControlTaskInitArg.positionSetting         = &positionXSetting;
@@ -62,6 +63,7 @@ void PositionControlTaskInit (void) {
     positionYControlTaskInitArg.switchLimiterUpStat     = &(sensorsInfo.limitYSwitchUp);
     positionYControlTaskInitArg.switchLimiterDownStat   = &(sensorsInfo.limitYSwitchDown);
     positionYControlTaskInitArg.currentPosition         = &(sensorsInfo.currentYPosition);
+    positionYControlTaskInitArg.positionOffset			= &(sensorsInfo.positionYOffset);
     positionYControlTaskInitArg.motorStatus             = &(sensorsInfo.motorYStatus);
     positionYControlTaskInitArg.motorPeakCurrent        = &(sensorsInfo.motorYPeakCurrent);
     positionXControlTaskInitArg.positionSetting         = &positionYSetting;
@@ -88,10 +90,10 @@ void PositionControlTask (void* argument) {
         queueSatus = osMessageQueueGet (posCtrlTaskArg->positionSettingQueue, &posCtrlData, 0, pdMS_TO_TICKS (PositionControlTaskTimeOut));
         if (queueSatus == osOK) {
             if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-                float posDiff = posCtrlData.positionSettingValue - *posCtrlTaskArg->currentPosition;
+                float posDiff = posCtrlData.positionSettingValue - (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset);
                 if (posDiff != 0) {
                     sign                  = posDiff > 0 ? 1 : -1;
-                    startPosition         = *posCtrlTaskArg->currentPosition;
+                    startPosition         = *posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset;
                     movementPhase         = startPhase;
                     moveCmdTimeoutCounter = 0;
                     timeLeftMS            = 0;
@@ -121,12 +123,12 @@ void PositionControlTask (void* argument) {
 
 
                     timeLeftMS += PositionControlTaskTimeOut;
-                    if (prevPosition == *posCtrlTaskArg->currentPosition) {
+                    if (prevPosition == (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset)) {
                         moveCmdTimeoutCounter += PositionControlTaskTimeOut;
                     } else {
                         moveCmdTimeoutCounter = 0;
                     }
-                    prevPosition = *posCtrlTaskArg->currentPosition;
+                    prevPosition = *posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset;
                     if (moveCmdTimeoutCounter > NO_MOVE_TIMEOUT_MS) {
                         movementPhase = idlePhase;
                         motorStatus   = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle, 0,
@@ -158,7 +160,7 @@ void PositionControlTask (void* argument) {
 
                         break;
                     case speedUpPhase:
-                        if ((abs (*posCtrlTaskArg->currentPosition - startPosition) >= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
+                        if ((abs ((*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) - startPosition) >= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
                             pwmValue    = MOTOR_HIGH_SPEED_PWM_VALUE;
                             motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
                             sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
@@ -170,7 +172,7 @@ void PositionControlTask (void* argument) {
                         }
                         break;
                     case movePhase:
-                        if (abs (*posCtrlTaskArg->currentPosition - *posCtrlTaskArg->positionSetting) <= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) {
+                        if (abs ((*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) - *posCtrlTaskArg->positionSetting) <= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) {
                             movementPhase = slowDownPhase;
 #ifdef DBG_POSITION
                             printf ("Axe %c slow down phase\n", posCtrlTaskArg->axe);
@@ -189,7 +191,7 @@ void PositionControlTask (void* argument) {
 #endif
                         break;
                     case stopPhase:
-                    	float posDiff = sign > 0 ? posCtrlData.positionSettingValue - *posCtrlTaskArg->currentPosition : *posCtrlTaskArg->currentPosition - posCtrlData.positionSettingValue;
+                    	float posDiff = sign > 0 ? posCtrlData.positionSettingValue - (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) : (*posCtrlTaskArg->currentPosition + *posCtrlTaskArg->positionOffset) - posCtrlData.positionSettingValue;
                         if ((posDiff <= 0) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
                             motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
                             0, 0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);

OZE_Sensor/Core/Src/uart_tasks.c

@@ -310,8 +310,8 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvTemperature[0], sizeof (float));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvTemperature[1], sizeof (float));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.fanVoltage, sizeof (float));
-            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvEncoderX, sizeof (float));
-            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvEncoderY, sizeof (float));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvEncoderXraw, sizeof (int32_t));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.pvEncoderYraw, sizeof (int32_t));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorXStatus, sizeof (uint8_t));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorYStatus, sizeof (uint8_t));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.motorXAveCurrent, sizeof (float));
@@ -329,6 +329,8 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.powerSupplyFailMask, sizeof (uint8_t));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.currentXPosition, sizeof (float));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.currentYPosition, sizeof (float));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionXOffset, sizeof (float));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionYOffset, sizeof (float));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionXWeak, sizeof (uint8_t));
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionYWeak, sizeof (uint8_t));
             osMutexRelease (sensorsInfoMutex);
@@ -442,11 +444,12 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         }
         break;
     case spSetEncoderXValue:
-        float enocoderXValue = 0;
-        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderXValue);
+    	float enocoderXOffset = 0;
+        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderXOffset);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-            sensorsInfo.pvEncoderX = enocoderXValue;
-            sensorsInfo.currentXPosition = 100 * enocoderXValue / MAX_X_AXE_ANGLE;
+//            sensorsInfo.pvEncoderX = enocoderXValue;
+//            sensorsInfo.currentXPosition = 100 * enocoderXValue / MAX_X_AXE_ANGLE;
+        	sensorsInfo.positionXOffset = enocoderXOffset;
             osMutexRelease (sensorsInfoMutex);
             respStatus = spOK;
         } else {
@@ -454,11 +457,12 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         }
         break;
     case spSetEncoderYValue:
-        float enocoderYValue = 0;
-        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderYValue);
+        float enocoderYOffset = 0;
+        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderYOffset);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-            sensorsInfo.pvEncoderY = enocoderYValue;
-            sensorsInfo.currentYPosition = 100 * enocoderYValue / MAX_X_AXE_ANGLE;
+//            sensorsInfo.pvEncoderY = enocoderYValue;
+//            sensorsInfo.currentYPosition = 100 * enocoderYValue / MAX_X_AXE_ANGLE;
+        	sensorsInfo.positionYOffset = enocoderYOffset;
             osMutexRelease (sensorsInfoMutex);
             respStatus = spOK;
         } else {