/*********************************************************************
; F U N C T I O N D E S C R I P T I O N
;---------------------------------------------------------------------
; NAME:
; DESCRIPTION:
; Input:
; Output:
; Used global variables:
; REMARKS when using this function:
;*********************************************************************/
Err_t receiveStatus(uint8_t id, uint8_t *status, uint8_t *param, uint8_t *parcount)
{
uint16_t start = 0;
uint8_t i, checksum = 0, receiveChecksum = 0;
/* Set PE2 to low & PE3 to high (enable receive) */
RECEN
;
do
{
start = (start << 8);
start |= (uint16_t)USARTReceive(USART_SERVO);
}
while (start != 0xFFFF);
if (id != USARTReceive(USART_SERVO))
return INVALID_ID;
*parcount = USARTReceive(USART_SERVO);
*status = USARTReceive(USART_SERVO);
if (*parcount > 2)
{
for (i = 0; i < (*parcount) - 2; i++)
{
*(param + i) = USARTReceive(USART_SERVO);
checksum += *(param + i);
}
}
receiveChecksum = USARTReceive(USART_SERVO);
checksum += (id + *parcount + *status);
checksum = ~checksum;
if (checksum != receiveChecksum)
return CRC_ERR;
if (*status == 0)
return ERROR_OK;
else
return STATUS_ERR;
}
char USART_ReadChar(USART_TypeDef *USARTPort)
{
uint8_t buf;
USARTReceive(USARTPort, &buf, 1);
return (char)buf;
}