void MX_UART5_Init(void)
{

  huart5.Instance = UART5;
  huart5.Init.BaudRate = 1000000;
  huart5.Init.WordLength = UART_WORDLENGTH_8B;
  huart5.Init.StopBits = UART_STOPBITS_1;
  huart5.Init.Parity = UART_PARITY_NONE;
  huart5.Init.Mode = UART_MODE_TX_RX;
  huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart5.Init.OverSampling = UART_OVERSAMPLING_8;
  HAL_HalfDuplex_Init(&huart5);

}

void PingServo(UART_HandleTypeDef* uartPtr)
{
uint32_t timeout = 1000; // в миллисекундах

uint8_t txData[6];

uint8_t servoId = 1;
uint8_t len = 2;
uint8_t instruction = 1;
uint8_t checksum = ~(servoId + len + instruction);

txData[0] = 0xFF;
txData[1] = 0xFF;
txData[2] = servoId;
txData[3] = len;
txData[4] = instruction;
txData[5] = checksum;

HAL_StatusTypeDef txStatus = HAL_UART_Transmit(uartPtr, txData, 6, timeout);
if(txStatus != HAL_OK)
{
printf("TX ERROR\r\n");
}


uint8_t rxData[7];

HAL_StatusTypeDef rxStatus = HAL_UART_Receive(uartPtr, rxData, 7, timeout);
if(rxStatus != HAL_OK)
{
printf("RX ERROR\r\n");
}
else
{
uint8_t rxId = rxData[3];
uint8_t rxLen = rxData[4];
uint8_t rxErr = rxData[5];
uint8_t rxChecksum = rxData[6];
if(rxChecksum != checksum)
{
printf("CHECKSUM ERROR: received %x vs sent %x\r\n", rxChecksum, checksum);
}
for(int i = 0; i < 7; i++)
{
printf("rxData[%i] == %x\r\n", i, rxData[i]);
}
}

printf("\r\n\r\n");

}