Position offset calculation update

OZE_Sensor/Core/Src/uart_tasks.c

@@ -419,6 +419,9 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         float motorXMaxCurrent = 0;
         ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorXMaxCurrent);
         uint32_t dacDataCh1 = (uint32_t)(4095 * motorXMaxCurrent / (EXT_VREF_mV * 0.001));
+        if (dacDataCh1 > 4095) {
+            dacDataCh1 = 4095;
+        }
         HAL_DAC_SetValue (&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, dacDataCh1);
         HAL_DAC_Start (&hdac1, DAC_CHANNEL_1);
         respStatus = spOK;
@@ -427,6 +430,9 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         float motorYMaxCurrent = 0;
         ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorYMaxCurrent);
         uint32_t dacDataCh2 = (uint32_t)(4095 * motorYMaxCurrent / (EXT_VREF_mV * 0.001));
+        if (dacDataCh2 > 4095) {
+            dacDataCh2 = 4095;
+        }
         HAL_DAC_SetValue (&hdac1, DAC_CHANNEL_2, DAC_ALIGN_12B_R, dacDataCh2);
         HAL_DAC_Start (&hdac1, DAC_CHANNEL_2);
         respStatus = spOK;
@@ -444,10 +450,10 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         }
         break;
     case spSetEncoderXValue:
-    	float enocoderXOffset = 0;
-        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderXOffset);
+        float enocoderXValue = 0;
+        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderXValue);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-        	sensorsInfo.positionXOffset = enocoderXOffset;
+            sensorsInfo.positionXOffset = enocoderXValue - sensorsInfo.currentXPosition;
             osMutexRelease (sensorsInfoMutex);
             respStatus = spOK;
         } else {
@@ -455,10 +461,10 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         }
         break;
     case spSetEncoderYValue:
-        float enocoderYOffset = 0;
-        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderYOffset);
+        float enocoderYValue = 0;
+        ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&enocoderYValue);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
-        	sensorsInfo.positionYOffset = enocoderYOffset;
+            sensorsInfo.positionYOffset = enocoderYValue - sensorsInfo.currentYPosition;
             osMutexRelease (sensorsInfoMutex);
             respStatus = spOK;
         } else {