int Serial_getchar_Blocking(FILE *Stream)
{
(void)Stream;
while (!(Serial_IsCharReceived()));
return Serial_ReceiveByte();
}
bool try_parse_message(void)
{
while (Serial_IsCharReceived()) {
(*inprog_message)[curmsg_firstempty] = Serial_ReceiveByte();
//Serial_SendByte((*inprog_message)[curmsg_firstempty]);
/* dumb idea for resync - we only care about one kind of message */
//*
curmsg_firstempty++;
if (curmsg_firstempty == 1) {
if ((*inprog_message)[0] != 0x44 && (*inprog_message)[0] != 0x19
&& (*inprog_message)[0] != 0x1b) {
curmsg_firstempty = 0;
}
} else if (curmsg_firstempty > 1) {
char len = (*inprog_message)[1];
if (curmsg_firstempty >= len || curmsg_firstempty >= 64) {
char (*t)[64] = usable_message;
usable_message = inprog_message;
inprog_message = t;
//for (int i = 0; i<64; i++) {
// (*inprog_message)[i]=0;
//}
curmsg_firstempty = 0;
rfid_usable_to_send = 1;
//we have a fully parsed message, and we have reset current
return 1;
}
}
//*/
}
return 0;
}
int Serial_getchar_Blocking(FILE *Stream)
{
USART_t* USART = fdev_get_udata(Stream);
while (!(Serial_IsCharReceived(USART)));
return Serial_ReceiveByte(USART);
}
int Serial_getchar(FILE *Stream)
{
(void)Stream;
if (!(Serial_IsCharReceived()))
return _FDEV_EOF;
return Serial_ReceiveByte();
}
static int SerialStream_RxByte(FILE *Stream)
{
(void)Stream;
if (!(Serial_IsCharReceived()))
return _FDEV_EOF;
return Serial_RxByte();
}
int Serial_getchar(FILE *Stream)
{
USART_t* USART = fdev_get_udata(Stream);
if (!(Serial_IsCharReceived(USART)))
return _FDEV_EOF;
return Serial_ReceiveByte(USART);
}
bool waitchar(unsigned char ptr) {
unsigned short ctr = 15000;
while (!(Serial_IsCharReceived())) {
if (ctr-- == 0) {
return false;
}
}
read_servo_buf[ptr] = UDR1;
return true;
}
void raw_read(unsigned char sz) {
cli();
if (sz > MAX_READ_SIZE) {
MY_Failure("RawReadSize", sz, MAX_READ_SIZE);
}
unsigned short ticks = MY_GetTicks();
unsigned char offset = 0;
while (MY_GetTicks() - ticks < RAW_READ_TIMEOUT && offset < sz) {
if (Serial_IsCharReceived()) {
read_servo_buf[offset] = UDR1;
++offset;
}
}
sei();
add_response_sz(OpRawRead, offset, read_servo_buf);
}
void read_servo(unsigned char id, unsigned char reg, unsigned char sz) {
cli();
UCSR1B &= ~(1 << RXEN1); // reset receiver
_delay_us(1);
UCSR1B |= (1 << RXEN1); // reset receiver
if (sz > MAX_READ_SIZE) {
MY_Failure("Read Size", sz, MAX_READ_SIZE);
}
if (id >= MAX_SERVOS) { // can't read from broadcast address
MY_Failure("Servo ID", id, MAX_SERVOS-1);
}
unsigned char cs = 0;
unsigned char ptr = 0;
unsigned char cmd[3] = { DXL_READ_DATA, reg, sz };
servo_cmd(id, 3, cmd, 0, cmd);
// empty the receive pipe
while (Serial_IsCharReceived()) {
read_servo_buf[0] = UDR1;
}
// now read response
read_servo_buf[0] = 0xff;
if (!waitchar(ptr) || read_servo_buf[ptr] != 0xff) {
error_recv(id, read_servo_buf[ptr] == 0xff ? ERR_NOTPRESENT : ERR_BAD_SYNC1, ptr);
goto out;
}
if (!waitchar(ptr) || read_servo_buf[ptr] != 0xff) {
error_recv(id, ERR_BAD_SYNC2, ptr);
goto out;
}
// id
if (!waitchar(ptr) || read_servo_buf[ptr] != id) {
error_recv(id, ERR_BAD_ID, ptr);
goto out;
}
// save the ID
++ptr;
// length
if (!waitchar(ptr) || read_servo_buf[ptr] != sz + 2) {
error_recv(id, ERR_BAD_LENGTH, ptr);
goto out;
}
cs = id + sz + 2;
if (!waitchar(ptr)) {
error_recv(id, ERR_BAD_STATUS, ptr);
goto out;
}
servo_stati[id] = read_servo_buf[ptr];
cs += read_servo_buf[ptr];
read_servo_buf[ptr] = reg;
// return also what the length is
++ptr;
for (unsigned char ix = 0; ix != sz; ++ix) {
if (!waitchar(ptr)) {
error_recv(id, ERR_READ_ERROR, ptr);
goto out;
}
cs += read_servo_buf[ptr];
// save the actual data
++ptr;
}
cs = ~cs;
if (!waitchar(ptr) || cs != read_servo_buf[ptr]) {
error_recv(id, ERR_BAD_CHECKSUM, ptr);
goto out;
}
// buf is id + reg + data
add_response_sz(OpReadServo, ptr, read_servo_buf);
out:
sei();
}
static inline int16_t Serial_BlockingReceiveByte(void)
{
while(!Serial_IsCharReceived());
return UDR1;
}
uint8_t BT::read(void)
{
if(!present)
return 1;
uint16_t bytes = 0;
dataId = 0;
dataSize = 0;
BT_SET_CTS;
uint16_t timeout;
for(;;)
{
timeout = 0;
while(!Serial_IsCharReceived())
{
wdt_reset();
_delay_us(10);
if(++timeout > (bytes > 0 ? 50000 : 5000))
break;
}
if(timeout > 50000)
{
debug(STR("TIMED OUT!"));
debug(STR("\r\n BYTES: "));
debug(bytes);
debug(STR("\r\n ID: "));
debug((uint8_t)buf[1]);
debug(STR("\r\n SIZE: "));
debug(dataSize);
debug(STR("\r\n DATA[0]: "));
debug(buf[0]);
debug(STR("\r\n DATA[1]: "));
debug((uint8_t)buf[1]);
debug(STR("\r\n DATA[2]: "));
debug((uint8_t)buf[2]);
debug(STR("\r\n DATA[3]: "));
debug((uint8_t)buf[3]);
debug(STR("\r\n DATA[4]: "));
debug(buf[4]);
debug(STR("\r\n DATA[5]: "));
debug(buf[5]);
debug(STR("\r\n"));
break;
}
else if(timeout > 5000 && bytes == 0)
{
break;
}
buf[bytes] = (char)Serial_ReceiveByte();
if(!(bytes == 0 && (buf[0] == '\n' || buf[0] == '\r'))) bytes++; // skip leading CR/LF
if(mode == BT_MODE_CMD)
{
if(bytes > 0 && buf[bytes - 1] == '\n') // just get one line at a time
break;
}
else
{
if(buf[0] != '$' && bytes > 1 && buf[bytes - 1] == '$')
{
buf[0] = '$';
bytes = 1;
}
if(dataSize > 0)
{
if(bytes > dataSize + 5) break;
}
else
{
if(bytes > 5 && buf[0] == '$' && buf[5] == ':')
{
dataId = buf[1];
dataType = buf[2];
*(&dataSize) = buf[3];
*(&dataSize + 1) = buf[4];
data = (buf + 6);
if(dataSize == 0) break;
}
else if(buf[0] != '$' && buf[bytes - 1] == '\n') // just get one line at a time
{
break;
}
}
}
if(bytes >= BT_BUF_SIZE)
break;
}
BT_CLR_CTS;
buf[bytes] = 0;
return bytes;
}