135,6 → 135,8 |
volatile unsigned int TX_position[4] = {0,0,0,0}; |
volatile unsigned int RX_cycle = 0; |
volatile unsigned int TX_cycle = 0; |
volatile unsigned int servo_skip = 0; |
volatile unsigned int lock_write = 0; |
|
const int BaudTable[] = { |
1200, |
155,7 → 157,7 |
} |
|
void servo_indication(unsigned port, BYTE data) { |
|
|
if (RXTX_addr[port][RX_position[port]] == NULL) { |
if (data != RXTX_buff[port][RX_position[port]]) |
servo_rx_error(port,1); |
182,17 → 184,25 |
} |
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) || |
(RX_cycle > TX_cycle)) |
servo_rx_error(port,2); |
servo_rx_error(port,3); |
|
} |
|
} |
|
extern int SCom_Error; |
|
void servo_confirm(unsigned port, BYTE msg_status) { |
|
if (msg_status == COM_ERROR) |
kern_printf("(SERVO: PORT:%d ERROR)",port); |
if (msg_status == COM_ERROR) { |
kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error); |
servo_skip = 1; |
} |
|
if (msg_status == COM_OK) { |
lock_write = 0; |
} |
|
} |
|
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
200,6 → 210,9 |
int i = 0, old, oldcycle, RX = 0; |
SYS_FLAGS f; |
|
while(lock_write == 1); |
lock_write = 1; |
|
f = kern_fsave(); |
|
old = TX_position[port]; |
251,8 → 264,10 |
|
int servo_wait(unsigned port, int RX) { |
|
servo_skip = 0; |
|
/* Active wait until number bytes received */ |
while (RXTX_buff[port][RX] == 0); |
while (RXTX_buff[port][RX] == 0 && servo_skip == 0); |
|
return 0; |
|