128,7 → 128,7 |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}}}; |
|
#define SERVO_TIMEOUT 20000 |
#define SERVO_TIMEOUT 1000000 |
|
#define RXTX_BUFF_MAX 100 |
volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
166,9 → 166,23 |
void servo_indication(unsigned port, BYTE data) { |
|
if (RXTX_addr[port][RX_position[port]] == NULL) { |
if (data != 0) |
if (data != 0) { |
servo_rx_error(port,1,data); |
while(RXTX_addr[port][RX_position[port]] != SERVO_SKIP) { |
RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
RX_position[port]++; |
|
if (RX_position[port] >= RXTX_BUFF_MAX) { |
RX_cycle[port]++; |
RX_position[port] = 0; |
} |
if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
(RX_cycle[port] > TX_cycle[port])) |
servo_rx_error(port,2,data); |
|
} |
} |
|
RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
|
RX_position[port]++; |
307,6 → 321,26 |
i++; |
} |
|
/* SERVO_SKIP close the command sequence */ |
RXTX_addr[port][TX_position[port]] = SERVO_SKIP; |
TX_position[port]++; |
if (TX_position[port] >= RXTX_BUFF_MAX) { |
TX_cycle[port]++; |
TX_position[port] = 0; |
} |
if (TX_position[port] == RX_position[port]) { |
TX_position[port] = old; |
TX_cycle[port] = oldcycle; |
lock_write[port] = 0; |
|
#ifdef SERVO_DEBUG |
kern_printf("(SERVO: BUFFER FULL)"); |
#endif |
|
kern_frestore(f); |
return -1; |
} |
|
kern_frestore(f); |
|
com_irq_send(port, len_msg, msg); |
320,25 → 354,27 |
volatile int start = kern_gettime(NULL), timeout = 0, temp; |
|
/* Active wait until number bytes received */ |
while(RXTX_buff[port][RX] == 0 && timeout == 0) |
while(RXTX_buff[port][RX] == SERVO_LOCK && timeout == 0) |
if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
|
if (timeout == 1) { |
SYS_FLAGS f; |
|
f = kern_fsave(); |
|
#ifdef SERVO_DEBUG |
kern_printf("(SERVO: TIMEOUT GET)"); |
#endif |
|
f = kern_fsave(); |
|
temp = RX; |
|
|
while(RXTX_addr[port][temp] != SERVO_SKIP) { |
RXTX_addr[port][temp] = SERVO_SKIP; |
RXTX_buff[port][temp] = SERVO_UNLOCK; |
temp--; |
if (temp < 0) temp = RXTX_BUFF_MAX-1; |
} |
|
|
kern_frestore(f); |
|
} |